#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "geometry_msgs/msg/vector3_stamped.hpp"
#include "std_srvs/srv/empty.hpp"

using namespace std::chrono_literals;

/**
 * A small convenience function for converting a thread ID to a string
 **/
std::string string_thread_id()
{
  auto hashed = std::hash<std::thread::id>()(std::this_thread::get_id());
  return std::to_string(hashed);
}



class PublisherNode : public rclcpp::Node
{
public:
  PublisherNode()
  : Node("PublisherNode")
  {
    publisher_ = this->create_publisher<geometry_msgs::msg::Vector3Stamped>("topic", 10);
    auto timer_callback =
      [this]() -> void {
        auto message = geometry_msgs::msg::Vector3Stamped();
        message.header.stamp = this->now();

        this->publisher_->publish(message);
      };
    timer_ = this->create_wall_timer(500ms, timer_callback);
  }

private:
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr publisher_;
};

class DualThreadedNode : public rclcpp::Node
{
public:
  DualThreadedNode()
  : Node("DualThreadedNode")
  {
    /* These define the callback groups
     * They don't really do much on their own, but they have to exist in order to
     * assign callbacks to them. They're also what the executor looks for when trying to run multiple threads
     */
    callback_group_subscriber1_ = this->create_callback_group(
      rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_subscriber2_ = this->create_callback_group(
      rclcpp::CallbackGroupType::MutuallyExclusive);

     auto service = [this](const std::shared_ptr<rmw_request_id_t> request_header,
      const std::shared_ptr<std_srvs::srv::Empty::Request> req,
      const std::shared_ptr<std_srvs::srv::Empty::Response> res) -> void {
      this->test_service(request_header, req, res);
    };

    callback_group4_ = this->create_callback_group(
    rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
    test_srv_ = this->create_service<std_srvs::srv::Empty>("test_service", service, rmw_qos_profile_default, callback_group4_);

    auto sub1_opt = rclcpp::SubscriptionOptions();
    sub1_opt.callback_group = callback_group_subscriber1_;

    subscription1_ = this->create_subscription<geometry_msgs::msg::Vector3Stamped>(
      "topic",
      rclcpp::QoS(10),
    
      std::bind(
        &DualThreadedNode::subscriber1_cb, 
        this,                             
        std::placeholders::_1),           
                      
      sub1_opt);                 
  }

private:
  

  /**
   * Every time the Publisher publishes something, all subscribers to the topic get poked
   * This function gets called when Subscriber1 is poked (due to the std::bind we used when defining it)
   */
  void subscriber1_cb(const geometry_msgs::msg::Vector3Stamped::SharedPtr msg)
  {
    
    vector_msg_ = msg;
  }

  void test_service(
  const std::shared_ptr<rmw_request_id_t> request_header,
  const std::shared_ptr<std_srvs::srv::Empty::Request> request,
  const std::shared_ptr<std_srvs::srv::Empty::Response> response)
{
    
    RCLCPP_INFO(this->get_logger(), "Received message in callback1");

    RCLCPP_INFO(this->get_logger(), "msg timestamp : %f", rclcpp::Time(vector_msg_->header.stamp ).seconds());

    auto received_time = this->get_clock()->now();

    while (received_time +  rclcpp::Duration(5,0) > rclcpp::Time(vector_msg_->header.stamp) && rclcpp::ok())
    {  
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    RCLCPP_INFO(this->get_logger(), "Exitted cb after 5 secs");

}

  rclcpp::CallbackGroup::SharedPtr callback_group_subscriber1_;
  rclcpp::CallbackGroup::SharedPtr callback_group_subscriber2_;
  rclcpp::CallbackGroup::SharedPtr callback_group4_;
  rclcpp::Subscription<geometry_msgs::msg::Vector3Stamped>::SharedPtr subscription1_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription2_;

  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr test_srv_;

  geometry_msgs::msg::Vector3Stamped::SharedPtr vector_msg_;
  rclcpp::Time stamp_;

};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);

  // You MUST use the MultiThreadedExecutor to use, well, multiple threads
  rclcpp::executors::MultiThreadedExecutor executor;
  auto pubnode = std::make_shared<PublisherNode>();
  auto subnode = std::make_shared<DualThreadedNode>();  // This contains BOTH subscriber callbacks.
                                                        // They will still run on different threads
                                                        // One Node. Two callbacks. Two Threads
  executor.add_node(pubnode);
  executor.add_node(subnode);
  executor.spin();
  rclcpp::shutdown();
  return 0;
}

# source ros1 workspace
# source ros2 workspace
ros2 ros1_bridge dynamic_bridge --bridge-all-topics;

# source ros1 workspace
rosservice call  /test_service std_srvs/srv/Empty;