#include "threads.h"
#include <errno.h>
#define sema_err (thrd_success - 1)
#define NSEC_PER_SEC ((time_t)1000000000L)

int sema_init(sema_t* sema, ssize_t value) {
    int rv;

    rv = mtx_init(&sema->__lock, mtx_plain);
    if (rv != thrd_success) return rv;

    rv = cnd_init(&sema->__cond);
    if (rv != thrd_success) {
        mtx_destroy(&sema->__lock);
        return rv;
    }

    sema->__val = value;
    return thrd_success;
}

int sema_get_value(sema_t* sema, ssize_t* p_value) {
    int rv;

    rv = mtx_lock(&sema->__lock);
    if (rv != thrd_success) return rv;

    *p_value = sema->__val;

    return mtx_unlock(&sema->__lock);
}

int sema_up(sema_t* sema) {
    return sema_up_many(sema, 1);
}

int sema_up_many(sema_t* sema, ssize_t n) {
    int rv;

    rv = mtx_lock(&sema->__lock);
    if (rv != thrd_success) return rv;

    sema->__val += n;

    rv = mtx_unlock(&sema->__lock);
    if (rv != thrd_success) return rv;

    for (ssize_t i = 0; i < n; i++) {
        rv = cnd_signal(&sema->__cond);
        if (rv != thrd_success) return rv;
    }
    return thrd_success;
}

int sema_down(sema_t* sema) {
    return sema_down_many(sema, 1);
}

int sema_down_many(sema_t* sema, ssize_t n) {
    int rv;

    rv = mtx_lock(&sema->__lock);
    if (rv != thrd_success) return rv;

    while (sema->__val < n) {
        rv = cnd_wait(&sema->__cond, &sema->__lock);
        if (rv != thrd_success) return rv;
    }

    sema->__val -= n;
    return mtx_unlock(&sema->__lock);
}

int sema_try_down(sema_t* sema) {
    return sema_try_down_many(sema, 1);
}

int sema_try_down_many(sema_t* sema, ssize_t n) {
    int rv;

    rv = mtx_lock(&sema->__lock);
    if (rv != thrd_success) return rv;

    if (sema->__val < n) {
        rv = mtx_unlock(&sema->__lock);
        if (rv != thrd_success) return rv;
        errno = EAGAIN;
        return sema_err;
    }

    sema->__val -= n;
    return mtx_unlock(&sema->__lock);
}

int sema_down_timed(sema_t* sema, const struct timespec* time) {
    return sema_down_many_timed(sema, time, 1);
}

static bool diff_timespec(
    struct timespec* dst,
    const struct timespec* src
) {
    *dst = (struct timespec) {
        .tv_sec = dst->tv_sec - src->tv_sec,
        .tv_nsec = dst->tv_nsec - src->tv_nsec
    };
    if (dst->tv_nsec < 0) {
        dst->tv_sec--;
        dst->tv_nsec += NSEC_PER_SEC;
    }
    return dst->tv_sec > 0 || (dst->tv_sec == 0 && dst->tv_nsec > 0);
}

int sema_down_many_timed(sema_t* sema, const struct timespec* time, ssize_t n) {
    int rv;
    struct timespec start, end, remaining = *time;

    timespec_get(&start, TIME_UTC);
    rv = mtx_timedlock(&sema->__lock, &remaining);
    if (rv != thrd_success) return rv;
    timespec_get(&end, TIME_UTC);
    diff_timespec(&end, &start);
    if (!diff_timespec(&remaining, &end)) return thrd_timedout;

    while (sema->__val < n) {
        timespec_get(&start, TIME_UTC);
        rv = cnd_timedwait(&sema->__cond, &sema->__lock, &remaining);
        if (rv != thrd_success) return rv;
        timespec_get(&end, TIME_UTC);
        diff_timespec(&end, &start);
        if (!diff_timespec(&remaining, &end)) return thrd_timedout;
    }

    sema->__val -= n;
    return mtx_unlock(&sema->__lock);
}

void sema_destroy(sema_t* sema) {
    cnd_destroy(&sema->__cond);
    mtx_destroy(&sema->__lock);
}