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


/**
 * @file s_curve_generator.cpp
 * @brief S 曲线轨迹生成器实现
 */

#include "common/curve/include/s_curve.h"
#include <cmath>
#include <algorithm>
#include <stdexcept>

namespace cmvr
{
SCurve::SCurve(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))
{}

void SCurve::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);
}

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

SCurveProfile SCurve::calculateProfile(double start_position, double end_position,
                                                 double start_velocity, double end_velocity)
{
  SCurveProfile profile;
  profile.j_max = max_jerk_;
  profile.a_max = max_acceleration_;
  profile.v_max = max_velocity_;
  profile.p0 = start_position;
  profile.v0 = start_velocity;
  profile.a0 = 0.0;  // Assume starting from zero acceleration
  profile.vf = end_velocity;

  double displacement = end_position - start_position;
  profile.distance = std::abs(displacement);
  profile.direction = (displacement >= 0) ? 1.0 : -1.0;

  if (profile.distance < EPSILON) {
    // 无需运动
    profile.total_time = 0.0;
    return profile;
  }

  const double start_speed = profile.direction * start_velocity;
  const double end_speed = profile.direction * end_velocity;
  if (start_speed < -VELOCITY_THRESHOLD || end_speed < -VELOCITY_THRESHOLD) {
    profile.total_time = 0.0;
    return profile;
  }

  if (std::abs(start_speed) > VELOCITY_THRESHOLD ||
      std::abs(end_speed) > VELOCITY_THRESHOLD) {
    auto calc_transition = [&](double v_from, double v_to,
                               double& t_j, double& t_a,
                               double& duration, double& distance) {
      t_j = 0.0;
      t_a = 0.0;
      duration = 0.0;
      distance = 0.0;

      const double dv = std::abs(v_to - v_from);
      if (dv <= 1e-12 || profile.a_max <= 1e-12 || profile.j_max <= 1e-12) {
        return;
      }

      const double dv_switch = profile.a_max * profile.a_max / profile.j_max;
      if (dv < dv_switch) {
        t_j = std::sqrt(dv / profile.j_max);
        t_a = 0.0;
      } else {
        t_j = profile.a_max / profile.j_max;
        t_a = dv / profile.a_max - t_j;
      }

      duration = 2.0 * t_j + t_a;
      distance = 0.5 * (v_from + v_to) * duration;
    };

    const double v_cap = std::max({profile.v_max, start_speed, end_speed});
    const double v_peak_min = std::max(start_speed, end_speed);

    double acc_tj = 0.0, acc_ta = 0.0, acc_duration = 0.0, acc_distance = 0.0;
    double dec_tj = 0.0, dec_ta = 0.0, dec_duration = 0.0, dec_distance = 0.0;
    calc_transition(start_speed, v_peak_min,
                    acc_tj, acc_ta, acc_duration, acc_distance);
    calc_transition(v_peak_min, end_speed,
                    dec_tj, dec_ta, dec_duration, dec_distance);
    const double d_min = acc_distance + dec_distance;
    if (profile.distance + EPSILON < d_min) {
      profile.total_time = 0.0;
      return profile;
    }

    calc_transition(start_speed, v_cap,
                    acc_tj, acc_ta, acc_duration, acc_distance);
    calc_transition(v_cap, end_speed,
                    dec_tj, dec_ta, dec_duration, dec_distance);
    const double d_cap = acc_distance + dec_distance;

    double v_peak = v_cap;
    double t4 = 0.0;

    if (profile.distance > d_cap + EPSILON) {
      t4 = (profile.distance - d_cap) / std::max(v_peak, EPSILON);
    } else {
      double lo = v_peak_min;
      double hi = v_cap;
      for (int iter = 0; iter < 80; ++iter) {
        const double mid = 0.5 * (lo + hi);
        double mid_acc_tj = 0.0, mid_acc_ta = 0.0, mid_acc_duration = 0.0, mid_acc_distance = 0.0;
        double mid_dec_tj = 0.0, mid_dec_ta = 0.0, mid_dec_duration = 0.0, mid_dec_distance = 0.0;
        calc_transition(start_speed, mid,
                        mid_acc_tj, mid_acc_ta, mid_acc_duration, mid_acc_distance);
        calc_transition(mid, end_speed,
                        mid_dec_tj, mid_dec_ta, mid_dec_duration, mid_dec_distance);
        const double d_mid = mid_acc_distance + mid_dec_distance;
        if (d_mid < profile.distance) {
          lo = mid;
        } else {
          hi = mid;
        }
      }
      v_peak = hi;
      calc_transition(start_speed, v_peak,
                      acc_tj, acc_ta, acc_duration, acc_distance);
      calc_transition(v_peak, end_speed,
                      dec_tj, dec_ta, dec_duration, dec_distance);
    }

    profile.t1 = acc_tj;
    profile.t2 = acc_ta;
    profile.t3 = acc_tj;
    profile.t4 = t4;
    profile.t5 = dec_tj;
    profile.t6 = dec_ta;
    profile.t7 = dec_tj;
    profile.v_cruise = v_peak;
    profile.a_limit = std::max(profile.j_max * profile.t1, profile.j_max * profile.t5);
    profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 +
                         profile.t5 + profile.t6 + profile.t7;
    return profile;
  }

  const double j = profile.j_max;
  const double a = profile.a_max;
  const double v = profile.v_max;
  const double t_j_a = a / j;

  if (v < a * t_j_a) {
    const double t_j_v = std::sqrt(v / j);
    const double s_min = 2.0 * j * t_j_v * t_j_v * t_j_v;
    if (profile.distance > s_min + EPSILON) {
      calculateLongProfile(profile);
    } else {
      calculateShortProfile(profile);
    }
  } else {
    const double t_a_v = v / a - t_j_a;
    const double s_half_v =
      a * (t_j_a * t_j_a + 1.5 * t_j_a * t_a_v + 0.5 * t_a_v * t_a_v);
    const double s_min = 2.0 * s_half_v;
    if (profile.distance > s_min + EPSILON) {
      calculateLongProfile(profile);
    } else {
      calculateShortProfile(profile);
    }
  }

  return profile;
}

void SCurve::calculateShortProfile(SCurveProfile& profile) const
{
  // 短距离：无巡航段（t4 = 0），与 moveL_SCurveLocal 中的 SCurveProfile1D 保持一致。
  const double j = profile.j_max;
  const double a = profile.a_max;
  const double v = profile.v_max;
  const double d = profile.distance;
  const double t_j_a = a / j;

  if (v < a * t_j_a) {
    // 达不到 a_max：极短距离时只有 4 个 jerk 段。
    const double t1 = std::cbrt(d / (2.0 * j));
    profile.t1 = t1;
    profile.t2 = 0.0;
    profile.t3 = t1;
    profile.t4 = 0.0;
    profile.t5 = t1;
    profile.t6 = 0.0;
    profile.t7 = t1;
    profile.v_cruise = j * t1 * t1;
    profile.a_limit = j * t1;
  } else {
    // 能达到 a_max，但达不到 v_max：求无巡航的匀加速持续时间。
    const double D = t_j_a * t_j_a + 4.0 * (d / a);
    double t2 = (-3.0 * t_j_a + std::sqrt(std::max(0.0, D))) * 0.5;

    if (t2 < 0.0) {
      const double t1 = std::cbrt(d / (2.0 * j));
      profile.t1 = t1;
      profile.t2 = 0.0;
      profile.t3 = t1;
      profile.t4 = 0.0;
      profile.t5 = t1;
      profile.t6 = 0.0;
      profile.t7 = t1;
      profile.v_cruise = j * t1 * t1;
      profile.a_limit = j * t1;
    } else {
      profile.t1 = t_j_a;
      profile.t2 = t2;
      profile.t3 = t_j_a;
      profile.t4 = 0.0;
      profile.t5 = t_j_a;
      profile.t6 = t2;
      profile.t7 = t_j_a;
      profile.v_cruise = a * (t2 + t_j_a);
      profile.a_limit = a;
    }
  }

  profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 +
                       profile.t5 + profile.t6 + profile.t7;
}

void SCurve::calculateLongProfile(SCurveProfile& profile) const
{
  // 含巡航段的完整 7 段式 S 曲线；与 SCurveProfile1D 的 full-profile 公式一致。
  const double j = profile.j_max;
  const double a = profile.a_max;
  const double v = profile.v_max;
  const double d = profile.distance;
  const double t_j_a = a / j;

  if (v < a * t_j_a) {
    // 达不到 a_max，但可以达到 v_max 并进入巡航。
    const double t_j_v = std::sqrt(v / j);
    const double s_min = 2.0 * j * t_j_v * t_j_v * t_j_v;
    profile.t1 = t_j_v;
    profile.t2 = 0.0;
    profile.t3 = t_j_v;
    profile.t4 = std::max(0.0, (d - s_min) / v);
    profile.t5 = t_j_v;
    profile.t6 = 0.0;
    profile.t7 = t_j_v;
    profile.v_cruise = v;
    profile.a_limit = j * t_j_v;
  } else {
    // 能达到 a_max 和 v_max。
    const double t_a = v / a - t_j_a;
    const double s_half =
      a * (t_j_a * t_j_a + 1.5 * t_j_a * t_a + 0.5 * t_a * t_a);
    const double s_min = 2.0 * s_half;

    profile.t1 = t_j_a;
    profile.t2 = t_a;
    profile.t3 = t_j_a;
    profile.t4 = std::max(0.0, (d - s_min) / v);
    profile.t5 = t_j_a;
    profile.t6 = t_a;
    profile.t7 = t_j_a;
    profile.v_cruise = v;
    profile.a_limit = a;
  }

  profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 +
                       profile.t5 + profile.t6 + profile.t7;
}

double SCurve::computeSegmentJerk(const SCurveProfile& profile, double t) const
{
  double j = profile.j_max * profile.direction;

  // 判断所处阶段
  double t_end1 = profile.t1;
  double t_end2 = t_end1 + profile.t2;
  double t_end3 = t_end2 + profile.t3;
  double t_end4 = t_end3 + profile.t4;
  double t_end5 = t_end4 + profile.t5;
  double t_end6 = t_end5 + profile.t6;
  // double t_end7 = t_end6 + profile.t7;  // = total_time

  if (t < t_end1) {
    return j;  // 段 1：正加加速度（加速）
  } else if (t < t_end2) {
    return 0.0;  // 段 2：加加速度为 0（匀加速）
  } else if (t < t_end3) {
    return -j;  // 段 3：负加加速度（减小加速度）
  } else if (t < t_end4) {
    return 0.0;  // 段 4：加加速度为 0（巡航）
  } else if (t < t_end5) {
    return -j;  // 段 5：负加加速度（开始减速）
  } else if (t < t_end6) {
    return 0.0;  // 段 6：加加速度为 0（匀减速）
  } else {
    return j;  // 段 7：正加加速度（减速结束）
  }
}

double SCurve::getPositionAtTime(const SCurveProfile& profile, double t) const
{
  if (t <= 0) return profile.p0;
  if (t >= profile.total_time) return profile.p0 + profile.distance * profile.direction;

  double j = profile.j_max * profile.direction;
  double p = profile.p0;
  double v = profile.v0;
  double a = profile.a0;

  // 时间边界
  double t_end1 = profile.t1;
  double t_end2 = t_end1 + profile.t2;
  double t_end3 = t_end2 + profile.t3;
  double t_end4 = t_end3 + profile.t4;
  double t_end5 = t_end4 + profile.t5;
  double t_end6 = t_end5 + profile.t6;

  // 依次处理各阶段
  auto processSegment = [&](double dt, double jerk) {
    p += v * dt + 0.5 * a * dt * dt + (1.0/6.0) * jerk * dt * dt * dt;
    v += a * dt + 0.5 * jerk * dt * dt;
    a += jerk * dt;
  };

  // 段 1
  if (t <= t_end1) {
    processSegment(t, j);
    return p;
  }
  processSegment(profile.t1, j);

  // 段 2
  if (t <= t_end2) {
    processSegment(t - t_end1, 0.0);
    return p;
  }
  processSegment(profile.t2, 0.0);

  // 段 3
  if (t <= t_end3) {
    processSegment(t - t_end2, -j);
    return p;
  }
  processSegment(profile.t3, -j);

  // 段 4（巡航）
  if (t <= t_end4) {
    processSegment(t - t_end3, 0.0);
    return p;
  }
  processSegment(profile.t4, 0.0);

  // 段 5
  if (t <= t_end5) {
    processSegment(t - t_end4, -j);
    return p;
  }
  processSegment(profile.t5, -j);

  // 段 6
  if (t <= t_end6) {
    processSegment(t - t_end5, 0.0);
    return p;
  }
  processSegment(profile.t6, 0.0);

  // 段 7
  processSegment(t - t_end6, j);
  return p;
}

double SCurve::getVelocityAtTime(const SCurveProfile& profile, double t) const
{
  if (t <= 0) return profile.v0;
  if (t >= profile.total_time) return profile.vf;

  double j = profile.j_max * profile.direction;
  double v = profile.v0;
  double a = profile.a0;

  // 时间边界
  double t_end1 = profile.t1;
  double t_end2 = t_end1 + profile.t2;
  double t_end3 = t_end2 + profile.t3;
  double t_end4 = t_end3 + profile.t4;
  double t_end5 = t_end4 + profile.t5;
  double t_end6 = t_end5 + profile.t6;

  auto processSegment = [&](double dt, double jerk) {
    v += a * dt + 0.5 * jerk * dt * dt;
    a += jerk * dt;
  };

  if (t <= t_end1) {
    v += a * t + 0.5 * j * t * t;
    return v;
  }
  processSegment(profile.t1, j);

  if (t <= t_end2) {
    v += a * (t - t_end1);
    return v;
  }
  processSegment(profile.t2, 0.0);

  if (t <= t_end3) {
    double dt = t - t_end2;
    v += a * dt + 0.5 * (-j) * dt * dt;
    return v;
  }
  processSegment(profile.t3, -j);

  if (t <= t_end4) {
    v += a * (t - t_end3);
    return v;
  }
  processSegment(profile.t4, 0.0);

  if (t <= t_end5) {
    double dt = t - t_end4;
    v += a * dt + 0.5 * (-j) * dt * dt;
    return v;
  }
  processSegment(profile.t5, -j);

  if (t <= t_end6) {
    v += a * (t - t_end5);
    return v;
  }
  processSegment(profile.t6, 0.0);

  double dt = t - t_end6;
  v += a * dt + 0.5 * j * dt * dt;
  return v;
}

double SCurve::getAccelerationAtTime(const SCurveProfile& profile, double t) const
{
  if (t <= 0 || t >= profile.total_time) return 0.0;

  double j = profile.j_max * profile.direction;
  double a = profile.a0;

  // 时间边界
  double t_end1 = profile.t1;
  double t_end2 = t_end1 + profile.t2;
  double t_end3 = t_end2 + profile.t3;
  double t_end4 = t_end3 + profile.t4;
  double t_end5 = t_end4 + profile.t5;
  double t_end6 = t_end5 + profile.t6;

  if (t <= t_end1) {
    return a + j * t;
  }
  a += j * profile.t1;

  if (t <= t_end2) {
    return a;
  }

  if (t <= t_end3) {
    return a + (-j) * (t - t_end2);
  }
  a += (-j) * profile.t3;

  if (t <= t_end4) {
    return a;  // Should be ~0
  }

  if (t <= t_end5) {
    return a + (-j) * (t - t_end4);
  }
  a += (-j) * profile.t5;

  if (t <= t_end6) {
    return a;
  }

  return a + j * (t - t_end6);
}

double SCurve::getJerkAtTime(const SCurveProfile& profile, double t) const
{
  if (t <= 0.0 || t >= profile.total_time) {
    return 0.0;
  }
  return computeSegmentJerk(profile, t);
}

void SCurve::generateTrajectory(const SCurveProfile& profile, double dt,
                                          std::vector<double>& positions,
                                          std::vector<double>& velocities,
                                          std::vector<double>& accelerations) const
{
  positions.clear();
  velocities.clear();
  accelerations.clear();

  if (profile.total_time <= 0 || dt <= 0) {
    positions.push_back(profile.p0);
    velocities.push_back(profile.v0);
    accelerations.push_back(profile.a0);
    return;
  }

  int num_points = static_cast<int>(std::ceil(profile.total_time / dt)) + 1;
  positions.reserve(num_points);
  velocities.reserve(num_points);
  accelerations.reserve(num_points);

  for (double t = 0; t <= profile.total_time; t += dt) {
    positions.push_back(getPositionAtTime(profile, t));
    velocities.push_back(getVelocityAtTime(profile, t));
    accelerations.push_back(getAccelerationAtTime(profile, t));
  }

  // 确保包含最终点
  if (positions.empty() ||
      std::abs(positions.back() - (profile.p0 + profile.distance * profile.direction)) > EPSILON) {
    positions.push_back(profile.p0 + profile.distance * profile.direction);
    velocities.push_back(profile.vf);
    accelerations.push_back(0.0);
  }
}

}
