#ifndef PATH_CURVE_H
#define PATH_CURVE_H

#include <vector>
#include <string>
#include <string>
#include <string>
#include <string>
#define _USE_MATH_DEFINES
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

/**
 * @brief 路径点结构体
 */
struct PathPoint {
    double x;       // x坐标
    double y;       // y坐标
    double theta;   // 切线方向角
    double kappa;   // 曲率

    PathPoint(double x_ = 0.0, double y_ = 0.0,
              double theta_ = 0.0, double kappa_ = 0.0)
        : x(x_), y(y_), theta(theta_), kappa(kappa_) {}
};

/**
 * @brief 路径曲线类
 * 支持多种曲线类型：直线、圆弧、贝塞尔曲线等
 */
class PathCurve {
public:
    enum CurveType {
        LINE,           // 直线
        CIRCLE_ARC,     // 圆弧
        CUBIC_BEZIER,   // 三次贝塞尔曲线
        SPLINE          // 样条曲线
    };

    PathCurve() = default;

    /**
     * @brief 生成直线路径
     * @param start 起点
     * @param end 终点
     * @param num_points 路径点数量
     */
    void generateLine(const PathPoint& start, const PathPoint& end, int num_points = 100);

    /**
     * @brief 生成圆弧路径
     * @param center_x 圆心x坐标
     * @param center_y 圆心y坐标
     * @param radius 半径
     * @param start_angle 起始角度 (rad)
     * @param end_angle 终止角度 (rad)
     * @param num_points 路径点数量
     */
    void generateCircleArc(double center_x, double center_y, double radius,
                          double start_angle, double end_angle, int num_points = 100);

    /**
     * @brief 生成三次贝塞尔曲线
     * @param p0 起点
     * @param p1 第一个控制点
     * @param p2 第二个控制点
     * @param p3 终点
     * @param num_points 路径点数量
     */
    void generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
                            const PathPoint& p2, const PathPoint& p3,
                            int num_points = 100);

    /**
     * @brief 从路径点数组生成路径
     * @param points 路径点数组
     */
    void setPathPoints(const std::vector<PathPoint>& points);

    /**
     * @brief 从CSV文件加载路径点
     * @param filename CSV文件路径
     * @param has_header 是否包含表头（默认true）
     * @return 是否加载成功
     */
    bool loadFromCSV(const std::string& filename, bool has_header = true);

    /**
     * @brief 将路径点保存到CSV文件
     * @param filename CSV文件路径
     * @return 是否保存成功
     */
    bool saveToCSV(const std::string& filename) const;

    /**
     * @brief 使用样条插值生成路径
     * @param key_points 关键路径点
     * @param num_points 生成的路径点总数
     * @param tension 张力参数
     */
    void generateSpline(const std::vector<PathPoint>& key_points,
                       int num_points = 100,
                       double tension = 0.5);

    /**
     * @brief 从CSV文件加载路径点
     * @param filename CSV文件路径
     * @param has_header 是否包含表头（默认true）
     * @return 是否加载成功
     */
    bool loadFromCSV(const std::string& filename, bool has_header = true);

    /**
     * @brief 将路径点保存到CSV文件
     * @param filename CSV文件路径
     * @return 是否保存成功
     */
    bool saveToCSV(const std::string& filename) const;

    /**
     * @brief 使用样条插值生成路径
     * @param key_points 关键路径点
     * @param num_points 生成的路径点总数
     * @param tension 张力参数
     */
    void generateSpline(const std::vector<PathPoint>& key_points,
                       int num_points = 100,
                       double tension = 0.5);

    /**
     * @brief 从CSV文件加载路径点
     * @param filename CSV文件路径
     * @param has_header 是否包含表头（默认true）
     * @return 是否加载成功
     */
    bool loadFromCSV(const std::string& filename, bool has_header = true);

    /**
     * @brief 将路径点保存到CSV文件
     * @param filename CSV文件路径
     * @return 是否保存成功
     */
    bool saveToCSV(const std::string& filename) const;

    /**
     * @brief 使用样条插值生成路径
     * @param key_points 关键路径点
     * @param num_points 生成的路径点总数
     * @param tension 张力参数
     */
    void generateSpline(const std::vector<PathPoint>& key_points,
                       int num_points = 100,
                       double tension = 0.5);

    /**
     * @brief 从CSV文件加载路径点
     * @param filename CSV文件路径
     * @param has_header 是否包含表头（默认true）
     * @return 是否加载成功
     */
    bool loadFromCSV(const std::string& filename, bool has_header = true);

    /**
     * @brief 将路径点保存到CSV文件
     * @param filename CSV文件路径
     * @return 是否保存成功
     */
    bool saveToCSV(const std::string& filename) const;

    /**
     * @brief 使用样条插值生成路径
     * @param key_points 关键路径点
     * @param num_points 生成的路径点总数
     * @param tension 张力参数
     */
    void generateSpline(const std::vector<PathPoint>& key_points,
                       int num_points = 100,
                       double tension = 0.5);

    /**
     * @brief 获取路径点
     */
    const std::vector<PathPoint>& getPathPoints() const { return path_points_; }

    /**
     * @brief 获取指定参数t处的路径点（线性插值）
     * @param t 参数 [0, 1]
     * @return 插值后的路径点
     */
    PathPoint getPointAt(double t) const;

    /**
     * @brief 获取路径长度
     */
    double getPathLength() const;

    /**
     * @brief 找到最近的路径点索引
     * @param x 查询点x坐标
     * @param y 查询点y坐标
     * @return 最近点的索引
     */
    int findNearestPoint(double x, double y) const;

private:
    std::vector<PathPoint> path_points_;

    // 计算两点间的曲率
    double computeCurvature(const PathPoint& p1, const PathPoint& p2,
                           const PathPoint& p3) const;
};

#endif // PATH_CURVE_H