/*
	APM_OBC.cpp

	Outback Challenge Failsafe module

	This library is free software; you can redistribute it and/or
	modify it under the terms of the GNU Lesser General Public License
	as published by the Free Software Foundation; either version 2.1
	of the License, or (at your option) any later version.
*/
#include <FastSerial.h>
#include <APM_OBC.h>

// table of user settable parameters
const AP_Param::GroupInfo APM_OBC::var_info[] PROGMEM = {
    // @Param: MAN_PIN
    // @DisplayName: Manual Pin
    // @Description: This sets a digital output pin to set high when in manual mode
    // @User: Advanced
    AP_GROUPINFO("MAN_PIN",     0, APM_OBC, _manual_pin,    -1),

    // @Param: HB_PIN
    // @DisplayName: Heartbeat Pin
    // @Description: This sets a digital output pin which is cycled at 10Hz when termination is not activated
    // @User: Advanced
    AP_GROUPINFO("HB_PIN",      1, APM_OBC, _heartbeat_pin, -1),

    // @Param: WP_COMMS
    // @DisplayName: Comms Waypoint
    // @Description: Waypoint number to navigate to on comms loss
    // @User: Advanced
    AP_GROUPINFO("WP_COMMS",    2, APM_OBC, _wp_comms_hold, 0),

    // @Param: GPS_LOSS
    // @DisplayName: GPS Loss Waypoint
    // @Description: Waypoint number to navigate to on GPS lock loss
    // @User: Advanced
    AP_GROUPINFO("WP_GPS_LOSS", 4, APM_OBC, _wp_gps_loss, 0),

    // @Param: TERMINATE
    // @DisplayName: Force Terminate
    // @Description: Can be set in flight to force termination of the heartbeat signal
    // @User: Advanced
    AP_GROUPINFO("TERMINATE",   5, APM_OBC, _terminate, 0),

    AP_GROUPEND
};

// access to geofence state
extern bool geofence_breached(void);

// function to change waypoint
extern void change_command(uint8_t cmd_index);

// for sending messages
extern void gcs_send_text_fmt(const prog_char_t *fmt, ...);

// check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code
void
APM_OBC::check(APM_OBC::control_mode mode,
	       uint32_t last_heartbeat_ms,
	       uint32_t last_gps_fix_ms)
{	
	// we always check for fence breach
	if (geofence_breached()) {
		if (!_terminate) {
			gcs_send_text_fmt(PSTR("Fence TERMINATE"));
			_terminate.set(1);
		}
	}
	
	// tell the failsafe board if we are in manual control
	// mode. This tells it to pass through controls from the
	// receiver
	if (_manual_pin != -1) {
		digitalWrite(_manual_pin, mode==OBC_MANUAL);
	}

	uint32_t now = millis();
	bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000);
	bool gps_lock_ok = ((now - last_gps_fix_ms) < 3000);

	switch (_state) {
	case STATE_PREFLIGHT:
		// we startup in preflight mode. This mode ends when
		// we first enter auto.
		if (mode == OBC_AUTO) {
			gcs_send_text_fmt(PSTR("Starting OBC_AUTO"));
			_state = STATE_AUTO;
		}
		break;

	case STATE_AUTO:
		// this is the normal mode. 
		if (!gcs_link_ok) {
			gcs_send_text_fmt(PSTR("State DATA_LINK_LOSS"));
			_state = STATE_DATA_LINK_LOSS;
			if (_wp_comms_hold) {
				if (_command_index != NULL) {
					_saved_wp = _command_index->get();
				}
				change_command(_wp_comms_hold);
			}
			break;
		}
		if (!gps_lock_ok) {
			gcs_send_text_fmt(PSTR("State GPS_LOSS"));
			_state = STATE_GPS_LOSS;
			if (_wp_gps_loss) {
				if (_command_index != NULL) {
					_saved_wp = _command_index->get() + 1;
				}
				change_command(_wp_gps_loss);
			}
			break;
		}
		break;

	case STATE_DATA_LINK_LOSS:
		if (!gps_lock_ok) {
			// losing GPS lock when data link is lost
			// leads to termination
			gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
			_terminate.set(1);
		} else if (gcs_link_ok) {
			_state = STATE_AUTO;
			gcs_send_text_fmt(PSTR("GCS OK"));
			change_command(_saved_wp);			
		}
		break;

	case STATE_GPS_LOSS:
		if (!gcs_link_ok) {
			// losing GCS link when GPS lock lost
			// leads to termination
			gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
			_terminate.set(1);
		} else if (gps_lock_ok) {
			gcs_send_text_fmt(PSTR("GPS OK"));
			_state = STATE_AUTO;
			change_command(_saved_wp);			
		}
		break;
	}

	// if we are not terminating then toggle the heartbeat pin at 10Hz
	if (!_terminate && _heartbeat_pin != -1) {
		_heartbeat_pin_value = !_heartbeat_pin_value;
		digitalWrite(_heartbeat_pin, _heartbeat_pin_value);
	}	
}