#ifndef WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP
#define WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP

#include "rclcpp/macros.hpp"
#include "webots_ros2_driver/PluginInterface.hpp"
#include "webots_ros2_driver/WebotsNode.hpp"

#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "/home/sweetai/webots_simulation/install/sweet_msg/include/sweet_msg/msg/movebase.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include <webots/Speaker.hpp>
#include <nav_msgs/msg/path.hpp>
#include <webots/Node.hpp>
#include <webots/Field.hpp>

#include <array>
#include <vector>

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

// #include "sweet_utils/Utils/common_utils.hpp"


namespace webots_robot_driver {
class WebotsRobotDriver : public webots_ros2_driver::PluginInterface {
public:
  void step() override;
  void init(webots_ros2_driver::WebotsNode *node,
            std::unordered_map<std::string, std::string> &parameters) override;

private:
  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
  void twist_callback(const geometry_msgs::msg::Twist::SharedPtr twist_msg);
  void move_base_callback(const sweet_msg::msg::Movebase::SharedPtr move_base_msg);
  void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
  void getRobotCenterPose(double &x, double &y, const float yaw);
  void pub_current_pose(const double lat, const double lon, const float roll, const float pitch, const float yaw);
  float get_bearing_in_degrees();


  rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr rtk_pose_publisher;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_cmd_vel_sub;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
      cmd_vel_subscription_;
  rclcpp::Subscription<sweet_msg::msg::Movebase>::SharedPtr move_base_sub;

  std::vector<std::array<double, 3>> path_points_;

  geometry_msgs::msg::Twist cmd_vel_msg;


  WbDeviceTag right_motor;
  WbDeviceTag left_motor;

  WbDeviceTag speaker;
  WbDeviceTag gp, imu, compass;

  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
} // namespace webots_robot_driver
#endif