#include "path_tracker.h"
#include <iostream>
#include <iomanip>
#include <sstream>
#include <cmath>

/**
 * @brief Simple console GUI display class
 * Display control values in table format in terminal
 */
class ConsoleGUI {
public:
    ConsoleGUI() = default;

    /**
     * @brief Clear screen
     */
    void clear() {
#ifdef _WIN32
        system("cls");
#else
        system("clear");
#endif
    }

    /**
     * @brief Show title
     */
    void showTitle() {
        std::cout << "\n";
        std::cout << "========================================================================\n";
        std::cout << "|   Single Steering Wheel AGV Path Tracking Control System - GUI     |\n";
        std::cout << "========================================================================\n";
        std::cout << "\n";
    }

    /**
     * @brief Show AGV parameters
     */
    void showAGVParams(const AGVModel& model) {
        std::cout << "+---------------------- AGV Parameters ----------------------+\n";
        std::cout << "| Wheelbase:             " << std::setw(8) << std::fixed << std::setprecision(2)
                  << model.getWheelbase() << " m                      |\n";
        std::cout << "| Max Velocity:          " << std::setw(8) << model.getMaxVelocity() << " m/s                    |\n";
        std::cout << "| Max Steering Angle:    " << std::setw(8) << (model.getMaxSteeringAngle() * 180.0 / M_PI)
                  << " degrees                |\n";
        std::cout << "+------------------------------------------------------------+\n\n";
    }

    /**
     * @brief Show control sequence table
     */
    void showControlTable(const ControlSequence& sequence, int max_rows = 20) {
        if (sequence.size() == 0) {
            std::cout << "Control sequence is empty!\n";
            return;
        }

        std::cout << "+---------------- Control Sequence ----------------+\n";
        std::cout << "| " << std::setw(8) << "Step"
                  << " | " << std::setw(8) << "Time(s)"
                  << " | " << std::setw(10) << "Velocity(m/s)"
                  << " | " << std::setw(12) << "Steering(deg)"
                  << " |\n";
        std::cout << "+----------+----------+------------+---------------+\n";

        int display_rows = std::min(max_rows, static_cast<int>(sequence.size()));
        int step = std::max(1, static_cast<int>(sequence.size()) / display_rows);

        for (size_t i = 0; i < sequence.size(); i += step) {
            if (i / step >= display_rows) break;

            double time = sequence.timestamps[i];
            double velocity = sequence.controls[i].v;
            double steering_deg = sequence.controls[i].delta * 180.0 / M_PI;

            std::cout << "| " << std::setw(8) << i
                      << " | " << std::setw(8) << std::fixed << std::setprecision(2) << time
                      << " | " << std::setw(12) << std::setprecision(4) << velocity
                      << " | " << std::setw(13) << std::setprecision(4) << steering_deg
                      << " |\n";
        }

        std::cout << "+----------+----------+------------+---------------+\n";
        std::cout << "Total steps: " << sequence.size() << "\n\n";
    }

    /**
     * @brief Show current state and control (dashboard style)
     */
    void showDashboard(const AGVModel::State& state, const AGVModel::Control& control, int step) {
        std::cout << "+------------ Current State (Step: " << std::setw(4) << step << ") ------------+\n";

        // Position
        std::cout << "|                                                            |\n";
        std::cout << "| Position:  X = " << std::setw(8) << std::fixed << std::setprecision(3)
                  << state.x << " m    Y = " << std::setw(8) << state.y << " m        |\n";

        // Heading
        double theta_deg = state.theta * 180.0 / M_PI;
        std::cout << "| Heading:  theta = " << std::setw(8) << std::setprecision(2)
                  << theta_deg << " degrees                         |\n";

        std::cout << "|                                                            |\n";
        std::cout << "+------------------------------------------------------------+\n";
        std::cout << "|                    Control Values                          |\n";
        std::cout << "+------------------------------------------------------------+\n";

        // Velocity bar
        std::cout << "| Velocity: " << std::setw(6) << std::setprecision(3) << control.v << " m/s  ";
        drawBar(control.v, 0, 2.0, 20);
        std::cout << " |\n";

        // Steering bar
        double delta_deg = control.delta * 180.0 / M_PI;
        std::cout << "| Steering: " << std::setw(6) << std::setprecision(2) << delta_deg << " deg  ";
        drawBar(delta_deg, -45, 45, 20);
        std::cout << " |\n";

        std::cout << "+------------------------------------------------------------+\n\n";
    }

    /**
     * @brief Show statistics
     */
    void showStatistics(const ControlSequence& sequence) {
        if (sequence.size() == 0) return;

        // Calculate statistics
        double avg_velocity = 0.0;
        double max_velocity = -1e9;
        double min_velocity = 1e9;
        double avg_steering = 0.0;
        double max_steering = -1e9;
        double min_steering = 1e9;

        for (const auto& ctrl : sequence.controls) {
            avg_velocity += ctrl.v;
            max_velocity = std::max(max_velocity, ctrl.v);
            min_velocity = std::min(min_velocity, ctrl.v);

            double delta_deg = ctrl.delta * 180.0 / M_PI;
            avg_steering += delta_deg;
            max_steering = std::max(max_steering, delta_deg);
            min_steering = std::min(min_steering, delta_deg);
        }

        avg_velocity /= sequence.size();
        avg_steering /= sequence.size();

        std::cout << "+---------------- Statistics ----------------+\n";
        std::cout << "| Velocity Statistics:                      |\n";
        std::cout << "|  Average: " << std::setw(8) << std::fixed << std::setprecision(4)
                  << avg_velocity << " m/s                       |\n";
        std::cout << "|  Maximum: " << std::setw(8) << max_velocity << " m/s                       |\n";
        std::cout << "|  Minimum: " << std::setw(8) << min_velocity << " m/s                       |\n";
        std::cout << "|                                            |\n";
        std::cout << "| Steering Angle Statistics:                 |\n";
        std::cout << "|  Average: " << std::setw(8) << std::setprecision(4)
                  << avg_steering << " degrees                   |\n";
        std::cout << "|  Maximum: " << std::setw(8) << max_steering << " degrees                   |\n";
        std::cout << "|  Minimum: " << std::setw(8) << min_steering << " degrees                   |\n";
        std::cout << "+--------------------------------------------+\n";
    }

private:
    /**
     * @brief Draw bar chart
     */
    void drawBar(double value, double min_val, double max_val, int width) {
        double normalized = (value - min_val) / (max_val - min_val);
        normalized = std::max(0.0, std::min(1.0, normalized));

        int filled = static_cast<int>(normalized * width);

        std::cout << "[";
        for (int i = 0; i < width; ++i) {
            if (i < filled) {
                std::cout << "=";
            } else {
                std::cout << " ";
            }
        }
        std::cout << "]";
    }
};


int main() {
    ConsoleGUI gui;

    // Create AGV model
    AGVModel agv_model(1.0, 2.0, M_PI / 4);

    // Create path tracker
    PathTracker tracker(agv_model);

    // Create circular arc path as example
    PathCurve path;
    path.generateCircleArc(5.0, 0.0, 5.0, 0.0, M_PI / 2, 100);
    tracker.setReferencePath(path);

    // Set initial state
    AGVModel::State initial_state(0.0, 0.0, 0.0);
    tracker.setInitialState(initial_state);

    // Generate control sequence
    tracker.generateControlSequence("pure_pursuit", 0.1, 10.0);
    const ControlSequence& sequence = tracker.getControlSequence();

    // Show interface
    gui.clear();
    gui.showTitle();
    gui.showAGVParams(agv_model);
    gui.showControlTable(sequence, 15);
    gui.showStatistics(sequence);

    // Real-time simulation display (optional)
    std::cout << "\nPlay real-time simulation? (y/n): ";
    char choice;
    std::cin >> choice;

    if (choice == 'y' || choice == 'Y') {
        for (size_t i = 0; i < sequence.size(); i += 5) {  // Display every 5 steps
            gui.clear();
            gui.showTitle();
            gui.showDashboard(sequence.predicted_states[i], sequence.controls[i], i);

            // Pause for viewing
#ifdef _WIN32
            system("timeout /t 1 /nobreak > nul");
#else
            system("sleep 0.5");
#endif
        }
    }

    std::cout << "\nProgram ended. Press Enter to exit...";
    std::cin.ignore();
    std::cin.get();

    return 0;
}