#include "wit_wt901ble_reader/wit_wt901ble_reader.h"

namespace wt901ble_reader
{

    Diagnostics::Diagnostics(string& nodename):nodename_(nodename), nh_("~")
    {
        imu_diagnostics_topic_name_ = "";
        if(!ros::param::get(nodename_ + "/imu_diagnostics_topic_name", imu_diagnostics_topic_name_))
        {
            imu_diagnostics_topic_name_ = "/imu_diagnostics";
            ROS_WARN("Node: %s cannot get params", nodename_.c_str());            
        }
        diagnostics_pub = nh_.advertise<diagnostic_msgs::DiagnosticArray>(imu_diagnostics_topic_name_, 10);
        diagnostics_thread = new boost::thread(&Diagnostics::run_diagnostics, this);
    }
    Diagnostics::~Diagnostics()
    {
        diagnostics_thread->join();
        delete(diagnostics_thread);
    }

    void Diagnostics::run_diagnostics()
    {
        ros::Duration(0.5).sleep();        
        connection.name = "connection";
        connection.hardware_id = "IMU_sensors";
        connection.level = connection.STALE;
        connection.message = "Initialized";
        pubDiagnostics();
        ros::Rate r(5);
        boost::lock_guard<boost::mutex> lock(mutex);
        while(ros::ok())
        {
            ros::spinOnce();
            if(!is_imu_connected)
            {               
                connection.level = connection.ERROR;
                connection.message = "Cannot connect to device";
            }
            else{
                if(is_imu_data_available)
                {
                    connection.level = connection.OK;
                    connection.message = "Connected to device, datas is received";
                }
                else
                {
                    connection.level = connection.WARN;
                    connection.message = "Connected to device but datas is not received";
                }                    
            }
            pubDiagnostics();
            r.sleep();
        }
    }
    void Diagnostics::pubDiagnostics() {
        diagnostic_msgs::DiagnosticArray diagnostic;
        diagnostic.status.push_back(connection);
        if(diagnostic.status != prev_diagnostic.status)
        {
            diagnostic.header.seq = prev_diagnostic.header.seq+1;
            diagnostic.header.stamp = ros::Time::now();
            diagnostic.header.frame_id = "frame_id";
            diagnostics_pub.publish(diagnostic);
            prev_diagnostic = diagnostic;
        }
    }

    wt901ble_node::wt901ble_node(const string& nodename):nodename_(nodename), nh_("~")
    {
        userParams = new UserParams();
        wt901ble_data_ = new wt901ble_data(); 
        if(LoadParams(userParams, nodename_)){
            ROS_INFO("%s : Params is loaded", nodename_.c_str());
        }
        else ROS_ERROR("%s : Fail to load params", nodename_.c_str());        
        isDataAvailable = false;
        wt901ble_data_pub_ = nh_.advertise<sensor_msgs::Imu>(userParams->topic_name,10,false);
        isConnected_ = false;
        is_imu_connected = false;
        is_imu_data_available = false;
        is_try_to_connect = false;
        is_reconnected = false;
        lost_data_count = 0;
        reconnect_timer = nh_.createWallTimer(::ros::WallDuration(0.5), &wt901ble_node::ReconnectSerial, this);
        get_and_publish_timer = nh_.createWallTimer(::ros::WallDuration(0.02), &wt901ble_node::GetAndPublishData, this);
        reconnect_timer.stop();
        if(detect_port(userParams->portname))
        {
            ROS_INFO("Port: %s is found",userParams->portname.c_str());    
            try {
                serial_ = new serial::Serial(userParams->portname, userParams->baudrate, serial::Timeout::simpleTimeout(500));
            }
            catch(const std::exception& ex)
            {
                isConnected_ = false;
                is_imu_connected = false;
                ROS_WARN("Can not open serial port: %s", userParams->portname.c_str());
                cerr << "Unhandled Exception: " << ex.what() << endl;
            }
            if(serial_!=NULL)
            {
                if(serial_-> isOpen()){
                    ROS_INFO("%s: Connected to serial port: %s, baudrate: %d", nodename_.c_str(), userParams->portname.c_str(), userParams->baudrate);
                    isConnected_ = true;
                    is_imu_connected = true;
                }   
            }
            get_and_publish_timer.start();
        }
        
    }

    wt901ble_node::~wt901ble_node()
    {
        delete(serial_);
        delete(userParams);
        delete(wt901ble_data_);
    }

    void wt901ble_node::GetAndPublishData(const ::ros::WallTimerEvent& timer_event)
    {
        if(isConnected_)
        {     
            if(recvData(data_buf))
            {
                // ROS_INFO_ONCE("%s: Start to get datas and publish", nodename_.c_str());
                // size_t data_buf_size = sizeof(data_buf)/sizeof(data_buf[0]);
                // int len = static_cast<int>(data_buf_size);
                // ROS_INFO("len: %d, data_buf: %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x", 
                // len, data_buf[0], data_buf[1], data_buf[2], data_buf[3], data_buf[4], data_buf[5], 
                // data_buf[6], data_buf[7], data_buf[8], data_buf[9], data_buf[10], data_buf[11],
                // data_buf[12], data_buf[13], data_buf[14], data_buf[15], data_buf[16], data_buf[17], data_buf[18], data_buf[19]);

                int16_t aX = (data_buf[3]<<8U)|data_buf[2];
                int16_t aY = (data_buf[5]<<8U)|data_buf[4];
                int16_t aZ = (data_buf[7]<<8U)|data_buf[6];
                int16_t wX = (data_buf[9]<<8U)|data_buf[8];
                int16_t wY = (data_buf[11]<<8U)|data_buf[10];
                int16_t wZ = (data_buf[13]<<8U)|data_buf[12];
                int16_t Roll = (data_buf[15]<<8U)|data_buf[14];
                int16_t Pitch = (data_buf[17]<<8U)|data_buf[16];
                int16_t Yaw = (data_buf[19]<<8U)|data_buf[18];

                wt901ble_data_->aX = ((double)(aX))/32768*156.9064;
                wt901ble_data_->aY = ((double)(aY))/32768*156.9064;
                wt901ble_data_->aZ = ((double)(aZ))/32768*156.9064;
                wt901ble_data_->wX = ((double)(wX))/32768*2000;
                wt901ble_data_->wY = ((double)(wY))/32768*2000;
                wt901ble_data_->wZ = ((double)(wZ))/32768*2000;
                wt901ble_data_->Roll = ((double)(Roll))/32768*180;
                wt901ble_data_->Pitch = ((double)(Pitch))/32768*180;
                wt901ble_data_->Yaw = ((double)(Yaw))/32768*180;
            
                wt901ble_data_->wX = (wt901ble_data_->wX*M_PI)/180;
                wt901ble_data_->wY = (wt901ble_data_->wY*M_PI)/180;
                wt901ble_data_->wZ = (wt901ble_data_->wZ*M_PI)/180;

                // ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f",
                // wt901ble_data_->Roll, wt901ble_data_->Pitch, wt901ble_data_->Yaw);
                wt901ble_data_->Roll = (wt901ble_data_->Roll*M_PI)/180;
                wt901ble_data_->Pitch = (wt901ble_data_->Pitch*M_PI)/180;
                wt901ble_data_->Yaw = (wt901ble_data_->Yaw*M_PI)/180;

                imu_data.header.frame_id = userParams->frame_id;
                imu_data.header.stamp = ros::Time::now();
                imu_data.orientation = getQuaternion(wt901ble_data_->Yaw, wt901ble_data_->Pitch, wt901ble_data_->Roll);
                // imu_data.orientation_covariance[0] = 2.8900000000000004e-08;
                // imu_data.orientation_covariance[4] = 2.8900000000000004e-08;
                // imu_data.orientation_covariance[8] = 2.8900000000000004e-08;
                imu_data.orientation_covariance[0] = 0.1;
                imu_data.orientation_covariance[4] = 0.1;
                imu_data.orientation_covariance[8] = 0.1;
                imu_data.angular_velocity.x = wt901ble_data_->wX;
                imu_data.angular_velocity.y = wt901ble_data_->wY;
                imu_data.angular_velocity.z = wt901ble_data_->wZ;                  
                // imu_data.angular_velocity_covariance[0] = 2.8900000000000004e-08;
                // imu_data.angular_velocity_covariance[4] = 2.8900000000000004e-08;
                // imu_data.angular_velocity_covariance[8] = 2.8900000000000004e-08;
                imu_data.angular_velocity_covariance[0] = 8e-6;
                imu_data.angular_velocity_covariance[4] = 8e-6;
                imu_data.angular_velocity_covariance[8] = 3e-7;
                imu_data.linear_acceleration.x =  wt901ble_data_->aX;
                imu_data.linear_acceleration.y =  wt901ble_data_->aY;
                imu_data.linear_acceleration.z =  wt901ble_data_->aZ;
                // imu_data.linear_acceleration_covariance[0] = 0.009604000000000001;
                // imu_data.linear_acceleration_covariance[4] = 0.009604000000000001;
                // imu_data.linear_acceleration_covariance[8] = 0.009604000000000001;
                imu_data.linear_acceleration_covariance[0] = 5e-5;
                imu_data.linear_acceleration_covariance[4] = 0.0001;
                imu_data.linear_acceleration_covariance[8] = 0.00013;                
                wt901ble_data_pub_.publish(imu_data);
                // ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f",
                // wt901ble_data_->Roll, wt901ble_data_->Pitch, wt901ble_data_->Yaw);
                // ROS_INFO("aX: %f, aY: %f, aZ: %f, wX: %f, wY: %f, wZ: %f, Roll: %f, Pitch: %f, Yaw: %f",
                // wt901ble_data_->aX,  wt901ble_data_->aY, wt901ble_data_->aZ,
                // wt901ble_data_->wX, wt901ble_data_->wY, wt901ble_data_->wZ,
                // wt901ble_data_->Roll, wt901ble_data_->Pitch, wt901ble_data_->Yaw);
                is_imu_data_available = true;
                lost_data_count = 0;
                is_try_to_connect = false;                
            }   
            else
            {
                lost_data_count ++;
                if(lost_data_count>30)
                {
                    is_imu_data_available = false;
                    if(is_try_to_connect ==false) { is_try_to_connect=true; reconnect_timer.start();}
                    // ROS_WARN("IMU reader node: lost_data");
                    lost_data_count = 0;
                }
            }      
        }
        
    }

    void wt901ble_node::ReconnectSerial(const ::ros::WallTimerEvent& timer_event)
    {
        if(is_try_to_connect)
        {
            if(!is_reconnected)
            {
                if(serial_!=NULL)
                {
                    isConnected_ = false;
                    is_imu_connected = false;
                    // boost::this_thread::sleep(boost::posix_time::milliseconds(500));
                    ros::Duration(0.5).sleep();
                    serial_->flush();
                    serial_->close();
                    delete(serial_);
                    serial_ = new serial::Serial(userParams->portname, userParams->baudrate, serial::Timeout::simpleTimeout(500));
                }
                if(serial_!=NULL)
                {
                    if(serial_-> isOpen()){
                        ROS_WARN("%s: Reconnected to serial port (IMU sensor): %s, baudrate: %d", nodename_.c_str(), userParams->portname.c_str(), userParams->baudrate);
                        isConnected_ = true;
                        is_imu_connected = true;
                    }   
                }
                is_reconnected = true;
                reconnect_timer.stop();
            }
        }
    }

    geometry_msgs::Quaternion wt901ble_node::getQuaternion(double yaw, double pitch, double roll)
    {
        double cy = cos(yaw * 0.5);
        double sy = sin(yaw * 0.5);
        double cp = cos(pitch * 0.5);
        double sp = sin(pitch * 0.5);
        double cr = cos(roll * 0.5);
        double sr = sin(roll * 0.5);
        geometry_msgs::Quaternion q;
        q.x = sr * cp * cy - cr * sp * sy;
        q.y = cr * sp * cy + sr * cp * sy;
        q.z = cr * cp * sy - sr * sp * cy;
        q.w = cr * cp * cy + sr * sp * sy;
        return q;
    }

    bool wt901ble_node::LoadParams(UserParams* userParams_, const string& node_name){
        bool result = true;
        try{            
            int baudrate;
            if(!ros::param::get(node_name + "/baudrate", baudrate)) result = false;
            if(!ros::param::get(node_name + "/portname", userParams_->portname)) result = false;
            if(!ros::param::get(node_name + "/topic_name", userParams_->topic_name)) result = false;
            if(!ros::param::get(node_name + "/frame_id", userParams_->frame_id)) result = false;
            userParams_->baudrate = static_cast<uint32_t>(baudrate);
        }
        catch(const std::exception& ex)
        {
            std::cerr << "Fail to load user params caught: " << ex.what() << std::endl;
            result = false;
        }
        return result;
    }

    size_t wt901ble_node::readN(uint8_t *buf, size_t len){
        size_t offset = 0;
        offset = serial_->read(buf,len);
        return offset;
    }

    bool wt901ble_node::recvData(uint8_t *buf){
        bool ret = false;
        uint8_t ch[2];
        if(readN(ch, 2) == 2)
        {
            if(ch[0]==wt901ble_header_imu[0]&&
                ch[1]==wt901ble_header_imu[1])
            {
                buf[0] = ch[0];
                buf[1] = ch[1];
                if(readN(&buf[2],18)==18)
                {
                    ret = true;
                }
            }
        }
        // if (readN(buf, wt901ble_data_length) != 20) {
        //     // continue;
        //     return false;
        // }
        // if (buf[0] == wt901ble_header_imu[0]&&
        //     buf[1] == wt901ble_header_imu[1])
        // {
        //     ret = true;
        // }    
        return ret;
    }
    
    bool wt901ble_node::receiveData()
    {
        bool ret = false;
        isDataAvailable = false;
        uint8_t ch;
        while (!ret) {

           if(readN(&ch, 1) != 1){
                continue;
           }
           if(ch == wt901ble_header_imu[0]){
                data_buf[0] = ch;
                if(readN(&ch, 1) == 1){
                    if(ch == wt901ble_header_imu[2])
                    {
                        data_buf[1] = ch;
                        if(readN(&data_buf[2], 18) == 18)
                        {
                            ret = true;
                            isDataAvailable = true;
                        }
                    }
                }
           }

        }
        return ret;
    }

    bool wt901ble_node::detect_port(string& port_name)
    {
        bool result = false;
        // vector<serial::PortInfo> devices_found = serial::list_ports();

        // vector<serial::PortInfo>::iterator iter = devices_found.begin();
        // bool result = false;
        // while( iter != devices_found.end() )
        // {
        //     serial::PortInfo device = *iter++;
        //     // ROS_INFO( "(%s, %s, %s)\n", device.port.c_str(), device.description.c_str(),
        //     // device.hardware_id.c_str() );
        //     if(device.port == port_name)
        //     {
        //         // ROS_INFO("Found port: %s",device.port.c_str());
        //         result = true;
        //         break;                
        //     }
        // }
        // return result;        
        string product = "USB Serial";
        char _port[50] = "";
        udevadmInfo *ludev = new udevadmInfo(product.c_str());
        if(ludev->init() == EXIT_FAILURE)
            return false;
        if(!ludev->scanDevices()) { return false; }
        else {
            strcpy(_port, ludev->port);
        }
        ROS_INFO("Found port: %s",_port);

        if(strstr(_port,"/dev/ttyUSB") != NULL) {
            // std::string cmd_ = "sudo chmod 777 " + (std::string)_port;
            // system(cmd_.c_str());
            userParams->portname = (std::string)_port;
            result = true;
        }
        return result;
    }

}