#include <stdint.h>
#include <stm32f4xx.h>
#include <stdbool.h>

volatile uint32_t ISR_Ticks = 0;

struct TASK {
    void (*fn)(void);
    uint32_t counter;
    uint32_t counter_rst;
};

uint8_t Tasks_len = 0;
struct TASK Tasks[8];

void SysTick_Handler()
{
    ISR_Ticks++;
}

bool addTask(void  (*fn)(void), uint32_t counter, uint32_t counter_init)
{
    if (Tasks_len >= 8) {
        return false;
    }
    Tasks[Tasks_len].fn = fn;
    Tasks[Tasks_len].counter = counter_init;
    Tasks[Tasks_len].counter_rst = counter;
    Tasks_len++;
    return true;
}

void taskGreen()
{
    GPIOD->ODR ^= GPIO_ODR_OD12;
}

void taskOrange()
{
    GPIOD->ODR ^= GPIO_ODR_OD13;
}

void taskRed()
{
    GPIOD->ODR ^= GPIO_ODR_OD14;
}

void taskBlue()
{
    GPIOD->ODR ^= GPIO_ODR_OD15;
}

enum STATE {
    STATE_GREEN,
    STATE_ORANGE,
    STATE_RED
};

int main(void)
{
    // GPIO Port D Clock Enable
    RCC->AHB1ENR = RCC_AHB1ENR_GPIODEN;
    // GPIO Port D Pin 15 down to 12 Push/Pull Output
    GPIOD->MODER = GPIO_MODER_MODER12_0
                   | GPIO_MODER_MODER13_0
                   | GPIO_MODER_MODER14_0
                   | GPIO_MODER_MODER15_0;
    // Set all leds off
    GPIOD->ODR = 0;


    // SysTick enable with interupt and clk source to AHB/8
    SysTick->CTRL = SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk;
    SysTick->LOAD = 2000; // 1 ms / (16 MHz / 8)

    addTask(*taskGreen, 200, 100);
    addTask(*taskOrange, 500, 200);
    addTask(*taskRed, 750, 300);
    addTask(*taskBlue, 300, 400);

    // Do forever:
    while (1)
    {
        // Wait a moment
        while (ISR_Ticks == 0)
        {
            __asm__("    WFI"); // sleep until SysTick
        }
        uint32_t ticks = ISR_Ticks;
        ISR_Ticks = 0;

        // decrement all counters
        for (uint8_t i=0; i<Tasks_len; i++)
        {
            if (Tasks[i].counter > ticks)
            {
                Tasks[i].counter -= ticks;
            }
            else
            {
                Tasks[i].counter = 0;
            }
        }

        // rust all tasks where the counter has run out
        for (uint8_t i=0; i<Tasks_len; i++)
        {
            if (Tasks[i].counter == 0)
            {
                Tasks[i].fn();
                Tasks[i].counter = Tasks[i].counter_rst;
            }
        }
    }
}