#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
#include <boost/dll/import.hpp>
#include <robot/console.h>

#define LIMIT_VEL_THETA 0.33

void mkt_algorithm::diff::PredictiveTrajectory::initialize(
    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{
  if (!initialized_)
  {
    nh_ = robot::NodeHandle("~");
    nh_priv_ = robot::NodeHandle(nh, name);
    name_ = name;
    tf_ = tf;
    traj_ = traj;
    costmap_robot_ = costmap_robot;
    this->getParams();
    nh_.param("publish_topic", enable_publish_, false);
    nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);

    footprint_spec_ = costmap_robot_->getRobotFootprint();
    std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
    if (footprint.size() > 1)
    {
      double min_length = 1e10;
      double max_length = 0.0;
      for (size_t i = 0; i < footprint.size(); ++i)
      {
        const robot_geometry_msgs::Point &p1 = footprint[i];
        const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
        double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
        if (len > max_length)
        {
          max_length = len;
        }
        if (len < min_length)
        {
          min_length = len;
        }
      }
      this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
      this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
    }
    this->initKalmanFilter();
    x_direction_ = y_direction_ = theta_direction_ = 0;
    this->initialized_ = true;
    robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__);
  }
}

mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory()
{
  if (kf_)
  {
    kf_.reset();
  }
}

void  mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter()
{
  // kalman
  last_actuator_update_ = robot::Time::now();
  n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
  m_ = 2; // measurements: x, y, theta
  double dt = control_duration_;

  // Khởi tạo ma trận
  A = Eigen::MatrixXd::Identity(n_, n_);
  C = Eigen::MatrixXd::Zero(m_, n_);
  Q = Eigen::MatrixXd::Zero(n_, n_);
  R = Eigen::MatrixXd::Identity(m_, m_);
  P = Eigen::MatrixXd::Identity(n_, n_);

  for (int i = 0; i < n_; i += 3)
  {
    A(i, i + 1) = dt;
    A(i, i + 2) = 0.5 * dt * dt;
    A(i + 1, i + 2) = dt;
  }

  C(0, 0) = 1;
  C(1, 3) = 1;
  Q(2, 2) = 0.1;
  Q(5, 5) = 0.6;

  R(0, 0) = 0.1;
  R(1, 1) = 0.2;

  P(3, 3) = 0.4;
  P(4, 4) = 0.4;
  P(5, 5) = 0.4;

  kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
  Eigen::VectorXd x0(n_);
  x0 << 0, 0, 0, 0, 0, 0;
  kf_->init(robot::Time::now().toSec(), x0);
}

void mkt_algorithm::diff::PredictiveTrajectory::getParams()
{
  robot_base_frame_ = nh_priv_.param<std::string>("robot_base_frame", std::string("base_link"));
  nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
  nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
  nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
  nh_priv_.param<int>("index_samples", index_samples_, 0);
  nh_priv_.param<bool>("follow_step_path", follow_step_path_, false);

  nh_priv_.param<bool>("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false);
  nh_priv_.param<double>("lookahead_dist", lookahead_dist_, 0.25);
  nh_priv_.param<double>("min_lookahead_dist", min_lookahead_dist_, 0.3);
  nh_priv_.param<double>("max_lookahead_dist", max_lookahead_dist_, 0.9);
  nh_priv_.param<double>("lookahead_time", lookahead_time_, 1.5);
  nh_priv_.param<double>("min_journey_squared", min_journey_squared_, std::numeric_limits<double>::infinity());
  nh_priv_.param<double>("max_journey_squared", max_journey_squared_, std::numeric_limits<double>::infinity());

  nh_priv_.param<bool>("use_rotate_to_heading", use_rotate_to_heading_, false);
  nh_priv_.param<double>("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785);

  nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.0);
  nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.0);
  if (trans_stopped_velocity_ <= min_approach_linear_velocity_ || trans_stopped_velocity_ - min_approach_linear_velocity_ < 0.02)
    trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.05;

  // Regulated linear velocity scaling
  nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
  nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
  nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);

  // Inflation cost scaling (Limit velocity by proximity to obstacles)
  nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
  nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
  nh_priv_.param<double>("cost_scaling_dist", cost_scaling_dist_, 0.6);
  nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
  if (inflation_cost_scaling_factor_ <= 0.0)
  {
    robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, "
             "it should be >0. Disabling cost regulated linear velocity scaling.");
    use_cost_regulated_linear_velocity_scaling_ = false;
  }
  double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
  control_duration_ = 1.0 / control_frequency;
  window_size_ = (size_t)(control_frequency * 3.0);

  if (traj_)
  {
    traj_.get()->getNodeHandle().param<double>("max_vel_x", max_vel_x_, 0.0);
    traj_.get()->getNodeHandle().param<double>("min_vel_x", min_vel_x_, 0.0);
    traj_.get()->getNodeHandle().param<double>("acc_lim_x", acc_lim_x_, 0.0);
    traj_.get()->getNodeHandle().param<double>("decel_lim_x", decel_lim_x_, 0.0);

    traj_.get()->getNodeHandle().param<double>("max_vel_y", max_vel_y_, 0.0);
    traj_.get()->getNodeHandle().param<double>("min_vel_y", min_vel_y_, 0.0);
    traj_.get()->getNodeHandle().param<double>("acc_lim_y", acc_lim_y_, 0.0);
    traj_.get()->getNodeHandle().param<double>("decel_lim_y", decel_lim_y_, 0.0);

    traj_.get()->getNodeHandle().param<double>("max_vel_theta", max_vel_theta_, 0.0);
    traj_.get()->getNodeHandle().param<double>("min_vel_theta", min_vel_theta_, 0.0);
    traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
    traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
  }
}

void mkt_algorithm::diff::PredictiveTrajectory::reset()
{
  if (this->initialized_)
  {
    robot::log_info("[%s:%d]\n PredictiveTrajectory is reset", __FILE__, __LINE__);
    reached_intermediate_goals_.clear();
    start_index_saved_vt_.clear();
    this->clear();
    x_direction_ = y_direction_ = theta_direction_ = 0;
    this->follow_step_path_ = false;
    this->nav_stop_ = false;
    last_actuator_update_ = robot::Time::now();
    prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D();

    if (kf_)
    {
      Eigen::VectorXd x0(n_);
      x0 << 0, 0, 0, 0, 0, 0;
      kf_->init(robot::Time::now().toSec(), x0);
    }
  }
}

void mkt_algorithm::diff::PredictiveTrajectory::stop()
{
  this->nav_stop_ = true;
}

void mkt_algorithm::diff::PredictiveTrajectory::resume()
{
  if (this->initialized_)
  {
    if(!this->nav_stop_)
      return;
    prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D();
    last_actuator_update_ = robot::Time::now();

    if (kf_)
    {
      Eigen::VectorXd x0(n_);
      x0 << 0, 0, 0, 0, 0, 0;
      kf_->init(robot::Time::now().toSec(), x0);
    }
    this->nav_stop_ = false;
  }
}

bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity,
                                                        const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
                                                        double &x_direction, double &y_direction, double &theta_direction)
{
  if (!initialized_)
  {
    robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__);
    return false;
  }

  std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
  if (footprint.size() > 1)
  {
    double min_length = 1e10;
    double max_length = 0.0;
    for (size_t i = 0; i < footprint.size(); ++i)
    {
      const robot_geometry_msgs::Point &p1 = footprint[i];
      const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
      double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
      if (len > max_length)
      {
        max_length = len;
      }
      if (len < min_length)
      {
        min_length = len;
      }
    }
    this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
    this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
  }

  if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
  {
    robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
    return false;
  }
  this->getParams();

  frame_id_path_ = global_plan.header.frame_id;
  goal_ = goal;
  global_plan_ = global_plan;

  // prune global plan to cut off parts of the past (spatially before the robot)
  if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0))
  {
    robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__);
    return false;
  }

  double S = std::numeric_limits<double>::infinity();
  S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0,
               costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0);
  const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_;
  S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S);
  compute_plan_.poses.clear();

  if ((unsigned int)global_plan_.poses.size() == 2)
  {
    double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x;
    double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y;
    if (hypot(dx, dy) <= 2.0 * costmap_robot_->getCostmap()->getResolution())
    {
      compute_plan_ = global_plan_;
    }
    else
    {
      robot::log_error("[%s:%d]\n The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution());
      return false;
    }
  }
  else
  {
    unsigned int start_index, goal_index;
    if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_))
    {
      robot::log_error("[%s:%d]\n computePlanCommand is Failed", __FILE__, __LINE__);
      return false;
    }
  }

  double lookahead_dist = this->getLookAheadDistance(velocity);
  transform_plan_.poses.clear();
  if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_))
  {
    robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
    return false;
  }
  // else
  // {
  //   robot::log_info("[%s:%d]\n Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_);
  // }

  x_direction = x_direction_;
  y_direction = y_direction_ = 0;
  theta_direction = theta_direction_;

  if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq)
  {
    const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
    int index;
    for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--)
    {
      robot_geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose;
      if (robot_nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_)
        break;
    }
    const robot_geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose;
    if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
    {
      const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
      const double dir_path = cos(fabs(path_angle - p2.theta));
      if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
        x_direction = dir_path > 0 ? FORWARD : BACKWARD;
    }
  }
  else
  {
    try
    {
      auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
      auto prev_carrot_pose_it = transform_plan_.poses.begin();
      double distance_it = 0;
      for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
      {
        double dx = it->pose.x - carrot_pose_it->pose.x;
        double dy = it->pose.x - carrot_pose_it->pose.y;
        distance_it += std::hypot(dx, dy);
        if (distance_it > costmap_robot_->getCostmap()->getResolution())
        {
          prev_carrot_pose_it = it;
          break;
        }
      }

      robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
                                      ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
                                      : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());

      robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);

      // teb_local_planner::PoseSE2 start_pose(front);
      // teb_local_planner::PoseSE2 goal_pose(back);
      // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
      const double dir_path = 0.0;

      if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
        x_direction = dir_path > 0 ? FORWARD : BACKWARD;
    }
    catch (std::exception &e)
    {
      robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
      return false;
    }
  }

  x_direction_ = x_direction;
  y_direction_ = y_direction;
  theta_direction_ = theta_direction;
  return true;
}

mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
    const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
  mkt_msgs::Trajectory2D result;
  result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
  if (!traj_)
    return result;
  if (compute_plan_.poses.size() < 2)
  {
    robot::log_warning("[%s:%d]\n Local compute plan is available", __FILE__, __LINE__);
    return result;
  }

  robot::Time current_time = robot::Time::now();
  double dt = (current_time - last_actuator_update_).toSec();
  last_actuator_update_ = current_time;

  robot_nav_2d_msgs::Twist2D drive_cmd;
  double sign_x = sign(x_direction_);
  robot_nav_2d_msgs::Twist2D twist;
  traj_->startNewIteration(velocity);
  while (traj_->hasMoreTwists())
  {
    twist = traj_->nextTwist();
  }
  drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
  double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;

  robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
  if (transformed_plan.poses.empty())
  {
    robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
    return result;
  }

  double lookahead_dist = getLookAheadDistance(velocity);
  double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y);

  if (transformed_plan.poses.empty())
  {
    robot::log_warning("[%s:%d]\n Transformed plan is empty after compute lookahead point", __FILE__, __LINE__);
    return result;
  }
  auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
  bool allow_rotate = false;
  nh_priv_.param("allow_rotate", allow_rotate, false);

  // double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y);
  robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
  const double distance_allow_rotate = min_journey_squared_;
  const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y);
  const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
  if (avoid_obstacles_)
    allow_rotate = journey_plan >= distance_allow_rotate &&
                   fabs(front.y) <= 0.2 &&
                   (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution());
  else
    allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;

  double angle_to_heading;
  if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
  {
    if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_))
    {
      if (!stopWithAccLimits(pose, velocity, drive_cmd))
        return result;
    }
    else
    {
      rotateToHeading(angle_to_heading, velocity, drive_cmd);
    }
  }
  else
  {
    const double vel_x_reduce = std::min(fabs(v_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;

    const auto &plan_back_pose = compute_plan_.poses.back();
    double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
    post_cost = std::max(post_cost, center_cost_);
    this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);

    const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
    const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
    double d_reduce = std::clamp(journey_plan, min_S, max_S);
    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 + max_path_distance_) >= 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);
    if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
    {
      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;
    }
    if (fabs(v_theta) > LIMIT_VEL_THETA)
    {
      robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result;
      cmd_vel.x = sign_x > 0
                      ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1))
                      : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1));
      cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5);
      this->moveWithAccLimits(velocity, cmd_vel, cmd_result);
      drive_cmd.x = std::copysign(cmd_result.x, sign_x);
      v_theta = drive_cmd.x * curvature;
    }

    if (journey_plan < min_journey_squared_)
    {
      if (transform_plan_.poses.size() > 2)
      {
        robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
        robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
        double dx = end.pose.x - start.pose.x;
        double dy = end.pose.y - start.pose.y;
        v_theta = atan2(dy, dx);
        if (v_theta > M_PI_2)
          v_theta -= M_PI;
        else if (v_theta < -M_PI_2)
          v_theta += M_PI;
        // v_theta *= 0.5;
        v_theta = std::clamp(v_theta, -0.02, 0.02);
      }
      else
        v_theta = 0.0;
    }
    double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 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);

      if (this->nav_stop_)
      {
        if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
        {
          if (!stopWithAccLimits(pose, velocity, drive_cmd))
            return result;
        }
        else
          drive_cmd = {};
        result.velocity = drive_cmd;
        return result;
      }
  
    Eigen::VectorXd y(2);
    y << drive_cmd.x, drive_cmd.theta;

    // Cập nhật lại A nếu dt thay đổi
    for (int i = 0; i < n_; ++i)
      for (int j = 0; j < n_; ++j)
        A(i, j) = (i == j ? 1.0 : 0.0);
    for (int i = 0; i < n_; i += 3)
    {
      A(i, i + 1) = dt;
      A(i, i + 2) = 0.5 * dt * dt;
      A(i + 1, i + 2) = dt;
    }
    kf_->update(y, dt, A);

    // Check if Kalman filter's estimated velocity exceeds v_max
    if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
                             cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
                             cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
                             cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE))
    {
      drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0];
    }
    else
    {
      drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
    }

    drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x);
    drive_cmd.theta = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA);
  }

  result.velocity = drive_cmd;
  prevous_drive_cmd_ = drive_cmd;
  return result;
}

bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
    const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x)
{
  bool curvature = false;
  double path_angle = 0;
  if (!global_plan.poses.empty())
  {
    if ((unsigned)global_plan.poses.size() > 2)
    {
      const double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
      const double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
      if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
      {
        double start_angle = atan2(start_direction_y, start_direction_x);
        for (unsigned int i = 1; i < (unsigned)global_plan.poses.size(); i++)
        {
          const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
          const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
          if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
          {
            double current_angle = atan2(current_direction_y, current_direction_x);
            if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > 1e-9)
            {
              curvature = true;
              break;
            }
            path_angle = current_angle;
          }
        }
      }
    }
  }
  // ROS_INFO_THROTTLE(0.1,"path_angle %.3f %.3f", path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x));
  // Whether we should rotate robot to rough path heading
  angle_to_path = curvature ? path_angle : std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
  angle_to_path = sign_x < 0
                      ? angles::normalize_angle(M_PI + angle_to_path)
                      : angles::normalize_angle(angle_to_path);

  double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
  // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
  double heading_rotate = rotate_to_heading_min_angle_;
  bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_);
  heading_rotate = is_stopped
                       ? rotate_to_heading_min_angle_ + heading_linear
                       : std::clamp(heading_rotate + heading_linear, 0.3, M_PI_2);

  bool result = use_rotate_to_heading_ && (is_stopped || sign(angle_to_path) * sign_x < 0) && fabs(angle_to_path) > heading_rotate;

  // if (result)
  // {
  //   ROS_WARN_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f",
  //                     velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_);
  //   ROS_WARN_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n",
  //                     is_stopped, path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x), angle_to_path, heading_rotate, sign_x);
  // }
  // else
  // {
  //   ROS_INFO_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f",
  //                     velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_);
  //   ROS_INFO_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n",
  //                     is_stopped, path_angle,std::atan2(carrot_pose.pose.y, carrot_pose.pose.x) ,angle_to_path, heading_rotate, sign_x);
  // }

  return result;
}

void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel)
{
  const double dt = control_duration_;
  const double theta_direction = angle_to_path > 0.0 ? 1.0 : -1.0;
  double vel_yaw = theta_direction * fabs(velocity.theta);
  double ang_diff = angle_to_path;
  double max_vel_theta = traj_ ? fabs(traj_->getTwistAngular(true).z) : 0.3;

  double angular_decel_zone = 0.3; // rad - khoảng cách bắt đầu giảm tốc
  nh_priv_.param("angular_decel_zone", angular_decel_zone, 0.5);
  angular_decel_zone += std::clamp(
      fabs(max_vel_theta) + (M_PI_2 - fabs(atan2(min_path_distance_, max_path_distance_))), 0.4, M_PI_2);
  double zone_begin = std::max(angular_decel_zone * 0.2, 0.2);
  double cosine_begin_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / zone_begin)));

  double reduce_vel_theta = std::min(max_vel_theta, 0.25 + min_vel_theta_);
  reduce_vel_theta = fabs(ang_diff) > zone_begin ? reduce_vel_theta : (reduce_vel_theta - min_vel_theta_) * cosine_begin_factor + min_vel_theta_;

  // === ANGULAR DECELERATION ZONE ===
  double target_angular_speed = max_vel_theta;
  if (fabs(ang_diff) < angular_decel_zone)
  {
    // ROS_WARN_THROTTLE(0.2,"%f %f %f %f", ang_diff, angular_decel_zone, zone_begin, max_vel_theta);
    double cosine_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / angular_decel_zone)));
    double cosine_speed = max_vel_theta * cosine_factor;

    // Choose deceleration profile
    target_angular_speed = cosine_speed; // Smoothest option

    // Ensure minimum speed to avoid stalling
    target_angular_speed = std::max(target_angular_speed, reduce_vel_theta);
  }
  // else
  //   ROS_INFO_THROTTLE(1,"%f %f %f %f", ang_diff, angular_decel_zone, max_vel_theta, reduce_vel_theta);

  // Apply direction
  double v_theta_desired = std::copysign(target_angular_speed, ang_diff);

  // === ACCELERATION LIMITS ===
  double max_acc_vel = vel_yaw + fabs(acc_lim_theta_) * dt;
  double min_acc_vel = vel_yaw - fabs(acc_lim_theta_) * dt;
  double v_theta_samp = std::clamp(v_theta_desired, min_acc_vel, max_acc_vel);

  // === STOPPING CONSTRAINT ===
  double max_speed_to_stop = sqrt(2.0 * fabs(decel_lim_theta_) * fabs(ang_diff));
  v_theta_samp = std::copysign(
      std::min(max_speed_to_stop, fabs(v_theta_samp)), ang_diff);

  // === FINAL LIMITS ===
  v_theta_samp = theta_direction > 0
                     ? std::clamp(v_theta_samp, min_vel_theta_, max_vel_theta)
                     : std::clamp(v_theta_samp, -max_vel_theta, -min_vel_theta_);

  cmd_vel.x = 0.0;
  cmd_vel.y = 0.0;
  cmd_vel.theta = v_theta_samp;
}

double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity)
{
  // If using velocity-scaled look ahead distances, find and clamp the dist
  // Else, use the static look ahead distance
  double lookahead_dist = lookahead_dist_;
  if (use_velocity_scaled_lookahead_dist_)
  {
    lookahead_dist = fabs(velocity.x) * lookahead_time_;
    lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_);
  }

  return lookahead_dist;
}

std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan)
{
  if (global_plan.poses.empty())
  {
    throw nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
  }

  if ((unsigned)global_plan.poses.size() < 2)
  {
    auto goal_pose_it = std::prev(global_plan.poses.end());
    return goal_pose_it;
  }
  unsigned int goal_index = (unsigned)global_plan.poses.size() - 1;

  double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
  double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
  double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x;
  double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y;

  // make sure that atan2 is defined
  double start_angle = atan2(start_direction_y, start_direction_x);
  double end_angle = atan2(end_direction_y, end_direction_x);
  double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);

  for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
  {
    if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
    {
      const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
      const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;

      if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
      {
        double current_angle = atan2(current_direction_y, current_direction_x);
        goal_index = i;
        if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
          continue;

        if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
        {
          goal_index = i;
          break;
        }
      }
    }
  }

  // Find the first pose which is at a distance greater than the lookahead distance
  auto goal_pose_it = std::find_if(
      global_plan.poses.begin(), global_plan.poses.begin() + goal_index,
      [&](const auto &ps)
      {
        return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist;
      });

  // If the number of poses is not far enough, take the last pose
  if (goal_pose_it == global_plan.poses.end())
  {
    goal_pose_it = std::prev(global_plan.poses.end());
  }
  return goal_pose_it;
}

robot_geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose)
{
  robot_geometry_msgs::PointStamped carrot_msg;
  carrot_msg.header = carrot_pose.header;
  carrot_msg.point.x = carrot_pose.pose.x;
  carrot_msg.point.y = carrot_pose.pose.y;
  carrot_msg.point.z = 0.5; // publish right over map to stand out
  return carrot_msg;
}

bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot)
{
  if (global_plan.poses.empty())
    return false;
  if (tf == nullptr)
    return false;
  try
  {
    // let's get the pose of the robot in the frame of the plan
    robot_nav_2d_msgs::Pose2DStamped robot;
    if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot))
    {
      throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
    }

    double dist_thresh_sq = dist_behind_robot * dist_behind_robot;

    // iterate plan until a pose close the robot is found
    std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator it = global_plan.poses.begin();
    std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator erase_end = it;
    while (it != global_plan.poses.end())
    {
      double dx = robot.pose.x - it->pose.x;
      double dy = robot.pose.y - it->pose.y;
      double dist_sq = dx * dx + dy * dy;
      if (dist_sq < dist_thresh_sq)
      {
        erase_end = it;
        break;
      }
      ++it;
    }
    if (erase_end == global_plan.poses.end())
      return false;

    if (erase_end != global_plan.poses.begin())
      global_plan.poses.erase(global_plan.poses.begin(), erase_end);
  }
  catch (const tf3::TransformException &ex)
  {
    robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what());
    return false;
  }
  return true;
}

bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
    TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
    const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
    robot_nav_2d_msgs::Path2D &transformed_plan)
{
  // this method is a slightly modified version of base_local_planner/goal_functions.h
  transformed_plan.poses.clear();
  transformed_plan.header.stamp = pose.header.stamp;
  transformed_plan.header.frame_id = robot_base_frame;

  if (global_plan.poses.empty())
    return false;

  if (tf == nullptr)
    return false;

  if (costmap == nullptr || costmap->getCostmap() == nullptr)
    return false;

  try
  {
    if (global_plan.poses.empty())
    {
      robot::log_error("[%s:%d]\n Received plan with zero length", __FILE__, __LINE__);
      return false;
    }

    // let's get the pose of the robot in the frame of the plan
    robot_nav_2d_msgs::Pose2DStamped robot_pose;
    if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
    {
      throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
    }

    // we'll discard points on the plan that are outside the local costmap
    double dist_threshold = std::max(costmap->getCostmap()->getSizeInCellsX() * costmap->getCostmap()->getResolution() / 2.0,
                                     costmap->getCostmap()->getSizeInCellsY() * costmap->getCostmap()->getResolution() / 2.0);

    dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are
    double sq_dist_threshold = dist_threshold * dist_threshold;
    // located on the border of the local costmap

    int i = 0;
    double sq_dist = std::numeric_limits<double>::infinity();

    // we need to loop to a point on the plan that is within a certain distance of the robot
    bool robot_reached = false;
    for (int j = 0; j < (int)global_plan.poses.size(); ++j)
    {
      double x_diff = robot_pose.pose.x - global_plan.poses[j].pose.x;
      double y_diff = robot_pose.pose.y - global_plan.poses[j].pose.y;
      double new_sq_dist = x_diff * x_diff + y_diff * y_diff;
      if (robot_reached && new_sq_dist > sq_dist_threshold)
      {
        break;
      } // force stop if we have reached the costmap border

      if (new_sq_dist < sq_dist) // find closest distance
      {
        sq_dist = new_sq_dist;
        i = j;
        if (sq_dist < costmap->getCostmap()->getResolution()) // 5 cm to the robot; take the immediate local minima; if it's not the global
          robot_reached = true;                               // minima, probably means that there's a loop in the path, and so we prefer this
      }
    }

    double plan_length = 0; // check cumulative Euclidean distance along the plan
    double x_direction_saved = 0.0;
    // now we'll transform until points are outside of our distance threshold
    while (i < (int)global_plan.poses.size() && sq_dist <= sq_dist_threshold &&
           (max_plan_length <= 0 || plan_length <= max_plan_length))
    {
      robot_nav_2d_msgs::Pose2DStamped stamped_pose;
      stamped_pose.pose = global_plan.poses[i].pose;
      stamped_pose.header.frame_id = global_plan.header.frame_id;

      robot_nav_2d_msgs::Pose2DStamped newer_pose;
      if (!robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, newer_pose))
      {
        ++i;
        continue;
      }
      transformed_plan.poses.push_back(newer_pose);

      double x_diff = robot_pose.pose.x - global_plan.poses[i].pose.x;
      double y_diff = robot_pose.pose.y - global_plan.poses[i].pose.y;
      sq_dist = x_diff * x_diff + y_diff * y_diff;

      // Calculate distance to previous pose
      if (i > 0 && max_plan_length > 0)
      {
        robot_geometry_msgs::Pose2D p1 = global_plan.poses[i].pose;
        robot_geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose;
        plan_length += std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2));
        if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
        {
          const double egde_angle = atan2(p1.y - p2.y, p1.x - p2.x);
          const double target_direction = cos(fabs(egde_angle - p1.theta));
          if (target_direction * x_direction_saved < 0.0)
          {
            plan_length = 0;
          }
          x_direction_saved = target_direction;
        }
      }
      ++i;
    }
  }
  catch (tf3::LookupException &ex)
  {
    robot::log_error("[%s:%d]\n No Transform available Error: %s", __FILE__, __LINE__, ex.what());
    return false;
  }
  catch (tf3::ConnectivityException &ex)
  {
    robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what());
    return false;
  }
  catch (tf3::ExtrapolationException &ex)
  {
    robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what());
    if (global_plan.poses.size() > 0)
      robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());

    return false;
  }
  return true;  
}

void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits(
    const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result)
{
  const double dt = control_duration_;

  // --- X axis ---
  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 vx = cmd_vel.x > 0.0 ? std::min(max_vel_x, std::max(min_vel_x, cmd_vel.x)) : std::max(-max_vel_x, std::min(-min_vel_x, cmd_vel.x));

  double max_acc_vx = velocity.x + fabs(acc_lim_x_) * dt;
  double min_acc_vx = velocity.x - fabs(decel_lim_x_) * dt;
  vx = std::clamp(vx, min_acc_vx, max_acc_vx);

  // --- Y axis ---
  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 vy = cmd_vel.y > 0.0 ? std::min(max_vel_y, std::max(min_vel_y, cmd_vel.y)) : std::max(-max_vel_y, std::min(-min_vel_y, cmd_vel.y));

  double max_acc_vy = velocity.y + fabs(acc_lim_y_) * dt;
  double min_acc_vy = velocity.y - fabs(decel_lim_y_) * dt;
  vy = std::clamp(vy, min_acc_vy, max_acc_vy);
  // --- Assign result ---
  cmd_vel_result.x = vx;
  cmd_vel_result.y = vy;
  // cmd_vel_result.theta = vth;
}

bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel)
{
  // slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible
  // but we'll use a tenth of a second to be consistent with the implementation of the local planner.
  const double dt = control_duration_;
  double vx = sign(velocity.x) * std::max(0.0, (fabs(velocity.x) - fabs(decel_lim_x_) * dt));
  double vy = sign(velocity.y) * std::max(0.0, (fabs(velocity.y) - fabs(decel_lim_y_) * dt));

  double vel_yaw = velocity.theta;
  double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt));

  // we do want to check whether or not the command is valid
  cmd_vel.x = vx;
  cmd_vel.y = vy;
  cmd_vel.theta = vth;
  return true;
}

void mkt_algorithm::diff::PredictiveTrajectory::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, double &sign_x)
{
  double curvature_vel = linear_vel;
  double cost_vel = linear_vel;
  double approach_vel = linear_vel;

  // std::stringstream ss;
  // ss << linear_vel << " ";
  // limit the linear velocity by curvature
  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_);
    }
  }
  // ss << linear_vel << " ";
  // limit the linear velocity by proximity to obstacles
  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);
  }

  // ss << linear_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;
  // ss << linear_vel << " ";
  // ROS_INFO_STREAM_THROTTLE(0.1, ss.str());
}

std::vector<robot_geometry_msgs::Point>
mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
{

  // Dịch sang tọa độ tuyệt đối
  std::vector<robot_geometry_msgs::Point> abs_footprint;
  double cos_th = std::cos(pose.theta);
  double sin_th = std::sin(pose.theta);
  for (auto pt : footprint)
  {
    pt.x *= 1.2;
    pt.y *= 1.2;
    robot_geometry_msgs::Point abs_pt;
    abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th;
    abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th;
    abs_footprint.push_back(abs_pt);
  }

  std::vector<robot_geometry_msgs::Point> points;
  for (size_t i = 0; i < abs_footprint.size(); ++i)
  {
    const robot_geometry_msgs::Point &start = abs_footprint[i];
    const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];

    double dx = end.x - start.x;
    double dy = end.y - start.y;
    double distance = std::hypot(dx, dy);
    int steps = std::max(1, static_cast<int>(std::floor(distance / resolution)));

    for (int j = 0; j <= steps; ++j)
    {
      if (j == steps && i != abs_footprint.size() - 1)
        continue; // tránh lặp
      double t = static_cast<double>(j) / steps;
      robot_geometry_msgs::Point pt;
      pt.x = start.x + t * dx;
      pt.y = start.y + t * dy;
      points.push_back(pt);
    }
  }
  return points;
}

double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, const double &y)
{
  unsigned int mx, my;
  unsigned char cost;
  if (costmap_robot_->getCostmap()->worldToMap(x, y, mx, my))
  {
    cost = costmap_robot_->getCostmap()->getCost(mx, my);
  }
  return static_cast<double>(cost);
}

void mkt_algorithm::diff::PredictiveTrajectory::updateCostAtOffset(
    TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose,
    double x_offset, double y_offset, double &cost_left, double &cost_right)
{
  robot_nav_2d_msgs::Pose2DStamped stamped_pose, transformed_pose;
  stamped_pose = base_pose;
  stamped_pose.pose.x += x_offset;
  stamped_pose.pose.y += y_offset;

  if (robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, transformed_pose))
  {
    double cost = this->costAtPose(stamped_pose.pose.x, stamped_pose.pose.y);
    if (transformed_pose.pose.y < 0)
      cost_right = std::max(cost_right, cost);
    else if (transformed_pose.pose.y > 0)
      cost_left = std::max(cost_left, cost);
  }
}

double mkt_algorithm::diff::PredictiveTrajectory::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
}

double mkt_algorithm::diff::PredictiveTrajectory::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
}

bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::vector<double> &angle_history, double current_angle,
                                                                          double amplitude_threshold, size_t window_size)
{

  angle_history.push_back(current_angle);
  if ((unsigned int)angle_history.size() > window_size)
    angle_history.erase(angle_history.begin());

  if ((unsigned int)angle_history.size() < 2)
    return false;

  double max_angle = *std::max_element(angle_history.begin(), angle_history.end());
  double min_angle = *std::min_element(angle_history.begin(), angle_history.end());

  double amplitude = max_angle - min_angle;
  // if(fabs(amplitude) > amplitude_threshold)
  //   ROS_INFO_THROTTLE(0.1,"%f %f %f %d %d", amplitude, max_angle , min_angle, (unsigned int)angle_history.size(), (unsigned int)window_size);
  return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0;
}

void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose)
{
  const auto &plan_back_pose = compute_plan_.poses.back();
  // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0;
  // const double offset_min = this->min_path_distance_;

  auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
  for (const auto &point : points_rb)
  {
    double cost_goal = costAtPose(point.x, point.y);
    double dx = point.x - pose.pose.x;
    double dy = point.y - pose.pose.y;
    double cos_yaw = cos(-pose.pose.theta);
    double sin_yaw = sin(-pose.pose.theta);
    double y_rel = dx * sin_yaw + dy * cos_yaw;
    const double epsilon = min_path_distance_;
    if (y_rel > epsilon)
    {
      cost_left_side_ = std::max(cost_left_side_, cost_goal);
    }
    else if (y_rel < -epsilon)
    {
      cost_right_side_ = std::max(cost_right_side_, cost_goal);
    }
  }

  unsigned int step_footprint = 10;
  if ((unsigned int)(compute_plan_.poses.size() - 1) < 10)
  {
    auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
    for (const auto &point : points)
    {
      double cost_goal = costAtPose(point.x, point.y);
      double dx = point.x - plan_back_pose.pose.x;
      double dy = point.y - plan_back_pose.pose.y;
      double cos_yaw = cos(-plan_back_pose.pose.theta);
      double sin_yaw = sin(-plan_back_pose.pose.theta);
      double y_rel = dx * sin_yaw + dy * cos_yaw;
      const double epsilon = min_path_distance_;
      if (y_rel > epsilon)
      {
        cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
      }
      else if (y_rel < -epsilon)
      {
        cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
      }
      else
        center_cost_ = std::max(center_cost_, cost_goal);
    }
  }
  else
  {
    for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint)
    {
      auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
      for (const auto &point : points)
      {
        double cost_goal = costAtPose(point.x, point.y);
        double dx = point.x - compute_plan_.poses[i].pose.x;
        double dy = point.y - compute_plan_.poses[i].pose.y;
        double cos_yaw = cos(-compute_plan_.poses[i].pose.theta);
        double sin_yaw = sin(-compute_plan_.poses[i].pose.theta);
        double y_rel = dx * sin_yaw + dy * cos_yaw;
        const double epsilon = min_path_distance_;
        if (y_rel > epsilon)
        {
          cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
        }
        else if (y_rel < -epsilon)
        {
          cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
        }
        else
          center_cost_ = std::max(center_cost_, cost_goal);
      }
      double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x;
      double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y;
      if (hypot(dx, dy) > 1.0)
        break;
    }
  }
}

score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
{
  return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();
}

// Register this planner as a GlobalPlanner plugin
BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory)