#include <robot/console.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 <tf3/LinearMath/Quaternion.h>
#include <tf3/utils.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot/time.h>
#include <robot/console.h>
#include <cmath>
#include <cstdio>

namespace two_points_planner 
{
  TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {}

  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);
      robot::log_info("TwoPointsPlanner: Name is %s", name.c_str());

      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("TwoPointsPlanner Initialized successfully");
      initialized_ = true;
      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_)
    {
      std::cerr << "TwoPointsPlanner: Global planner is not initialized" << std::endl;
      return false;
    }

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

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

    plan.clear();
    plan.push_back(start);
    printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n",
      start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);

    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))
    {
      std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl;
      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))
    {
      std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
      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;
    double distance = std::sqrt(dx * dx + dy * dy);
    double theta;
    if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
    {
      theta = 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);
      if(cos(goal_theta - theta) < 0) theta += M_PI;
    }
    else 
    {
      std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl;
      return false;
    }

    // Lấy độ phân giải của costmap
    double resolution = costmap_robot_->getCostmap()->getResolution();

    // 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_geometry_msgs::PoseStamped pose;
      pose.header.stamp = plan_time;
      pose.header.frame_id = costmap_robot_->getGlobalFrameID();
      pose.pose.position.x = start.pose.position.x + fraction * dx;
      pose.pose.position.y = start.pose.position.y + fraction * dy;
      pose.pose.position.z = start.pose.position.z;

      // Nội suy hướng
      tf3::Quaternion interpolated_quat;
      interpolated_quat.setRPY(0, 0, theta);

      // Chuyển đổi trực tiếp từ tf3::Quaternion sang robot_geometry_msgs::Quaternion
      pose.pose.orientation.x = interpolated_quat.x();
      pose.pose.orientation.y = interpolated_quat.y();
      pose.pose.orientation.z = interpolated_quat.z();
      pose.pose.orientation.w = interpolated_quat.w();

      plan.push_back(pose);
    }
    plan.push_back(goal);
    return true;
  }

  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)