#include <mkt_algorithm/diff/pure_pursuit.h>

void mkt_algorithm::diff::PurePursuit::computePurePursuit(
                                                          const score_algorithm::TrajectoryGenerator::Ptr &traj,
                                                          const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
                                                          const robot_nav_2d_msgs::Twist2D &velocity,
                                                          const double &min_approach_linear_velocity,
                                                          const double &journey_plan,
                                                          const double &sign_x,
                                                          const double &lookahead_dist_min,
                                                          const double &lookahead_dist_max,
                                                          const double &lookahead_dist,
                                                          const double &lookahead_time,
                                                          const double &dt,
                                                          robot_nav_2d_msgs::Twist2D &drive_cmd)
{
  if (!traj)
    return;
  const double velocity_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x;
  const double vel_x_reduce = std::min(fabs(velocity_max), 0.3);
  double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
  carrot_dist2 = std::max(carrot_dist2, 0.05);
  double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;

  if (max_lateral_accel_ > 1e-6)
  {
    const double curvature_abs = std::max(fabs(curvature), 1e-6);
    const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs);
    drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit);
  }


  double post_cost = 0.0; 
  double distance_error = 0.0;
  robot_nav_2d_msgs::Twist2D twist = traj->getTwist();
  this->applyConstraints(distance_error, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);


  double d_reduce = std::clamp(journey_plan, lookahead_dist_min, lookahead_dist_max);

  double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
  double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
  double v_min =
      journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity) * cosine_factor_begin_reduce + min_approach_linear_velocity;
  v_min *= sign_x;

  double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
  double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
  double vel_reduce = sign_x > 0
                          ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
                          : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
  drive_cmd.x = (journey_plan) >= d_reduce ? drive_cmd.x : vel_reduce;

  double v_theta = drive_cmd.x * curvature;
  double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);

  carrot_dist2 *= 0.6;
  curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
  v_theta = drive_cmd.x * curvature;

  double limit_acc_theta = fabs(v_theta) > 0.15 ? 1.0 : 1.8;
  double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
  double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
  drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
}

void mkt_algorithm::diff::PurePursuit::applyConstraints(
  const double &dist_error, const double &lookahead_dist,
  const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
  const double &pose_cost, double &linear_vel, const double &sign_x)
{
  double curvature_vel = linear_vel;
  double cost_vel = linear_vel;
  double approach_vel = linear_vel;


  if (use_regulated_linear_velocity_scaling_)
  {
    const double &min_rad = regulated_linear_scaling_min_radius_;
    const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad;
    if (radius < min_rad)
    {
      curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad);
      robot_nav_2d_msgs::Twist2D cmd, result;
      cmd.x = curvature_vel;
      this->moveWithAccLimits(velocity, cmd, result);
      curvature_vel = result.x;
      linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_);
    }
  }

  if (use_cost_regulated_linear_velocity_scaling_ &&
      pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
      pose_cost != static_cast<double>(robot_costmap_2d::FREE_SPACE))
  {
    const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius();
    const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
                                                std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
                                            inscribed_radius;

    if (min_distance_to_obstacle < cost_scaling_dist_)
    {
      cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_;
    }
    robot_nav_2d_msgs::Twist2D cmd, result;
    cmd.x = cost_vel;
    this->moveWithAccLimits(velocity, cmd, result);
    cost_vel = result.x;
    linear_vel = std::min(cost_vel, curvature_vel);
  }
  // ss << linear_vel << " ";
  // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed
  // if the actual lookahead distance is shorter than requested, that means we're at the
  // end of the path. We'll scale linear velocity by error to slow to a smooth stop.
  // This expression is eq. to
  //      (1) holding time to goal, t, constant using the theoretical
  //          lookahead distance and proposed velocity and
  //      (2) using t with the actual lookahead
  //          distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t).

  double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr
                                ? 2.0 * costmap_robot_->getCostmap()->getResolution()
                                : 0.1;
  if (dist_error > dist_error_limit)
  {
    double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0;
    double unbounded_vel = approach_vel * velocity_scaling;
    if (unbounded_vel < min_approach_linear_velocity_)
    {
      approach_vel = min_approach_linear_velocity_;
    }
    else
    {
      approach_vel *= velocity_scaling;
    }

    // Use the lowest velocity between approach and other constraints, if all overlapping
    robot_nav_2d_msgs::Twist2D cmd, result;
    cmd.x = approach_vel;
    this->moveWithAccLimits(velocity, cmd, result);
    approach_vel = result.x;
    linear_vel = std::min(linear_vel, approach_vel);
  }

  // Limit linear velocities to be valid
  double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x);
  double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x);
  double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y);
  double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y);
  double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y);
  double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y);
  double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel);
  linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel);
  linear_vel = sign_x * linear_vel;
}

double mkt_algorithm::diff::PurePursuit::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
  double journey_plan)
{
double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x +
carrot_pose.pose.y * carrot_pose.pose.y);

// Avoid division by zero and handle backward motion
if (carrot_distance < 1e-3)
return journey_plan;

// Project remaining path onto carrot direction
double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle)
return journey_plan * std::max(0.0, alignment);                // Only positive projection
}


double mkt_algorithm::diff::PurePursuit::computeDecelerationFactor(double remaining_distance, double decel_distance)
{
  if (remaining_distance >= decel_distance)
  {
    return 1.0; // Full speed
  }

  // Smooth transition using cosine function
  double ratio = fabs(remaining_distance / decel_distance);
  return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0
}