#ifndef MASTER_MACHINE_H
#define MASTER_MACHINE_H

#include "bsp_usart.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
#include "usart.h"
#include "daemon.h"
#include <string.h>

/*
 * 上位机串口协议 — 四种帧类型, 通过帧头区分
 *
 * ---- 自动/视觉追踪数据帧 0xA5 (10 字节) ----
 *   [SOF:0xA5][error_x:4][error_y:4][checksum:1]
 *   error_x/y: float, 归一化误差, 视觉PID外环反馈值
 *
 * ---- 手动控制数据帧 0xA6 (10 字节) ----
 *   [SOF:0xA6][yaw_velocity:4][pitch_velocity:4][checksum:1]
 *   yaw_velocity/pitch_velocity: float, 角速度 (degrees/s)
 *
 * ---- 命令帧 0xA7 (3 字节) ----
 *   [SOF:0xA7][cmd:1][checksum:1]
 *   cmd: 0x01=自动模式, 0x02=手动模式, 0x03=激光开, 0x04=激光关, 0x05=调试模式
 *
 * ---- 调试调参帧 0xA8 (50 字节) ----
 *   [SOF:0xA8][vision_yaw_Kp:4][vision_yaw_Ki:4][vision_yaw_Kd:4]
 *   [vision_pitch_Kp:4][vision_pitch_Ki:4][vision_pitch_Kd:4]
 *   [motor_yaw_Kp:4][motor_yaw_Ki:4][motor_yaw_Kd:4]
 *   [motor_pitch_Kp:4][motor_pitch_Ki:4][motor_pitch_Kd:4][checksum:1]
 *   全部 float, 依次为视觉环 yaw/pitch 和电机角度环 yaw/pitch 的 PID 参数
 *
 * checksum: 帧头到数据末尾前一字节累加和取低8位
 */

#define VISION_SOF_AUTO    0xA5
#define VISION_SOF_MANUAL  0xA6
#define VISION_SOF_CMD     0xA7
#define VISION_SOF_DEBUG   0xA8

#define VISION_AUTO_LEN    10
#define VISION_MANUAL_LEN  10
#define VISION_CMD_LEN     3
#define VISION_DEBUG_LEN   50

/* 命令码 */
#define VISION_CMD_SWITCH_AUTO   0x01
#define VISION_CMD_SWITCH_MANUAL 0x02
#define VISION_CMD_LASER_ON       0x03
#define VISION_CMD_LASER_OFF      0x04
#define VISION_CMD_SWITCH_DEBUG   0x05

/* 工作模式 */
typedef enum
{
    VISION_MODE_AUTO   = 0,  // 自动/视觉追踪
    VISION_MODE_MANUAL = 1,  // 手动
    VISION_MODE_DEBUG  = 2,  // 调试
} Vision_Mode_e;

typedef struct
{
    /* 自动模式数据 */
    float error_x;
    float error_y;
    uint8_t auto_updated;   // 收到新的自动数据帧标志

    /* 手动模式数据 */
    float yaw_degree_velocity;
    float pitch_degree_velocity;
    uint8_t manual_updated; // 收到新的手动数据帧标志

    /* 调试模式数据 */
    float vision_yaw_Kp, vision_yaw_Ki, vision_yaw_Kd;
    float vision_pitch_Kp, vision_pitch_Ki, vision_pitch_Kd;
    float motor_yaw_Kp, motor_yaw_Ki, motor_yaw_Kd;
    float motor_pitch_Kp, motor_pitch_Ki, motor_pitch_Kd;
    uint8_t debug_updated;

    /* 命令 */
    uint8_t cmd;            // 最新命令码
    uint8_t cmd_pending;    // 有待处理命令标志

    float frequency;        // 接收频率 (Hz)
} VisionData_t;

extern VisionData_t vision_data;

void Vision_Uart_Init(UART_HandleTypeDef *_handle);

#endif // MASTER_MACHINE_H