#include "msystem.h"
#include "modbusTCP_TC.h"
#include "modbusTCP_contex.h"
#include "libinetsocket.h"


static int modbusTCP_TC_send(modbusTCP_TC_t* dev, uint8_t* data, int length)
{
    int ret = 0;
    if (length > 0 && dev->fd > 0)
    {
        pthread_mutex_lock(&dev->mutex);
        ret = send(dev->fd, data, length, 0);
        pthread_mutex_unlock(&dev->mutex);
        if (ret <= 0) { shutdown(dev->fd, SHUT_RDWR); close(dev->fd); dev->fd = -1; printe("ret:%d, errno:%d\n", ret, errno); return -1; }
        // printline(data, length, "modbusTCP_TC(%d).send", dev->fd);
    }
    return ret;
}

static int modbusTCP_TC_recv(modbusTCP_TC_t* dev, uint8_t* buff, int length, int timeout)
{
    fd_set readfds;
    FD_ZERO(&readfds);
    FD_SET(dev->fd, &readfds);
    struct timeval tv;
    int64_t now_ms = systime_ms();
    int64_t end_ms = systime_ms() + timeout;
    int rxbyte = 0;
    for(; now_ms < end_ms; now_ms = systime_ms())
    {
        timeout = end_ms - now_ms;
        tv.tv_sec = timeout/1000;
        tv.tv_usec = (timeout%1000)*1000;
        if (select(dev->fd+1, &readfds, NULL, NULL, &tv) > 0)
        {
            if(FD_ISSET(dev->fd, &readfds))
            {
                int ret = read(dev->fd, &buff[rxbyte], length-rxbyte);
                if (ret > 0) { rxbyte += ret; }
                if (ret < 0) {
                    if (errno == EINTR) { continue; }
                    else{ return -1; }
                }
                if (rxbyte == length) break;
            }
        }
    }
    return rxbyte;
}

static int modbusTCP_TC_wait_frame(modbusTCP_TC_t* dev)
{
    modbusTCP_frame_t* pkg = (modbusTCP_frame_t*)&dev->frame;
    int FRAME_LENGTH = 8;
    pthread_mutex_lock(&dev->mutex);
    dev->rxbyte = 0;
    while(dev->rxbyte < 8 && dev->fd > 0)
    {
        int ret = recv(dev->fd, &dev->rxbuff[dev->rxbyte], 8 - (dev->rxbyte), 0);
        if (ret <= 0) { shutdown(dev->fd, SHUT_RDWR); close(dev->fd); dev->fd = -1; printe("ret:%d, errno:%d\n", ret, errno); pthread_mutex_unlock(&dev->mutex); return -1; }
        if (ret > 0) dev->rxbyte += ret;
        if (dev->rxbyte >= 8)
        {
            pkg->transID = (dev->rxbuff[0]<<8) | dev->rxbuff[1];
            pkg->protoID = (dev->rxbuff[2]<<8) | dev->rxbuff[3];
            pkg->datalen = (dev->rxbuff[4]<<8) | dev->rxbuff[5];
            pkg->slaveID = dev->rxbuff[6];
            pkg->command = dev->rxbuff[7];

            if (pkg->protoID == 0x0000)
            {
                FRAME_LENGTH = 8 + pkg->datalen;
                break;
            }
            memmove(&dev->rxbuff[0], &dev->rxbuff[1], dev->rxbyte - 1);
            dev->rxbyte = dev->rxbyte - 1;
        }
    }
    while(dev->rxbyte < FRAME_LENGTH)
    {
        int ret = recv(dev->fd, &dev->rxbuff[dev->rxbyte], FRAME_LENGTH - (dev->rxbyte), 0);
        if (ret <= 0) {  shutdown(dev->fd, SHUT_RDWR); close(dev->fd); dev->fd = -1;  printe("ret:%d, errno:%d\n", ret, errno); pthread_mutex_unlock(&dev->mutex); return -1;}
        dev->rxbyte += ret;
    }
    pthread_mutex_unlock(&dev->mutex);
    // printline(dev->rxbuff, dev->rxbyte, "modbusTCP_TC(%d).recv", dev->fd);
    return pkg->command;
}

int modbusTCP_TC_cmd_0x80(modbusTCP_TC_t* dev, uint8_t slave)
{
    uint8_t *stream = dev->txbuff;
    int idx = 8;
    int64_t time_ms = systime_ms();
    idx = encode_U64(stream, idx, 1, &time_ms);
    dev->txbyte = encode_HEADER(stream, dev->transID++, slave, modbusTCP_CMD_0x80, NULL, idx - 8);

    if (modbusTCP_TC_send(dev, dev->txbuff, dev->txbyte) == dev->txbyte)
    {
        if (modbusTCP_TC_wait_frame(dev) == modbusTCP_CMD_0x80)
        {
            return 0;
        }
    }
    return -1;
}

int modbusTCP_TC_cmd_0x81(modbusTCP_TC_t* dev, uint8_t slave)
{
    GUNSlot_t *gunSlot = &dev->gunSlot[slave-1];
    uint8_t *stream = dev->txbuff;
    int idx = 8;
    dev->txbyte = encode_HEADER(stream, dev->transID++, slave, modbusTCP_CMD_0x81, NULL, idx - 8);

    if (modbusTCP_TC_send(dev, dev->txbuff, dev->txbyte) == dev->txbyte)
    {
        if (modbusTCP_TC_wait_frame(dev) == modbusTCP_CMD_0x81)
        {
            idx = 8;
            stream = dev->rxbuff;
            idx = decode_U32(stream, idx, 1, &gunSlot->supportedMinPowerKW);
            idx = decode_U32(stream, idx, 1, &gunSlot->supportedMaxPowerKW);
            idx = decode_U16(stream, idx, 1, &gunSlot->supportedMaxVoltage);
            idx = decode_U16(stream, idx, 1, &gunSlot->supportedMaxCurrent);
            idx = decode_U16(stream, idx, 1, &gunSlot->supportedMinVoltage);
            idx = decode_U16(stream, idx, 1, &gunSlot->supportedMinCurrent);
            // printv("maxVoltage:%d, minVoltage:%d\n", gunSlot->supportedMaxVoltage, gunSlot->supportedMinVoltage);
            return 0;
        }
    }
    return -1;
}

int modbusTCP_TC_cmd_0x82(modbusTCP_TC_t* dev, uint8_t slave)
{
    GUNSlot_t *gunSlot = &dev->gunSlot[slave-1];
    uint8_t *stream = dev->txbuff;
    int idx = 8;
    idx = encode_U16(stream, idx, 1, &gunSlot->permittedMaxCurrent);
    idx = encode_U16(stream, idx, 1, &gunSlot->batteryVoltage);
    dev->txbyte = encode_HEADER(stream, dev->transID++, slave, modbusTCP_CMD_0x82, NULL, idx - 8);

    if (modbusTCP_TC_send(dev, dev->txbuff, dev->txbyte) == dev->txbyte)
    {
        if (modbusTCP_TC_wait_frame(dev) == modbusTCP_CMD_0x82)
        {
            return 0;
        }
    }
    return -1;
}

int modbusTCP_TC_cmd_0x83(modbusTCP_TC_t* dev, uint8_t slave, uint8_t mode, float requestVoltage, float requestCurrent)
{
    uint8_t *stream = dev->txbuff;
    int idx = 8;
    int64_t time_ms = systime_ms();
    idx = encode_U8(stream, idx, 1, &mode);
    idx = encode_U32(stream, idx, 1, (uint32_t*)&requestVoltage);
    idx = encode_U32(stream, idx, 1, (uint32_t*)&requestCurrent);
    dev->txbyte = encode_HEADER(stream, dev->transID++, slave, modbusTCP_CMD_0x83, NULL, idx - 8);

    if (modbusTCP_TC_send(dev, dev->txbuff, dev->txbyte) == dev->txbyte)
    {
        if (modbusTCP_TC_wait_frame(dev) == modbusTCP_CMD_0x83)
        {
            return 0;
        }
    }
    return -1;
}

static void modbusTCP_TC_connect(modbusTCP_TC_t* dev)
{
    if (dev->fd <= 0)
    {
        if (strlen(dev->connect_ip_string) > 0)
        {
            dev->fd = socket(AF_INET, SOCK_STREAM, 0);
            int enable = 1;
            setsockopt(dev->fd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable));
            struct sockaddr_in peer;
            peer.sin_family = AF_INET;
            peer.sin_port = htons(dev->connect_port);
            peer.sin_addr.s_addr = inet_addr(dev->connect_ip_string);
            int ret = connect(dev->fd, (struct sockaddr *)&peer, sizeof(peer));
            if (ret < 0)
            {
                printe("socket connect(%s:%d).fd=%d, error=%d, ret=%d\n", dev->connect_ip_string, dev->connect_port, dev->fd, errno, ret);
                shutdown(dev->fd, SHUT_RDWR); close(dev->fd); dev->fd = -1;
            }else{
                printi("socket connect(%s:%d).fd=%d OK.\n", dev->connect_ip_string, dev->connect_port, dev->fd);
            }
        }
    }
}

void* modbusTCP_TC_loop(void* args)
{
    uint8_t buff[2048];
    modbusTCP_frame_t header;
    modbusTCP_frame_t *pkg = &header;
    modbusTCP_TC_t* dev = (modbusTCP_TC_t*)args;
    while (dev->quit == 0)
    {
        sleep(1);
        modbusTCP_TC_cmd_0x80(dev, 1);
        sleep(1);
        modbusTCP_TC_cmd_0x80(dev, 2);
        if (dev->fd <= 0)
        {
            modbusTCP_TC_connect(dev);
        }
    }
    return NULL;
}

int modbusTCP_TC_init(modbusTCP_TC_t* dev)
{
    if (dev)
    {
        ccu_config_t* config = &ccu_config;
        dev->quit = 0;
        dev->fd = -1;
        pthread_mutex_init(&dev->mutex, NULL);
        // socketpair(AF_UNIX, SOCK_STREAM, 0, dev->pipe_fd);
        if (strncmp(config->pcutc.connect_ip_string, "eth", 3) == 0)
        {
            struct sockaddr_in if_addr;
            socklen_t addrlen = sizeof(if_addr);
            if (get_ipv4_address(config->pcutc.connect_ip_string, &if_addr, dev->connect_ip_string, sizeof(dev->connect_ip_string)) == 0)
            {
                printd("get_ipv4_address(%s:%d)\n", dev->connect_ip_string, dev->connect_port);
            }else{
                memset(dev->connect_ip_string, 0, sizeof(dev->connect_ip_string));
                printe("get_ipv4_address(%s:%d), failed.\n", config->pcutc.connect_ip_string, dev->connect_port);
            }
        }else{
            sprintf(dev->connect_ip_string, "%s", config->pcutc.connect_ip_string);
        }
        
        dev->connect_port = config->pcutc.connect_port;
        modbusTCP_TC_connect(dev);
        mthread_init(&dev->thread, modbusTCP_TC_loop, dev);
    }
    printd("fd:%d\n", dev->fd);
    return 0;
}

void modbusTCP_TC_exit(modbusTCP_TC_t* dev)
{
    if (dev)
    {
        dev->quit = 1;
        mthread_exit(&dev->thread);
        shutdown(dev->fd, SHUT_RDWR);
        // shutdown(dev->pipe_fd[0], SHUT_RDWR);
        // shutdown(dev->pipe_fd[1], SHUT_RDWR);
        printd("------------------\n");
        close(dev->fd);
        // close(dev->pipe_fd[0]);
        // close(dev->pipe_fd[1]);
        dev->fd = -1;
        pthread_mutex_destroy(&dev->mutex);
    }
}
