#ifndef __CONTROL_RATE_H
#define __CONTROL_RATE_H

#include "flt_butter.h"
#include "stdbool.h"
#include <stdint.h>

struct pid_value_rpy
{
    float angle_p;
    float gyro_p;
    float gyro_i;
    float gyro_d;
    unsigned char gyro_imax;
};
extern struct pid_value_rpy pid_v_roll;
extern struct pid_value_rpy pid_v_pitch;
extern struct pid_value_rpy pid_v_yaw;

struct pid_value_ap
{
    float dist_p;
    float speed_p;
    float acc_p;
    float acc_i;
    float acc_d;
    float brake_gyro;
};
extern struct pid_value_ap pid_v_alt;
extern struct pid_value_ap pid_v_pos;

struct pid_method_rpy
{
    float angle_c; // 当前角度值
    float angle_t; // 目标角度值
    float angle_e; // 角度差

    float angle_p_item; // angle----P值

    float angle_i_item;             // angle----I值
    unsigned int angle_i_time_last; // angle----上次积分的时间戳

    float angle_pid_out; // 当前PID的输出

    float gyro_c;      // 当前角速度值
    float gyro_c_last; //
    float gyro_t;      // 目标角速度值
    float gyro_t_last; // 上一次的目标角速度值
    float gyro_e;      // 角速度差
    float gyro_e_last;

    bool gyro_filter_ready; // 滤波参数清零

    LpfButter2 gyro_dirv_filter;

    float gyro_p_item; // gyro----P值

    float gyro_i_item; // gyro----I值

    float gyro_d_item;      // gyro----D值
    float gyro_d_item_last; // gyro----上一次D值
    float gyro_d_filter;    // D值得滤波系数

    float pid_out; // 当前PID的输出

    float rotate_angle;       // 需要旋转的角度
    int8_t enable_cw_rotate;  // 使能顺时针旋转
    int8_t enable_ccw_rotate; // 使能逆时针旋转
    bool enable_tyaw;
};
extern struct pid_method_rpy pid_m_roll;
extern struct pid_method_rpy pid_m_pitch;
extern struct pid_method_rpy pid_m_yaw;

struct pid_method_ap
{
    int loc_c; // 当前位置
    int loc_t; // 目标位置
    int loc_e; // 位置差

    float loc_p_item; // loction----P值

    float vel_c;           // 当前速度值
    float vel_t;           // 目标速度值
    float vel_t_last;      // 上一次的目标速度值
    float vel_e;           // 速度差
    float vel_e_last;      // 上一次的速度差
    bool vel_filter_ready; // 滤波参数清零

    float vel_p_item;             // vel----P值
    float vel_i_item;             // vel----I值
    unsigned int vel_i_time_last; // vel----上次积分的时间戳
    float vel_d_item;             // vel----D值
    float vel_d_item_last;        // vel----D值上一次的值
    float vel_dv_item;            // vel----D值
    float vel_dv_item_last;       // vel----D值上一次的值

    float accel_ff_item; // 目标速度微分出来的加速度前馈
    LpfButter2 accel_ff_butter_filt;

    float acc_c;           // 当前加速度值
    float acc_c_last;      // 上一次加速度值
    float acc_t;           // 目标加速度值
    float acc_e;           // 加速度差
    float acc_e_last;      // 上一次的加速度的差
    bool acc_filter_reset; // 滤波参数清零

    float acc_p_item; // acc----P值

    float acc_i_item;             // acc----I值
    unsigned int acc_i_time_last; // acc----上次积分的时间戳
    float acc_d_item;             // acc----D值
    bool thr_hold;

    float pid_out_last; // 上次的PID输出
    float pid_out;      // 当前PID的输出
};
extern struct pid_method_ap pid_m_alt;
extern struct pid_method_ap pid_m_posx;
extern struct pid_method_ap pid_m_posy;
extern float attitude_head;

float rate_pid_ctl_rp(struct pid_method_rpy *method,
                      struct pid_value_rpy *value, float dt);
float rate_pid_ctl_yaw(struct pid_method_rpy *method,
                       struct pid_value_rpy *value, float dt);
float rate_pid_ctl_thr(void);

void clear_rate_i_item(struct pid_method_rpy *method);
void init_attitude_head(void);

#endif