#include "Blimp.h"

/*
 * High level calls to set and update flight modes logic for individual
 * flight modes is in control_acro.cpp, control_stabilize.cpp, etc
 */

/*
  constructor for Mode object
 */
Mode::Mode(void) :
    g(blimp.g),
    g2(blimp.g2),
    // wp_nav(blimp.wp_nav),
    // loiter_nav(blimp.loiter_nav),
    // pos_control(blimp.pos_control),
    inertial_nav(blimp.inertial_nav),
    ahrs(blimp.ahrs),
    // attitude_control(blimp.attitude_control),
    motors(blimp.motors),
    channel_right(blimp.channel_right),
    channel_front(blimp.channel_front),
    channel_down(blimp.channel_down),
    channel_yaw(blimp.channel_yaw),
    G_Dt(blimp.G_Dt)
{ };

// return the static controller object corresponding to supplied mode
Mode *Blimp::mode_from_mode_num(const Mode::Number mode)
{
    Mode *ret = nullptr;

    switch (mode) {
    case Mode::Number::MANUAL:
        ret = &mode_manual;
        break;
    case Mode::Number::LAND:
        ret = &mode_land;
        break;
    default:
        break;
    }

    return ret;
}


// set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was successfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool Blimp::set_mode(Mode::Number mode, ModeReason reason)
{

    // return immediately if we are already in the desired mode
    if (mode == control_mode) {
        control_mode_reason = reason;
        return true;
    }

    Mode *new_flightmode = mode_from_mode_num((Mode::Number)mode);
    if (new_flightmode == nullptr) {
        gcs().send_text(MAV_SEVERITY_WARNING,"No such mode");
        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
        return false;
    }

    bool ignore_checks = !motors->armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform

    // ensure vehicle doesn't leap off the ground if a user switches
    // into a manual throttle mode from a non-manual-throttle mode
    // (e.g. user arms in guided, raises throttle to 1300 (not enough to
    // trigger auto takeoff), then switches into manual):
    bool user_throttle = new_flightmode->has_manual_throttle();
    if (!ignore_checks &&
        ap.land_complete &&
        user_throttle &&
        !blimp.flightmode->has_manual_throttle() &&
        new_flightmode->get_pilot_desired_throttle() > blimp.get_non_takeoff_throttle()) {
        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: throttle too high");
        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
        return false;
    }

    if (!ignore_checks &&
        new_flightmode->requires_GPS() &&
        !blimp.position_ok()) {
        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name());
        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
        return false;
    }

    // check for valid altitude if old mode did not require it but new one does
    // we only want to stop changing modes if it could make things worse
    if (!ignore_checks &&
        !blimp.ekf_alt_ok() &&
        flightmode->has_manual_throttle() &&
        !new_flightmode->has_manual_throttle()) {
        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name());
        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
        return false;
    }

    if (!new_flightmode->init(ignore_checks)) {
        gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name());
        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
        return false;
    }

    // perform any cleanup required by previous flight mode
    exit_mode(flightmode, new_flightmode);

    // store previous flight mode (only used by tradeheli's autorotation)
    prev_control_mode = control_mode;

    // update flight mode
    flightmode = new_flightmode;
    control_mode = mode;
    control_mode_reason = reason;
    logger.Write_Mode((uint8_t)control_mode, reason);
    gcs().send_message(MSG_HEARTBEAT);

    // update notify object
    notify_flight_mode();

    // return success
    return true;
}

bool Blimp::set_mode(const uint8_t new_mode, const ModeReason reason)
{
    static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
    if (reason == ModeReason::GCS_COMMAND && blimp.failsafe.radio) {
        // don't allow mode changes while in radio failsafe
        return false;
    }
#endif
    return blimp.set_mode(static_cast<Mode::Number>(new_mode), reason);
}

// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
void Blimp::update_flight_mode()
{
    // surface_tracking.invalidate_for_logging();  // invalidate surface tracking alt, flight mode will set to true if used

    flightmode->run();
}

// exit_mode - high level call to organise cleanup as a flight mode is exited
void Blimp::exit_mode(Mode *&old_flightmode,
                      Mode *&new_flightmode)
{

    // smooth throttle transition when switching from manual to automatic flight modes
    if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && motors->armed() && !ap.land_complete) {
        // this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
        // set_accel_throttle_I_from_pilot_throttle();
    }

    // cancel any takeoffs in progress
    // old_flightmode->takeoff_stop();
}

// notify_flight_mode - sets notify object based on current flight mode.  Only used for OreoLED notify device
void Blimp::notify_flight_mode()
{
    AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();
    AP_Notify::flags.flight_mode = (uint8_t)control_mode;
    notify.set_flight_mode_str(flightmode->name4());
}

void Mode::update_navigation()
{
    // run autopilot to make high level decisions about control modes
    run_autopilot();
}

// returns desired angle in centi-degrees
void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) const
{
    // throttle failsafe check
    if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) {
        right_out = 0;
        front_out = 0;
        return;
    }
    // fetch roll and pitch inputs
    right_out = channel_right->get_control_in();
    front_out = channel_front->get_control_in();


    // // do circular limit
    // float total_in = norm(pitch_out, roll_out);
    // if (total_in > angle_limit) {
    //     float ratio = angle_limit / total_in;
    //     roll_out *= ratio;
    //     pitch_out *= ratio;
    // }

    // do lateral tilt to euler roll conversion
    // roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000)));

    // roll_out and pitch_out are returned
}

// bool Mode::_TakeOff::triggered(const float target_climb_rate) const
// {
//     if (!blimp.ap.land_complete) {
//         // can't take off if we're already flying
//         return false;
//     }
//     if (target_climb_rate <= 0.0f) {
//         // can't takeoff unless we want to go up...
//         return false;
//     }

//     if (blimp.motors->get_spool_state() != Fins::SpoolState::THROTTLE_UNLIMITED) {
//         // hold aircraft on the ground until rotor speed runup has finished
//         return false;
//     }

//     return true;
// }

bool Mode::is_disarmed_or_landed() const
{
    if (!motors->armed() || !blimp.ap.auto_armed || blimp.ap.land_complete) {
        return true;
    }
    return false;
}

void Mode::zero_throttle_and_relax_ac(bool spool_up)
{
    if (spool_up) {
        motors->set_desired_spool_state(Fins::DesiredSpoolState::THROTTLE_UNLIMITED);
    } else {
        motors->set_desired_spool_state(Fins::DesiredSpoolState::SHUT_DOWN);
    }
}

// void Mode::zero_throttle_and_hold_attitude()
// {
//     // run attitude controller
//     attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
//     attitude_control->set_throttle_out(0.0f, false, blimp.g.throttle_filt);
// }

// void Mode::make_safe_spool_down()
// {
//     // command aircraft to initiate the shutdown process
//     motors->set_desired_spool_state(Fins::DesiredSpoolState::GROUND_IDLE);
//     switch (motors->get_spool_state()) {

//     case Fins::SpoolState::SHUT_DOWN:
//     case Fins::SpoolState::GROUND_IDLE:
//         // relax controllers during idle states
//         // attitude_control->reset_rate_controller_I_terms_smoothly();
//         // attitude_control->set_yaw_target_to_current_heading();
//         break;

//     case Fins::SpoolState::SPOOLING_UP:
//     case Fins::SpoolState::THROTTLE_UNLIMITED:
//     case Fins::SpoolState::SPOOLING_DOWN:
//         // while transitioning though active states continue to operate normally
//         break;
//     }

//     // pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
//     // pos_control->update_z_controller();
//     // we may need to move this out
//     // attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
// }

/*
  get a height above ground estimate for landing
 */
int32_t Mode::get_alt_above_ground_cm(void)
{
    int32_t alt_above_ground_cm;

    if (blimp.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) {
        return alt_above_ground_cm;
    }

    // Assume the Earth is flat:
    return blimp.current_loc.alt;
}

float Mode::throttle_hover() const
{
    return motors->get_throttle_hover();
}

// transform pilot's manual throttle input to make hover throttle mid stick
// used only for manual throttle modes
// thr_mid should be in the range 0 to 1
// returns throttle output 0 to 1
float Mode::get_pilot_desired_throttle() const
{
    const float thr_mid = throttle_hover();
    int16_t throttle_control = channel_down->get_control_in();

    int16_t mid_stick = blimp.get_throttle_mid();
    // protect against unlikely divide by zero
    if (mid_stick <= 0) {
        mid_stick = 500;
    }

    // ensure reasonable throttle values
    throttle_control = constrain_int16(throttle_control,0,1000);

    // calculate normalised throttle input
    float throttle_in;
    if (throttle_control < mid_stick) {
        throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
    } else {
        throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
    }

    const float expo = constrain_float(-(thr_mid-0.5f)/0.375f, -0.5f, 1.0f);
    // calculate the output throttle using the given expo function
    float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
    return throttle_out;
}

// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
float Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
    return blimp.get_pilot_desired_yaw_rate(stick_angle);
}

float Mode::get_pilot_desired_climb_rate(float throttle_control)
{
    return blimp.get_pilot_desired_climb_rate(throttle_control);
}

float Mode::get_non_takeoff_throttle()
{
    return blimp.get_non_takeoff_throttle();
}

bool Mode::set_mode(Mode::Number mode, ModeReason reason)
{
    return blimp.set_mode(mode, reason);
}

void Mode::set_land_complete(bool b)
{
    return blimp.set_land_complete(b);
}

GCS_Blimp &Mode::gcs()
{
    return blimp.gcs();
}

uint16_t Mode::get_pilot_speed_dn()
{
    return blimp.get_pilot_speed_dn();
}