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

#include "timer_a1.h"
#include "pwm.h"
#include "motor.h"
#include "clock.h"
#include "cpu.h"
#include "bump.h"
#include "delay.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 REDLED (*((volatile uint8_t *)(0x42098040)))

void TimedWaitTouchRelease(uint32_t time) {
    Delay1ms(time); // run for a while and then stop
    MotorStop();
    while (!(SW1IN || SW2IN)) {}  // wait for touch
    while (SW1IN || SW2IN) {}     // wait for release
}

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

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 Task(void) {
    static uint32_t time;
    ++time;
    REDLED ^= 1;
}

int main_periodic_test(void) {
    ClockInit48MHz();
    RedLEDInit();
    TimerA1Init(&Task, 500);  // 1000 Hz
    CPU_cpsie(); // Enable interrupts
    while (true) {}
}

int main_motor_test(void) {
    ClockInit48MHz();
    SwitchInit();
    PWMInitMotor(7502);     // your function
    MotorInit();        // your function
    while (true) {
        TimedWaitTouchRelease(1000);
        MotorForward(1500, 1500);  // your function
        TimedWaitTouchRelease(2000);
        MotorBackward(1500, 1500); // your function
        TimedWaitTouchRelease(2000);
        MotorLeft(1200, 1200);     // your function
        TimedWaitTouchRelease(1000);
        MotorRight(1200, 1200);    // your function
    }
}

// Write a program that uses PWM to move the robot
// but uses TimerA1 to periodically check the bump switches, 
// stopping the robot on collision
int main(void) {
    ClockInit48MHz();
    // write code here
    CPU_cpsie(); // Enable interrupts
    while (true) {
        // write code here
    }
}