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

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

#define MIN_OPT 0
#define MAX_OPT 6

const char *defalut_server_ip    = "192.168.4.179";
const short defalut_server_port  = 4000;
static unsigned char CAM_START[] = {0x24, 0x03, 0x01, 0x00, 0x44, 0xFB}; // 24 03 01 01 01 D3 C5
static unsigned char CAM_STOP[]  = {0x24, 0x03, 0x02, 0x00, 0xB4, 0xFB}; // 24 03 02 01 01 D3 35

static unsigned char IMU_START[] = {0x24, 0x04, 0x01, 0x00, 0x85, 0x4A}; // 24 04 01 01 01 A7 C4
static unsigned char IMU_STOP[]  = {0x24, 0x04, 0x02, 0x00, 0x75, 0x4A}; // 24 04 02 01 01 A7 34

static unsigned char MCS_START[] = {0x24, 0x02, 0x01, 0x00, 0x84, 0xAA}; // 24 02 01 01 01 2F C4
static unsigned char MCS_STOP[]  = {0x24, 0x02, 0x02, 0x00, 0x74, 0xAA}; // 24 02 02 01 01 2F 34

void start_mcs(const char *path)
{
    int     ns = 0, nr = 0;
    char    msg[TCPBUFSIZE] = { 0 };
    unsigned char buff[TCPBUFSIZE]={0};
    
    if (path == NULL || strlen(path) == 0)
        return ;
    
    tcpcli_t   *tcpcli = NULL;
    tcpcli = opentcpcli(path, msg);
    if (tcpcli == NULL)
    {
        printf(" start_mcs tcp client open failed, path = %s, msg = %s\n", path, msg);
        return;     
    }  
    
    ////////////////////////////--MCS--///////////////////////////////
    ns = writetcpcli(tcpcli, MCS_START, sizeof(MCS_START), msg);
    if (ns <= 0)
    {
        printf(" start_mcs writetcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
        
    nr = readtcpcli(tcpcli, buff, TCPBUFSIZE, msg);
    if (nr <= 0)
    {
        printf(" start_mcs readtcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
    ////////////////////////////--MCS--///////////////////////////////
    
    closetcpcli(tcpcli);
    printf(" start mcs successfully\n");
    
    return; 
}

void stop_mcs(const char *path)
{
    int     ns = 0, nr = 0;
    char    msg[TCPBUFSIZE] = { 0 };
    unsigned char buff[TCPBUFSIZE]={0};
    
    if (path == NULL || strlen(path) == 0)
        return ;
    
    tcpcli_t   *tcpcli = NULL;
    tcpcli = opentcpcli(path, msg);
    if (tcpcli == NULL)
    {
        printf(" stop_all tcp client open failed, path = %s, msg = %s\n", path, msg);
        return;     
    }
    
    ////////////////////////////--MCS--///////////////////////////////
    ns = writetcpcli(tcpcli, MCS_STOP, sizeof(MCS_STOP), msg);
    if (ns <= 0)
    {
        printf(" stop_all writetcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
        
    nr = readtcpcli(tcpcli, buff, TCPBUFSIZE, msg);
    if (nr <= 0)
    {
        printf(" stop_all readtcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
    ////////////////////////////--CAM--///////////////////////////////
    
    closetcpcli(tcpcli);
    printf(" stop all successfully\n");

    return;
}

void start_imu(const char *path)
{
    int     ns = 0, nr = 0;
    char    msg[TCPBUFSIZE] = { 0 };
    unsigned char buff[TCPBUFSIZE]={0};
    
    if (path == NULL || strlen(path) == 0)
        return ;
    
    tcpcli_t   *tcpcli = NULL;
    tcpcli = opentcpcli(path, msg);
    if (tcpcli == NULL)
    {
        printf(" start_imu tcp client open failed, path = %s, msg = %s\n", path, msg);
        return;     
    }
    
    ////////////////////////////--IMU--///////////////////////////////
    ns = writetcpcli(tcpcli, IMU_START, sizeof(IMU_START), msg);
    if (ns <= 0)
    {
        printf(" start_imu writetcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
        
    nr = readtcpcli(tcpcli, buff, TCPBUFSIZE, msg);
    if (nr <= 0)
    {
        printf(" start_imu readtcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
    ////////////////////////////--IMU--///////////////////////////////
    
    ////////////////////////////--MCS--///////////////////////////////
    ns = writetcpcli(tcpcli, MCS_START, sizeof(MCS_START), msg);
    if (ns <= 0)
    {
        printf(" start_imu writetcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
        
    nr = readtcpcli(tcpcli, buff, TCPBUFSIZE, msg);
    if (nr <= 0)
    {
        printf(" start_imu readtcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
    ////////////////////////////--MCS--///////////////////////////////
    
    closetcpcli(tcpcli);
    printf(" start imu successfully\n");
    
    return;
}

void stop_imu(const char *path)
{
    int     ns = 0, nr = 0;
    char    msg[TCPBUFSIZE] = { 0 };
    unsigned char buff[TCPBUFSIZE]={0};
    
    if (path == NULL || strlen(path) == 0)
        return ;
    
    tcpcli_t   *tcpcli = NULL;
    tcpcli = opentcpcli(path, msg);
    if (tcpcli == NULL)
    {
        printf(" stop_imu tcp client open failed, path = %s, msg = %s\n", path, msg);
        return;     
    }
    
    ////////////////////////////--IMU--///////////////////////////////
    ns = writetcpcli(tcpcli, IMU_STOP, sizeof(IMU_STOP), msg);
    if (ns <= 0)
    {
        printf(" stop_imu writetcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
        
    nr = readtcpcli(tcpcli, buff, TCPBUFSIZE, msg);
    if (nr <= 0)
    {
        printf(" stop_imu readtcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
    ////////////////////////////--IMU--///////////////////////////////
    
    closetcpcli(tcpcli);
    printf(" stop imu successfully\n");

    return;
}

void start_cam(const char *path)
{
    int     ns = 0, nr = 0;
    char    msg[TCPBUFSIZE] = { 0 };
    unsigned char buff[TCPBUFSIZE]={0};
    
    if (path == NULL || strlen(path) == 0)
        return ;
    
    tcpcli_t   *tcpcli = NULL;
    tcpcli = opentcpcli(path, msg);
    if (tcpcli == NULL)
    {
        printf(" start_cam tcp client open failed, path = %s, msg = %s\n", path, msg);
        return;     
    }
    
    ////////////////////////////--CAM--///////////////////////////////
    ns = writetcpcli(tcpcli, CAM_START, sizeof(CAM_START), msg);
    if (ns <= 0)
    {
        printf(" start_cam writetcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
        
    nr = readtcpcli(tcpcli, buff, TCPBUFSIZE, msg);
    if (nr <= 0)
    {
        printf(" start_cam readtcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
    ////////////////////////////--CAM--///////////////////////////////
    
    ////////////////////////////--MCS--///////////////////////////////
    ns = writetcpcli(tcpcli, MCS_START, sizeof(MCS_START), msg);
    if (ns <= 0)
    {
        printf(" start_cam writetcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
        
    nr = readtcpcli(tcpcli, buff, TCPBUFSIZE, msg);
    if (nr <= 0)
    {
        printf(" start_cam readtcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
    ////////////////////////////--MCS--///////////////////////////////
    
    closetcpcli(tcpcli);
    printf(" start cam successfully\n");
    
    return;
}

void stop_cam(const char *path)
{   
    int     ns = 0, nr = 0;
    char    msg[TCPBUFSIZE] = { 0 };
    unsigned char buff[TCPBUFSIZE]={0};
    
    if (path == NULL || strlen(path) == 0)
        return ;
    
    tcpcli_t   *tcpcli = NULL;
    tcpcli = opentcpcli(path, msg);
    if (tcpcli == NULL)
    {
        printf(" stop_cam tcp client open failed, path = %s, msg = %s\n", path, msg);
        return;     
    }
    
    ////////////////////////////--CAM--///////////////////////////////
    ns = writetcpcli(tcpcli, CAM_STOP, sizeof(CAM_STOP), msg);
    if (ns <= 0)
    {
        printf(" stop_cam writetcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
        
    nr = readtcpcli(tcpcli, buff, TCPBUFSIZE, msg);
    if (nr <= 0)
    {
        printf(" stop_cam readtcpcli failed, msg = %s\n", msg);
        closetcpcli(tcpcli);
        return;   
    }
    ////////////////////////////--CAM--///////////////////////////////
    
    closetcpcli(tcpcli);
    printf(" stop cam successfully\n");

    return;
}

void start_all(const char *path)
{
    start_mcs(path); // 开启主控
    sleep(1);        // 休眠1s
    start_cam(path); // 开启相机
    sleep(1);        // 休眠1s
    start_imu(path); // 开启IMU
    return;
}

void stop_all(const char *path)
{
    stop_cam(path); // 开启相机
    sleep(1);        // 休眠1s
    stop_imu(path); // 开启IMU
    return;
}

void print_usage(int argc, char *argv[])
{
    printf("\n");
    printf(" Usage : %s option [-ip ip] [-port port]\n", argv[0]);
    printf("    option\n");
    
    // 开启指令
    printf("            1 - start mcs\n");
    printf("            2 - start both imu and camera\n");
    printf("            3 - start camera only\n");
    printf("            4 - start imu only\n");
    
    // 关闭指令
    printf("            5 - stop  mcs\n");
    printf("            6 - stop  both imu and camera\n");
    printf("            7 - stop  camera only\n");
    printf("            8 - stop  imu only\n");
    
    // ip和端口
    printf("\n");
    printf("    ip      default is %s\n", defalut_server_ip);
    printf("    port    default is %hd\n", defalut_server_port);
    printf("\n");
}

int main(int argc, char *argv[])
{
    int     option = 0;
    char    ipaddr[16] = {0};
    short   port = -1;
    char    path[32] = { 0 };
    const char *ip_ptr = defalut_server_ip;
    
    if (argc < 2 || sscanf(argv[1], "%d", &option) != 1)
    {
        print_usage(argc, argv);
        return 0;
    }
    
    if (option < MIN_OPT || option > MAX_OPT)
    {
        fprintf(stderr, " error invalid option %d\n", option);
        return 0;
    }
    
    for(int i = 2; i <argc; i++)
    {
        if (!strcmp(argv[i], "-ip") && i < argc - 1 && argv[i+1][0] != '-' && strlen(argv[i+1]) < 16)
            strcpy(ipaddr, argv[++i]);
        if (!strcmp(argv[i], "-port") && i < argc - 1 && argv[i+1][0] != '-')
            sscanf(argv[++i], "%hd", &port);
    }
    
    if (strlen(ipaddr) != 0)
        ip_ptr = ipaddr;
    
    sprintf(path, "%s:%hd", ip_ptr, port > 0 ? port : defalut_server_port);
    printf(" path = %s\n", path);
     
    switch(option)
    {
        case 0: print_usage(argc, argv); break;
        
        // 开启指令
        case 1: start_mcs(path); break;
        case 2: start_all(path); break;
        case 3: start_cam(path); break;
        case 4: start_imu(path); break;
        
        // 关闭指令
        case 5: stop_mcs(path);  break;
        case 6: stop_all(path);  break;
        case 7: stop_cam(path);  break;
        case 8: stop_imu(path);  break;
        
        // 错误指令
        default: fprintf(stderr, " error invalid option %d\n", option);
    }
    
    return 0;
}