#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/TwistStamped.h>
#include <cmath>
#include <vector>
#include <fstream>
#include <sstream>
#include <limits>

// 车辆参数
const double L_F = 0.19;             // 前轴到质心距离 (m)
const double L_R = 0.13;             // 后轴到质心距离 (m)
const double WHEEL_BASE = L_F + L_R; // 轴距 L = 0.32 m

// 控制参数
const double LOOKAHEAD_DIST = 0.5;   // 前视距离 Ld (m)
const double TARGET_SPEED   = 1.0;   // 目标车速 (m/s)
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...");

        // 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) 订阅当前位置和航向
        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) 发布期望控制指令
        cmd_pub_ = nh.advertise<geometry_msgs::TwistStamped>("/desired_status", 10);

        // 初始化上次时间点
        last_time_ = ros::Time::now();
    }

    void update() {
        // 必须收到一次完整的 x,y,yaw
        if (!got_x_ || !got_y_ || !got_yaw_) {
            return;
        }

        ros::Time now = ros::Time::now();
        double dt = (now - last_time_).toSec();

        // 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;
        }

        // ------- Pure Pursuit 核心 -------
        // 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_);
        for (size_t i = nearest_idx; i < path_.size(); ++i) {
            double dx = path_[i].first  - current_x_;
            double dy = path_[i].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);
            if (Ld >= LOOKAHEAD_DIST && x_local > 0.0) {
                goal_idx = i;
                found = true;
                break;
            }
        }
        if (!found) {
            ROS_WARN("[Search] No front point ≥%.2fm found; using last valid point", LOOKAHEAD_DIST);
        }

        double goal_x = path_[goal_idx].first;
        double goal_y = path_[goal_idx].second;

        // C) 坐标转换到车辆系
        double dx = goal_x - current_x_;
        double dy = goal_y - current_y_;
        double x_local =  cy * dx + sy * dy;
        double y_local = -sy * dx + cy * dy;
        double Ld = std::hypot(x_local, y_local);

        // D) 计算转向角 δ = atan2(2 L sinα, Ld)
        double alpha = std::atan2(y_local, x_local);
        double steer_rad = 0.0;
        if (Ld > 1e-6) {
            steer_rad = std::atan2(2.0 * WHEEL_BASE * std::sin(alpha), Ld);
        }
        double steer_deg = steer_rad * 180.0 / M_PI;

        // 5) 速度 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;

        // 6) 发布指令
        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("[Publish] speed=%.3f m/s, steer=%.3f deg (goal_idx=%zu)", 
                 cmd_speed, steer_deg, goal_idx);

        // 更新历史状态
        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_;

    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);
    while (ros::ok()) {
        ros::spinOnce();
        pp.update();
        rate.sleep();
    }
    return 0;
}