//
// Created by lgv on 2026/3/9.
//

#pragma once

#include <algorithm>
#include <cmath>
#include <vector>

namespace cmvr
{

/**
 * @brief 单次速度切换的 S 曲线剖面参数
 *
 * 本类表示从任意 (v0, a0) 连续规划到 (vf, 0) 的 3 段 jerk-limited profile：
 *
 *   seg1: jerk = j1, duration = t1
 *   seg2: jerk = j2, duration = t2   (当前实现 j2 = 0)
 *   seg3: jerk = j3, duration = t3
 *
 * 其中 j1/j3 为 ±j_max，j2 为 0。
 */
struct SCurveVelocityProfile
{
  double t1;
  double t2;
  double t3;

  double total_time;

  double j1;
  double j2;
  double j3;

  double v0;
  double a0;
  double vf;

  bool valid;

  SCurveVelocityProfile()
    : t1(0.0), t2(0.0), t3(0.0)
    , total_time(0.0)
    , j1(0.0), j2(0.0), j3(0.0)
    , v0(0.0), a0(0.0), vf(0.0)
    , valid(false)
  {}
};

/**
 * @brief S 曲线速度规划器状态
 */
struct SCurveVelocityPlannerState
{
  double velocity;
  double acceleration;
  double jerk;

  double target_velocity;

  double elapsed_time;
  bool has_active_profile;
  bool is_moving;

  SCurveVelocityPlannerState()
    : velocity(0.0)
    , acceleration(0.0)
    , jerk(0.0)
    , target_velocity(0.0)
    , elapsed_time(0.0)
    , has_active_profile(false)
    , is_moving(false)
  {}
};

/**
 * @brief 单轴速度型 S 曲线规划器
 *
 * 语义：
 * - setTargetVelocity(v): 设置新目标并重规划
 * - update(dt): 只采样当前 active profile
 * - overwriteState(v,a): 仅覆盖当前状态并清掉旧 profile
 * - synchronizeAndReplan(v,a): 用测量状态同步，并按当前目标重规划
 *
 * 数学目标：
 * - 从任意 (v0, a0) 连续规划到 (vf, 0)
 * - 满足 |v| <= v_max, |a| <= a_max, |j| <= j_max
 */
class SCurveVelocityPlanner1D
{
public:
  SCurveVelocityPlanner1D(double max_velocity = 1.0,
                          double max_acceleration = 1.0,
                          double max_jerk = 5.0);

  void setConstraints(double max_velocity, double max_acceleration, double max_jerk);
  void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const;

  /**
   * @brief 设置目标变化阈值
   *
   * 目标变化量小于该阈值时，不触发重规划。
   */
  void setTargetReplanThreshold(double threshold);

  /**
   * @brief 设置同步反馈时的重规划阈值
   *
   * 测量速度与当前 profile 采样状态的偏差小于该阈值时，
   * synchronizeAndReplan() 只对齐当前状态，不打断现有 profile。
   */
  void setSynchronizeReplanThreshold(double threshold);

  void initialize(double velocity = 0.0, double acceleration = 0.0);
  void reset();

  /**
   * @brief 仅覆盖当前状态，并清掉旧 profile
   *
   * 注意：
   * - 这个接口不会自动重规划
   * - 用于初始化、调试、或显式中断旧轨迹
   */
  void overwriteState(double velocity,
                      double acceleration = 0.0,
                      bool keep_target_velocity = true);

  /**
   * @brief 用测量状态同步，并按当前目标重新规划
   *
   * 这是闭环场景真正该用的接口。
   */
  void synchronizeAndReplan(double velocity,
                            double acceleration = 0.0);

  /**
   * @brief 设置目标速度，并立即重规划
   */
  void setTargetVelocity(double target_velocity);

  /**
   * @brief 平滑停下
   */
  void stop() { setTargetVelocity(0.0); }

  /**
   * @brief 更新一步（只采样）
   * @param dt 时间步长
   * @return 当前采样后的速度
   */
  double update(double dt);

  /**
   * @brief 计算单次速度切换剖面
   */
  SCurveVelocityProfile calculateProfile(double start_velocity,
                                         double end_velocity,
                                         double start_acceleration = 0.0) const;

  /**
   * @brief 给定剖面下，获取时刻 t 的速度
   */
  double getVelocityAtTime(const SCurveVelocityProfile& profile, double t) const;

  /**
   * @brief 给定剖面下，获取时刻 t 的加速度
   */
  double getAccelerationAtTime(const SCurveVelocityProfile& profile, double t) const;

  /**
   * @brief 给定剖面下，获取时刻 t 的 jerk
   */
  double getJerkAtTime(const SCurveVelocityProfile& profile, double t) const;

  /**
   * @brief 按固定时间间隔生成轨迹点
   */
  void generateTrajectory(const SCurveVelocityProfile& profile, double dt,
                          std::vector<double>& velocities,
                          std::vector<double>& accelerations,
                          std::vector<double>& jerks) const;

  const SCurveVelocityPlannerState& getState() const { return state_; }
  const SCurveVelocityProfile& getActiveProfile() const { return active_profile_; }

  double getVelocity() const { return state_.velocity; }
  double getAcceleration() const { return state_.acceleration; }
  double getJerk() const { return state_.jerk; }
  double getTargetVelocity() const { return state_.target_velocity; }
  bool hasActiveProfile() const { return state_.has_active_profile; }
  bool isMoving() const { return state_.is_moving; }

private:
  double max_velocity_;
  double max_acceleration_;
  double max_jerk_;

  double target_replan_threshold_;
  double synchronize_replan_threshold_;

  SCurveVelocityPlannerState state_;
  SCurveVelocityProfile active_profile_;

  static constexpr double EPSILON = 1e-9;
  static constexpr double VELOCITY_THRESHOLD = 1e-6;
  static constexpr double ACCELERATION_THRESHOLD = 1e-6;
  static constexpr double DEFAULT_TARGET_REPLAN_THRESHOLD = 1e-9;
  static constexpr double DEFAULT_SYNCHRONIZE_REPLAN_THRESHOLD = 5e-2;

private:
  void updateIsMovingFlag();

  /**
   * @brief 判断 profile 是否满足速度上限
   */
  bool isVelocityLimitSatisfied(const SCurveVelocityProfile& profile) const;

  /**
   * @brief 计算完整剖面终点状态
   */
  void integrateWholeProfile(const SCurveVelocityProfile& profile,
                             double& v_end,
                             double& a_end) const;

  static double clamp(double value, double min_val, double max_val)
  {
    return std::max(min_val, std::min(max_val, value));
  }

  static double sign(double value)
  {
    if (value > EPSILON) return 1.0;
    if (value < -EPSILON) return -1.0;
    return 0.0;
  }
};

} // namespace cmvr
