#include "cascade_slave_tcp.h"
#include "stdlib.h"
#include "cascade.h"
#include "thread.h"


Modbus_Manger_Tcp* tcp_modbus_init(const char* ip, uint16_t port,uint8_t addr,int power_type)
{
    Modbus_Manger_Tcp* tcp  = NULL;
    tcp = (Modbus_Manger_Tcp*)malloc(sizeof(Modbus_Manger_Tcp));
    if(!tcp)
    {
        log_d("malloc modubus manger faield!!!\n");
        goto err1;
    }
    memset(tcp, 0, sizeof(Modbus_Manger_Tcp));
    
    tcp->ctx = modbus_new_tcp(ip, port);
    if(!tcp->ctx)
    {
        log_d("malloc modbus_tcp ctx faield!!!\n");
        goto err2;
    }
    
    struct timeval response_timeout;
    response_timeout.tv_sec = 1;
    response_timeout.tv_usec = 0;
    modbus_set_response_timeout(tcp->ctx, 0,800000);
    modbus_set_byte_timeout(tcp->ctx,0,50000);
    modbus_set_error_recovery(tcp->ctx,MODBUS_ERROR_RECOVERY_PROTOCOL);
    modbus_set_slave(tcp->ctx,addr);
    strcpy(tcp->ip,ip);
    tcp->port = port;
    tcp->slave_id = addr;
    tcp->power_type = power_type;
    return tcp;
err2:
    free(tcp);
err1:
    return tcp;
}


void  tcp_modbus_deinit(Modbus_Manger_Tcp **tcp)
{
    Modbus_Manger_Tcp** tmp = tcp;
    if(tmp)
    {
        if(*tmp)
        {
            modbus_free((*tmp)->ctx);
            modbus_close((*tmp)->ctx);

            free(*tmp);
            *tmp = 0;   
        }
    }
    
}

static void memswap(uint8_t *buf, int len)
{
    int i;
    uint8_t tmp;

    for(i=0; i<len; i+=2) {
        tmp = buf[i];
        buf[i] = buf[i+1];
        buf[i+1] = tmp;
    }
}

static int _mb_hdr(uint8_t *data, int datalen, mb_hdr_t *h)
{
    h->addr = data[0];
    h->func = data[1];
    h->reg  = data[2]<<8 | data[3];
    h->regcnt = data[4]<<8 | data[5];

    h->dlen = 0;
    h->data = NULL;
    if(datalen>8) {
        h->dlen = data[6];
        h->data = data+7;

        memswap(h->data, h->dlen+h->dlen%2);
    }
    //print_hdr("sss", h);

    return 0;
}
static inline GlobalDeviceManager* get_dm(void)
{
    return &__globalDeviceManage;
}

static void *pthread_handle_tcp(void *arg)
{
    uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH] = {0};
    int ret = 0;
    //Modbus_Manger_Tcp *tcp = (Modbus_Manger_Tcp *)(arg);
    int fd = *(int*)(arg);
    Modbus_Manger_Tcp *mb =tcp_modbus_init("0.0.0.0",CASCADE_SLAVE_PORT,1,0);
    struct timeval response_timeout;
    response_timeout.tv_sec = 1;
    response_timeout.tv_usec = 0;
    modbus_set_response_timeout(mb->ctx, 0,800000);
    modbus_set_byte_timeout(mb->ctx,0,50000);
    modbus_set_error_recovery(mb->ctx,MODBUS_ERROR_RECOVERY_PROTOCOL);
    //mb->ctx->s = fd;
    //mdbus_tcp_setfd(mb->ctx,fd);
    mb->ctx->s = fd;
    int rc = 0;
    mb_hdr_t h;
    while(1)
    {
        rc = modbus_receive(mb->ctx,query);
        log_d("recive modbus rc=%d\n",rc);
        if(rc > 0)
        {
            _mb_hdr(&query[6],rc,&h);
            if(h.func==MODBUS_FC_READ_HOLDING_REGISTERS) {
                cascade_slave_read(h.reg, h.regcnt);
                modbus_reply(mb->ctx,query,rc,cascade_slave_map());
            }
            else if(h.func==MODBUS_FC_WRITE_SINGLE_REGISTER) {
                cascade_slave_write(h.reg, h.regcnt);
                modbus_reply(mb->ctx,query,rc,cascade_slave_map());
            }
        }else if(rc < 0)
        {
            log_d("thread out!!!!!\n");
            break;
        }else
        {
            continue;
        }
    }
    modbus_close(mb->ctx);
    modbus_free(mb->ctx);
    mb->ctx = 0;
    free(mb);
    mb = NULL;
    pthread_exit(NULL);
}



void* tcp_modbus_thread(void *arg)
{
    //GlobalDeviceManager *mgr = (GlobalDeviceManager *)(arg);
    
    GlobalDeviceManager *mgr = get_dm();
    Modbus_Manger_Tcp *tcp = mgr->md_tcp;
    tcp->connection = modbus_tcp_listen(tcp->ctx,CASCADE_SLAVE_CONNECTION);
    modbus_tcp_accept(tcp->ctx,&tcp->connection);
    uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH] = {0};
    mb_hdr_t h;
    log_d("start tcp modbus thread!!!\n");
    int rc = 0;
    pthread_t thread_id = 0;
    int recive_fd = 0;

    while(1)
    {
        if(tcp->ctx){

            struct sockaddr_in addr;
            socklen_t addrlen;
            addrlen = sizeof(addr);
#ifdef HAVE_ACCEPT4
                /* Inherit socket flags and use accept4 call */
                recive_fd = accept4(tcp->connection, (struct sockaddr *)&addr, &addrlen, SOCK_CLOEXEC);
#else
                recive_fd = accept(tcp->connection, (struct sockaddr *)&addr, &addrlen);
#endif
            if(recive_fd > 0)
            {
                pthread_create(&thread_id,NULL,pthread_handle_tcp,(void*)(&recive_fd));
                pthread_detach(thread_id);
            }
        }
    }
    pthread_exit(NULL);
}