#include "control_generator.h"
#define _USE_MATH_DEFINES
#include <cmath>
#include <algorithm>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

ControlGenerator::ControlGenerator(const AGVModel& model)
    : model_(model) {
}

ControlSequence ControlGenerator::generate(const PathCurve& path,
                                          const AGVModel::State& initial_state,
                                          double dt, double horizon) {
    // 使用Pure Pursuit作为默认算法
    return generatePurePursuit(path, initial_state, dt, 1.5, 1.0, horizon);
}

ControlSequence ControlGenerator::generatePurePursuit(const PathCurve& path,
                                                     const AGVModel::State& initial_state,
                                                     double dt,
                                                     double lookahead_distance,
                                                     double desired_velocity,
                                                     double horizon) {
    ControlSequence sequence;
    sequence.clear();

    AGVModel::State current_state = initial_state;
    double current_time = 0.0;

    while (current_time < horizon) {
        // 找到前视点
        PathPoint target = findLookaheadPoint(path, current_state, lookahead_distance);

        // 计算控制量
        AGVModel::Control control = computePurePursuitControl(
            current_state, target, desired_velocity);

        // 保存控制量和状态
        sequence.controls.push_back(control);
        sequence.timestamps.push_back(current_time);
        sequence.predicted_states.push_back(current_state);

        // 更新状态
        current_state = model_.update(current_state, control, dt);
        current_time += dt;

        // 修复: 检查是否接近路径终点（阈值放宽以确保完整追踪）
        const auto& path_points = path.getPathPoints();
        if (!path_points.empty()) {
            const PathPoint& end_point = path_points.back();
            double dx = current_state.x - end_point.x;
            double dy = current_state.y - end_point.y;
            double distance_to_end = std::sqrt(dx * dx + dy * dy);

            if (distance_to_end < 0.5) {
                break; // 已到达终点附近
            }
        }
    }

    return sequence;
}

ControlSequence ControlGenerator::generateStanley(const PathCurve& path,
                                                 const AGVModel::State& initial_state,
                                                 double dt,
                                                 double k_gain,
                                                 double desired_velocity,
                                                 double horizon) {
    ControlSequence sequence;
    sequence.clear();

    AGVModel::State current_state = initial_state;
    double current_time = 0.0;

    const auto& path_points = path.getPathPoints();
    if (path_points.empty()) {
        return sequence;
    }

    while (current_time < horizon) {
        // 找到最近的路径点
        int nearest_idx = path.findNearestPoint(current_state.x, current_state.y);

        // 如果找不到最近点（不应该发生），使用第一个点
        if (nearest_idx < 0) {
            nearest_idx = 0;
        }

        PathPoint nearest_point = path_points[nearest_idx];

        // 计算控制量
        AGVModel::Control control = computeStanleyControl(
            current_state, nearest_point, k_gain, desired_velocity);

        // 保存控制量和状态
        sequence.controls.push_back(control);
        sequence.timestamps.push_back(current_time);
        sequence.predicted_states.push_back(current_state);

        // 更新状态
        current_state = model_.update(current_state, control, dt);
        current_time += dt;

        // 修复: 检查是否接近路径终点（阈值放宽以确保完整追踪）
        const PathPoint& end_point = path_points.back();
        double dx = current_state.x - end_point.x;
        double dy = current_state.y - end_point.y;
        double distance_to_end = std::sqrt(dx * dx + dy * dy);

        if (distance_to_end < 0.5) {
            break;
        }
    }

    return sequence;
}

AGVModel::Control ControlGenerator::computePurePursuitControl(
    const AGVModel::State& state,
    const PathPoint& target_point,
    double desired_velocity) {

    AGVModel::Control control;
    control.v = desired_velocity;

    // 计算目标点在车辆坐标系中的位置
    double dx = target_point.x - state.x;
    double dy = target_point.y - state.y;

    // 转换到车辆坐标系
    double cos_theta = std::cos(state.theta);
    double sin_theta = std::sin(state.theta);
    double target_x = cos_theta * dx + sin_theta * dy;
    double target_y = -sin_theta * dx + cos_theta * dy;

    // Pure Pursuit公式计算转向角
    double ld = std::sqrt(target_x * target_x + target_y * target_y);
    if (ld < 1e-3) {
        control.delta = 0.0;
    } else {
        // delta = atan(2 * L * sin(alpha) / ld)
        // 其中 alpha 是前视点相对于车辆航向的角度
        double alpha = std::atan2(target_y, target_x);
        double wheelbase = model_.getWheelbase();
        control.delta = std::atan2(2.0 * wheelbase * std::sin(alpha), ld);
    }

    return control;
}

AGVModel::Control ControlGenerator::computeStanleyControl(
    const AGVModel::State& state,
    const PathPoint& nearest_point,
    double k_gain,
    double desired_velocity) {

    AGVModel::Control control;
    control.v = desired_velocity;

    // 航向误差
    double heading_error = normalizeAngle(nearest_point.theta - state.theta);

    // 横向误差
    double dx = state.x - nearest_point.x;
    double dy = state.y - nearest_point.y;
    double cross_track_error = -std::sin(nearest_point.theta) * dx +
                               std::cos(nearest_point.theta) * dy;

    // Stanley控制律
    // delta = heading_error + atan(k * cross_track_error / v)
    double crosstrack_term = std::atan2(k_gain * cross_track_error,
                                       std::abs(control.v) + 0.1);
    control.delta = heading_error + crosstrack_term;

    return control;
}

PathPoint ControlGenerator::findLookaheadPoint(const PathCurve& path,
                                              const AGVModel::State& state,
                                              double lookahead_distance) const {
    const auto& path_points = path.getPathPoints();
    if (path_points.empty()) {
        return PathPoint(state.x, state.y, state.theta, 0.0);
    }

    // 找到最近的路径点
    int nearest_idx = path.findNearestPoint(state.x, state.y);

    // 如果找不到最近点，返回起点或状态点
    if (nearest_idx < 0) {
        return path_points[0];
    }

    // 从最近点开始向前搜索前视点
    for (size_t i = static_cast<size_t>(nearest_idx); i < path_points.size(); ++i) {
        double dx = path_points[i].x - state.x;
        double dy = path_points[i].y - state.y;
        double distance = std::sqrt(dx * dx + dy * dy);

        if (distance >= lookahead_distance) {
            return path_points[i];
        }
    }

    // 如果没有找到，返回路径的最后一个点
    return path_points.back();
}

double ControlGenerator::normalizeAngle(double angle) const {
    while (angle > M_PI) angle -= 2.0 * M_PI;
    while (angle < -M_PI) angle += 2.0 * M_PI;
    return angle;
}
