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


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

typedef struct _ASleep {
    _Cor_Mutex mutex;
    unsigned users;
    Loop_Timer *sleepers;
    int lo;
    int hi;
    int max;
    _Cor_Semaphore sem;
    bool sleeploop_cancel;
    _Cor_Thread sleeploop;
    bool started;
} _ASleep;

static _ASleep asleep;

static void *ASleep_Sleeper(void *param);

static void ASleep_ctor(
    _ASleep *me
){
    me->sleepers = NULL;
    me->lo = 0;
    me->hi = 0;
    me->max = 0;

    _Cor_Mutex_ctor(&me->mutex);
    _Cor_Semaphore_ctor(&me->sem);
    me->sleeploop_cancel = false;
    _Cor_Thread_ctor(&me->sleeploop, ASleep_Sleeper, me);
    me->started = true;
}



static void ASleep_dtor(
    _ASleep *me
){
    me->started = false;
    me->sleeploop_cancel = true;
    _Cor_Semaphore_Signal(&me->sem);
    _Cor_Thread_Join(&me->sleeploop);
    _Cor_Thread_dtor(&me->sleeploop);
    _Cor_Sempahore_dtor(&me->sem);
    _Cor_Mutex_dtor(&me->mutex);
    free(me->sleepers);
}


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


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


static void ASleep_AddSleeper(
    _ASleep *me,
    int64_t when,
    Future *who
){
    _Cor_Mutex_Lock(&me->mutex);

    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 (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;
    }

    _Cor_Mutex_Unlock(&me->mutex);
    _Cor_Semaphore_Signal(&me->sem);
}


static void ASleep_RemoveSleeper(
    _ASleep *me,
    int64_t when,
    Future *who
){
    _Cor_Mutex_Lock(&me->mutex);

    int i;
    for (i = me->lo; i != me->hi; ){
        if (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;
        }
    }

    _Cor_Mutex_Unlock(&me->mutex);
    _Cor_Semaphore_Signal(&me->sem);
}


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

    while (!me->sleeploop_cancel){
        _Cor_Mutex_Lock(&me->mutex);
        bool got_one;
        if (me->lo != me->hi){
            int64_t when = me->sleepers[0].when;
            _Cor_Mutex_Unlock(&me->mutex);
            got_one = _Cor_Semaphore_Wait(&me->sem, when);
        } else {
            _Cor_Mutex_Unlock(&me->mutex);
            got_one = _Cor_Semaphore_Wait(&me->sem, -1);
        }
        if (!got_one){
            // timed out, so...
            // issue any due timers
            int64_t now = _Cor_Realtime_Now();
            while(me->lo != me->hi && 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;
                }
                _Cor_Mutex_Unlock(&me->mutex);
                Future_SetResult(fut, false, NULL);
                _Cor_Mutex_Lock(&me->mutex);
                now = _Cor_Realtime_Now();
            }
            _Cor_Mutex_Unlock(&me->mutex);
        }
    }

    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;
    }
    int64_t endtime = _Cor_Realtime_Now() + (int64_t)(delay * 1000000000);

    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;
}