//
// Created by hshine on 22-12-23.
//

#ifndef RM_SERIAL_RM_SERIAL_HPP
#define RM_SERIAL_RM_SERIAL_HPP

#include "serial/serial.h"
#include "ros/ros.h"
#include "rm_serial/gimbal_msg.h"
#include "geometry_msgs/TwistStamped.h"
#include <geometry_msgs/TransformStamped.h>
#include "packet.hpp"

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

#include <memory>
#include <thread>
namespace rm_serial
{
    class RMSerial
    {
    public:
        explicit RMSerial();
        ~RMSerial() ;

        void receiveData();
        void sendData();
        void gimbal_cmd_cb(const rm_serial::gimbal_msg::ConstPtr& gimbal_msg);
        void chassis_cmd_cb(const geometry_msgs::Twist::ConstPtr& cmd_vel_msg);
        void reopenPort();
        void set_param();
      private:
        std::string device_name_;
        std::unique_ptr<serial::Serial> serial_;
        std::shared_ptr<ros::NodeHandle> serial_node_;
        std::shared_ptr<ros::Publisher> latency_pub_;//communicate delay
        //std::shared_ptr<ros::Publisher> joint_state_pub_;
        ros::Publisher joint_state_pub,game_progress_pub,outpost_hp_pub,arial_goal_pub;
        ros::Subscriber gimbal_cmd_sub,chassis_cmd_sub;
        double last_cmd_x,last_cmd_y;
        bool initial_set_param_ = false;
        uint8_t previous_receive_color_ = 0;

        tf2_ros::TransformBroadcaster lidar2baselink;
        std::thread receive_thread_;
        SendPacket packet_;
    };
}

#endif //RM_SERIAL_RM_SERIAL_HPP