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

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

#include "../inc/msp432p401r.h"

#define DUMP_SIZE 16

uint8_t bump_status;
uint8_t bump_dump[DUMP_SIZE];
int32_t refl_dump[DUMP_SIZE];

void DebugDump(uint8_t bump, int32_t refl) {
    static int idx = 0;
    idx = (idx + 1) % DUMP_SIZE;

    bump_dump[idx] = bump;
    refl_dump[idx] = refl;
}

// **************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
}

volatile uint8_t reflectance = 0xff;
int tick_count = 0;

void SysTick_Handler(void) {  // every 1ms
    ++tick_count;

    if (1 == (tick_count % 10)) {
        ReflectanceStart(); // every 10ms
    }

    if (2 == (tick_count % 10)) {
        uint8_t r = ReflectanceEnd();
        DebugDump( BumpRead(), ReflectancePosition(r) );
        return;
    }
}

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

    while (true);
}