/**
 * @file curtis_demo.cpp
 * @brief Curtis 电机控制器键盘控制演示程序
 *
 * 功能说明：
 * - 演示 Curtis 电机控制器的完整使用流程
 * - 提供键盘控制接口（WASD + QE + 空格 + ESC）
 * - 实时显示控制器状态
 * - 故障和急停检测
 *
 * 编译：
 *   cd build
 *   cmake ..
 *   make curtis_demo
 *
 * 运行：
 *   ./curtis_demo
 */

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

using namespace std;

/**
 * @brief 打印控制说明
 */
void printInstructions() {
    cout << "\n===== Curtis 电机控制器键盘控制演示 =====" << endl;
    cout << "\n控制说明：" << endl;
    cout << "  W/S - 前进/后退 (30%速度)" << endl;
    cout << "  A/D - 左转/右转 (30度)" << endl;
    cout << "  Q/E - 提升/下降 (50%速度)" << endl;
    cout << "  H   - 鸣喇叭" << endl;
    cout << "  T   - 切换龟速模式" << endl;
    cout << "  空格 - 全部停止" << endl;
    cout << "  ESC - 退出程序" << endl;
    cout << "  P   - 打印状态" << endl;
    cout << "==========================================\n" << endl;
}

/**
 * @brief 主函数
 */
int main() {
    cout << "===== Curtis 电机控制器 CAN 通讯演示 =====\n" << endl;

    // ============ 1. 创建控制器对象 ============
    cout << "步骤 1: 创建 Curtis 控制器对象..." << endl;
    CurtisMotorController curtis(VCI_USBCAN2, 0, 0);

    // ============ 2. 连接 ============
    cout << "\n步骤 2: 连接到 Curtis 控制器..." << endl;
    if (!curtis.Connect()) {
        cerr << "连接失败！请检查：" << endl;
        cerr << "  1. CAN 设备是否已连接" << endl;
        cerr << "  2. 驱动是否正确安装" << endl;
        cerr << "  3. 设备权限是否正确" << endl;
        return -1;
    }

    // 等待控制器稳定
    Sleep(100);

    // ============ 3. 主控制循环 ============
    cout << "\n步骤 3: 进入主控制循环" << endl;
    printInstructions();

    bool running = true;
    bool turtleMode = false;  // 龟速模式

    CurtisCommand cmd;
    cmd.controlBits0 = 0x00;  // 调速开关有效（bit0=0）
    cmd.controlBits1 = CURTIS_CTRL_AGV_CONNECTED;

    DWORD lastSendTime = GetTickCount();
    DWORD lastPrintTime = GetTickCount();
    DWORD lastStatusCheckTime = GetTickCount();
    int loopCount = 0;
    int errorCount = 0;

    // 设置状态回调（可选）
    curtis.SetStatusCallback([](const CurtisStatus& status) {
        // 状态更新时自动调用
        if (status.driveError || status.steerError) {
            cout << "[警告] 检测到故障！" << endl;
        }
    });

    while (running) {
        DWORD currentTime = GetTickCount();

        // ---- 接收状态（每次循环） ----
        curtis.ReceiveStatus();

        // ---- 键盘控制 ----
        if (_kbhit()) {
            char ch = _getch();

            switch (ch) {
                case 'w': case 'W':
                    cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(30.0f);
                    cout << "[控制] 前进 30%" << endl;
                    break;

                case 's': case 'S':
                    cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(-30.0f);
                    cout << "[控制] 后退 30%" << endl;
                    break;

                case 'a': case 'A':
                    cmd.steerAngle = CurtisMotorController::AngleDegreeToCAN(-30.0f);
                    cout << "[控制] 左转 30°" << endl;
                    break;

                case 'd': case 'D':
                    cmd.steerAngle = CurtisMotorController::AngleDegreeToCAN(30.0f);
                    cout << "[控制] 右转 30°" << endl;
                    break;

                case 'q': case 'Q':
                    cmd.liftSpeedLeft = CurtisMotorController::SpeedPercentToCAN(50.0f);
                    cout << "[控制] 提升" << endl;
                    break;

                case 'e': case 'E':
                    cmd.liftSpeedLeft = CurtisMotorController::SpeedPercentToCAN(-50.0f);
                    cout << "[控制] 下降" << endl;
                    break;

                case 'h': case 'H':
                    cmd.controlBits0 |= CURTIS_CTRL_HORN;
                    cout << "[控制] 鸣喇叭" << endl;
                    Sleep(500);
                    cmd.controlBits0 &= ~CURTIS_CTRL_HORN;
                    break;

                case 't': case 'T':
                    turtleMode = !turtleMode;
                    if (turtleMode) {
                        cmd.controlBits0 |= CURTIS_CTRL_TURTLE_MODE;
                        cout << "[控制] 龟速模式 ON" << endl;
                    } else {
                        cmd.controlBits0 &= ~CURTIS_CTRL_TURTLE_MODE;
                        cout << "[控制] 龟速模式 OFF" << endl;
                    }
                    break;

                case ' ':  // 空格 - 全部停止
                    cmd.driveSpeed = 0;
                    cmd.steerAngle = 0;
                    cmd.liftSpeedLeft = 0;
                    cmd.liftSpeedRight = 0;
                    cmd.controlBits0 &= ~(CURTIS_CTRL_HORN | CURTIS_CTRL_TURTLE_MODE);
                    cout << "[控制] 全部停止" << endl;
                    break;

                case 'p': case 'P':
                    curtis.PrintStatus();
                    break;

                case 27:  // ESC - 退出
                    cout << "\n[控制] 用户请求退出" << endl;
                    running = false;
                    break;

                default:
                    break;
            }
        }

        // ---- 定期检查故障和状态（每500ms） ----
        if (currentTime - lastStatusCheckTime >= 500) {
            const CurtisStatus& status = curtis.GetStatus();

            // 检查故障
            if (status.driveError != 0 || status.steerError != 0) {
                cout << "[警告] " << curtis.GetErrorString() << endl;
                errorCount++;
            }

            // 检查急停
            if (status.isEmergencyStop) {
                cout << "[警告] 急停状态！停止所有运动" << endl;
                cmd.driveSpeed = 0;
                cmd.steerAngle = 0;
                cmd.liftSpeedLeft = 0;
                cmd.liftSpeedRight = 0;
            }

            // 检查通讯超时
            if (curtis.IsTimeout(200)) {
                cout << "[警告] 通讯超时！" << endl;
            }

            lastStatusCheckTime = currentTime;
        }

        // ---- 定时发送命令（15ms周期，≤20ms） ----
        if (currentTime - lastSendTime >= 15) {
            if (!curtis.SendCommand(cmd)) {
                cerr << "[错误] 发送命令失败！" << endl;
                running = false;
            }
            lastSendTime = currentTime;
        }

        // ---- 定时打印状态（5秒） ----
        if (currentTime - lastPrintTime >= 5000) {
            curtis.PrintStatus();
            cout << "[信息] 循环次数: " << loopCount
                 << ", 错误次数: " << errorCount << endl;
            lastPrintTime = currentTime;
        }

        // 避免CPU占用过高
        Sleep(5);
        loopCount++;
    }

    cout << "\n[统计] 总循环次数: " << loopCount << endl;
    cout << "[统计] 错误次数: " << errorCount << endl;

    // ============ 4. 断开连接 ============
    cout << "\n步骤 4: 断开连接..." << endl;
    curtis.Disconnect();

    cout << "\n===== 程序结束 =====" << endl;
    return 0;
}