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

#include "bump.h"
#include "systick.h"
#include "motor_simple.h"
#include "clock.h"

#include "inc/msp432p401r.h"

// bit-banded addresses, positive logic
#define SW2IN ((*((volatile uint8_t *)(0x42098010)))^1)
#define SW1IN ((*((volatile uint8_t *)(0x42098004)))^1)

#define SIZE 100

// sinusoidal duty plot table
const uint32_t pulse_us[SIZE] = {
    5000, 5308, 5614, 5918, 6219, 6514, 6804, 7086, 7361, 7626,
    7880, 8123, 8354, 8572, 8776, 8964, 9137, 9294, 9434, 9556,
    9660, 9746, 9813, 9861, 9890, 9900, 9890, 9861, 9813, 9746,
    9660, 9556, 9434, 9294, 9137, 8964, 8776, 8572, 8354, 8123,
    7880, 7626, 7361, 7086, 6804, 6514, 6219, 5918, 5614, 5308,
    5000, 4692, 4386, 4082, 3781, 3486, 3196, 2914, 2639, 2374,
    2120, 1877, 1646, 1428, 1224, 1036,  863,  706,  566,  444,
     340,  254,  187,  139,  110,  100,  110,  139,  187,  254,
     340,  444,  566,  706,  863, 1036, 1224, 1428, 1646, 1877,
    2120, 2374, 2639, 2914, 3196, 3486, 3781, 4082, 4386, 4692
};

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

void SwitchInit(void) {
    P1->SEL0 &= ~0x12;
    P1->SEL1 &= ~0x12;    // 1) configure P1.4 and P1.1 as GPIO
    P1->DIR &= ~0x12;     // 2) make P1.4 and P1.1 in
    P1->REN |= 0x12;      // 3) enable pull resistors on P1.4 and P1.1
    P1->OUT |= 0x12;      //    P1.4 and P1.1 are pull-up
}

void WaitTouchRelease(void) {
    while(!(SW1IN || SW2IN));  // wait for touch
    while(  SW1IN || SW2IN );  // wait for release
}

int main_f(void) {
    ClockInit48MHz();
    SysTickInit();
    RedLEDInit();
    uint32_t H =  5000;
    uint32_t L = 10000 - H;

    while (true) {
        P1->OUT |= 0x01;
        SysTickWait1us(H);
        P1->OUT &= ~0x01;
        SysTickWait1us(L);
    }
}

// The heartbeat starts when the operator pushes Button 1
// The heartbeat stops when the operator pushes Button 2
// When beating, the P1.0 LED oscillates at 100 Hz (too fast to see with the eye)
// and the duty cycle is varied sinuosoidally once a second
int main_s(void) { 
    uint32_t H;
    uint32_t L;
    uint32_t i = 0;
    ClockInit48MHz();
    SysTickInit();
    RedLEDInit();

    while (true) {
        H = pulse_us[i];
        L = 10000 - H;     // 2. calculate L 
        P1->OUT |= 0x01;   // 3. set P1.0 high
        SysTickWait1us(H); // 4. wait H us
        P1->OUT &= ~0x01;  // 5. clear P1.0 low
        SysTickWait1us(L); // 6. Wait L us
        i = (i + 1) % 100;
    }
}


void MotorTestPulse(void);
int main(void) {
    ClockInit48MHz();
    SysTickInit();
    SwitchInit();
    BumpInit();
    MotorInit(); // your function

    // MotorForward(1500, 100);
    // return 0;

    while (true) {
        WaitTouchRelease();
        MotorForward(1500, 100);  // your function
        WaitTouchRelease();
        MotorBackward(1500, 100); // your function
        WaitTouchRelease();
        MotorLeft(1500, 100);     // your function
        WaitTouchRelease();
        MotorRight(1500, 100);    // your function
    }
}