#include <unistd.h>
#include <signal.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tr1/functional>

#include "udp_t.h"
#include "X_Define_Internal.h"
#include "common_utils.hpp"
#include "honeywell_i300.hpp"
#include "stim300.hpp"

// ROS MSG
#include "common/device_status_ros_msg.h"

// 配置参数
static std::string work_dir  = "~";
static int imu_id = -1;
static int imu_type = -1;
static int listen_port = 6000;
static std::string topic_pub = "";

// 节点是否退出
static bool run_flag = false;
static pthread_mutex_t run_flag_mutex_lock;   // 节点运行状态控制互斥锁

// ROS通讯相关
static ros::Publisher pub_stat;
static ros::Publisher pub_data;
    
// 程序开始时间
static double ep0[6] = { 0.0 };

static long imu_count = 0;

void *thread_udprcv(void *args)
{
    thread_udpsvr_arg* para = (thread_udpsvr_arg*)args;
    
    int     nr = 0;
    double  ep[6] = { 0.0 };
    udp_t   *udp = NULL;
    char    msg[UDPBUFSIZE] = { 0 };
    unsigned char buff[UDPBUFSIZE]={0};
    bool    flag = false;
    
    bool    first_run = true;
    common::device_status_ros_msg   udp_stat_msg;
    common::device_status_ros_msg   imu_stat_msg;
    udp_stat_msg.type = DEVICE_TYPE_IMU_UDP;  udp_stat_msg.id = 0;
    imu_stat_msg.type = DEVICE_TYPE_IMU;      imu_stat_msg.id = imu_id;
    
    // 获取运行状态控制信息
    pthread_mutex_lock(&run_flag_mutex_lock);   // 获取互斥锁
    flag = run_flag;
    pthread_mutex_unlock(&run_flag_mutex_lock); // 释放互斥锁
    
    while(flag)
    {
        udp = openudpsvr(para->path, msg);
        if (udp == NULL)
        {
            printf(" udp server open failed, path = %s, msg = %s\n", para->path, msg);        
        }
        
        if (first_run == true)
        {
            // 上报状态，节点已经准备就绪
            // 为了防止ROS通讯中数据丢失，这里发送三次吧
            udp_stat_msg.status = DEVICE_STATUS_START;
            imu_stat_msg.status = DEVICE_STATUS_START;
            if (pub_stat)
            {
                pub_stat.publish(udp_stat_msg);
                pub_stat.publish(imu_stat_msg);
                
                sleep(1);
                pub_stat.publish(udp_stat_msg);
                pub_stat.publish(imu_stat_msg);
                
                sleep(1);
                pub_stat.publish(udp_stat_msg);
                pub_stat.publish(imu_stat_msg);
            }
            
            first_run = false;   
        }
        
        while(flag && (nr = readudpsvr(udp, buff, UDPBUFSIZE, msg)) != -1)
        {            
            // 回调函数
            if (para->handle)
                para->handle(buff, nr);
            else
                fprintf(stdout, " nr = %d\n", nr);
                
            // 获取运行状态控制信息
            pthread_mutex_lock(&run_flag_mutex_lock);   // 获取互斥锁
            flag = run_flag;
            pthread_mutex_unlock(&run_flag_mutex_lock); // 释放互斥锁
        }
        
        if (0)
        {
            timeget(ep);
            fprintf(stdout, " Warning : no data recieved at %04.0lf %02.0lf %02.0lf %02.0lf %02.0lf %06.3lf\n", 
                ep[0], ep[1], ep[2], ep[3], ep[4], ep[5]);
        }
        
        closeudpsvr(udp);
        
        // 获取运行状态控制信息
        pthread_mutex_lock(&run_flag_mutex_lock);   // 获取互斥锁
        flag = run_flag;
        pthread_mutex_unlock(&run_flag_mutex_lock); // 释放互斥锁
    }
    
    return NULL;
}

void imuHandler_stim300(const unsigned char*buff, const int len)
{
    int     flag = 1;
    raw_stim300_t   rawImu;
    
    imu_count++;
    if (imu_count >= 10000)
        imu_count = 0;
    
    // 上报状态，节点处于运行状态
    common::device_status_ros_msg   udp_stat_msg;
    common::device_status_ros_msg   imu_stat_msg;
        
    udp_stat_msg.type = DEVICE_TYPE_IMU_UDP; udp_stat_msg.id = 0;
    imu_stat_msg.type = DEVICE_TYPE_IMU;     imu_stat_msg.id = imu_id;
    udp_stat_msg.status = DEVICE_STATUS_ACTIVE;
    imu_stat_msg.status = DEVICE_STATUS_ACTIVE;
    if (imu_count%10 == 0 && pub_stat)
    {
        pub_stat.publish(udp_stat_msg);
        pub_stat.publish(imu_stat_msg); 
    }
        
    // 数据输出
    char    fileName[256] = {0};
    static FILE *fp_bin = NULL, *fp_txt = NULL;
    
    if (ep0[0] == 0.0)
        timeget(ep0); // 获取程序开始运行时间
        
    // 第一次需要打开记录文件
    if (fp_bin == NULL)
    {
        sprintf(fileName, "%s/imu_udp_%04.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf.log", 
            work_dir.c_str(), ep0[0], ep0[1], ep0[2], ep0[3], ep0[4], ep0[5]);
        //printf("fileName = %s\n", fileName_bin);
        fp_bin = fopen(fileName, "wb");
    }
    
    // 解码完之后的文本文件
    if (fp_txt == NULL)
    {
        sprintf(fileName, "%s/imu_udp_%04.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf.txt", 
            work_dir.c_str(), ep0[0], ep0[1], ep0[2], ep0[3], ep0[4], ep0[5]);
        //printf("fileName = %s\n", fileName);
        fp_txt = fopen(fileName, "w");    
    }
    
    if (fp_bin != NULL)
    {
        fwrite(buff, sizeof(unsigned char), len, fp_bin);
        fflush(fp_bin);
    }
    
    if (len != 80)
    {
        fprintf(stdout, " imuHandler_stim300 invalid len = %d\n", len);
        return ;
    }
    
    // 数据结构
    //012345678901234567890123456789012345
    //$IMU,000000012,123456,777777,123456,*******(42byte)\r\n
    
    // 解析时间
    flag = flag & sscanf((char *)&buff[5], "%ld", &rawImu.seq);
    flag = flag & sscanf((char *)&buff[15], "%d", &rawImu.week);
    flag = flag & sscanf((char *)&buff[22], "%d", &rawImu.sow);
    flag = flag & sscanf((char *)&buff[29], "%d", &rawImu.usec);
    
    if (!flag)
    {
        printf(" imuHandler_stim300 decode time failed\n");
        return;
    }
    
    // 解析IMU数据
    if (!decode_stim300(buff + 36, 42, &rawImu))
    {
        printf(" imuHandler_stim300 decode_stim300 failed\n");
        return;
    }
    
    
    // 发送ROS消息
    if (pub_data)
    {
        double	sec = rawImu.week*604800.0 + rawImu.sow + rawImu.usec*1.0E-06;
        sensor_msgs::Imu imu_msg;
        imu_msg.header.stamp = ros::Time(sec);
        imu_msg.header.frame_id = "imu";
        imu_msg.linear_acceleration.x = rawImu.DeltaVelocityX;
        imu_msg.linear_acceleration.y = rawImu.DeltaVelocityY;
        imu_msg.linear_acceleration.z = rawImu.DeltaVelocityZ;
        imu_msg.angular_velocity.x  = rawImu.DeltaAngleX;
        imu_msg.angular_velocity.y  = rawImu.DeltaAngleY;
        imu_msg.angular_velocity.z  = rawImu.DeltaAngleZ;
        pub_data.publish(imu_msg);
    }
    
    if (0)
    {
        fprintf(stdout, " %09ld %04d %07d %06d %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %X\n", 
            rawImu.seq, rawImu.week, rawImu.sow, rawImu.usec,
            rawImu.DeltaAngleX, rawImu.DeltaAngleY, rawImu.DeltaAngleZ,
            rawImu.DeltaVelocityX, rawImu.DeltaVelocityY, rawImu.DeltaVelocityZ, rawImu.status);
    }
    
    // 解码完之后写为文本文件
    if (fp_txt != NULL)
    {
        fprintf(fp_txt, " %09ld %04d %07d %06d %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %X\n", 
            rawImu.seq, rawImu.week, rawImu.sow, rawImu.usec,
            rawImu.DeltaAngleX, rawImu.DeltaAngleY, rawImu.DeltaAngleZ,
            rawImu.DeltaVelocityX, rawImu.DeltaVelocityY, rawImu.DeltaVelocityZ, rawImu.status);
        fflush(fp_txt);
    }
    
}

void imuHandler_i300(const unsigned char*buff, const int len)
{
    int     flag = 1;
    raw_hw_i300_t       rawImu;
    
    imu_count++;
    if (imu_count >= 10000)
        imu_count = 0;
    
    // 上报状态，节点处于运行状态
    common::device_status_ros_msg   udp_stat_msg;
    common::device_status_ros_msg   imu_stat_msg;
    udp_stat_msg.type = DEVICE_TYPE_IMU_UDP; udp_stat_msg.id = 0;
    imu_stat_msg.type = DEVICE_TYPE_IMU;     imu_stat_msg.id = imu_id;
    udp_stat_msg.status = DEVICE_STATUS_ACTIVE;
    imu_stat_msg.status = DEVICE_STATUS_ACTIVE;
    if (imu_count%10 == 0 && pub_stat)
    {
        pub_stat.publish(udp_stat_msg);
        pub_stat.publish(imu_stat_msg);
    }
    
    // 数据输出
    char    fileName[256] = {0};
    static FILE *fp_bin = NULL, *fp_txt = NULL;
    
    if (ep0[0] == 0.0)
        timeget(ep0); // 获取程序开始运行时间
        
    // 第一次需要打开记录文件
    if (fp_bin == NULL)
    {
        sprintf(fileName, "%s/imu_udp_%04.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf.log", 
            work_dir.c_str(), ep0[0], ep0[1], ep0[2], ep0[3], ep0[4], ep0[5]);
        //printf("fileName = %s\n", fileName_bin);
        fp_bin = fopen(fileName, "wb");
    }
    
    // 解码完之后的文本文件
    if (fp_txt == NULL)
    {
        sprintf(fileName, "%s/imu_udp_%04.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf.txt", 
            work_dir.c_str(), ep0[0], ep0[1], ep0[2], ep0[3], ep0[4], ep0[5]);
        //printf("fileName = %s\n", fileName);
        fp_txt = fopen(fileName, "w");    
    }
    
    if (fp_bin != NULL)
    {
        fwrite(buff, sizeof(unsigned char), len, fp_bin);
        fflush(fp_bin);
    }   
    
    if (len != 70)
    {
        fprintf(stdout, " imuHandler_stim300 invalid len = %d\n", len);
        return ;
    }
    
    // 数据结构
    //012345678901234567890123456789012345
    //$IMU,000000012,123456,777777,123456,*******(32byte)\r\n
    
    // 解析时间
    flag = flag & sscanf((char *)&buff[5], "%ld", &rawImu.seq);
    flag = flag & sscanf((char *)&buff[15], "%d", &rawImu.week);
    flag = flag & sscanf((char *)&buff[22], "%d", &rawImu.sow);
    flag = flag & sscanf((char *)&buff[29], "%d", &rawImu.usec);
    if (!flag)
    {
        printf(" imuHandler_i300 decode time failed\n");
        return;
    }
    
    // 解析IMU数据
    if (!decode_raw_hw_i300_OXA3(&buff[36], 32, &rawImu))
    {
        printf(" imuHandler_i300 decode_raw_hw_i300_OXA3 failed\n");
        return;
    }
    
    // 发送ROS消息
    if (pub_data)
    {
        double	sec = rawImu.week*604800.0 + rawImu.sow + rawImu.usec*1.0E-06;
        sensor_msgs::Imu imu_msg;
        imu_msg.header.stamp = ros::Time(sec);
        imu_msg.header.frame_id = "imu";
        imu_msg.linear_acceleration.x = rawImu.DeltaVelocityX;
        imu_msg.linear_acceleration.y = rawImu.DeltaVelocityY;
        imu_msg.linear_acceleration.z = rawImu.DeltaVelocityZ;
        imu_msg.angular_velocity.x  = rawImu.DeltaAngleX;
        imu_msg.angular_velocity.y  = rawImu.DeltaAngleY;
        imu_msg.angular_velocity.z  = rawImu.DeltaAngleZ;
        pub_data.publish(imu_msg);
    }
    
    if (0)
    {
        fprintf(stdout, " %09ld %04d %07d %06d %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %X\n", 
            rawImu.seq, rawImu.week, rawImu.sow, rawImu.usec,
            rawImu.DeltaAngleX, rawImu.DeltaAngleY, rawImu.DeltaAngleZ,
            rawImu.DeltaVelocityX, rawImu.DeltaVelocityY, rawImu.DeltaVelocityZ, rawImu.status);
    }
    
    // 解码完之后写为文本文件
    if (fp_txt != NULL)
    {
        fprintf(fp_txt, " %09ld %04d %07d %06d %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %X\n", 
            rawImu.seq, rawImu.week, rawImu.sow, rawImu.usec,
            rawImu.DeltaAngleX, rawImu.DeltaAngleY, rawImu.DeltaAngleZ,
            rawImu.DeltaVelocityX, rawImu.DeltaVelocityY, rawImu.DeltaVelocityZ, rawImu.status);
        fflush(fp_txt);
    }
    
    return;
}

void stop_run(int signum)
{
    // 改变运行状态控制信息
    pthread_mutex_lock(&run_flag_mutex_lock);   // 获取互斥锁
    run_flag = false;
    pthread_mutex_unlock(&run_flag_mutex_lock); // 释放互斥锁
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "imu_data_rcv_node");
    ros::NodeHandle nh("~");
    
    // Ctrl+C
    signal(SIGINT,  stop_run);
    signal(SIGTERM, stop_run);
    
    run_flag = true;
    pthread_mutex_init(&run_flag_mutex_lock, NULL);
    
    // 配置参数
    nh.param("work_dir",    work_dir,  std::string("~"));
    nh.param("imu_id",      imu_id,  -1);
    nh.param("imu_type",    imu_type,  -1); // 0 - stim300; 1 - i300
    nh.param("listen_port", listen_port,  6000);
    nh.param("topic_pub",   topic_pub, std::string("/imu_raw"));
    
    // 参数检查
    if (work_dir == "" || work_dir == "~" || work_dir == "/xxx")
        work_dir = std::string(std::getenv("HOME"));
    if (topic_pub == "" || topic_pub == "/xxx")
        topic_pub = "/imu_raw";
    if (imu_id < 0 || imu_id > MAX_IMU_COUNT - 1)
    {
        printf(" %s invalid imu_id = %d\n", ros::this_node::getName().c_str(), imu_id);
        return 0;
    }
    
    printf("work_dir    = %s\n", work_dir.c_str());
    printf("imu_id      = %d\n", imu_id);
    printf("imu_type    = %d\n", imu_type);
    printf("listen_port = %d\n", listen_port);
    printf("topic_pub   = %s\n", topic_pub.c_str());
    
    // ROS发布消息
    pub_stat = nh.advertise<common::device_status_ros_msg>(DEVICE_STATUS_ROS_TOPIC, 1); // 
    pub_data = nh.advertise<sensor_msgs::Imu>(topic_pub, 1); // 
 
    // 线程参数
    pthread_t           thread_udp;
    thread_udpsvr_arg   thread_udp_arg;
    sprintf(thread_udp_arg.path, ":%d", listen_port);
    if (imu_type == 0)
        thread_udp_arg.handle = std::bind(&imuHandler_stim300, std::tr1::placeholders::_1, std::tr1::placeholders::_2);
    else if (imu_type == 1)
        thread_udp_arg.handle = std::bind(&imuHandler_i300, std::tr1::placeholders::_1, std::tr1::placeholders::_2);
    else
    {
        printf(" %s invalid imu_type = %d\n", ros::this_node::getName().c_str(), imu_type);
        return 0;
    }
       
    // 开启线程
    run_flag = true;
    if (pthread_create(&thread_udp, NULL, thread_udprcv, (void*)&thread_udp_arg))
    {
        printf(" thread create error\n");
        return 0;
    }
    
    // 等待线程结束
    pthread_join(thread_udp, NULL);
    
    return 0;
}