#include"custom_planner/merge_path_calc.h"

namespace custom_planner {

    MergePathCalc::MergePathCalc(){
        curve_design = new Curve_common();
        spline_inf = new Spline_Inf();
    }
    MergePathCalc::~MergePathCalc(){
        // Giải phóng bộ nhớ
        delete curve_design;
        delete spline_inf;
    }

    std::vector<robot_geometry_msgs::PoseStamped> MergePathCalc::calculateNURBSPath(vector<robot_geometry_msgs::PoseStamped>& control_points, bool reverse)
    {
        std::vector<robot_geometry_msgs::PoseStamped> nurbs_path;
        std::vector<robot_geometry_msgs::PoseStamped> saved_poses; // Lưu các pose đã tính toán

        if((int)nurbs_path.size() > 0) 
        {
            nurbs_path.clear(); // Xóa đường cong cũ nếu có
            saved_poses.clear(); // Xóa các pose đã lưu
        }
        
        // Chuyển đổi control points sang định dạng Eigen::Vector3d
        std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> eigen_control_points;
        for(const auto& cp : control_points) 
        {
            Eigen::Vector3d point(cp.pose.position.x, cp.pose.position.y, 0);
            eigen_control_points.push_back(point);
        }
        // Tạo knot vector tự động
        std::vector<double> knot_vector;
        int degree = 2;
        int n = control_points.size() - 1; // n + 1 control points
        int m = n + degree + 1; // m + 1 knots
        // Tạo uniform knot vector
        for(int i = 0; i <= m; i++) 
        {
            if(i <= degree) knot_vector.push_back(0.0);
            else knot_vector.push_back(1.0);
            // else knot_vector.push_back((double)(i - degree)/(m - 2*degree));
        }

        // Điều chỉnh weights để tăng ảnh hưởng của các control points
        std::vector<double> weights(control_points.size(), 1.0);

        spline_inf->knot_vector.clear();
        spline_inf->control_point.clear();
        spline_inf->weight.clear();

        // Cài đặt thông số cho spline
        curve_design->ReadSplineInf(spline_inf, degree + 1, eigen_control_points, knot_vector);
        curve_design->ReadSplineInf(spline_inf, weights, false);

        // Tính độ dài đường cong để xác định số điểm cần lấy mẫu
        double NURBS_length = calculateNURBSLength(curve_design, spline_inf, 0.01);
        double desired_spacing = 0.05; // Khoảng cách mong muốn giữa các điểm
        double step = calculateAdaptiveStep(NURBS_length, desired_spacing);

        for(double t = 0.0; t <= 1.0; t += step) 
        {
            robot_geometry_msgs::Point point = curve_design->CalculateCurvePoint(spline_inf, t, true);
            if(!std::isnan(point.x) && !std::isnan(point.y)) 
            {
                robot_geometry_msgs::PoseStamped pose;
                pose.header.stamp = robot::Time::now();
                pose.pose.position = point;
                pose.pose.orientation.x = 0.0;
                pose.pose.orientation.y = 0.0;
                pose.pose.orientation.z = 0.0;
                pose.pose.orientation.w = 1.0;
                
                // nurbs_path.push_back(pose);
                saved_poses.push_back(pose);
                if(saved_poses.size() > 1) 
                {
                    updatePoseOrientation(saved_poses, nurbs_path, reverse);
                }
            }
        }
        if(!saved_poses.empty()) 
        {
            double end_yaw;
            if(!reverse)
                end_yaw = calculateAngle(saved_poses.back().pose.position.x,
                                        saved_poses.back().pose.position.y,
                                        control_points.back().pose.position.x,
                                        control_points.back().pose.position.y);
            else 
                end_yaw = calculateAngle(control_points.back().pose.position.x,
                                        control_points.back().pose.position.y,
                                        saved_poses.back().pose.position.x,
                                        saved_poses.back().pose.position.y);
            end_yaw = normalizeAngle(end_yaw);

            tf3::Quaternion q;
            q.setRPY(0, 0, end_yaw);
            saved_poses.back().pose.orientation.x = q.x();
            saved_poses.back().pose.orientation.y = q.y();
            saved_poses.back().pose.orientation.z = q.z();
            saved_poses.back().pose.orientation.w = q.w();
            
            nurbs_path.push_back(saved_poses.back());
        }
        return nurbs_path;
    }

    double MergePathCalc::calculateNURBSLength(Curve_common* curve_design, 
                                            Spline_Inf* spline_inf,
                                            double initial_step) 
    {
        double length = 0.0;
        robot_geometry_msgs::Point prev_point, curr_point;
        std::vector<double> segment_lengths;
        
        static double step;
        // Đường cong bậc 2 trở lên thì bước nhảy bằng initial_step
        step = initial_step;

        // Tính độ dài với step
        for(double u = 0; u <= 1; u += step) 
        {
            // Lấy điểm trên đường cong tại tham số u
            curr_point = curve_design->CalculateCurvePoint(spline_inf, u, true);
            
            if(u > 0 && !std::isnan(curr_point.x) && !std::isnan(curr_point.y)) 
            {
                double segment_length = std::sqrt(std::pow(curr_point.x - prev_point.x, 2) + 
                                                std::pow(curr_point.y - prev_point.y, 2));
                length += segment_length;
                segment_lengths.push_back(segment_length);
            }
            prev_point = curr_point;
        }
        return length;
    }

    void MergePathCalc::updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
                                            std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
                                            bool reverse) 
    {
        double yaw;
        // ROS_WARN("[merge_path_calc] DEBUG: reverse(%x)", reverse);
        if(!reverse)
            yaw = calculateAngle(saved_poses[saved_poses.size() - 2].pose.position.x,
                                saved_poses[saved_poses.size() - 2].pose.position.y,
                                saved_poses.back().pose.position.x,
                                saved_poses.back().pose.position.y);
        else
            yaw = calculateAngle(saved_poses.back().pose.position.x,
                                saved_poses.back().pose.position.y,
                                saved_poses[saved_poses.size() - 2].pose.position.x,
                                saved_poses[saved_poses.size() - 2].pose.position.y);

        yaw = normalizeAngle(yaw);
        tf3::Quaternion q;
        q.setRPY(0, 0, yaw);
        saved_poses[saved_poses.size() - 2].pose.orientation.x = q.x();
        saved_poses[saved_poses.size() - 2].pose.orientation.y = q.y();
        saved_poses[saved_poses.size() - 2].pose.orientation.z = q.z();
        saved_poses[saved_poses.size() - 2].pose.orientation.w = q.w();

        nurbs_path.push_back(saved_poses[saved_poses.size() - 2]);
    }

    int MergePathCalc::findNearestPointOnPath(Pose& pose, 
                                        std::vector<Pose>& posesOnPathWay,
                                        point_type type)
    {
        // Input validation - FIX: Thêm validation cơ bản
        if (posesOnPathWay.empty()) {
            return -1;
        }

        // Các hằng số chung cho cả START và GOAL
        static const double MAX_DISTANCE = 1.0;  // Ngưỡng khoảng cách tối đa (m)
        static const double MIN_DISTANCE = 0.1;  // Khoảng cách tối thiểu (m)
        static const double MIN_SCALE = 0.2;     // Giá trị tối thiểu của hệ số
        static const double MAX_SCALE = 1.0;     // Giá trị tối đa của hệ số
        static const double MAX_ANGLE_THRESHOLD = 0.77 * M_PI; // Ngưỡng góc tối đa
        
        int nearest_idx = 0;
        
        if(type == START) 
        {
            // int idx_check = edge_front_end_idx_;
            int idx_check = static_cast<int>(posesOnPathWay.size());
            // FIX: Validate edge_front_end_idx_ trước khi sử dụng
            if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
                return -1;
            }

            // === BƯỚC 1: TÌM ĐIỂM GẦN NHẤT CƠ BẢN ===
            double min_dist = std::numeric_limits<double>::max();
            double distance;
            
            for (int i = 0; i < idx_check; ++i) 
            {
                distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(), 
                                    posesOnPathWay[i].getY() - pose.getY());

                if (distance < min_dist) 
                {
                    min_dist = distance;
                    nearest_idx = i;
                }
            }

            // Lưu chỉ mục của điểm gần nhất
            store_start_nearest_idx_ = nearest_idx; 

            // === BƯỚC 2: TÍNH TOÁN HỆ SỐ SCALE ĐỘNG ===
            // Nội suy tuyến tính ngược: khoảng cách giảm → scale tăng
            double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) * 
                        (MAX_DISTANCE - std::min(min_dist, MAX_DISTANCE)) / 
                        (MAX_DISTANCE - MIN_DISTANCE);
            
            // === BƯỚC 3: TÍNH TOÁN NGƯỠNG GÓC ===
            double angle_threshold = (0.5 + std::abs(scale)) * 0.5 * M_PI;
            if(angle_threshold >= MAX_ANGLE_THRESHOLD) 
                angle_threshold = MAX_ANGLE_THRESHOLD;
            
            // === BƯỚC 4: TÌM ĐIỂM THỎA MÃN ĐIỀU KIỆN GÓC ===
            for(int i = store_start_nearest_idx_; i <= idx_check; i++)
            {
                // Bounds checking đầy đủ trước khi truy cập
                if (i + 1 < static_cast<int>(posesOnPathWay.size()) && i + 1 <= idx_check) 
                {
                    double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i + 1]);
                    
                    if(std::abs(angle) >= angle_threshold)
                    {
                        nearest_idx = i;
                        break;
                    }
                }
                
            
                if(i >= idx_check - 1)
                {
                    // Đã đến cuối mà không tìm thấy điểm phù hợp
                    return -1;  // Không tìm thấy điểm phù hợp
                }
            }
        }
        else if(type == GOAL) 
        {
            // FIX: Validate edge_back_start_idx_ trước khi sử dụng
            if (edge_back_start_idx_ < 0 || edge_back_start_idx_ >= static_cast<int>(posesOnPathWay.size())) {
                return -1;
            }

            // === BƯỚC 1: TÌM ĐIỂM GẦN NHẤT CƠ BẢN (DUYỆT NGƯỢC) ===
            double min_dist = std::numeric_limits<double>::max();
            
            for (int i = static_cast<int>(posesOnPathWay.size()) - 1; i >= edge_back_start_idx_; --i) 
            {
                double distance = std::hypot(posesOnPathWay[i].getX() - pose.getX(), 
                                        posesOnPathWay[i].getY() - pose.getY());
                if (distance < min_dist) 
                {
                    min_dist = distance;
                    nearest_idx = i;
                }
            }

            // Lưu chỉ mục của điểm gần nhất
            store_goal_nearest_idx_ = nearest_idx; 
            
            // === BƯỚC 2: TÍNH TOÁN HỆ SỐ SCALE ĐỘNG (GIỐNG START) ===
            // Nội suy tuyến tính ngược: khoảng cách giảm → scale tăng
            double scale = MIN_SCALE + (MAX_SCALE - MIN_SCALE) * 
                        (MAX_DISTANCE - std::min(min_dist, MAX_DISTANCE)) / 
                        (MAX_DISTANCE - MIN_DISTANCE);
            
            // === BƯỚC 3: TÍNH TOÁN NGƯỠNG GÓC (GIỐNG START) ===
            double angle_threshold = (0.5 + std::abs(scale)) * 0.5 * M_PI;
            if(angle_threshold >= MAX_ANGLE_THRESHOLD) 
                angle_threshold = MAX_ANGLE_THRESHOLD;
            
            
            // === BƯỚC 4: TÌM ĐIỂM THỎA MÃN ĐIỀU KIỆN GÓC (DUYỆT NGƯỢC) ===
            // Bắt đầu từ điểm đã tìm được, duyệt ngược để tìm điểm thỏa mãn góc
            for(int i = nearest_idx; i >= edge_back_start_idx_; i--)
            {
                // FIX: Bounds checking đầy đủ trước khi truy cập
                if (i - 1 >= edge_back_start_idx_ && i - 1 >= 0) 
                {
                    double angle = signedAngle(pose, posesOnPathWay[i], posesOnPathWay[i - 1]);
                    
                    if(std::abs(angle) >= angle_threshold)
                    {
                        nearest_idx = i;
                        break;
                    }
                }
                
                // FIX: Logic condition sửa lại để rõ ràng
                if(i <= edge_back_start_idx_ + 1)
                {
                    // Đã đến đầu mà không tìm thấy điểm phù hợp
                    return -1;
                }
            }
            
        }
        // FIX: Thêm else case để handle invalid type
        else {
            return -1;
        }
        
        return nearest_idx;
    }
    
    bool MergePathCalc::findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
                                        const robot_geometry_msgs::PoseStamped& pose,
                                        std::vector<Pose>& posesOnPathWay,
                                        point_type type)
    {
        // FIX: Input validation
        if (posesOnPathWay.empty()) return false;

        // FIX: Chỉ clear một lần duy nhất thay vì clear nhiều lần
        control_points.clear();
        
        if(type == START)
        {
            Pose start_pose;
            start_pose.setX(pose.pose.position.x);
            start_pose.setY(pose.pose.position.y);
            start_pose.setYaw(data_convert::getYaw(pose.pose.orientation));

            int idx = findNearestPointOnPath(start_pose, posesOnPathWay, START);

            // int idx_check = edge_front_end_idx_;
            int idx_check = static_cast<int>(posesOnPathWay.size());
            
            // Trường hợp tạo đường thẳng
            if(idx == -1)
            {
                // FIX: Validate idx_check trước khi sử dụng
                if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
                    return false;
                }
                
                start_target_idx_ = idx_check - 1;

                double dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX();
                double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY();
                
                // Thêm điểm đầu
                control_points.push_back(pose);
                
                // Với bậc 2, thêm 1 điểm trung gian nằm trên đường thẳng
                robot_geometry_msgs::PoseStamped mid_pose;
                mid_pose.pose.position.x = start_pose.getX() + dx/2.0;
                mid_pose.pose.position.y = start_pose.getY() + dy/2.0;
                mid_pose.pose.orientation = pose.pose.orientation;
                // FIX: Copy header từ pose gốc
                mid_pose.header = pose.header;
                control_points.push_back(mid_pose);
                
                // Thêm điểm cuối
                robot_geometry_msgs::PoseStamped end_pose;
                end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
                end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
                end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
                // FIX: Copy header từ pose gốc
                end_pose.header = pose.header;
                control_points.push_back(end_pose);
                
                return true;
            }
            // Tìm đường cong
            else
            {
                start_target_idx_ = idx;

                // Tạo các control point cho NURBS
                control_points.push_back(pose);
                
                robot_geometry_msgs::PoseStamped mid_pose;
                mid_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
                mid_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
                mid_pose.pose.orientation = pose.pose.orientation;
                // FIX: Copy header từ pose gốc
                mid_pose.header = pose.header;
                control_points.push_back(mid_pose);
                

                double dx_sm = start_pose.getX() - posesOnPathWay[start_target_idx_].getX();
                double dy_sm = start_pose.getY() - posesOnPathWay[start_target_idx_].getY();
                double dist_sm = std::hypot(dx_sm,dy_sm);

                // FIX: Bỏ biến end_idx_ không sử dụng
                // end_idx_ = start_target_idx_;

                bool found_suitable_point = false; // FIX: Thêm flag để tránh goto
                for(int i = start_target_idx_; i <= idx_check; i++)
                {
                    // FIX: Validate bounds trước khi truy cập
                    if (i >= static_cast<int>(posesOnPathWay.size())) {
                        break;
                    }
                    
                    double dx_me = posesOnPathWay[i].getX() - posesOnPathWay[start_target_idx_].getX();
                    double dy_me = posesOnPathWay[i].getY() - posesOnPathWay[start_target_idx_].getY();
                    double dist_me = std::hypot(dx_me,dy_me);
                    if(dist_me >= dist_sm)
                    {
                        start_target_idx_ = i;
                        found_suitable_point = true;
                        break;
                    }
                    if(i == idx_check)
                    {
                        // FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line
                        found_suitable_point = false;
                        break;
                    }
                }
                
                // FIX: Xử lý trường hợp không tìm thấy điểm phù hợp bằng cách tạo straight line
                if (!found_suitable_point) {
                    // Clear và tạo straight line thay vì goto
                    control_points.clear();
                    
                    // int idx_check = edge_front_end_idx_;
                    int idx_check = static_cast<int>(posesOnPathWay.size());
                    // FIX: Validate lại edge_front_end_idx_
                    if (idx_check <= 0 || idx_check > static_cast<int>(posesOnPathWay.size())) {
                        return false;
                    }
                    
                    start_target_idx_ = idx_check - 1;
                    
                    double dx = posesOnPathWay[start_target_idx_].getX() - start_pose.getX();
                    double dy = posesOnPathWay[start_target_idx_].getY() - start_pose.getY();
                    
                    control_points.push_back(pose);
                    
                    robot_geometry_msgs::PoseStamped mid_pose_straight;
                    mid_pose_straight.pose.position.x = start_pose.getX() + dx/2.0;
                    mid_pose_straight.pose.position.y = start_pose.getY() + dy/2.0;
                    mid_pose_straight.pose.orientation = pose.pose.orientation;
                    mid_pose_straight.header = pose.header;
                    control_points.push_back(mid_pose_straight);
                    
                    robot_geometry_msgs::PoseStamped end_pose_straight;
                    end_pose_straight.pose.position.x = posesOnPathWay[start_target_idx_].getX();
                    end_pose_straight.pose.position.y = posesOnPathWay[start_target_idx_].getY();
                    end_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[start_target_idx_].getYaw());
                    end_pose_straight.header = pose.header;
                    control_points.push_back(end_pose_straight);
                    
                    return true;
                }
                
                robot_geometry_msgs::PoseStamped end_pose;
                end_pose.pose.position.x = posesOnPathWay[start_target_idx_].getX();
                end_pose.pose.position.y = posesOnPathWay[start_target_idx_].getY();
                end_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(0.0);
                // FIX: Copy header từ pose gốc
                end_pose.header = pose.header;
                control_points.push_back(end_pose);

                return true;
            }
        }
        else if(type == GOAL) 
        {
            Pose goal_pose;
            goal_pose.setX(pose.pose.position.x);
            goal_pose.setY(pose.pose.position.y);
            goal_pose.setYaw(data_convert::getYaw(pose.pose.orientation));
            
            int idx = findNearestPointOnPath(goal_pose, posesOnPathWay, GOAL);
            
            if(idx == -1) 
            {
                // Trường hợp tạo đường thẳng
                // FIX: Validate size trước khi sử dụng
                if (posesOnPathWay.empty()) {
                    return false;
                }
                
                goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
                
                // Thêm điểm đầu từ đường đã có
                robot_geometry_msgs::PoseStamped start_pose;
                start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
                start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
                start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
                // FIX: Copy header từ pose gốc
                start_pose.header = pose.header;
                control_points.push_back(start_pose);
                
                // Vector hướng từ goal tới start
                double dx = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
                double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
                
                // Thêm 1 điểm trung gian cho bậc 2
                robot_geometry_msgs::PoseStamped mid_pose;
                mid_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx/2.0;
                mid_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy/2.0;
                mid_pose.pose.orientation = start_pose.pose.orientation;
                // FIX: Copy header từ pose gốc
                mid_pose.header = pose.header;
                control_points.push_back(mid_pose);
                
                // Thêm điểm goal
                control_points.push_back(pose);
                return true;
            } 
            else 
            {
                // Trường hợp tạo đường cong
                goal_target_idx_ = idx;

                double dx = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
                double dy = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
                double dist_me = std::hypot(dx, dy);
                
                bool found_suitable_point = false; // FIX: Thêm flag để tránh goto
                for(int i = goal_target_idx_; i >= edge_back_start_idx_; i--)
                {
                    // FIX: Validate bounds trước khi truy cập
                    if (i < 0 || i >= static_cast<int>(posesOnPathWay.size())) {
                        break;
                    }
                    
                    double dx_sm = posesOnPathWay[i].getX() - posesOnPathWay[goal_target_idx_].getX();
                    double dy_sm = posesOnPathWay[i].getY() - posesOnPathWay[goal_target_idx_].getY();
                    double dist_sm = std::hypot(dx_sm,dy_sm);
                    if(dist_sm >= dist_me)
                    {
                        goal_target_idx_ = i;
                        found_suitable_point = true;
                        break;
                    }
                    if(i == edge_back_start_idx_)
                    {
                        // FIX: Thay vì goto, sử dụng flag để xử lý trường hợp straight line
                        found_suitable_point = false;
                        break;
                    }
                }
                
                // FIX: Xử lý trường hợp không tìm thấy điểm phù hợp bằng cách tạo straight line
                if (!found_suitable_point) {
                    // Clear và tạo straight line thay vì goto
                    control_points.clear();
                    
                    // FIX: Validate lại size
                    if (posesOnPathWay.empty()) {
                        return false;
                    }
                    
                    goal_target_idx_ = static_cast<int>(posesOnPathWay.size()) - 1;
                    
                    robot_geometry_msgs::PoseStamped start_pose_straight;
                    start_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
                    start_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
                    start_pose_straight.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
                    start_pose_straight.header = pose.header;
                    control_points.push_back(start_pose_straight);
                    
                    double dx_straight = goal_pose.getX() - posesOnPathWay[goal_target_idx_].getX();
                    double dy_straight = goal_pose.getY() - posesOnPathWay[goal_target_idx_].getY();
                    
                    robot_geometry_msgs::PoseStamped mid_pose_straight;
                    mid_pose_straight.pose.position.x = posesOnPathWay[goal_target_idx_].getX() + dx_straight/2.0;
                    mid_pose_straight.pose.position.y = posesOnPathWay[goal_target_idx_].getY() + dy_straight/2.0;
                    mid_pose_straight.pose.orientation = start_pose_straight.pose.orientation;
                    mid_pose_straight.header = pose.header;
                    control_points.push_back(mid_pose_straight);
                    
                    control_points.push_back(pose);
                    return true;
                }
                
                // Thêm điểm đầu từ đường đã có
                robot_geometry_msgs::PoseStamped start_pose;
                start_pose.pose.position.x = posesOnPathWay[goal_target_idx_].getX();
                start_pose.pose.position.y = posesOnPathWay[goal_target_idx_].getY();
                start_pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(posesOnPathWay[goal_target_idx_].getYaw());
                // FIX: Copy header từ pose gốc
                start_pose.header = pose.header;
                control_points.push_back(start_pose);

                robot_geometry_msgs::PoseStamped mid_pose;
                mid_pose.pose.position.x = posesOnPathWay[idx].getX();
                mid_pose.pose.position.y = posesOnPathWay[idx].getY();
                mid_pose.pose.orientation = pose.pose.orientation;
                // FIX: Copy header từ pose gốc
                mid_pose.header = pose.header;
                control_points.push_back(mid_pose);
                
                // Thêm điểm goal
                control_points.push_back(pose);
                return true;
            }
        }
        // FIX: Thêm else case để handle invalid type
        else {
            return false;
        }
        
        // FIX: Code không bao giờ reach được đây, nhưng để đảm bảo
        // return false; // Không tìm thấy control points
    }

    double MergePathCalc::signedAngle(Pose& pose, std::vector<Pose>& posesOnPathWay,
                                    point_type type, int& idx)
    {
        if(type == START)
        {
            // Vector AB
            double bax = pose.getX() - posesOnPathWay[idx].getX();
            double bay = pose.getY() - posesOnPathWay[idx].getY();
        
            // Vector BC
            double bcx = posesOnPathWay[idx+1].getX() - posesOnPathWay[idx].getX();
            double bcy = posesOnPathWay[idx+1].getY() - posesOnPathWay[idx].getY();

            // Tích vô hướng
            double dot = bax * bcx + bay * bcy;
            // Tích hướng 2D (z-component)
            double cross = bax * bcy - bay * bcx;

            // Góc có hướng [-π, π]
            double angle_rad = std::atan2(cross, dot);
            return angle_rad;
        }
        else if(type == GOAL)
        {
            // Vector AB
            double bax = posesOnPathWay[idx-1].getX() - posesOnPathWay[idx].getX();
            double bay = posesOnPathWay[idx-1].getY() - posesOnPathWay[idx].getY();
        
            // Vector BC
            double bcx = pose.getX() - posesOnPathWay[idx].getX();
            double bcy = pose.getY() - posesOnPathWay[idx].getY();

            // Tích vô hướng
            double dot = bax * bcx + bay * bcy;
            // Tích hướng 2D (z-component)
            double cross = bax * bcy - bay * bcx;

            // Góc có hướng [-π, π]
            double angle_rad = std::atan2(cross, dot);
            return angle_rad;
        }
        else return -1; // Trả về -1 nếu không phải START hoặc GOAL
            
    }

    double MergePathCalc::signedAngle(Pose& start_pose, Pose& mid_pose, Pose& end_pose)
    {
      // Vector AB
      double bax = mid_pose.getX() - start_pose.getX();
      double bay = mid_pose.getY() - start_pose.getY();
  
      // Vector BC
      double bcx = mid_pose.getX() - end_pose.getX();
      double bcy = mid_pose.getY() - end_pose.getY();
  
      // Tích vô hướng
      double dot = bax * bcx + bay * bcy;
      // Tích hướng 2D (z-component)
      double cross = bax * bcy - bay * bcx;
  
      // Góc có hướng [-π, π]
      double angle_rad = std::atan2(cross, dot);
      return angle_rad;
    }
    
} // namespace custom_planner