#ifndef __COMMAND_H
#define __COMMAND_H

#define COMMAND_FIFO_BUFFER_SIZE 64

#include "zf_common_typedef.h"
/*
---- 手动控制数据帧 0xA6 (10 字节) ----
[SOF:0xA6][motor_speed_base:2][angle_velocity:4][checksum:1]
motor_speed_base:int16_t 基础直行速度
angle_velocity:float 角度速率，单位度每秒
手动控制数据帧只有在手动模式下才起作用，其他模式不起作用
---- 命令帧 0xA7 (3 字节) ----
[SOF:0xA7][cmd:1][checksum:1]
cmd: 0x01=自动模式, 0x02=手动模式, 0x03=调试模式
---- 调试调参帧 0xA8 (x 字节) ----
包括左右电机速度环KpKiKd、角度环KpKiKd、位置环KpKiKd
调试调参帧只有在调试模式下才起作用，其他模式不起作用
*/
#define COMMAND_SOF_MANUAL 0xA6
#define COMMAND_SOF_MODE   0xA7
#define COMMAND_SOF_DEBUG  0xA8

#define COMMAND_MANUAL_LEN 8   // SOF(1)+motor_speed_base(2)+angle_velocity(4)+checksum(1)
#define COMMAND_MODE_LEN   3   // SOF(1)+cmd(1)+checksum(1)
#define COMMAND_DEBUG_LEN  38  // SOF(1)+9*4(KpKiKd)+checksum(1)

void command_init();
void command_task();
void command_send_angle_velocity(float* pointer_angle_velocity);

/* 工作模式 */
typedef enum
{
    COMMAND_MODE_AUTO   = 0,  // 自动
    COMMAND_MODE_MANUAL = 1,  // 手动
    COMMAND_MODE_DEBUG  = 2,  // 调试
} Vision_Mode_e;
extern volatile Vision_Mode_e current_mode;

void SwitchToAutoMode();
void SwitchToManualMode();
void SwitchToDebugMode();

#endif