#ifndef _WT901BLE_READER_H_
#define _WT901BLE_READER_H_

#pragma once

#include <iostream>
#include <string>
#include <vector>
#include <stdlib.h>
#include <unistd.h>
#include <cstdint>
#include <cmath>
#include <serial/serial.h>
#include <chrono>
#include <sys/time.h>
#include <ctime>
#include <ros/ros.h>
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Quaternion.h"
#include <boost/thread.hpp>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>

#include <poll.h>
#include <cstdio>
#include <cstdlib>
#include <cerrno>
#include <cstring>
#include <sys/signalfd.h>
#include <csignal>
#include <libudev.h>

#include "wit_wt901ble_reader/udevadm_info.h"

using namespace std;

namespace wt901ble_reader
{
    static bool is_imu_connected = false;
    static bool is_imu_data_available = false;
    static uint16_t lost_data_count = 0;
    const uint8_t wt901ble_data_length = 20;
    const uint8_t wt901ble_header_imu[2] = {0x55, 0x61};

    struct wt901ble_data{
        wt901ble_data()
        {
            aX = 0;
            aY = 0;
            aZ = 0;
            wX = 0;
            wY = 0;
            wZ = 0;
            Roll = 0;
            Pitch = 0;
            Yaw = 0;
        }
        double aX;
        double aY;
        double aZ;
        double wX;
        double wY;
        double wZ;
        double Roll;
        double Pitch;
        double Yaw;
    };
    struct UserParams{
        UserParams(){
            baudrate = 115200;
            portname = "/dev/ttyUSB2";
            topic_name = "/imu";
            frame_id = "imu";            
        }
        uint32_t baudrate;
        string portname;
        string topic_name;
        string frame_id;
    };

    class wt901ble_node
    {
        public:
            wt901ble_node(const string& nodename);
            ~wt901ble_node();                 
        private:
            ros::NodeHandle nh_;
            ros::Publisher wt901ble_data_pub_;
            ros::WallTimer get_and_publish_timer;
            ros::WallTimer reconnect_timer;
            serial::Serial* serial_;
            UserParams* userParams;
            wt901ble_data* wt901ble_data_;
            bool LoadParams(UserParams* userParams_, const string& node_name);
            void GetAndPublishData(const ::ros::WallTimerEvent& timer_event);
            void ReconnectSerial(const ::ros::WallTimerEvent& timer_event);
            size_t readN(uint8_t *buf, size_t len);
            bool receiveData();
            bool recvData(uint8_t *buf);
            geometry_msgs::Quaternion getQuaternion(double yaw_, double pitch_, double roll_);
            bool detect_port(string& port_name);
            sensor_msgs::Imu imu_data;
            string nodename_;
            bool isConnected_;
            bool isDataAvailable;
            bool is_try_to_connect;
            bool is_reconnected;
            uint8_t data_buf[wt901ble_data_length]{0};
    };

    class Diagnostics
    {
        public:
        Diagnostics(string& nodename);
        ~Diagnostics();
        private:
        ros::NodeHandle nh_;
        ros::Publisher diagnostics_pub;
        boost::thread* diagnostics_thread;
        boost::mutex mutex;
        void run_diagnostics();
        void pubDiagnostics();
        string nodename_;
        string imu_diagnostics_topic_name_;
        diagnostic_msgs::DiagnosticStatus connection;
        diagnostic_msgs::DiagnosticArray prev_diagnostic;
    };
}

#endif