#include "component_manager/service_component.hpp"
#include "component_interfaces/srv/detect_object.hpp"

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_msgs/msg/tf_message.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <ignition/transport/Node.hh>
#include <ros_gz_bridge/convert.hpp>

#define IGN_DEFAULT_BASE_FRAME "world"
#define IGN_DEFAULT_WORLD_NAME "mir"

namespace scene_component
{

class IgnDetectObjectService: public 
  env_manager::component_manager::ServiceComponent<component_interfaces::srv::DetectObject>
{
  public:
    IgnDetectObjectService(const rclcpp::NodeOptions &options)
      : env_manager::component_manager::ServiceComponent<component_interfaces::srv::DetectObject>(options)
    {
      std::string ign_topic_name = "/world/" + std::string(IGN_DEFAULT_WORLD_NAME) + "/dynamic_pose/info";
      _tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
      _ign_node = std::make_shared<ignition::transport::Node>();
      _ign_node->Subscribe(ign_topic_name, &IgnDetectObjectService::on_pose, this);
    }

    void callback(
      std::shared_ptr<component_interfaces::srv::DetectObject::Request> request,
      std::shared_ptr<component_interfaces::srv::DetectObject::Response> response)
    {
      auto kind = static_cast<DetectObjectReqKind>(request->req_kind);
      bool status = false;
      switch(kind)
      {
        case DETECT: {
          (void) kind;
          break;
        } case FOLLOW: {
          auto ok = std::find(_follow_frames.begin(), _follow_frames.end(), 
              request->object_name) == _follow_frames.end();
          if (ok)
            _follow_frames.push_back(request->object_name);
          status = ok; 
          break;
        } case CANCEL: {
          auto it = std::find(_follow_frames.begin(), _follow_frames.end(), 
              request->object_name);
          auto ok = it != _follow_frames.end();
          if (ok)
            _follow_frames.erase(it);
          status = ok;
          break;
        }
      }
      response->call_status = status;
    }

  private:
    enum DetectObjectReqKind
    {
      DETECT=0,
      FOLLOW=1,
      CANCEL=2
    };

    void on_pose(const ignition::msgs::Pose_V &pose_v)
    {
      auto poses = pose_v.pose();
      for (const auto& obj: _follow_frames)
      {
        auto it = std::find_if(poses.begin(), poses.end(), 
          [&obj](const auto& pose){ return pose.name() == obj;});
        if (it == poses.end())
          continue;

        geometry_msgs::msg::PoseStamped rpose;
        ros_gz_bridge::convert_gz_to_ros(*it, rpose);
        geometry_msgs::msg::TransformStamped t;

        t.header.stamp = this->get_clock()->now();
        t.header.frame_id = IGN_DEFAULT_BASE_FRAME;
        t.child_frame_id = it->name();
        t.transform.translation.x = rpose.pose.position.x;
        t.transform.translation.y = rpose.pose.position.y;
        t.transform.translation.z = rpose.pose.position.z;
        t.transform.rotation.x = rpose.pose.orientation.x;
        t.transform.rotation.y = rpose.pose.orientation.y;
        t.transform.rotation.z = rpose.pose.orientation.z;
        t.transform.rotation.w = rpose.pose.orientation.w;

        _tf2_broadcaster->sendTransform(t);
      }
    }

    std::vector<std::string> _follow_frames;
    std::unique_ptr<tf2_ros::TransformBroadcaster> _tf2_broadcaster;
    std::shared_ptr<ignition::transport::Node> _ign_node;

};

} // namespace scene_component

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnDetectObjectService)