#include "kpl.h"

void kpl_task_queue_init(kpl_task_queue *task_queue) {
    __atomic_store_n(&task_queue->dummy.next, NULL, __ATOMIC_RELAXED);
    __atomic_store_n(&task_queue->head, &task_queue->dummy, __ATOMIC_RELAXED);
    task_queue->tail = (kpl_task*) &task_queue->dummy;
}

void kpl_task_queue_add(kpl_task_queue *task_queue, kpl_task *task) {
    __atomic_store_n(&task->next, NULL, __ATOMIC_RELAXED);
    kpl_task *head = __atomic_exchange_n(&task_queue->head, task, __ATOMIC_SEQ_CST);
    __atomic_store_n(&head->next, task, __ATOMIC_RELEASE);
}

kpl_task *kpl_task_queue_next(kpl_task_queue *task_queue) {
    kpl_task *task = NULL;
    for (;;) {
        kpl_task *tail = task_queue->tail, *next = __atomic_load_n(&tail->next, __ATOMIC_ACQUIRE);
        if (tail == (kpl_task*) &task_queue->dummy) {
            if (!next)
                break;
            tail = task_queue->tail = next;
            next = __atomic_load_n(&tail->next, __ATOMIC_ACQUIRE);
        }
        if (!next) {
            kpl_task *head = __atomic_load_n(&task_queue->head, __ATOMIC_ACQUIRE);
            if (tail != head)
                continue;
            kpl_task_queue_add(task_queue, (kpl_task*) &task_queue->dummy);
            next = __atomic_load_n(&tail->next, __ATOMIC_ACQUIRE);
        }
        if (next) {
            task_queue->tail = next;
            task = tail;
            break;
        }
    }
    if (task)
        __atomic_store_n(&task->next, NULL, __ATOMIC_RELAXED);
    return task;
}

bool kpl_ring_queue_init(kpl_ring_queue *ring_queue, int8_t size_pow2) {
    if (size_pow2 < KPL_RING_QUEUE_MIN_POW2 || size_pow2 > KPL_RING_QUEUE_MAX_POW2)
        return false;
    size_t size = 1L << size_pow2;
    ring_queue->items = calloc(1, sizeof(kpl_ring_queue_item) * size);
    ring_queue->items_mask = size - 1;
    __atomic_store_n(&ring_queue->head_index, 0, __ATOMIC_RELAXED);
    __atomic_store_n(&ring_queue->tail_index, 0, __ATOMIC_RELAXED);
    for (size_t index = 0; index < size; index++)
        __atomic_store_n(&ring_queue->items[index].index, index, __ATOMIC_RELAXED);
    return true;
}

void kpl_ring_queue_free(kpl_ring_queue *ring_queue) {
    free(ring_queue->items);
}

bool kpl_ring_queue_add(kpl_ring_queue *ring_queue, void *data) {
    for (;;) {
        ssize_t tail_index = __atomic_load_n(&ring_queue->tail_index, __ATOMIC_RELAXED);
        kpl_ring_queue_item *item = &ring_queue->items[tail_index & ring_queue->items_mask];
        ssize_t item_index = __atomic_load_n(&item->index, __ATOMIC_ACQUIRE), diff = item_index - tail_index;
        if (diff > 0)
            continue;
        if (diff < 0)
            break;
        if (!__atomic_compare_exchange_n(&ring_queue->tail_index, &tail_index, tail_index + 1, true,
                __ATOMIC_RELAXED, __ATOMIC_RELAXED))
            continue;
        item->data = data;
        __atomic_store_n(&item->index, tail_index + 1, __ATOMIC_RELEASE);
        return true;
    }
    return false;
}

void *kpl_ring_queue_next(kpl_ring_queue *ring_queue) {
    for (;;) {
        ssize_t head_index = __atomic_load_n(&ring_queue->head_index, __ATOMIC_RELAXED);
        kpl_ring_queue_item *item = &ring_queue->items[head_index & ring_queue->items_mask];
        ssize_t item_index = __atomic_load_n(&item->index, __ATOMIC_ACQUIRE),
            diff = item_index - (head_index + 1);
        if (diff > 0)
            continue;
        if (diff < 0)
            break;
        if (!__atomic_compare_exchange_n(&ring_queue->head_index, &head_index, head_index + 1, true,
                __ATOMIC_RELAXED, __ATOMIC_RELAXED))
            continue;
         void *data = item->data;
         item->data = NULL;
         __atomic_store_n(&item->index, head_index + ring_queue->items_mask + 1, __ATOMIC_RELEASE);
         return data;
    }
    return NULL;
}