#include "TIM.h"
#include <stm32f4xx.h>
#include <stm32f4xx_rcc.h>
#include <stm32f4xx_tim.h>
#include <misc.h>

#define NUMBER_OF_TIMERS 6
#define VECTOR_TABLE 0x2001c000
#define TIMER_INDEX_OFFSET 2

TIM_TypeDef* TIMx[NUMBER_OF_TIMERS] = {TIM2, TIM3, TIM4, TIM5, TIM6, TIM7};
int32_t timIrqVectorOffsets[NUMBER_OF_TIMERS] = {0xb0, 0xb4, 0xb8, 0x108, 0x118, 0x11c};
uint8_t irqChannels[NUMBER_OF_TIMERS] = {TIM2_IRQn, TIM3_IRQn, TIM4_IRQn, TIM5_IRQn, TIM6_DAC_IRQn, TIM7_IRQn};
uint32_t RCC_APB1Periph[NUMBER_OF_TIMERS] = {RCC_APB1Periph_TIM2, RCC_APB1Periph_TIM3, RCC_APB1Periph_TIM4, RCC_APB1Periph_TIM5, RCC_APB1Periph_TIM6, RCC_APB1Periph_TIM7};

typedef struct {
    void (*callback)();
    bool used;
    bool repeat;
    uint32_t ticksCurrent;
    uint32_t ticksTotal;
} Timer;

static Timer timers[NUMBER_OF_TIMERS] = {{0,0,0,0,0}};

static void timIrqHandler() {
    for (uint8_t i = 0; i < NUMBER_OF_TIMERS; ++i) {
        TIM_TypeDef* TIM = TIMx[i];
        if (TIM_GetITStatus(TIM, TIM_IT_Update) != RESET) {
            TIM_ClearITPendingBit(TIM, TIM_IT_Update);
            Timer* timer = &timers[i];
            if (timer->ticksCurrent++ >= timers->ticksTotal) {
                if (timer->repeat) {
                    timer->ticksCurrent = 0;
                } else {
                    TIM_Cmd(TIM, DISABLE);
                    timer->used = false;
                }
                if (timer->callback) timer->callback();
            }
        }
    }
}

static void initializeTimer(uint8_t timerIndex, uint16_t period) {
    // Enable clock
    RCC_APB1PeriphClockCmd(RCC_APB1Periph[timerIndex], ENABLE);

    TIM_TypeDef* TIM = TIMx[timerIndex];
    TIM_DeInit (TIM);

    // Time base config
    TIM_TimeBaseInitTypeDef timeBaseStructure;
    timeBaseStructure.TIM_Prescaler = 42000 - 1; // 250 microseconds
    timeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    timeBaseStructure.TIM_Period = period;
    timeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    timeBaseStructure.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM, &timeBaseStructure);
}

static void enableTimerInterrupt(uint8_t timerIndex) {
    // Enable timer irq
    TIM_ITConfig(TIMx[timerIndex], TIM_IT_Update, ENABLE);

    // Enable irq in NVIC
    NVIC_InitTypeDef nvicInitStructure;
    nvicInitStructure.NVIC_IRQChannel = irqChannels[timerIndex];
    nvicInitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    nvicInitStructure.NVIC_IRQChannelSubPriority = 1;
    nvicInitStructure.NVIC_IRQChannelCmd = ENABLE;

    // Set irq vector
    *((void (**)(void)) (VECTOR_TABLE + timIrqVectorOffsets[timerIndex])) = timIrqHandler;

    NVIC_SetPriority(irqChannels[timerIndex], 0);
    NVIC_Init(&nvicInitStructure);
}

static void enableAdcTrigger(uint8_t timerIndex) {
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);

    TIM_TypeDef* TIM = TIMx[timerIndex];
    TIM_SelectOutputTrigger (TIM, TIM_TRGOSource_Update);

    TIM_ClearITPendingBit (TIM, TIM_IT_Update);
    TIM_SetCounter (TIM, 0);
    TIM_ITConfig (TIM, TIM_IT_Update, ENABLE);
}

static void setTimerState(uint8_t timerIndex, FunctionalState state) {
    TIM_Cmd(TIMx[timerIndex], state);
}

static uint8_t getAvailableTimer() {
    for (uint8_t i = 0; i < NUMBER_OF_TIMERS; ++i) {
        Timer* timer = &timers[i];
        if (!timer->used) {
            timer->used = true;
            timer->ticksCurrent = 0;
            return i + TIMER_INDEX_OFFSET;
        }
    }
    return 0;
}

static uint8_t startTimer(int32_t ms, void (*callback)(), bool repeat) {
    if (ms <= 0) callback();

    uint8_t availableTimer = getAvailableTimer();
    if (!availableTimer) return 0; // No timer available

    uint8_t timerIndex = availableTimer - TIMER_INDEX_OFFSET;

    Timer* timer = &timers[timerIndex];
    timer->callback = callback;
    timer->repeat = repeat;
    timer->ticksTotal = (2 * ms) / 65536 + 1;
    initializeTimer(timerIndex, (2 * ms) / timer->ticksTotal);
    enableTimerInterrupt(timerIndex);
    setTimerState(timerIndex, ENABLE);
    return availableTimer;
}

static bool invalidTimer(uint8_t timer) {
    return !(timer >= TIMER_INDEX_OFFSET && timer < NUMBER_OF_TIMERS + TIMER_INDEX_OFFSET);
}

uint8_t TIM_startTimer(int32_t ms, void (*callback)()) {
    return startTimer(ms, callback, false);
}

uint8_t TIM_startRepeatingTimer(int32_t ms, void (*callback)()) {
    return startTimer(ms, callback, true);
}

uint32_t TIM_getTimerValue(uint8_t timer) {
    if (invalidTimer(timer)) return 0;
    uint8_t timerIndex = timer - TIMER_INDEX_OFFSET;
    uint32_t cycles = timers[timerIndex].ticksCurrent * 42000 + TIM_GetCounter(TIMx[timerIndex]);
    return cycles;
}

uint32_t TIM_cyclesToMs(uint32_t cycles) {
    return cycles / (42000 * 2);
}

void TIM_setTimerState(uint8_t timer, bool state) {
    if (invalidTimer(timer)) return;
    setTimerState(timer - TIMER_INDEX_OFFSET, state);
}

void TIM_cancelTimer(uint8_t timer) {
    if (invalidTimer(timer)) return;
    uint8_t timerIndex = timer - TIMER_INDEX_OFFSET;
    setTimerState(timerIndex, DISABLE);
    timers[timerIndex].used = false;
}

uint8_t TIM_startAdcTimer(int32_t ms) {
    uint8_t availableTimer = getAvailableTimer();
    if (!availableTimer) return false; // No timer available

    uint8_t timerIndex = availableTimer - TIMER_INDEX_OFFSET;

    initializeTimer(timerIndex, 2 * ms);
    enableAdcTrigger(timerIndex);
    setTimerState(timerIndex, ENABLE);
    return availableTimer;
}