/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
  OBC failsafe board

  Jack Pittar and Andrew Tridgell

*/

#define MODE_PWM_PIN 9 //Command pulse read on input 11
#define MUXA_PIN     3  // H H rc    , L H micro
#define MUXB_PIN     4  // H L backup, L L prime
#define LED_PIN      13
#define OBC_MODE_PIN 12 //high for OBC termination settings
#define AILERON_PIN  5
#define ELEVATOR_PIN 6
#define THROTTLE_PIN 7
#define RUDDER_PIN   8

#define HEARTBEAT_PRIMARY_PIN 11
#define TERMINATE_PRIMARY_PIN 10

#define HEARTBEAT_BACKUP_PIN A1
#define TERMINATE_BACKUP_PIN A0

// set to 1 if backup autopilot is installed
#define BACKUP_AUTOPILOT_INSTALLED 0

enum mux_mode {
	MUX_MODE_RC      = 1,
	MUX_MODE_MICRO   = 2,
	MUX_MODE_BACKUP  = 3,
	MUX_MODE_PRIMARY = 4
};

// heartbeat status
static bool heartbeat_primary_ok   = true;
static bool heartbeat_backup_ok    = true;



// check heartbeat status
static void update_heartbeat(void)
{
	static uint8_t last_hb_primary;
	static uint8_t last_hb_backup;
	static uint32_t last_primary_t;
	static uint32_t last_backup_t;
	uint8_t hb_primary   = digitalRead(HEARTBEAT_PRIMARY_PIN);
	uint8_t hb_backup    = digitalRead(HEARTBEAT_BACKUP_PIN);
	uint32_t tnow = millis();
//	Serial.println(hb_primary);
	// we consider the heartbeat to be OK if it changed in the
	// last 0.4 seconds
	const uint32_t hb_check_time = 400;
	if (hb_primary != last_hb_primary) {
		last_hb_primary = hb_primary;
		last_primary_t = tnow;
		//Serial.println("hb1 change");
	}
	if (hb_backup != last_hb_backup) {
		last_hb_backup = hb_backup;
		last_backup_t = tnow;
		//Serial.println("hb2 change");
	}
	if (tnow < hb_check_time) {
		// not enough time has passed for a measurement
		return;
	}
	heartbeat_primary_ok   = ((tnow - last_primary_t)   < hb_check_time);
	heartbeat_backup_ok = ((tnow - last_backup_t) < hb_check_time);
}


static void set_mux_mode(uint8_t mode)
{
	switch ((enum mux_mode)mode) {
	case MUX_MODE_RC:
		digitalWrite(MUXA_PIN, HIGH);
		digitalWrite(MUXB_PIN, HIGH);
		break;
	case MUX_MODE_MICRO:
		digitalWrite(MUXA_PIN, LOW);
		digitalWrite(MUXB_PIN, HIGH);
		break;
	case MUX_MODE_BACKUP:
		digitalWrite(MUXA_PIN, HIGH);
		digitalWrite(MUXB_PIN, LOW);
		break;
	case MUX_MODE_PRIMARY:
		digitalWrite(MUXA_PIN, LOW);
		digitalWrite(MUXB_PIN, LOW);
		break;		
	}
}

// send pulses to servos
static void set_servos(uint16_t aileron, uint16_t elevator,
		       uint16_t throttle, uint16_t rudder)
{
	digitalWrite(AILERON_PIN, HIGH);          
	delayMicroseconds(aileron);
	digitalWrite(AILERON_PIN, LOW);  

	digitalWrite(ELEVATOR_PIN, HIGH);
	delayMicroseconds(elevator);
	digitalWrite(ELEVATOR_PIN, LOW);

	digitalWrite(THROTTLE_PIN, HIGH);
	delayMicroseconds(throttle);
	digitalWrite(THROTTLE_PIN, LOW); 

	digitalWrite(RUDDER_PIN, HIGH);
	delayMicroseconds(rudder);
	digitalWrite(RUDDER_PIN, LOW);
}

static void set_servos_terminate(uint8_t obc_mode)
{
	set_mux_mode(MUX_MODE_MICRO);
	if (obc_mode) {
		set_servos(1000, 2000, 1000, 1000);
	} else {
		set_servos(1500, 1500, 1200, 1500);
	}
}


void setup() 
{
	// input pins
	pinMode(HEARTBEAT_PRIMARY_PIN, INPUT);
	pinMode(TERMINATE_PRIMARY_PIN, INPUT);
	pinMode(HEARTBEAT_BACKUP_PIN, INPUT);
	pinMode(TERMINATE_BACKUP_PIN, INPUT);
	pinMode(MODE_PWM_PIN, INPUT);
	pinMode(OBC_MODE_PIN, INPUT);

	// output pins
	pinMode(LED_PIN, OUTPUT);
	pinMode(MUXA_PIN, OUTPUT);
	pinMode(MUXB_PIN, OUTPUT);
	pinMode(AILERON_PIN, OUTPUT); 
	pinMode(ELEVATOR_PIN, OUTPUT);
	pinMode(THROTTLE_PIN, OUTPUT);
	pinMode(RUDDER_PIN, OUTPUT);   

	digitalWrite(LED_PIN, LOW);
	set_mux_mode(MUX_MODE_MICRO);

	Serial.begin(115200);
	Serial.println("OBC Failsafe Starting\n");
}   

void loop() 
{
	static uint32_t last_status_t;
	uint32_t tnow = millis();
	static uint8_t led_state;
	static bool has_terminated = false;
	static uint8_t termination_counter;
	static uint16_t loop_counter;

	loop_counter++;

	// check for heartbeat
	update_heartbeat();

	// see if we are in manual mode. Note that on the Uno,
	// pulseIn is not very accurate. The +90 brings us much closer
	// to the real pulse width
	uint16_t mode_pwm = pulseIn(MODE_PWM_PIN, HIGH, 25000) + 90;
	uint16_t manual_mode = (mode_pwm > 1750 && mode_pwm < 2100);

	// check the state of the terminate pins
	uint8_t terminate_primary = digitalRead(TERMINATE_PRIMARY_PIN);
	uint8_t terminate_backup  = digitalRead(TERMINATE_BACKUP_PIN);

	// see if the OBC mode jumper is set
	uint8_t obc_mode = digitalRead(OBC_MODE_PIN);

	if (tnow - last_status_t > 1000) {
		last_status_t = tnow;
		// show status once a second
		Serial.print("OBC:"); Serial.print(obc_mode);
		Serial.print(" Mode:"); Serial.print(mode_pwm);
		Serial.print(" HB1:"); Serial.print(heartbeat_primary_ok);
		Serial.print(" HB2:"); Serial.print(heartbeat_backup_ok);
		Serial.print(" TERM1:"); Serial.print(terminate_primary);
		Serial.print(" TERM2:"); Serial.print(terminate_backup);
		Serial.print(" TERMINATED:"); Serial.print(has_terminated);
		Serial.print(" LOOP:"); Serial.print(loop_counter);
		Serial.println();
		delayMicroseconds(5000);
		loop_counter = 0;
		// flash LED once a second so we know failsafe board
		// is working
		led_state = !led_state;
		digitalWrite(LED_PIN, led_state);
	}

	// allow reset via serial connection
	if (Serial.available() > 0) {
		char c = (char)Serial.read();
		if (c == 'r') {
			Serial.println("RESET BOARD");
			has_terminated = false;
			termination_counter = 0;
		}
	}

	// if we are not in OBC mode, and the mode control 
	// channel is high, then give RC control
	if (!obc_mode && manual_mode) {
		// give manual control via RC
		set_mux_mode(MUX_MODE_RC);		
		return;
	}

#if BACKUP_AUTOPILOT_INSTALLED == 0
	terminate_backup = false;
#endif

	// see if termination is set by a functioning autopilot
	if (heartbeat_primary_ok && terminate_primary) {
		termination_counter++;
	} else if (heartbeat_backup_ok && terminate_backup) {
		termination_counter++;
	} else if (obc_mode && !heartbeat_primary_ok && !heartbeat_backup_ok) {
		// if in OBC mode and neither autopilot is OK, then
		// terminate
		termination_counter++;
	} else {
		termination_counter = 0;
	}

	// use the termination counter to debounce the termination
	// pins
	if (termination_counter > 10) {
		termination_counter = 0;
		has_terminated = true;
	}

	// if we have terminated then setup the servos
	if (has_terminated) {
		set_servos_terminate(obc_mode);
		return;		
	}

	bool heartbeat_ok = heartbeat_primary_ok;
#if BACKUP_AUTOPILOT_INSTALLED
	if (heartbeat_backup_ok) {
		heartbeat_ok = true;
	}
#endif

	if (heartbeat_ok) {
		// at least one autopilot is healthy
		if (manual_mode) {
			// we want manual/RC control
			set_mux_mode(MUX_MODE_RC);
		} else if (heartbeat_primary_ok) {
			set_mux_mode(MUX_MODE_PRIMARY);
		} else {
			set_mux_mode(MUX_MODE_BACKUP);
		}
	} else {
		// neither autopilot is OK
		if (obc_mode) {
			set_servos_terminate(obc_mode);
		} else {
			// give RC control 
			set_mux_mode(MUX_MODE_RC);
		}
	}
}