// ============================================================================
// QT GUI 自定义路径功能 - 代码片段
// 将这些代码添加到 qt_gui_demo.cpp 中对应位置
// ============================================================================

// ----------------------------------------------------------------------------
// 1. 头文件部分 (第1-16行附近)
// ----------------------------------------------------------------------------
#include "path_tracker.h"
#include <QApplication>
#include <QMainWindow>
#include <QWidget>
#include <QPushButton>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QComboBox>
#include <QDoubleSpinBox>
#include <QTableWidget>
#include <QGroupBox>
#include <QPainter>
#include <QTimer>
#include <QHeaderView>
#include <QFileDialog>      // 新增
#include <QMessageBox>      // 新增
#include <QInputDialog>     // 新增
#include <cmath>

// ----------------------------------------------------------------------------
// 2. 路径类型选择 (MainWindow构造函数中，约第275-280行)
// ----------------------------------------------------------------------------
path_combo_ = new QComboBox(this);
path_combo_->addItem("Circle Arc");
path_combo_->addItem("Straight Line");
path_combo_->addItem("S-Curve");
path_combo_->addItem("Load from CSV");      // 新增
path_combo_->addItem("Custom Spline");       // 新增
path_layout->addWidget(path_combo_);

// ----------------------------------------------------------------------------
// 3. MainWindow 类成员变量 (private部分，约第527-530行)
// ----------------------------------------------------------------------------
private:
    // ... 其他成员变量 ...
    QTimer* animation_timer_;
    int animation_step_;
    bool animation_running_ = false;
    
    // 新增: 自定义路径支持
    PathCurve custom_path_;
    bool custom_path_loaded_ = false;
};

// ----------------------------------------------------------------------------
// 4. generateControl() 方法 - 完整替换版本
// ----------------------------------------------------------------------------
void generateControl() {
    updateAGVModel();

    PathCurve path;
    QString path_type = path_combo_->currentText();

    // === 新增: CSV文件加载 ===
    if (path_type == "Load from CSV") {
        QString filename = QFileDialog::getOpenFileName(
            this, 
            "Open CSV Path File", 
            "", 
            "CSV Files (*.csv);;All Files (*)");
        
        if (filename.isEmpty()) {
            return;  // User cancelled
        }
        
        if (!path.loadFromCSV(filename.toStdString(), true)) {
            QMessageBox::warning(
                this, 
                "Load Error", 
                "Failed to load CSV file!\n\n"
                "Please check:\n"
                "- File format (x,y per line)\n"
                "- File exists and readable");
            return;
        }
        
        QMessageBox::information(
            this, 
            "Load Success", 
            QString("Loaded %1 points from CSV\nPath length: %2 meters")
                .arg(path.getPathPoints().size())
                .arg(path.getPathLength(), 0, 'f', 2));
    } 
    // === 新增: 样条插值 ===
    else if (path_type == "Custom Spline") {
        bool ok;
        int num_points = QInputDialog::getInt(
            this, 
            "Spline Key Points",
            "Enter number of key points (2-10):", 
            4, 2, 10, 1, &ok);
        
        if (!ok) return;

        std::vector<PathPoint> key_points;
        for (int i = 0; i < num_points; ++i) {
            double x = QInputDialog::getDouble(
                this, 
                "Key Point Input",
                QString("Point %1 - X coordinate:").arg(i + 1), 
                i * 3.0, -100.0, 100.0, 2, &ok);
            if (!ok) return;
            
            double y = QInputDialog::getDouble(
                this, 
                "Key Point Input",
                QString("Point %1 - Y coordinate:").arg(i + 1),
                (i % 2 == 0) ? 0.0 : 3.0, -100.0, 100.0, 2, &ok);
            if (!ok) return;
            
            key_points.push_back(PathPoint(x, y));
        }

        int total_points = QInputDialog::getInt(
            this,
            "Spline Parameters",
            "Total points to generate:", 
            200, 50, 1000, 50, &ok);
        if (!ok) total_points = 200;

        double tension = QInputDialog::getDouble(
            this,
            "Spline Parameters", 
            "Tension (0.0=smooth, 1.0=tight):",
            0.5, 0.0, 1.0, 1, &ok);
        if (!ok) tension = 0.5;

        path.generateSpline(key_points, total_points, tension);
        
        QMessageBox::information(
            this, 
            "Spline Generated",
            QString("Generated spline path:\n"
                    "Key points: %1\n"
                    "Total points: %2\n"
                    "Path length: %3 m")
                .arg(key_points.size())
                .arg(path.getPathPoints().size())
                .arg(path.getPathLength(), 0, 'f', 2));
    }
    // === 原有路径类型 ===
    else if (path_type == "Circle Arc") {
        path.generateCircleArc(5.0, 0.0, 5.0, M_PI, M_PI / 2, 100);
    } 
    else if (path_type == "Straight Line") {
        PathPoint start(0, 0, 0, 0);
        PathPoint end(10, 0, 0, 0);
        path.generateLine(start, end, 100);
    } 
    else if (path_type == "S-Curve") {
        PathPoint p0(0, 0, 0, 0);
        PathPoint p1(3, 2, 0, 0);
        PathPoint p2(7, 2, 0, 0);
        PathPoint p3(10, 0, 0, 0);
        path.generateCubicBezier(p0, p1, p2, p3, 100);
    }

    // === 新增: 路径验证 ===
    if (path.getPathPoints().empty()) {
        QMessageBox::warning(
            this, 
            "Invalid Path", 
            "Path has no points!");
        return;
    }

    // === 以下代码保持不变 ===
    tracker_->setReferencePath(path);
    AGVModel::State initial_state(0.0, 0.0, 0.0);
    tracker_->setInitialState(initial_state);

    QString algo = algorithm_combo_->currentText();
    std::string algo_str = (algo == "Pure Pursuit") ? "pure_pursuit" : "stanley";

    double dt = dt_spin_->value();
    double horizon = horizon_spin_->value();

    tracker_->generateControlSequence(algo_str, dt, horizon);
    const ControlSequence& sequence = tracker_->getControlSequence();

    visualization_->setPath(path);
    visualization_->setControlSequence(sequence);
    visualization_->setCurrentStep(0);
    visualization_->setShowAnimation(true);

    updateTable(sequence);
    updateStatistics(sequence);

    start_btn_->setEnabled(true);
    start_btn_->setText("Start Animation");
    animation_running_ = false;
}

// ============================================================================
// 使用说明:
// 
// 1. 将上述代码片段复制到 qt_gui_demo.cpp 的对应位置
// 2. 重新编译: cd build && cmake .. && cmake --build .
// 3. 运行: ./agv_qt_gui
// 4. 在界面中选择 "Load from CSV" 或 "Custom Spline"
// 5. 点击 "Generate Control" 按钮
// 6. 按照提示操作
// ============================================================================