#include "mode_gcs_tax_launch_run.h"
#include "auto_pilot.h"
#include "control_attitude.h"
#include "control_rate.h"
#include "control_throttle.h"
#include "euler.h"
#include "helpler_funtions.h"
#include "lowpass_filter.h"
#include "matrix.h"
#include "mode_attitude.h"
#include "my_math.h"
#include "params.h"
#include "pilot_navigation.h"
#include "soft_can_yy.h"
#include "soft_imu.h"
#include "soft_time.h"
#include "soft_voltage.h"
#include "flight_mode.h"
#include "soft_motor_output.h"
#include <math.h>
#include <stdint.h>


uint8_t __mode_state = 0;  /* 模式运行阶段 */
uint32_t __time_stamp = 0; /* 定时计数 */

struct yy_hdw_pos
{
    uint8_t pos_f; /* m */
    uint8_t pos_r;
    uint8_t pos_d;
};

/**
 * @brief 模式初始化
 *
 * @return true-初始化成功
 */
bool gcs_tax_launch_init(void)
{
    __mode_state = __STATE_CHECK_ACC_OVERLOAD;
    __time_stamp = micros();

    butter2_filter_init(&_cxy.vt_ff_flt[0], 200, 10);
    butter2_filter_init(&_cxy.vt_ff_flt[1], 200, 10);

    return true;
}

/**
 * @brief 检测过载
 *
 * @param dt 运行周期
 */
static void _check_acc_overload_run(float dt)
{
    const float acc_overload_thr = 1.0f * 9.8f;
    static uint32_t acc_over_load_count = 0;

    float acc_norm = 0.01f * sqrtf(raw_imu._x_acc * raw_imu._x_acc +
                                   raw_imu._y_acc * raw_imu._y_acc +
                                   raw_imu._z_acc * raw_imu._z_acc);
    if (acc_norm > acc_overload_thr)
    {
        if (acc_over_load_count++ >= 2)
        {
            // __mode_state = __STATE_SELF_STABILITY;
            // __time_stamp = micros();
        }
    }
}

/**
 * @brief 起飞自稳定
 *
 * @param dt 运行周期
 */
static void _self_stability_run(float dt)
{
    if (thr_lock_status == LOCKED)
    {
        if (micros() - __time_stamp > 0.3f * 1e6) /* 延迟 0.3s 后解锁 */
        {
            __mode_state = __STATE_LAUNCH_GO;
            pid_m_alt.thr_hold = true;

            set_thr_lock_status(UNLOCKED, LAUNCHUNLOCK);
            set_automode_target_yaw(pid_m_yaw.angle_c);

            althold_state = NO_ALT_HOLD;

            /* 清除定点积分 */
            clear_poshold_i_item(&pid_m_posx);
            clear_poshold_i_item(&pid_m_posy);
            clear_poshold_i_item(&pid_m_alt);

            /* 清除内环积分 */
            clear_rate_i_item(&pid_m_roll);
            clear_rate_i_item(&pid_m_pitch);
            clear_rate_i_item(&pid_m_yaw);

            ground_air_status = IN_AIR;
            throttle_pidctrl_status = THROTTLE_INAIR_RUNNING;
            pid_m_alt.vel_t = pid_m_alt.vel_c;
            pid_m_roll.angle_t = 0;
            pid_m_pitch.angle_t = 0;
        }
    }
}

extern void control_xy(struct controller_xy *cxy, float dt);
float record_target_alt = 50 * 100; // cm
float thr_max = 1400.0f; // 限制油门最大为
uint16_t angle_plan_run_cnt = 0;
#define _CXY_0_RATIO (-0.8f) // X轴加速度积分结果的比例系数（经验值，需根据实测调整）
#define _CXY_1_RATIO (-0.8f) // Y轴加速度积分结果的比例系数
#define _HWD_INT_PERIOD (5.0f) // 以5s为积分周期

#define _ROLL_THRESHOLD  (5.0f) // ROLL角度阈值（度）
#define _PITCH_THRESHOLD (5.0f) // PITCH角度阈值（度）
#define _ANGLE_PERIOD    (3.0f) // 角度变化阈值时间周期（秒）
#define _GO_BACK_TIME_PERIOD      (5.0f) // 打反向运动时常
#define _GO_BACK_EULER_ANGLE (15.0f) // 打反向欧拉角阈值（度）

#define START_UP_STATE1_DURATION (2 * 1e6) // 弹起阶段稳定时长
#define YAW_DELAY_AFTER_LAUNCH (6.0f * 1e6) // 起飞后航向控制延迟
#define START_UP_STATE2_THR_RAMP_DURATION (1 * 1e6) 
// 启动阶段1给1450油门 3秒后进入策略
#define START_UP_STATE0_THR_MAX (1400.0f) // 弹射阶段0 最大油门
#define START_UP_STATE1_THR_MAX (1430.0f) // 弹起阶段1 最大油门
#define START_UP_STATE2_THR_MAX (1480.0f) // 弹起阶段2 最大油门
typedef enum
{
    HDW_STATE_POSITION = 0, // 互定位位置控制
    HDW_STATE_ACC_INT ,       // 加速度积分
    HDW_STATE_BACK_EULER,        // 打反向欧拉角
    HDW_STATE_RECOVER            // 恢复阶段
} hdw_state_t;

typedef struct
{
    hdw_state_t state;
    uint32_t state_enter_t;

    // 加速度积分
    bool acc_int_active;
    uint32_t acc_int_start_t;
    float acc_int_x;
    float acc_int_y;

    // 姿态监控
    bool angle_monitor_triggered;
    uint32_t angle_over_start_t;
    bool roll_triggered;
    bool pitch_triggered;

    // 反向欧拉
    bool euler_dir_set;
    int8_t roll_dir;   // -1 / 0 / +1
    int8_t pitch_dir;

} hdw_ctx_t;



static hdw_ctx_t hdw_ctx = {
    .state = HDW_STATE_ACC_INT,
    .state_enter_t = 0,

    .acc_int_active = false,
    .acc_int_start_t = 0,
    .acc_int_x = 0.0f,
    .acc_int_y = 0.0f,

    .angle_monitor_triggered = false,
    .angle_over_start_t = 0,
    .roll_triggered = false,
    .pitch_triggered = false,

    .euler_dir_set = false,
    .roll_dir = 0,
    .pitch_dir = 0,
};

static inline void no_hdw_reset(hdw_ctx_t *ctx)
{
    ctx->state = HDW_STATE_ACC_INT;
    ctx->state_enter_t = 0;

    ctx->acc_int_active = false;
    ctx->acc_int_start_t = 0;
    ctx->acc_int_x = 0.0f;
    ctx->acc_int_y = 0.0f;

    ctx->angle_monitor_triggered = false;
    ctx->angle_over_start_t = 0;
}
static inline void no_hdw_enter_state(hdw_ctx_t *ctx, hdw_state_t new_state)
{
    ctx->state = new_state;
    ctx->state_enter_t = micros();

    // 状态切换通用清理
    ctx->angle_monitor_triggered = false;
    ctx->acc_int_active = false;
   
    
}
// 无互定位下状态机
static void hdw_run_no_insgps(hdw_ctx_t *ctx, float dt)
{
    float ax_cmd = 0.0f;
    float ay_cmd = 0.0f;

    switch (ctx->state)
    {
    case HDW_STATE_POSITION:break;
    /*================ 1. 加速度积分 =================*/
    case HDW_STATE_ACC_INT:
    {

        if (!ctx->acc_int_active)
        {
            ctx->acc_int_x = 0;
            ctx->acc_int_y = 0;
            ctx->acc_int_start_t = micros();
            ctx->acc_int_active = true;
        }

        ctx->acc_int_x += pid_m_posx.acc_c * dt * 0.01f;
        ctx->acc_int_y += pid_m_posy.acc_c * dt * 0.01f;
       
        ax_cmd = constrain_float(ctx->acc_int_x * _CXY_0_RATIO, -3.0f, 3.0f);
        ay_cmd = constrain_float(ctx->acc_int_y * _CXY_1_RATIO, -3.0f, 3.0f);

        // 姿态异常检测
        // if ((fabsf(pid_m_roll.angle_t - pid_m_roll.angle_c) > _ROLL_THRESHOLD ||
        //     fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _PITCH_THRESHOLD) )
        // {
        //     if(ctx->angle_monitor_triggered == false)
        //     {
        //         ctx->angle_monitor_triggered = true;
        //         ctx->angle_over_start_t = micros();
        //     }
            
        //     if (micros() - ctx->angle_over_start_t > _ANGLE_PERIOD * 1e6)
        //     {
        //         bool roll_over  = fabsf(pid_m_roll.angle_t  - pid_m_roll.angle_c)  > _ROLL_THRESHOLD;
        //         bool pitch_over = fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _PITCH_THRESHOLD;

        //         ctx->roll_triggered  = roll_over;
        //         ctx->pitch_triggered = pitch_over;
        //         ctx->euler_dir_set = false;
        //         angle_plan_run_cnt++;
        //         no_hdw_enter_state(ctx, HDW_STATE_BACK_EULER);
        //     }
        // }else
        // {
        //     ctx->angle_monitor_triggered = false;
        // }

        // 周期性重置积分
        if (micros() - ctx->acc_int_start_t > _HWD_INT_PERIOD * 1e6)
        {
            ctx->acc_int_x = 0;
            ctx->acc_int_y = 0;

            ctx->acc_int_start_t = micros();
        }
        break;
    }

    /*================ 2. 打反向欧拉 =================*/
    case HDW_STATE_BACK_EULER:
    {
        ax_cmd = 0.0f; 
        ay_cmd = 0.0f;
        
        if (!ctx->euler_dir_set)
        {
            ctx->roll_dir  = (pid_m_roll.angle_c > 0) ? 1 : -1;
            ctx->pitch_dir = (pid_m_pitch.angle_c > 0) ? 1 : -1;
            ctx->euler_dir_set = true;
        }

        if (micros() - ctx->state_enter_t > _GO_BACK_TIME_PERIOD * 1e6)
        {
            no_hdw_enter_state(ctx, HDW_STATE_ACC_INT);
        }
        break;
    }

    /*================ 3. 恢复 =================*/
    case HDW_STATE_RECOVER:
    {
        if (micros() - ctx->state_enter_t > 1.0f * 1e6) // 以0姿态角持续1秒
        {
            no_hdw_enter_state(ctx, HDW_STATE_ACC_INT);
        }
        break;
    }

    default:
        no_hdw_enter_state(ctx, HDW_STATE_ACC_INT);
        break;
    }

    /* 统一输出期望加速度 */
    _cxy.at[0] = ax_cmd; // 只有在加速度积分下有用 反向欧拉直接控制欧拉角覆盖期望加速度
    _cxy.at[1] = ay_cmd;
}

// 设置反向欧拉角
static void no_hdw_apply_back_euler(hdw_ctx_t *ctx)
{
    if (ctx->state != HDW_STATE_BACK_EULER)
        return;

    if (ctx->roll_triggered)
        pid_m_roll.angle_t = -ctx->roll_dir * _GO_BACK_EULER_ANGLE;
    else
        pid_m_roll.angle_t = 0;

    if (ctx->pitch_triggered)
        pid_m_pitch.angle_t = -ctx->pitch_dir * _GO_BACK_EULER_ANGLE;
    else
        pid_m_pitch.angle_t = 0;
}


void __tax_launch_go_run(float dt)
{
    const struct yy_can_rx_msg_data *yycan_data = yy_can_rx_data_get();
    hdw_ctx_t *hdw_ctx_p = &hdw_ctx;
    // start_up_ctx_t *start_up_ctx_p = &start_up_ctx;
    /**************************************/
    if (pid_m_alt.thr_hold == true)
    {
        thr_max =  START_UP_STATE0_THR_MAX;
        if (micros() - __time_stamp > 2.0f * 1e6)
        {
            pid_m_alt.thr_hold = false;
        }

        althold_state = NO_ALT_HOLD;

        t_roll_last = pid_m_roll.angle_t = 0;
        t_pitch_last = pid_m_pitch.angle_t = 0;

        _cxy.reset = true;

        /*弹射后2秒钟，记录当时的高度，作为自动控制时候的目标高度*/
        // record_target_alt = pid_m_alt.loc_c;
        record_target_alt = 50 * 100; // 50m高度
        no_hdw_enter_state(hdw_ctx_p, HDW_STATE_ACC_INT);     
    }
    else
    {
        // alt_reached = true;
        thr_max = START_UP_STATE2_THR_MAX; // 油门限制1480
        hdw_run_no_insgps(hdw_ctx_p, dt); // 无互定位状态机
          
        // 大地系下的期望加速度 _cxy.at[0], _cxy.at[1]
        float dcm_z[3] = {_cxy.at[0], _cxy.at[1], GRAVITY_MSS};
        Vector_Normalize(dcm_z, 3);

        float dcm_y[3] = {0, 1, 0};
        dcm_y[2] = -(dcm_z[0] * dcm_y[0] + dcm_z[1] * dcm_y[1]) / dcm_z[2];
        Vector_Normalize(dcm_y, 3);

        float dcm_x[3] = {0, 0, 0};
        Vector_CrossProduct_3D(dcm_y, dcm_z, dcm_x);

        float dcm[3][3];
        for (int i = 0; i < 3; ++i)
        {
            dcm[i][0] = dcm_x[i];
            dcm[i][1] = dcm_y[i];
            dcm[i][2] = dcm_z[i];
        }
        euler_angle_t euler = {0};
        Euler_ByDcm(dcm, &euler);
        pid_m_roll.angle_t = apply(euler.roll * RAD_TO_DEG, t_roll_last, 15.0f, dt);
        pid_m_pitch.angle_t = apply(euler.pitch * RAD_TO_DEG, t_pitch_last, 15.0f, dt);

        // no_hdw_apply_back_euler(hdw_ctx_p); // 打反向欧拉角时 覆盖期望欧拉
        t_roll_last = pid_m_roll.angle_t;
        t_pitch_last = pid_m_pitch.angle_t;

        althold_state = NO_ALT_HOLD;

        /********************优先响应高度****************/
       
        // if (!isnan(yycan_data->txgl_alt_sp) && yycan_data->txgl_alt_sp > 0 && yycan_data->txgl_alt_sp <= 100)
        // {
        //     record_target_alt = yycan_data->txgl_alt_sp * 100.0f; 
        //     if(debug_mode == false)
        //     {
        //         if (fabsf(yycan_data->txgl_alt_sp * 100 - pid_m_alt.loc_c) < 30.0f)
        //         {
        //             updata_cmd_status(ALT_EXECUTE, true);
        //             updata_cmd_status(ALT_SETTING, false);
        //             alt_reached = true;
        //         }
        //     }
        //     else
        //     {
        //         if(micros() - recv_alt_time > 3 * 1e6 && alt_reached == false)
        //         {
        //             updata_cmd_status(ALT_EXECUTE, true);
        //             updata_cmd_status(ALT_SETTING, false);
        //             alt_reached = true;
        //         }
        //     }
        // }
    
             
        //pid_m_alt.loc_t = record_target_alt;
        //float perror_u = (record_target_alt - pid_m_alt.loc_c);
        // pid_m_alt.vel_t = constrain_float(perror_u * 0.8f, -100.0f, 100.0f); // 期望速度倍做了限制 -100~100cm/s
        // pid_m_alt.vel_t = constrain_float(perror_u * 0.8f, -50.0f, 50.0f); // 期望速度倍做了限制 -50~50cm/s
        pid_m_alt.vel_t = 100.0f;  // 期望速度80cm/s 以0.8m/s的速度撞击没有那么大的冲击, 加长高油门时间 减小减速油门时间 2：1
        /********************高度达到后，相应航向****************/
        if(((micros()-__time_stamp) > 6.0f * 1e6))
        {
            alt_reached = true;
        }
        if (alt_reached) // 差不多就是4秒达到最高点
        {
            // 航向到位，更新flag
            if (pid_m_yaw.enable_tyaw)
            {            
                set_automode_target_yaw(yycan_data->txgl_yaw_sp);

                if (fabsf(wrap_180(pid_m_yaw.angle_c - yycan_data->txgl_yaw_sp)) < 5.0f)
                {
                    updata_cmd_status(YAW_EXECUTE, true);
                    updata_cmd_status(YAW_SETTING, false);

                    pid_m_yaw.enable_tyaw = false;

                    yaw_reached = true;
                }              
            }
           
        }
        else
        {
            t_roll_last  = pid_m_roll.angle_t = 0;
            t_pitch_last = pid_m_pitch.angle_t = 0;
            hdw_ctx_p->acc_int_x = 0; // 上升到最高点之前不进行加速度积分策略
            hdw_ctx_p->acc_int_y = 0;
        }

        /*所有动作暂停指令 实际台子无该指令*/
        if ((yycan_data->txgl_drone_cmd & CTL_STOP_CMD_MASK) == CTL_STOP_ACTION)
        {
            set_automode_target_yaw(wrap_360(pid_m_yaw.angle_c)); // 航向锁定
            pid_m_alt.vel_t = 0; // 垂速为0
        }
    }
    Calculate_Target_Az(fast_loop_dt);
}

void gcs_tax_launch_run(float dt)
{
    switch (__mode_state)
    {
    case __STATE_CHECK_ACC_OVERLOAD:
        _check_acc_overload_run(dt);
        break;
    case __STATE_SELF_STABILITY:
        _self_stability_run(dt);
        break;
    case __STATE_LAUNCH_GO:
        __tax_launch_go_run(dt);
        break;
    default:
        break;
    }
}

void gcs_tax_launch_exit(void)
{
    pid_m_alt.thr_hold = false;
}