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

int main() {
    std::cout << "Generating AGV demo data files..." << std::endl;

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

    // Create path tracker
    PathTracker tracker(agv_model);

    // Create circular arc path from (0,0) to (5,5)
    PathCurve path;
    path.generateCircleArc(5.0, 0.0, 5.0, M_PI, 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 using Pure Pursuit
    std::cout << "Generating control sequence with Pure Pursuit..." << std::endl;
    tracker.generateControlSequence("pure_pursuit", 0.1, 10.0);

    // Save files
    tracker.saveControlSequence("control_sequence.csv");
    tracker.saveTrajectory("trajectory.csv");

    std::cout << "\nData files generated successfully!" << std::endl;
    std::cout << "  - control_sequence.csv" << std::endl;
    std::cout << "  - trajectory.csv" << std::endl;
    std::cout << "\nYou can now run: python visualize.py" << std::endl;

    return 0;
}