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

#include "../common/rtos.h"
#include "../common/pwm.h"
#include "../common/bump.h"
#include "../common/reflectance.h"
#include "../common/motor.h"
#include "../common/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 WaitTouchRelease(void) {
    while(!(SW1IN || SW2IN));  // wait for touch
    while(  SW1IN || SW2IN );  // wait for release
}

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 led_task(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 drive_task(void) {
    while (1) {
        bump_pressed = 0;
        MotorForward(1000, 1000);
        Wait(&bump_pressed);
        MotorBackward(800, 800);
        Sleep(1000);
        MotorLeft(800, 800);
        Sleep(2000);
    }
}

uint32_t refl_tick_count = 0;
void reflectance_task(void) {
    ++refl_tick_count;

    if (1 == (refl_tick_count % 10)) {
        ReflectanceStart();
    }

    if (2 == (refl_tick_count % 10)) {
        reflectance_data  = ReflectanceEnd();
    }
}

void bump_task(void) {
    bump_data = BumpRead();
    if ((~bump_data & BUMP_PINS)) {
        Signal(&bump_pressed);
    }
}

int main(void) {
    BumpInit();
    ReflectanceInit();
    PWMInitMotor(3751);
    MotorInit();

    RedLEDInit();
    ColorLEDInit();
    SwitchInit();
    RoundRobinInit();
    AddThreads((void (*[])(void)){
        led_task,
        drive_task
    });
    AddPeriodicEventThreads((PeriodicEvent_t []) {
        {reflectance_task, 1}, 
        {bump_task, 100}
    });
    RoundRobinLaunch(48000); // 1ms
    return 0;   // never executes
}