#include "mode_attitude.h"
#include "auto_pilot.h"
#include "control_attitude.h"
#include "control_rate.h"
#include "control_throttle.h"
#include "euler.h"
#include "flight_mode.h"
#include "helpler_funtions.h"
#include "math.h"
#include "matrix.h"
#include "params.h"
#include "soft_imu.h"
#include "soft_port_uart4.h"
#include "soft_rc_input.h"
#include <my_math.h>
#include <soft_motor_output.h>
#include "mode_gcs_tax_launch_run.h"
#include "soft_time.h"
struct controller_xy _cxy;


bool attitude_init(void)
{
    init_attitude_head();

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

    return true;
}

float dist_comp[2] = {0.0f};
void control_xy(struct controller_xy *cxy, float dt)
{
    const float P_KP = pid_v_pos.dist_p;
    const float V_KP = pid_v_pos.speed_p;
    const float V_KI = pid_v_pos.acc_i;
    const float V_KD = pid_v_pos.acc_p;

    cxy->pc[0] = pid_m_posx.loc_c * 0.01f;
    cxy->pc[1] = pid_m_posy.loc_c * 0.01f;

    cxy->vc[0] = pid_m_posx.vel_c * 0.01f;
    cxy->vc[1] = pid_m_posy.vel_c * 0.01f;

    cxy->ac[0] = pid_m_posx.acc_c * 0.01f;
    cxy->ac[1] = pid_m_posy.acc_c * 0.01f;

    /* 按需重置控制器 */
    if (cxy->reset)
    {
        cxy->reset = false;

        cxy->pt[0] = cxy->pc[0];
        cxy->pt[1] = cxy->pc[1];

        cxy->vt[0] = cxy->vc[0];
        cxy->vt[1] = cxy->vc[1];

        cxy->at[0] = cxy->ac[0];
        cxy->at[1] = cxy->ac[1];

        cxy->ve_integ[0] = cxy->ve_integ[1] = 0;
        cxy->pos_integ[0] = cxy->pos_integ[1] = 0;

        butter2_filter_reset(&cxy->vt_ff_flt[0], 0);
        butter2_filter_reset(&cxy->vt_ff_flt[1], 0);
    }

    /*将无人机调整到互定位的桶的中心*/
    for (int i = 0; i < 2; i++)
    {
        if (cxy->pt[i] > 0.1f)
        {
            cxy->pt[i] -= 0.002f;
        }
        else if (cxy->pt[i] < -0.1f)
        {
            cxy->pt[i] += 0.002f;
        }
    }

    float vt[2] = {0}, ve[2] = {0};
    static float target_pos_shift[2] = {0};

    for (int i = 0; i < 2; ++i)
    {

        // /*如果位置积分持续存在，则需要将位置向相反的方向移动，否则会丢失互定位*/
        // if(fabsf(cxy->pos_integ[i]) > 0.2f) // 0.2~1.0m 的误差区间，是消除稳态误差的关键阶段 —— 积分项缓慢累加，精准 “拉回” 目标点，同时避免震荡
        // {
        //     target_pos_shift[i] += (cxy->pos_integ[i] * 1.0f - target_pos_shift[i]) * 0.001f;
        // }
        // else // 若位置误差 <0.2m，说明已接近目标点，积分项继续累加可能导致 “积分震荡”（来回穿越目标点
        // {
           target_pos_shift[i] = 0.0f;
        //}

        vt[i] = (cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) * P_KP;

        // if(fabsf(cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) < 3.0f 
        //     &&fabsf(cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) > 1.0f )
        //     cxy->pos_integ[i] += (cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) * dt * V_KI;

        // cxy->pos_integ[i] = constrain_float(cxy->pos_integ[i], -2.0f, 2.0f);    
        // 若位置误差 > 1.0m，说明载体离目标点还很远，此时优先用比例项产生大速度指令快速逼近，积分项累加反而可能导致超调

    }
    #if 0
    for (int i = 0; i < 2; ++i)
    {
        /*如果位置积分持续存在，则需要将位置向相反的方向移动，否则会丢失互定位*/
        if(fabsf(cxy->pos_integ[i]) > 0.2f)
        {
            // 优化1：加快目标位置偏移响应速度（系数从0.001→0.01），快速抵消积分饱和影响
            target_pos_shift[i] += (cxy->pos_integ[i] * 1.0f - target_pos_shift[i]) * 0.01f;
        }
        else
        {
        target_pos_shift[i] = 0.0f;
        }

        // 速度指令 = 修正后的位置误差 × 位置环比例系数（P_KP）
        vt[i] = (cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) * P_KP;

        // 优化2：缩小积分触发区间（从1~3m→0.2~1.0m），仅小误差时积分，避免大误差饱和
        if(fabsf(cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) < 1.0f 
            && fabsf(cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) > 0.2f )
        {
            // 积分累积：误差×时间步长×积分系数
            cxy->pos_integ[i] += (cxy->pt[i] + target_pos_shift[i] - cxy->pc[i]) * dt * V_KI;
        }

        // 优化3：减小积分饱和值（从±2.0f→±0.5f），避免加速度额外偏移过大
        cxy->pos_integ[i] = constrain_float(cxy->pos_integ[i], -0.5f, 0.5f);
        
        // 优化4：添加积分衰减逻辑，无误差时快速清零，避免积分残留
        cxy->pos_integ[i] *= 0.98f;
    }
    #endif
    dist_comp[0] = target_pos_shift[0];
    dist_comp[1] = target_pos_shift[1];

    Vector_ConstrainNorm(vt, 2, 2.0f);

    /* 限制目标速度增量 */
    const float ACC_MAX = GRAVITY_MSS * tanf(DEG_TO_RAD * 35.0f);
    float delt_vt[2] = {0};
    for (int i = 0; i < 2; ++i)
    {
        delt_vt[i] = vt[i] - cxy->vt[i];
    }
    Vector_ConstrainNorm(delt_vt, 2, ACC_MAX * dt);
    for (int i = 0; i < 2; ++i)
    {
        vt[i] = cxy->vt[i] + delt_vt[i];
    }

    for (int i = 0; i < 2; ++i)
    {
        ve[i] = vt[i] - (1.0f * cxy->vc[i]);
    }
    for (int i = 0; i < 2; ++i)
    {
        float ve_integ = cxy->ve_integ[i] + dt * ve[i] * V_KI;
        cxy->ve_integ[i] = constrain_float(ve_integ, -10.0f, 10.0f);
    }

    float vt_ff[2] = {delt_vt[0] * V_KD / dt, delt_vt[1] * V_KD / dt};
    Vector_ConstrainNorm(vt_ff, 2, 2.0f);
    for (int i = 0; i < 2; ++i)
    {
        vt_ff[i] = butter2_filter_apply(&cxy->vt_ff_flt[i], vt_ff[i]);
    }

    float at[2] = {0};
    for (int i = 0; i < 2; ++i)
    {
        at[i] = V_KP * ve[i] + cxy->ve_integ[i] + vt_ff[i] - V_KD * 1.5f * cxy->ac[i] + cxy->pos_integ[i];
    }
    Vector_ConstrainNorm(at, 2, 10.0f);
    float delt_at[2] = {0};
    for (int i = 0; i < 2; ++i)
    {
        delt_at[i] = at[i] - cxy->at[i];
    }
    Vector_ConstrainNorm(delt_at, 2, 35.0f * dt);
    for (int i = 0; i < 2; ++i)
    {
        at[i] = cxy->at[i] + delt_at[i];
    }

    for (int i = 0; i < 2; ++i)
    {
        cxy->vt[i] = vt[i];
        cxy->at[i] = at[i];
    }
}



#define _ATT_CXY_0_RATIO (-0.8f) // 比例暂时以1除以积分周期（s）为准
#define _ATT_CXY_1_RATIO (-0.8f)
#define _ATT_HWD_INT_PERIOD (5.0f) // 以5s为积分周期

#define _ATT_ROLL_THRESHOLD  (5.0f) // ROLL角度阈值（度）
#define _ATT_PITCH_THRESHOLD (5.0f) // PITCH角度阈值（度）
#define _ATT_ANGLE_PREIOD    (3.0f) // 角度变化阈值时间周期（秒）
#define _ATT_GO_BACK_TIME_PERIOD  (5.0f) // 打反向运动时常
#define _ATT_GO_BACK_EULER_ANGLE (15.0f) // 打反向欧拉角阈值（度）
typedef enum
{
    HDW_STATE_ACC_INT = 0,       // 加速度积分
    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)
    {
    /*================ 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;
        // 水平方向无加速度时应该是姿态角为0度 
        // 姿态异常检测 
        // float att_abs = fmaxf(fabsf(pid_m_roll.angle_c),
        //                     fabsf(pid_m_pitch.angle_c));

        // if (att_abs > 8.0f)
        // {
        //     // 姿态开始不可信，慢慢衰减积分
        //     ctx->acc_int_x *= 0.98f;
        //     ctx->acc_int_y *= 0.98f;
        // }

        ax_cmd = constrain_float(ctx->acc_int_x * _ATT_CXY_0_RATIO, -3.0f, 3.0f);
        ay_cmd = constrain_float(ctx->acc_int_y * _ATT_CXY_1_RATIO, -3.0f, 3.0f);

        // 姿态异常检测
        // if ((fabsf(pid_m_roll.angle_t - pid_m_roll.angle_c) > _ATT_ROLL_THRESHOLD ||
        //     fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _ATT_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 > _ATT_ANGLE_PREIOD * 1e6)
        //     {
        //         bool roll_over  = fabsf(pid_m_roll.angle_t  - pid_m_roll.angle_c)  > _ATT_ROLL_THRESHOLD;
        //         bool pitch_over = fabsf(pid_m_pitch.angle_t - pid_m_pitch.angle_c) > _ATT_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 > _ATT_HWD_INT_PERIOD * 1e6)
        {
            ctx->acc_int_x = 0;
            ctx->acc_int_y = 0;
            // float leak = dt / _HWD_INT_PERIOD;
            // ctx->acc_int_x *= (1.0f - leak);
            // ctx->acc_int_y *= (1.0f - leak);

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

    /*================ 2. 打反向欧拉 =================*/
    case HDW_STATE_BACK_EULER:
    {
        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 > _ATT_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 * _ATT_GO_BACK_EULER_ANGLE;
    else
        pid_m_roll.angle_t = 0;

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

void attitude_run(void)
{
    hdw_ctx_t *ctx = &hdw_ctx;

    if (comp_rc_status == COMP_NORMAL && rc_signal_health == RC_SIGNAL_HEALTH)
    {
        if (ground_air_status == IN_AIR)
        {
            if(rc_in[RC_CH8] > 1500)
            {                
                // if(ins.insgps_ok == INSGPS)
                // {
                //     control_xy(&_cxy, fast_loop_dt);
                //     no_hdw_reset(ctx); // 复位无互定位下的控制器
                // }
                // else              
                // {
                //    _cxy.reset = true; // 重置互定位控制器
                   // hdw_run_no_insgps(ctx, fast_loop_dt); // 无互定位状态机 
                //}
                
                _cxy.at[0]=0;
                _cxy.at[1]=0;
                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, fast_loop_dt);
                pid_m_pitch.angle_t = apply(euler.pitch * RAD_TO_DEG, t_pitch_last, 15.0f, fast_loop_dt);

                // no_hdw_apply_back_euler(ctx); // 打反向欧拉角时 覆盖期望欧拉
                t_roll_last = pid_m_roll.angle_t;
                t_pitch_last = pid_m_pitch.angle_t;
            }
            else
            {
                _cxy.reset = true;
                get_pilot_desired_lean_angles(rc_in[RC_ROLL], rc_in[RC_PITCH]);
            }           
        }
        else
        {
            _cxy.reset = true;
            get_pilot_desired_lean_angles(rc_in[RC_ROLL], rc_in[RC_PITCH]);

            /* 清除内环积分 */
            clear_rate_i_item(&pid_m_roll);
            clear_rate_i_item(&pid_m_pitch);
            clear_rate_i_item(&pid_m_yaw);
        }
        get_pilot_desired_climb_rate_fromrc(rc_in[RC_THR]);
    }
    else
    {
        _cxy.reset = true;
        get_pilot_desired_lean_angles(1500, 1500);
        get_pilot_desired_yaw_angle_fromrc(1500, fast_loop_dt);
        get_pilot_desired_climb_rate_fromrc(1500);
    }

    Calculate_Target_Az(fast_loop_dt);
}

void attitude_exit(void)
{
    pid_m_posx.vel_i_item = 0.0f;
    pid_m_posy.vel_i_item = 0.0f;

    pid_m_posx.acc_filter_reset = true;
    pid_m_posy.acc_filter_reset = true;

    /* 在有落地上锁检测的模式都需要以下代码，以免进入触地怠速后切出到别的模式无法恢复油门控制
     */
    if (ground_air_status == IN_AIR && throttle_pidctrl_status == THROTTLE_ONGROUND_IDLE)
    {
        throttle_pidctrl_status = THROTTLE_INAIR_RUNNING;
    }
}