#include "path_tracker.h"
#include <iostream>

#define _USE_MATH_DEFINES
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

int main() {
    std::cout << "========================================" << std::endl;
    std::cout << "    Single Steering Wheel AGV Path Tracking Control System Demo" << std::endl;
    std::cout << "========================================\n" << std::endl;

    // 1. Create AGV model
    double wheelbase = 1.0;           // Wheelbase 1.0m
    double max_velocity = 2.0;        // Max velocity 2.0 m/s
    double max_steering = M_PI / 4;   // Max steering angle 45 degrees
    AGVModel agv_model(wheelbase, max_velocity, max_steering);

    std::cout << "AGV Parameters:" << std::endl;
    std::cout << "  Wheelbase: " << wheelbase << " m" << std::endl;
    std::cout << "  Max Velocity: " << max_velocity << " m/s" << std::endl;
    std::cout << "  Max Steering Angle: " << (max_steering * 180.0 / M_PI) << " degrees" << std::endl;

    // 2. Create path tracker
    PathTracker tracker(agv_model);

    // 3. Define reference path
    PathCurve path;

    std::cout << "Please select path type:" << std::endl;
    std::cout << "1. Straight line path" << std::endl;
    std::cout << "2. Circular arc path" << std::endl;
    std::cout << "3. Bezier curve path" << std::endl;
    std::cout << "4. S-curve path (combined)" << std::endl;
    std::cout << "Enter choice (1-4): ";

    int choice;
    std::cin >> choice;

    switch (choice) {
        case 1: {
            // Straight line: from (0,0) to (10,10)
            PathPoint start(0.0, 0.0);
            PathPoint end(10.0, 10.0);
            path.generateLine(start, end, 100);
            std::cout << "\nGenerated straight line path: (0,0) -> (10,10)" << std::endl;
            break;
        }
        case 2: {
            // Circular arc: center (5,0), radius 5m, from 0 to 90 degrees
            path.generateCircleArc(5.0, 0.0, 5.0, 0.0, M_PI / 2, 100);
            std::cout << "\nGenerated circular arc path: center (5,0), radius 5m" << std::endl;
            break;
        }
        case 3: {
            // Bezier curve
            PathPoint p0(0.0, 0.0);
            PathPoint p1(3.0, 5.0);
            PathPoint p2(7.0, 5.0);
            PathPoint p3(10.0, 0.0);
            path.generateCubicBezier(p0, p1, p2, p3, 100);
            std::cout << "\nGenerated Bezier curve path" << std::endl;
            break;
        }
        case 4: {
            // S-curve (two connected arcs)
            std::vector<PathPoint> points;

            // First segment: arc to the right
            PathCurve arc1;
            arc1.generateCircleArc(2.5, 0.0, 2.5, M_PI, M_PI / 2, 50);
            auto arc1_points = arc1.getPathPoints();
            points.insert(points.end(), arc1_points.begin(), arc1_points.end());

            // Second segment: arc to the left
            PathCurve arc2;
            arc2.generateCircleArc(2.5, 5.0, 2.5, -M_PI / 2, 0, 50);
            auto arc2_points = arc2.getPathPoints();
            points.insert(points.end(), arc2_points.begin(), arc2_points.end());

            path.setPathPoints(points);
            std::cout << "\nGenerated S-curve path" << std::endl;
            break;
        }
        default:
            std::cout << "Invalid choice, using default straight line path" << std::endl;
            PathPoint start(0.0, 0.0);
            PathPoint end(10.0, 10.0);
            path.generateLine(start, end, 100);
            break;
    }

    tracker.setReferencePath(path);
    std::cout << "Path length: " << path.getPathLength() << " m" << std::endl;
    std::cout << "Path points: " << path.getPathPoints().size() << std::endl;

    // 4. Set initial state
    AGVModel::State initial_state(0.0, 0.0, 0.0);  // Start at (0,0), heading 0 degrees
    tracker.setInitialState(initial_state);
    std::cout << "\nInitial state: x=" << initial_state.x
              << ", y=" << initial_state.y
              << ", theta=" << (initial_state.theta * 180.0 / M_PI) << " degrees" << std::endl;

    // 5. Select control algorithm
    std::cout << "\nPlease select control algorithm:" << std::endl;
    std::cout << "1. Pure Pursuit" << std::endl;
    std::cout << "2. Stanley" << std::endl;
    std::cout << "Enter choice (1-2): ";

    int algo_choice;
    std::cin >> algo_choice;

    std::string algorithm = (algo_choice == 2) ? "stanley" : "pure_pursuit";
    std::cout << "\nUsing algorithm: " << algorithm << std::endl;

    // 6. Generate control sequence
    std::cout << "\nGenerating control sequence..." << std::endl;
    double dt = 0.1;        // Time step 0.1s
    double horizon = 20.0;  // Prediction horizon 20s

    if (!tracker.generateControlSequence(algorithm, dt, horizon)) {
        std::cerr << "Control sequence generation failed!" << std::endl;
        return 1;
    }

    std::cout << "Control sequence generation completed!" << std::endl;

    // 7. Display control sequence
    tracker.printControlSequence();

    // 8. Save to file
    std::cout << "\nSave control sequence to file? (y/n): ";
    char save_choice;
    std::cin >> save_choice;

    if (save_choice == 'y' || save_choice == 'Y') {
        tracker.saveControlSequence("control_sequence.csv");
        tracker.saveTrajectory("trajectory.csv");
        std::cout << "\nFiles saved!" << std::endl;
        std::cout << "  - control_sequence.csv (control sequence)" << std::endl;
        std::cout << "  - trajectory.csv (predicted trajectory)" << std::endl;
        std::cout << "\nNote: You can visualize CSV files using Python/MATLAB/Excel" << std::endl;
    }

    std::cout << "\n========================================" << std::endl;
    std::cout << "           Demo Program Ended" << std::endl;
    std::cout << "========================================" << std::endl;

    return 0;
}