#include "flight_mode.h"
#include "auto_pilot.h"
#include "control_rate.h"
#include "control_throttle.h"
#include "mode_attitude.h"
#include "mode_gcs_tax_launch_run.h"
#include "mode_vehicle_land.h"
#include "mode_vehicle_launch.h"
#include "params.h"
#include "soft_flash.h"
#include "soft_gps.h"
#include "soft_gs.h"
#include "soft_imu.h"
#include "soft_motor_output.h"
#include "soft_rc_input.h"
#include "soft_time.h"
#include "soft_voltage.h"

unsigned char control_mode = CONTROL_GCS;
unsigned char flight_mode = MODE_DEFAULT;
unsigned char flight_mode_flag = MODE_DEFAULT;

// @brief 飞行模式切换
void flight_mode_switch(void)
{
    static short gcs_remote_switch = 1700;
    //=============== 如果是遥控器控制	====================
    if (control_mode == CONTROL_REMOTE)
    {
        if (comp_rc_status == COMP_NORMAL &&
            rc_signal_health == RC_SIGNAL_HEALTH)
        {
            // 姿态模式
            if (rc_in[RC_CH5] >= MID_S && rc_in[RC_CH5] <= MAX_E)
            {
                control_mode = CONTROL_REMOTE;
                set_flight_mode(ATTITUDE);
            }
            // GPS模式
            else if (rc_in[RC_CH5] >= MIN_S && rc_in[RC_CH5] <= MIN_E)
            {
                if (ins.insgps_ok == INSGPS)
                {
                    control_mode = CONTROL_GCS;
                    flight_mode_flag = GCS_VEHICLE_LAUNCH;

                    set_flight_mode(flight_mode_flag);
                     __mode_state = __STATE_LAUNCH_GO;
                     
                     pid_m_alt.thr_hold == false;
                     althold_state = NO_ALT_HOLD;
                     record_target_alt = pid_m_alt.loc_c; // 需要修改
                }
            }
        }
        else
        {
            if (ground_air_status == ON_GROUND)
            {
                // 避免遥控器解锁然后接收机受干扰后飞机一直怠速转无法停转。
                if (thr_lock_status == UNLOCKED)
                {
                    set_thr_lock_status(LOCKED, RCBADLOCK);
                }
            }
        }
    }
    // 如果是地面站控制
    else if (control_mode == CONTROL_GCS)
    {
        set_flight_mode(flight_mode_flag);

        // RC开并且RC状态健康
        if (comp_rc_status == COMP_NORMAL &&
            rc_signal_health == RC_SIGNAL_HEALTH)
        {
            // 地面站模式下  从GPS模式切到姿态模式时  遥控器重新获取控制权
            if (rc_in[RC_CH5] >= MIN_S && rc_in[RC_CH5] <= MIN_E)
            {
                gcs_remote_switch = rc_in[RC_CH5];
            }
            else if ((rc_in[RC_CH5] >= MAX_S && rc_in[RC_CH5] <= MAX_E) &&
                     (gcs_remote_switch >= MIN_S && gcs_remote_switch <= MIN_E))
            {
                gcs_remote_switch = rc_in[RC_CH5];
                control_mode = CONTROL_REMOTE;
                set_flight_mode(ATTITUDE);
            }
        }
    }
}


// 电压低标志位
bool battary_volt_islow = false;

// 托盘摇杆是否有效
comp_status rock_isenable = COMP_NOEXIST;



// @brief 模式退出
void exit_cur_mode(unsigned char cur_mode, unsigned char new_mode)
{
    switch (cur_mode)
    {
    case ATTITUDE:
        attitude_exit();
        break;
    case GCS_VEHICLE_LAUNCH:
        gcs_tax_launch_exit();
        break;
    default:
        break;
    }
}

// @brief 设置飞行模式
bool set_flight_mode(unsigned char new_mode)
{
    bool success = false;

    if (new_mode == flight_mode)
    {
        return true;
    }

    switch (new_mode)
    {
    case ATTITUDE:
        success = attitude_init();
        break;
    case GCS_VEHICLE_LAUNCH:
        success = gcs_tax_launch_init();
        break;
    default:
        success = false;
        break;
    }

    if (success)
    {
        exit_cur_mode(flight_mode, new_mode);
        flight_mode = new_mode;
    }

    return success;
}

// @brief 模式更新
void update_flight_mode(void)
{
    switch (flight_mode)
    {
    case ATTITUDE:
        attitude_run();
        break;
    case GCS_VEHICLE_LAUNCH:
        gcs_tax_launch_run(fast_loop_dt);
        break;
    default:
        break;
    }
}