#include <ros/ros.h>                          // ROS 核心头文件，提供 ROS 节点、日志、时间等功能
#include <std_msgs/Float32.h>                 // ROS 标准消息：Float32，用于接收位置和方向角数据
#include <geometry_msgs/TwistStamped.h>       // ROS 消息：TwistStamped，用于发布控制指令
#include <cmath>                              // C++ 数学库，提供 sin、cos、hypot、atan2 等函数
#include <vector>                             // C++ 容器 vector，用于存储路径点
#include <fstream>                            // 文件流，用于读取路径文件及日志
#include <sstream>                            // 字符串流，用于解析每行路径点
#include <limits>                             // 数值极限，用于初始化最小距离

// ———————— 参数配置 ————————
// 车辆几何参数
const double L_F = 0.19;             // 前轴到质心距离 (米)
const double L_R = 0.13;             // 后轴到质心距离 (米)
const double WHEEL_BASE = L_F + L_R; // 车轴距 = 前轴到质心 + 后轴到质心

// 控制算法参数
const double LOOKAHEAD_DIST = 0.5;   // 基础前视距离 (米)
const int    MAX_LOOKAHEAD_MULT = 5; // 最大前视距离倍数，最多放大到 5 倍 LOOKAHEAD_DIST
const double TARGET_SPEED   = 1.0;   // 目标巡航速度 (米/秒)
const double KP             = 0.5;   // 速度 P 控制器增益

class PurePursuit {
public:
    // 构造函数：完成路径加载、话题订阅和发布器的初始化
    PurePursuit(ros::NodeHandle &nh)
      : current_x_(0.0), current_y_(0.0), current_yaw_(0.0),
        got_x_(false), got_y_(false), got_yaw_(false),
        first_run_(true)
    {
        ROS_INFO("[Init] PurePursuit starting up...");

        // 打开或创建日志文件，追加模式
        const std::string log_path = "/home/nvidia/anmpc/cmd.csv";
        bool file_exists = std::ifstream(log_path).good();
        log_file_.open(log_path, std::ios::app);
        if (!file_exists) {
            log_file_ << "time,current_x,current_y,goal_x,goal_y,steer_deg,cmd_speed\n";
        }

        // 1) 从文件加载全局路径点
        std::ifstream infile("/home/nvidia/anmpc/test_c.txt");
        if (!infile) {
            ROS_ERROR("无法打开路径文件: /home/nvidia/anmpc/test_c.txt");
            ros::shutdown();             // 打不开文件则退出节点
        }
        double px, py;
        std::string line;
        while (std::getline(infile, line)) {
            std::istringstream ss(line);
            if (ss >> px >> py) {        // 解析两列浮点数
                path_.emplace_back(px, py);  // 存入路径向量
            }
        }
        infile.close();
        if (path_.empty()) {
            ROS_ERROR("路径文件内容为空或格式错误");
            ros::shutdown();
        }
        ROS_INFO("[Init] Loaded %zu waypoints for path tracking.", path_.size());

        // 2) 订阅当前车辆位姿（x, y, yaw）
        sub_x_   = nh.subscribe("/the_x", 10, &PurePursuit::xCB,   this);
        sub_y_   = nh.subscribe("/the_y", 10, &PurePursuit::yCB,   this);
        sub_yaw_ = nh.subscribe("/yaw",   10, &PurePursuit::yawCB, this);

        // 3) 发布期望控制指令到 /desired_status
        cmd_pub_ = nh.advertise<geometry_msgs::TwistStamped>("/desired_status", 10);

        // 记录初始化时刻，用于速度计算
        last_time_ = ros::Time::now();
    }

    // update() 在主循环中被周期调用：完成前视点搜索、转向角和速度计算，并发布
    void update() {
        // 如果任意位姿尚未收到，跳过
        if (!got_x_ || !got_y_ || !got_yaw_) return;

        ros::Time now = ros::Time::now();
        double dt = (now - last_time_).toSec();  // 自上次 update 以来的时间差

        // 4) 计算当前速度（首次运行跳过）
        double current_speed = 0.0;
        if (!first_run_ && dt > 1e-6) {
            double dx = current_x_ - last_x_;
            double dy = current_y_ - last_y_;
            current_speed = std::hypot(dx, dy) / dt;  // 欧氏距离 / 时间 = 速度
        }

        // ———— A) 最近路径点索引 ———
        size_t nearest_idx = 0;
        double min_d = std::numeric_limits<double>::infinity();
        for (size_t i = 0; i < path_.size(); ++i) {
            double dx = path_[i].first  - current_x_;
            double dy = path_[i].second - current_y_;
            double d  = std::hypot(dx, dy);
            if (d < min_d) {
                min_d = d;
                nearest_idx = i;             // 更新最近点索引
            }
        }

        // ———— B) 动态放大前视搜索 ———
        size_t goal_idx = path_.size() - 1;
        bool   found = false;
        double cy = std::cos(current_yaw_), sy = std::sin(current_yaw_);
        size_t path_size = path_.size();

        for (int m = 1; m <= MAX_LOOKAHEAD_MULT && !found; ++m) {
            double search_dist = LOOKAHEAD_DIST * m;
            for (size_t j = 0; j < path_size; ++j) {
                size_t i = (nearest_idx + j) % path_size; // 支持路径结尾环绕

                double dx = path_[i].first  - current_x_;
                double dy = path_[i].second - current_y_;
                // 转到车身坐标系: x_local 为前向，y_local 为横向
                double x_local =  cy * dx + sy * dy;
                double y_local = -sy * dx + cy * dy;
                double Ld = std::hypot(x_local, y_local);

                // 必须是车头前方 (x_local > 一定阈值) 且满足放大后前视距离
                if (Ld >= search_dist && x_local > 0.1) { // 保证确实在前方，避免后方点
                    goal_idx = i;
                    found = true;
                    ROS_WARN("[Search] found forward point at %dx LOOKAHEAD (idx=%zu)", m, goal_idx);
                    break;
                }

                // 防止搜索过多，最多搜索一定范围
                if (j > 100) {
                    ROS_WARN("[Search] search limit reached without finding suitable point.");
                    break;
                }
            }
        }

        ROS_INFO("[Track] Current=(%.3f,%.3f), nearest_idx=%zu, nearest_pt=(%.3f,%.3f), goal_idx=%zu, goal_pt=(%.3f,%.3f)",
                 current_x_, current_y_, nearest_idx,
                 path_[nearest_idx].first, path_[nearest_idx].second,
                 goal_idx,
                 path_[goal_idx].first, path_[goal_idx].second);

        // ———— C) 降级策略 ———
        if (!found) {
            if (nearest_idx + 1 < path_.size()) {
                goal_idx = nearest_idx + 1;  // 退而求其次，取下一个点
                ROS_WARN("[Fallback] use next point idx=%zu", goal_idx);
            } else {
                // 如果已经在路径尾，则继续使用最后一个点
                ROS_WARN("[Fallback] at path end, use last idx=%zu", goal_idx);
            }
        }

        // ———— D) 计算转向角 ———
        double dx = path_[goal_idx].first  - current_x_;
        double dy = path_[goal_idx].second - current_y_;
        double x_local =  cy * dx + sy * dy;
        double y_local = -sy * dx + cy * dy;
        double Ld = std::hypot(x_local, y_local);
        double alpha = std::atan2(y_local, x_local);  // 目标点相对航向的偏角
        double steer_rad = 0.0;
        if (Ld > 1e-6) {
            // Pure Pursuit 公式：δ = atan2(2 L wheel_base sin α, Ld)
            steer_rad = std::atan2(2.0 * WHEEL_BASE * std::sin(alpha), Ld);
            //steer_rad = alpha;
        }
        double steer_deg = steer_rad * 180.0 / M_PI;  // 转为度，便于阅读

        // ———— E) 速度 P 控制 ———
        double cmd_speed = TARGET_SPEED;
        if (!first_run_) {
            double err = TARGET_SPEED - current_speed;
            cmd_speed = TARGET_SPEED + KP * err;
        }
        // 限制在 [0, 2×TARGET_SPEED] 范围
        if (cmd_speed < 0.0) cmd_speed = 0.0;
        if (cmd_speed > 2.0 * TARGET_SPEED) cmd_speed = 2.0 * TARGET_SPEED;

        // ———— F) 发布控制指令 ———
        geometry_msgs::TwistStamped cmd;
        cmd.header.stamp = now;
        cmd.header.frame_id = "base_link";
        cmd.twist.linear.x  = cmd_speed;    // 前进速度
        cmd.twist.angular.x = steer_deg;    // 转向角度 (度)
        cmd_pub_.publish(cmd);

        // 日志输出：速度、转向、最近点 & 目标点索引
        ROS_INFO("[PUB] spd=%.3f m/s steer=%.3f° (nearest=%zu goal=%zu)",
                 cmd_speed, steer_deg, nearest_idx, goal_idx);

        // ———— G) 写日志到 CSV ———
        log_file_ << now.toSec() << ","
                  << current_x_ << "," << current_y_ << ","
                  << path_[goal_idx].first << "," << path_[goal_idx].second << ","
                  << steer_deg << "," << cmd_speed << "\n";
        log_file_.flush();

        // 更新历史，用于下一次速度计算
        last_x_    = current_x_;
        last_y_    = current_y_;
        last_time_ = now;
        first_run_ = false;
    }

private:
    // 回调：设置当前位置/方向并标记已接收
    void xCB(const std_msgs::Float32::ConstPtr& m) { current_x_   = m->data; got_x_   = true; }
    void yCB(const std_msgs::Float32::ConstPtr& m) { current_y_   = m->data; got_y_   = true; }
    void yawCB(const std_msgs::Float32::ConstPtr& m){ current_yaw_ = m->data; got_yaw_ = true; }

    // 数据成员
    std::vector<std::pair<double,double>> path_;  // 全局路径点
    ros::Subscriber sub_x_, sub_y_, sub_yaw_;     // 位姿订阅者
    ros::Publisher  cmd_pub_;                     // 控制指令发布者
    std::ofstream   log_file_;                    // CSV日志文件

    double current_x_, current_y_, current_yaw_;  // 当前位姿
    double last_x_, last_y_;                      // 上一时刻位置，用于速度计算
    ros::Time last_time_;                         // 上一时刻时间
    bool got_x_, got_y_, got_yaw_, first_run_;    // 标志位
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "pure_pursuit");        // 节点初始化
    ros::NodeHandle nh;
    PurePursuit pp(nh);                           // 创建控制器实例

    ros::Rate rate(10);                           // 10Hz 控制循环
    while (ros::ok()) {
        ros::spinOnce();      // 调用所有回调
        pp.update();          // 计算并发布控制
        rate.sleep();         // 等待下一个周期
    }
    return 0;
}