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

#include "../common/cpu.h"
#include "../common/motor.h"
#include "../common/pwm.h"
#include "../common/clock.h"
#include "../common/input_capture.h"
#include "../common/timer_a1.h"
#include "../common/tachometer.h"

#include "inc/msp432p401r.h"

void RedLEDInit(void) {
    P1->SEL0 &= ~0x01;
    P1->SEL1 &= ~0x01;   // 1) configure P1.0 as GPIO
    P1->DIR |= 0x01;     // 2) make P1.0 out
}

// bit-banded address
#define REDLED (*((volatile uint8_t *)(0x42098040)))

void ColorLEDInit(void) {
    P2->SEL0 &= ~0x07;
    P2->SEL1 &= ~0x07;    // 1) configure P2.2-P2.0 as GPIO
    P2->DIR |= 0x07;      // 2) make P2.2-P2.0 out
    P2->DS |= 0x07;       // 3) activate increased drive strength
    P2->OUT &= ~0x07;     //    all LEDs off
}

// bit-banded addresses
#define BLUEOUT  (*((volatile uint8_t *)(0x42098068)))
#define GREENOUT (*((volatile uint8_t *)(0x42098064)))
#define REDOUT   (*((volatile uint8_t *)(0x42098060)))

uint16_t period0, period1;  // (1/SMCLK) units = 83.3 ns units
uint16_t first0, first1;   // Timer A3 first edge, P10.4/.5
bool done0, done1;        // set each rising

// max period is (2^16-1)*83.3 ns = 5.4612 ms
// min period determined by time to run ISR, which is about 1 us
void PeriodMeasure0(uint16_t time) {
    REDOUT ^= 1;
    period0 = time - first0;  // 83.3 ns resolution
    first0 = time; // setup for next
    done0 = true;
}

// max period is (2^16-1)*83.3 ns = 5.4612 ms
// min period determined by time to run ISR, which is about 1 us
void PeriodMeasure1(uint16_t time) {
    REDLED ^= 1;
    period1 = time - first1;  // 83.3 ns resolution
    first1 = time; // setup for next
    done1 = true;
}

void dummy() {
    volatile int i = 1;
    i = i + 2;
    return;
}

int main_f(void) {
    CPU_cpsid(); // Disable interrupts
    ClockInit48MHz();  // 48 MHz clock; 12 MHz Timer A clock
    RedLEDInit();
    ColorLEDInit();
    PWMInitMotor(3751);
    MotorInit();
    // TimerA1Init(&dummy, 1000);
    TimerA3CaptureInit(&PeriodMeasure0, &PeriodMeasure1);
    // MotorForward(3751, 3751);  // 50%
    // MotorForward(1000, 1000);
    CPU_cpsie(); // Enable interrupts
    CPU_wfi(); // Wait for interrupt
    return 0;
}

uint16_t speed_buf[500];       // RPM, radius 0.035m wheels
uint32_t period_buf[500];      // 1/12MHz = 0.083 usec
uint32_t duty;                 // 0 to 15000
uint32_t time;                 // in 0.01 sec

void Collect(void) {
    GREENOUT ^= 1;
    if (!done0) period0 = 0xFFFF - 1;  // stopped
    if (!done1) period1 = 0xFFFF - 1;
    done0 = done1 = false; // set on subsequent
    switch (time) {
        case 0:
            duty = 1875; // 25%
            MotorForward(duty, duty);
            break;
        case 100:
            duty = 3751; // 50%
            MotorForward(duty, duty);
            break;
        case 200:
            duty = 5626; // 75%
            MotorForward(duty, duty);
            break;
        case 300:
            duty = 3751; // 50%
            MotorForward(duty, duty);
            break;
        case 400:
            duty = 1875; // 25%
            MotorForward(duty, duty);
            break;
        case 500:
            MotorStop();
            TIMER_A1->CTL &= ~0x30; // Halt timer A1
            break;
        default:
            speed_buf[time] = 2000000 / period0;
            period_buf[time] = period0;
            break;
    }
    ++time;
}

int main_s(void) {
    CPU_cpsid(); // Disable interrupts
    ClockInit48MHz();  // 48 MHz clock; 12 MHz Timer A clock
    RedLEDInit();
    ColorLEDInit();
    PWMInitMotor(3751);
    MotorInit();
    TimerA3CaptureInit(&PeriodMeasure0, &PeriodMeasure1);
    TimerA1Init(&Collect, 5000);  // 100 Hz
    CPU_cpsie(); // Enable interrupts
    CPU_wfi(); // Wait for interrupt
    return 0;
}

int main(void) {
    CPU_cpsid(); // Disable interrupts
    ClockInit48MHz();
    PWMInitMotor(3751);
    MotorInit();
    TachometerInit();
    // MotorForward(2000, 2000); // confirm if rotating forward
    //MotorBackward(2000, 2000); // confirm if rotating backward
    CPU_cpsie(); // Enable interrupts
    // CPU_wfi(); // Wait for interrupt
    while (1);
    return 0;
}