#include <amr_control/amr_publiser.h>
#include <robot/robot.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <robot_costmap_2d/cost_values.h>
#include <boost/bind.hpp>


namespace amr_control
{

AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
  : nh_(nh),
    move_base_ptr_(move_base_ptr)
{
  // Initialize default topic names
  std::string global_costmap_topic = "/global_costmap/costmap";
  std::string local_costmap_topic = "/local_costmap/costmap";
  std::string cmd_vel_topic = "/cmd_vel";
  // Get topic names from parameter server if available
  nh_.param("global_costmap_topic", global_costmap_topic, global_costmap_topic);
  nh_.param("local_costmap_topic", local_costmap_topic, local_costmap_topic);
  nh_.param("cmd_vel_topic", cmd_vel_topic, cmd_vel_topic);
  // Initialize publishers with callback for new subscriptions
  global_planner_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
    global_costmap_topic, 1, 
    boost::bind(&AmrPublisher::onNewSubscriptionGlobal, this, _1));
  global_planner_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(global_costmap_topic + "_updates", 1);
  global_planner_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(global_costmap_topic + "/footprint", 1);
  global_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(global_costmap_topic + "/plan", 1);
  
  local_planner_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
    local_costmap_topic, 1, 
    boost::bind(&AmrPublisher::onNewSubscriptionLocal, this, _1));
  local_planner_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(local_costmap_topic + "_updates", 1);
  local_planner_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(local_costmap_topic + "/footprint", 1);
  local_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(local_costmap_topic + "/plan", 1);

  cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic, 1);
  cmd_vel_publishing_active_ = false;
  robot::log_info("[AmrPublisher] Initialized. Cmd vel topic: %s", cmd_vel_topic.c_str());
  // Initialize topic names in CostmapObject
  global_planner_obj_.topic_ = global_costmap_topic;
  local_planner_obj_.topic_ = local_costmap_topic;
  
  // Initialize other CostmapObject members
  global_planner_obj_.active_ = false;
  local_planner_obj_.active_ = false;

  // Initialize plan publishing flags
  global_plan_publishing_active_ = false;
  local_plan_publishing_active_ = false;


  robot::log_info("[AmrPublisher] Initialized. Global costmap topic: %s, Local costmap topic: %s",
                  global_costmap_topic.c_str(), local_costmap_topic.c_str());
  robot::log_info("[AmrPublisher] Global footprint topic: %s/footprint, Local footprint topic: %s/footprint",
                  global_costmap_topic.c_str(), local_costmap_topic.c_str());
              
}

AmrPublisher::~AmrPublisher()
{
  stopPublishing();
  stopCmdVelPublishing();
  stopPlanPublishing();
}

void AmrPublisher::convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid, 
                                              nav_msgs::OccupancyGrid &nav_grid)
{
  // Convert header
  nav_grid.header.seq = robot_grid.header.seq;
  nav_grid.header.frame_id = robot_grid.header.frame_id;
  nav_grid.header.stamp = ros::Time::now();
  
  // Convert map metadata
  nav_grid.info.map_load_time = ros::Time::now();
  nav_grid.info.resolution = robot_grid.info.resolution;
  nav_grid.info.width = robot_grid.info.width;
  nav_grid.info.height = robot_grid.info.height;

  // Convert origin
  nav_grid.info.origin.position.x = robot_grid.info.origin.position.x;
  nav_grid.info.origin.position.y = robot_grid.info.origin.position.y;
  nav_grid.info.origin.position.z = robot_grid.info.origin.position.z;
  nav_grid.info.origin.orientation.x = robot_grid.info.origin.orientation.x;
  nav_grid.info.origin.orientation.y = robot_grid.info.origin.orientation.y;
  nav_grid.info.origin.orientation.z = robot_grid.info.origin.orientation.z;
  nav_grid.info.origin.orientation.w = robot_grid.info.origin.orientation.w;

  // Copy occupancy data (both use int8_t, so direct copy is safe)
  nav_grid.data = robot_grid.data;
}

void AmrPublisher::convertToRobotOccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate &robot_grid_update, 
                                                      map_msgs::OccupancyGridUpdate &grid_update)
{
  // Convert header
  grid_update.header.stamp = ros::Time::now();
  grid_update.header.frame_id = robot_grid_update.header.frame_id;
  grid_update.x = robot_grid_update.x;
  grid_update.y = robot_grid_update.y;
  grid_update.width = robot_grid_update.width;
  grid_update.height = robot_grid_update.height;
  grid_update.data = robot_grid_update.data;
}

void AmrPublisher::convertToNavPolygonStamped(const robot_geometry_msgs::PolygonStamped &robot_footprint, 
                                               geometry_msgs::PolygonStamped &nav_footprint)
{
  // Convert header
  nav_footprint.header.seq = robot_footprint.header.seq;
  nav_footprint.header.frame_id = robot_footprint.header.frame_id;
  nav_footprint.header.stamp = ros::Time::now();
  
  // Convert polygon points
  nav_footprint.polygon.points.clear();
  nav_footprint.polygon.points.reserve(robot_footprint.polygon.points.size());
  for (const auto &robot_point : robot_footprint.polygon.points)
  {
    geometry_msgs::Point32 nav_point;
    nav_point.x = robot_point.x;
    nav_point.y = robot_point.y;
    nav_point.z = robot_point.z;
    nav_footprint.polygon.points.push_back(nav_point);
  }
}

void AmrPublisher::publishGlobalCostmap(double publish_rate)
{
  if (!move_base_ptr_)
  {
    robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish global costmap");
    return;
  }

  try
  {
    // Get global costmap data from move_base
    robot::move_base_core::PlannerDataOutput global_data = move_base_ptr_->getGlobalData();
    
    if(!global_data.is_costmap_updated)
    {
      nav_msgs::OccupancyGrid nav_grid;
      convertToNavOccupancyGrid(global_data.costmap, nav_grid);
      global_planner_obj_.pub_.publish(nav_grid);
    }
    else
    {
      map_msgs::OccupancyGridUpdate grid_update;
      convertToRobotOccupancyGridUpdate(global_data.costmap_update, grid_update);
      global_planner_obj_.update_pub_.publish(grid_update);
    }
    
    // Publish footprint
    publishGlobalFootprint();
    
    // robot::log_debug("[AmrPublisher] Published global costmap: %dx%d, frame_id='%s'", 
    //                  nav_grid.info.width, nav_grid.info.height, nav_grid.header.frame_id.c_str());
  }
  catch (const std::exception &e)
  {
    robot::log_error("[AmrPublisher] Error publishing global costmap: %s", e.what());
  }
}

void AmrPublisher::publishLocalCostmap(double publish_rate)
{
  if (!move_base_ptr_)
  {
    robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish local costmap");
    return;
  }

  try
  {
    // Get local costmap data from move_base
    robot::move_base_core::PlannerDataOutput local_data = move_base_ptr_->getLocalData();
    

    if(!local_data.is_costmap_updated)
    {
      nav_msgs::OccupancyGrid nav_grid;
      convertToNavOccupancyGrid(local_data.costmap, nav_grid);
      local_planner_obj_.pub_.publish(nav_grid);
    }
    else
    {
      map_msgs::OccupancyGridUpdate grid_update;
      convertToRobotOccupancyGridUpdate(local_data.costmap_update, grid_update);
      local_planner_obj_.update_pub_.publish(grid_update);
    }
    
    // Publish footprint
    publishLocalFootprint();
    
    // robot::log_debug("[AmrPublisher] Published local costmap: %dx%d, frame_id='%s'", 
    //                  nav_grid.info.width, nav_grid.info.height, nav_grid.header.frame_id.c_str());
  }
  catch (const std::exception &e)
  {
    robot::log_error("[AmrPublisher] Error publishing local costmap: %s", e.what());
  }
}

void AmrPublisher::publishCostmaps(double publish_rate)
{
  publishGlobalCostmap(publish_rate);
  publishLocalCostmap(publish_rate);
}

void AmrPublisher::publishGlobalFootprint()
{
  if (!move_base_ptr_)
  {
    robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish global footprint");
    return;
  }

  try
  {
    // Get global data from move_base
    robot::move_base_core::PlannerDataOutput global_data = move_base_ptr_->getGlobalData();
    
    // Convert and publish footprint
    geometry_msgs::PolygonStamped nav_footprint;
    convertToNavPolygonStamped(global_data.footprint, nav_footprint);
    global_planner_obj_.footprint_pub_.publish(nav_footprint);
    
    // robot::log_debug("[AmrPublisher] Published global footprint with %zu points, frame_id='%s'", 
    //                  nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str());
  }
  catch (const std::exception &e)
  {
    robot::log_error("[AmrPublisher] Error publishing global footprint: %s", e.what());
  }
}

void AmrPublisher::publishLocalFootprint()
{
  if (!move_base_ptr_)
  {
    robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish local footprint");
    return;
  }

  try
  {
    // Get local data from move_base
    robot::move_base_core::PlannerDataOutput local_data = move_base_ptr_->getLocalData();
    
    // Convert and publish footprint
    geometry_msgs::PolygonStamped nav_footprint;
    convertToNavPolygonStamped(local_data.footprint, nav_footprint);
    local_planner_obj_.footprint_pub_.publish(nav_footprint);
    
    // robot::log_debug("[AmrPublisher] Published local footprint with %zu points, frame_id='%s'", 
    //                  nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str());
  }
  catch (const std::exception &e)
  {
    robot::log_error("[AmrPublisher] Error publishing local footprint: %s", e.what());
  }
}

void AmrPublisher::publishFootprints()
{
  publishGlobalFootprint();
  publishLocalFootprint();
}

void AmrPublisher::startPublishing(double global_rate, double local_rate)
{
  stopPublishing(); // Stop any existing publishing first

  // Start global costmap timer if rate > 0
  if (global_rate > 0.0)
  {
    global_planner_obj_.active_ = true;
    global_planner_obj_.timer_ = nh_.createTimer(
      ros::Duration(1.0 / global_rate),
      &AmrPublisher::globalCostmapTimerCallback,
      this
    );
    robot::log_info("[AmrPublisher] Started publishing global costmap at %.2f Hz", global_rate);
  }

  // Start local costmap timer if rate > 0
  if (local_rate > 0.0)
  {
    local_planner_obj_.active_ = true;
    local_planner_obj_.timer_ = nh_.createTimer(
      ros::Duration(1.0 / local_rate),
      &AmrPublisher::localCostmapTimerCallback,
      this
    );
    robot::log_info("[AmrPublisher] Started publishing local costmap at %.2f Hz", local_rate);
  }
}

void AmrPublisher::stopPublishing()
{
  // Stop timers
  if (global_planner_obj_.active_)
  {
    global_planner_obj_.timer_.stop();
    global_planner_obj_.active_ = false;
  }
  
  if (local_planner_obj_.active_)
  {
    local_planner_obj_.timer_.stop();
    local_planner_obj_.active_ = false;
  }

  robot::log_info("[AmrPublisher] Stopped publishing costmaps");
}

void AmrPublisher::globalCostmapTimerCallback(const ros::TimerEvent &event)
{
  if (global_planner_obj_.active_)
  {
    publishGlobalCostmap();
  }
}

void AmrPublisher::localCostmapTimerCallback(const ros::TimerEvent &event)
{
  if (local_planner_obj_.active_)
  {
    publishLocalCostmap();
  }
}

void AmrPublisher::onNewSubscriptionLocal(const ros::SingleSubscriberPublisher& pub)
{
  robot::log_info("[AmrPublisher] New subscription: %s", pub.getSubscriberName().c_str());
  publishLocalCostmap();
}


void AmrPublisher::onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub)
{
  robot::log_info("[AmrPublisher] New subscription: %s", pub.getSubscriberName().c_str());
  publishGlobalCostmap();
}

void AmrPublisher::publishCmdVel()
{
  if(!move_base_ptr_)
  {
    return;
  }

  if(!move_base_ptr_->getFeedback()->is_ready)
  {
    return;
  }

  robot_nav_2d_msgs::Twist2DStamped twist = move_base_ptr_->getTwist();
  if(twist.header.stamp < robot::Time::now() - robot::Duration(0.05))
  {
    return;
  }

  geometry_msgs::Twist cmd_vel;
  cmd_vel.linear.x = twist.velocity.x;
  cmd_vel.linear.y = twist.velocity.y;
  cmd_vel.angular.z = twist.velocity.theta;
  cmd_vel_pub_.publish(cmd_vel);
}

void AmrPublisher::startCmdVelPublishing(double rate)
{
  stopCmdVelPublishing(); // Stop any existing publishing first

  if (rate <= 0.0)
  {
    robot::log_warning("[AmrPublisher] Invalid cmd_vel publishing rate: %.2f Hz. Must be > 0", rate);
    return;
  }

  ros::WallDuration period(1.0 / rate);
  cmd_vel_timer_ = nh_.createWallTimer(
    period,
    &AmrPublisher::cmdVelTimerCallback,
    this,
    false  // oneshot = false
  );

  cmd_vel_publishing_active_ = true;
  robot::log_info("[AmrPublisher] Started publishing cmd_vel at %.2f Hz using ros::WallTimer", rate);
}

void AmrPublisher::stopCmdVelPublishing()
{
  if (cmd_vel_publishing_active_)
  {
    cmd_vel_timer_.stop();
    cmd_vel_publishing_active_ = false;
    robot::log_info("[AmrPublisher] Stopped publishing cmd_vel");
  }
}

void AmrPublisher::cmdVelTimerCallback(const ros::WallTimerEvent &event)
{
  (void)event;
  if (cmd_vel_publishing_active_)
  {
    publishCmdVel();
  }
}

void AmrPublisher::convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, 
                          nav_msgs::Path &nav_path)
{
  // Convert header
  nav_2d_msgs::Path2D path2d;
  path2d.header.seq = robot_path.header.seq;
  path2d.header.frame_id = robot_path.header.frame_id;
  path2d.header.stamp = ros::Time::now();
  path2d.poses.resize(robot_path.poses.size());
  for (unsigned int i = 0; i < robot_path.poses.size(); i++)
  {
    path2d.poses[i].pose.x = robot_path.poses[i].pose.x;
    path2d.poses[i].pose.y = robot_path.poses[i].pose.y;
    path2d.poses[i].pose.theta = robot_path.poses[i].pose.theta;
  }
  nav_path = nav_2d_utils::pathToPath(path2d);
}

void AmrPublisher::publishGlobalPlan()
{
  if(!move_base_ptr_)
  {
    return;
  }
  
  // Check if plan is empty
  if(cached_global_plan_.poses.empty())
  {
    return;
  }
  
  robot::Time now = robot::Time::now();
  robot::Time plan_stamp = cached_global_plan_.header.stamp;
  
  // Check if timestamp is zero (uninitialized)
  if(plan_stamp.sec == 0 && plan_stamp.nsec == 0)
  {
    return;
  }
  
  robot::Duration age = now - plan_stamp;
  robot::Duration max_age(0.5);
  
  // Check if timestamp is not older than 0.5 seconds
  if(plan_stamp < now - max_age)
  {
    return;
  }
  
  nav_msgs::Path nav_path;
  convertToNavPath(cached_global_plan_, nav_path);
  
  global_planner_obj_.plan_pub_.publish(nav_path);
}

void AmrPublisher::publishLocalPlan()
{
  if(!move_base_ptr_)
  {
    return;
  }
  
  // Check if plan is empty
  if(cached_local_plan_.poses.empty())
  {
    return;
  }
  
  robot::Time now = robot::Time::now();
  robot::Time plan_stamp = cached_local_plan_.header.stamp;
  
  // Check if timestamp is zero (uninitialized)
  if(plan_stamp.sec == 0 && plan_stamp.nsec == 0)
  {
    return;
  }
  
  
  robot::Duration age = now - plan_stamp;
  robot::Duration max_age(0.5);
  
  // Check if timestamp is not older than 0.5 seconds
  if(plan_stamp < now - max_age)
  {
    return;
  }
  
  nav_msgs::Path nav_path;
  convertToNavPath(cached_local_plan_, nav_path);
  local_planner_obj_.plan_pub_.publish(nav_path);
}

void AmrPublisher::globalPlanTimerCallback(const ros::TimerEvent &event)
{
  if (!move_base_ptr_)
  {
    return;
  }
  
  // Update cached global plan
  try
  {
    cached_global_plan_ = move_base_ptr_->getGlobalData().plan;
    // Publish if timestamp is valid
    publishGlobalPlan();
  }
  catch (const std::exception &e)
  {
    robot::log_error("[AmrPublisher] Error updating global plan: %s", e.what());
  }
}

void AmrPublisher::localPlanTimerCallback(const ros::TimerEvent &event)
{
  if (!move_base_ptr_)
  {
    return;
  }
  
  // Update cached local plan
  try
  {
    cached_local_plan_ = move_base_ptr_->getLocalData().plan;
    // Publish if timestamp is valid
    publishLocalPlan();
  }
  catch (const std::exception &e)
  {
    robot::log_error("[AmrPublisher] Error updating local plan: %s", e.what());
  }
}

void AmrPublisher::startPlanPublishing(double global_rate, double local_rate)
{
  stopPlanPublishing(); // Stop any existing publishing first

  // Start global plan timer if rate > 0
  if (global_rate > 0.0)
  {
    global_plan_publishing_active_ = true;
    global_plan_timer_ = nh_.createTimer(
      ros::Duration(1.0 / global_rate),
      &AmrPublisher::globalPlanTimerCallback,
      this
    );
    robot::log_info("[AmrPublisher] Started publishing global plan at %.2f Hz", global_rate);
  }

  // Start local plan timer if rate > 0
  if (local_rate > 0.0)
  {
    local_plan_publishing_active_ = true;
    local_plan_timer_ = nh_.createTimer(
      ros::Duration(1.0 / local_rate),
      &AmrPublisher::localPlanTimerCallback,
      this
    );
    robot::log_info("[AmrPublisher] Started publishing local plan at %.2f Hz", local_rate);
  }
}

void AmrPublisher::stopPlanPublishing()
{
  // Stop timers
  if (global_plan_publishing_active_)
  {
    global_plan_timer_.stop();
    global_plan_publishing_active_ = false;
  }
  
  if (local_plan_publishing_active_)
  {
    local_plan_timer_.stop();
    local_plan_publishing_active_ = false;
  }

  robot::log_info("[AmrPublisher] Stopped publishing plans");
}

} // namespace amr_control

