#include "control_throttle.h"
#include "auto_pilot.h"
#include "common.h"
#include "control_attitude.h"
#include "control_rate.h"
#include "flight_mode.h"
#include "hard_imu_uart3.h"
#include "helpler_funtions.h"
#include "lowpass_filter.h"
#include "my_crc.h"
#include "my_math.h"
#include "params.h"
#include "pilot_navigation.h"
#include "soft_flash.h"
#include "soft_gps.h"
#include "soft_gs.h"
#include "soft_imu.h"
#include "soft_motor_output.h"
#include "soft_port_uart4.h"
#include "soft_rc_input.h"
#include "soft_sdcard.h"
#include "soft_time.h"
#include "soft_voltage.h"
#include "ver_config.h"
// #include "control_poshold.h"
#include <math.h>

/* 油门控制输出状态 */
ThrCtrolStatusType throttle_pidctrl_status = THROTTLE_OFF;

/* 是否允许调试解锁, 若允许则会禁用部分解锁条件限制 */
bool _enable_unlock_tune = false;

void throttle_enable_unlock_tune(void) { _enable_unlock_tune = true; }

bool throttle_get_enable_unlock_tune(void)
{
    return _enable_unlock_tune;
}

/**
 * @brief 是否允许地面站指令解锁
 *
 * @return int
 */
int judge_pilot_gsunlock_allow(void)
{
    if (thr_lock_status == UNLOCKED)
        return UNLOCK_ALLOW;

    // imu 数据错误
    if (imu_link_status != COMP_NORMAL)
    {
        return UNLOCK_NOIMU;
    }

    // imu 通讯错误包
    if (dma_iserror == true)
    {

        return UNLOCK_DMAERR;
    }

    // 无解算定位
    if (ins.insgps_ok != INSGPS)
    {
        return UNLOCK_NOINSGPS;
    }

    // GPS 状态标志位异常不允许解锁
    if ((raw_gps._gps_status & GPS_STATUS1_ERROR_MASK) ||
        (raw_gps._gps_status2 & GPS_STATUS2_ERROR_MASK))
    {
        return UNLOCK_GPSERR;
    }

    // IMU 磁状态异常
    if (raw_gps._mag_status & MAG_STATUS_ERROR_MASK)
    {
        return UNLOCK_MAGERROR;
    }

    // 动力异常
    if (Motor_GetFailsafeNum() > 0 && _enable_unlock_tune == false)
    {
        return UNLOCK_SERVO_FAILSAFE;
    }

    // IMU 陀螺加速度计异常
    if (raw_gps._acc_gyro_status & ACCGYRO_STATUS_ERROR_MASK)
    {
        return UNLOCK_ACCERR;
    }

    // 速度超范围不允许解锁
    if (thr_lock_status == LOCKED && ins.insgps_ok == INSGPS &&
        (fabsf(pid_m_posx.vel_c) > 40.0f || fabsf(pid_m_posy.vel_c) > 40.0f ||
         fabsf(pid_m_alt.vel_c) > 40.0f))
    {
        return UNLOCK_VELERR;
    }

    // 加计超范围不允许解锁
    if (thr_lock_status == LOCKED &&
        (fabsf(pid_m_posx.acc_c) > 80.0f || fabsf(pid_m_posy.acc_c) > 80.0f ||
         fabsf(pid_m_alt.acc_c) > 80.0f))
    {
        return UNLOCK_ACCERR;
    }

    // 陀螺超范围不允许解锁
    if (thr_lock_status == LOCKED &&
        (fabsf(pid_m_roll.gyro_c) > 10.0f ||
         fabsf(pid_m_pitch.gyro_c) > 10.0f || fabsf(pid_m_yaw.gyro_c) > 10.0f))
    {
        return UNLOCK_GYROERROR;
    }

    // 姿态倾斜超范围不允许解锁
    if (thr_lock_status == LOCKED && (fabsf(pid_m_roll.angle_c) > 30.0f ||
                                      fabsf(pid_m_pitch.angle_c) > 30.0f))
    {
        return UNLOCK_LEAN_ANGLE;
    }

    // 温度超范围不允许解锁
    if (thr_lock_status == LOCKED && ins.temprature > 80)
    {
        return UNLOCK_OVER_TEMPRATURE;
    }

    // 电压低不允许解锁
    if (battary_volt_islow)
    {
        return UNLOCK_VOLT_LOW;
    }

    // 智能电池故障不允许解锁
    const VoltageBms *bms = Voltage_GetBmsInfo();
    if (bms->link_status == COMP_NORMAL)
    {
        if (bms->electricity_percentage < params_get_value(ParamNum_BmsLowCapcityPercentage))
        {
            return UNLOCK_BMS_LOW_CAPACITY;
        }

        if (bms->fault_status >= BMS_FAULT_SERIOUS)
        {
            return UNLOCK_BMS_FAULT;
        }
    }
    else if (bms->link_status == COMP_LOST)
    {
        return UNLOCK_BMS_LINK_LOST;
    }

    // sd 卡故障不允许解锁
    if (sdcard_initok() == 0)
    {
        return UNLOCK_SD_FAIL;
    }

    return UNLOCK_ALLOW;
}

/**
 * @brief 是否允许遥控器解锁
 *
 * @return int
 */
int judge_pilot_rcunlock_allow(void)
{
    if (thr_lock_status == UNLOCKED)
        return UNLOCK_ALLOW;
    // 遥控器位置不对
    if ((rc_in[RC_CH6] < MAX_S || rc_in[RC_CH6] > MAX_E ||
         rc_in[RC_CH7] < MAX_S || rc_in[RC_CH7] > MAX_E) &&
        thr_lock_status == LOCKED && comp_rc_status == COMP_NORMAL)
    {
        return UNLOCK_RCERR;
    }

    // imu 数据错误
    if (imu_link_status != COMP_NORMAL)
    {
        return UNLOCK_NOIMU;
    }

    // imu 通讯错误包
    if (dma_iserror == true)
    {
        return UNLOCK_DMAERR;
    }

    // GPS 状态标志位异常不允许解锁
    if (((raw_gps._gps_status & GPS_STATUS1_ERROR_MASK) ||
         (raw_gps._gps_status2 & GPS_STATUS2_ERROR_MASK)) &&
        _enable_unlock_tune == false)
    {
        return UNLOCK_GPSERR;
    }

    // IMU 磁状态异常
    if ((raw_gps._mag_status & MAG_STATUS_ERROR_MASK) &&
        _enable_unlock_tune == false)
    {
        return UNLOCK_MAGERROR;
    }

    if (Motor_GetFailsafeNum() > 0 && _enable_unlock_tune == false)
    {
        return UNLOCK_SERVO_FAILSAFE;
    }

    // IMU 陀螺加速度计异常
    if (raw_gps._acc_gyro_status & ACCGYRO_STATUS_ERROR_MASK)
    {
        return UNLOCK_ACCERR;
    }

    // 速度超范围不允许解锁
    if (thr_lock_status == LOCKED && ins.insgps_ok == INSGPS &&
        (fabsf(pid_m_posx.vel_c) > 40.0f || fabsf(pid_m_posy.vel_c) > 40.0f ||
         fabsf(pid_m_alt.vel_c) > 40.0f) &&
        _enable_unlock_tune == false)
    {
        return UNLOCK_VELERR;
    }

    // 加计超范围不允许解锁
    if (thr_lock_status == LOCKED &&
        (fabsf(pid_m_posx.acc_c) > 80.0f || fabsf(pid_m_posy.acc_c) > 80.0f ||
         fabsf(pid_m_alt.acc_c) > 80.0f) &&
        _enable_unlock_tune == false)
    {
        return UNLOCK_ACCERR;
    }

    // 陀螺超范围不允许解锁
    if (thr_lock_status == LOCKED &&
        (fabsf(pid_m_roll.gyro_c) > 10.0f ||
         fabsf(pid_m_pitch.gyro_c) > 10.0f ||
         fabsf(pid_m_yaw.gyro_c) > 10.0f) &&
        _enable_unlock_tune == false)
    {
        return UNLOCK_GYROERROR;
    }

    // 姿态倾斜超范围不允许解锁
    if (thr_lock_status == LOCKED && (fabsf(pid_m_roll.angle_c) > 30.0f ||
                                      fabsf(pid_m_pitch.angle_c) > 30.0f))
    {
        return UNLOCK_LEAN_ANGLE;
    }

    // 温度超范围不允许解锁
    if (thr_lock_status == LOCKED && ins.temprature > 80)
    {
        return UNLOCK_OVER_TEMPRATURE;
    }

    // 电压低不允许解锁
    if (battary_volt_islow && _enable_unlock_tune == false)
    {
        return UNLOCK_VOLT_LOW;
    }

    const VoltageBms *bms = Voltage_GetBmsInfo();
    if (bms->link_status == COMP_NORMAL)
    {
        if (bms->electricity_percentage < params_get_value(ParamNum_BmsLowCapcityPercentage) && _enable_unlock_tune == false)
        {
            return UNLOCK_BMS_LOW_CAPACITY;
        }

        if (bms->fault_status >= BMS_FAULT_SERIOUS && _enable_unlock_tune == false)
        {
            return UNLOCK_BMS_FAULT;
        }
    }
    else if (bms->link_status == COMP_LOST && _enable_unlock_tune == false)
    {
        return UNLOCK_BMS_LINK_LOST;
    }

    // sd卡故障不允许解锁
    if (sdcard_initok() == 0 && _enable_unlock_tune == false)
    {
        return UNLOCK_SD_FAIL;
    }

    return UNLOCK_ALLOW;
}

#define PILOT_RCUNLOCK_PERIOD 200000
unsigned int pilot_rcunlock_time = 0, pilot_rcunlock_delay = 0;

// 上锁方式的标志位
char thr_lock_flag = DEFLOCK;
// 解锁方式的标志位
char thr_unlock_flag = DEFUNLOCK;

// 上锁状态位
char thr_lock_status = LOCKED;
// 起飞状态位
char ground_air_status = ON_GROUND;

void pilot_unlock_decide(void)
{
    int rcunlock_allow_flag = judge_pilot_rcunlock_allow();

    // 遥控器模式下
    if (control_mode == CONTROL_REMOTE && rc_signal_health == RC_SIGNAL_HEALTH)
    { // 遥控器解锁
        if (thr_lock_status == LOCKED && rcunlock_allow_flag == UNLOCK_ALLOW &&
            (rc_in[RC_CH1] > 1950) && (rc_in[RC_CH2] > 1950) &&
            (rc_in[RC_CH3] < 1050) && (rc_in[RC_CH4] < 1050))
        {
            pilot_rcunlock_delay += micros() - pilot_rcunlock_time;
            pilot_rcunlock_time = micros();

            if (pilot_rcunlock_delay > PILOT_RCUNLOCK_PERIOD)
            {
                pilot_rcunlock_delay = 0;

                set_thr_lock_status(UNLOCKED, RCUNLOCK);

                // 解锁成功
                if (thr_lock_status == UNLOCKED)
                {
                    pid_m_yaw.angle_t = pid_m_yaw.angle_c;

                    // 每次解锁清气压,清零气压由控制端来完成，因为给IMU发命令会导致DMA错误
                    unclock_clear_altitude();

                    // 初始化定点参数
                    clear_poshold_i_item(&pid_m_posx);
                    clear_poshold_i_item(&pid_m_posy);
                    clear_poshold_i_item(&pid_m_alt);
                }
                else
                {
                }
            }
        }
        else
        {
            pilot_rcunlock_delay = 0;
            pilot_rcunlock_time = micros();
        }
    }
    else if (control_mode == CONTROL_GCS)
    {
    }
}

/*
飞行器起飞识别
*/

// 基础油门
float base_thr = 1000; // HOVER_THR; 
void pilot_takeoff_decide(void)
{
    // 遥控器 姿态模式/定点模式 下起飞判断
    if (control_mode == CONTROL_REMOTE &&
        (flight_mode == ATTITUDE || flight_mode == GPS_POSHOLD) &&
        thr_lock_status == UNLOCKED && ground_air_status == ON_GROUND)
    {
        if (rc_in[RC_CH3] <= (1200 + RC_DEAD_ZONE))
        {
            record_position(&pid_m_posx.loc_t, &pid_m_posy.loc_t,
                            pid_m_posx.loc_c, pid_m_posy.loc_c);
            clear_poshold_i_item(&pid_m_posx);
            clear_poshold_i_item(&pid_m_posy);
            clear_poshold_i_item(&pid_m_alt);
            // 解锁后没有推油门的落锁设置在解锁判断里面
        }
        else if (rc_in[RC_CH3] > (1200 + RC_DEAD_ZONE))
        {
            ground_air_status = IN_AIR;
            throttle_pidctrl_status = THROTTLE_INAIR_RUNNING;
        }
    }
}

/**
 * @brief 上锁识别 TODO:遥控器上锁识别放到手动飞行模式中去
 *
 */
void pilot_lock_decide(void)
{
    static uint32_t remote_rclock_time = 0, remote_rclock_delay = 0;

    // 如果是在遥控器模式下
    if (control_mode == CONTROL_REMOTE && rc_signal_health == RC_SIGNAL_HEALTH)
    {
        // 如果已经解锁
        if (thr_lock_status == UNLOCKED)
        {
            if (flight_mode != GPS_RTH)
            {
                // 检测遥控器主动操作上锁
                if ((rc_in[RC_CH1] < 1050) && (rc_in[RC_CH4] > 1950) &&
                    (rc_in[RC_CH3] < 1050) && (rc_in[RC_CH2] > 1950))
                {
                    remote_rclock_delay += micros() - remote_rclock_time;
                    remote_rclock_time = micros();

                    if (remote_rclock_delay > 0.2f * 1000000)
                    {
                        remote_rclock_delay = 0;

                        set_thr_lock_status(LOCKED, RCLOCK);
                    }
                }
                else
                {
                    remote_rclock_delay = 0;
                    remote_rclock_time = micros();
                }
            }
        }
        // 如果已经上锁
        else
        {
            // 清零计时变量
            remote_rclock_delay = 0;
            remote_rclock_time = micros();
        }
    }
}

/*
通过遥控器的值获取期望爬升速度
*/
char althold_state = NO_ALT_HOLD;
extern float thr_max;
// 有个±5的余量，避免垂直速度方向有偏差导致小速度时无法正确执行。15太大了。
void get_pilot_desired_climb_rate_fromrc(short thr_in)
{
    float desired_rate = 0.0f; // cm/s
    static unsigned int stop_time = 0;

    // ensure a reasonable throttle value
    thr_in = constrain_int16(thr_in, 1000, 2000);

    if (rc_in[RC_CH8] < 1500)
    {
        althold_state = NO_ALT_HOLD;
        pid_m_alt.loc_t = pid_m_alt.loc_c;
        pid_m_alt.vel_t = 0;
        return;
    }
    // 8通 且遥控器健康
    if(rc_in[RC_CH8] > 1500 && comp_rc_status == COMP_NORMAL && rc_signal_health == RC_SIGNAL_HEALTH)
    {
        pid_m_alt.vel_t = 150; // 期望高度等于当前高度+
        althold_state = NO_ALT_HOLD;
        thr_max = 1700.0f;
        return;
    }
    // check throttle is above, below or in the deadband
    if (thr_in < 1500 - RC_DEAD_ZONE)
    {
        // below the deadband
        desired_rate = -10 + ((parinf._par_maxdownspeed * 10) * (thr_in - (1500 - RC_DEAD_ZONE)) / 450.0f);

        althold_state = NO_ALT_HOLD;
        pid_m_alt.vel_t = desired_rate;
        stop_time = 0;
    }
    else if (thr_in > 1500 + RC_DEAD_ZONE)
    {
        // above the deadband
        desired_rate = 20 + ((parinf._par_maxupspeed * 10) * (thr_in - (1500 + RC_DEAD_ZONE)) / 450.0f);

        althold_state = NO_ALT_HOLD;
        pid_m_alt.vel_t = desired_rate;
        stop_time = 0;
    }
    else
    {
        if (althold_state != ALT_HOLD)
        {
            pid_m_alt.vel_t = 0.0f;

            if (fabsf(pid_m_alt.vel_c) > 30.0f)
                stop_time = micros();

            if ((unsigned int)(micros() - stop_time > 1500000)) // 1.5s定高
            {
                althold_state = ALT_HOLD;
                pid_m_alt.loc_t = pid_m_alt.loc_c;
            }
        }
    }
}

/**************************实现函数********************************************
 *函数原型:		void Acc_AltHold_Control()
 *功　　能:	    高度控制
 *******************************************************************************/

void Calculate_Target_Az(float dt)
{
    // 如果是定高状态，则将高度差计算在内，否则置0
    if (althold_state == ALT_HOLD)
    {
        pid_m_alt.vel_t = cal_tar_vel_z(pid_m_alt.loc_t, pid_m_alt.loc_c, 400, 400);

        // 计算垂直速度差
        pid_m_alt.vel_e = pid_m_alt.vel_t - pid_m_alt.vel_c;
        // 速度的差需要放开，这样从升降速切换到定高时能快速定住
        pid_m_alt.vel_e = constrain_float(pid_m_alt.vel_e, -500.0f, 500.0f);
    }
    else if (althold_state == NO_ALT_HOLD)
    {
        // 计算垂直速度差
        pid_m_alt.vel_e = pid_m_alt.vel_t - pid_m_alt.vel_c;
        // 速度的差需要放开，这样从升降速切换到定高时能快速定住
        pid_m_alt.vel_e = constrain_float(pid_m_alt.vel_e, -500.0f, 500.0f);
    }

    pid_m_alt.vel_p_item = pid_m_alt.vel_e * pid_v_alt.speed_p;
    pid_m_alt.acc_t = pid_m_alt.vel_p_item;
}

/*
高度通道加速度pi到油门
*/
void rate_pid_acctothrottle_alt(float dt)
{
    pid_m_alt.acc_t = constrain_float(pid_m_alt.acc_t, -400.0f, 400.0f);
    pid_m_alt.acc_e = pid_m_alt.acc_t - pid_m_alt.acc_c;
    pid_m_alt.acc_e = constrain_float(pid_m_alt.acc_e, -500.0f, 500.0f);

    //----------P--------------------
    // 加速度差转换为电机输出量 ,系数为0.4
    pid_m_alt.acc_p_item = pid_m_alt.acc_e * pid_v_alt.acc_p;

    //------------------I-----------
    // 加速度差的积分，系数为1.5，1m/s^2的加速度差1s积分1500
    pid_m_alt.acc_i_item += dt * pid_v_alt.acc_i * constrain_float(pid_m_alt.acc_e, -100.0f, +100.0f) * 10.0f;

    pid_m_alt.acc_i_item = constrain_float(pid_m_alt.acc_i_item, 1000 - conf_par.idle_speed, 1650 - base_thr); // -100 300

    // 在地上的时候积分不累加
    if (ground_air_status != IN_AIR)
    {
        pid_m_alt.acc_i_item = 0.0f;
    }

    // 油门输出量
    pid_m_alt.pid_out = pid_m_alt.acc_p_item + pid_m_alt.acc_i_item;

    // 去掉Z轴声音会变大
    pid_m_alt.pid_out = pid_m_alt.pid_out_last + constrain_float((pid_m_alt.pid_out - pid_m_alt.pid_out_last), -10.0f, 10.0f);
    pid_m_alt.pid_out_last = pid_m_alt.pid_out;

    pid_m_alt.pid_out = constrain_float(pid_m_alt.pid_out, -200.0f, 600.0f);
    // pid_m_alt.pid_out = constrain_float(pid_m_alt.pid_out, 0.0f, 600.0f); // 抗冲击必须有
    // 如果监测到可能在地面，则油门控制 pid 量清空
    if (throttle_pidctrl_status == THROTTLE_OFF ||
        throttle_pidctrl_status == THROTTLE_ONGROUND_IDLE)
    {
        pid_m_alt.pid_out = 0.0f;
        pid_m_alt.acc_i_item = 0.0f;
    }

    if (pid_m_alt.thr_hold == true)
    {
        pid_m_alt.acc_i_item = 100;
        pid_m_alt.pid_out_last = 100;
        pid_m_alt.pid_out = 100;
       
    }
}

bool unlock_fail = false;

void set_thr_lock_status(uint8_t lock_status, uint8_t reason)
{
    if (lock_status == LOCKED)
    {
        thr_lock_status = LOCKED;
        ground_air_status = ON_GROUND;

        throttle_pidctrl_status = THROTTLE_OFF;

        notify_imu_lock = true;
        thr_lock_flag = reason;

        wp_have_cycle_times = 0;
    }
    else if (lock_status == UNLOCKED)
    {
        notify_imu_unlock = true;
        {
            thr_lock_status = UNLOCKED;
            ground_air_status = ON_GROUND;

            thr_unlock_flag = reason;
            throttle_pidctrl_status = THROTTLE_ONGROUND_IDLE;
            unlock_fail = false;
        }

        if (thr_lock_status == LOCKED)
        {
            unlock_fail = true;
        }
    }
}

void clear_poshold_i_item(struct pid_method_ap *method)
{
    method->acc_i_item = 0.0f;
    method->vel_i_item = 0.0f;
}