#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include <exec/list.h>
#include <exec/mem.h>
#include <exec/hal.h>
#include <exec/debcon.h>
#include <exec/task.h>

struct List tasks;
struct Task default_task;
struct Task *current_task;
int interrupts_enabled = 1;
int execTicks = 0;

void TasksInit(void) {
    NewList(&tasks, LP_BLOCK);

    /* create idle task */
    default_task.node.name = "Idle Task";
    AddTask(&default_task, (uintptr_t)HalIdleTask, 0);
    current_task = &default_task;
}

void AddTask(struct Task *task, uintptr_t initial_pc, uintptr_t final_pc) {
    if (!final_pc) final_pc = (uintptr_t)HalIdleTask;
    HalSetupTask(task, initial_pc, final_pc);
    Enqueue(&tasks, (struct Node *)task);
}

struct Task *ThisTask(void) {
    return current_task;
}

void Yield(void) {
    int ie = HalCheckInterrupts();
    HalDisableInterrupts();
    struct Task *prev_task;
    if (!current_task) {
        if (ie) HalEnableInterrupts();
        return;
    }
    prev_task = current_task;
    for (;;) {
        if (current_task->node.next == (struct Node *)&tasks.tail) {
            current_task = (struct Task *)tasks.head;
        } else {
            current_task = (struct Task *)current_task->node.next;
        }

        if (current_task == &default_task) {
            // BUG: idle task should be executed when it's the only runnable task (not a deadlock if blocking due to Delay)
            continue;
        }

        /* signal blocking */
        if (current_task->sig_wait) {
            if (!(current_task->sig_received & current_task->sig_wait)) {
                continue;
            }
        } else {
            /* semaphore blocking */
            if (current_task->blocked_address) {
                int blocked_value = atomic_load_explicit(
                    current_task->blocked_address, memory_order_relaxed);
                if (blocked_value == current_task->blocked_value) {
                    //DebconPutChar('0'+blocked_value);
                    //DebconPutChar('0'+current_task->blocked_value);
                    DebconPrint("BLOCK: "); DebconPrint(current_task->node.name); DebconPutChar('\n');
                    continue;
                } else {
                    current_task->blocked_address = NULL;
                }
            }
        }
        break;
    }
    #if 1
    DebconPrint("YIELD: ");
    DebconPrint(current_task->node.name);
    #endif

    if (prev_task != current_task) HalContextSwitch(&prev_task->sp, current_task->sp);
    if (ie) HalEnableInterrupts();
}

struct Task *CreateTask(char *name, int8_t pri, uintptr_t initial_pc, size_t stack_size) {
    struct Task *task = (struct Task *)AllocMem(sizeof(struct Task));
    memset(task, 0, sizeof(struct Task));
    task->node.name = name;
    task->node.priority = pri;
    task->sp = (uintptr_t)AllocMem(stack_size) + stack_size - sizeof(uintptr_t);
    task->forbids = 0xFF;
    task->sig_alloc = 15;
    AddTask(task, initial_pc, 0);
    return task;
}

void Block(struct Task *task, atomic_int *address, int value) {
    task->blocked_address = address;
    task->blocked_value = value;
    if (task == current_task) Yield();
}

int Forbid(void) {
    int state = HalCheckInterrupts();
    HalDisableInterrupts();
    current_task->forbids <<= 1;
    current_task->forbids |= state&1;
    return state;
}

int Permit(void) {
    int state = current_task->forbids & 1;
    current_task->forbids >>= 1;
    if (state) {
        HalEnableInterrupts();
    }
    return state;
}

uint8_t AllocSignal(uint8_t hint) {
    Forbid();
    if(current_task->sig_alloc & SIGNAL(hint)) {
        for (hint = SIGNAL_MY_BASE; hint < SIGNAL_MAX; hint++) {
            if(!(current_task->sig_alloc & SIGNAL(hint))) {
                break;
            }
        }
    }
    current_task->sig_alloc |= SIGNAL(hint);
    Permit();
    return hint;
}

void FreeSignal(uint8_t signal) {
    Forbid();
    current_task->sig_alloc &= ~SIGNAL(signal);
    Permit();
}

void Signal(struct Task *task, Signals signals) {
    Forbid();
    task->sig_received |= signals;
    if (task != current_task && task->sig_received & task->sig_wait && task->node.priority > current_task->node.priority) {
        Yield();
    }
    Permit();
}

Signals Wait(Signals signals) {
    Forbid();
    current_task->sig_wait |= signals;
    while (!(current_task->sig_received & current_task->sig_wait)) {
        Yield();
    }
    if (current_task->sig_alloc > 0x10000) {
        DebconPrint("Signal leak detected in task: ");
        DebconPrint(current_task->node.name);
        DebconPutChar('\n');
        for(;;);
    }
    //DebconPrintHex((uint32_t)signals);
    Signals received = current_task->sig_received & current_task->sig_wait;
    current_task->sig_received &= ~signals;
    current_task->sig_wait &= ~signals;
    Permit();
    return received;
}

void Delay(int ticks) {
    ThisTask()->blocked_address = (atomic_int *)&execTicks;
    ThisTask()->blocked_value = execTicks + ticks;
    Yield();
}

void InterruptsSaveAndDisable(void) {
    interrupts_enabled = HalCheckInterrupts();
    HalDisableInterrupts();
}

void InterruptsRestore(void) {
    if (interrupts_enabled) {
        HalEnableInterrupts();
    }
}