#include <assert.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#ifndef WIN32
#include <sys/select.h>
#include <sys/time.h>
#else
#include <Winsock2.h>
#include "windows/WinPorting.h"
#endif

#include "dbg.h"
#include "eventlog.h"
#include "lcm_internal.h"

typedef enum {
    LCM_LOGPROV_READ_MODE = 0,
    LCM_LOGPROV_WRITE_MODE = 1,
    LCM_LOGPROV_APPEND_MODE = 2,
} lcm_log_provider_mode_t;

typedef struct _lcm_provider_t lcm_logprov_t;
struct _lcm_provider_t {
    lcm_t *lcm;

    char *filename;

    // The log file mode (reading, writing, append)
    lcm_log_provider_mode_t log_mode;

    lcm_eventlog_t *log;
    lcm_eventlog_event_t *event;

    double speed;
    int64_t next_clock_time;
    int64_t start_timestamp;

    int thread_created;
    GThread *timer_thread;
    int notify_pipe[2];
    int timer_pipe[2];
};

static void lcm_logprov_destroy(lcm_logprov_t *lr)
{
    dbg(DBG_LCM, "closing lcm log provider context\n");
    if (lr->thread_created) {
        /* Destroy the timer thread */
        int64_t abort_cmd = -1;
        int status = lcm_internal_pipe_write(lr->timer_pipe[1], &abort_cmd, sizeof(abort_cmd));
        if (status < 0) {
            perror(__FILE__ " - write (abort_cmd)");
        }
        g_thread_join(lr->timer_thread);
    }

    if (lr->notify_pipe[0] >= 0)
        lcm_internal_pipe_close(lr->notify_pipe[0]);
    if (lr->notify_pipe[1] >= 0)
        lcm_internal_pipe_close(lr->notify_pipe[1]);
    if (lr->timer_pipe[0] >= 0)
        lcm_internal_pipe_close(lr->timer_pipe[0]);
    if (lr->timer_pipe[1] >= 0)
        lcm_internal_pipe_close(lr->timer_pipe[1]);

    if (lr->event)
        lcm_eventlog_free_event(lr->event);
    if (lr->log)
        lcm_eventlog_destroy(lr->log);

    free(lr->filename);
    free(lr);
}

static int64_t timestamp_now(void)
{
    GTimeVal tv;
    g_get_current_time(&tv);
    return (int64_t) tv.tv_sec * 1000000 + tv.tv_usec;
}

static void *timer_thread(void *user)
{
    lcm_logprov_t *lr = (lcm_logprov_t *) user;
    int64_t abstime;
    struct timeval sleep_tv;

    while (lcm_internal_pipe_read(lr->timer_pipe[0], &abstime, 8) == 8) {
        if (abstime < 0)
            return NULL;

        int64_t now = timestamp_now();

        if (abstime > now) {
            int64_t sleep_utime = abstime - now;
            sleep_tv.tv_sec = sleep_utime / 1000000;
            sleep_tv.tv_usec = sleep_utime % 1000000;

            // sleep until the next timed message, or until an abort message
            fd_set fds;
            FD_ZERO(&fds);
            FD_SET(lr->timer_pipe[0], &fds);

            int status = select(lr->timer_pipe[0] + 1, &fds, NULL, NULL, &sleep_tv);

            if (0 == status) {
                // select timed out
                if (lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
                    perror(__FILE__ " - write (timer select)");
                }
            }
        } else {
            if (lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
                perror(__FILE__ " - write (timer)");
            }
        }
    }
    perror("timer_thread read failed");
    return NULL;
}

static void new_argument(gpointer key, gpointer value, gpointer user)
{
    lcm_logprov_t *lr = (lcm_logprov_t *) user;
    if (!strcmp((char *) key, "speed")) {
        char *endptr = NULL;
        lr->speed = strtod((char *) value, &endptr);
        if (endptr == value)
            fprintf(stderr, "Warning: Invalid value for speed\n");
    } else if (!strcmp((char *) key, "start_timestamp")) {
        char *endptr = NULL;
        lr->start_timestamp = strtoll((char *) value, &endptr, 10);
        if (endptr == value)
            fprintf(stderr, "Warning: Invalid value for start_timestamp\n");
    } else if (!strcmp((char *) key, "mode")) {
        const char *mode = (char *) value;
        if (!strcmp(mode, "r")) {
            lr->log_mode = LCM_LOGPROV_READ_MODE;
        } else if (!strcmp(mode, "w")) {
            lr->log_mode = LCM_LOGPROV_WRITE_MODE;
        } else if (!strcmp(mode, "a")) {
            lr->log_mode = LCM_LOGPROV_APPEND_MODE;
        } else {
            fprintf(stderr, "Warning: Invalid value for mode: %s\n", mode);
        }
    } else {
        fprintf(stderr, "Warning: unrecognized option: [%s]\n", (const char *) key);
    }
}

static int load_next_event(lcm_logprov_t *lr)
{
    if (lr->event)
        lcm_eventlog_free_event(lr->event);

    lr->event = lcm_eventlog_read_next_event(lr->log);
    if (!lr->event)
        return -1;

    return 0;
}

static lcm_provider_t *lcm_logprov_create(lcm_t *parent, const char *target, const GHashTable *args)
{
    if (!target || !strlen(target)) {
        fprintf(stderr, "Error: Missing filename\n");
        return NULL;
    }

    lcm_logprov_t *lr = (lcm_logprov_t *) calloc(1, sizeof(lcm_logprov_t));
    lr->lcm = parent;
    lr->filename = strdup(target);
    lr->speed = 1;
    lr->next_clock_time = -1;
    lr->start_timestamp = -1;

    g_hash_table_foreach((GHashTable *) args, new_argument, lr);

    dbg(DBG_LCM, "Initializing LCM log provider context...\n");
    dbg(DBG_LCM, "Filename %s\n", lr->filename);

    if (lcm_internal_pipe_create(lr->notify_pipe) != 0) {
        perror(__FILE__ " - pipe (notify)");
        lcm_logprov_destroy(lr);
        return NULL;
    }
    if (lcm_internal_pipe_create(lr->timer_pipe) != 0) {
        perror(__FILE__ " - pipe (timer)");
        lcm_logprov_destroy(lr);
        return NULL;
    }
    // fcntl (lcm->notify_pipe[1], F_SETFL, O_NONBLOCK);

    switch (lr->log_mode) {
    case LCM_LOGPROV_READ_MODE:
        lr->log = lcm_eventlog_create(lr->filename, "r");
        break;
    case LCM_LOGPROV_WRITE_MODE:
        lr->log = lcm_eventlog_create(lr->filename, "w");
        break;
    case LCM_LOGPROV_APPEND_MODE:
        lr->log = lcm_eventlog_create(lr->filename, "a");
        break;
    default:
        fprintf(stderr, "Error: invalid log_mode: %d\n", lr->log_mode);
        lcm_logprov_destroy(lr);
        return NULL;
    }

    if (!lr->log) {
        fprintf(stderr, "Error: Failed to open %s: %s\n", lr->filename, strerror(errno));
        lcm_logprov_destroy(lr);
        return NULL;
    }

    // only start the reader thread if we're in read mode
    if (lr->log_mode == LCM_LOGPROV_READ_MODE) {
        if (load_next_event(lr) < 0) {
            fprintf(stderr, "Error: Failed to read first event from log\n");
            lcm_logprov_destroy(lr);
            return NULL;
        }

        /* Start the reader thread */
        lr->timer_thread = g_thread_create(timer_thread, lr, TRUE, NULL);
        if (!lr->timer_thread) {
            fprintf(stderr, "Error: LCM failed to start timer thread\n");
            lcm_logprov_destroy(lr);
            return NULL;
        }
        lr->thread_created = 1;

        if (lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
            perror(__FILE__ " - write (reader create)");
        }

        if (lr->start_timestamp > 0) {
            dbg(DBG_LCM, "Seeking to timestamp: %lld\n", (long long) lr->start_timestamp);
            lcm_eventlog_seek_to_timestamp(lr->log, lr->start_timestamp);
        }
    }

    return lr;
}

static int lcm_logprov_get_fileno(lcm_logprov_t *lr)
{
    return lr->notify_pipe[0];
}

static int lcm_logprov_handle(lcm_logprov_t *lr)
{
    lcm_recv_buf_t rbuf;

    if (!lr->event)
        return -1;

    char ch;
    int status = lcm_internal_pipe_read(lr->notify_pipe[0], &ch, 1);
    if (status == 0) {
        fprintf(stderr, "Error: lcm_handle read 0 bytes from notify_pipe\n");
        return -1;
    } else if (status < 0) {
        fprintf(stderr, "Error: lcm_handle read: %s\n", strerror(errno));
        return -1;
    }

    int64_t now = timestamp_now();
    /* Initialize the wall clock if this is the first time through */
    if (lr->next_clock_time < 0)
        lr->next_clock_time = now;

    //    rbuf.channel = lr->event->channel,
    rbuf.data = (uint8_t *) lr->event->data;
    rbuf.data_size = lr->event->datalen;
    rbuf.recv_utime = lr->next_clock_time;
    rbuf.lcm = lr->lcm;

    if (lcm_try_enqueue_message(lr->lcm, lr->event->channel))
        lcm_dispatch_handlers(lr->lcm, &rbuf, lr->event->channel);

    int64_t prev_log_time = lr->event->timestamp;
    if (load_next_event(lr) < 0) {
        /* end-of-file reached.  This call succeeds, but next call to
         * _handle will fail */
        lr->event = NULL;
        if (lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
            perror(__FILE__ " - write(notify)");
        }
        return 0;
    }

    /* Compute the wall time for the next event */
    if (lr->speed > 0)
        lr->next_clock_time += (lr->event->timestamp - prev_log_time) / lr->speed;
    else
        lr->next_clock_time = now;

    if (lr->next_clock_time > now) {
        int wstatus = lcm_internal_pipe_write(lr->timer_pipe[1], &lr->next_clock_time, 8);
        if (wstatus < 0) {
            perror(__FILE__ " - write(timer_pipe)");
        }
    } else {
        int wstatus = lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1);
        if (wstatus < 0) {
            perror(__FILE__ " - write(notify_pipe)");
        }
    }

    return 0;
}

static int lcm_logprov_publish(lcm_logprov_t *lcm, const char *channel, const void *data,
                               unsigned int datalen)
{
    if (lcm->log_mode == LCM_LOGPROV_READ_MODE) {
        dbg(DBG_LCM, "Called publish(), but lcm file provider is in read mode\n");
        return -1;
    }
    int channellen = strlen(channel);

    int64_t mem_sz = sizeof(lcm_eventlog_event_t) + channellen + 1 + datalen;

    lcm_eventlog_event_t *le = (lcm_eventlog_event_t *) malloc(mem_sz);
    memset(le, 0, mem_sz);

    GTimeVal tv;
    g_get_current_time(&tv);
    le->timestamp = (int64_t) tv.tv_sec * 1000000 + tv.tv_usec;
    ;
    le->channellen = channellen;
    le->datalen = datalen;
    // log_write_event will handle le.eventnum.

    le->channel = ((char *) le) + sizeof(lcm_eventlog_event_t);
    strcpy(le->channel, channel);
    le->data = le->channel + channellen + 1;
    assert((char *) le->data + datalen == (char *) le + mem_sz);
    memcpy(le->data, data, datalen);

    lcm_eventlog_write_event(lcm->log, le);
    free(le);

    return 0;
}

#ifdef WIN32
static lcm_provider_vtable_t logprov_vtable;
#else
static lcm_provider_vtable_t logprov_vtable = {
    .create = lcm_logprov_create,
    .destroy = lcm_logprov_destroy,
    .subscribe = NULL,
    .unsubscribe = NULL,
    .publish = lcm_logprov_publish,
    .handle = lcm_logprov_handle,
    .get_fileno = lcm_logprov_get_fileno,
};
#endif

static lcm_provider_info_t logprov_info;

void lcm_logprov_provider_init(GPtrArray *providers)
{
#ifdef WIN32
    // Microsoft VS compiler issues. Can't do this statically
    logprov_vtable.create = lcm_logprov_create;
    logprov_vtable.destroy = lcm_logprov_destroy;
    logprov_vtable.subscribe = NULL;
    logprov_vtable.unsubscribe = NULL;
    logprov_vtable.publish = lcm_logprov_publish;
    logprov_vtable.handle = lcm_logprov_handle;
    logprov_vtable.get_fileno = lcm_logprov_get_fileno;
#endif

    logprov_info.name = "file";
    logprov_info.vtable = &logprov_vtable;

    g_ptr_array_add(providers, &logprov_info);
}
