#ifndef __IMU_NODE_HH
#define __IMU_NODE_HH

#include "JY901.h"
#include <boost/thread.hpp>
#include "serial/serial.h"

#include <chrono>
#include <iostream>
#include <mutex>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#define		iBufferSize 50
#define     UARTBufferLength 1000

#include "JY901.h"
CJY901 JY901;

class ImuNode
{
    public:
        ImuNode(std::string port, unsigned long baud);
        ~ImuNode();
        void publish_data();

    protected:
        static void thread_get_data(ImuNode *imu_node)
        {
            int countDown=20;
            while ((imu_node->b_connected_ || countDown-- > 0) && !imu_node->b_exit_)
            {
                // std::cout<<imu_node->my_serial->isOpen()<<std::endl;
                if (imu_node->my_serial->isOpen())
                {
                    imu_node->b_connected_ = true;
                    // std::vector<uint8_t> temp_buffer;
                    int count = imu_node->my_serial->read(imu_node->p_temp_buf, iBufferSize);
                    // std::cout<<"count: "<<count<<std::endl;
                    std::unique_lock<std::mutex> lck(imu_node->mtx);
                    // end 添加后在范围内
                    if ((imu_node->end + count) <= UARTBufferLength)
                    {
                        // std::cout<<"bbb"<<std::endl;
                        // std::cout<<"end: "<<imu_node->end<<std::endl;
                        memcpy((imu_node->p_buffer + imu_node->end), imu_node->p_temp_buf, count);
                        // std::cout<<"start: "<<imu_node->start<<std::endl;
                        if (imu_node->start > imu_node->end && imu_node->start <= imu_node->end + count)
                        {
                            imu_node->start = imu_node->end + count + 1;
                            imu_node->start %= UARTBufferLength;
                        }
                        imu_node->end += count;
                        imu_node->end %= UARTBufferLength;
                    }
                    // 超出范围
                    else
                    {
                        // std::cout<<"ccc"<<std::endl;
                        int offset = UARTBufferLength - imu_node->end;
                        // std::cout<<"offset: "<<offset<<std::endl;
                        memcpy((imu_node->p_buffer + imu_node->end), imu_node->p_temp_buf, offset);
                        memcpy(imu_node->p_buffer, (imu_node->p_temp_buf + offset), count - offset);
                        if (imu_node->start > imu_node->end || (imu_node->start < imu_node->end && imu_node->start < (imu_node->end + count) % UARTBufferLength))
                        {
                            imu_node->start = (imu_node->end + count) % UARTBufferLength + 1;
                        }
                        imu_node->end += count;
                        imu_node->end %= UARTBufferLength;
                    }
                }
                else
                {
                    imu_node->b_connected_ = false;
                }

                // std::cout << "count down: " << countDown << std::endl;
                usleep(5 * 1000);
            }

            std::cout << "数据获取线程退出" << std::endl;
        };

    public:
        bool b_exit_;
        // 0默认，1正向，2反向
        int m_angle_status_[3]={0,0,0};
    protected:

        bool b_connected_;
        
        std::mutex mtx;
        //std::unique_lock<std::mutex> buffer_lock;
        serial::Serial *my_serial;
        char *p_buffer;
        uint8_t *p_temp_buf;        
        int start,end;
        boost::thread*  read_thread_  ;

        ros::NodeHandle nh_;
        ros::Publisher odom_pub_;
        tf::Pose last_pose_, curr_pose_;
        geometry_msgs::Twist last_vel_, curr_vel_;
        ros::Time last_time_, curr_time_, ori_time_;
        tf::TransformBroadcaster tf_broadcaster_;

};

#endif