#include "dock_planner/dock_planner.h"
#include <cmath>
#include <algorithm>
#include <boost/dll/alias.hpp>

namespace dock_planner 
{
    /**
     * @brief Constructor mặc định
     * Khởi tạo tất cả các pointer về nullptr và trạng thái chưa initialized
     */
    DockPlanner::DockPlanner() : costmap_robot_(nullptr), 
                                costmap_(nullptr), 
                                initialized_(false), 
                                check_free_space_(false),
                                cost_threshold_(200),
                                calib_safety_distance_(0.05) {}

    /**
     * @brief Constructor với parameters
     * @param name Tên của planner
     * @param costmap_robot Pointer tới costmap ROS wrapper
     * Tự động gọi initialize() nếu được cung cấp costmap
     */
    DockPlanner::DockPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) 
        : costmap_robot_(nullptr), 
        costmap_(nullptr), 
        initialized_(false), 
        check_free_space_(false),
        cost_threshold_(200),
        calib_safety_distance_(0.05)
    {
        initialize(name, costmap_robot);
    }

    /**
     * @brief Khởi tạo global planner
     * @param name Tên của planner instance
     * @param costmap_robot Pointer tới costmap ROS wrapper
     * 
     * Thiết lập:
     * - Liên kết với costmap
     * - Tạo publisher cho visualization
     * - Load parameters từ ROS parameter server
     * - Đánh dấu trạng thái initialized
     */
    bool DockPlanner::initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) 
    {
        if (!initialized_) 
        {
            costmap_robot_ = costmap_robot;
            costmap_ = costmap_robot_->getCostmap();
            frame_id_ = costmap_robot_->getGlobalFrameID();
            calc_plan_to_dock_.costmap_ = costmap_robot_->getCostmap();
            
            robot::NodeHandle private_nh("~/" + name);

            // Load parameters from the private namespace
            private_nh.getParam("check_free_space", check_free_space_);
            private_nh.getParam("cost_threshold", cost_threshold_);
            private_nh.getParam("calib_safety_distance", calib_safety_distance_);

            calc_plan_to_dock_.cost_threshold_ = cost_threshold_;
            calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_;


            // plan_pub_ = private_nh.advertise<robot_nav_msgs::Path>("plan", 1);

            robot_nav_2d_msgs::Polygon2D footprint_dock ;//= robot_nav_2d_utils::footprintFromParams(private_nh,false);
            
            for(auto pt : footprint_dock.points)
            {
                robot_geometry_msgs::Point footprint_point;
                footprint_point.x = pt.x;
                footprint_point.y = pt.y;
                footprint_point.z = 0.0;
                footprint_dock_.push_back(footprint_point);
            }
            
            initialized_ = true;
            // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_);
        }
    }
    
    bool DockPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
                                const robot_geometry_msgs::PoseStamped& goal,
                                std::vector<robot_geometry_msgs::PoseStamped>& plan)
    {
        if(!initialized_) return false;
        if(!plan.empty()) plan.clear();
        vector<robot_geometry_msgs::Pose> planMoveToDock;
        std::vector<robot_geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();

        // footprint_dock_ = costmap_robot_->getRobotFootprint();
        int degree = 2;
        double step = 0.02;
        bool status_makePlanMoveToDock = calc_plan_to_dock_.makePlanMoveToDock(start, goal, footprint_robot, footprint_dock_, degree, step, planMoveToDock, true);
        robot::Time plan_time = robot::Time::now();

        if(!status_makePlanMoveToDock) return false;
        for(int i = 0; i < (int)planMoveToDock.size(); i++)
        {
          robot_geometry_msgs::PoseStamped pose;
          pose.header.stamp = plan_time;
          pose.header.frame_id = costmap_robot_->getGlobalFrameID();
          pose.pose.position.x = planMoveToDock[i].position.x;
          pose.pose.position.y = planMoveToDock[i].position.y;
          pose.pose.position.z = 0;
          pose.pose.orientation = planMoveToDock[i].orientation;
          plan.push_back(pose);
        }
        publishPlan(plan);
        return true;
    }

    // void DockPlanner::publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) 
    // {
    //     robot_nav_msgs::Path path_msg;
    //     path_msg.header.stamp = robot::Time::now();
    //     path_msg.header.frame_id = frame_id_;
    //     path_msg.poses = plan;
    //     plan_pub_.publish(path_msg);
    // }

    // Export factory function
    static boost::shared_ptr<robot_nav_core::BaseGlobalPlanner> dock_planner_plugin() {
        return boost::make_shared<dock_planner::DockPlanner>();
    }


} // namespace dock_planner

// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(dock_planner::dock_planner_plugin, DockPlanner)