#pragma once

#ifdef __cplusplus
extern "C"
{
#endif

#include "bsp/struct_typedef.h"
#include "device/motor_dm.h"
#include "component/pid.h"
#include "device/motor_rm.h"
#include "component/ahrs.h"
#include "module/cmd.h"

#define GIMBAL_OK 0
#define GIMBAL_ERR -1
#define GIMBAL_ERR_NULL -2
#define GIMBAL_ERR_MODE (-3) 
#define GIMBAL_ERR_TYPE (-4) 




    /* 软件限位 */
    typedef struct
    {
        float max;
        float min;
    } Gimbal_Limit_t;

    /* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
    typedef struct
    {
        MOTOR_RM_Param_t yaw_6020_motor;   /* yaw轴6020电机参数 */
        MOTOR_DM_Param_t yaw_4310_motor;   /* yaw轴4310电机参数 */
        MOTOR_DM_Param_t pitch_4310_motor; /* pitch轴6020电机参数 */
        struct
        {
            KPID_Params_t yaw_6020_motor_omega;   /* yaw轴6020角速度环PID参数 */
            KPID_Params_t yaw_6020_motor_angle;   /* yaw轴6020角位置环PID参数 */
            KPID_Params_t yaw_4310_motor_omega;   /* yaw轴4310角速度环PID参数 */
            KPID_Params_t yaw_4310_motor_angle;   /* yaw轴4310角位置环PID参数 */
            KPID_Params_t pitch_4310_motor_omega; /* pitch轴角速度环PID参数 */
            KPID_Params_t pitch_4310_motor_angle; /* pitch轴角位置环PID参数 */
        } pid;

        /* 低通滤波器截止频率 */
        struct
        {
            float out;  /* 电机输出 */
            float gyro; /* 陀螺仪数据 */
        } low_pass_cutoff_freq;

        struct
        {
            float yaw_6020;   /* 小yaw轴6020机械限位 */
            float yaw_4310;   /* 大yaw轴4310机械限位 */
            float pitch_4310; /* pitch轴4310机械限位 */
        } mech_zero;

        struct
        {
            float yaw_6020;   /* 小yaw轴6020机械限位行程 -1表示无限位 */
            float yaw_4310;   /* 大yaw轴4310机械限位行程 -1表示无限位*/
            float pitch_4310; /* pitch轴4310机械限位行程 -1表示无限位*/
        } travel;

    } Gimbal_Param_t;

    typedef struct
    {
        AHRS_Gyro_t gyro;
        AHRS_Eulr_t eulr;
    } Gimbal_IMU_t;

    /* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
    typedef struct
    {
        Gimbal_IMU_t imu; 
        struct
        {
            MOTOR_Feedback_t yaw_4310_motor_feedback;
            MOTOR_Feedback_t pitch_4310_motor_feedback;
            MOTOR_Feedback_t yaw_6020_motor_feedback;
        } motor;
    } Gimbal_feedback_t;

    /* 云台输出数据的结构体*/
    typedef struct
    {
        float yaw_6020;   /* yaw轴6020电机输出 */
        float yaw_4310;   /* yaw轴4310电机输出 */
        float pitch_4310; /* pitch轴4310电机输出 */
    } Gimbal_Output_t;

    typedef struct
    {
        uint64_t last_wakeup;
        float dt;
        Gimbal_Param_t *param; /* 云台的参数，用Gimbal_Init设定 */

        /* 模块通用 */
        Gimbal_Mode_t mode; /* 云台模式 */

        /* PID计算的目标值 */
        struct
        {
            AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
            float yaw_4310; /* 大yaw电机目标角度 */
        } setpoint;

        struct
        {
            KPID_t yaw_6020_angle;   /* yaw轴6020角位置环PID */
            KPID_t yaw_6020_omega;   /* yaw轴6020角速度环PID */
            KPID_t yaw_4310_angle;   /* yaw轴4310角位置环PID */
            KPID_t yaw_4310_omega;   /* yaw轴4310角速度环PID */
            KPID_t pitch_4310_angle; /* pitch轴4310角位置环PID */
            KPID_t pitch_4310_omega; /* pitch轴4310角速度环PID */
        } pid;

        struct
        {
            Gimbal_Limit_t yaw_6020;
            Gimbal_Limit_t yaw_4310;
            Gimbal_Limit_t pitch_4310;
        } limit;

        struct
        {
            LowPassFilter2p_t yaw_6020;
            LowPassFilter2p_t yaw_4310;
            LowPassFilter2p_t pitch_4310;
        } filter_out;

        Gimbal_Output_t out;        /* 云台输出 */
        Gimbal_feedback_t feedback; /* 反馈 */

		
    } Gimbal_t;


  /* Exported functions prototypes -------------------------------------------- */

/**
 * \brief 初始化云台
 *
 * \param g 包含云台数据的结构体
 * \param param 包含云台参数的结构体指针
 * \param target_freq 任务预期的运行频率
 *
 * \return 函数运行结果
 */
int8_t Gimbal_Init(Gimbal_t *g,  Gimbal_Param_t *param,
                   float target_freq);

/**
 * \brief 通过CAN设备更新云台反馈信息
 *
 * \param gimbal 云台
 * \param can CAN设备
 *
 * \return 函数运行结果
 */
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal);

int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu);
/**
 * \brief 运行云台控制逻辑
 *
 * \param g 包含云台数据的结构体
 * \param fb 云台反馈信息
 * \param g_cmd 云台控制指令
 * \param dt_sec 两次调用的时间间隔
 *
 * \return 函数运行结果
 */
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);

/**
 * \brief 云台输出
 *
 * \param s 包含云台数据的结构体
 * \param out CAN设备云台输出结构体
 */
void Gimbal_Output(Gimbal_t *g);

#ifdef __cplusplus
}
#endif