#include <chrono>
#include <cmath>
#include <memory>
#include <sstream>
#include <string>
#include <vector>
#include <algorithm>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class EthercatVelLoop : public rclcpp::Node
{
public:
  EthercatVelLoop() : Node("ethercat_vel_loop")
  {
    // 参数
    topic_ = declare_parameter<std::string>("topic", "/ethercat/set");
    max_vel_ = declare_parameter<int>("max_vel", 30000);

    // 兼容旧参数名 forward_duration_sec（如果外部还在用）
    double forward_duration = declare_parameter<double>("forward_duration_sec", 3.0);
    ramp_duration_ = declare_parameter<double>("ramp_duration_sec", forward_duration);

    decel_duration_ = declare_parameter<double>("decel_duration_sec", 1.0);
    stop_duration_  = declare_parameter<double>("stop_duration_sec", 1.0);
    rate_hz_        = declare_parameter<double>("rate_hz", 10.0);
    motor_ids_      = declare_parameter<std::vector<int64_t>>("motor_ids", {0, 1, 2});

    if (rate_hz_ <= 0.0) {
      RCLCPP_WARN(get_logger(), "rate_hz <= 0, force to 10 Hz");
      rate_hz_ = 10.0;
    }

    pub_ = create_publisher<std_msgs::msg::String>(topic_, 10);

    timer_period_ = std::chrono::duration<double>(1.0 / rate_hz_);
    timer_ = create_wall_timer(
      std::chrono::duration_cast<std::chrono::milliseconds>(timer_period_),
      std::bind(&EthercatVelLoop::onTimer, this));

    RCLCPP_INFO(get_logger(),
      "Started ethercat_vel_loop: topic=%s, max_vel=%d, ramp=%.3fs, decel=%.3fs, stop=%.3fs, rate=%.1fHz",
      topic_.c_str(), max_vel_, ramp_duration_, decel_duration_, stop_duration_, rate_hz_);
  }

private:
  enum class Phase { FWD_ACCEL, FWD_DECEL, STOP1, REV_ACCEL, REV_DECEL, STOP2 };

  void onTimer()
  {
    time_in_phase_ += std::chrono::duration<double>(timer_period_).count();

    int target_vel = 0;

    switch (phase_) {
      case Phase::FWD_ACCEL: {
        // 0 -> +max_vel_，线性加速，持续 ramp_duration_
        double frac = ramp_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / ramp_duration_) : 1.0;
        target_vel = static_cast<int>(std::round(max_vel_ * frac));
        if (time_in_phase_ >= ramp_duration_) {
          // 加速完成后：如果需要减速缓冲，进入减速段，否则直接停
          if (decel_duration_ > 0.0) switchPhase(Phase::FWD_DECEL);
          else                       switchPhase(Phase::STOP1);
        }
        break;
      }
      case Phase::FWD_DECEL: {
        // +max_vel_ -> 0，线性减速，持续 decel_duration_
        double frac = decel_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / decel_duration_) : 1.0;
        target_vel = static_cast<int>(std::round(max_vel_ * (1.0 - frac)));
        if (time_in_phase_ >= decel_duration_) {
          switchPhase(Phase::STOP1);
        }
        break;
      }
      case Phase::STOP1: {
        target_vel = 0;
        if (time_in_phase_ >= stop_duration_) {
          switchPhase(Phase::REV_ACCEL);
        }
        break;
      }
      case Phase::REV_ACCEL: {
        // 0 -> -max_vel_，线性加速，持续 ramp_duration_
        double frac = ramp_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / ramp_duration_) : 1.0;
        target_vel = -static_cast<int>(std::round(max_vel_ * frac));
        if (time_in_phase_ >= ramp_duration_) {
          if (decel_duration_ > 0.0) switchPhase(Phase::REV_DECEL);
          else                       switchPhase(Phase::STOP2);
        }
        break;
      }
      case Phase::REV_DECEL: {
        // -max_vel_ -> 0，线性减速，持续 decel_duration_
        double frac = decel_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / decel_duration_) : 1.0;
        target_vel = -static_cast<int>(std::round(max_vel_ * (1.0 - frac)));
        if (time_in_phase_ >= decel_duration_) {
          switchPhase(Phase::STOP2);
        }
        break;
      }
      case Phase::STOP2: {
        target_vel = 0;
        if (time_in_phase_ >= stop_duration_) {
          switchPhase(Phase::FWD_ACCEL);
        }
        break;
      }
    }

    // 拼装字符串："0 vel XXX; 1 vel XXX; 2 vel XXX"
    std::ostringstream oss;
    for (size_t i = 0; i < motor_ids_.size(); ++i) {
      if (i > 0) oss << "; ";
      oss << motor_ids_[i] << " vel " << target_vel;
    }

    std_msgs::msg::String msg;
    msg.data = oss.str();
    pub_->publish(msg);
  }

  void switchPhase(Phase next)
  {
    phase_ = next;
    time_in_phase_ = 0.0;
  }

  // 参数
  std::string topic_;
  int max_vel_;
  double ramp_duration_;
  double decel_duration_;
  double stop_duration_;
  double rate_hz_;
  std::vector<int64_t> motor_ids_;

  // 发布与定时
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::chrono::duration<double> timer_period_;

  // 状态机
  Phase phase_{Phase::FWD_ACCEL};
  double time_in_phase_{0.0};
};

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