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

#pragma once

#include "s_curve_velocity_planner.h"

#include <algorithm>
#include <cmath>

namespace cmvr
{

struct SCurvePositionPlannerState
{
  double position;
  double velocity;
  double acceleration;
  double jerk;
  double target_position;
  bool is_moving;

  SCurvePositionPlannerState()
    : position(0.0)
    , velocity(0.0)
    , acceleration(0.0)
    , jerk(0.0)
    , target_position(0.0)
    , is_moving(false)
  {}
};

/**
 * @brief 单轴位置跟踪规划器
 *
 * 结构：
 * - 外环：position_error -> desired_velocity
 * - 内环：SCurveVelocityPlanner1D 负责 jerk-limited 速度规划
 *
 * 这个类用于高频变化目标的位置跟踪，不承担严格点到点 profile 语义。
 */
class SCurvePositionPlanner1D
{
public:
  SCurvePositionPlanner1D(double max_velocity = 3.0,
                          double max_acceleration = 10.0,
                          double max_jerk = 50.0);

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

  void setPositionGain(double position_gain);
  double getPositionGain() const { return position_gain_; }

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

  void setTarget(double target_position);
  double update(double dt);

  const SCurvePositionPlannerState& getState() const { return state_; }
  double getPosition() const { return state_.position; }
  double getVelocity() const { return state_.velocity; }
  double getAcceleration() const { return state_.acceleration; }
  double getJerk() const { return state_.jerk; }
  double getTargetPosition() const { return state_.target_position; }
  bool isMoving() const { return state_.is_moving; }

private:
  double max_velocity_;
  double max_acceleration_;
  double max_jerk_;

  SCurvePositionPlannerState state_;
  SCurveVelocityPlanner1D velocity_planner_;
  double position_gain_;

  static constexpr double EPSILON = 1e-9;
  static constexpr double VELOCITY_THRESHOLD = 1e-6;
  static constexpr double ACCELERATION_THRESHOLD = 1e-6;
  static constexpr double POSITION_THRESHOLD = 1e-7;

  double computeDesiredVelocity(double position_error) const;
  double computePlannerStoppingDistance(double velocity, double acceleration) 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
