// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
#if ADVANCED_FAILSAFE == ENABLED
    // this is to allow the failsafe module to deliberately crash 
    // the vehicle. Only used in extreme circumstances to meet the
    // OBC rules
    if (g2.afs.should_crash_vehicle()) {
        g2.afs.terminate_vehicle();
        return;
    }
#endif

    // Update arming delay state
    if (ap.in_arming_delay && (!motors.armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
        ap.in_arming_delay = false;
    }

    // check if we are performing the motor test
    if (ap.motor_test) {
        motor_test_output();
    } else {
        bool interlock = motors.armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop;
        if (!motors.get_interlock() && interlock) {
            motors.set_interlock(true);
            Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
        } else if (motors.get_interlock() && !interlock) {
            motors.set_interlock(false);
            Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
        }

        // send output signals to motors
        motors.output();
    }
}

// Run landing gear controller at 10Hz
void Copter::landinggear_update()
{
    // exit immediately if no landing gear output has been enabled
    if (!SRV_Channels::function_assigned(SRV_Channel::k_landing_gear_control)) {
        return;
    }

    // last status (deployed or retracted) used to check for changes, initialised to startup state of landing gear
    static bool last_deploy_status = landinggear.deployed();

    // if we are doing an automatic landing procedure, force the landing gear to deploy.
    // To-Do: should we pause the auto-land procedure to give time for gear to come down?
    if (control_mode == LAND ||
       (control_mode == RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) ||
       (control_mode == AUTO && auto_mode == Auto_Land) ||
       (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) {
        landinggear.set_position(AP_LandingGear::LandingGear_Deploy_And_Keep_Deployed);
    }

    // send event message to datalog if status has changed
    if (landinggear.deployed() != last_deploy_status) {
        if (landinggear.deployed()) {
            Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED);
        } else {
            Log_Write_Event(DATA_LANDING_GEAR_RETRACTED);
        }
    }

    last_deploy_status = landinggear.deployed();
}

// Run landing gear controller at 10Hz
void Sub::landinggear_update(){

    // If landing gear control is active, run update function.
    if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){

        // last status (deployed or retracted) used to check for changes
        static bool last_deploy_status;

        // if we are doing an automatic landing procedure, force the landing gear to deploy.
        // To-Do: should we pause the auto-land procedure to give time for gear to come down?
        if (control_mode == LAND ||
           (control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) ||
           (control_mode == AUTO && auto_mode == Auto_Land) ||
           (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) {
            landinggear.force_deploy(true);
        }

        landinggear.update();

        // send event message to datalog if status has changed
        if (landinggear.deployed() != last_deploy_status){
            if (landinggear.deployed()) {
                Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED);
            } else {
                Log_Write_Event(DATA_LANDING_GEAR_RETRACTED);
            }
        }

        last_deploy_status = landinggear.deployed();        
    }
}

void Copter::set_motor_emergency_stop(bool b)
{
    if(ap.motor_emergency_stop != b) {
        ap.motor_emergency_stop = b;
    }

    // Log new status
    if (ap.motor_emergency_stop){
        Log_Write_Event(DATA_MOTORS_EMERGENCY_STOPPED);
    } else {
        Log_Write_Event(DATA_MOTORS_EMERGENCY_STOP_CLEARED);
    }
}

// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
#if ADVANCED_FAILSAFE == ENABLED
    // this is to allow the failsafe module to deliberately crash
    // the vehicle. Only used in extreme circumstances to meet the
    // OBC rules
    if (g2.afs.should_crash_vehicle()) {
        g2.afs.terminate_vehicle();
        if (!g2.afs.terminating_vehicle_via_landing()) {
            return;
        }
        // landing must continue to run the motors output
    }
#endif

    // Update arming delay state
    if (ap.in_arming_delay && (!motors->armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
        ap.in_arming_delay = false;
    }

    // output any servo channels
    SRV_Channels::calc_pwm();

    // cork now, so that all channel outputs happen at once
    SRV_Channels::cork();

    // update output on any aux channels, for manual passthru
    SRV_Channels::output_ch_all();

    // check if we are performing the motor test
    if (ap.motor_test) {
        motor_test_output();
    } else {
        bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop();
        if (!motors->get_interlock() && interlock) {
            motors->set_interlock(true);
            Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
        } else if (motors->get_interlock() && !interlock) {
            motors->set_interlock(false);
            Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
        }

        // send output signals to motors
        motors->output();
    }

    // push all channels
    SRV_Channels::push();
}

void Sub::set_surfaced(bool at_surface) {
	if(ap.at_surface == at_surface) { // do nothing if state unchanged
		return;
	}

	ap.at_surface = at_surface;

	if(!ap.at_surface) {
		surface_detector_count = 0;
	    Log_Write_Event(DATA_SURFACED);
		gcs_send_text(MAV_SEVERITY_INFO, "Off Surface");
	} else {
		Log_Write_Event(DATA_NOT_SURFACED);
		gcs_send_text(MAV_SEVERITY_INFO, "Surfaced");
	}
}

void Sub::set_bottomed(bool at_bottom)
{

    if (ap.at_bottom == at_bottom) { // do nothing if state unchanged
        return;
    }

    ap.at_bottom = at_bottom;

    bottom_detector_count = 0;

    if (ap.at_bottom) {
        Log_Write_Event(DATA_BOTTOMED);
    } else {
        Log_Write_Event(DATA_NOT_BOTTOMED);
    }
}

// do_gripper - control gripper
void Copter::do_gripper(const AP_Mission::Mission_Command& cmd)
{
    // Note: we ignore the gripper num parameter because we only support one gripper
    switch (cmd.content.gripper.action) {
        case GRIPPER_ACTION_RELEASE:
            g2.gripper.release();
            Log_Write_Event(DATA_GRIPPER_RELEASE);
            break;
        case GRIPPER_ACTION_GRAB:
            g2.gripper.grab();
            Log_Write_Event(DATA_GRIPPER_GRAB);
            break;
        default:
            // do nothing
            break;
    }
}

// flip_init - initialise flip controller
bool Copter::flip_init(bool ignore_checks)
{

#if FRAME_CONFIG == HELI_FRAME
    // Flip mode not available for helis as it is untested.
    return false;
#endif

    // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
    if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) {
        return false;
    }

    // if in acro or stabilize ensure throttle is above zero
    if (ap.throttle_zero && (control_mode == ACRO || control_mode == STABILIZE)) {
        return false;
    }

    // ensure roll input is less than 40deg
    if (abs(channel_roll->control_in) >= 4000) {
        return false;
    }

    // only allow flip when flying
    if (!motors.armed() || ap.land_complete) {
        return false;
    }

    // capture original flight mode so that we can return to it after completion
    flip_orig_control_mode = control_mode;

    // initialise state
    flip_state = Flip_Start;
    flip_start_time = millis();

    flip_roll_dir = flip_pitch_dir = 0;

    // choose direction based on pilot's roll and pitch sticks
    if (channel_pitch->control_in > 300) {
        flip_pitch_dir = FLIP_PITCH_BACK;
    }else if(channel_pitch->control_in < -300) {
        flip_pitch_dir = FLIP_PITCH_FORWARD;
    }else if (channel_roll->control_in >= 0) {
        flip_roll_dir = FLIP_ROLL_RIGHT;
    }else{
        flip_roll_dir = FLIP_ROLL_LEFT;
    }

    // log start of flip
    Log_Write_Event(DATA_FLIP_START);

    // capture current attitude which will be used during the Flip_Recovery stage
    flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -aparm.angle_max, aparm.angle_max);
    flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -aparm.angle_max, aparm.angle_max);
    flip_orig_attitude.z = ahrs.yaw_sensor;

    return true;
}

// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
void Copter::heli_update_rotor_speed_targets()
{

    static bool rotor_runup_complete_last = false;

    // get rotor control method
    uint8_t rsc_control_mode = motors.get_rsc_mode();

    rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in);


    switch (rsc_control_mode) {
        case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH:
            // pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom
            if (rsc_control_deglitched > 10) {
                motors.set_interlock(true);
                motors.set_desired_rotor_speed(rsc_control_deglitched);
            } else {
                motors.set_interlock(false);
                motors.set_desired_rotor_speed(0);
            }
            break;
        case ROTOR_CONTROL_MODE_SPEED_SETPOINT:
        case ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT:
        case ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT:
            // pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode
            // other than being used to create a crude estimate of rotor speed
            if (rsc_control_deglitched > 0) {
                motors.set_interlock(true);
                motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
            }else{
                motors.set_interlock(false);
                motors.set_desired_rotor_speed(0);
            }
            break;
    }

    // when rotor_runup_complete changes to true, log event
    if (!rotor_runup_complete_last && motors.rotor_runup_complete()){
        Log_Write_Event(DATA_ROTOR_RUNUP_COMPLETE);
    } else if (rotor_runup_complete_last && !motors.rotor_runup_complete()){
        Log_Write_Event(DATA_ROTOR_SPEED_BELOW_CRITICAL);
    }
    rotor_runup_complete_last = motors.rotor_runup_complete();
}

// land_nogps_run - runs the land controller
//      pilot controls roll and pitch angles
//      should be called at 100hz or more
void Copter::ModeLand::nogps_run()
{
    float target_roll = 0.0f, target_pitch = 0.0f;
    float target_yaw_rate = 0;

    // process pilot inputs
    if (!copter.failsafe.radio) {
        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
            // exit land if throttle is high
            copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
        }

        if (g.land_repositioning) {
            // apply SIMPLE mode transform to pilot inputs
            update_simple_mode();

            // get pilot desired lean angles
            get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
        }

        // get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
    }

    // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
    if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
        // call attitude controller
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
        attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
        motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
        // multicopters do not stabilize roll/pitch/yaw when disarmed
        attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif

        // disarm when the landing detector says we've landed
        if (ap.land_complete) {
            copter.init_disarm_motors();
        }
        return;
    }

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // call attitude controller
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

    // pause before beginning land descent
    if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
        land_pause = false;
    }

    land_run_vertical_control(land_pause);
}

// save_trim - adds roll and pitch trims from the radio to ahrs
void Copter::save_trim()
{
    // save roll and pitch trim
    float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f);
    float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f);
    ahrs.add_trim(roll_trim, pitch_trim);
    Log_Write_Event(DATA_SAVE_TRIM);
    gcs_send_text(MAV_SEVERITY_INFO, "Trim saved");
}

// save_trim - adds roll and pitch trims from the radio to ahrs
void Copter::save_trim()
{
    // save roll and pitch trim
    float roll_trim = ToRad((float)channel_roll->control_in/100.0f);
    float pitch_trim = ToRad((float)channel_pitch->control_in/100.0f);
    ahrs.add_trim(roll_trim, pitch_trim);
    Log_Write_Event(DATA_SAVE_TRIM);
    gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Trim saved"));
}

// ---------------------------------------------
void Copter::set_simple_mode(uint8_t b)
{
    if(ap.simple_mode != b) {
        if(b == 0) {
            Log_Write_Event(DATA_SET_SIMPLE_OFF);
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode off");
        } else if(b == 1) {
            Log_Write_Event(DATA_SET_SIMPLE_ON);
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode on");
        } else {
            // initialise super simple heading
            update_super_simple_bearing(true);
            Log_Write_Event(DATA_SET_SUPERSIMPLE_ON);
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
        }
        ap.simple_mode = b;
    }
}

// set_home - sets ahrs home (used for RTL) to specified location
//  initialises inertial nav and compass on first call
//  returns true if home location set successfully
bool Copter::set_home(const Location& loc, bool lock)
{
    // check location is valid
    if (loc.lat == 0 && loc.lng == 0) {
        return false;
    }

    // check EKF origin has been set
    Location ekf_origin;
    if (!ahrs.get_origin(ekf_origin)) {
        return false;
    }

    // check home is close to EKF origin
    if (far_from_EKF_origin(loc)) {
        return false;
    }

    const bool home_was_set = ahrs.home_is_set();

    // set ahrs home (used for RTL)
    ahrs.set_home(loc);

    // init inav and compass declination
    if (!home_was_set) {
        // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
        scaleLongDown = longitude_scale(loc);
        // record home is set
        Log_Write_Event(DATA_SET_HOME);

#if MODE_AUTO_ENABLED == ENABLED
        // log new home position which mission library will pull from ahrs
        if (should_log(MASK_LOG_CMD)) {
            AP_Mission::Mission_Command temp_cmd;
            if (mission.read_cmd_from_storage(0, temp_cmd)) {
                DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
            }
        }
#endif
    }

    // lock home position
    if (lock) {
        ahrs.lock_home();
    }

    // log ahrs home and ekf origin dataflash
    ahrs.Log_Write_Home_And_Origin();

    // send new home and ekf origin to GCS
    gcs().send_home();
    gcs().send_ekf_origin();

    // return success
    return true;
}

// flip_init - initialise flip controller
bool Copter::ModeFlip::init(bool ignore_checks)
{
    // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
    if (copter.control_mode != ACRO &&
        copter.control_mode != STABILIZE &&
        copter.control_mode != ALT_HOLD &&
        copter.control_mode != FLOWHOLD) {
        return false;
    }

    // if in acro or stabilize ensure throttle is above zero
    if (ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) {
        return false;
    }

    // ensure roll input is less than 40deg
    if (abs(channel_roll->get_control_in()) >= 4000) {
        return false;
    }

    // only allow flip when flying
    if (!motors->armed() || ap.land_complete) {
        return false;
    }

    // capture original flight mode so that we can return to it after completion
    orig_control_mode = copter.control_mode;

    // initialise state
    _state = FlipState::Start;
    start_time_ms = millis();

    roll_dir = pitch_dir = 0;

    // choose direction based on pilot's roll and pitch sticks
    if (channel_pitch->get_control_in() > 300) {
        pitch_dir = FLIP_PITCH_BACK;
    } else if (channel_pitch->get_control_in() < -300) {
        pitch_dir = FLIP_PITCH_FORWARD;
    } else if (channel_roll->get_control_in() >= 0) {
        roll_dir = FLIP_ROLL_RIGHT;
    } else {
        roll_dir = FLIP_ROLL_LEFT;
    }

    // log start of flip
    Log_Write_Event(DATA_FLIP_START);

    // capture current attitude which will be used during the FlipState::Recovery stage
    const float angle_max = copter.aparm.angle_max;
    orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max);
    orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max);
    orig_attitude.z = ahrs.yaw_sensor;

    return true;
}

// check for ekf yaw reset and adjust target heading
void Copter::check_ekf_yaw_reset()
{
    float yaw_angle_change_rad = 0.0f;
    uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
    if (new_ekfYawReset_ms != ekfYawReset_ms) {
        attitude_control.shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);
        ekfYawReset_ms = new_ekfYawReset_ms;
        Log_Write_Event(DATA_EKF_YAW_RESET);
    }
}

// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
void Copter::parachute_release()
{
    // send message to gcs and dataflash
    gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released");
    Log_Write_Event(DATA_PARACHUTE_RELEASED);

    // disarm motors
    init_disarm_motors();

    // release parachute
    parachute.release();
}

// control winch based on mission command
void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
{
    // Note: we ignore the gripper num parameter because we only support one gripper
    switch (cmd.content.winch.action) {
        case WINCH_RELAXED:
            g2.winch.relax();
            Log_Write_Event(DATA_WINCH_RELAXED);
            break;
        case WINCH_RELATIVE_LENGTH_CONTROL:
            g2.winch.release_length(cmd.content.winch.release_length, cmd.content.winch.release_rate);
            Log_Write_Event(DATA_WINCH_LENGTH_CONTROL);
            break;
        case WINCH_RATE_CONTROL:
            g2.winch.set_desired_rate(cmd.content.winch.release_rate);
            Log_Write_Event(DATA_WINCH_RATE_CONTROL);
            break;
        default:
            // do nothing
            break;
    }
}

// ---------------------------------------------
void Copter::set_simple_mode(uint8_t b)
{
    if(ap.simple_mode != b){
        switch (b) {
        case 0:
            Log_Write_Event(DATA_SET_SIMPLE_OFF);
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode off");
            break;
        case 1:
            Log_Write_Event(DATA_SET_SIMPLE_ON);
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode on");
            break;
        default:
            // initialise super simple heading
            update_super_simple_bearing(true);
            Log_Write_Event(DATA_SET_SUPERSIMPLE_ON);
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
            break;
        }
        ap.simple_mode = b;
    }
}