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

#include "rtos.h"
#include "pwm.h"
#include "bump.h"
#include "reflectance.h"
#include "motor.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)

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

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

#define START_REFLECTANCE 9

int32_t bump_pressed; // semaphore

uint8_t bump_data, reflectance_data;

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

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
}

void Task1(void) {
    while (true) {
        if (SW1IN && SW2IN) {
            GREENOUT = 1;
        } else {
            GREENOUT = 0;
        }
        if (SW1IN || SW2IN) {
            REDLED = 1;
        } else {
            REDLED = 0;
        }
        if (SW1IN) {
            BLUEOUT = 1;
        } else {
            BLUEOUT = 0;
        }
        if (SW2IN) {
            REDOUT = 1;
        } else {
            REDOUT = 0;
        }
    }
}

void Task2(void) {
    while (true) {
        // Motor goes forward and waits until bump is pressed
        // When pressed, go backward for some time and turn left for some time
    }
}

void Task3(void) {
    // Read reflectance similar to Lab 3
}

void Task4(void) {
    // Read bump and signal if any pressed
}

int main(void) {
    BumpInit();
    ReflectanceInit();
    PWMInitMotor(?);
    MotorInit();
    RedLEDInit();
    ColorLEDInit();
    SwitchInit();
    RoundRobinInit();
    AddThreads((void (*[])(void)){Task1, Task2});
    AddPeriodicEventThreads((PeriodicEvent_t []){{Task3, 1}, {Task4, 100}});
    RoundRobinLaunch(48000); // 1ms
    return 0;   // never executes
}