// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

/*
 *       This event will be called when the failsafe changes
 *       boolean failsafe reflects the current state
 */
static void failsafe_on_event()
{
    // This is how to handle a failsafe.
    switch(control_mode) {
        case STABILIZE:
        case ACRO:
            // if throttle is zero disarm motors
            if (g.rc_3.control_in == 0) {
                init_disarm_motors();
            }else if(ap.home_is_set == true) {
                set_mode(RTL);
            }else{
                // We have no GPS so we will crash land in alt hold mode
                // the low throttle will bring us down at the maximum
                // the low throttle will bring us down at the maximum descent speed
                // To-Do: make LAND a throttle mode so it can operate without GPS
                set_mode(ALT_HOLD);
                set_new_altitude(0);
            }
            break;
        case AUTO:
            // throttle_fs_action is 1 do RTL, everything else means continue with the mission
            if (g.throttle_fs_action == THROTTLE_FAILSAFE_ACTION_ALWAYS_RTL) {
                set_mode(RTL);
            }
            // if throttle_fs_action is 2 (i.e. THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION) no need to do anything
            break;
        default:
            if(ap.home_is_set == true) {
                // same as above ^
                // do_rtl sets the altitude to the current altitude by default
                set_mode(RTL);
            }else{
                // We have no GPS so we will crash land in alt hold mode
                // the low throttle will bring us down at the maximum descent speed
                // To-Do: make LAND a throttle mode so it can operate without GPS
                set_mode(ALT_HOLD);
                set_new_altitude(0);
            }
            break;
    }
}

// failsafe_off_event - respond to radio contact being regained
// we must be in AUTO, LAND or RTL modes
// or Stabilize or ACRO mode but with motors disarmed
static void failsafe_off_event()
{
    // no need to do anything
    // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
}

static void low_battery_event(void)
{
    static uint32_t last_low_battery_message;
    uint32_t tnow = millis();
    if (((uint32_t)(tnow - last_low_battery_message)) > 5000) {
        // only send this message at 5s intervals at most or we may
        // flood the link
        gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!"));
        last_low_battery_message = tnow;
    }

    set_low_battery(true);
    // if we are in Auto mode, come home
    if(control_mode >= AUTO)
        set_mode(RTL);
}


static void update_events()     // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
    if(event_repeat == 0 || (millis() - event_timer) < event_delay)
        return;

    if (event_repeat > 0) {
        event_repeat--;
    }

    if(event_repeat != 0) {             // event_repeat = -1 means repeat forever
        event_timer = millis();

        if (event_id >= CH_5 && event_id <= CH_8) {
            if(event_repeat%2) {
                APM_RC.OutputCh(event_id, event_value);                 // send to Servos
            } else {
                APM_RC.OutputCh(event_id, event_undo_value);
            }
        }

        if  (event_id == RELAY_TOGGLE) {
            relay.toggle();
        }
    }
}