#ifndef APP_DIFFERENTIAL_DRIVE_H
#define APP_DIFFERENTIAL_DRIVE_H

#ifdef __cplusplus
extern "C"
{
#endif

#include "app_config.h"

#define  SPEED_MODE          0x0C
#define  THROTTLE_MODE       0x05 
#define  TORQUE_MODE         0x0A   
  
#define  MOTOR_MODE          THROTTLE_MODE

#define  SPEED_PID_MODE        0
#define  THROTTLE_PID_MODE     1  
  
  
  
  

#define  SWITCH_SPEED          20.0f
  
  
#define  DIFF_SPEED            20.0f  
  

typedef enum
{
    STATE_INIT,        ///< 初始状态（转速为0且等待扭矩方向判定）
    STATE_FORWARD,     ///< 正向旋转状态
    STATE_BACKWARD,    ///< 反向旋转状态
    DEC_FORWARD,       ///< 正向减速状态
    DEC_FORWARD2,       ///< 正向溜坡状态
    DEC_BACKWARD,      ///< 反向减速状态
    DEC_BACKWARD2      ///< 反向溜坡状态
  
} MotorState;  
  
  
// 状态机内部状态
typedef enum 
{
    STATE_CALIB_INIT, // 初始状态：检测方向一致性
    STATE_CALIB_JUDGE // 判断状态：300ms验证期
} DirState;


typedef enum
{
    FORWARD,   ///< 正向旋转状态
    BACKWARD   ///< 反向旋转状态
} MotorDir;  



typedef enum
{
    MODE_MANUAL, // 手动模式
    MODE_AUTO    // 自动模式    
} ControlMode;

typedef struct DiffData
{
    ControlMode mode ;               // 控制模式
    MotorState state;                //当前状态机状态    
    MotorDir   motor_dir;           // 当前车辆方向  
    DirState   dir_state;          
    
    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;     // 当前右后电机速度
    
    uint8_t out_motor_dir;            // 电机状态    
    
    float speed_last;                 // 速度过去值   
    float torq_last;                  // 扭矩过去值 
    uint8_t counter;                  // 方向判断计数器   
    
    float left_speed_diff;            // 左侧转速差   
    float right_speed_diff;           // 右侧转速差
    
    float left_diff_touue;            // 左侧扭矩差   
    float right_diff_touue;           // 右侧扭矩差  
    
    float diff_dead_zone;             // 差速速度死区     
  
    
    float out_motor_speed[4];
    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;      // 输出右电机速度
    uint16_t max_Torq;                // 最大扭矩限制  
    uint16_t speed_inter;
    
    
} DiffData;


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

void diffAppInit(void);

#ifdef __cplusplus
}
#endif

#endif // APP_DIFFERENTIAL_DRIVE_H