#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <string.h>
#include "../custom_fifo.h"
typedef struct _modbus_rtu_udp
{
    /* TCP port */
    int port;
    /* IP address */
    char ip[16];
    struct sockaddr_in server_addr;
    uint8_t rx_buff[255];
    custom_fifo fifo;
} modbus_rtu_udp_t;

static int _modbus_rtu_udp_connect(modbus_t *ctx)
{
    int rc;

    struct sockaddr_in *addr = &(((modbus_rtu_udp_t *)ctx->backend_data)->server_addr);
    modbus_rtu_udp_t *ctx_rtu_udp = ctx->backend_data;
    int flags = SOCK_DGRAM;

#ifdef OS_WIN32
    if (_modbus_tcp_init_win32() == -1)
    {
        return -1;
    }
#endif

#ifdef SOCK_CLOEXEC
    flags |= SOCK_CLOEXEC;
#endif

#ifdef SOCK_NONBLOCK
    flags |= SOCK_NONBLOCK;
#endif

    ctx->s = socket(PF_INET, flags, 0);
    if (ctx->s < 0)
    {
        return -1;
    }

    if (ctx->debug)
    {
        printf("Connecting to %s:%d\n", ctx_rtu_udp->ip, ctx_rtu_udp->port);
    }

    memset(addr, 0, sizeof(*addr));
    addr->sin_family = PF_INET;
    addr->sin_port = htons(ctx_rtu_udp->port);
    rc = inet_pton(addr->sin_family, ctx_rtu_udp->ip, &(addr->sin_addr));

    if (rc <= 0)
    {
        if (ctx->debug)
        {
            fprintf(stderr, "Invalid IP address: %s\n", ctx_rtu_udp->ip);
        }
        close(ctx->s);
        ctx->s = rc;
    }

    fifo_clear(&ctx_rtu_udp->fifo);
    return 0;
}

static ssize_t _modbus_rtu_udp_send(modbus_t *ctx, const uint8_t *req, int req_length)
{
    struct sockaddr_in *addr = &((modbus_rtu_udp_t *)ctx->backend_data)->server_addr;
    return sendto(ctx->s, (const char *)req, req_length, 0, (struct sockaddr*)addr, sizeof(*addr));
}

static ssize_t _modbus_rtu_udp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length)
{
    int rc = 0;
    modbus_rtu_udp_t *ctx_rtu_udp = (modbus_rtu_udp_t *)ctx->backend_data;
    if (fifi_get_counter(&ctx_rtu_udp->fifo) > 0)
    {
        rc = fifo_pop_buff(&ctx_rtu_udp->fifo, rsp, rsp_length);
    }
    else
    {
        unsigned char tmp_buff[256] = {0};
        rc = recvfrom(ctx->s, tmp_buff, sizeof(tmp_buff), 0, NULL, NULL);
        if (rc > 0)
        {
            fifo_push_buff(&ctx_rtu_udp->fifo, tmp_buff, rc);
            rc = fifo_pop_buff(&ctx_rtu_udp->fifo, rsp, rsp_length);
        }
    }
    return rc;
}
#if 0
static unsigned int _modbus_rtu_tcp_is_connected(modbus_t *ctx)
{
    return ctx->s >= 0;
}
#endif

static void _modbus_rtu_udp_close(modbus_t *ctx)
{
    if (ctx->s >= 0)
    {
        close(ctx->s);
        ctx->s = -1;
    }
}

static int _modbus_rtu_udp_flush(modbus_t *ctx)
{
    int rc;
    int rc_sum = 0;
    modbus_rtu_udp_t *ctx_rtu_udp = (modbus_rtu_udp_t *)ctx->backend_data;
    do
    {
        /* Extract the garbage from the socket */
        char devnull[MODBUS_TCP_MAX_ADU_LENGTH];
#ifndef OS_WIN32
        rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, MSG_DONTWAIT);
#else
        /* On Win32, it's a bit more complicated to not wait */
        fd_set rset;
        struct timeval tv;

        tv.tv_sec = 0;
        tv.tv_usec = 0;
        FD_ZERO(&rset);
        FD_SET(ctx->s, &rset);
        rc = select(ctx->s + 1, &rset, NULL, NULL, &tv);
        if (rc == -1)
        {
            return -1;
        }

        if (rc == 1)
        {
            /* There is data to flush */
            rc = recvfrom(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, 0, NULL, NULL);
        }
#endif
        if (rc > 0)
        {
            rc_sum += rc;
        }
    } while (rc == MODBUS_TCP_MAX_ADU_LENGTH);
    fifo_clear(&ctx_rtu_udp->fifo);
    return rc_sum;
}

static int _modbus_rtu_udp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
{
    int s_rc = 0;
    modbus_rtu_udp_t *ctx_rtu_udp = (modbus_rtu_udp_t *)ctx->backend_data;
    if (fifi_get_counter(&ctx_rtu_udp->fifo) > 0)
    {
        s_rc = 1;
    }
    else
    {
        fifo_clear(&ctx_rtu_udp->fifo);
        while ((s_rc = select(ctx->s + 1, rset, NULL, NULL, tv)) == -1)
        {
            if (errno == EINTR)
            {
                if (ctx->debug)
                {
                    fprintf(stderr, "A non blocked signal was caught\n");
                }
                /* Necessary after an error */
                FD_ZERO(rset);
                FD_SET(ctx->s, rset);
            }
            else
            {
                return -1;
            }
        }
    }

    if (s_rc == 0)
    {
        errno = ETIMEDOUT;
        return -1;
    }

    return s_rc;
}

static void _modbus_rtu_udp_free(modbus_t *ctx)
{
    if (ctx->backend_data)
    {
        free(ctx->backend_data);
    }
    free(ctx);
}

const modbus_backend_t _modbus_rtu_udp_backend = {
    _MODBUS_BACKEND_TYPE_RTU,
    _MODBUS_RTU_HEADER_LENGTH,
    _MODBUS_RTU_CHECKSUM_LENGTH,
    MODBUS_RTU_MAX_ADU_LENGTH,
    _modbus_set_slave,
    _modbus_rtu_build_request_basis,
    _modbus_rtu_build_response_basis,
    _modbus_rtu_prepare_response_tid,
    _modbus_rtu_send_msg_pre,
    _modbus_rtu_udp_send,
    _modbus_rtu_receive,
    _modbus_rtu_udp_recv,
    _modbus_rtu_check_integrity,
    _modbus_rtu_pre_check_confirmation,
    _modbus_rtu_udp_connect,
    //_modbus_rtu_tcp_is_connected,
    _modbus_rtu_udp_close,
    _modbus_rtu_udp_flush,
    _modbus_rtu_udp_select,
    _modbus_rtu_udp_free,
	NULL
};

modbus_t *
modbus_new_rtu_udp(const char *ip, int port)
{
    modbus_t *ctx;
    modbus_rtu_udp_t *ctx_rtu_udp = NULL;
    size_t dest_size;
    size_t ret_size;

#if defined(OS_BSD)
    /* MSG_NOSIGNAL is unsupported on *BSD so we install an ignore
       handler for SIGPIPE. */
    struct sigaction sa;

    sa.sa_handler = SIG_IGN;
    if (sigaction(SIGPIPE, &sa, NULL) < 0)
    {
        /* The debug flag can't be set here... */
        fprintf(stderr, "Could not install SIGPIPE handler.\n");
        return NULL;
    }
#endif

    ctx = (modbus_t *)malloc(sizeof(modbus_t));
    if (ctx == NULL)
    {
        return NULL;
    }
    _modbus_init_common(ctx);

    /* Could be changed after to reach a remote serial Modbus device */
    ctx->slave = MODBUS_TCP_SLAVE;

    ctx->backend = &_modbus_rtu_udp_backend;

    ctx->backend_data = (modbus_rtu_udp_t *)malloc(sizeof(modbus_rtu_udp_t));
    if (ctx->backend_data == NULL)
    {
        modbus_free(ctx);
        errno = ENOMEM;
        return NULL;
    }
    memset(ctx->backend_data, 0, sizeof(modbus_rtu_udp_t));
    ctx_rtu_udp = (modbus_rtu_udp_t *)ctx->backend_data;

    if (ip != NULL)
    {
        dest_size = sizeof(char) * 16;
        ret_size = strlcpy(ctx_rtu_udp->ip, ip, dest_size);
        if (ret_size == 0)
        {
            fprintf(stderr, "The IP string is empty\n");
            modbus_free(ctx);
            errno = EINVAL;
            return NULL;
        }

        if (ret_size >= dest_size)
        {
            fprintf(stderr, "The IP string has been truncated\n");
            modbus_free(ctx);
            errno = EINVAL;
            return NULL;
        }
    }
    else
    {
        ctx_rtu_udp->ip[0] = '0';
    }
    ctx_rtu_udp->port = port;
    fifo_init(&ctx_rtu_udp->fifo, ctx_rtu_udp->rx_buff, sizeof(ctx_rtu_udp->rx_buff));

    return ctx;
}