#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 <cmath>

#include <QFileDialog>
#include <QMessageBox>
#include <QInputDialog>

/**
 * @brief AGV Path Visualization Widget
 * Displays the reference path and AGV trajectory
 */
class PathVisualizationWidget : public QWidget {
    Q_OBJECT

public:
    explicit PathVisualizationWidget(QWidget* parent = nullptr)
        : QWidget(parent), current_step_(0), show_animation_(false) {
        setMinimumSize(600, 600);
        setStyleSheet("background-color: white;");
    }

    void setPath(const PathCurve& path) {
        path_ = path;
        update();
    }

    void setControlSequence(const ControlSequence& sequence) {
        sequence_ = sequence;
        current_step_ = 0;
        update();
    }

    void setCurrentStep(int step) {
        current_step_ = step;
        update();
    }

    void setShowAnimation(bool show) {
        show_animation_ = show;
        update();
    }

protected:
    void paintEvent(QPaintEvent* event) override {
        QPainter painter(this);
        painter.setRenderHint(QPainter::Antialiasing);

        // Calculate coordinate transformation
        const auto& path_points = path_.getPathPoints();
        if (path_points.empty()) return;

        // Find bounds
        double min_x = 1e9, max_x = -1e9, min_y = 1e9, max_y = -1e9;
        for (const auto& pt : path_points) {
            min_x = std::min(min_x, pt.x);
            max_x = std::max(max_x, pt.x);
            min_y = std::min(min_y, pt.y);
            max_y = std::max(max_y, pt.y);
        }

        // Add trajectory points
        if (!sequence_.predicted_states.empty()) {
            for (const auto& state : sequence_.predicted_states) {
                min_x = std::min(min_x, state.x);
                max_x = std::max(max_x, state.x);
                min_y = std::min(min_y, state.y);
                max_y = std::max(max_y, state.y);
            }
        }

        // Add margin
        double margin = 0.5;
        min_x -= margin; max_x += margin;
        min_y -= margin; max_y += margin;

        double range_x = max_x - min_x;
        double range_y = max_y - min_y;
        double range = std::max(range_x, range_y);

        // Center the view
        double center_x = (min_x + max_x) / 2.0;
        double center_y = (min_y + max_y) / 2.0;

        // Scale to fit widget with padding
        int padding = 40;
        // Prevent division by zero if all points are at the same location
        if (range < 1e-6) {
            range = 1.0;
        }
        double scale = std::min(width() - 2 * padding, height() - 2 * padding) / range;

        // Coordinate transformation: world to screen
        auto toScreen = [&](double x, double y) -> QPointF {
            double sx = (x - center_x) * scale + width() / 2.0;
            double sy = height() / 2.0 - (y - center_y) * scale;  // Flip Y axis
            return QPointF(sx, sy);
        };

        // Draw grid
        painter.setPen(QPen(QColor(220, 220, 220), 1));
        int grid_lines = 10;
        for (int i = 0; i <= grid_lines; ++i) {
            double t = static_cast<double>(i) / grid_lines;
            double x = min_x + t * range;
            double y = min_y + t * range;

            QPointF p1 = toScreen(x, min_y);
            QPointF p2 = toScreen(x, min_y + range);
            painter.drawLine(p1, p2);

            p1 = toScreen(min_x, y);
            p2 = toScreen(min_x + range, y);
            painter.drawLine(p1, p2);
        }

        // Draw axes
        painter.setPen(QPen(Qt::black, 2));
        QPointF origin = toScreen(0, 0);
        QPointF x_axis = toScreen(1, 0);
        QPointF y_axis = toScreen(0, 1);

        painter.drawLine(origin, x_axis);
        painter.drawLine(origin, y_axis);
        painter.drawText(x_axis + QPointF(5, 5), "X");
        painter.drawText(y_axis + QPointF(5, 5), "Y");

        // Draw reference path
        painter.setPen(QPen(QColor(100, 100, 255), 3, Qt::DashLine));
        for (size_t i = 1; i < path_points.size(); ++i) {
            QPointF p1 = toScreen(path_points[i-1].x, path_points[i-1].y);
            QPointF p2 = toScreen(path_points[i].x, path_points[i].y);
            painter.drawLine(p1, p2);
        }

        // Draw path points
        painter.setPen(Qt::NoPen);
        painter.setBrush(QColor(100, 100, 255, 100));
        for (const auto& pt : path_points) {
            QPointF p = toScreen(pt.x, pt.y);
            painter.drawEllipse(p, 3, 3);
        }

        // Draw predicted trajectory
        if (!sequence_.predicted_states.empty()) {
            painter.setPen(QPen(QColor(255, 100, 100), 2));
            for (size_t i = 1; i < sequence_.predicted_states.size(); ++i) {
                QPointF p1 = toScreen(sequence_.predicted_states[i-1].x,
                                     sequence_.predicted_states[i-1].y);
                QPointF p2 = toScreen(sequence_.predicted_states[i].x,
                                     sequence_.predicted_states[i].y);
                painter.drawLine(p1, p2);
            }

            // Draw current AGV position
            if (show_animation_ && current_step_ < sequence_.predicted_states.size()) {
                const auto& state = sequence_.predicted_states[current_step_];
                QPointF pos = toScreen(state.x, state.y);

                // Draw AGV body (rectangle)
                painter.save();
                painter.translate(pos);
                painter.rotate(-state.theta * 180.0 / M_PI);  // Rotate to heading

                double agv_length = 0.3 * scale;
                double agv_width = 0.2 * scale;

                painter.setBrush(QColor(50, 200, 50));
                painter.setPen(QPen(Qt::black, 2));
                painter.drawRect(QRectF(-agv_length/2, -agv_width/2,
                                       agv_length, agv_width));

                // Draw heading indicator (arrow)
                painter.setBrush(QColor(255, 50, 50));
                painter.drawEllipse(QPointF(agv_length/2, 0),
                                   agv_width/4, agv_width/4);

                painter.restore();

                // Draw position label
                painter.setPen(Qt::black);
                painter.drawText(pos + QPointF(10, -10),
                               QString("Step: %1").arg(current_step_));
            }
        }

        // Draw legend
        int legend_x = 10;
        int legend_y = 10;
        painter.fillRect(legend_x, legend_y, 150, 80, QColor(255, 255, 255, 200));
        painter.setPen(Qt::black);
        painter.drawRect(legend_x, legend_y, 150, 80);

        // Reference path
        painter.setPen(QPen(QColor(100, 100, 255), 3, Qt::DashLine));
        painter.drawLine(legend_x + 10, legend_y + 20, legend_x + 40, legend_y + 20);
        painter.setPen(Qt::black);
        painter.drawText(legend_x + 50, legend_y + 25, "Reference Path");

        // Trajectory
        painter.setPen(QPen(QColor(255, 100, 100), 3));
        painter.drawLine(legend_x + 10, legend_y + 40, legend_x + 40, legend_y + 40);
        painter.setPen(Qt::black);
        painter.drawText(legend_x + 50, legend_y + 45, "Trajectory");

        // AGV
        painter.fillRect(legend_x + 10, legend_y + 55, 30, 15, QColor(50, 200, 50));
        painter.drawRect(legend_x + 10, legend_y + 55, 30, 15);
        painter.drawText(legend_x + 50, legend_y + 65, "AGV");
    }

private:
    PathCurve path_;
    ControlSequence sequence_;
    int current_step_;
    bool show_animation_;
};

/**
 * @brief Main Window for AGV Path Tracking GUI
 */
class MainWindow : public QMainWindow {
    Q_OBJECT

public:
    MainWindow(QWidget* parent = nullptr) : QMainWindow(parent) {
        setWindowTitle("AGV Path Tracking Control System - Qt GUI");
        resize(1200, 800);

        // Create central widget
        QWidget* central = new QWidget(this);
        setCentralWidget(central);

        QHBoxLayout* main_layout = new QHBoxLayout(central);

        // Left panel: Visualization
        visualization_ = new PathVisualizationWidget(this);
        main_layout->addWidget(visualization_, 2);

        // Right panel: Controls and data
        QWidget* right_panel = new QWidget(this);
        QVBoxLayout* right_layout = new QVBoxLayout(right_panel);

        // AGV Parameters Group
        QGroupBox* param_group = new QGroupBox("AGV Parameters", this);
        QVBoxLayout* param_layout = new QVBoxLayout(param_group);

        wheelbase_spin_ = createParamRow("Wheelbase (m):", 0.5, 3.0, 1.0, param_layout);
        max_vel_spin_ = createParamRow("Max Velocity (m/s):", 0.5, 5.0, 2.0, param_layout);
        max_steer_spin_ = createParamRow("Max Steering (deg):", 10, 60, 45, param_layout);

        right_layout->addWidget(param_group);

        // Control Parameters Group
        QGroupBox* control_group = new QGroupBox("Control Parameters", this);
        QVBoxLayout* control_layout = new QVBoxLayout(control_group);

        // Algorithm selection
        QHBoxLayout* algo_layout = new QHBoxLayout();
        algo_layout->addWidget(new QLabel("Algorithm:", this));
        algorithm_combo_ = new QComboBox(this);
        algorithm_combo_->addItem("Pure Pursuit");
        algorithm_combo_->addItem("Stanley");
        algo_layout->addWidget(algorithm_combo_);
        control_layout->addLayout(algo_layout);
        // Path type selection
        QHBoxLayout* path_layout = new QHBoxLayout();
        path_layout->addWidget(new QLabel("Path Type:", this));
        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_);
        control_layout->addLayout(path_layout);

        dt_spin_ = createParamRow("Time Step (s):", 0.01, 1.0, 0.1, control_layout);
        horizon_spin_ = createParamRow("Horizon (s):", 1.0, 100.0, 50.0, control_layout);

        right_layout->addWidget(control_group);

        control_layout->addLayout(path_layout);

        // 添加自定义路径按钮
        QHBoxLayout* custom_btn_layout = new QHBoxLayout();


        QPushButton* load_csv_btn = new QPushButton("Browse CSV...", this);
        connect(load_csv_btn, &QPushButton::clicked, [this]() {
            QString filename = QFileDialog::getOpenFileName(
                this, "Open CSV Path File", "", "CSV Files (*.csv)");
            if (!filename.isEmpty()) {
                // 修复: 使用toLocal8Bit以正确处理Windows路径（包括中文路径）
                if (custom_path_.loadFromCSV(filename.toLocal8Bit().constData(), true)) {
                    custom_path_loaded_ = true;
                    QMessageBox::information(this, "Success", 
                        QString("Loaded %1 points from CSV!").arg(
                            custom_path_.getPathPoints().size()));
                } else {
                    QMessageBox::warning(this, "Error", "Failed to load CSV file!");
                }
            }
        });
        custom_btn_layout->addWidget(load_csv_btn);

        QPushButton* save_csv_btn = new QPushButton("Save Path...", this);
        connect(save_csv_btn, &QPushButton::clicked, [this]() {
            QString filename = QFileDialog::getSaveFileName(
                this, "Save Path as CSV", "my_path.csv", "CSV Files (*.csv)");
                // 修复: 使用toLocal8Bit以正确处理Windows路径（包括中文路径）
            if (!filename.isEmpty() && custom_path_loaded_) {
                if (custom_path_.saveToCSV(filename.toLocal8Bit().constData())) {
                    QMessageBox::information(this, "Success", "Path saved!");
                }
            }
        });
        custom_btn_layout->addWidget(save_csv_btn);

        control_layout->addLayout(custom_btn_layout);



        // Buttons
        QHBoxLayout* button_layout = new QHBoxLayout();

        QPushButton* generate_btn = new QPushButton("Generate Control", this);
        connect(generate_btn, &QPushButton::clicked, this, &MainWindow::generateControl);
        button_layout->addWidget(generate_btn);

        start_btn_ = new QPushButton("Start Animation", this);
        connect(start_btn_, &QPushButton::clicked, this, &MainWindow::toggleAnimation);
        start_btn_->setEnabled(false);
        button_layout->addWidget(start_btn_);

        right_layout->addLayout(button_layout);

        // Statistics Group
        stats_group_ = new QGroupBox("Statistics", this);
        QVBoxLayout* stats_layout = new QVBoxLayout(stats_group_);

        stats_label_ = new QLabel("No data", this);
        stats_label_->setWordWrap(true);
        stats_layout->addWidget(stats_label_);

        right_layout->addWidget(stats_group_);

        // Control Sequence Table
        table_ = new QTableWidget(this);
        table_->setColumnCount(4);
        table_->setHorizontalHeaderLabels({"Step", "Time(s)", "Velocity(m/s)", "Steering(deg)"});
        table_->horizontalHeader()->setStretchLastSection(true);
        table_->setAlternatingRowColors(true);
        right_layout->addWidget(table_);

        main_layout->addWidget(right_panel, 1);

        // Initialize AGV model
        updateAGVModel();

        // Animation timer
        animation_timer_ = new QTimer(this);
        connect(animation_timer_, &QTimer::timeout, this, &MainWindow::updateAnimation);
    }

private slots:
    void generateControl() {
        // Update AGV model
        updateAGVModel();

        // Create path based on selection
        PathCurve path;
        QString path_type = path_combo_->currentText();

        if (path_type == "Load from CSV") {
            if (!custom_path_loaded_) {
                QMessageBox::warning(this, "Warning", 
                    "Please load a CSV file first using 'Browse CSV...' button!");
                return;
            }
            path = custom_path_;
        } 
        else if (path_type == "Custom Spline") {
            if (!custom_path_loaded_) {
                // 如果没有预加载，让用户输入关键点
                bool ok;
                int num_points = QInputDialog::getInt(this, "Spline Input",
                    "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",
                        QString("Point %1 - X coordinate:").arg(i+1), 
                        i * 3.0, -100, 100, 2, &ok);
                    if (!ok) return;
                    
                    double y = QInputDialog::getDouble(this, "Key Point",
                        QString("Point %1 - Y coordinate:").arg(i+1),
                        (i % 2) * 3.0, -100, 100, 2, &ok);
                    if (!ok) return;
                    
                    key_points.push_back(PathPoint(x, y));
                }

                path.generateSpline(key_points, 200, 0.5);
                custom_path_ = path;
                custom_path_loaded_ = true;
            } else {
                path = custom_path_;
            }
        }
        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, "Error", "Invalid path!");
            return;
        }

        // Set up tracker
        tracker_->setReferencePath(path);

        // 修复: 从路径起点获取初始状态，确保完美匹配
        const auto& path_points = path.getPathPoints();
        AGVModel::State initial_state;
        if (!path_points.empty()) {
            const PathPoint& start = path_points[0];
            initial_state = AGVModel::State(start.x, start.y, start.theta);
        } else {
            initial_state = AGVModel::State(0.0, 0.0, 0.0);
        }
        tracker_->setInitialState(initial_state);

        // Generate control sequence
        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();
        // 修复: 使用GUI中的速度参数
        double desired_velocity = max_vel_spin_->value();

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

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

        // Update table
        updateTable(sequence);

        // Update statistics
        updateStatistics(sequence);

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

    void toggleAnimation() {
        const ControlSequence& sequence = tracker_->getControlSequence();
        if (sequence.size() == 0) return;

        if (animation_running_) {
            // Pause the animation
            animation_timer_->stop();
            start_btn_->setText("Resume Animation");
            animation_running_ = false;
        } else {
            // Resume or start animation
            // Only reset to beginning if animation has finished
            if (animation_step_ >= sequence.size()) {
                animation_step_ = 0;
            }
            animation_timer_->start(100);  // 100ms interval
            start_btn_->setText("Pause Animation");
            animation_running_ = true;
        }
    }

    void updateAnimation() {
        const ControlSequence& sequence = tracker_->getControlSequence();
        if (animation_step_ >= sequence.size()) {
            animation_timer_->stop();
            start_btn_->setText("Restart Animation");
            animation_running_ = false;
            return;
        }

        visualization_->setCurrentStep(animation_step_);

        // Highlight current row in table
        table_->selectRow(animation_step_);

        animation_step_++;
    }

private:
    void updateAGVModel() {
        double wheelbase = wheelbase_spin_->value();
        double max_vel = max_vel_spin_->value();
        double max_steer = max_steer_spin_->value() * M_PI / 180.0;

        model_ = std::make_unique<AGVModel>(wheelbase, max_vel, max_steer);
        tracker_ = std::make_unique<PathTracker>(*model_);
    }

    QDoubleSpinBox* createParamRow(const QString& label, double min, double max,
                                    double value, QVBoxLayout* layout) {
        QHBoxLayout* row = new QHBoxLayout();
        row->addWidget(new QLabel(label, this));

        QDoubleSpinBox* spin = new QDoubleSpinBox(this);
        spin->setRange(min, max);
        spin->setValue(value);
        spin->setSingleStep((max - min) / 100.0);
        spin->setDecimals(2);
        row->addWidget(spin);

        layout->addLayout(row);
        return spin;
    }

    void updateTable(const ControlSequence& sequence) {
        table_->setRowCount(sequence.size());

        for (size_t i = 0; i < sequence.size(); ++i) {
            table_->setItem(i, 0, new QTableWidgetItem(QString::number(i)));
            table_->setItem(i, 1, new QTableWidgetItem(
                QString::number(sequence.timestamps[i], 'f', 2)));
            table_->setItem(i, 2, new QTableWidgetItem(
                QString::number(sequence.controls[i].v, 'f', 4)));
            table_->setItem(i, 3, new QTableWidgetItem(
                QString::number(sequence.controls[i].delta * 180.0 / M_PI, 'f', 2)));
        }
    }

    void updateStatistics(const ControlSequence& sequence) {
        if (sequence.size() == 0) {
            stats_label_->setText("No data");
            return;
        }

        double avg_vel = 0, max_vel = -1e9, min_vel = 1e9;
        double avg_steer = 0, max_steer = -1e9, min_steer = 1e9;

        for (const auto& ctrl : sequence.controls) {
            avg_vel += ctrl.v;
            max_vel = std::max(max_vel, ctrl.v);
            min_vel = std::min(min_vel, ctrl.v);

            double delta_deg = ctrl.delta * 180.0 / M_PI;
            avg_steer += delta_deg;
            max_steer = std::max(max_steer, delta_deg);
            min_steer = std::min(min_steer, delta_deg);
        }

        avg_vel /= sequence.size();
        avg_steer /= sequence.size();

        QString stats = QString(
            "Total Steps: %1\n\n"
            "Velocity:\n"
            "  Avg: %2 m/s\n"
            "  Max: %3 m/s\n"
            "  Min: %4 m/s\n\n"
            "Steering:\n"
            "  Avg: %5°\n"
            "  Max: %6°\n"
            "  Min: %7°"
        ).arg(sequence.size())
         .arg(avg_vel, 0, 'f', 4)
         .arg(max_vel, 0, 'f', 4)
         .arg(min_vel, 0, 'f', 4)
         .arg(avg_steer, 0, 'f', 2)
         .arg(max_steer, 0, 'f', 2)
         .arg(min_steer, 0, 'f', 2);

        stats_label_->setText(stats);
    }

    // Widgets
    PathVisualizationWidget* visualization_;
    QTableWidget* table_;
    QComboBox* algorithm_combo_;
    QComboBox* path_combo_;
    QDoubleSpinBox* wheelbase_spin_;
    QDoubleSpinBox* max_vel_spin_;
    QDoubleSpinBox* max_steer_spin_;
    QDoubleSpinBox* dt_spin_;
    QDoubleSpinBox* horizon_spin_;
    QPushButton* start_btn_;
    QGroupBox* stats_group_;
    QLabel* stats_label_;

    // Model and tracker
    std::unique_ptr<AGVModel> model_;
    std::unique_ptr<PathTracker> tracker_;

    // Animation
    QTimer* animation_timer_;
    int animation_step_;
    bool animation_running_ = false;

    //自定义路径
    PathCurve custom_path_;
    bool custom_path_loaded_ = false;
};

int main(int argc, char* argv[]) {
    QApplication app(argc, argv);

    MainWindow window;
    window.show();

    return app.exec();
}

#include "qt_gui_demo.moc"