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

#include "bump.h"
#include "delay.h"
#include "clock.h"
#include "reflectance.h"
#include "cpu.h"

#include "inc/msp432p401r.h"

#define REDLED (*((volatile uint8_t *)(0x42098040)))

#define DUMP_SIZE 256

uint8_t bump_status;
uint8_t bump_dump[DUMP_SIZE];

void DebugDump(uint8_t bump) {
    static int idx = 0;
    idx = (idx + 1) % DUMP_SIZE;
    bump_dump[idx] = bump;
}

// **************SysTick_Init*********************
// Initialize SysTick periodic interrupts
// Input: interrupt period
//           Units of period are in bus clock period
//           Maximum is 2^24-1
//           Minimum is determined by execution time of the ISR
// Input: priority 0 (high) to 7 (low)
// Output: none
void SysTickInit(uint32_t period, uint32_t priority) {
    SysTick->CTRL = 0;             // 1) disable SysTick during setup
    SysTick->LOAD = period - 1;    // 2) reload value sets period
    SysTick->VAL = 0;              // 3) any write to current clears it
    SCB->SHP[11] = priority << 5;  // set priority into top 3 bits of 8-bit register
    SysTick->CTRL = 0x00000007;    // 4) enable SysTick with core clock and interrupts
}

void SysTick_Handler(void) {  // every 1ms
    // REDLED = 1;
    // uint8_t bump = BumpRead();
    // REDLED = 0;
    volatile int i = 0;
    ++i;
}

int main(void) {
    ClockInit48MHz();
    BumpInit();
    ReflectanceInit();
    SysTickInit(480000, 1); // 1ms SysTickInit
    CPU_cpsie();             // Enable interrupts

    while (true) {
        REDLED = 1;
        REDLED = 0;
    }
}