#include "SysTick.h"
#include "stm32f4xx.h"
#include "core_cm4.h"
#include "stdbool.h"
//#include "stm32f4xx_rcc.h"

#define SystemCoreClock 168000000
#define SYSTICK_IRQ_VECTOR 0x2001C03C
#define PRELOAD_VALUE 168000

typedef struct {
    int counter;
    void (*callback)(void);
    bool stale;
} TIMER;

TIMER timers[10];

void systickIrq () {
    for(int x = 0; x < 10; x++) {
        if(!timers[x].stale){
            if(timers[x].counter == 0){
                timers[x].stale = true;
                timers[x].callback();
            } else {
                timers[x].counter--;
            }
        }
    }
    //DUMP("interrupt");
}


void SysTick_initTimers() {

    for (int c = 0; c < 10; c++) {
        timers[c].stale = true;
        timers[c].counter = 0;
    } 
    SysTick_Config(PRELOAD_VALUE);
    //RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
    *((void (**)(void)) SYSTICK_IRQ_VECTOR) = systickIrq;
    
}

int i;

int SysTick_startTimer(int ms, void (*callback)(void)) {
    for(i = 0; i < 10; i++){
        if(timers[i].stale){
            break;
        }
    }
    if (i >= 10){
        return 0;
    }
    timers[i].stale = false;
    timers[i].counter = ms;
    timers[i].callback = callback;
}