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

#include "planner/s_curve_planner/include/s_curve_position_planner.h"

#include <algorithm>
#include <cmath>

namespace cmvr
{

SCurvePositionPlanner1D::SCurvePositionPlanner1D(double max_velocity,
                                                 double max_acceleration,
                                                 double max_jerk)
  : max_velocity_(std::abs(max_velocity))
  , max_acceleration_(std::abs(max_acceleration))
  , max_jerk_(std::abs(max_jerk))
  , velocity_planner_(max_velocity_, max_acceleration_, max_jerk_)
  , position_gain_(1.0)
{
  velocity_planner_.setTargetReplanThreshold(std::max(1e-3, 0.02 * max_velocity_));
  reset();
}

void SCurvePositionPlanner1D::setConstraints(double max_velocity,
                                             double max_acceleration,
                                             double max_jerk)
{
  max_velocity_ = std::abs(max_velocity);
  max_acceleration_ = std::abs(max_acceleration);
  max_jerk_ = std::abs(max_jerk);
  velocity_planner_.setConstraints(max_velocity_, max_acceleration_, max_jerk_);
  velocity_planner_.setTargetReplanThreshold(std::max(1e-3, 0.02 * max_velocity_));
}

void SCurvePositionPlanner1D::getConstraints(double& max_velocity,
                                             double& max_acceleration,
                                             double& max_jerk) const
{
  max_velocity = max_velocity_;
  max_acceleration = max_acceleration_;
  max_jerk = max_jerk_;
}

void SCurvePositionPlanner1D::setPositionGain(double position_gain)
{
  position_gain_ = std::max(0.0, position_gain);
}

void SCurvePositionPlanner1D::initialize(double position, double velocity, double acceleration)
{
  state_.position = position;
  velocity_planner_.initialize(velocity, acceleration);
  state_.velocity = velocity_planner_.getVelocity();
  state_.acceleration = velocity_planner_.getAcceleration();
  state_.jerk = 0.0;
  state_.target_position = position;
  state_.is_moving = false;
}

void SCurvePositionPlanner1D::reset()
{
  state_ = SCurvePositionPlannerState();
  velocity_planner_.reset();
}

void SCurvePositionPlanner1D::setTarget(double target_position)
{
  state_.target_position = target_position;
}

double SCurvePositionPlanner1D::update(double dt)
{
  if (dt <= 0.0 || dt > 0.1) {
    return state_.position;
  }

  const double position_error = state_.target_position - state_.position;
  if (std::abs(position_error) < POSITION_THRESHOLD &&
      std::abs(velocity_planner_.getVelocity()) < VELOCITY_THRESHOLD &&
      std::abs(velocity_planner_.getAcceleration()) < ACCELERATION_THRESHOLD) {
    velocity_planner_.overwriteState(0.0, 0.0, false);
    state_.position = state_.target_position;
    state_.velocity = 0.0;
    state_.acceleration = 0.0;
    state_.jerk = 0.0;
    state_.is_moving = false;
    return state_.position;
  }

  velocity_planner_.setTargetVelocity(computeDesiredVelocity(position_error));

  const double old_velocity = velocity_planner_.getVelocity();
  const double new_velocity = velocity_planner_.update(dt);

  state_.velocity = new_velocity;
  state_.acceleration = velocity_planner_.getAcceleration();
  state_.jerk = velocity_planner_.getJerk();
  state_.position += 0.5 * (old_velocity + new_velocity) * dt;

  const double updated_error = state_.target_position - state_.position;
  if (std::abs(updated_error) < POSITION_THRESHOLD &&
      std::abs(state_.velocity) < VELOCITY_THRESHOLD &&
      std::abs(state_.acceleration) < ACCELERATION_THRESHOLD) {
    velocity_planner_.overwriteState(0.0, 0.0, false);
    state_.position = state_.target_position;
    state_.velocity = 0.0;
    state_.acceleration = 0.0;
    state_.jerk = 0.0;
    state_.is_moving = false;
    return state_.position;
  }

  state_.is_moving = velocity_planner_.isMoving() ||
                     std::abs(updated_error) > POSITION_THRESHOLD;
  return state_.position;
}

double SCurvePositionPlanner1D::computeDesiredVelocity(double position_error) const
{
  if (std::abs(position_error) <= POSITION_THRESHOLD) {
    return 0.0;
  }

  const double proportional_velocity =
      clamp(position_gain_ * position_error, -max_velocity_, max_velocity_);

  return proportional_velocity;
  const double current_velocity = velocity_planner_.getVelocity();
  const double current_acceleration = velocity_planner_.getAcceleration();
  const double stopping_distance =
      computePlannerStoppingDistance(current_velocity, current_acceleration);

  if (std::abs(current_velocity) > VELOCITY_THRESHOLD &&
      sign(current_velocity) != sign(position_error)) {
    return 0.0;
  }

  if (std::abs(current_velocity) > VELOCITY_THRESHOLD &&
      sign(current_velocity) == sign(position_error) &&
      std::abs(position_error) <= std::abs(stopping_distance) * 1.05) {
    return 0.0;
  }

  double low = 0.0;
  double high = max_velocity_;
  for (int i = 0; i < 40; ++i) {
    const double mid = 0.5 * (low + high);
    const double distance =
        std::abs(computePlannerStoppingDistance(sign(position_error) * mid, 0.0));
    if (distance <= std::abs(position_error)) {
      low = mid;
    } else {
      high = mid;
    }
  }

  return sign(position_error) *
         std::min(std::abs(proportional_velocity), low);
}

double SCurvePositionPlanner1D::computePlannerStoppingDistance(double velocity,
                                                               double acceleration) const
{
  if (std::abs(velocity) <= VELOCITY_THRESHOLD &&
      std::abs(acceleration) <= ACCELERATION_THRESHOLD) {
    return 0.0;
  }

  const auto profile = velocity_planner_.calculateProfile(velocity, 0.0, acceleration);
  if (!profile.valid || profile.total_time <= EPSILON) {
    return 0.0;
  }

  const int steps = std::max(16, static_cast<int>(std::ceil(profile.total_time / 0.002)));
  const double dt = profile.total_time / static_cast<double>(steps);

  double distance = 0.0;
  double previous_time = 0.0;
  double previous_velocity = velocity_planner_.getVelocityAtTime(profile, 0.0);
  for (int i = 1; i <= steps; ++i) {
    const double t = (i == steps) ? profile.total_time : i * dt;
    const double current_velocity = velocity_planner_.getVelocityAtTime(profile, t);
    distance += 0.5 * (previous_velocity + current_velocity) * (t - previous_time);
    previous_time = t;
    previous_velocity = current_velocity;
  }

  return distance;
}

}  // namespace cmvr
