#include "amr_control/sensor_converter.h"

amr_control::SensorConverter::SensorConverter(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
  : nh_(nh), move_base_ptr_(move_base_ptr)
{
    map_server_sub_ = nh_.subscribe("/map", 1, &SensorConverter::mapServerCallback, this);
    laser_f_scan_sub_ = nh_.subscribe("/f_scan", 1, &SensorConverter::laserFScanCallback, this);
    laser_b_scan_sub_ = nh_.subscribe("/b_scan", 1, &SensorConverter::laserBScanCallback, this);
}

amr_control::SensorConverter::~SensorConverter()
{
    map_server_sub_.shutdown();
    laser_f_scan_sub_.shutdown();
    laser_b_scan_sub_.shutdown();
}

void amr_control::SensorConverter::requestMapFromServer(ros::NodeHandle &nh)
{
  // Thử request map từ service trước (nếu map server có service)
  ros::ServiceClient map_client = nh.serviceClient<nav_msgs::GetMap>("/static_map");
  
  if (map_client.exists())
  {
    nav_msgs::GetMap srv;
    if (map_client.call(srv))
    {
      robot::log_info("[%s:%d] Got map from service", __FILE__, __LINE__);
      // Convert và add map
      convertAndAddMap(srv.response.map);
      return;
    }
    else
    {
      robot::log_warning("[%s:%d] Service call failed, trying to wait for message...", __FILE__, __LINE__);
    }
  }
  else
  {
    robot::log_info("[%s:%d] Map service not available, waiting for message on /map topic...", __FILE__, __LINE__);
  }
  
  // Nếu không có service hoặc service call failed, đợi message từ topic
  nav_msgs::OccupancyGridConstPtr map_msg = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("/map", nh, ros::Duration(5.0));
  if (map_msg)
  {
    robot::log_info("[%s:%d] Got map from topic", __FILE__, __LINE__);
    convertAndAddMap(*map_msg);
  }
  else
  {
    robot::log_warning("[%s:%d] No map received after waiting 5 seconds", __FILE__, __LINE__);
  }
}

void amr_control::SensorConverter::convertAndAddMap(const nav_msgs::OccupancyGrid &nav_map)
{
  if (move_base_ptr_ != nullptr)
  {
    robot_nav_msgs::OccupancyGrid map;
    map.header.frame_id = nav_map.header.frame_id;
    map.header.stamp = robot::Time::now();
    map.header.seq = nav_map.header.seq;

    map.info.map_load_time = robot::Time::now();
    map.info.width = nav_map.info.width;
    map.info.height = nav_map.info.height;
    map.info.resolution = nav_map.info.resolution;
    map.info.origin.position.x = nav_map.info.origin.position.x;
    map.info.origin.position.y = nav_map.info.origin.position.y;
    map.info.origin.position.z = nav_map.info.origin.position.z;
    map.info.origin.orientation.x = nav_map.info.origin.orientation.x;
    map.info.origin.orientation.y = nav_map.info.origin.orientation.y;
    map.info.origin.orientation.z = nav_map.info.origin.orientation.z;
    map.info.origin.orientation.w = nav_map.info.origin.orientation.w;
    
    map.data = nav_map.data;
    move_base_ptr_->addStaticMap("navigation_map", map);
  }
}

void amr_control::SensorConverter::mapServerCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
  robot::log_info("[%s:%d]\n Adding static map to move_base from callback... %p", __FILE__, __LINE__, move_base_ptr_.get());
  convertAndAddMap(*msg);
}

void amr_control::SensorConverter::laserFScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
  if (move_base_ptr_ != nullptr && msg)
  {
    // Convert sensor_msgs::LaserScan to robot_sensor_msgs::LaserScan
    robot_sensor_msgs::LaserScan robot_scan;
    robot_scan.header.frame_id = msg->header.frame_id;
    robot_scan.header.stamp = robot::Time(msg->header.stamp.toSec());
    robot_scan.header.seq = msg->header.seq;
    robot_scan.angle_min = msg->angle_min;
    robot_scan.angle_max = msg->angle_max;
    robot_scan.angle_increment = msg->angle_increment;
    robot_scan.time_increment = msg->time_increment;
    robot_scan.scan_time = msg->scan_time;
    robot_scan.range_min = msg->range_min;
    robot_scan.range_max = msg->range_max;
    robot_scan.ranges = msg->ranges;
    robot_scan.intensities = msg->intensities;
    
    move_base_ptr_->addLaserScan("f_scan", robot_scan);
  }
}

void amr_control::SensorConverter::laserBScanCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
  if (move_base_ptr_ != nullptr && msg)
  {
    // Convert sensor_msgs::LaserScan to robot_sensor_msgs::LaserScan
    robot_sensor_msgs::LaserScan robot_scan;
    robot_scan.header.frame_id = msg->header.frame_id;
    robot_scan.header.stamp = robot::Time(msg->header.stamp.toSec());
    robot_scan.header.seq = msg->header.seq;
    robot_scan.angle_min = msg->angle_min;
    robot_scan.angle_max = msg->angle_max;
    robot_scan.angle_increment = msg->angle_increment;
    robot_scan.time_increment = msg->time_increment;
    robot_scan.scan_time = msg->scan_time;
    robot_scan.range_min = msg->range_min;
    robot_scan.range_max = msg->range_max;
    robot_scan.ranges = msg->ranges;
    robot_scan.intensities = msg->intensities;
    
    move_base_ptr_->addLaserScan("b_scan", robot_scan);
  }
}