#include "nanomodbus.h"
#undef NDEBUG
#include <assert.h>
#include <pthread.h>
#include <stdio.h>
#include <string.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <time.h>
#include <unistd.h>

#define expect(expr) assert(expr)

#define check(err) (expect((err) == NMBS_ERROR_NONE))

#define reset(nmbs) (memset(&(nmbs), 0, sizeof(nmbs_t)))

#define test(f) (nesting++, (f), nesting--)

#define UNUSED_PARAM(x) ((x) = (x))


const uint8_t TEST_SERVER_ADDR = 1;


unsigned int nesting = 0;

int sockets[2] = {-1, -1};

bool server_stopped = true;
pthread_mutex_t server_stopped_m = PTHREAD_MUTEX_INITIALIZER;
pthread_t server_thread;
nmbs_t CLIENT, SERVER;


#define should(s)                                                                                                      \
    for (unsigned int i = 0; i < nesting; i++) {                                                                       \
        printf("\t");                                                                                                  \
    }                                                                                                                  \
    printf("Should %s\n", (s))


uint64_t now_ms() {
    struct timespec ts = {0, 0};
    clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
    return (uint64_t) (ts.tv_sec) * 1000 + (uint64_t) (ts.tv_nsec) / 1000000;
}


void platform_sleep(uint32_t milliseconds, void* arg) {
    UNUSED_PARAM(arg);
    usleep(milliseconds * 1000);
}


void reset_sockets() {
    if (sockets[0] != -1)
        close(sockets[0]);

    if (sockets[1] != -1)
        close(sockets[1]);

    expect(socketpair(AF_UNIX, SOCK_STREAM, 0, sockets) == 0);
}


int read_byte_fd(int fd, uint8_t* b, int32_t timeout_ms) {
    fd_set rfds;
    FD_ZERO(&rfds);
    FD_SET(fd, &rfds);

    struct timeval* tv_p = NULL;
    struct timeval tv;
    if (timeout_ms >= 0) {
        tv_p = &tv;
        tv.tv_sec = timeout_ms / 1000;
        tv.tv_usec = (timeout_ms % 1000) * 1000;
    }

    int ret = select(fd + 1, &rfds, NULL, NULL, tv_p);
    if (ret == 0) {
        return 0;
    }
    else if (ret == 1) {
        ssize_t r = read(fd, b, 1);
        if (r != 1)
            return -1;
        else {
            return 1;
        }
    }
    else
        return -1;
}


int write_byte_fd(int fd, uint8_t b, int32_t timeout_ms) {
    fd_set wfds;
    FD_ZERO(&wfds);
    FD_SET(fd, &wfds);

    struct timeval* tv_p = NULL;
    struct timeval tv;
    if (timeout_ms >= 0) {
        tv_p = &tv;
        tv.tv_sec = timeout_ms / 1000;
        tv.tv_usec = (timeout_ms % 1000) * 1000;
    }

    int ret = select(fd + 1, NULL, &wfds, NULL, tv_p);
    if (ret == 0) {
        return 0;
    }
    else if (ret == 1) {
        ssize_t r = write(fd, &b, 1);
        if (r != 1)
            return -1;
        else {
            return 1;
        }
    }
    else
        return -1;
}


int read_byte_socket_server(uint8_t* b, int32_t timeout_ms, void* arg) {
    UNUSED_PARAM(arg);
    return read_byte_fd(sockets[0], b, timeout_ms);
}


int write_byte_socket_server(uint8_t b, int32_t timeout_ms, void* arg) {
    UNUSED_PARAM(arg);
    return write_byte_fd(sockets[0], b, timeout_ms);
}


int read_byte_socket_client(uint8_t* b, int32_t timeout_ms, void* arg) {
    UNUSED_PARAM(arg);
    return read_byte_fd(sockets[1], b, timeout_ms);
}


int write_byte_socket_client(uint8_t b, int32_t timeout_ms, void* arg) {
    UNUSED_PARAM(arg);
    return write_byte_fd(sockets[1], b, timeout_ms);
}


nmbs_platform_conf nmbs_platform_conf_server;
nmbs_platform_conf* platform_conf_socket_server(nmbs_transport transport) {
    nmbs_platform_conf_server.transport = transport;
    nmbs_platform_conf_server.read_byte = read_byte_socket_server;
    nmbs_platform_conf_server.write_byte = write_byte_socket_server;
    nmbs_platform_conf_server.sleep = platform_sleep;
    return &nmbs_platform_conf_server;
}


nmbs_platform_conf nmbs_platform_conf_client;
nmbs_platform_conf* platform_conf_socket_client(nmbs_transport transport) {
    nmbs_platform_conf_client.transport = transport;
    nmbs_platform_conf_client.read_byte = read_byte_socket_client;
    nmbs_platform_conf_client.write_byte = write_byte_socket_client;
    nmbs_platform_conf_client.sleep = platform_sleep;
    return &nmbs_platform_conf_client;
}


bool is_server_listen_thread_stopped() {
    bool stopped = false;
    expect(pthread_mutex_lock(&server_stopped_m) == 0);
    stopped = server_stopped;
    expect(pthread_mutex_unlock(&server_stopped_m) == 0);
    return stopped;
}


void* server_listen_thread() {
    while (true) {
        if (is_server_listen_thread_stopped())
            break;

        check(nmbs_server_poll(&SERVER));
    }

    return NULL;
}


void stop_client_and_server() {
    if (!is_server_listen_thread_stopped()) {
        expect(pthread_mutex_lock(&server_stopped_m) == 0);
        server_stopped = true;
        expect(pthread_mutex_unlock(&server_stopped_m) == 0);
        expect(pthread_join(server_thread, NULL) == 0);
    }
}


void start_client_and_server(nmbs_transport transport, const nmbs_callbacks* server_callbacks) {
    expect(pthread_mutex_destroy(&server_stopped_m) == 0);
    expect(pthread_mutex_init(&server_stopped_m, NULL) == 0);

    reset_sockets();

    reset(SERVER);
    reset(CLIENT);

    check(nmbs_server_create(&SERVER, TEST_SERVER_ADDR, platform_conf_socket_server(transport), server_callbacks));
    check(nmbs_client_create(&CLIENT, platform_conf_socket_client(transport)));

    nmbs_set_destination_rtu_address(&CLIENT, TEST_SERVER_ADDR);
    nmbs_set_read_timeout(&SERVER, 500);
    nmbs_set_byte_timeout(&SERVER, 100);

    nmbs_set_read_timeout(&CLIENT, 5000);
    nmbs_set_byte_timeout(&CLIENT, 100);

    expect(pthread_mutex_lock(&server_stopped_m) == 0);
    server_stopped = false;
    expect(pthread_mutex_unlock(&server_stopped_m) == 0);
    expect(pthread_create(&server_thread, NULL, server_listen_thread, &SERVER) == 0);
}