#include <loc_base/loc_base.h>
#include <loc_core/localization.h>
#include <tf2_ros/transform_listener.h>
#include <std_msgs/UInt16.h>

// std::shared_ptr<loc_base::LocBase> loc_base_ptr = nullptr;
std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr = nullptr;
static loc_core::BaseLocalization* loc_base_;

void callBack(const std_msgs::UInt16& msg)
{
  std::stringstream directories;
  if (loc_base_)
  {
    switch (msg.data)
    {
    case 1:
      loc_base_->startMapping();
      /* code */
      break;
    case 2:
      loc_base_->stopMapping();
      /* code */
      break;
    case 3:
      loc_base_->listMapFiles(directories);
      ROS_INFO_STREAM(directories.str());
      break;   
    default:
      break;
    }
  }
}

void createMapCallBack(const std_msgs::String& msg)
{
  if (loc_base_)
    loc_base_->saveActivateMapFileName(msg.data);
}

void changeMapCallBack(const std_msgs::String& msg)
{
  if (loc_base_)
    loc_base_->changeStaticMap(msg.data);
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "loc_base_node");
  ros::start();
  ros::NodeHandle nh(ros::this_node::getName());

  std::shared_ptr<tf2_ros::Buffer> tf = std::make_shared<tf2_ros::Buffer>();
  tf2_ros::TransformListener tf2(*tf);
  // loc_base_ptr = std::make_shared<loc_base::LocBase>();
  pluginlib::ClassLoader<loc_core::BaseLocalization>  loc_base_loader_("loc_core", "loc_core::BaseLocalization");

  std::string obj_name("loc_base::LocBase");
  if (tf == nullptr)
    throw std::runtime_error("tf2_ros::Buffer object is null");
  try
  {
    loc_base_ptr = loc_base_loader_.createUniqueInstance(obj_name);
    loc_base_ptr->initialize(nh, tf);
    loc_base_ = loc_base_ptr.get();
  }
  catch (pluginlib::PluginlibException& ex)
  {
    ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
    exit(1);
  }
  catch (std::exception& e)
  {
    ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
    exit(1);
  }

  ros::Subscriber sub = nh.subscribe("mode", 1000, callBack);
  ros::Subscriber save_map_sub = nh.subscribe("save_map", 1000, createMapCallBack);
  ros::Subscriber change_map_sub = nh.subscribe("change_map", 1000, changeMapCallBack);
  ros::Publisher state = nh.advertise<std_msgs::UInt16>("state", 1);
  ros::Rate rate(3);
  
  while (ros::ok())
  {
    std_msgs::UInt16 state_msg;
    state_msg.data = loc_base_->getState();
    state.publish(state_msg);
    rate.sleep();
    ros::spinOnce();
  }

  ros::spin();
  loc_base_ptr.reset();
  return(0);
}