#include "task.h"

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h>

typedef struct TaskNode_t {
    void (*f)(void);
    struct TaskNode_t *next;
    uint16_t runTime;
    uint8_t overflow;
} TaskNode;

typedef struct {
    TaskNode *head;
    TaskNode *tail;
} TaskList;

volatile bool flTask = false;

#define MAX_TASKS (20)
static TaskNode nodes[MAX_TASKS];

static TaskList tasks;
static TaskList freeSlots;

static volatile uint16_t ticks = 0;

void task_queue(TaskList *lst, TaskNode *task);

void task_init(TimerResolution res)
{
    /*
     * Setup lists
     */
    tasks.head = NULL;
    tasks.tail = NULL;

    TaskNode *last, *cur;

    last = &nodes[0];
    freeSlots.head = last;

    for (int i = 1; i < MAX_TASKS; ++i) {
        cur = &nodes[i];

        last->next = cur;
        last = cur;
    }

    last->next = NULL;
    freeSlots.tail = last;

    /*
     * Setup timer
     *
     * Clear interrupt flag
     */
    TIFR1 |= (1<<OCF1A);

    /*
     * CTC Mode: WGM1n: 0100
     * Output Disabled: COM1xn: 0
     */
    TCCR1A = (0<<WGM11) | (0<<WGM10);

    /*
     * CTC Mode: WGM1n: 0100
     *
     * Clock speed:
     *   20MHz system clock
     *   TimerMilli:   CLK/1 (20MHz) with OCR1A = 20000 (1kHz frequency)
     *   Timer100Nano: CLK/1 (20MHz) with OCR1A = 2000 (10kHz frequency)
     */
    TCCR1B = (0<<WGM13) | (1<<WGM12) | (0<<CS12) | (0<<CS11) | (1<<CS10);
    switch (res) {
        case TimerMilli:
            OCR1A = 20000;
            break;
        case Timer100Nano:
            OCR1A = 2000;
            break;
    }

    /*
     * Enable interrupt
     */
    TIMSK1 = (1<<OCIE1A);
}

bool task_add(void (*f)(void), uint16_t delay)
{
    if (!freeSlots.head) {
        return false;
    }

    PORTD |= (1<<PD2);

    TaskNode *task = freeSlots.head;
    freeSlots.head = task->next;

    cli();

    task->runTime = ticks + delay;
    task->overflow = task->runTime < ticks;

    sei();

    task->f = f;

    task_queue(&tasks, task);

    PORTD &= ~(1<<PD2);

    return true;
}

void task_queue(TaskList *lst, TaskNode *task)
{
    if (!lst->head) {
        lst->head = task;
    } else {
        lst->tail->next = task;
    }
    lst->tail = task;
    task->next = NULL;
}

bool task_process_next(void)
{
    static TaskNode *task = NULL;
    static TaskNode *prev = NULL;

    if (!task) {
        task = tasks.head;
        prev = NULL;
    }

    if (!task) {
        return false;
    }

    if (ticks == task->runTime || (ticks > task->runTime && !task->overflow)) {
        PORTD |= (1<<PD4);

        task->f();

        PORTD &= ~(1<<PD4);

        if (prev) {
            prev->next = task->next;
        } else {
            tasks.head = task->next;
        }

        task_queue(&freeSlots, task);
    } else if (ticks < task->runTime && task->overflow) {
        task->overflow = 0;
    }

    prev = task;
    task = task->next;

    return !!task;
}

/*!
 * OCR1A == TCNT1 interrupt.
 */
ISR(TIMER1_COMPA_vect)
{
    ++ticks; // allow overflow
    flTask = true;
}