#include "path_curve.h"
#include <fstream>
#include <sstream>
#include <iostream>

// CSV加载功能实现
bool PathCurve::loadFromCSV(const std::string& filename, bool has_header) {
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cerr << "Error: Cannot open file " << filename << std::endl;
        return false;
    }

    std::vector<PathPoint> points;
    std::string line;
    int line_num = 0;

    // 跳过表头
    if (has_header && std::getline(file, line)) {
        line_num++;
    }

    while (std::getline(file, line)) {
        line_num++;
        // 跳过空行和注释行
        if (line.empty() || line[0] == '#') {
            continue;
        }

        std::stringstream ss(line);
        std::string token;
        std::vector<double> values;
        bool parse_error = false;

        // 解析CSV行
        while (std::getline(ss, token, ',')) {
            try {
                // 去除前后空格
                size_t start = token.find_first_not_of(" \t\r\n");
                size_t end = token.find_last_not_of(" \t\r\n");
                if (start == std::string::npos) {
                    // 空token，跳过整行
                    parse_error = true;
                    break;
                }
                std::string trimmed = token.substr(start, end - start + 1);
                values.push_back(std::stod(trimmed));
            } catch (const std::exception&) {
                std::cerr << "Error parsing line " << line_num << ": " << line << std::endl;
                parse_error = true;
                break;
            }
        }

        // 如果解析出错或值数量不足，跳过整行
        if (parse_error) {
            continue;
        }

        // 根据列数创建路径点
        if (values.size() >= 2) {
            PathPoint p;
            p.x = values[0];
            p.y = values[1];
            p.theta = (values.size() >= 3) ? values[2] : 0.0;
            p.kappa = (values.size() >= 4) ? values[3] : 0.0;
            points.push_back(p);
        }
    }

    file.close();

    if (points.empty()) {
        std::cerr << "Error: No valid path points loaded from " << filename << std::endl;
        return false;
    }

    // 设置路径点（会自动计算theta和kappa）
    setPathPoints(points);
    
    std::cout << "Successfully loaded " << points.size() << " points from " << filename << std::endl;
    return true;
}

// CSV保存功能实现
bool PathCurve::saveToCSV(const std::string& filename) const {
    std::ofstream file(filename);
    if (!file.is_open()) {
        std::cerr << "Error: Cannot create file " << filename << std::endl;
        return false;
    }

    // 写入表头
    file << "# Custom Path Data\n";
    file << "# x(m), y(m), theta(rad), kappa(1/m)\n";

    // 写入路径点
    for (const auto& p : path_points_) {
        file << p.x << ", " << p.y << ", " << p.theta << ", " << p.kappa << "\n";
    }

    file.close();
    std::cout << "Successfully saved " << path_points_.size() << " points to " << filename << std::endl;
    return true;
}

// Catmull-Rom样条插值实现
void PathCurve::generateSpline(const std::vector<PathPoint>& key_points,
                               int num_points,
                               double tension) {
    if (key_points.size() < 2) {
        std::cerr << "Error: Need at least 2 key points for spline interpolation" << std::endl;
        return;
    }

    path_points_.clear();
    path_points_.reserve(num_points);

    // 参数化张力 (0 = 最平滑, 1 = 最紧)
    double s = (1.0 - tension) / 2.0;

    // 对每一段进行插值
    int segments = static_cast<int>(key_points.size()) - 1;
    int points_per_segment = num_points / segments;

    for (int seg = 0; seg < segments; ++seg) {
        // 获取控制点（使用边界处理）
        PathPoint p0 = (seg == 0) ? key_points[0] : key_points[seg - 1];
        PathPoint p1 = key_points[seg];
        PathPoint p2 = key_points[seg + 1];
        PathPoint p3 = (seg == segments - 1) ? key_points[seg + 1] : key_points[seg + 2];

        // 对每一段生成点
        int points_in_this_segment = (seg == segments - 1) ? 
            (num_points - seg * points_per_segment) : points_per_segment;

        for (int i = 0; i < points_in_this_segment; ++i) {
            double t = static_cast<double>(i) / points_per_segment;
            double t2 = t * t;
            double t3 = t2 * t;

            // Catmull-Rom 基函数
            double b0 = s * ((-t3 + 2.0*t2 - t));
            double b1 = s * ((3.0*t3 - 5.0*t2) / 2.0) + 1.0;
            double b2 = s * ((-3.0*t3 + 4.0*t2 + t) / 2.0);
            double b3 = s * (t3 - t2);

            // 调整系数以确保通过控制点
            if (seg > 0 || i > 0) {
                b0 *= 2.0;
                b1 = b1 * 2.0 - b0 / 2.0;
                b2 = b2 * 2.0 - b3 / 2.0;
                b3 *= 2.0;
            }

            PathPoint p;
            p.x = b0*p0.x + b1*p1.x + b2*p2.x + b3*p3.x;
            p.y = b0*p0.y + b1*p1.y + b2*p2.y + b3*p3.y;
            p.theta = 0.0;  // 将由setPathPoints计算
            p.kappa = 0.0;  // 将由setPathPoints计算

            path_points_.push_back(p);
        }
    }

    // 重新计算所有点的theta和kappa
    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]);
            }
        }
    }

    std::cout << "Generated spline with " << path_points_.size() 
              << " points from " << key_points.size() << " key points" << std::endl;
}