#include "include/path_curve.h"
#include <iostream>

int main() {
    std::cout << "Testing CSV loading..." << std::endl;

    PathCurve path;
    std::cout << "Attempting to load smooth_path_arc.csv..." << std::endl;

    bool success = path.loadFromCSV("smooth_path_arc.csv", true);

    if (success) {
        std::cout << "CSV loaded successfully!" << std::endl;
        const auto& points = path.getPathPoints();
        std::cout << "Total points: " << points.size() << std::endl;

        if (!points.empty()) {
            std::cout << "First point: (" << points[0].x << ", " << points[0].y
                      << ", " << points[0].theta << ", " << points[0].kappa << ")" << std::endl;
            std::cout << "Last point: (" << points.back().x << ", " << points.back().y
                      << ", " << points.back().theta << ", " << points.back().kappa << ")" << std::endl;
        }
    } else {
        std::cout << "Failed to load CSV!" << std::endl;
        return 1;
    }

    return 0;
}
