#include "asleep.h"
#include "task.h"
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <errno.h>
#include <math.h>
#include <string.h>
#include "timespec_utils.h"


typedef struct Loop_Timer {
    struct timespec when;
    Future *who;
} Loop_Timer;

typedef struct _ASleep {
    pthread_mutex_t mutex;
    unsigned users;
    Loop_Timer *sleepers;
    int lo;
    int hi;
    int max;
    pthread_cond_t cond;
    bool sleeploop_cancel;
    pthread_t sleeploop;
    bool started;
} _ASleep;

static _ASleep asleep;

static void *ASleep_Sleeper(void *param);

static void ASleep_ctor(
    _ASleep *me
){
    int r;

    me->sleepers = NULL;
    me->lo = 0;
    me->hi = 0;
    me->max = 0;

    r = pthread_mutex_init(&me->mutex, NULL);
    assert(r == 0);
    r = pthread_cond_init(&me->cond, NULL);
    assert(r == 0);
    me->sleeploop_cancel = false;
    r = pthread_create(&me->sleeploop, NULL, ASleep_Sleeper, me);
    assert(r == 0);
    me->started = true;
}



static void ASleep_dtor(
    _ASleep *me
){
    int r;
    me->started = false;
    me->sleeploop_cancel = true;
    r = pthread_cond_signal(&me->cond);
    assert(r == 0);
    r = pthread_join(me->sleeploop, NULL);
    assert(r == 0);
    r = pthread_cond_destroy(&me->cond);
    assert(r == 0);
    pthread_mutex_destroy(&me->mutex);
    assert(r == 0);
    free(me->sleepers);
}


void ASleep_StartSystem(){
    ASleep_ctor(&asleep);
}


void ASleep_StopSystem(){
    ASleep_dtor(&asleep);
}


static void ASleep_AddSleeper(
    _ASleep *me,
    struct timespec when,
    Future *who
){
    int r = pthread_mutex_lock(&me->mutex);
    assert(r == 0);

    int nsleepers;
    if (me->lo <= me->hi){
        int nsleepers = me->hi - me->lo;
        // ensure there's always an empty slot so that lo == hi always means no sleepers
        if (nsleepers+1 >= me->max) {
            me->max = (me->max == 0) ? 4 : me->max * 2;
            me->sleepers = realloc(me->sleepers, me->max * sizeof(Loop_Timer));
            assert(me->sleepers);
        }
    } else {
        nsleepers = me->hi + me->max - me->lo;
        // ensure there's always an empty slot so that lo == hi always means no sleepers
        if (nsleepers+1 >= me->max) {
            int oldmax = me->max;
            me->max = (me->max == 0) ? 4 : me->max * 2;
            me->sleepers = realloc(me->sleepers, me->max * sizeof(Loop_Timer));
            assert(me->sleepers);
            memmove(&me->sleepers[me->lo], &me->lo + me->max - oldmax, sizeof(*me->sleepers) * oldmax - me->lo);
            me->lo += me->max - oldmax;
        }
    }

    // insertion sort - we're assuming there's not going to be many sleepers active at once
    int i;
    int nexti;
    for (i = me->hi; i != me->lo; i = nexti){
        nexti = i-1;
        if (i < 0){
            nexti += me->max;
        }
        if (timespec_lte(me->sleepers[nexti].when, when)){
            break;
        }
        me->sleepers[i] = me->sleepers[nexti];
    }
    me->sleepers[i].when = when;
    me->sleepers[i].who = who;
    me->hi += 1;
    if (me->hi >= me->max){
        me->hi -= me->max;
    }

    r = pthread_cond_signal(&me->cond);
    assert(r == 0);

    r = pthread_mutex_unlock(&me->mutex);
    assert(r == 0);
}


static void ASleep_RemoveSleeper(
    _ASleep *me,
    struct timespec when,
    Future *who
){
    int r = pthread_mutex_lock(&me->mutex);
    assert(r == 0);

    int i;
    for (i = me->lo; i != me->hi; ){
        if (timespec_eq(me->sleepers[i].when, when) && me->sleepers[i].who == who){
            int previ = i;
            i += 1;
            if (i >= me->max){
                i -= me->max;
            }
            for (; i != me->hi; ){
                me->sleepers[previ] = me->sleepers[i];
                previ = i;
                i += 1;
                if (i >= me->max){
                    i -= me->max;
                }
            }
            me->hi = previ;
            break;
        }
        i += 1;
        if (i >= me->max){
            i -= me->max;
        }
    }

    r = pthread_cond_signal(&me->cond);
    assert(r == 0);

    r = pthread_mutex_unlock(&me->mutex);
    assert(r == 0);
}


static void *ASleep_Sleeper(
    void *param
){
    _ASleep *me = (_ASleep *)param;
    int r;

    while (!me->sleeploop_cancel){
        r = pthread_mutex_lock(&me->mutex);
        assert(r == 0);
        if (me->lo != me->hi){
            struct timespec when = me->sleepers[0].when;

            r = pthread_mutex_unlock(&me->mutex);
            assert(r == 0);
            r = pthread_cond_timedwait(&me->cond, &me->mutex, &when);
            assert(r == 0 || r == ETIMEDOUT);
        } else {
            r = pthread_mutex_unlock(&me->mutex);
            assert(r == 0);
            r = pthread_cond_wait(&me->cond, &me->mutex);
            assert(r == 0);
        }
        // issue any due timers
        struct timespec now;
        r = clock_gettime(CLOCK_REALTIME, &now);
        assert(r == 0);
        while(me->lo != me->hi && timespec_lte(me->sleepers[me->lo].when, now)) {
            Future *fut = me->sleepers[me->lo].who;
            me->lo += 1;
            if (me->lo >= me->max){
                me->lo -= me->max;
            }
            r = pthread_mutex_unlock(&me->mutex);
            assert(r == 0);
            Future_SetResult(fut, false, NULL);
            r = pthread_mutex_lock(&me->mutex);
            assert(r == 0);
            r = clock_gettime(CLOCK_REALTIME, &now);
            assert(r == 0);
        }
        r = pthread_mutex_unlock(&me->mutex);
        assert(r == 0);
    }

    return NULL;
}


// value is a pointer to a float delay in seconds
bool ASleep(
    float delay,
    void **value
){
    assert(current_task && asleep.started);
    if (delay < 0){
        delay = 0;
    }
    float secs = floorf(delay);
    float nsecs = (delay - secs) * 1e9;
    struct timespec endtime;
    clock_gettime(CLOCK_REALTIME, &endtime);
    endtime.tv_nsec += (int)nsecs;
    if (endtime.tv_nsec > 1000000000){
        endtime.tv_sec += 1;
        endtime.tv_nsec -= 1000000000;
    }
    endtime.tv_sec += (int)secs;

    Future fut;
    Future_ctor(&fut);
    ASleep_AddSleeper(&asleep, endtime, &fut);
    bool res = Future_Await(&fut, value);
    ASleep_RemoveSleeper(&asleep, endtime, &fut);
    Future_dtor(&fut);

    return res;
}