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

/**
 * @brief 平滑路径生成器类
 * 用于生成各种类型的平滑路径并保存为CSV文件
 */
class SmoothPathGenerator {
public:
    /**
     * @brief 生成圆弧平滑路径
     * @param filename 输出文件名
     * @param center_x 圆心X坐标
     * @param center_y 圆心Y坐标
     * @param radius 半径
     * @param start_angle 起始角度（弧度）
     * @param end_angle 终止角度（弧度）
     * @param num_points 路径点数量
     */
    static bool generateCircleArc(const std::string& filename,
                                  double center_x, double center_y, double radius,
                                  double start_angle, double end_angle,
                                  int num_points = 200) {
        PathCurve path;
        path.generateCircleArc(center_x, center_y, radius,
                              start_angle, end_angle, num_points);

        if (path.saveToCSV(filename)) {
            std::cout << "✓ Circle arc path saved: " << filename << std::endl;
            std::cout << "  Points: " << num_points << std::endl;
            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
            return true;
        }
        return false;
    }

    /**
     * @brief 生成S型曲线（三次贝塞尔曲线）
     * @param filename 输出文件名
     * @param start_x 起点X
     * @param start_y 起点Y
     * @param end_x 终点X
     * @param end_y 终点Y
     * @param control_offset 控制点偏移量
     * @param num_points 路径点数量
     */
    static bool generateSCurve(const std::string& filename,
                               double start_x, double start_y,
                               double end_x, double end_y,
                               double control_offset = 3.0,
                               int num_points = 200) {
        PathPoint p0(start_x, start_y);
        PathPoint p1(start_x + control_offset, start_y + control_offset);
        PathPoint p2(end_x - control_offset, end_y + control_offset);
        PathPoint p3(end_x, end_y);

        PathCurve path;
        path.generateCubicBezier(p0, p1, p2, p3, num_points);

        if (path.saveToCSV(filename)) {
            std::cout << "✓ S-curve path saved: " << filename << std::endl;
            std::cout << "  Points: " << num_points << std::endl;
            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
            return true;
        }
        return false;
    }

    /**
     * @brief 生成样条曲线路径
     * @param filename 输出文件名
     * @param key_points 关键点数组
     * @param num_points 生成的路径点总数
     * @param tension 张力参数（0-1，越大越紧）
     */
    static bool generateSpline(const std::string& filename,
                               const std::vector<PathPoint>& key_points,
                               int num_points = 200,
                               double tension = 0.5) {
        if (key_points.size() < 2) {
            std::cerr << "✗ Error: At least 2 key points required" << std::endl;
            return false;
        }

        PathCurve path;
        path.generateSpline(key_points, num_points, tension);

        if (path.saveToCSV(filename)) {
            std::cout << "✓ Spline path saved: " << filename << std::endl;
            std::cout << "  Key points: " << key_points.size() << std::endl;
            std::cout << "  Total points: " << num_points << std::endl;
            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
            return true;
        }
        return false;
    }

    /**
     * @brief 生成复杂的平滑路径（组合多个关键点）
     * @param filename 输出文件名
     * @param num_points 路径点数量
     */
    static bool generateComplexPath(const std::string& filename,
                                    int num_points = 300) {
        // 定义关键点：模拟AGV在仓库中的复杂路径
        std::vector<PathPoint> key_points = {
            PathPoint(0.0, 0.0),      // 起点
            PathPoint(5.0, 0.5),      // 第一段轻微转弯
            PathPoint(8.0, 3.0),      // 上升
            PathPoint(10.0, 6.0),     // 继续上升
            PathPoint(11.0, 9.0),     // 接近顶部
            PathPoint(10.5, 12.0),    // 转向
            PathPoint(8.0, 13.0),     // 左转
            PathPoint(5.0, 13.5),     // 继续左转
            PathPoint(2.0, 12.0),     // 下降
            PathPoint(0.0, 10.0)      // 终点
        };

        PathCurve path;
        path.generateSpline(key_points, num_points, 0.3);

        if (path.saveToCSV(filename)) {
            std::cout << "✓ Complex smooth path saved: " << filename << std::endl;
            std::cout << "  Key points: " << key_points.size() << std::endl;
            std::cout << "  Total points: " << num_points << std::endl;
            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
            return true;
        }
        return false;
    }

    /**
     * @brief 生成环形平滑路径
     * @param filename 输出文件名
     * @param radius 半径
     * @param num_points 路径点数量
     */
    static bool generateLoop(const std::string& filename,
                            double radius = 5.0,
                            int num_points = 300) {
        PathCurve path;
        path.generateCircleArc(0.0, 0.0, radius, 0.0, 2 * M_PI, num_points);

        if (path.saveToCSV(filename)) {
            std::cout << "✓ Loop path saved: " << filename << std::endl;
            std::cout << "  Radius: " << radius << " m" << std::endl;
            std::cout << "  Points: " << num_points << std::endl;
            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
            return true;
        }
        return false;
    }

    /**
     * @brief 生成Figure-8路径（8字形）
     * @param filename 输出文件名
     * @param size 8字大小
     * @param num_points 路径点数量
     */
    static bool generateFigure8(const std::string& filename,
                                double size = 5.0,
                                int num_points = 400) {
        std::vector<PathPoint> key_points = {
            PathPoint(0.0, 0.0),
            PathPoint(size, size),
            PathPoint(size * 2, 0.0),
            PathPoint(size, -size),
            PathPoint(0.0, 0.0)
        };

        PathCurve path;
        path.generateSpline(key_points, num_points, 0.4);

        if (path.saveToCSV(filename)) {
            std::cout << "✓ Figure-8 path saved: " << filename << std::endl;
            std::cout << "  Size: " << size << " m" << std::endl;
            std::cout << "  Points: " << num_points << std::endl;
            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
            return true;
        }
        return false;
    }

    /**
     * @brief 打印使用说明
     */
    static void printUsage() {
        std::cout << "\n========================================" << std::endl;
        std::cout << "   Smooth Path Generator" << std::endl;
        std::cout << "========================================\n" << std::endl;
        std::cout << "This tool generates various smooth paths for AGV navigation.\n" << std::endl;
        std::cout << "Available path types:" << std::endl;
        std::cout << "  1. Simple smooth path (default)" << std::endl;
        std::cout << "  2. Circle arc" << std::endl;
        std::cout << "  3. S-curve" << std::endl;
        std::cout << "  4. Complex path" << std::endl;
        std::cout << "  5. Loop path" << std::endl;
        std::cout << "  6. Figure-8 path" << std::endl;
        std::cout << "\nAll paths will be saved as CSV files.\n" << std::endl;
    }
};

int main(int argc, char* argv[]) {
    SmoothPathGenerator::printUsage();

    std::cout << "Generating smooth paths...\n" << std::endl;

    // 1. 生成默认的平滑路径 - smooth_path.csv
    std::cout << "[1] Generating default smooth path..." << std::endl;
    std::vector<PathPoint> default_key_points = {
        PathPoint(0.0, 0.0),
        PathPoint(3.0, 1.0),
        PathPoint(6.0, 3.0),
        PathPoint(9.0, 3.5),
        PathPoint(12.0, 3.0)
    };
    SmoothPathGenerator::generateSpline("smooth_path.csv", default_key_points, 200, 0.5);

    // 2. 生成圆弧路径
    std::cout << "\n[2] Generating circle arc path..." << std::endl;
    SmoothPathGenerator::generateCircleArc("smooth_path_arc.csv",
                                           5.0, 0.0, 5.0,
                                           M_PI, M_PI / 2, 150);

    // 3. 生成S型曲线
    std::cout << "\n[3] Generating S-curve path..." << std::endl;
    SmoothPathGenerator::generateSCurve("smooth_path_scurve.csv",
                                        0.0, 0.0, 10.0, 0.0, 2.5, 200);

    // 4. 生成复杂路径
    std::cout << "\n[4] Generating complex path..." << std::endl;
    SmoothPathGenerator::generateComplexPath("smooth_path_complex.csv", 300);

    // 5. 生成环形路径
    std::cout << "\n[5] Generating loop path..." << std::endl;
    SmoothPathGenerator::generateLoop("smooth_path_loop.csv", 5.0, 300);

    // 6. 生成8字形路径
    std::cout << "\n[6] Generating Figure-8 path..." << std::endl;
    SmoothPathGenerator::generateFigure8("smooth_path_figure8.csv", 4.0, 400);

    std::cout << "\n========================================" << std::endl;
    std::cout << "✓ All smooth paths generated successfully!" << std::endl;
    std::cout << "========================================\n" << std::endl;

    std::cout << "Generated files:" << std::endl;
    std::cout << "  • smooth_path.csv        - Default smooth path" << std::endl;
    std::cout << "  • smooth_path_arc.csv    - Circle arc" << std::endl;
    std::cout << "  • smooth_path_scurve.csv - S-curve" << std::endl;
    std::cout << "  • smooth_path_complex.csv - Complex path" << std::endl;
    std::cout << "  • smooth_path_loop.csv   - Loop path" << std::endl;
    std::cout << "  • smooth_path_figure8.csv - Figure-8 path" << std::endl;

    std::cout << "\nYou can load these CSV files in the Qt GUI application:" << std::endl;
    std::cout << "  1. Run: ./build/Debug/agv_qt_gui.exe" << std::endl;
    std::cout << "  2. Select 'Load from CSV' in Path Type" << std::endl;
    std::cout << "  3. Choose one of the generated CSV files" << std::endl;

    return 0;
}