#ifndef MERGER_PATH_CALC_H_
#define MERGER_PATH_CALC_H_
#include <robot_geometry_msgs/PoseStamped.h>
#include <vector>
#include <cmath>

#include <tf3/convert.h>

#include "custom_planner/pose.h"
#include "custom_planner/Curve_common.h"
#include <data_convert/data_convert.h>

namespace custom_planner
{
    //Lưu thông tin các cạnh
    struct InfEdge{
        int start_edge_idx;
        int end_edge_idx;
        bool isCurve;
        double lenght_edge;
    };
    
    //Lưu trữ gosc chuyển hướng
    struct RobotDirectionChangeAngle{
        int start_edge_idx_first;
        int end_edge_idx_first;
        int start_edge_idx_second;
        int end_edge_idx_second;
    };
  
    enum point_type
    {
        START = 0,
        GOAL = 1,
        UNKNOWN = 3
    };
    class MergePathCalc
    {
    public:
        MergePathCalc();
        ~MergePathCalc();

        /**
         * \brief Tính toán bước nhảy thích hợp dựa trên độ dài đoạn và khoảng cách điểm mong muốn.
         * \param segment_length Độ dài của đoạn đường.
         * \param desired_point_spacing Khoảng cách giữa các điểm mong muốn (mặc định là 0.02).
         * \return Trả về bước nhảy thích hợp để lấy mẫu điểm trên đoạn đường.
         * \details
         * Hàm này sẽ tính toán bước nhảy dựa trên độ dài đoạn đường và khoảng cách giữa các điểm mong muốn.
         */
        inline double calculateAdaptiveStep(double segment_length, 
                                            double desired_point_spacing = 0.02) 
        {
        return desired_point_spacing / segment_length;
        }

        /**
         * \brief Tìm điểm gần nhất trên đường đi.
         * \param pose Vị trí hiện tại của robot.
         * \param posesOnPathWay Các điểm trên đường đi.
         * \return Trả về chỉ mục của điểm gần nhất trong posesOnPathWay.
         * \details
         * Hàm này sẽ tìm điểm gần nhất trên đường đi so với vị trí hiện tại của robot.
         * Nếu khoảng cách đến điểm gần nhất nhỏ hơn biding_distance, nó sẽ
         * tiếp tục tìm điểm xa hơn để đảm bảo ràng buộc khoảng cách.
         * Nếu tìm thấy, nó sẽ trả về chỉ mục của điểm gần nhất trong posesOnPathWay.
         * Nếu không tìm thấy điểm nào thỏa mãn, nó sẽ trả về -1.
         */
        int findNearestPointOnPath(Pose& pose, 
                                std::vector<Pose>& posesOnPathWay,
                                point_type type);

        /**
        * \brief Tính độ dài của đường NURBS.
        * \param curve_design Con trỏ đến đối tượng CurveCommon chứa các hàm tính toán đường cong.
        * \param spline_inf Con trỏ đến đối tượng Spline_Inf chứa thông tin về đường cong NURBS.
        * \param degree Bậc của đường NURBS (1, 2 hoặc 3).
        * \param initial_step Bước nhảy ban đầu để lấy mẫu điểm trên đường cong.
        * \return Trả về độ dài của đường NURBS.
        * \details
        * Hàm này sẽ tính độ dài của đường NURBS bằng cách lấy mẫu các điểm trên đường cong
        * và tính khoảng cách giữa các điểm liên tiếp. Nó sẽ trả về tổng độ dài của đường cong.
        */
        double calculateNURBSLength(Curve_common* curve_design, 
                                    Spline_Inf* spline_inf,
                                    double initial_step = 0.01);

        /**
         * \brief Tìm các điểm điều khiển NURBS.
         * \param start Vị trí bắt đầu.
         * \param goal Vị trí kết thúc.
         * \param posesOnPathWay Các điểm trên đường đi.
         * \param degree Bậc của đường cong NURBS (2 hoặc 3).
         * \return Trả về một vector chứa các điểm điều khiển NURBS.
         * \details
         * Hàm này tìm các điểm điều khiển NURBS dựa trên vị trí bắt đầu, kết thúc và các điểm trên đường đi.
         * Nó sử dụng bậc của đường cong NURBS để xác định số lượng điểm điều khiển cần thiết.
         */
        bool findNURBSControlPoints(vector<robot_geometry_msgs::PoseStamped>& control_points,
                                    const robot_geometry_msgs::PoseStamped& start, 
                                    std::vector<Pose>& posesOnPathWay,
                                    point_type type);
        
        /**
         * \brief Tính toán đường đi NURBS.
         * \param degree Bậc của đường NURBS (2 hoặc 3).
         * \return Trả về một vector chứa các PoseStamped trên đường đi NURBS.
         * \details
         * Hàm này sẽ tính toán đường đi NURBS dựa trên các điểm điều khiển đã tìm được.
         * Nó sẽ sử dụng các hàm từ CurveCommon và Spline_Inf để tính toán các điểm trên đường cong.
         */
        std::vector<robot_geometry_msgs::PoseStamped> calculateNURBSPath(vector<robot_geometry_msgs::PoseStamped>& control_points,
                                                                bool reverse);

        bool handleEdgeIntersection(vector<Pose>& savePosesOnEdge, 
                                    vector<Pose>& posesOnEdge, 
                                    size_t current_edge_idx);

        /**
         * \brief Tính toán góc giữa hai điểm.
         * \param x1 Tọa độ x của điểm đầu tiên.
         * \param y1 Tọa độ y của điểm đầu tiên.
         * \param x2 Tọa độ x của điểm thứ hai.
         * \param y2 Tọa độ y của điểm thứ hai.
         * \return Trả về góc tính bằng radian từ điểm đầu đến điểm thứ hai.
         * \details
         * Hàm này sẽ tính toán góc giữa hai điểm (x1, y1) và (x2, y2) theo công thức atan2.
         */
        inline double calculateAngle(double x1, double y1, double x2, double y2)
        {
            double angle = atan2(y2 - y1, x2 - x1);
            return angle;
        }

        inline double normalizeAngle(double angle) 
        {
            while (angle > M_PI)
                angle -= 2.0 * M_PI;
            while (angle <= -M_PI)
                angle += 2.0 * M_PI;
            return angle;
        }

        /**
         * \brief Tính góc có hướng giữa hai vector.
         * \param start_pose Vị trí bắt đầu.
         * \param posesOnPathWay Các điểm trên đường đi.
         * \param idx Chỉ mục của điểm cần tính trong posesOnPathWay.
         * \return Trả về góc có hướng giữa hai vector.
         * \details
         * Hàm này sẽ tính góc có hướng giữa vector từ start_pose đến điểm hiện tại
         * và vector từ điểm hiện tại đến điểm tiếp theo trong posesOnPathWay.
         */
        double signedAngle(Pose& start_pose, 
                        std::vector<Pose>& posesOnPathWay, 
                        point_type type,
                        int& idx);
        
        double signedAngle(Pose& start_pose, Pose& mid_pose, Pose& end_pose);

        // Chỉ mục của điểm mục tiêu trong posesOnPathWay
        int start_target_idx_ = 0;
        int goal_target_idx_ = 0;
        int store_start_nearest_idx_ = 0;
        int store_goal_nearest_idx_ = 0;
        int end_idx_;

        int edge_front_start_idx_ = 0;
        int edge_front_end_idx_ = 0;
        int edge_back_start_idx_ = 0;
        int edge_back_end_idx_ = 0;

        bool status_yaw_edge_end_ = true;


    private:
        // vector<robot_geometry_msgs::PoseStamped> control_points;
        
        bool normal_plag = false;

        // CurveCommon* curve_design = nullptr;
        // Spline_Inf* spline_inf = nullptr;
        Spline_Inf* spline_inf = nullptr;
        Curve_common* curve_design = nullptr;
        
        /**
         * \brief Cập nhật hướng của các Pose trong đường đi NURBS.
         * \param saved_poses Vector chứa các Pose đã lưu.
         * \param nurbs_path Vector chứa đường đi NURBS.
         * \details
         * Hàm này sẽ cập nhật hướng (yaw) của các Pose trong saved_poses dựa trên hướng của đường đi NURBS.
         * Nó sẽ tính toán góc yaw dựa trên vị trí của các Pose và cập nhật chúng.
         */
        void updatePoseOrientation(std::vector<robot_geometry_msgs::PoseStamped>& saved_poses,
                                std::vector<robot_geometry_msgs::PoseStamped>& nurbs_path,
                                bool reverse);

        /**
         * \brief Thiết lập hướng (yaw) của các Pose trên đường NuRBS.
         * \param Poses Vector chứa các PoseStamped.
         * \param reverse Nếu true, sẽ đảo ngược hướng của các Pose.
         * \details
         * Hàm này sẽ thiết lập hướng (yaw) cho tất cả các Pose trong vector Poses.
         * Nếu reverse là true, nó sẽ đảo ngược hướng của các Pose.
         * Nếu reverse là false, nó sẽ giữ nguyên hướng của các Pose.
         */
        static void setYawAllPosesOnEdge(std::vector<robot_geometry_msgs::PoseStamped>& Poses, 
                                        bool reverse = false);
        

    };

} // namespace custom_planner
#endif // MERGE_PATH_CALC_H_