#ifndef APP_DIFFERENTIAL_DRIVE_H
#define APP_DIFFERENTIAL_DRIVE_H

#ifdef __cplusplus
extern "C"
{
#endif

#include "app_config.h"

#define  SPEED_FITER_NUM       6  
  
  
#define  SPEED_PID_MODE        1
#define  THROTTLE_PID_MODE     0  
  
#define  TURN_MIN_TOUQUE       1   //n*m

#define  SPEED_MODE          0x01
#define  TORQUE_MODE         0x02   
  
  
#define  TORQUE_HYSTERESIS_THRESHOLD           0.3f   
  
  
#define  MOTOR_MODE          SPEED_MODE  
 
  
#define  ALPHA                 0.1f  // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
#define  LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev))
  
// 状态机内部状态
typedef enum
{
    STATE_INIT,        ///< 初始状态（转速为0且等待扭矩方向判定）
    STATE_FORWARD,     ///< 前进
    STATE_BACKWARD,    ///< 后退
} MotorState;  
  
typedef enum
{
    MODE_MANUAL, // 手动模式
    MODE_AUTO    // 自动模式    
} ControlMode;

typedef struct DiffData
{
    ControlMode mode ;               // 控制模式
    MotorState motor_state[4];       //当前车辆状态    
    float desired_speed;             // 期望速度    
    float desired_curvature;         // 期望曲率
    float left_motor_speed;          // 当前左电机速度
    float right_motor_speed;         // 当前右电机速度
    float left_front_motor_speed;    // 当前左前电机速度
    float right_front_motor_speed;   // 当前右前电机速度    
    float left_rear_motor_speed;     // 当前左后电机速度
    float right_rear_motor_speed;    // 当前右后电机速度     
    float speed;                     // 当前车速
    float curvature;                 // 当前曲率
    float yaw_rate;                  // 当前角速度
    float desired_yaw_rate;          // 期望角速度
    float acceleration;              // 当前加速度
    float deceleration;              // 当前减速度
    float max_speed;                 // 最大速度
    float desired_acceleration;      // 期望加速度
    float desired_deceleration;      // 期望减速度
    uint8_t emergency_stop_switch;   // 急停开关
    uint8_t remote_emergency_stop;   // 遥控器急停开关
    uint8_t emergency_stop_state;    // 急停状态
    float out_left_motor_speed;      // 输出左电机速度
    float out_right_motor_speed;     // 输出右电机速度
    float out_torq[4];               //4个电机扭矩
    float max_Torq;                  // 最大扭矩限制   
    float min_Torq;                  // 最小扭矩限制 
    uint16_t  left_motor_feed_power;     // 左侧馈电功率 
    uint16_t  right_motor_feed_power;    // 右侧馈电功率
    uint8_t   left_motor_state;        //左侧电机状态，1刹车，0停下或加速  2驻车
    uint8_t   right_motor_state;       //右侧电机状态，1刹车，0停下或加速 
    uint16_t  max_feed_power;         //最大馈电功率     
    
} DiffData;


// 声明外部变量
extern DiffData diff_data;

void diffAppInit(void);

#ifdef __cplusplus
}
#endif

#endif // APP_DIFFERENTIAL_DRIVE_H