/**
 * @file CurtisMotorController.cpp
 * @brief 柯蒂斯电机控制器实现文件
 */

#include "../include/can/CurtisMotorController.h"
#include <iostream>
#include <sstream>
#include <cstring>
#include <windows.h>

// ==================== 构造函数和析构函数 ====================

CurtisMotorController::CurtisMotorController(DWORD deviceType,
                                             DWORD deviceIndex,
                                             DWORD canChannel)
    : m_deviceType(deviceType)
    , m_deviceIndex(deviceIndex)
    , m_canChannel(canChannel)
    , m_isConnected(false)
    , m_statusCallback(nullptr)
{
}

CurtisMotorController::~CurtisMotorController() {
    Disconnect();
}

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

bool CurtisMotorController::Connect() {
    if (m_isConnected) {
        std::cout << "[Curtis] 警告: 已经连接" << std::endl;
        return true;
    }

    std::cout << "[Curtis] 正在连接 Curtis 控制器..." << std::endl;

    // 步骤1: 打开 CAN 设备
    std::cout << "[Curtis] 1/4 打开 CAN 设备 (类型=" << m_deviceType
              << ", 索引=" << m_deviceIndex << ")..." << std::endl;

    DWORD ret = VCI_OpenDevice(m_deviceType, m_deviceIndex, 0);
    if (ret != 1) {
        std::cerr << "[Curtis] 错误: 打开设备失败 (返回值=" << ret << ")" << std::endl;
        return false;
    }
    std::cout << "[Curtis] ✓ 设备打开成功" << std::endl;

    // 步骤2: 配置 CAN 参数（125kbps）
    std::cout << "[Curtis] 2/4 初始化 CAN (125kbps)..." << std::endl;

    VCI_INIT_CONFIG config;
    config.AccCode = 0x80000008;
    config.AccMask = 0xFFFFFFFF;  // 接收所有帧
    config.Filter = 1;             // 接收标准帧和扩展帧
    config.Timing0 = 0x03;         // 125 kbps
    config.Timing1 = 0x1C;
    config.Mode = 0;               // 正常模式
    config.Reserved = 0;

    ret = VCI_InitCAN(m_deviceType, m_deviceIndex, m_canChannel, &config);
    if (ret != 1) {
        std::cerr << "[Curtis] 错误: 初始化 CAN 失败 (返回值=" << ret << ")" << std::endl;
        VCI_CloseDevice(m_deviceType, m_deviceIndex);
        return false;
    }
    std::cout << "[Curtis] ✓ CAN 初始化成功" << std::endl;

    // 步骤3: 启动 CAN 通道
    std::cout << "[Curtis] 3/4 启动 CAN 通道..." << std::endl;
    ret = VCI_StartCAN(m_deviceType, m_deviceIndex, m_canChannel);
    if (ret != 1) {
        std::cerr << "[Curtis] 错误: 启动 CAN 失败 (返回值=" << ret << ")" << std::endl;
        VCI_CloseDevice(m_deviceType, m_deviceIndex);
        return false;
    }
    std::cout << "[Curtis] ✓ CAN 通道启动成功" << std::endl;

    // 步骤4: 清空缓冲区
    std::cout << "[Curtis] 4/4 清空缓冲区..." << std::endl;
    VCI_ClearBuffer(m_deviceType, m_deviceIndex, m_canChannel);
    std::cout << "[Curtis] ✓ 缓冲区清空完成" << std::endl;

    m_isConnected = true;
    std::cout << "[Curtis] ========== 连接成功！=========" << std::endl;
    std::cout << "[Curtis] 准备通讯（请以 ≤20ms 周期发送命令）" << std::endl;

    return true;
}

void CurtisMotorController::Disconnect() {
    if (!m_isConnected) {
        return;
    }

    std::cout << "[Curtis] 正在断开连接..." << std::endl;

    // 发送停止命令
    std::cout << "[Curtis] 发送停止命令..." << std::endl;
    SendStopCommand();
    Sleep(50);  // 等待命令发送

    // 关闭设备
    std::cout << "[Curtis] 关闭 CAN 设备..." << std::endl;
    VCI_CloseDevice(m_deviceType, m_deviceIndex);

    m_isConnected = false;
    std::cout << "[Curtis] ✓ 断开连接完成" << std::endl;
}

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

bool CurtisMotorController::SendCommand(const CurtisCommand& cmd) {
    if (!m_isConnected) {
        std::cerr << "[Curtis] 错误: 未连接到控制器" << std::endl;
        return false;
    }

    // 限制数值范围
    int16_t driveSpeed = cmd.driveSpeed;
    if (driveSpeed < CURTIS_DRIVE_SPEED_MIN) driveSpeed = CURTIS_DRIVE_SPEED_MIN;
    if (driveSpeed > CURTIS_DRIVE_SPEED_MAX) driveSpeed = CURTIS_DRIVE_SPEED_MAX;

    int16_t liftLeft = cmd.liftSpeedLeft;
    if (liftLeft < CURTIS_LIFT_SPEED_MIN) liftLeft = CURTIS_LIFT_SPEED_MIN;
    if (liftLeft > CURTIS_LIFT_SPEED_MAX) liftLeft = CURTIS_LIFT_SPEED_MAX;

    int16_t liftRight = cmd.liftSpeedRight;
    if (liftRight < CURTIS_LIFT_SPEED_MIN) liftRight = CURTIS_LIFT_SPEED_MIN;
    if (liftRight > CURTIS_LIFT_SPEED_MAX) liftRight = CURTIS_LIFT_SPEED_MAX;

    int16_t steerAngle = cmd.steerAngle;
    if (steerAngle < CURTIS_STEER_ANGLE_MIN) steerAngle = CURTIS_STEER_ANGLE_MIN;
    if (steerAngle > CURTIS_STEER_ANGLE_MAX) steerAngle = CURTIS_STEER_ANGLE_MAX;

    // 发送帧1: 0x1E5 - 行走和提升命令
    BYTE data1[8];
    data1[0] = cmd.controlBits0;
    data1[1] = cmd.controlBits1 | CURTIS_CTRL_AGV_CONNECTED;  // 确保AGV标识位
    data1[2] = driveSpeed & 0xFF;
    data1[3] = (driveSpeed >> 8) & 0xFF;
    data1[4] = liftLeft & 0xFF;
    data1[5] = (liftLeft >> 8) & 0xFF;
    data1[6] = liftRight & 0xFF;
    data1[7] = (liftRight >> 8) & 0xFF;

    if (!SendFrame(CURTIS_ID_DRIVE_CMD, data1, 8)) {
        std::cerr << "[Curtis] 错误: 发送 0x1E5 失败" << std::endl;
        return false;
    }

    // 发送帧2: 0x2E5 - 转向命令
    BYTE data2[8] = {0};
    data2[0] = steerAngle & 0xFF;
    data2[1] = (steerAngle >> 8) & 0xFF;
    // Byte2-7 已初始化为0

    if (!SendFrame(CURTIS_ID_STEER_CMD, data2, 8)) {
        std::cerr << "[Curtis] 错误: 发送 0x2E5 失败" << std::endl;
        return false;
    }

    // 保存最后发送的命令
    m_lastCommand = cmd;

    return true;
}

bool CurtisMotorController::SendStopCommand() {
    CurtisCommand stopCmd;
    stopCmd.controlBits0 = 0x00;  // 调速开关有效
    stopCmd.controlBits1 = CURTIS_CTRL_AGV_CONNECTED;
    stopCmd.driveSpeed = 0;
    stopCmd.liftSpeedLeft = 0;
    stopCmd.liftSpeedRight = 0;
    stopCmd.steerAngle = 0;

    return SendCommand(stopCmd);
}

void CurtisMotorController::EmergencyStop() {
    if (!m_isConnected) {
        return;
    }

    std::cout << "[Curtis] !!! 执行紧急停车 !!!" << std::endl;

    CurtisCommand stopCmd;
    stopCmd.controlBits0 = 0x00;
    stopCmd.controlBits1 = CURTIS_CTRL_EMERGENCY_STOP | CURTIS_CTRL_AGV_CONNECTED;
    stopCmd.driveSpeed = 0;
    stopCmd.liftSpeedLeft = 0;
    stopCmd.liftSpeedRight = 0;
    stopCmd.steerAngle = 0;

    // 连续发送3次确保收到
    for (int i = 0; i < 3; i++) {
        SendCommand(stopCmd);
        Sleep(10);
    }
}

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

bool CurtisMotorController::ReceiveStatus() {
    if (!m_isConnected) {
        return false;
    }

    VCI_CAN_OBJ rxFrames[2500];
    DWORD count = VCI_Receive(m_deviceType, m_deviceIndex,
                              m_canChannel, rxFrames, 2500, 0);

    if (count == (DWORD)-1) {
        std::cerr << "[Curtis] 错误: USB 断开或设备不存在" << std::endl;
        m_isConnected = false;
        return false;
    }

    bool statusUpdated = false;

    for (DWORD i = 0; i < count; i++) {
        if (rxFrames[i].ID == CURTIS_ID_STATUS) {
            ParseStatusFrame(rxFrames[i]);
            statusUpdated = true;

            // 调用回调函数
            if (m_statusCallback) {
                m_statusCallback(m_status);
            }
        }
        else if (rxFrames[i].ID == CURTIS_ID_RESERVED) {
            // 0x360 保留帧，暂不处理
        }
    }

    return statusUpdated;
}

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

bool CurtisMotorController::IsTimeout(uint32_t timeoutMs) const {
    if (m_status.lastUpdateTime == 0) {
        return true;  // 从未接收过数据
    }

    uint32_t currentTime = GetTickCount();
    uint32_t elapsed = currentTime - m_status.lastUpdateTime;

    return elapsed > timeoutMs;
}

std::string CurtisMotorController::GetErrorString() const {
    std::ostringstream oss;

    if (m_status.driveError != 0) {
        oss << "行走故障=0x" << std::hex << (int)m_status.driveError << " ";
    }
    if (m_status.steerError != 0) {
        oss << "转向故障=0x" << std::hex << (int)m_status.steerError << " ";
    }
    if (m_status.isEmergencyStop) {
        oss << "[急停] ";
    }

    std::string result = oss.str();
    return result.empty() ? "无故障" : result;
}

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

int16_t CurtisMotorController::SpeedPercentToCAN(float percent) {
    if (percent > 100.0f) percent = 100.0f;
    if (percent < -100.0f) percent = -100.0f;
    return static_cast<int16_t>(percent * 40.95f);
}

float CurtisMotorController::SpeedCANToPercent(int16_t canValue) {
    return canValue / 40.95f;
}

int16_t CurtisMotorController::AngleDegreeToCAN(float degrees) {
    if (degrees > 90.0f) degrees = 90.0f;
    if (degrees < -90.0f) degrees = -90.0f;
    return static_cast<int16_t>(degrees * 10.0f);
}

float CurtisMotorController::AngleCANToDegree(int16_t canValue) {
    return canValue / 10.0f;
}

void CurtisMotorController::PrintStatus() const {
    std::cout << "\n========== Curtis 状态 ==========" << std::endl;
    std::cout << "行走故障: 0x" << std::hex << (int)m_status.driveError
              << "  转向故障: 0x" << (int)m_status.steerError << std::dec << std::endl;
    std::cout << "转向角度: " << m_status.currentAngle << "°" << std::endl;
    std::cout << "电机转速: " << m_status.motorSpeed << std::endl;
    std::cout << "电量: " << (int)m_status.batteryLevel << "%" << std::endl;
    std::cout << "模式: " << (m_status.isAutoMode ? "自动" : "手动")
              << "  急停: " << (m_status.isEmergencyStop ? "是" : "否") << std::endl;
    std::cout << "================================\n" << std::endl;
}

// ==================== 内部辅助函数 ====================

bool CurtisMotorController::SendFrame(UINT canId, const BYTE* data, BYTE len) {
    VCI_CAN_OBJ frame;
    memset(&frame, 0, sizeof(VCI_CAN_OBJ));

    frame.ID = canId;
    frame.SendType = 0;        // 正常发送（自动重试）
    frame.RemoteFlag = 0;      // 数据帧
    frame.ExternFlag = 0;      // 标准帧
    frame.DataLen = len;

    if (len > 8) len = 8;
    memcpy(frame.Data, data, len);

    DWORD sent = VCI_Transmit(m_deviceType, m_deviceIndex,
                              m_canChannel, &frame, 1);

    return sent == 1;
}

void CurtisMotorController::ParseStatusFrame(const VCI_CAN_OBJ& frame) {
    // Byte0: 行走故障代码
    m_status.driveError = frame.Data[0];

    // Byte1: 转向故障代码
    m_status.steerError = frame.Data[1];

    // Byte2-3: 转向角度反馈
    int16_t angleRaw = static_cast<int16_t>((frame.Data[3] << 8) | frame.Data[2]);
    m_status.currentAngle = angleRaw / 10.0f;

    // Byte4-5: 电机转速
    m_status.motorSpeed = static_cast<int16_t>((frame.Data[5] << 8) | frame.Data[4]);

    // Byte6: 电量
    m_status.batteryLevel = frame.Data[6];

    // Byte7: 状态位
    m_status.isAutoMode = (frame.Data[7] & CURTIS_STATUS_AUTO_MODE) != 0;
    m_status.isEmergencyStop = (frame.Data[7] & CURTIS_STATUS_EMERGENCY_STOP) != 0;

    // 更新时间戳
    m_status.lastUpdateTime = GetTickCount();
}
