#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdarg.h>
#include <string.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <errno.h> // errno, perror()
#include <unistd.h> // close()
#include <termios.h>
#include <time.h>
#include <sys/time.h>

#define printa(fmt,...)  printf("%s:%d, %s | "fmt, __FILE__, __LINE__, __FUNCTION__, ##__VA_ARGS__)
#define printe(fmt,...)  printf("%s:%d, %s | "fmt, __FILE__, __LINE__, __FUNCTION__, ##__VA_ARGS__)
#define printw(fmt,...)  printf("%s:%d, %s | "fmt, __FILE__, __LINE__, __FUNCTION__, ##__VA_ARGS__)
#define printi(fmt,...)  printf("%s:%d, %s | "fmt, __FILE__, __LINE__, __FUNCTION__, ##__VA_ARGS__)
#define printd(fmt,...)  printf("%s:%d, %s | "fmt, __FILE__, __LINE__, __FUNCTION__, ##__VA_ARGS__)
#define printv(fmt,...)  printf("%s:%d, %s | "fmt, __FILE__, __LINE__, __FUNCTION__, ##__VA_ARGS__)

#define MODBUS_CMD_0x20     0x20
#define MODBUS_CMD_0x21     0x21
#define MODBUS_CMD_0x22     0x22
#define MODBUS_CMD_0x23     0x23
#define MODBUS_CMD_0x24     0x24
#define MODBUS_CMD_0x25     0x25

typedef struct com_ttySx
{
    int fd;
    char devname[256];

    int debug;

    struct termios save_termios;
    struct termios curr_termios;

    pthread_mutex_t mutex;
}com_ttySx_t;

static com_ttySx_t router4G;

int64_t systime_ms(void)
{
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (int64_t) tv.tv_sec * 1000 + tv.tv_usec / 1000;
}

int printline(uint8_t *data, int N, const char* fmt, ...)
{
    if ( N > 0 && data != NULL)
    {
        va_list valist;
        va_start(valist, fmt);
        int len = vprintf(fmt, valist);
        va_end(valist);
        printf("(%3d):", N);
        int i;
        for (i=0; i<N; i++)
        {
            printf(" %02X", data[i]);
        }
        printf("\n");
    }
    return N;
}

int com_ttySx_init(com_ttySx_t* dev, const char* devname, int baudrate, int databits, int parbits, int stopbits)
{
    printi("devname:%s, B:%d, %d, %c, %d\n", devname, baudrate, databits, parbits, stopbits);
    sprintf(dev->devname, "%s", devname);
    dev->debug = 1;
    dev->fd = open(devname, O_RDWR | O_NOCTTY | O_SYNC);//| O_NOCTTY | O_NDELAY
    if (dev->fd < 0) {
        printe("tty:%s\n",dev->devname);
        return errno;
    }
    memset(&dev->save_termios, 0, sizeof(dev->save_termios));
    if (tcgetattr(dev->fd, &dev->save_termios) != 0) 
    {
        printe("tty:%s\n",dev->devname);
        return errno;
    }
    // dev->curr_termios = dev->save_termios;
    memset(&dev->curr_termios, 0, sizeof(dev->curr_termios));

    int baud = 0;
    switch (baudrate) {
        case 1500000: baud = B1500000; break;
        case 1152000: baud = B1152000; break;
        case 1000000: baud = B1000000; break;
        case 921600:  baud = B921600; break;
        case 576000:  baud = B576000; break;
        case 500000:  baud = B500000; break;
        case 460800:  baud = B460800; break;
        case 230400:  baud = B230400; break;
        case 115200:  baud = B115200; break;
        case 57600:   baud = B57600;  break;
        case 38400:   baud = B38400;  break;
        case 19200:   baud = B19200;  break;
        case 9600:    baud = B9600;   break;
        default: baud = B4800;
    }
    
    cfsetospeed(&dev->curr_termios, baud);
    cfsetispeed(&dev->curr_termios, baud);
    
    dev->curr_termios.c_cflag |= CLOCAL | CREAD; /* ignore modem control lines and enable receiver */
    dev->curr_termios.c_cflag &= ~CSIZE;
    switch(databits)
    {
        case 5: dev->curr_termios.c_cflag |= CS5; break;
        case 6: dev->curr_termios.c_cflag |= CS6; break;
        case 7: dev->curr_termios.c_cflag |= CS7; break;
        default: dev->curr_termios.c_cflag |= CS8;
    }
    
    switch(parbits)
    {
        case 'o':
        case 'O': 
            dev->curr_termios.c_cflag |= PARENB; 
            dev->curr_termios.c_cflag |= PARODD; 
            dev->curr_termios.c_iflag |= INPCK; 
            dev->curr_termios.c_iflag &=~ISTRIP; 
            break;
        case 'e':
        case 'E': 
            dev->curr_termios.c_cflag |= PARENB; 
            dev->curr_termios.c_cflag &=~PARODD; 
            dev->curr_termios.c_iflag |= INPCK; 
            dev->curr_termios.c_iflag &=~ISTRIP; 
            break;
        case 'n':
        case 'N':
        default:  
            dev->curr_termios.c_cflag &=~PARENB; 
            dev->curr_termios.c_cflag &=~PARODD;
    }
    dev->curr_termios.c_lflag &=~ICANON;//原始模式 
    //dev->curr_termios.c_lflag |=ICANON; //标准模式
    dev->curr_termios.c_cflag &= ~CRTSCTS;
    switch(stopbits)
    {
        case 1:dev->curr_termios.c_cflag &= ~CSTOPB;break;
        default:dev->curr_termios.c_cflag |= CSTOPB;break;
    }

	/* set timeout in deciseconds for non-canonical read */
	dev->curr_termios.c_cc[VTIME] = 0;
	/* set minimum number of characters for non-canonical read */
	dev->curr_termios.c_cc[VMIN] = 0;
	/* flushes data received but not read */
	tcflush(dev->fd, TCIOFLUSH);

    if (tcsetattr(dev->fd, TCSANOW, &dev->curr_termios) != 0) 
    {
        printe("tty:%s\n",dev->devname);
        return errno;
    }

    return 0;
}
void com_ttySx_exit(com_ttySx_t* dev)
{
    if (dev)
    {
        if (dev->fd >= 0)
        {
            if (tcsetattr(dev->fd, TCSANOW, &dev->save_termios) != 0)
            {
                printe("tty:%s\n",dev->devname);
            }
            close(dev->fd);
            dev->fd = -1;
        }
    }
}
int com_ttySx_send(com_ttySx_t* dev, char* txbuff, int txlen)
{
    if (dev)
    {
        if (dev->fd > 0)
        {
            if (dev->debug) printline(txbuff, txlen, "%s->send", dev->devname);
            int ret = write(dev->fd, txbuff, txlen);
            if (ret < 0)
            {
                close(dev->fd);
                dev->fd = -1;
            }
            return ret;
        }
        return -2;
    }
    return -1;
}

int com_ttySx_flush(com_ttySx_t* dev)
{
    if (dev)
    {
        if (dev->fd > 0)
        {
            return tcflush(dev->fd, TCIOFLUSH);
        }
        return -2;
    }
    return -1;
}

int com_ttySx_recv_with_timeout(com_ttySx_t* dev, char* rxbuff, int max_bytes, int timeout)
{
    if (dev)
    {
        if (dev->fd >0)
        {
            int ret, i;
            int read_bytes = 0;
            int64_t timeout_ms = systime_ms() + timeout;
            fd_set rset;
            struct timeval tv;

            while(read_bytes < max_bytes)
            {
                int64_t remain_ms = timeout_ms - systime_ms();
                FD_ZERO(&rset);
                FD_SET(dev->fd, &rset);
                tv.tv_sec = remain_ms/1000;
                tv.tv_usec = (remain_ms%1000) * 1000;
                switch(select(dev->fd + 1, &rset, NULL, NULL, &tv))
                {
                    case 0:
                        if (dev->debug) printline(rxbuff, read_bytes, "%s->recv", dev->devname);
                        return read_bytes;
                    case -1:
                        printe("tty:%s\n",dev->devname);
                        close(dev->fd);
                        dev->fd = -1;
                        return -1;
                    default:
                        if(FD_ISSET(dev->fd, &rset))
                        {
                            int readb = read(dev->fd, &rxbuff[read_bytes], (max_bytes - read_bytes));
                            if (readb>=0)
                            {
                                read_bytes += readb;
                            }else{
                                printe("tty:%s, read:%d, errno:%d\n", dev->devname, readb, errno);
                            }
                        }else{
                            printe("tty:%s\n", dev->devname);
                        }
                }
            }
            if (dev->debug) printline(rxbuff, read_bytes, "%s->recv", dev->devname);
            return read_bytes;
        }
        return -2;
    }
    return -1;
}

/* Table of CRC values for high-order byte */
static const uint8_t table_crc_hi[] = {
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40};

/* Table of CRC values for low-order byte */
static const uint8_t table_crc_lo[] = {
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5,
    0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B,
    0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE,
    0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6,
    0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
    0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
    0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8,
    0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C,
    0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21,
    0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
    0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A,
    0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
    0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7,
    0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51,
    0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
    0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98,
    0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D,
    0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
    0x41, 0x81, 0x80, 0x40};

uint16_t modbus_crc16(uint8_t *buffer, int length)
{
    uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
    uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
    unsigned int i;        /* will index into CRC lookup */

    /* pass through message buffer */
    while (length--) {
        i = crc_lo ^ *buffer++; /* calculate the CRC  */
        crc_lo = crc_hi ^ table_crc_hi[i];
        crc_hi = table_crc_lo[i];
    }

    return (crc_hi << 8 | crc_lo);
}

int router4G_init(const char* devname, int baudrate, int databits, int parbits, int stopbits)
{
    com_ttySx_t *tty = &router4G;
    com_ttySx_init(tty, devname, 115200, 8, 'N', 1);
    printd("step{fd:%d, debug:%d}\n", tty->fd, tty->debug);
}

void router4G_exit(void)
{
    com_ttySx_t *tty = &router4G;
    com_ttySx_exit(tty);
    printd("step{fd:%d, debug:%d}\n", tty->fd, tty->debug);
}

static int router4G_waitACK(com_ttySx_t *dev, char* rxbuff, uint8_t cmd, int timeout)
{
    printd("step\n");
    int nbyte = com_ttySx_recv_with_timeout(dev, rxbuff, 5, timeout);
    if (nbyte == 5)
    {
        printd("step\n");
        if (rxbuff[0]==0x1)
        {
            printd("step\n");
            if (rxbuff[1]==cmd)
            {
                printd("step\n");
                int rbyte = rxbuff[2];
                int xbyte = 0;
                if (rbyte > 0) { xbyte = com_ttySx_recv_with_timeout(dev, &rxbuff[5], rbyte, timeout); }
                if (xbyte == rbyte)
                {
                    printd("step\n");
                    nbyte = rxbuff[2] + 3;
                    uint16_t crcr = (rxbuff[nbyte+1]<<8)|rxbuff[nbyte];
                    uint16_t crcv = modbus_crc16(rxbuff, nbyte);
                    if (crcr==crcv)
                    {
                        printd("step\n");
                        return nbyte;
                    }
                }
            }
            if (rxbuff[1]==(cmd|0x80))
            {
                printd("step\n");
                int errcode = rxbuff[2];
                return -errcode;
            }
        }
    }
    return -1;
}

int router4G_Switch2WAN_DHCP(int mtu)
{
    int tx_bytes = 0;
    int rx_bytes = 0;
    uint8_t buff[128];
    com_ttySx_t *tty = &router4G;
    com_ttySx_flush(tty);
    printd("step\n");

    buff[tx_bytes++] = 0x1;
    buff[tx_bytes++] = MODBUS_CMD_0x20;
    buff[tx_bytes++] = tx_bytes;
    tx_bytes += sprintf(&buff[tx_bytes], "dhcp=1");
    if (mtu > 0) { tx_bytes += sprintf(&buff[tx_bytes], ",mut=%d", mtu); }
    buff[2] = tx_bytes-3;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_send(tty, buff, tx_bytes);
    int nbyte = router4G_waitACK(tty, buff, MODBUS_CMD_0x20, 3000);
    if (nbyte > 0)
    {
        return 0;
    }
    return -1;
}

int router4G_Switch2WAN_StaticIP(const char *ipaddr, const char *netmask, const char* gateway, const char* dns1, const char *dns2, int mtu)
{
    int tx_bytes = 0;
    int rx_bytes = 0;
    uint8_t buff[256];
    com_ttySx_t *tty = &router4G;
    com_ttySx_flush(tty);
    printd("step\n");

    buff[tx_bytes++] = 0x1;
    buff[tx_bytes++] = MODBUS_CMD_0x20;
    buff[tx_bytes++] = tx_bytes;
    tx_bytes += sprintf(&buff[tx_bytes], "dhcp=0");
    if (ipaddr!=NULL) { tx_bytes += sprintf(&buff[tx_bytes], ",ipaddr=%s", ipaddr); }
    if (netmask!=NULL) { tx_bytes += sprintf(&buff[tx_bytes], ",netmask=%s", netmask); }
    if (gateway!=NULL) { tx_bytes += sprintf(&buff[tx_bytes], ",gateway=%s", gateway); }
    if (dns1!=NULL) { tx_bytes += sprintf(&buff[tx_bytes], ",dns1=%s", dns1); }
    if (dns2!=NULL) { tx_bytes += sprintf(&buff[tx_bytes], ",dns2=%s", dns2); }
    if (mtu > 0) { tx_bytes += sprintf(&buff[tx_bytes], ",mut=%d", mtu); }
    buff[2] = tx_bytes-3;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_send(tty, buff, tx_bytes);
    int nbyte = router4G_waitACK(tty, buff, MODBUS_CMD_0x20, 3000);
    if (nbyte > 0)
    {
        return 0;
    }
    return -1;
}

int router4G_Switch2APN(const char *apn_name)
{
    int tx_bytes = 0;
    int rx_bytes = 0;
    uint8_t buff[128];
    com_ttySx_t *tty = &router4G;
    com_ttySx_flush(tty);
    printd("step\n");

    buff[tx_bytes++] = 0x1;
    buff[tx_bytes++] = MODBUS_CMD_0x21;
    buff[tx_bytes++] = tx_bytes;
    if (apn_name != NULL) { tx_bytes += sprintf(&buff[tx_bytes], ",apn=%s", apn_name); }
    buff[2] = tx_bytes-3;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_send(tty, buff, tx_bytes);
    int nbyte = router4G_waitACK(tty, buff, MODBUS_CMD_0x21, 3000);
    if (nbyte > 0)
    {
        return 0;
    }
    return -1;
}

int router4G_Switch2WIFI(void)
{
    int tx_bytes = 0;
    int rx_bytes = 0;
    uint8_t buff[128];
    com_ttySx_t *tty = &router4G;
    com_ttySx_flush(tty);
    printd("step\n");

    buff[tx_bytes++] = 0x1;
    buff[tx_bytes++] = MODBUS_CMD_0x22;
    buff[tx_bytes++] = tx_bytes;
    buff[2] = tx_bytes-3;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_send(tty, buff, tx_bytes);
    int nbyte = router4G_waitACK(tty, buff, MODBUS_CMD_0x22, 3000);
    printd("step, nbyte:%d\n", nbyte);
    if (nbyte > 0)
    {
        return 0;
    }
    return -1;
}

int router4G_ScanWIFI(int timeout)
{
    int tx_bytes = 0;
    int rx_bytes = 0;
    uint8_t buff[128];
    com_ttySx_t *tty = &router4G;
    com_ttySx_flush(tty);
    printd("step\n");

    buff[tx_bytes++] = 0x1;
    buff[tx_bytes++] = MODBUS_CMD_0x23;
    buff[tx_bytes++] = tx_bytes;
    if (timeout > 0) { tx_bytes += sprintf(&buff[tx_bytes], "scan_time=%d", timeout); }
    buff[2] = tx_bytes-3;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_send(tty, buff, tx_bytes);
    int nbyte = router4G_waitACK(tty, buff, MODBUS_CMD_0x23, 3000);
    printd("step, nbyte:%d\n", nbyte);
    if (nbyte > 0)
    {
        return 0;
    }
    return -1;
}

int router4G_ReadWIFIList(uint8_t *allinfo)
{
    int tx_bytes = 0;
    int rx_bytes = 0;
    uint8_t buff[128];
    com_ttySx_t *tty = &router4G;
    com_ttySx_flush(tty);
    printd("step\n");

    buff[tx_bytes++] = 0x1;
    buff[tx_bytes++] = MODBUS_CMD_0x24;
    buff[tx_bytes++] = tx_bytes;
    buff[2] = tx_bytes-3;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_send(tty, buff, tx_bytes);
    int nbyte = router4G_waitACK(tty, buff, MODBUS_CMD_0x24, 3000);
    printd("step, nbyte:%d\n", nbyte);
    if (nbyte > 0)
    {
        memcpy(allinfo, &buff[3], nbyte - 3);
        return 0;
    }
    return -1;
}

int router4G_EnableWIFI(const char *security_mode, const char *encryption_mode, const char *ssid, const char *passwd)
{
    int tx_bytes = 0;
    int rx_bytes = 0;
    uint8_t buff[256];
    com_ttySx_t *tty = &router4G;
    com_ttySx_flush(tty);
    printd("step\n");

    buff[tx_bytes++] = 0x1;
    buff[tx_bytes++] = MODBUS_CMD_0x25;
    buff[tx_bytes++] = tx_bytes;
    if (security_mode!=NULL) { tx_bytes += sprintf(&buff[tx_bytes], "security_mode=%s", security_mode); }
    if (encryption_mode!=NULL) { tx_bytes += sprintf(&buff[tx_bytes], ",encryption_mode=%s", encryption_mode); }
    if (ssid!=NULL) { tx_bytes += sprintf(&buff[tx_bytes], ",ssid=%s", ssid); }
    if (passwd!=NULL) { tx_bytes += sprintf(&buff[tx_bytes], ",passwd=%s", passwd); }
    buff[2] = tx_bytes-3;
    uint16_t crcv = modbus_crc16(buff, tx_bytes);
    buff[tx_bytes++] = (crcv>>0)&0xFF;
    buff[tx_bytes++] = (crcv>>8)&0xFF;
    com_ttySx_send(tty, buff, tx_bytes);
    int nbyte = router4G_waitACK(tty, buff, MODBUS_CMD_0x25, 3000);
    printd("step, nbyte:%d\n", nbyte);
    if (nbyte > 0)
    {
        return 0;
    }
    return -1;
}

int main(int argc, char* argv[])
{
    char* ttyname=NULL;
    char* devmode=NULL;

    if (argc > 1) { ttyname = argv[1]; }
    if (argc > 2) { devmode = argv[2]; }

    if (ttyname==NULL || devmode==NULL)
    {
        printd("usage: router4G tty mode [...]\n");
        exit(0);
    }

    router4G_init(ttyname, 115200, 8, 'N', 1);

    if (strcmp(devmode, "WAN_DHCP")==0)
    {
        int mtu = 0;
        if (argc > 3) { mtu = atoi(argv[3]); }
        router4G_Switch2WAN_DHCP(mtu); 
    }

    if (strcmp(devmode, "WAN_StaticIP")==0)
    {
        char *ipaddr = NULL;
        char *netmask = NULL;
        char *gateway = NULL;
        char *dns1 = NULL;
        char *dns2 = NULL;
        int mtu=0;

        if (argc>3) { ipaddr = argv[3]; }
        if (argc>4) { netmask = argv[4]; }
        if (argc>5) { gateway = argv[5]; }
        if (argc>6) { dns1 = argv[6]; }
        if (argc>7) { dns2 = argv[7]; }
        if (argc>8) { mtu = atoi(argv[8]); }

        if (ipaddr != NULL) router4G_Switch2WAN_StaticIP(ipaddr, netmask, gateway, dns1, dns2, 100);
    }

    if (strcmp(devmode, "APN")==0)
    {
        char *apn = NULL;
        if (argc > 3) { apn = argv[3]; }
         router4G_Switch2APN(apn); 
    }


    if (strcmp(devmode, "WIFI")==0)
    {
        char *security_mode = NULL;
        char *encryption_mode = NULL;
        char *ssid = NULL;
        char *passwd = NULL;

        if (argc>3) {  security_mode = argv[3];}
        if (argc>4) {  encryption_mode = argv[4];}
        if (argc>5) {  ssid = argv[5];}
        if (argc>6) {  passwd = argv[6];}
        
        if (security_mode==NULL)
        {
            router4G_Switch2WIFI();
            sleep(10);
            router4G_ScanWIFI(30);
            printd("Please wait %d second....\n", 30);
            sleep(30);
            uint8_t buff[1024];
            if (router4G_ReadWIFIList(buff) == 0)
            {
                printf("SSID:%s\n", buff);
            }
        }
        else
        {
            router4G_EnableWIFI(security_mode, encryption_mode, ssid, passwd);
        }
    }
    
    router4G_exit();
    return 0;
}
