#include "control_throttle.h"
#include "flight_mode.h"
#include "helpler_funtions.h"
#include "lowpass_filter.h"
#include "my_math.h"
#include "params.h"
#include "soft_gs.h"
#include "soft_motor_output.h"
#include "soft_rc_input.h"
#include "soft_time.h"
#include "soft_imu.h"
#include <control_rate.h>
#include <math.h>

// malloc测试，定义结构体指针的话必须要malloc申请空间，
// 否则只会分配4字节的指针空间，不会自动分配结构体内存空间，导致无法计算以及赋值
// 所以决定定义变量时都定义为变量，不定义指针，
struct pid_method_rpy pid_m_roll;
struct pid_method_rpy pid_m_pitch;
struct pid_method_rpy pid_m_yaw;
struct pid_method_ap pid_m_alt;
struct pid_method_ap pid_m_posx;
struct pid_method_ap pid_m_posy;

struct pid_value_rpy pid_v_roll;
struct pid_value_rpy pid_v_pitch;
struct pid_value_rpy pid_v_yaw;
struct pid_value_ap pid_v_alt;
struct pid_value_ap pid_v_pos;

// 陀螺积分的虚拟机头方向
float attitude_head = 0.0f;

/*
内环的速率控制函数
*/
float rate_pid_ctl_rp(struct pid_method_rpy *method, struct pid_value_rpy *value, float dt)
{
    // 计算目标角速度与当前角度的差
    method->gyro_e = method->gyro_t - method->gyro_c;

    if (method->gyro_filter_ready != true)
    {
        method->gyro_filter_ready = true;
        butter2_filter_init(&method->gyro_dirv_filter, 400, 25);
        // butter2_filter_init(&method->gyro_dirv_filter, 400, 20);
        method->gyro_d_item = 0.f;
    }
    else
    {
        float dirv = 0.f;
        if (dt > 0.f)
        {
            dirv = value->gyro_d * (method->gyro_e - method->gyro_e_last) / dt;
            dirv = constrain_float(dirv, -120, 120);
            method->gyro_e_last = method->gyro_e;
        }

        method->gyro_d_item = butter2_filter_apply(&method->gyro_dirv_filter, dirv);
    }

    if (ground_air_status == ON_GROUND)
    {
        method->gyro_d_item = 0.f;
        butter2_filter_reset(&method->gyro_dirv_filter, 0);
    }

    // 计算角速度的P值
    method->gyro_p_item = constrain_float(method->gyro_e, -250, 250) * value->gyro_p;

    method->gyro_i_item += method->gyro_e * dt * value->gyro_i * 10.0f;
    // 角速度的I值得限幅
    method->gyro_i_item = constrain_float(method->gyro_i_item, -value->gyro_imax, value->gyro_imax);

   
    method->pid_out = method->gyro_p_item + method->gyro_i_item + method->gyro_d_item;

    if ((control_mode == CONTROL_REMOTE) &&
        (rc_signal_health == RC_SIGNAL_HEALTH) && (rc_in[RC_CH1] < 1050) &&
        
        (rc_in[RC_CH4] > 1950) && (rc_in[RC_CH3] < 1050) &&
        (rc_in[RC_CH2] > 1950))
        method->pid_out = 0.0f;

    return method->pid_out;
}

/*
内环的速率控制函数
*/
float rate_pid_ctl_yaw(struct pid_method_rpy *method,
                       struct pid_value_rpy *value, float dt)
{
    // 如果电机输出有饱和，则目标航向角速度指数递减
    if (MotorOutput_GetYawRestrictionStatus())
    {
        pid_m_yaw.gyro_t = 0.5f * pid_m_yaw.gyro_c;
    }

    // 计算目标角速度与当前角度的差
    method->gyro_e = method->gyro_t - method->gyro_c;

    if (method->gyro_filter_ready != true)
    {
        method->gyro_filter_ready = true;
        butter2_filter_init(&method->gyro_dirv_filter, 400, 30);
        method->gyro_d_item = 0.f;
    }
    else
    {
        float dirv = 0.f;
        if (dt > 0.f)
        {
            dirv = value->gyro_d * (method->gyro_e - method->gyro_e_last) / dt;
            dirv = constrain_float(dirv, -100, 100);
            method->gyro_e_last = method->gyro_e;
        }

        method->gyro_d_item = butter2_filter_apply(&method->gyro_dirv_filter, dirv);
    }

    if (ground_air_status == ON_GROUND)
    {
        method->gyro_d_item = 0.f;
        butter2_filter_reset(&method->gyro_dirv_filter, 0);
    }

    // 角速度差限幅
    method->gyro_e = constrain_float(method->gyro_e, -200.0f, 200.0f);

    // 计算角速度的P值
    method->gyro_p_item = method->gyro_e * value->gyro_p;

    // 计算角速度的I值
    method->gyro_i_item += method->gyro_e * dt * value->gyro_i * 10.0f;
    // 角速度的I值得限幅

    if (MotorOutput_GetYawRestrictionStatus())
    {
        method->gyro_i_item = constrain_float( method->gyro_i_item, -2.0f * value->gyro_imax, 2.0f * value->gyro_imax);
    }
    else
    {
        method->gyro_i_item = constrain_float(method->gyro_i_item, -value->gyro_imax, value->gyro_imax);
    }

    method->pid_out =
        method->gyro_p_item + method->gyro_i_item + method->gyro_d_item;
    method->pid_out = constrain_float(method->pid_out, -300.0f, +300.0f);

    if ((control_mode == CONTROL_REMOTE) &&
        (rc_signal_health == RC_SIGNAL_HEALTH) && (rc_in[RC_CH1] < 1050) &&
        (rc_in[RC_CH4] > 1950) && (rc_in[RC_CH3] < 1050) &&
        (rc_in[RC_CH2] > 1950))
        method->pid_out = 0.0f;

    return method->pid_out;
}
extern float thr_max; // 前面上升阶段为了减小电流 限制油门最大为 1400 
/*
内环获取油门量
*/
float rate_pid_ctl_thr(void)
{
    float throttle_ctrl_value = 1000.0f;

    /* 根据油门控制状态标志，选择油门输出是 灭车、控制量、怠速 */
    switch (throttle_pidctrl_status)
    {
    case THROTTLE_INAIR_RUNNING:
        if ((rc_in[RC_CH8] < 1500) && (comp_rc_status == COMP_NORMAL) &&
            (rc_signal_health == RC_SIGNAL_HEALTH) &&
            (control_mode == CONTROL_REMOTE))
        {
            throttle_ctrl_value = rc_in[RC_CH3];
            throttle_ctrl_value = constrain_float(throttle_ctrl_value, 1000.0f, 2000.0f);
            base_thr = throttle_ctrl_value;
            pid_m_alt.acc_i_item = 0;
        }
        else
        {
            if (base_thr < HOVER_THR) // 电机怠速是默认等级2 怠速油门10 基础油门宏默认是应该给到1300 如果后续需要响应高度需要变换 
                base_thr += 1.0f;

            throttle_ctrl_value = base_thr + pid_m_alt.pid_out;
            throttle_ctrl_value = constrain_float(throttle_ctrl_value, 1000.0f, thr_max);
        }
        break;

    case THROTTLE_ONGROUND_IDLE:
        if ((rc_in[RC_CH8] < 1500) && (comp_rc_status == COMP_NORMAL) &&
            (rc_signal_health == RC_SIGNAL_HEALTH) &&
            (control_mode == CONTROL_REMOTE))
        {
            throttle_ctrl_value = rc_in[RC_CH3];
            base_thr = throttle_ctrl_value;
            pid_m_alt.acc_i_item = 0;
        }
        else
        {
            throttle_ctrl_value = conf_par.idle_speed + 25.0f;
        }
        break;

    case THROTTLE_OFF:
    default:
        break;
    }
    return throttle_ctrl_value;
}

/*
清零积分项
*/
void clear_rate_i_item(struct pid_method_rpy *method)
{
    method->gyro_i_item = 0.0f;
}

void init_attitude_head(void)
{
    attitude_head = pid_m_yaw.angle_c;
    pid_m_yaw.angle_t = pid_m_yaw.angle_c;
}