*
 * modbusTcp.c
 *
 * Created: 19/08/2019 14:05:11
 *  Author: diego
 */
#include "modbusTcp.h"

uint8_t actConns = 0;

uint16_t    swapBytes(uint8_t *ptr)
{
    uint16_t    out = ((uint16_t) * (ptr + 1)) | ((uint16_t) * (ptr) << 8);
    return out;
}

void    modbusTcpStart(uint16_t port)
{
    volatile err_t err;

    struct tcp_pcb *pcb;

    pcb = tcp_new();
    //LWIP_ASSERT("falla al crear el pcb de modbus tcp", pcb != NULL);

    if(pcb != NULL)
    {
        err = tcp_bind(pcb, IP_ADDR_ANY, port);
        //tcp_setprio(pcb, MODBUS_TCP_PRIO);
        //LWIP_ASSERT("Falla al iniciar modbus TCP (bind)", err == ERR_OK);

        if(err == ERR_OK)
        {
            pcb = tcp_listen(pcb);
            //LWIP_ASSERT("Falla al iniciar modbus TCP (listen)", err == ERR_OK);
            tcp_accept(pcb, modbusTcpAccept);
            return;
        }
    }
    else
        return;
}

err_t   modbusTcpAccept(void *arg, struct tcp_pcb *mPcb, err_t err)
{
    err_t ret_err;
    mbTcp_status *mbStatus;

    LWIP_UNUSED_ARG(arg);
    LWIP_UNUSED_ARG(err);

    if(actConns >= MTCP_MAX_CONNS)
    {
        return ERR_MEM;
    }
    else
    {
        actConns++;
    }

    tcp_setprio(mPcb, TCP_PRIO_MAX);

    mbStatus = (mbTcp_status *)mem_malloc(sizeof(mbTcp_status));

    if(mbStatus != NULL)
    {
        mbStatus->estado = MB_CONNECTED;
        mbStatus->pcb = mPcb;
        mbStatus->retries = 0;
        mbStatus->sendP = NULL;
        /* pass newly allocated es to our callbacks */
        tcp_arg(mPcb, mbStatus);
        tcp_recv(mPcb, modbusTcpRecv);
        tcp_err(mPcb, modbusTcpError);
        tcp_poll(mPcb, modbusTcpPoll, 0);
        ret_err = ERR_OK;
    }
    else
    {
        ret_err = ERR_MEM;
    }

    return ret_err;
}


err_t   modbusTcpRecv(void *arg, struct tcp_pcb *mPcb, struct pbuf *p, err_t err)
{

    mbTcp_status *mbStatus;
    volatile    mbTcp_mbapHeader    headerTmp;

    mbStatus = (mbTcp_status *)arg;
    uint8_t *payloadPtr     =   (uint8_t *) p->payload;

    //tcp_output(mPcb);

    if(p == NULL)
    {
        if(mbStatus->sendP == NULL)
        {
            if(getQueueLength(&MTCP_QUEUE) == 0)
            {
                // no quedan respuestas para mandar
                // ni bloques que procesar
                modbusTcpClose(mPcb, (mbTcp_status *)arg);
            }
            else
            {
                mbStatus->estado = MB_WAIT_TO_CLOSE;
                //pbuf_free(p);
            }

            // esto es que queda algo para procesar en la cola
            // la conexion se cerrara luego de que se procesen los que quedan
            // simplemente se descarta el entrante

        }
        else
        {
            // queda algo para mandar
            // asi que lo mandamos
            tcp_sent(mPcb, modbusTcpSent);
            //modbusTcpSend(mPcb, mbStatus);
        }
    }
    else
        if(err != ERR_OK)
        {
            mbStatus->sendP = NULL;
            tcp_recved(mPcb, p->tot_len);
            pbuf_free(p);
            return  err;
        }
        else
        {
            switch(mbStatus->estado)
            {
                case MB_CONNECTED:
                    if(p->len == 0)
                    {
                        tcp_recved(mPcb, p->tot_len);
                        pbuf_free(p);
                        break;
                    }

                    headerTmp.transactionId =   swapBytes(payloadPtr);
                    headerTmp.protocol      =   swapBytes(payloadPtr + 2);
                    headerTmp.len           =   swapBytes(payloadPtr + 4);
                    headerTmp.unitId        =   *(payloadPtr + 6);


                    if(mTcpCallGood(mbStatus, p, headerTmp) != 0)
                    {
                        if(p != NULL)
                        {
                            pbuf_free(p);
                        }

                        tcp_recved(mPcb, p->tot_len);
                        break;
                    }

                    // asumimos que los chequeos y demas fueron realizados por el stack

                    break;

                case MB_WAIT_TO_CLOSE:
                    // notese que no ponemos mbStatus->p a NULL
                    tcp_recved(mPcb, p->tot_len);
                    pbuf_free(p);
                    return ERR_OK;
                    break;

                case MB_CLOSING:
                    tcp_recved(mPcb, p->tot_len);
                    mbStatus->sendP = NULL;
                    pbuf_free(p);
                    return ERR_OK;
                    break;

                default:
                    tcp_recved(mPcb, p->tot_len);
                    mbStatus->sendP = NULL;
                    pbuf_free(p);
                    return ERR_OK;
                    break;
            }
        }
}

void    modbusTcpError(void *arg, err_t err)
{
    mbTcp_status *mbStatus;

    LWIP_UNUSED_ARG(err);

    mbStatus = (mbTcp_status *)arg;



    tcp_arg(mbStatus->pcb, NULL);
    tcp_sent(mbStatus->pcb, NULL);
    tcp_recv(mbStatus->pcb, NULL);
    tcp_err(mbStatus->pcb, NULL);
    tcp_poll(mbStatus->pcb, NULL, 0);
    tcp_close(mbStatus->pcb);

    /*if(mbStatus != NULL)
    {
        mem_free(mbStatus);
    }*/
}

void    modbusTcpSend(struct tcp_pcb *mPcb, mbTcp_status *mbStatus)
{
    err_t wr_err = ERR_OK;

    while((wr_err == ERR_OK) &&
            (mbStatus->sendP != NULL) &&
            (mbStatus->sendLen <= tcp_sndbuf(mPcb))
         )
    {
        wr_err = tcp_write(mPcb, mbStatus->sendP, mbStatus->sendLen, 1);

        if(wr_err == ERR_OK)
        {
            mbStatus->sendLen = 0;
            break;
        }
        else
            if(wr_err == ERR_MEM)
            {
                // por ahora simplemente no mandamos nada
                // idealmente seria defer to poll
            }
    }

    return;
}

err_t   modbusTcpPoll(void *arg, struct tcp_pcb *mPcb)
{

}

void    modbusTcpClose(struct tcp_pcb *mPcb, mbTcp_status *mbStatus)
{
    tcp_arg(mPcb, NULL);
    tcp_sent(mPcb, NULL);
    tcp_recv(mPcb, NULL);
    tcp_err(mPcb, NULL);
    tcp_poll(mPcb, NULL, 0);

    if(mbStatus != NULL)
    {
        mem_free(mbStatus);
    }

    tcp_close(mPcb);
    actConns--;
}

err_t   modbusTcpSent(void *arg, struct tcp_pcb *mPcb, u16_t len)
{
    mbTcp_status *mbStatus;

    mbStatus = (mbTcp_status *)arg;

    LWIP_UNUSED_ARG(len);

    mbStatus->retries = 0;

    if(mbStatus->estado == MB_WAIT_TO_CLOSE)
    {
        if(getQueueLength(&MTCP_QUEUE) == 0)
        {
            // no quedan respuestas para mandar
            // ni bloques que procesar
            modbusTcpClose(mPcb, (mbTcp_status *)arg);
        }
    }

    // no quedan respuestas para mandar
    // y ya no habia nada en la cola
    if(mbStatus->estado == MB_CLOSING)
    {
        modbusTcpClose(mPcb, mbStatus);
    }

    return ERR_OK;
}