#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
#include <boost/dll/alias.hpp>


mkt_algorithm::diff::PredictiveTrajectory::PredictiveTrajectory()
  : initialized_(false),
    nav_stop_(false),
    x_direction_(0.0),
    y_direction_(0.0),
    theta_direction_(0.0),
    use_velocity_scaled_lookahead_dist_(false),
    lookahead_time_(0.0),
    lookahead_dist_(0.0),
    min_lookahead_dist_(0.0),
    max_lookahead_dist_(0.0),
    max_lateral_accel_(0.0),
    use_rotate_to_heading_(false),
    rotate_to_heading_min_angle_(0.0),
    min_path_distance_(0.0),
    max_path_distance_(0.0),
    use_final_heading_alignment_(false),
    final_heading_xy_tolerance_(0.0),
    final_heading_angle_tolerance_(0.0),
    final_heading_min_velocity_(0.0),
    final_heading_kp_angular_(0.0),
    final_heading_ki_angular_(0.0),
    final_heading_kd_angular_(0.0),
    near_goal_heading_integral_(0.0),
    near_goal_heading_last_error_(0.0),
    near_goal_heading_was_active_(false),
    min_approach_linear_velocity_(0.0),
    use_cost_regulated_linear_velocity_scaling_(false),
    inflation_cost_scaling_factor_(0.0),
    cost_scaling_dist_(0.0),
    cost_scaling_gain_(0.0),
    control_duration_(0.0),
    kf_(nullptr),
    kf_q_v_(0.25),
    kf_q_w_(0.8),
    kf_r_v_(0.05),
    kf_r_w_(0.08),
    kf_p0_(0.5),
    kf_filter_angular_(false),
    m_(0),
    n_(0)
{
}

mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {}

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_ = nh;
    nh_priv_ = robot::NodeHandle(nh, name);
    robot::log_info_at(__FILE__, __LINE__, "nh_ : %s", nh_.getNamespace().c_str());
    robot::log_info_at(__FILE__, __LINE__, "nh_priv_ : %s", nh_priv_.getNamespace().c_str());
    name_ = name;
    tf_ = tf;
    traj_ = traj;
    costmap_robot_ = costmap_robot;
    nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
    this->getParams();

    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;
    }

    // kalman
    last_actuator_update_ = robot::Time::now();
    // State: [v, a, j, w, alpha, jw] where:
    //  - v: linear velocity, a: linear accel, j: linear jerk
    //  - w: angular velocity, alpha: angular accel, jw: angular jerk
    // Measurement: [v, w]
    n_ = 6;
    m_ = 2;
    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::Zero(m_, m_);
    P = Eigen::MatrixXd::Identity(n_, n_) * std::max(1e-9, kf_p0_);

    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) = std::max(1e-12, kf_q_v_);
    Q(5, 5) = std::max(1e-12, kf_q_w_);

    R(0, 0) = std::max(1e-12, kf_r_v_);
    R(1, 1) = std::max(1e-12, kf_r_w_);

    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);

#ifdef BUILD_WITH_ROS 
    ros::NodeHandle nh_ros;
    current_goal_pub_ = nh_ros.advertise<geometry_msgs::PoseStamped>("current_goal", 10);
    closet_robot_goal_pub_ = nh_ros.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 10);
    lookahead_point_pub_ = nh_ros.advertise<geometry_msgs::PoseStamped>("lookahead_point", 10);
#endif

    x_direction_ = y_direction_ = theta_direction_ = 0;
    this->initialized_ = true;
    robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__);
  }
}

void mkt_algorithm::diff::PredictiveTrajectory::getParams()
{
  robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
  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);

  // Final heading alignment (Arc Motion)
  nh_priv_.param<bool>("use_final_heading_alignment", use_final_heading_alignment_, true);
  nh_priv_.param<double>("final_heading_xy_tolerance", final_heading_xy_tolerance_, 0.15);
  nh_priv_.param<double>("final_heading_angle_tolerance", final_heading_angle_tolerance_, 0.05);
  nh_priv_.param<double>("final_heading_min_velocity", final_heading_min_velocity_, 0.05);
  nh_priv_.param<double>("final_heading_kp_angular", final_heading_kp_angular_, 1.5);
  nh_priv_.param<double>("final_heading_ki_angular", final_heading_ki_angular_, 0.0);
  nh_priv_.param<double>("final_heading_kd_angular", final_heading_kd_angular_, 0.0);

  nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.01);
  nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.01);
  if (trans_stopped_velocity_ < 0.02)
    trans_stopped_velocity_ = 0.5 * min_approach_linear_velocity_;
  if (trans_stopped_velocity_ > min_approach_linear_velocity_)
    trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.01;

  // 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);
  nh_priv_.param<double>("max_lateral_accel", max_lateral_accel_, 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.", __FILE__, __LINE__);
    use_cost_regulated_linear_velocity_scaling_ = false;
  }

  // Kalman filter tuning (filtering v and w commands)
  nh_priv_.param<double>("kf_q_v", kf_q_v_, kf_q_v_);
  nh_priv_.param<double>("kf_q_w", kf_q_w_, kf_q_w_);
  nh_priv_.param<double>("kf_r_v", kf_r_v_, kf_r_v_);
  nh_priv_.param<double>("kf_r_w", kf_r_w_, kf_r_w_);
  nh_priv_.param<double>("kf_p0", kf_p0_, kf_p0_);
  nh_priv_.param<bool>("kf_filter_angular", kf_filter_angular_, kf_filter_angular_);

  double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
  control_duration_ = 1.0 / control_frequency;

  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);
    traj_.get()->getNodeHandle().param<double>("min_speed_xy", min_speed_xy_, 0.0);
    if(fabs(min_speed_xy_) > sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_))
    {
      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
    }
    if(fabs(min_speed_xy_) > sqrt(min_vel_x_ * min_vel_x_ + min_vel_y_ * min_vel_y_))
    {
      min_speed_xy_ = sqrt(max_vel_x_ * max_vel_x_ + max_vel_y_ * max_vel_y_);
    }

    traj_.get()->getNodeHandle().param<double>("max_speed_xy", max_speed_xy_, 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();
    near_goal_heading_integral_ = 0.0;
    near_goal_heading_last_error_ = 0.0;
    near_goal_heading_was_active_ = false;

    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;
  }
  if(robot::Time::now() - last_actuator_update_ > robot::Duration(0.5))
  {
    last_actuator_update_ = robot::Time::now();
  }

  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;
  }

  const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
  if(fabs(carrot_pose.pose.y) > 0.2)
  {
    lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
    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;
    } 
  }  

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

  if ((unsigned int)(compute_plan_.poses.size() > 1) && 
      journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1) >= costmap_robot_->getCostmap()->getResolution())
  {
    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 if(compute_plan_.poses.size() == 1)
  {
    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.y - 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());

      
      auto goal_pose = compute_plan_.poses.front().pose;
      auto start_pose = pose.pose;
      double angle_path = atan2(goal_pose.y - start_pose.y, goal_pose.x - start_pose.x);
      double dir_path = cos(fabs(angle_path - goal_pose.theta));
      if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
        x_direction = dir_path > 0 ? FORWARD : BACKWARD;
    }
    catch (std::exception &e)
    {
      robot::log_warning_throttle(0.2, "[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
      x_direction = x_direction_; 
    }
  }

  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();
  const 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 (robot::ok() && traj_->hasMoreTwists())
  {
    twist = traj_->nextTwist();
  }
  double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
  drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
  // drive_cmd.x = sqrt(twist.x * twist.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__);
    stopWithAccLimits(pose, velocity, drive_cmd, dt);
    result.velocity = drive_cmd;
    prevous_drive_cmd_ = drive_cmd;
    return result;
  }
  double lookahead_dist = getLookAheadDistance(velocity);
  auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
  robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);

#ifdef BUILD_WITH_ROS
  {
    geometry_msgs::PoseStamped lookahead_point;
    lookahead_point.header.stamp = ros::Time::now();
    lookahead_point.header.frame_id = carrot_pose_stamped.header.frame_id;
    lookahead_point.pose.position.x = carrot_pose_stamped.pose.position.x;
    lookahead_point.pose.position.y = carrot_pose_stamped.pose.position.y;
    lookahead_point.pose.position.z = carrot_pose_stamped.pose.position.z;
    lookahead_point.pose.orientation.x = carrot_pose_stamped.pose.orientation.x;
    lookahead_point.pose.orientation.y = carrot_pose_stamped.pose.orientation.y;
    lookahead_point.pose.orientation.z = carrot_pose_stamped.pose.orientation.z;
    lookahead_point.pose.orientation.w = carrot_pose_stamped.pose.orientation.w;
    lookahead_point_pub_.publish(lookahead_point);
  }
#endif

  bool allow_rotate = false;
  nh_priv_.param("allow_rotate", allow_rotate, false);

  const double distance_allow_rotate = min_journey_squared_;
  const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
  allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
  // robot_geometry_msgs::Pose2D back_pose = transformed_plan.poses.back().pose;
  // allow_rotate |= fabs(atan2(back_pose.y, back_pose.x) - back_pose.theta) > M_PI / 3.0;

  allow_rotate &= (fabs(transformed_plan.poses.front().pose.y) <= 0.5);

  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, dt))
        return result;
    }
    else
    {
      rotateToHeading(angle_to_heading, velocity, drive_cmd, dt);
    }
  }
  else
  {
//     // === Final Heading Alignment Check ===
//     double xy_error = 0.0, heading_error = 0.0;
//     if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
//     {
//       // Use Arc Motion controller for final heading alignment
//       alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
// #ifdef BUILD_WITH_ROS
//       ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
//                         heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
// #endif
//     }
//     else
//     {
      robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target, dt);
      carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
      
      // Normal Pure Pursuit
      this->computePurePursuit(
      carrot_pose, 
      drive_target,
      velocity, 
      transformed_plan,
      min_approach_linear_velocity_, 
      sign_x, 
      dt, 
      drive_cmd);
    // }
    applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
    if (this->nav_stop_)
    {
      if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
      {
        if (!stopWithAccLimits(pose, velocity, drive_cmd, dt))
          return result;
      }
      else
        drive_cmd = {};
      result.velocity = drive_cmd;
      return result;
    }
    
  }
  result.poses.clear();
  result.poses.reserve(transformed_plan.poses.size());
  for (const auto &pose_stamped : transformed_plan.poses)
  {
    result.poses.push_back(pose_stamped.pose);
    if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 && 
      fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 && 
      fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6)
    break;
  }

  if(fabs(v_max == 0.0))
  {
    drive_cmd.x = 0.0;
    robot::log_warning_throttle(0.2, "[%s:%d]\n v_max is 0.0", __FILE__, __LINE__);
    return result;
  }
  result.velocity = drive_cmd;
  prevous_drive_cmd_ = drive_cmd;
  return result;
}

void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
  const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
  const robot_nav_2d_msgs::Twist2D &drive_target,
  const robot_nav_2d_msgs::Twist2D &velocity,
  const robot_nav_2d_msgs::Path2D &trajectory,
  const double &min_approach_linear_velocity,
  const double &sign_x,
  const double &dt,
  robot_nav_2d_msgs::Twist2D &drive_cmd)
{
  // 1) Curvature from pure pursuit
  const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
  const double L2_min = 0.01;          // m^2, chỉnh theo nhu cầu (0.02–0.1)
  const double L2_safe = std::max(L2, L2_min);
  const double L = std::sqrt(L2_safe);

  const double kappa = 2.0 * carrot_pose.pose.y / L2_safe;

  // 3) Adjust speed using Hermite trajectory curvature + remaining distance
  double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x);
  // const double L_min = 0.1;  // m, chỉnh theo nhu cầu
  // double scale_close = std::clamp(L / L_min, 0.0, 1.0);
  // v_target *= scale_close;
  const double y_abs = std::fabs(carrot_pose.pose.y);
  const double y_soft = 0.1;
  if (y_abs > y_soft) 
  {
    double scale = y_soft / y_abs;  // y càng lớn => scale càng nhỏ
    scale = std::clamp(scale, 0.6, 1.0); // không giảm quá sâu
    v_target *= scale;
    robot_nav_2d_msgs::Twist2D cmd, result;
    cmd.x = v_target;
    this->moveWithAccLimits(velocity, cmd, result, dt);
    v_target = result.x;
  }

  // 4) Maintain minimum approach speed
  if (std::fabs(v_target) < min_approach_linear_velocity)
    v_target = std::copysign(min_approach_linear_velocity, sign_x);
  // 5) Angular speed from curvature
  double w_target = v_target * kappa;
  if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
  {
    if (trajectory.poses.size() >= 2) {
      const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
      double heading_ref = 0.0;
      for(int i = trajectory.poses.size() - 2; i >= 0; i--)
      {
        const auto& p = trajectory.poses[i].pose;
        const auto& dx =  p1.x - p.x ;
        const auto& dy =  p1.y - p.y ;
        if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
        {
          if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
            continue;
          heading_ref = std::atan2(dy, dx);
          if(sign_x < 0.0)
          heading_ref += std::copysign(M_PI, heading_ref) * (-1.0);
          break;
        }
      }

      const double error = heading_ref;
      double w_heading = 0.0;
      pid(error, 
        near_goal_heading_integral_, 
        near_goal_heading_last_error_, 
        dt, 
        final_heading_kp_angular_, 
        final_heading_ki_angular_, 
        final_heading_kd_angular_, 
        w_heading);
      // Apply acceleration limits
      double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
      w_target = velocity.theta + dw_heading;
      w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
    }
    else
    {
      w_target = std::clamp(w_target, -0.001, 0.001);
      near_goal_heading_was_active_ = false;
    }
  }
  else
  {
    near_goal_heading_was_active_ = false;
  }
  w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));

  // 6) Apply acceleration limits (linear + angular)
  const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt);
  const double dw = std::clamp(w_target - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);

  drive_cmd.x = velocity.x + dv;
  drive_cmd.theta = velocity.theta + dw;
  
  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);
  double v_min = min_approach_linear_velocity_;
  drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
  drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
  if (kf_filter_angular_)
    drive_cmd.theta = std::clamp(kf_->state()[3], -fabs(drive_target.theta), fabs(drive_target.theta));
  // robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
}

void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
  const robot_nav_2d_msgs::Path2D &plan,
  const robot_nav_2d_msgs::Twist2D &velocity,
  robot_nav_2d_msgs::Twist2D &drive_cmd,
  const double &sign_x,
  const double &dt)
{
  double s = plan.poses.empty() 
    ? min_journey_squared_
    : journey(plan.poses, 0, plan.poses.size() - 1);
  double S_th = 0.1 + max_path_distance_ + fabs(velocity.x) * lookahead_time_;  
  double S_final = std::min(S_th * 0.2, 0.1 + max_path_distance_);

  double max_speed = fabs(drive_cmd.x);
  double target_speed = max_speed;
  if (s < S_th)
  {
    double r = std::clamp(s / S_th, 0.0, 1.0);
    double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
    target_speed = max_speed * cosine_factor;
  }
  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
  double reduce_speed = std::min(max_speed, v_min);
  if (s < S_final)
  {
    double r = std::clamp(s / S_final, 0.0, 1.0);
    double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
    reduce_speed = (reduce_speed - min_approach_linear_velocity_) * cosine_factor + min_approach_linear_velocity_;
    reduce_speed = std::max(reduce_speed, min_approach_linear_velocity_);
  }
  target_speed = std::max(target_speed, reduce_speed);

  double v_desired = std::copysign(target_speed, sign_x);
  double max_acc_vel = velocity.x + fabs(acc_lim_x_) * dt;
  double min_acc_vel = velocity.x - fabs(acc_lim_x_) * dt;
  double v_samp = std::clamp(v_desired, min_acc_vel, max_acc_vel);

  double max_speed_to_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, s));
  v_samp = std::copysign(std::min(max_speed_to_stop, fabs(v_samp)), sign_x);
  if (sign_x > 0.0)
    drive_cmd.x = std::clamp(v_samp, min_approach_linear_velocity_, max_speed);
  else
    drive_cmd.x = std::clamp(v_samp, -max_speed, -min_approach_linear_velocity_);
}

bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
    const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, 
    robot_nav_2d_msgs::Twist2D velocity, double &angle_to_path, const double &sign_x)
{
  
  const bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_ + 0.01, trans_stopped_velocity_ + 0.01);
  // const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
  // const double max_kappa = calculateMaxKappa(global_plan);
  // const bool curvature = max_kappa > straight_threshold;
  double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
  if(is_stopped && global_plan.poses.size() >= 4 &&
  journey(global_plan.poses, 0, global_plan.poses.size() - 1) >= 0.7 * min_lookahead_dist_)
  {
    const auto& p1 = global_plan.poses[2];
    for(int i = 3; i < global_plan.poses.size(); i++)
    {
      const auto& p = global_plan.poses[i];
      const auto& dx = p.pose.x - p1.pose.x;
      const auto& dy = p.pose.y - p1.pose.y;
      if(std::hypot(dx, dy) > costmap_robot_->getCostmap()->getResolution())
      {
        if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9)
          continue;
        path_angle = std::atan2(dy, dx);
        break;
      }
    }
  }
  
  // Whether we should rotate robot to rough path heading
  // if(sign_x < 0.0)
  //   path_angle += std::copysign(M_PI, path_angle) * (-1.0);
  // angle_to_path = path_angle;
  angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle;
  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_;
  
  heading_rotate = is_stopped
                       ? rotate_to_heading_min_angle_ + heading_linear
                       : std::clamp(heading_rotate + heading_linear, angle_threshold_ + rotate_to_heading_min_angle_, M_PI_2);

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

  bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
#ifdef BUILD_WITH_ROS
  if (result)
    ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
#else
  if (result)
    robot::log_info_throttle(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
#endif
  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)
{
  const double theta_direction = angle_to_path > 0.0 ? 1.0 : -1.0;
  double ang_diff = angle_to_path;
  double max_vel_theta = traj_ ? fabs(traj_->getTwistAngular(true).z) : 0.3;
  double min_vel_theta = min_vel_theta_;

  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)
  {
    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);
  }
  // Apply direction
  double v_theta_desired = std::copysign(target_angular_speed, ang_diff);

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

  // === ACCELERATION LIMITS ===
  double max_acc_vel = fabs(acc_lim_theta_) * dt;
  double domega = std::clamp(v_theta_samp - velocity.theta, -max_acc_vel, max_acc_vel);

  // === FINAL LIMITS ===
  cmd_vel.x = 0.0;
  cmd_vel.y = 0.0;
  cmd_vel.theta = velocity.theta + domega;
  // robot::log_info("cmd_vel.theta: %f, velocity.theta: %f, domega: %f, max_acc_vel: %f, v_theta_samp: %f", cmd_vel.theta, velocity.theta, domega, max_acc_vel, v_theta_samp);
}

bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
    const robot_nav_2d_msgs::Path2D &trajectory,
    const robot_nav_2d_msgs::Pose2DStamped &goal,
    const robot_nav_2d_msgs::Twist2D &velocity,
    double &xy_error,
    double &heading_error,
    const double &sign_x)
{
  if (!use_final_heading_alignment_)
    return false;

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

  // Calculate distance to goal (last point in trajectory is closest to goal in robot frame)
  const auto &last_pose = trajectory.poses.back().pose;
  xy_error = std::hypot(last_pose.x, last_pose.y);

  // Transform goal heading to robot frame
  // In robot frame, robot heading is 0, so heading_error = goal_heading_in_robot_frame
  heading_error = 0.0;
  if(trajectory.poses.size() >= 2)
  {
    const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
    for(int i = trajectory.poses.size() - 2; i >= 0; i--)
    {
      const auto& p = trajectory.poses[i].pose;
      const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
      const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
      if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
      {
        heading_error = angles::normalize_angle(std::atan2(dy, dx));
        break;
      }
    }
  }

  // Check if we're close enough to goal position to start final heading alignment
  double distance_threshold = final_heading_xy_tolerance_;

  // Dynamic threshold: increase when moving fast to allow smooth transition
  double velocity_factor = std::max(0.0, std::fabs(velocity.x) * 0.5);
  distance_threshold += velocity_factor;

  bool near_goal = xy_error < distance_threshold;
  bool heading_not_reached = std::fabs(heading_error) > final_heading_angle_tolerance_;

  return near_goal && heading_not_reached;
}

void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
    const double &xy_error,
    const double &heading_error,
    const robot_nav_2d_msgs::Twist2D &velocity,
    const double &sign_x,
    const double &dt,
    robot_nav_2d_msgs::Twist2D &cmd_vel)
{
  // ============================================================
  // Arc Motion Controller for Final Heading Alignment
  // ============================================================
  //
  // Strategy:
  // 1. Linear velocity: proportional to xy_error, decreasing as we approach goal
  // 2. Angular velocity: proportional to heading_error with feedforward
  // 3. Coordinate both to arrive at goal with correct heading
  //
  // Key insight: For arc motion, the relationship is:
  //   arc_length = radius * angle
  //   v = omega * radius
  //
  // We want to cover xy_error distance while rotating heading_error angle
  // So ideal radius = xy_error / |heading_error| (when heading_error != 0)
  // ============================================================

  const double abs_heading_error = std::fabs(heading_error);
  const double heading_sign = (heading_error > 0) ? 1.0 : -1.0;

  // --- Linear velocity calculation ---
  // Base velocity proportional to distance, with minimum for smooth motion
  double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error);
  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
  v_base = std::max(v_base, final_heading_min_velocity_);
  v_base = std::min(v_base, v_min);

  // Scale down when heading error is large (prioritize rotation)
  double heading_scale = 1.0;
  if (abs_heading_error > 0.1)  // ~5.7 degrees
  {
    // Reduce linear velocity when heading error is significant
    heading_scale = std::clamp(0.3 / abs_heading_error, 0.3, 1.0);
  }

  double v_target = v_base * heading_scale * sign_x;

  // --- Angular velocity calculation ---
  double omega_target = 0.0;

  if (abs_heading_error > final_heading_angle_tolerance_)
  {
    // Method 1: Proportional control
    double omega_p = 0.0;
    pid(heading_error, 
      near_goal_heading_integral_, 
      near_goal_heading_last_error_, 
      dt, 
      final_heading_kp_angular_, 
      final_heading_ki_angular_, 
      final_heading_kd_angular_, 
      omega_p);

    // Method 2: Arc-based feedforward (coordinate linear and angular motion)
    double omega_arc = 0.0;
    if (xy_error > 0.01 && abs_heading_error > 0.01)
    {
      // Ideal omega to complete rotation while traveling remaining distance
      // omega = v * heading_error / xy_error
      omega_arc = std::fabs(v_target - velocity.x) * heading_error / xy_error;
    }

    // Blend proportional and arc-based control
    // Use more arc-based when far, more proportional when close
    double blend_factor = std::clamp(xy_error / final_heading_xy_tolerance_, 0.0, 1.0);
    omega_target = blend_factor * omega_arc + (1.0 - blend_factor) * omega_p;

    // Reduce angular speed as heading error approaches zero
    double heading_scale = std::clamp(
        abs_heading_error / std::max(final_heading_angle_tolerance_, 1e-3),
        0.0, 1.0);
    heading_scale = heading_scale * heading_scale; // smoother near zero
    omega_target *= heading_scale;

    // Ensure minimum angular velocity for responsiveness
    double omega_min = min_vel_theta_;
    if (std::fabs(omega_target) < omega_min &&
        abs_heading_error > final_heading_angle_tolerance_ * 3.0)
    {
      omega_target = heading_sign * omega_min;
    }
  }
  
  // --- Apply acceleration limits ---
  // Linear
  double v_current = velocity.x;
  double dv_max = acc_lim_x_ * dt;
  double dv = std::clamp(v_target - v_current, -dv_max, dv_max);
  cmd_vel.x = v_current + dv;

  // Angular
  double omega_current = velocity.theta;
  double domega_max = acc_lim_theta_ * dt;
  double domega = std::clamp(omega_target - omega_current, -domega_max, domega_max);
  cmd_vel.theta = omega_current + domega;

  // --- Apply velocity limits ---
  cmd_vel.x = std::clamp(cmd_vel.x, -v_min, v_min);
  cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_);

  // --- Safety: ensure we can stop ---
  double max_v_to_stop = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error);
  if (std::fabs(cmd_vel.x) > max_v_to_stop)
  {
    cmd_vel.x = std::copysign(max_v_to_stop, cmd_vel.x);
  }

  cmd_vel.y = 0.0;

#ifdef BUILD_WITH_ROS
  ROS_DEBUG_THROTTLE(0.2, "[FinalHeading] xy=%.3f, heading=%.3f, v=%.3f, omega=%.3f",
                     xy_error, heading_error, cmd_vel.x, cmd_vel.theta);
#endif
}

void mkt_algorithm::diff::PredictiveTrajectory::pid(
  const double &error, double &integral, double &last_error, const double &dt, const double &kp, const double &ki, const double &kd, double &output)
{
  if (!near_goal_heading_was_active_)
  {
    integral = 0.0;
    last_error = error;
    near_goal_heading_was_active_ = true;
  }
  const double dt_safe = std::max(dt, 1e-6);
  double w_p = kp * error;
  integral += error * dt_safe;
  const double integral_cap = (std::fabs(ki) > 1e-9)? (2.0 / ki) : 1e6;
  integral = std::clamp(integral, -integral_cap, integral_cap);
  double w_i = ki * integral;
  double w_d = kd * (error - last_error) / dt_safe;
  last_error = error;
  output = w_p + w_i + w_d;
}

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 robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");

  if (global_plan.poses.size() < 2)
    return std::prev(global_plan.poses.end());

  unsigned int goal_index = (unsigned)global_plan.poses.size() - 1;

  // Góc đầu và ngưỡng gãy (fixed)
  const auto &p0 = global_plan.poses[0].pose;
  const auto &p1 = global_plan.poses[1].pose;
  double start_angle = atan2(p1.y - p0.y, p1.x - p0.x);
  double turn_threshold = M_PI_2 * 0.6; // ~54deg, chỉnh theo nhu cầu

  // Dừng nếu gãy góc
  for (unsigned int i = 1; i < global_plan.poses.size(); ++i)
  {
    const auto &a = global_plan.poses[i - 1].pose;
    const auto &b = global_plan.poses[i].pose;
    double current_angle = atan2(b.y - a.y, b.x - a.x);
    double delta = angles::normalize_angle(current_angle - start_angle);
    goal_index = i;
    if (fabs(delta) >= turn_threshold)
      break;
  }
  // Chọn điểm lookahead theo khoảng cách
  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 (goal_pose_it == global_plan.poses.end())
    goal_pose_it = std::prev(global_plan.poses.end());

  return goal_pose_it;
}

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 robot_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\n", __FILE__, __LINE__, ex.what());
    return false;
  }
  return true;
}

double mkt_algorithm::diff::PredictiveTrajectory::calculateMaxKappa(const robot_nav_2d_msgs::Path2D &path, const double preview_distance)
{
  double traveled = 0.0;
  double max_kappa = 0.0;
  for (size_t i = 1; i + 1 < path.poses.size(); ++i)
  {
    const auto &p0 = path.poses[i - 1].pose;
    const auto &p1 = path.poses[i].pose;
    const auto &p2 = path.poses[i + 1].pose;

    const double a = std::hypot(p1.x - p0.x, p1.y - p0.y);
    const double b = std::hypot(p2.x - p1.x, p2.y - p1.y);
    const double c = std::hypot(p2.x - p0.x, p2.y - p0.y);
    traveled += a;

    if (a > 1e-6 && b > 1e-6 && c > 1e-6)
    {
      const double cross = (p1.x - p0.x) * (p2.y - p0.y) - (p1.y - p0.y) * (p2.x - p0.x);
      const double area2 = std::fabs(cross); // 2 * area
      const double kappa = 2.0 * area2 / (a * b * c);
      max_kappa = std::max(max_kappa, std::fabs(kappa));
    }

    if (traveled >= preview_distance)
      break;
  }
  return max_kappa;
}

double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajectory(
  const robot_nav_2d_msgs::Twist2D &velocity,
  const robot_nav_2d_msgs::Path2D &trajectory,
  double v_target,
  const double &sign_x)
{
  if(trajectory.poses.size() < 2)
    return min_approach_linear_velocity_ * sign_x;

  double preview_distance = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_);

  double max_kappa = calculateMaxKappa(trajectory, preview_distance);
  double v_limit = std::fabs(v_target);
  double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
  
  const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
  const double v_min = std::min(fabs(v_limited), min_speed_xy_);
  if (journey_distance < min_journey_squared_)
  {
    v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, v_min) * sign_x;
  }

  if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
  {
    const double v_curve = std::sqrt(max_lateral_accel_ / max_kappa);
    v_limit = std::min(v_limit, v_curve);
  }

  if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_)
    v_limit = v_min * sign_x;

  if (fabs(decel_lim_x_) > 1e-6)
  {
    const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
    const double v_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, remaining));
    v_limit = std::min(v_limit, v_stop);
  }
  v_limit = std::min(v_limit, fabs(v_target));

  return std::copysign(v_limit, sign_x);
}

robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTrajectory(
  const robot_nav_2d_msgs::Path2D &path, 
  const robot_nav_2d_msgs::Twist2D &drive_target,
  const robot_nav_2d_msgs::Twist2D &velocity,
  const double &sign_x,
  robot_nav_2d_msgs::Twist2D &drive_cmd, 
  const double &dt)
{
  if (path.poses.empty())
  {
    drive_cmd.x = 0.0;
    drive_cmd.theta = 0.0;
    return robot_nav_2d_msgs::Path2D();
  }

  drive_cmd.x = drive_target.x;
  drive_cmd.theta = max_vel_theta_;
  double max_kappa = calculateMaxKappa(path);
  const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
  // nếu đường thẳng
  if (max_kappa <= straight_threshold) 
  { 
    if(fabs(path.poses.back().pose.x) < min_lookahead_dist_ * 0.8)
    {
      if(fabs(path.poses.back().pose.x) < min_journey_squared_)
        drive_cmd.theta = 0.01;
      return generateParallelPath(path, sign_x);
    }
    return generateHermiteTrajectory(path, sign_x);
  }
  else // nếu đường cong
  {
    const double v_limited = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
    const double v_min = std::min(fabs(v_limited), min_speed_xy_);
    if(fabs(drive_cmd.x) < v_min) 
    {
      drive_cmd.x = std::copysign(v_min, sign_x);
    }
    return generateHermiteQuadraticTrajectory(path, sign_x);
  }
}

robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateParallelPath(
  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
{
  robot_nav_2d_msgs::Path2D parallel_path;
  parallel_path.header = path.header;
  int N = path.poses.size();
  if (N < 2) return parallel_path;

  double offset_y = path.poses.front().pose.y;

  parallel_path.poses.resize(N);
  for (int i = 0; i < N; ++i)
  {
    const auto& p = path.poses[i].pose;

    double dx, dy;
    if (i == 0) {
      dx = path.poses[i+1].pose.x - p.x;
      dy = path.poses[i+1].pose.y - p.y;
    } else if (i == N-1) {
      dx = p.x - path.poses[i-1].pose.x;
      dy = p.y - path.poses[i-1].pose.y;
    } else {
      dx = path.poses[i+1].pose.x - path.poses[i-1].pose.x;
      dy = path.poses[i+1].pose.y - path.poses[i-1].pose.y;
    }
    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
      continue;
    double theta = atan2(dy, dx);
    double x_off = p.x - offset_y * sin(theta)*sign_x;
    double y_off = p.y - offset_y * cos(theta)*sign_x;

    parallel_path.poses[i].header = path.poses[i].header;
    parallel_path.poses[i].pose.x = x_off;
    parallel_path.poses[i].pose.y = y_off;
    parallel_path.poses[i].pose.theta = sign_x < 0 ? angles::normalize_angle(theta + M_PI) : theta;
  }
  return parallel_path;
}

robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
{
  robot_nav_2d_msgs::Path2D hermite_trajectory;
  hermite_trajectory.poses.clear();
  hermite_trajectory.header = path.header;

  if (path.poses.empty())
    return hermite_trajectory;

  const auto &goal = path.poses.back();
  if (hermite_trajectory.header.frame_id.empty())
    hermite_trajectory.header.frame_id = goal.header.frame_id;
  if (hermite_trajectory.header.stamp.isZero())
    hermite_trajectory.header.stamp = goal.header.stamp;

  const double x = goal.pose.x;
  const double y = goal.pose.y;
  double theta = goal.pose.theta;
  const double L = std::hypot(x, y);
  if (L < 1e-6) {
    robot_nav_2d_msgs::Pose2DStamped pose_stamped;
    pose_stamped.pose.x = x;
    pose_stamped.pose.y = y;
    pose_stamped.pose.theta = theta;
    pose_stamped.header.stamp = hermite_trajectory.header.stamp;
    pose_stamped.header.frame_id = hermite_trajectory.header.frame_id;
    hermite_trajectory.poses.push_back(pose_stamped);
    return hermite_trajectory;
  }

  int samples = L/costmap_robot_->getCostmap()->getResolution();
  if (samples < 1) {
    samples = 1;
  }
  hermite_trajectory.poses.reserve(samples + 1);
  const double Lnegative = sign_x < 0 ? -L : L;
  for (int i = 0; i <= samples; ++i) {
    double t = static_cast<double>(i) / samples;
    double t2 = t * t;
    double t3 = t2 * t;

    double h00 =  2 * t3 - 3 * t2 + 1;
    double h10 =      t3 - 2 * t2 + t;
    double h01 = -2 * t3 + 3 * t2;
    double h11 =      t3 -     t2;

    // Hermite interpolation
    double px = h10 * Lnegative + h01 * x + h11 * Lnegative * std::cos(theta);
    double py = h01 * y + h11 * Lnegative * std::sin(theta);

    // First derivatives for heading
    double dh00 = 6 * t2 - 6 * t;
    double dh10 = 3 * t2 - 4 * t + 1;
    double dh01 = -6 * t2 + 6 * t;
    double dh11 = 3 * t2 - 2 * t;

    double dx = dh10 * Lnegative + dh01 * x + dh11 * Lnegative * std::cos(theta);
    double dy = dh01 * y + dh11 * Lnegative * std::sin(theta);

    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
      continue;
    double heading = std::atan2(dy, dx);

    robot_nav_2d_msgs::Pose2DStamped pose_out;
    pose_out.pose.x = px;
    pose_out.pose.y = py;
    pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
    pose_out.header.stamp = hermite_trajectory.header.stamp;
    pose_out.header.frame_id = hermite_trajectory.header.frame_id;
    hermite_trajectory.poses.push_back(pose_out);
  }
  return hermite_trajectory;
}

robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory(
  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
{
  robot_nav_2d_msgs::Path2D trajectory;
  trajectory.poses.clear();
  trajectory.header = path.header;
  if (path.poses.empty())
    return trajectory;

  const auto &goal = path.poses.back();
  if (trajectory.header.frame_id.empty())
    trajectory.header.frame_id = goal.header.frame_id;
  if (trajectory.header.stamp.isZero())
    trajectory.header.stamp = goal.header.stamp;

  const double x = goal.pose.x;
  const double y = goal.pose.y;
  const double theta = sign_x < 0 ? angles::normalize_angle(goal.pose.theta + M_PI) : goal.pose.theta;
  const double L = std::hypot(x, y);
  if (L < 1e-6)
  {
    robot_nav_2d_msgs::Pose2DStamped pose_stamped;
    pose_stamped.pose.x = 0.0;
    pose_stamped.pose.y = 0.0;
    pose_stamped.pose.theta = 0.0;
    pose_stamped.header.stamp = trajectory.header.stamp;
    pose_stamped.header.frame_id = trajectory.header.frame_id;
    trajectory.poses.push_back(pose_stamped);
    return trajectory;
  }

  int samples = L/costmap_robot_->getCostmap()->getResolution();
  if (samples < 1) {
    samples = 1;
  }
  trajectory.poses.reserve(samples + 1);

  // Quadratic Hermite with end tangent aligned to carrot_pose heading:
  // p(0) = 0, p(1) = goal, p'(1) = v1
  const double v1x = L * std::cos(theta);
  const double v1y = L * std::sin(theta);
  const double ax = v1x - x;
  const double ay = v1y - y;
  const double bx = 2.0 * x - v1x;
  const double by = 2.0 * y - v1y;

  trajectory.poses.reserve(samples + 1);
  for (int i = 0; i <= samples; ++i)
  {
    double t = static_cast<double>(i) / samples;
    double t2 = t * t;

    double px = ax * t2 + bx * t;
    double py = ay * t2 + by * t;

    double dx = 2.0 * ax * t + bx;
    double dy = 2.0 * ay * t + by;
    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
      continue;
    double heading = std::atan2(dy, dx);

    robot_nav_2d_msgs::Pose2DStamped pose_out;
    pose_out.pose.x = px;
    pose_out.pose.y = py;
    pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
    pose_out.header.stamp = trajectory.header.stamp;
    pose_out.header.frame_id = trajectory.header.frame_id;
    trajectory.poses.push_back(pose_out);
  }

  return trajectory;
}

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 robot_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\n", __FILE__, __LINE__, ex.what());
    return false;
  }
  catch (tf3::ConnectivityException &ex)
  {
    robot::log_error("[%s:%d]\n Connectivity Error: %s\n", __FILE__, __LINE__, ex.what());
    return false;
  }
  catch (tf3::ExtrapolationException &ex)
  {
    robot::log_error("[%s:%d]\n Extrapolation Error: %s\n", __FILE__, __LINE__, ex.what());
    if (global_plan.poses.size() > 0)
      robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s\n", __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)
{
  // --- 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, const double &dt)
{
  // 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.
  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));

  cmd_vel.x = vx;
  cmd_vel.y = vy;
  cmd_vel.theta = vth;
  return true;
}

std::vector<robot_geometry_msgs::Point>
mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(const robot_geometry_msgs::Pose2D &pose, const std::vector<robot_geometry_msgs::Point> &footprint, const 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);
}

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

BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory)