#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <sstream>
#include <iomanip>
#include <cmath>
#include <chrono>
#include <string>
#include <vector>

using namespace std::chrono_literals;

class PosRampNode : public rclcpp::Node {
public:
  PosRampNode() : Node("pos_ramp_node"),
                  steady_clock_(RCL_STEADY_TIME)
  {
    // 参数
    declare_parameter<int>("num_joints", 6);                    // 轴数
    declare_parameter<int>("start_pos", 0);                     // 起始位置（计数）
    declare_parameter<int>("end_pos", 2000);                    // 终点位置（计数）
    declare_parameter<double>("v_max", 8000.0);                 // 最大速度（counts/s）
    declare_parameter<double>("a_max", 20000.0);                // 最大加速度（counts/s^2）
    declare_parameter<double>("pause_sec", 5.0);                // 暂停时长（s）
    declare_parameter<int>("rate_hz", 100);                     // 发布频率（Hz）

    get_parameter("num_joints", num_joints_);
    get_parameter("start_pos", start_pos_);
    get_parameter("end_pos", end_pos_);
    get_parameter("v_max", v_max_);
    get_parameter("a_max", a_max_);
    get_parameter("pause_sec", pause_sec_);
    get_parameter("rate_hz", rate_hz_);

    if (rate_hz_ <= 0) rate_hz_ = 100;
    dt_ = 1.0 / static_cast<double>(rate_hz_);

    pub_ = create_publisher<std_msgs::msg::String>("ethercat/set", 10);

    // 轨迹状态初始化
    phase_ = Phase::MoveOut;
    pos_   = static_cast<double>(start_pos_);
    vel_   = 0.0;
    goal_  = static_cast<double>(end_pos_);
    last_  = steady_clock_.now();

    timer_ = create_wall_timer(
      std::chrono::milliseconds(static_cast<int>(1000.0 / rate_hz_)),
      std::bind(&PosRampNode::onTimer, this));

    RCLCPP_INFO(get_logger(),
      "pos_ramp_node started: num_joints=%d, start=%d, end=%d, v_max=%.1f, a_max=%.1f, pause=%.1fs, rate=%d Hz",
      num_joints_, start_pos_, end_pos_, v_max_, a_max_, pause_sec_, rate_hz_);
  }

private:
  enum class Phase { MoveOut, HoldOut, MoveBack, HoldStart };

  void onTimer() {
    // 用稳态时钟计算 dt，尽量跟墙钟定时器保持一致
    const rclcpp::Time now = steady_clock_.now();
    double dt = (now - last_).seconds();
    last_ = now;
    if (dt <= 0.0) dt = dt_;     // 回退到标称步长
    // 为了轨迹稳定，限制 dt 的异常尖峰
    if (dt > 3 * dt_) dt = dt_;

    switch (phase_) {
      case Phase::MoveOut:   stepMotion(+1, dt, static_cast<double>(end_pos_), Phase::HoldOut);   break;
      case Phase::HoldOut:   stepHold(dt, Phase::MoveBack, static_cast<double>(start_pos_));      break;
      case Phase::MoveBack:  stepMotion(-1, dt, static_cast<double>(start_pos_), Phase::HoldStart);break;
      case Phase::HoldStart: stepHold(dt, Phase::MoveOut, static_cast<double>(end_pos_));         break;
    }

    // 发布到 ethercat/set：对所有轴发相同位置
    const int pos_i = static_cast<int>(std::llround(pos_));
    std::ostringstream ss;
    for (int i = 0; i < num_joints_; ++i) {
      if (i) ss << "; ";
      ss << i << " pos " << pos_i;
    }
    std_msgs::msg::String msg;
    msg.data = ss.str();
    pub_->publish(msg);
  }

  // 运动步进：梯形速度（加速/匀速/减速），避免超调，平滑到达 goal
  void stepMotion(int dir, double dt, double goal_target, Phase next_hold_phase) {
    const double s_rem = (goal_target - pos_);          // 剩余位移
    const double sign  = (dir >= 0) ? 1.0 : -1.0;
    const double a     = a_max_ * sign;

    // 需要的减速距离 d = v^2 / (2a_max)
    const double d_decel = (vel_*vel_) / (2.0 * a_max_ + 1e-9);

    // 决策：加速还是减速（或保持）
    if (sign * s_rem <= d_decel) {
      // 该减速了
      vel_ -= a_max_ * sign * dt;
      // 过零保护
      if (sign * vel_ < 0.0) vel_ = 0.0;
    } else {
      // 加速直到 v_max
      vel_ += a * dt;
      const double v_lim = v_max_;
      if (std::fabs(vel_) > v_lim) vel_ = v_lim * sign;
    }

    // 积分位置
    pos_ += vel_ * dt;

    // 过冲保护：若越过目标，截断到目标并进入下一相位
    if ((sign > 0.0 && pos_ >= goal_target) ||
        (sign < 0.0 && pos_ <= goal_target)) {
      pos_ = goal_target;
      vel_ = 0.0;
      phase_ = next_hold_phase;
      hold_t_ = 0.0;
      goal_   = (next_hold_phase == Phase::HoldOut)
                  ? static_cast<double>(end_pos_)
                  : static_cast<double>(start_pos_);
      RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000,
                           "Reached %.3f, entering hold.", goal_target);
    }
  }

  // 暂停相位：计时到 pause_sec_ 后切换到下一运动相位，并设置新目标
  void stepHold(double dt, Phase next_move_phase, double next_goal) {
    hold_t_ += dt;
    pos_     = goal_;   // 保持
    vel_     = 0.0;
    if (hold_t_ >= pause_sec_) {
      phase_ = next_move_phase;
      goal_  = next_goal;
      RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000,
                           "Leaving hold, next goal=%.3f", next_goal);
    }
  }

  // ---- members ----
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Clock steady_clock_;
  rclcpp::Time  last_;

  int   num_joints_{6};
  int   start_pos_{0};
  int   end_pos_{2000};
  int   rate_hz_{100};
  double v_max_{8000.0};
  double a_max_{20000.0};
  double pause_sec_{5.0};
  double dt_{0.01};

  Phase  phase_{Phase::MoveOut};
  double pos_{0.0};
  double vel_{0.0};
  double goal_{0.0};
  double hold_t_{0.0};
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PosRampNode>());
  rclcpp::shutdown();
  return 0;
}
