#include <stdio.h>
#include <unistd.h>
#include <signal.h>

#include <iostream>
#include <string>
#include <tr1/functional>
using namespace std::tr1::placeholders;

#include "udp_t.h"
#include "time_sync_msg.h"
#include "honeywell_i300.hpp"
#include "time_utils.hpp"

#ifdef _WINDOWS  
#define ACCESS _access  
#define MKDIR(a) _mkdir((a)) 
#define STRDUP _strdup
#define STRTOK(str, delimiters, next_token)  strtok_s(str, delimiters, next_token)
#else
#define ACCESS access  
#define MKDIR(a) mkdir((a),0755)
#define STRDUP strdup
#define STRTOK(str, delimiters, next_token)  strtok(str, delimiters)
#endif

static int  udp_rcv_run_flag = 0;
pthread_t           udprcv_thread_id;
static pthread_mutex_t  udprcv_thread_mutex_lock;

static void timeget(double ep[6])
{
    if (ep == NULL)
        return;
    
#ifdef WIN32
    SYSTEMTIME ts;
    
    GetSystemTime(&ts); /* utc */
    ep[0]=ts.wYear; ep[1]=ts.wMonth;  ep[2]=ts.wDay;
    ep[3]=ts.wHour; ep[4]=ts.wMinute; ep[5]=ts.wSecond+ts.wMilliseconds*1E-3;
#else
    struct timeval tv;
    struct tm *tt;
    
    if (!gettimeofday(&tv,NULL)&&(tt=gmtime(&tv.tv_sec))) {
        ep[0]=tt->tm_year+1900; ep[1]=tt->tm_mon+1; ep[2]=tt->tm_mday;
        ep[3]=tt->tm_hour; ep[4]=tt->tm_min; ep[5]=tt->tm_sec+tv.tv_usec*1E-6;
    }
#endif
    
    return;
}

void *thread_udprcv(void *args)
{
    thread_udpsvr_arg* para = (thread_udpsvr_arg*)args;
    
    int     nr = 0, runflag = 0;
    double  ep[6] = {0.0};
    udp_t   *udp = NULL;
    char    msg[UDPBUFSIZE] = { 0 };
    unsigned char buff[UDPBUFSIZE]={0};

    pthread_mutex_lock(&udprcv_thread_mutex_lock);   // 获取互斥锁
    runflag = udp_rcv_run_flag;
    pthread_mutex_unlock(&udprcv_thread_mutex_lock); // 释放互斥锁
    
    while(runflag)
    {
        udp = openudpsvr(para->path, msg);
        if (udp == NULL)
        {
            printf(" udp server open failed, path = %s, msg = %s\n", para->path, msg);
            continue;      
        }
        
        while(runflag && (nr = readudpsvr(udp, buff, UDPBUFSIZE, msg)) != -1)
        {
            if (para->handle)
                para->handle(buff, nr);
            else
                printf("%s", buff);
                
            pthread_mutex_lock(&udprcv_thread_mutex_lock);   // 获取互斥锁
            runflag = udp_rcv_run_flag;
            pthread_mutex_unlock(&udprcv_thread_mutex_lock); // 释放互斥锁
        }
        
        timeget(ep);
        fprintf(stdout, " Warning : no imu 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(&udprcv_thread_mutex_lock);   // 获取互斥锁
        runflag = udp_rcv_run_flag;
        pthread_mutex_unlock(&udprcv_thread_mutex_lock); // 释放互斥锁
    }
    
    return NULL;
}

void callback(const unsigned char *buff, const int len)
{
    int     flag = 1, week = 0;
    double  ep[6] = {0.0}, sow = 0.0;
    raw_hw_i300_t       rawImu;
    
    static std::string workdir = "";
    static char    fileName_bin[256] = {0}, fileName_txt[256] = {0};
    static FILE    *fp_bin = NULL, *fp_txt = NULL;
    
    timeget(ep);
    epoch2gpst(ep, &week, &sow);
    
    if (workdir == "" || workdir == "~" || workdir == "/xxx")
        workdir = std::string(std::getenv("HOME"));
            
    // 原始的二进制文件
    if (1 && fp_bin == NULL)
    {
        sprintf(fileName_bin, "%s/rawImu_%04.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf.bin", 
            workdir.c_str(), ep[0], ep[1], ep[2], ep[3], ep[4], ep[5]);
        printf("fileName_bin = %s\n", fileName_bin);
        fp_bin = fopen(fileName_bin, "wb");
    }
    
    // 解码完之后的文本文件
    if (1 && fp_txt == NULL)
    {
        sprintf(fileName_txt, "%s/rawImu_%04.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf.txt", 
            workdir.c_str(), ep[0], ep[1], ep[2], ep[3], ep[4], ep[5]);
        printf("fileName_txt = %s\n", fileName_txt);
        fp_txt = fopen(fileName_txt, "w");    
    }
    
    // 原始数据输出到文件
    if (1 && fp_bin)
    {
        fwrite(buff, sizeof(unsigned char), len, fp_bin);
        fflush(fp_bin);
    }
    
    // 解析IMU数据
    if (!decode_raw_hw_i300_OXA3(&buff[36], 32, &rawImu))
    {
        printf(" imuHandler decode_raw_hw_i300_OXA3 failed\n");
        return;
    }
    
    // 解析时间
    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 decode time failed\n");
        return;
    }
    
    if (1 && fp_txt != NULL)
    {
        fprintf(fp_txt, " %4d %13.6lf %09ld %04d %07d %06d %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %13.10lf %X\n", 
            week, sow, 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 sighandler(int signum)
{
    pthread_mutex_lock(&udprcv_thread_mutex_lock);   // 获取互斥锁
    udp_rcv_run_flag = 0;
    pthread_mutex_unlock(&udprcv_thread_mutex_lock); // 释放互斥锁 
}

void print_usage(int argc, char *argv[])
{
    printf("\n");
    printf(" Usage : %s port\n", argv[0]);
    printf("\n");
}

int main(int argc, char *argv[])
{
    short   port = -1;
    char    path[8] = { 0 };
    thread_udpsvr_arg   thread_arg;
    
    signal(SIGINT, sighandler);
    
    if (argc < 2 || sscanf(argv[1], "%hd", &port) != 1)
    {
        print_usage(argc, argv);
        return 0;
    }
    
    if (port < 0 || port > 65535)
    {
        fprintf(stderr, " error invalid port %d\n", port);
        return 0;
    }
    
    sprintf(path, ":%hd", port);
    printf("path = %s\n", path);

    // 参数初始化
    //memset(&thread_arg, 0, sizeof(thread_arg));
    strcpy(thread_arg.path, path);
    thread_arg.handle = std::bind(&callback, _1, _2);

    // 开启线程
    udp_rcv_run_flag = 1;
    pthread_mutex_init(&udprcv_thread_mutex_lock, NULL);
    if (pthread_create(&udprcv_thread_id, NULL, thread_udprcv, (void*)&thread_arg))
    {
        printf(" thread create error\n");
        return 0;
    }

    pthread_join(udprcv_thread_id, NULL);
    
    return 0;
}