/**
 * @file curtis_path_tracking_demo.cpp
 * @brief Curtis 电机控制器路径跟踪演示程序
 *
 * 功能说明：
 * - 演示如何使用 Curtis 控制器进行路径跟踪
 * - 结合 path_tracker 和 CurtisMotorController
 * - 实时故障检测和紧急停止
 * - 提供矩形路径和圆形路径示例
 *
 * 编译：
 *   cd build
 *   cmake ..
 *   make curtis_path_tracking_demo
 *
 * 运行：
 *   ./curtis_path_tracking_demo
 */

#include "../include/can/CurtisMotorController.h"
#include "../include/path_tracker.h"
#include "../include/path_curve.h"
#include "../include/agv_model.h"
#include <iostream>
#include <vector>
#include <cmath>
#include <windows.h>

using namespace std;

/**
 * @brief 路径点结构
 */
struct PathPoint {
    double x;
    double y;
    double targetSpeed;  ///< 目标速度百分比

    PathPoint(double x_, double y_, double speed_ = 30.0)
        : x(x_), y(y_), targetSpeed(speed_) {}
};

/**
 * @brief 生成矩形路径
 */
vector<PathPoint> generateRectanglePath() {
    vector<PathPoint> path;

    // 5m x 3m 矩形
    path.push_back(PathPoint(0.0, 0.0, 30.0));   // 起点
    path.push_back(PathPoint(5.0, 0.0, 30.0));   // 直行5米
    path.push_back(PathPoint(5.0, 3.0, 25.0));   // 右转前进3米
    path.push_back(PathPoint(0.0, 3.0, 25.0));   // 左转前进5米
    path.push_back(PathPoint(0.0, 0.0, 20.0));   // 返回起点

    return path;
}

/**
 * @brief 生成圆形路径
 */
vector<PathPoint> generateCirclePath(double radius = 3.0, int points = 36) {
    vector<PathPoint> path;

    for (int i = 0; i <= points; i++) {
        double angle = 2.0 * M_PI * i / points;
        double x = radius * cos(angle);
        double y = radius * sin(angle);
        path.push_back(PathPoint(x, y, 25.0));
    }

    return path;
}

/**
 * @brief 执行路径跟踪
 */
bool executePathTracking(CurtisMotorController& curtis,
                         const vector<PathPoint>& path,
                         AGVModel& agv,
                         PathTracker& tracker) {
    if (path.empty()) {
        cerr << "[路径跟踪] 错误: 路径为空" << endl;
        return false;
    }

    cout << "[路径跟踪] 开始执行，共 " << path.size() << " 个路径点" << endl;

    CurtisCommand cmd;
    cmd.controlBits0 = 0x00;
    cmd.controlBits1 = CURTIS_CTRL_AGV_CONNECTED;

    bool success = true;

    for (size_t i = 0; i < path.size(); i++) {
        cout << "\n[路径跟踪] 路径点 " << (i + 1) << "/" << path.size()
             << ": (" << path[i].x << ", " << path[i].y
             << "), 速度=" << path[i].targetSpeed << "%" << endl;

        // 设置目标点
        double targetX = path[i].x;
        double targetY = path[i].y;

        // 持续跟踪该路径点，直到到达
        DWORD startTime = GetTickCount();
        const DWORD maxTime = 10000;  // 最大10秒

        while (GetTickCount() - startTime < maxTime) {
            // 接收 Curtis 状态
            curtis.ReceiveStatus();
            const CurtisStatus& status = curtis.GetStatus();

            // 检查故障
            if (status.driveError || status.steerError) {
                cerr << "[路径跟踪] 检测到故障: " << curtis.GetErrorString() << endl;
                curtis.EmergencyStop();
                return false;
            }

            // 检查急停
            if (status.isEmergencyStop) {
                cerr << "[路径跟踪] 检测到急停，中止路径跟踪" << endl;
                return false;
            }

            // 获取 AGV 当前状态（这里使用模拟数据，实际应用中应从传感器获取）
            State currentState;
            currentState.x = agv.getState().x;
            currentState.y = agv.getState().y;
            currentState.theta = agv.getState().theta;
            currentState.v = status.motorSpeed / 100.0;  // 模拟速度
            currentState.omega = 0.0;

            // 计算到目标点的距离
            double dx = targetX - currentState.x;
            double dy = targetY - currentState.y;
            double distance = sqrt(dx * dx + dy * dy);

            // 如果接近目标点（< 0.1米），进入下一个点
            if (distance < 0.1) {
                cout << "[路径跟踪] 到达路径点 " << (i + 1) << endl;
                break;
            }

            // 计算目标方向角
            double targetAngle = atan2(dy, dx);

            // 计算转向角（简化版，实际应使用路径跟踪器）
            double angleError = targetAngle - currentState.theta;
            // 归一化到 [-π, π]
            while (angleError > M_PI) angleError -= 2 * M_PI;
            while (angleError < -M_PI) angleError += 2 * M_PI;

            // 转向控制（限制在 ±45度）
            double steerAngleDeg = angleError * 180.0 / M_PI;
            if (steerAngleDeg > 45.0) steerAngleDeg = 45.0;
            if (steerAngleDeg < -45.0) steerAngleDeg = -45.0;

            // 速度控制（根据角度误差调整速度）
            double speedScale = 1.0 - abs(angleError) / M_PI;
            if (speedScale < 0.3) speedScale = 0.3;  // 最小30%速度

            double actualSpeed = path[i].targetSpeed * speedScale;

            // 设置命令
            cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(actualSpeed);
            cmd.steerAngle = CurtisMotorController::AngleDegreeToCAN(steerAngleDeg);

            // 发送命令
            if (!curtis.SendCommand(cmd)) {
                cerr << "[路径跟踪] 发送命令失败" << endl;
                return false;
            }

            // 更新 AGV 模型（模拟）
            ControlInput control;
            control.v = actualSpeed / 100.0;
            control.delta = steerAngleDeg * M_PI / 180.0;
            agv.update(control, 0.015);  // 15ms

            // 打印进度
            if ((GetTickCount() - startTime) % 500 == 0) {
                cout << "  位置: (" << currentState.x << ", " << currentState.y
                     << "), 距离: " << distance << "m" << endl;
            }

            Sleep(15);  // 15ms周期
        }

        // 超时检查
        if (GetTickCount() - startTime >= maxTime) {
            cerr << "[路径跟踪] 超时: 无法到达路径点 " << (i + 1) << endl;
            success = false;
            break;
        }
    }

    // 路径结束，停止车辆
    cout << "\n[路径跟踪] 路径执行完成，停止车辆" << endl;
    cmd.driveSpeed = 0;
    cmd.steerAngle = 0;
    for (int i = 0; i < 10; i++) {
        curtis.SendCommand(cmd);
        Sleep(15);
    }

    return success;
}

/**
 * @brief 主函数
 */
int main() {
    cout << "===== Curtis 路径跟踪演示 =====\n" << endl;

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

    // 创建 AGV 模型（用于模拟）
    AGVModel agv;
    State initialState;
    initialState.x = 0.0;
    initialState.y = 0.0;
    initialState.theta = 0.0;
    initialState.v = 0.0;
    initialState.omega = 0.0;
    agv.setState(initialState);

    // 创建路径跟踪器
    PathTracker tracker;

    // ============ 2. 连接 ============
    cout << "\n步骤 2: 连接到 Curtis 控制器..." << endl;
    if (!curtis.Connect()) {
        cerr << "连接失败！" << endl;
        return -1;
    }

    Sleep(100);

    // ============ 3. 选择路径 ============
    cout << "\n步骤 3: 选择路径类型" << endl;
    cout << "1 - 矩形路径 (5m x 3m)" << endl;
    cout << "2 - 圆形路径 (半径 3m)" << endl;
    cout << "请选择 (1/2): ";

    int choice;
    cin >> choice;

    vector<PathPoint> path;
    if (choice == 1) {
        path = generateRectanglePath();
        cout << "已选择: 矩形路径" << endl;
    } else if (choice == 2) {
        path = generateCirclePath();
        cout << "已选择: 圆形路径" << endl;
    } else {
        cerr << "无效选择，使用默认矩形路径" << endl;
        path = generateRectanglePath();
    }

    // ============ 4. 执行路径跟踪 ============
    cout << "\n步骤 4: 执行路径跟踪" << endl;
    cout << "按 Enter 开始..." << endl;
    cin.ignore();
    cin.get();

    bool success = executePathTracking(curtis, path, agv, tracker);

    if (success) {
        cout << "\n✓ 路径跟踪成功完成" << endl;
    } else {
        cout << "\n✗ 路径跟踪失败" << endl;
    }

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

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