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

#include "serial_t.h"
#include "X_Define_Internal.h"
#include "common_utils.hpp"
#include "device_status.hpp"

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

static std::string work_dir  = "~";
static std::string serial_path = "";
static int gps_id = -1;

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

// 程序开始时间
static double   ep0[6] = { 0.0 };
static char     logFileName[256] = {0};
static FILE*    fp_log  = NULL;

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, "mcs_serial_node");
    ros::NodeHandle nh("~");
    
    const int       mode = SERIAL_MODE_RW;
    char            msg[4096]={0};
    int             nr=0, ns=0;
    unsigned char   buff[4096] = {0};
    serial_t        *serial = NULL;
    bool            flag =false; 
    
    // 配置参数
    nh.param("work_dir",    work_dir,    std::string("/xxx"));
    nh.param("serial_path", serial_path, std::string(""));
    
    fprintf(stdout, "work_dir     = %s\n", work_dir.c_str());
    fprintf(stdout, "serial_path  = %s\n", serial_path.c_str());
    
    // 参数检查
    if (work_dir == "" || work_dir == "~" || work_dir == "/xxx")
        work_dir = std::string(std::getenv("HOME"));
    
    timeget(ep0);
    if (fp_log == NULL)
    {
        sprintf(logFileName, "%s/%s_%04.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf_%02.0lf.log",
            work_dir.c_str(), ros::this_node::getName().c_str(), ep0[0], ep0[1], ep0[2], ep0[3], ep0[4], ep0[5]);
        fp_log = fopen(logFileName, "w"); 
    }
    
    run_flag = true;
    pthread_mutex_init(&run_flag_mutex_lock, NULL);
    
    signal(SIGINT,  stop_run);
    signal(SIGTERM, stop_run);
    
    serial = openserial(serial_path.c_str(), mode, msg);
    if(serial == NULL)
    {
        printf("open serial error, %s, msg = %s\n", serial_path.c_str(), msg);
        return -1;
    }
    
    pthread_mutex_lock(&run_flag_mutex_lock);   // 获取互斥锁
    flag = run_flag;
    pthread_mutex_unlock(&run_flag_mutex_lock); // 释放互斥锁        
    while(run_flag)
    {
        memset(buff, 0, sizeof(buff));
        nr=readserial(serial, buff, 4096, msg);
        if(nr <= 0)
            continue;
        
        if (fp_log != NULL)
        {
            fprintf(fp_log, "[%.3lf] %s", timeget_syssec(), buff);  
            fflush(fp_log);   
        }
        
        pthread_mutex_lock(&run_flag_mutex_lock);   // 获取互斥锁
        flag = run_flag;
        pthread_mutex_unlock(&run_flag_mutex_lock); // 释放互斥锁  
    }
    
    
    closeserial(serial);
    return 0;
}