// Main state machine loop for AP_Limits. Called from slow or superslow loop.

#if AP_LIMITS == ENABLED

uint8_t lim_state = 0, lim_old_state = 0;

void set_recovery_home_alt() {

	uint32_t return_altitude_cm_ahl = 0; // in centimeters above home level.
	uint32_t amin_meters_ahl, amax_meters_ahl;

	// for flying vehicles only
	if (altitude_limit.enabled()) {

		amin_meters_ahl = (uint32_t) (altitude_limit.min_alt());
		amax_meters_ahl = (uint32_t) (altitude_limit.max_alt());

		// See if we have a meaningful setting
		if (amax_meters_ahl && ((amax_meters_ahl - amin_meters_ahl) > 1)) {
			// there is a max_alt set
			// set a return altitude that is halfway between the minimum and maximum altitude setting.
			// return_altitude is in centimeters, not meters, so we multiply
			return_altitude_cm_ahl = (uint32_t) (home.alt + (100 * (uint16_t) ((amax_meters_ahl - amin_meters_ahl) / 2)));
		}
	} else {
		return_altitude_cm_ahl = (uint32_t) (home.alt + g.RTL_altitude*100);
	}
	// final sanity check
	// if our return is less than 4 meters from ground, set it to 4m, to clear "people" height.
	if ((return_altitude_cm_ahl - (uint32_t) home.alt) < 400) {
		return_altitude_cm_ahl = home.alt + 400;
	}
	guided_WP.id = 0;
	guided_WP.p1 = 0;
	guided_WP.options = 0;
	guided_WP.lat = home.lat;
	guided_WP.lng = home.lng;
	guided_WP.alt = return_altitude_cm_ahl;
}

static void limits_loop() {

	lim_state = limits.state();

	// Use limits channel to determine LIMITS_ENABLED or LIMITS_DISABLED state
	if (lim_state != LIMITS_DISABLED  && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) < LIMITS_ENABLE_PWM) {
		limits.set_state(LIMITS_DISABLED);
	}
	else if (lim_state == LIMITS_DISABLED && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) >= LIMITS_ENABLE_PWM) {
		limits.set_state(LIMITS_ENABLED);
	}

	if ((uint32_t) millis() - (uint32_t) limits.last_status_update > 1000) { // more than a second has passed - time for an update
		gcs_send_message(MSG_LIMITS_STATUS);
	}

	if (lim_state != lim_old_state) { // state changed
		lim_old_state = lim_state; // we only use lim_oldstate here, for reporting purposes. So, reset it.
		gcs_send_message(MSG_LIMITS_STATUS);

		if (limits.debug()) switch (lim_state) {
			case LIMITS_INIT: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: INIT")); break;
			case LIMITS_DISABLED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: DISABLED")); break;
			case LIMITS_ENABLED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: ENABLED")); break;
			case LIMITS_TRIGGERED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: TRIGGERED")); break;
			case LIMITS_RECOVERING: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: RECOVERING")); break;
			case LIMITS_RECOVERED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: RECOVERED")); break;
			default: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: UNKNOWN")); break;
		}
	}

	switch (limits.state()) {

		// have not initialized yet
		case LIMITS_INIT:
			if (limits.init()) { // initialize system

				// See what the "master" on/off swith is and go to the appropriate start state
				if (!limits.enabled()) {
					limits.set_state(LIMITS_DISABLED);
				}
				else {
					limits.set_state(LIMITS_ENABLED);
				}
			}
			break;

		// We have been switched off
		case LIMITS_DISABLED:

			// check if we have been switched on
			if (limits.enabled()) {
				limits.set_state(LIMITS_ENABLED);
				break;
			}
			break;

		// Limits module is enabled
		case LIMITS_ENABLED:

			// check if we've been switched off
			if (!limits.enabled()) {
				limits.set_state(LIMITS_DISABLED);
				break;
			}

			// Until motors are armed, do nothing, just wait in ENABLED state
			if (!motors.armed()) {

				// we are waiting for motors to arm
				// do nothing
				break;
			}

			bool required_only;

			required_only = (limits.last_clear == 0); // if we haven't yet 'cleared' all limits, check required limits only

			// check if any limits have been breached and trigger if they have
			if  (limits.check_triggered(required_only)) {

				//
				// TRIGGER - BREACH OF LIMITS
				//
				// make a note of which limits triggered, so if we know if we recovered them
				limits.mods_recovering = limits.mods_triggered;

				limits.last_action = 0;
				limits.last_trigger = millis();
				limits.breach_count++;

				limits.set_state(LIMITS_TRIGGERED);
				break;
			}

			if (motors.armed() && limits.enabled() && !limits.mods_triggered) {

				// All clear.
				if (limits.debug()) gcs_send_text(SEVERITY_LOW, "Limits - All Clear");
				limits.last_clear = millis();
			}

			break;

		// Limits have been triggered
		case LIMITS_TRIGGERED:

			// check if we've been switched off
			if (!limits.enabled()) {
				limits.set_state(LIMITS_DISABLED);
				break;
			}

#if LIMITS_TRIGGERED_PIN > 0
            digitalWrite(LIMITS_TRIGGERED_PIN, HIGH);
#endif

			if (limits.debug()) {
				if (limits.mods_triggered & LIMIT_GPSLOCK) gcs_send_text(SEVERITY_LOW, "!GPSLock");
				if (limits.mods_triggered & LIMIT_GEOFENCE) gcs_send_text(SEVERITY_LOW, "!Geofence");
				if (limits.mods_triggered & LIMIT_ALTITUDE) gcs_send_text(SEVERITY_LOW, "!Altitude");
			}

			// If the motors are not armed, we have triggered pre-arm checks. Do nothing
			if (motors.armed() == false) {
				limits.set_state(LIMITS_ENABLED); // go back to checking limits
				break;
			}

			// If we are triggered but no longer in breach, that means we recovered
			// somehow, via auto recovery or pilot action
			if (!limits.check_all()) {
				limits.last_recovery = millis();
				limits.set_state(LIMITS_RECOVERED);
				break;
			}
			else {
				limits.set_state(LIMITS_RECOVERING);
				limits.last_action = 0; // reset timer
				// We are about to take action on a real breach. Make sure we notify immediately
			    gcs_send_message(MSG_LIMITS_STATUS);
				break;
			}
			break;

		// Take action to recover
		case LIMITS_RECOVERING:
			// If the motors are not armed, we have triggered pre-arm checks. Do nothing
			if (motors.armed() == false) {
				limits.set_state(LIMITS_ENABLED); // go back to checking limits
				break;
			}

			// check if we've been switched off
			if (!limits.enabled() && limits.old_mode_switch == oldSwitchPosition) {
				limits.old_mode_switch = 0;
	            reset_control_switch();
				limits.set_state(LIMITS_DISABLED);
				break;
			}

			// Still need action?
			if (limits.check_all() == 0) { // all triggers clear
				limits.set_state(LIMITS_RECOVERED);
				break;
			}

			if (limits.mods_triggered != limits.mods_recovering)  { // if any *new* triggers, hit the trigger again
				//
				// TRIGGER - BREACH OF LIMITS
				//
				// make a note of which limits triggered, so if we know if we recovered them
				limits.mods_recovering = limits.mods_triggered;

				limits.last_action = 0;
				limits.last_trigger = millis();
				limits.breach_count++;

				limits.set_state(LIMITS_TRIGGERED);
				limits.set_state(LIMITS_TRIGGERED);
				break;
			}

			// Recovery Action
			// if there was no previous action, take action, take note of time  send GCS.
			if (limits.last_action == 0) {

				// save mode switch
				limits.old_mode_switch = oldSwitchPosition;


//				// Take action
//				// This ensures no "radical" RTL, like a full throttle take-off,happens if something triggers at ground level
//				if ((uint32_t) current_loc.alt < ((uint32_t)home.alt * 200) ) { // we're under 2m (200cm), already at "people" height or on the ground
//					if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action: near ground - do nothing"));
//					// TODO: Will this work for a plane? Does it make sense in general?
//
//					//set_mode(LAND);
//					limits.last_action = millis(); // start counter
//				    gcs_send_message(MSG_LIMITS_STATUS);
//
//					break;
//				}


				// TODO: This applies only to planes - hold for porting
//					if (control_mode == MANUAL && g.auto_trim) {
//					            // make sure we don't auto trim the surfaces on this change
//					            control_mode = STABILIZE;
//					}


				switch (limits.recmode()) {

				case 0:			// RTL mode

					if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action - RTL"));

					set_mode(RTL);
					limits.last_action = millis();
					gcs_send_message(MSG_LIMITS_STATUS);
					break;

				case 1:			// Bounce mode

					if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action - bounce mode, POSITION"));
					// ALT_HOLD gives us yaw hold, roll& pitch hold and throttle hold.
					// It is like position hold, but without manual throttle control.

					//set_recovery_home_alt();
					set_mode(POSITION);
					throttle_mode = THROTTLE_AUTO;
					limits.last_action = millis();
					gcs_send_message(MSG_LIMITS_STATUS);
					break;

				}
				break;
			}


			// In bounce mode, take control for 3 seconds, and then wait for the pilot to make us "safe".
			// If the vehicle does not recover, the escalation action will trigger.
			if (limits.recmode() == 1) {

					if (control_mode == POSITION && ((uint32_t)millis() - (uint32_t)limits.last_action) > 3000) {
						if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Recovery Bounce: Returning control to pilot"));
						set_mode(STABILIZE);
					} else if (control_mode == STABILIZE && ((uint32_t)millis() - (uint32_t)limits.last_action) > 6000) {
						// after 3 more seconds, reset action counter to take action again
						limits.last_action = 0;
					}
			}

			// ESCALATE We have not recovered after 2 minutes of recovery action

			if (((uint32_t)millis() - (uint32_t)limits.last_action) > 120000 ) {

				// TODO: Secondary recovery
				if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Recovery Escalation: RTL"));
				set_mode(RTL);
				limits.last_action = millis();
				break;
			}
			break;

		// Have recovered, relinquish control and re-enable
		case LIMITS_RECOVERED:


			// check if we've been switched off
			if (!limits.enabled()) {
				limits.set_state(LIMITS_DISABLED);
				break;
			}

#if LIMITS_TRIGGERED_PIN > 0
            digitalWrite(LIMITS_TRIGGERED_PIN, LOW);
#endif

			// Reset action counter
			limits.last_action = 0;

			if (((uint32_t)millis() - (uint32_t)limits.last_recovery) > (uint32_t)(limits.safetime() * 1000)) { // Wait "safetime" seconds of recovery before we give back control

				// Our recovery action worked.
				limits.set_state(LIMITS_ENABLED);

				// Switch to stabilize
				if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - Returning controls"));
				set_mode(STABILIZE);				limits.last_recovery = millis();

				break;
			}
			break;

		default:
			if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits: unknown state"));
			break;
	}
}

// This function below, should really be in the AP_Limits class, but it is impossible to untangle the mavlink includes.

void limits_send_mavlink_status(mavlink_channel_t chan) {

	limits.last_status_update = millis();

	if (limits.enabled()) {
		mavlink_msg_limits_status_send(chan,
				(uint8_t) limits.state(),
				(uint32_t) limits.last_trigger,
				(uint32_t) limits.last_action,
				(uint32_t) limits.last_recovery,
				(uint32_t) limits.last_clear,
				(uint16_t) limits.breach_count,
				(LimitModuleBits) limits.mods_enabled,
				(LimitModuleBits) limits.mods_required,
				(LimitModuleBits) limits.mods_triggered);
	}
}

#endif