#include "autoware_v2x/v2x_node.hpp"
#include "autoware_v2x/v2x_app.hpp"
#include "autoware_v2x/time_trigger.hpp"
#include "autoware_v2x/router_context.hpp"
#include "autoware_v2x/positioning.hpp"
#include "autoware_v2x/security.hpp"
#include "autoware_v2x/link_layer.hpp"
#include "autoware_v2x/cpm_application.hpp"

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include <vanetza/asn1/cpm.hpp>
#include <vanetza/facilities/cpm_functions.hpp>
#include <sstream>
#include <memory>
#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace gn = vanetza::geonet;

using namespace vanetza;
using namespace vanetza::facilities;
using namespace std::chrono;

namespace v2x
{
  V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
    using std::placeholders::_1;
    subscription_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));

    subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));

    publisher_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/objects", rclcpp::QoS{10});

    this->declare_parameter<std::string>("network_interface", "vmnet1");
    this->declare_parameter<bool>("is_sender", true);

    app = new V2XApp(this);
    boost::thread v2xApp(boost::bind(&V2XApp::start, app));

    RCLCPP_INFO(get_logger(), "V2X Node Launched");

    rclcpp::Time current_time = this->now();
    RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds());

  }

  void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) {
    autoware_auto_perception_msgs::msg::PredictedObjects output_dynamic_object_msg;
    std_msgs::msg::Header header;
    rclcpp::Time current_time = this->now();
    output_dynamic_object_msg.header.frame_id = "map";
    output_dynamic_object_msg.header.stamp = current_time;

    for (CpmApplication::Object obj : *objectsStack) {
      autoware_auto_perception_msgs::msg::PredictedObject object;
      autoware_auto_perception_msgs::msg::ObjectClassification classification;
      autoware_auto_perception_msgs::msg::Shape shape;
      autoware_auto_perception_msgs::msg::PredictedObjectKinematics kinematics;

      classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR;
      classification.probability = 0.99;

      shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
      shape.dimensions.x = obj.shape_x / 10.0;
      shape.dimensions.y = obj.shape_y / 10.0;
      shape.dimensions.z = obj.shape_z / 10.0;

      kinematics.initial_pose_with_covariance.pose.position.x = obj.position_x;
      kinematics.initial_pose_with_covariance.pose.position.y = obj.position_y;
      kinematics.initial_pose_with_covariance.pose.position.z = 0.1;

      kinematics.initial_pose_with_covariance.pose.orientation.x = obj.orientation_x;
      kinematics.initial_pose_with_covariance.pose.orientation.y = obj.orientation_y;
      kinematics.initial_pose_with_covariance.pose.orientation.z = obj.orientation_z;
      kinematics.initial_pose_with_covariance.pose.orientation.w = obj.orientation_w;

      object.classification.emplace_back(classification);
      object.shape = shape;
      object.kinematics = kinematics;

      output_dynamic_object_msg.objects.push_back(object);
    }

    current_time = this->now();
    // RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds());

    publisher_->publish(output_dynamic_object_msg);
  }

  void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
    rclcpp::Time current_time = this->now();
    rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg.
    // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size());

    // Measuring T_A1R1
    // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds());
    // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds());
    app->objectsCallback(msg);
  }

  void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
    app->tfCallback(msg);
  }
}

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode)