#include "reflectance.h"

#include "delay.h"

#include "inc/msp432p401r.h"

#define BIT_2 (uint8_t)0x4
#define BIT_3 (uint8_t)0x8

#define ALL_BITS (uint8_t)0xff
#define NONE_BITS (uint8_t)0x00

void ReflectanceInit(void) {
    P5->SEL0 &= ~BIT_3; // EVEN LEDs
    P5->SEL1 &= ~BIT_3; // configure P5.3 as GPIO
    P5->DIR  |=  BIT_3; // make P5.3 output
    P5->OUT  &= ~BIT_3; // set output low

    P9->SEL0 &= ~BIT_2; // ODD LEDs
    P9->SEL1 &= ~BIT_2; // configure P9.2 as GPIO
    P9->DIR  |=  BIT_2; // make P9.2 output
    P9->OUT  &= ~BIT_2; // set output low

    P7->SEL0 &= NONE_BITS;
    P7->SEL1 &= NONE_BITS;  // configure P7.0-7 as GPIO
    P7->DIR  &= NONE_BITS;  // make them input
}

// 0 -> while, 1 -> black
uint8_t ReflectanceRead(uint32_t time) {
    uint8_t input;

    // 1) Set P5.3 and P9.2 high (turn on IR LED)
    P5->OUT  |= BIT_3;
    P9->OUT  |= BIT_2;

    // 2) Make P7.7 – P7.0 outputs, and set them high (charging 8 capacitors)
    P7->DIR  = ALL_BITS;
    P7->OUT  = ALL_BITS;

    // 3) Wait 10 us, Delay1us(10);
    Delay1us(10);

    // 4) Make P7.7 – P7.0 input
    P7->DIR  = NONE_BITS;

    // 5) Wait time us, Delay1us(time);
    Delay1us(time);

    // 6) Read P7.7 – P7.0 inputs (converts voltages into binary)
    input = P7->IN;

    // 7) Set P5.3 and P9.2 low (turn off IR LED, saving power)
    P5->OUT  &= ~BIT_3;
    P9->OUT  &= ~BIT_2;

    P7->DIR  = NONE_BITS;
    P7->OUT  = NONE_BITS;

    // 8) Return 8-bit binary measured in step 6
    return input;
}

int32_t ReflectancePosition(uint8_t sensor_data) {
    const int32_t W[] = {334, 238, 142, 48, -48, -142, -238, -334};

    // Data falls into four categories:
    // 1) <all 0’s> (off the line or on white surface)
    if (!sensor_data) return 0; // off the line or on white surface

    // 2) <some 0’s, some 1’s>, e.g., 00000111 (off to the left)
    // 3) <some 0’s, some 1’s, some 0’s>, e.g., 00110000 (over the line)
    // 4) <some 1’s, some 0’s>, e.g., 11110000 (off to the right)
    int32_t i, up, down, b;
    for (up = down = i = 0; i < 8; ++i) {
        b = (sensor_data & (1 << i)) != 0;
        up += b * W[i];
        down += b;
    }

    return  up / down;
}

void ReflectanceStart(void) {
    // turn on 4 even IR LEDs
    // turn on 4 odd IR LEDs
    P5->OUT  |= BIT_3;
    P9->OUT  |= BIT_2;

    // make P7.7-P7.0 out
    P7->DIR  = ALL_BITS;

    // prime for measurement
    P7->OUT  = ALL_BITS;

    // wait 10 us
    Delay1us(10);

    // make P7.7-P7.0 in
    P7->DIR = NONE_BITS;
}

uint8_t ReflectanceEnd(void) {
    uint8_t input;
    // convert P7.0 input to digital
    input = P7->IN;

    // turn off 4 even IR LEDs
    // turn off 4 odd IR LEDs
    P5->OUT  &= ~BIT_3;
    P9->OUT  &= ~BIT_2;

    return input;
}