#include "path_tracker.h"
#include <iostream>
#include <iomanip>

#define _USE_MATH_DEFINES
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

PathTracker::PathTracker(const AGVModel& model)
    : model_(model)
    , control_generator_(model)
    , initial_state_(0.0, 0.0, 0.0) {
}

void PathTracker::setReferencePath(const PathCurve& path) {
    reference_path_ = path;
}

void PathTracker::setInitialState(const AGVModel::State& state) {
    initial_state_ = state;
}

bool PathTracker::generateControlSequence(const std::string& algorithm,
                                         double dt, double horizon,
                                         double desired_velocity) {
    if (reference_path_.getPathPoints().empty()) {
        std::cerr << "Error: Reference path is empty!" << std::endl;
        return false;
    }

    if (algorithm == "pure_pursuit") {
        // 修复: 自适应前视距离 = 速度 × 2.0，最小1.0米
        double lookahead = std::max(1.0, desired_velocity * 2.0);
        control_sequence_ = control_generator_.generatePurePursuit(
            reference_path_, initial_state_, dt, lookahead, desired_velocity, horizon);
    } else if (algorithm == "stanley") {
        // 修复: 增加k_gain到2.0以提高响应性
        control_sequence_ = control_generator_.generateStanley(
            reference_path_, initial_state_, dt, 2.0, desired_velocity, horizon);
    } else {
        std::cerr << "Error: Unknown algorithm type \"" << algorithm << "\"" << std::endl;
        return false;
    }

    return true;
}

void PathTracker::printControlSequence() const {
    if (control_sequence_.size() == 0) {
        std::cout << "Control sequence is empty!" << std::endl;
        return;
    }

    std::cout << "\n========== Control Sequence ==========" << std::endl;
    std::cout << std::fixed << std::setprecision(4);
    std::cout << std::setw(8) << "Time(s)"
              << std::setw(12) << "Velocity(m/s)"
              << std::setw(15) << "Steering(rad)"
              << std::setw(15) << "Steering(deg)" << std::endl;
    std::cout << std::string(50, '-') << std::endl;

    for (size_t i = 0; i < control_sequence_.size(); ++i) {
        double time = control_sequence_.timestamps[i];
        double velocity = control_sequence_.controls[i].v;
        double steering_rad = control_sequence_.controls[i].delta;
        double steering_deg = steering_rad * 180.0 / M_PI;

        std::cout << std::setw(8) << time
                  << std::setw(12) << velocity
                  << std::setw(15) << steering_rad
                  << std::setw(15) << steering_deg << std::endl;
    }

    std::cout << "=============================" << std::endl;
    std::cout << "Total control steps: " << control_sequence_.size() << std::endl;
}

bool PathTracker::saveControlSequence(const std::string& filename) const {
    std::ofstream file(filename);
    if (!file.is_open()) {
        std::cerr << "Unable to open file: " << filename << std::endl;
        return false;
    }

    file << "# AGV Control Sequence" << std::endl;
    file << "# Time(s), Velocity(m/s), Steering(rad), Steering(deg)" << std::endl;
    file << std::fixed << std::setprecision(6);

    for (size_t i = 0; i < control_sequence_.size(); ++i) {
        double time = control_sequence_.timestamps[i];
        double velocity = control_sequence_.controls[i].v;
        double steering_rad = control_sequence_.controls[i].delta;
        double steering_deg = steering_rad * 180.0 / M_PI;

        file << time << ", "
             << velocity << ", "
             << steering_rad << ", "
             << steering_deg << std::endl;
    }

    file.close();
    std::cout << "Control sequence saved to: " << filename << std::endl;
    return true;
}

bool PathTracker::saveTrajectory(const std::string& filename) const {
    std::ofstream file(filename);
    if (!file.is_open()) {
        std::cerr << "Unable to open file: " << filename << std::endl;
        return false;
    }

    file << "# AGV Predicted Trajectory" << std::endl;
    file << "# x(m), y(m), theta(rad), theta(deg)" << std::endl;
    file << std::fixed << std::setprecision(6);

    for (size_t i = 0; i < control_sequence_.predicted_states.size(); ++i) {
        const auto& state = control_sequence_.predicted_states[i];
        double theta_deg = state.theta * 180.0 / M_PI;

        file << state.x << ", "
             << state.y << ", "
             << state.theta << ", "
             << theta_deg << std::endl;
    }

    file.close();
    std::cout << "Trajectory saved to: " << filename << std::endl;
    return true;
}
