#include <robot/robot.h>
#include <two_points_planner/two_points_planner.h>
#include <robot_costmap_2d/inflation_layer.h>
#include <boost/dll/alias.hpp>
#include <robot_nav_msgs/Path.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_utils/conversions.h>
#include <tf3/LinearMath/Quaternion.h>
#include <tf3/utils.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <cmath>
#include <cstdio>

namespace two_points_planner 
{
  TwoPointsPlanner::TwoPointsPlanner()
    : initialized_(false),
      lethal_obstacle_(0),
      inscribed_inflated_obstacle_(0),
      circumscribed_cost_(0),
      allow_unknown_(false),
      costmap_robot_(nullptr),
      current_env_width_(0),
      current_env_height_(0)
  {
  }

  TwoPointsPlanner::TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
    : initialized_(false), costmap_robot_(NULL)
  {
    initialize(name, costmap_robot);
  }

  bool TwoPointsPlanner::initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
  {
    if (!initialized_)
    {
      robot::NodeHandle nh_priv_("~/" + name);
      int lethal_obstacle;
      nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20);
      lethal_obstacle_ = (unsigned char)lethal_obstacle;
      inscribed_inflated_obstacle_ = lethal_obstacle_ - 1;

      nh_priv_.getParam("allow_unknown", allow_unknown_, true);

      name_ = name;
      costmap_robot_ = costmap_robot;

      
      
      if(!costmap_robot_ || !costmap_robot_->getCostmap()) 
      {
        robot::log_warning("[%s:%d]TwoPointsPlanner Initialized failed", __FILE__, __LINE__);
        return false;
      }
      current_env_width_ = costmap_robot_->getCostmap()->getSizeInCellsX();
      current_env_height_ = costmap_robot_->getCostmap()->getSizeInCellsY();
      footprint_ = costmap_robot_->getRobotFootprint();

      robot::log_info("size x: %d, size y: %d, resolution: %f", costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY(), costmap_robot_->getCostmap()->getResolution());

      robot::log_info("TwoPointsPlanner Initialized successfully");
      initialized_ = true;
      return true;
    }
    robot::log_info("[%s:%d]\n TwoPointsPlanner: Already initialized", __FILE__, __LINE__);
    return true;
  }

  unsigned char TwoPointsPlanner::computeCircumscribedCost()
  {
    unsigned char result = 0;


    if (!costmap_robot_)
    {
      std::cerr << "TwoPointsPlanner: Costmap is not initialized" << std::endl;
      return 0;
    }

    // Bug 
    // // check if the costmap has an inflation layer
    // for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
    //   layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
    //   ++layer)
    // {
    //   boost::shared_ptr<robot_costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<robot_costmap_2d::InflationLayer>(*layer);
    //   if (!inflation_layer)
    //     continue;

    //   result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution()));
    // }
    return result;
  }

  // Taken from Sachin's sbpl_cart_planner
  // This rescales the costmap according to a rosparam which sets the obstacle cost
  unsigned char TwoPointsPlanner::costMapCostToSBPLCost(unsigned char newcost)
  {
    if (newcost == robot_costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == robot_costmap_2d::NO_INFORMATION))
      return lethal_obstacle_;
    else if (newcost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
      return inscribed_inflated_obstacle_;
    else if (newcost == 0)
      return 0;
    else if (newcost == robot_costmap_2d::NO_INFORMATION)
      return  robot_costmap_2d::FREE_SPACE;
    else
    {
      unsigned char cost = newcost;
      if (cost == 0)
        cost = 1;
      return cost;
    }
  }

  bool TwoPointsPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
    const robot_geometry_msgs::PoseStamped& goal,
    std::vector<robot_geometry_msgs::PoseStamped>& plan)
  {
    if (!initialized_)
    {
      robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
      return false;
    }

    robot::Time start_time = robot::Time::now();
    robot::Rate rate(1.0);
    while(costmap_robot_->getCostmap()->getSizeInCellsX() == 0 || costmap_robot_->getCostmap()->getSizeInCellsY() == 0){
      robot::log_warning("Waiting for costmap to be initialized...");
      rate.sleep();
      if((robot::Time::now() - start_time).toSec() > 5.0){
        robot::log_error("Costmap not initialized after 10 seconds, exiting...");
        exit(1);
      }
    }
    robot::log_warning("abc testttttt!, SizeInCellsX = %d, SizeInCellsY = %d", costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
    
    robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start);
    robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);
    robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", 
      start_2d.pose.x, start_2d.pose.y, start_2d.pose.theta, goal_2d.pose.x, goal_2d.pose.y, goal_2d.pose.theta);

    bool do_init = false;
    if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
      current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY())
    {
      robot::log_warning("[%s:%d]\n TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.", __FILE__, __LINE__,
        current_env_width_, current_env_height_,
        costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
      do_init = true;
    }
    else if (footprint_ != costmap_robot_->getRobotFootprint())
    {
      robot::log_warning("[%s:%d]\n TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.", __FILE__, __LINE__);
      do_init = true;
    }

    if (do_init)
    {
      initialized_ = false;
      initialize(name_, costmap_robot_);
    }

    plan.clear();
    // plan.push_back(start);
    
    // unsigned int mx_start, my_start;
    // unsigned int mx_end, my_end;
    // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
    //   start.pose.position.y,
    //   mx_start, my_start)

    //   || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
    //     goal.pose.position.y,
    //     mx_end, my_end))
    // {
    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
    //   return false;
    // }
    // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
    // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
    // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
    //   || end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
    // {
    //   robot::log_error("[%s:%d]\n TwoPointsPlanner: base_pootprint in infated obstacle", __FILE__, __LINE__);
    //   return false;
    // }
    robot::Time plan_time = robot::Time::now();
    // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
    const double dx = goal.pose.position.x - start.pose.position.x;
    const double dy = goal.pose.position.y - start.pose.position.y;
    const double distance = std::sqrt(dx * dx + dy * dy);
    double theta;
    // Lấy độ phân giải của costmap
    double resolution = costmap_robot_->getCostmap()->getResolution();
    
    if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
    {
      theta = std::atan2(dy, dx);
      // tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y, 
      //                           goal.pose.orientation.z, goal.pose.orientation.w);
      // double goal_theta = tf3::getYaw(goal_quat);
      double goal_theta = robot_nav_2d_utils::poseStampedToPose2D(goal).pose.theta;
      if(cos(goal_theta - theta) < 0) theta += M_PI;
    }
    else 
    { 
      robot_geometry_msgs::PoseStamped pose = start;
      pose.pose.position.x += resolution * cos(theta);
      pose.pose.position.y += resolution * sin(theta);
      plan.push_back(pose);
      plan.push_back(goal);
      return true;
    }
    // Tính số điểm cần chia
    int num_points = std::ceil(distance / resolution);

    // Chia nhỏ tuyến đường
    for (int i = 0; i <= num_points; i++)
    {
      double fraction = static_cast<double>(i) / num_points;

      robot_nav_2d_msgs::Pose2DStamped pose2d;
      pose2d.header.stamp = plan_time;
      pose2d.header.frame_id = costmap_robot_->getGlobalFrameID();
      pose2d.pose.x = start.pose.position.x + fraction * dx;
      pose2d.pose.y = start.pose.position.y + fraction * dy;
      pose2d.pose.theta = theta;

      // Nội suy hướng
      robot_geometry_msgs::PoseStamped pose = robot_nav_2d_utils::pose2DToPoseStamped(pose2d);
      plan.push_back(pose);
    }
    plan.push_back(goal);
    return true;
  }

  robot_nav_core::BaseGlobalPlanner::Ptr TwoPointsPlanner::create()
  {
    return std::make_shared<TwoPointsPlanner>();
  }

} // namespace two_points_planner

// Register this planner as a GlobalPlanner plugin
BOOST_DLL_ALIAS(two_points_planner::TwoPointsPlanner::create, TwoPointsPlanner)