#include "path_curve.h"
#include <limits>
#include <algorithm>

#define _USE_MATH_DEFINES
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

void PathCurve::generateLine(const PathPoint& start, const PathPoint& end, int num_points) {
    path_points_.clear();
    path_points_.reserve(num_points);

    for (int i = 0; i < num_points; ++i) {
        double t = static_cast<double>(i) / (num_points - 1);
        PathPoint p;
        p.x = start.x + t * (end.x - start.x);
        p.y = start.y + t * (end.y - start.y);
        p.theta = std::atan2(end.y - start.y, end.x - start.x);
        p.kappa = 0.0; // 直线曲率为0

        path_points_.push_back(p);
    }
}

void PathCurve::generateCircleArc(double center_x, double center_y, double radius,
                                 double start_angle, double end_angle, int num_points) {
    path_points_.clear();
    path_points_.reserve(num_points);

    double angle_diff = end_angle - start_angle;
    // 归一化角度差
    while (angle_diff > M_PI) angle_diff -= 2.0 * M_PI;
    while (angle_diff < -M_PI) angle_diff += 2.0 * M_PI;

    for (int i = 0; i < num_points; ++i) {
        double t = static_cast<double>(i) / (num_points - 1);
        double angle = start_angle + t * angle_diff;

        PathPoint p;
        p.x = center_x + radius * std::cos(angle);
        p.y = center_y + radius * std::sin(angle);

        // 切线方向垂直于半径方向
        if (angle_diff > 0) {
            p.theta = angle + M_PI / 2.0;
        } else {
            p.theta = angle - M_PI / 2.0;
        }

        // 圆的曲率是半径的倒数
        p.kappa = 1.0 / radius;
        if (angle_diff < 0) p.kappa = -p.kappa;

        path_points_.push_back(p);
    }
}

void PathCurve::generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
                                   const PathPoint& p2, const PathPoint& p3,
                                   int num_points) {
    path_points_.clear();
    path_points_.reserve(num_points);

    for (int i = 0; i < num_points; ++i) {
        double t = static_cast<double>(i) / (num_points - 1);
        double t2 = t * t;
        double t3 = t2 * t;
        double mt = 1.0 - t;
        double mt2 = mt * mt;
        double mt3 = mt2 * mt;

        // 贝塞尔曲线公式
        PathPoint p;
        p.x = mt3 * p0.x + 3.0 * mt2 * t * p1.x + 3.0 * mt * t2 * p2.x + t3 * p3.x;
        p.y = mt3 * p0.y + 3.0 * mt2 * t * p1.y + 3.0 * mt * t2 * p2.y + t3 * p3.y;

        // 一阶导数（切线方向）
        double dx = 3.0 * mt2 * (p1.x - p0.x) + 6.0 * mt * t * (p2.x - p1.x) +
                   3.0 * t2 * (p3.x - p2.x);
        double dy = 3.0 * mt2 * (p1.y - p0.y) + 6.0 * mt * t * (p2.y - p1.y) +
                   3.0 * t2 * (p3.y - p2.y);

        p.theta = std::atan2(dy, dx);

        // 二阶导数（用于计算曲率）
        double ddx = 6.0 * mt * (p2.x - 2.0 * p1.x + p0.x) +
                    6.0 * t * (p3.x - 2.0 * p2.x + p1.x);
        double ddy = 6.0 * mt * (p2.y - 2.0 * p1.y + p0.y) +
                    6.0 * t * (p3.y - 2.0 * p2.y + p1.y);

        // 曲率公式 κ = (x'y'' - y'x'') / (x'^2 + y'^2)^(3/2)
        double velocity_squared = dx * dx + dy * dy;
        if (velocity_squared > 1e-6) {
            p.kappa = (dx * ddy - dy * ddx) / std::pow(velocity_squared, 1.5);
        } else {
            p.kappa = 0.0;
        }

        path_points_.push_back(p);
    }
}

void PathCurve::setPathPoints(const std::vector<PathPoint>& points) {
    path_points_ = points;

    // 计算每个点的切线方向和曲率
    for (size_t i = 0; i < path_points_.size(); ++i) {
        if (i == 0 && path_points_.size() > 1) {
            // 第一个点
            double dx = path_points_[i + 1].x - path_points_[i].x;
            double dy = path_points_[i + 1].y - path_points_[i].y;
            path_points_[i].theta = std::atan2(dy, dx);
        } else if (i == path_points_.size() - 1 && path_points_.size() > 1) {
            // 最后一个点
            double dx = path_points_[i].x - path_points_[i - 1].x;
            double dy = path_points_[i].y - path_points_[i - 1].y;
            path_points_[i].theta = std::atan2(dy, dx);
        } else if (path_points_.size() > 2) {
            // 中间点
            double dx = path_points_[i + 1].x - path_points_[i - 1].x;
            double dy = path_points_[i + 1].y - path_points_[i - 1].y;
            path_points_[i].theta = std::atan2(dy, dx);

            // 计算曲率（使用三点法）
            if (i > 0 && i < path_points_.size() - 1) {
                path_points_[i].kappa = computeCurvature(
                    path_points_[i - 1], path_points_[i], path_points_[i + 1]);
            }
        }
    }
}

PathPoint PathCurve::getPointAt(double t) const {
    if (path_points_.empty()) {
        return PathPoint();
    }

    if (path_points_.size() == 1) {
        return path_points_[0];
    }

    // 限制t在[0, 1]范围内
    t = std::max(0.0, std::min(1.0, t));
    double index_float = t * (path_points_.size() - 1);
    size_t index = static_cast<size_t>(index_float);

    if (index >= path_points_.size() - 1) {
        return path_points_.back();
    }

    // 线性插值
    double alpha = index_float - index;
    const PathPoint& p1 = path_points_[index];
    const PathPoint& p2 = path_points_[index + 1];

    PathPoint result;
    result.x = p1.x + alpha * (p2.x - p1.x);
    result.y = p1.y + alpha * (p2.y - p1.y);
    result.theta = p1.theta + alpha * (p2.theta - p1.theta);
    result.kappa = p1.kappa + alpha * (p2.kappa - p1.kappa);

    return result;
}

double PathCurve::getPathLength() const {
    double length = 0.0;
    for (size_t i = 1; i < path_points_.size(); ++i) {
        double dx = path_points_[i].x - path_points_[i-1].x;
        double dy = path_points_[i].y - path_points_[i-1].y;
        length += std::sqrt(dx * dx + dy * dy);
    }
    return length;
}

int PathCurve::findNearestPoint(double x, double y) const {
    if (path_points_.empty()) {
        return -1;
    }

    int nearest_index = 0;
    double min_distance = std::numeric_limits<double>::max();

    for (size_t i = 0; i < path_points_.size(); ++i) {
        double dx = x - path_points_[i].x;
        double dy = y - path_points_[i].y;
        double distance = std::sqrt(dx * dx + dy * dy);

        if (distance < min_distance) {
            min_distance = distance;
            nearest_index = static_cast<int>(i);
        }
    }

    return nearest_index;
}

double PathCurve::computeCurvature(const PathPoint& p1, const PathPoint& p2,
                                  const PathPoint& p3) const {
    // 使用三点计算曲率
    double dx1 = p2.x - p1.x;
    double dy1 = p2.y - p1.y;
    double dx2 = p3.x - p2.x;
    double dy2 = p3.y - p2.y;

    double cross = dx1 * dy2 - dy1 * dx2;
    double d1 = std::sqrt(dx1 * dx1 + dy1 * dy1);
    double d2 = std::sqrt(dx2 * dx2 + dy2 * dy2);

    if (d1 < 1e-6 || d2 < 1e-6) {
        return 0.0;
    }

    // Menger曲率公式
    double area = std::abs(cross) / 2.0;
    double d3_sq = (p3.x - p1.x) * (p3.x - p1.x) + (p3.y - p1.y) * (p3.y - p1.y);
    double d3 = std::sqrt(d3_sq);

    if (d3 < 1e-6) {
        return 0.0;
    }

    return 4.0 * area / (d1 * d2 * d3);
}