/**
 * @file CurtisMotorController.h
 * @brief 柯蒂斯（Curtis）1298+1220C 电机控制器 CAN 通讯接口
 * @details 封装柯蒂斯电机控制器的 CAN 通讯协议，提供易用的控制接口
 *
 * 协议版本: V4.0
 * 波特率: 125 kbps
 * CAN ID:
 *   - 0x1E5: AGV → Curtis 控制命令（行走、提升）
 *   - 0x2E5: AGV → Curtis 转向命令
 *   - 0x260: Curtis → AGV 状态反馈
 *   - 0x360: Curtis → AGV 保留
 */

#ifndef CURTIS_MOTOR_CONTROLLER_H
#define CURTIS_MOTOR_CONTROLLER_H

#include "../../lib/ControlCAN.h"
#include <stdint.h>
#include <string>
#include <functional>

// ==================== CAN ID 定义 ====================
#define CURTIS_ID_DRIVE_CMD     0x1E5    ///< 行走/提升命令
#define CURTIS_ID_STEER_CMD     0x2E5    ///< 转向命令
#define CURTIS_ID_STATUS        0x260    ///< 状态反馈
#define CURTIS_ID_RESERVED      0x360    ///< 保留

// ==================== 数据范围定义 ====================
#define CURTIS_DRIVE_SPEED_MIN         -4096
#define CURTIS_DRIVE_SPEED_MAX         4095
#define CURTIS_LIFT_SPEED_MIN          -4096
#define CURTIS_LIFT_SPEED_MAX          4095
#define CURTIS_STEER_ANGLE_MIN         -900     ///< -90度
#define CURTIS_STEER_ANGLE_MAX         900      ///< +90度

// ==================== 控制位掩码定义（Byte0）====================
#define CURTIS_CTRL_SPEED_SWITCH        0x01  ///< 调速开关（0=有效）
#define CURTIS_CTRL_EMERGENCY_REVERSE   0x02  ///< 紧急反向
#define CURTIS_CTRL_TURTLE_MODE         0x04  ///< 龟速开关
#define CURTIS_CTRL_HORN                0x08  ///< 喇叭
#define CURTIS_CTRL_LIFT_LEFT_UP        0x10  ///< 左侧提升
#define CURTIS_CTRL_LIFT_LEFT_DOWN      0x20  ///< 左侧下降
#define CURTIS_CTRL_LIFT_RIGHT_UP       0x40  ///< 右侧提升
#define CURTIS_CTRL_LIFT_RIGHT_DOWN     0x80  ///< 右侧下降

// ==================== 控制位掩码定义（Byte1 - AGV专用）====================
#define CURTIS_CTRL_AGV_CONNECTED       0x01  ///< AGV连接标识（必须设置）
#define CURTIS_CTRL_EMERGENCY_STOP      0x02  ///< 紧急停车
#define CURTIS_CTRL_CENTERING_DONE      0x04  ///< 找中完成

// ==================== 状态位掩码定义（Byte7）====================
#define CURTIS_STATUS_AUTO_MODE         0x01  ///< 0=手动, 1=自动
#define CURTIS_STATUS_EMERGENCY_STOP    0x02  ///< 0=无急停, 1=急停

/**
 * @brief Curtis 控制器状态结构
 */
struct CurtisStatus {
    uint8_t  driveError;        ///< 行走故障代码
    uint8_t  steerError;        ///< 转向故障代码
    float    currentAngle;      ///< 当前转向角度（度）
    int16_t  motorSpeed;        ///< 电机转速
    uint8_t  batteryLevel;      ///< 电量百分比 0-100
    bool     isAutoMode;        ///< 是否自动模式
    bool     isEmergencyStop;   ///< 是否急停状态
    uint32_t lastUpdateTime;    ///< 最后更新时间戳（毫秒）

    CurtisStatus() : driveError(0), steerError(0), currentAngle(0.0f),
                     motorSpeed(0), batteryLevel(0), isAutoMode(false),
                     isEmergencyStop(false), lastUpdateTime(0) {}
};

/**
 * @brief Curtis 控制命令结构
 */
struct CurtisCommand {
    int16_t driveSpeed;         ///< 行走速度 -4096~4095
    int16_t liftSpeedLeft;      ///< 左侧提升速度 -4096~4095
    int16_t liftSpeedRight;     ///< 右侧提升速度 -4096~4095
    int16_t steerAngle;         ///< 转向角度 -900~900
    uint8_t controlBits0;       ///< 控制位0
    uint8_t controlBits1;       ///< 控制位1（AGV专用）

    CurtisCommand() : driveSpeed(0), liftSpeedLeft(0), liftSpeedRight(0),
                      steerAngle(0), controlBits0(0), controlBits1(CURTIS_CTRL_AGV_CONNECTED) {}
};

/**
 * @brief 柯蒂斯电机控制器类
 *
 * 提供完整的柯蒂斯 1298+1220C 电机控制器 CAN 通讯功能
 *
 * 使用流程：
 * 1. 创建对象
 * 2. 调用 Connect() 连接设备
 * 3. 在主循环中以 ≤20ms 周期调用 SendCommand() 和 ReceiveStatus()
 * 4. 结束时调用 Disconnect()
 *
 * @code
 * CurtisMotorController curtis;
 * curtis.Connect();
 *
 * while (running) {
 *     CurtisCommand cmd;
 *     cmd.driveSpeed = SpeedPercentToCAN(30.0f);
 *     curtis.SendCommand(cmd);
 *     curtis.ReceiveStatus();
 *     Sleep(15);  // 15ms周期
 * }
 *
 * curtis.Disconnect();
 * @endcode
 */
class CurtisMotorController {
public:
    /**
     * @brief 状态回调函数类型
     * @param status 更新后的状态
     */
    using StatusCallback = std::function<void(const CurtisStatus&)>;

    /**
     * @brief 构造函数
     * @param deviceType CAN 设备类型（VCI_USBCAN2 = 4）
     * @param deviceIndex 设备索引（第几个设备，从0开始）
     * @param canChannel CAN 通道（0 = CAN1, 1 = CAN2）
     */
    CurtisMotorController(DWORD deviceType = VCI_USBCAN2,
                          DWORD deviceIndex = 0,
                          DWORD canChannel = 0);

    /**
     * @brief 析构函数，自动断开连接
     */
    ~CurtisMotorController();

    // ==================== 连接管理 ====================

    /**
     * @brief 连接到 Curtis 控制器
     * @details 执行完整的连接流程：
     *   1. 打开 CAN 设备
     *   2. 初始化 CAN（125kbps）
     *   3. 启动 CAN 通道
     *   4. 清空缓冲区
     * @return true=成功, false=失败
     */
    bool Connect();

    /**
     * @brief 断开与 Curtis 控制器的连接
     * @details 执行安全断开流程：
     *   1. 发送停止命令
     *   2. 关闭 CAN 设备
     */
    void Disconnect();

    /**
     * @brief 检查是否已连接
     * @return true=已连接, false=未连接
     */
    bool IsConnected() const { return m_isConnected; }

    // ==================== 命令发送 ====================

    /**
     * @brief 发送控制命令到 Curtis 控制器
     * @details 发送两个 CAN 帧：
     *   - 0x1E5: 行走和提升命令
     *   - 0x2E5: 转向命令
     * @param cmd 控制命令
     * @return true=成功, false=失败
     * @note 必须以 ≤20ms 周期持续发送，否则控制器会超时保护
     */
    bool SendCommand(const CurtisCommand& cmd);

    /**
     * @brief 发送停止命令
     * @details 所有速度和角度设为0
     * @return true=成功, false=失败
     */
    bool SendStopCommand();

    /**
     * @brief 紧急停车
     * @details 连续发送3次急停命令确保接收
     */
    void EmergencyStop();

    // ==================== 状态接收 ====================

    /**
     * @brief 接收 Curtis 控制器状态反馈
     * @details 接收并解析 0x260 状态帧，更新内部状态
     * @return true=接收到新状态, false=没有新数据
     */
    bool ReceiveStatus();

    /**
     * @brief 获取当前状态
     * @return 状态结构的引用
     */
    const CurtisStatus& GetStatus() const { return m_status; }

    /**
     * @brief 获取上次发送的命令
     * @return 命令结构的引用
     */
    const CurtisCommand& GetLastCommand() const { return m_lastCommand; }

    /**
     * @brief 设置状态更新回调函数
     * @param callback 状态更新时调用的函数
     */
    void SetStatusCallback(StatusCallback callback) {
        m_statusCallback = callback;
    }

    // ==================== 故障和超时检测 ====================

    /**
     * @brief 检查是否有故障
     * @return true=有故障, false=无故障
     */
    bool HasError() const {
        return m_status.driveError != 0 || m_status.steerError != 0;
    }

    /**
     * @brief 检查通讯是否超时
     * @param timeoutMs 超时时间（毫秒）
     * @return true=超时, false=正常
     */
    bool IsTimeout(uint32_t timeoutMs = 100) const;

    /**
     * @brief 获取故障信息字符串
     * @return 故障描述
     */
    std::string GetErrorString() const;

    // ==================== 工具函数 ====================

    /**
     * @brief 速度百分比转 CAN 值
     * @param percent 速度百分比 -100% ~ +100%
     * @return CAN 值 -4096 ~ +4095
     */
    static int16_t SpeedPercentToCAN(float percent);

    /**
     * @brief CAN 值转速度百分比
     * @param canValue CAN 值 -4096 ~ +4095
     * @return 速度百分比 -100% ~ +100%
     */
    static float SpeedCANToPercent(int16_t canValue);

    /**
     * @brief 角度转 CAN 值（转向命令）
     * @param degrees 角度 -90° ~ +90°
     * @return CAN 值 -900 ~ +900
     */
    static int16_t AngleDegreeToCAN(float degrees);

    /**
     * @brief CAN 值转角度（状态反馈）
     * @param canValue CAN 值 -16383 ~ +16383
     * @return 角度 -90° ~ +90°
     */
    static float AngleCANToDegree(int16_t canValue);

    /**
     * @brief 打印当前状态到控制台
     */
    void PrintStatus() const;

private:
    // 设备参数
    DWORD m_deviceType;
    DWORD m_deviceIndex;
    DWORD m_canChannel;
    bool m_isConnected;

    // 状态和命令
    CurtisStatus m_status;
    CurtisCommand m_lastCommand;

    // 回调函数
    StatusCallback m_statusCallback;

    // 内部辅助函数
    bool SendFrame(UINT canId, const BYTE* data, BYTE len);
    void ParseStatusFrame(const VCI_CAN_OBJ& frame);
};

#endif // CURTIS_MOTOR_CONTROLLER_H