#include "tachometer.h"

#include "input_capture.h"

#include "inc/msp432p401r.h"

#define P5_0_IN (*((volatile uint8_t *)(0x42098800)))
#define P5_2_IN (*((volatile uint8_t *)(0x42098808)))

uint16_t tach_r_time1, tach_r_time2, tach_l_time1, tach_l_time2;

// incremented with every step forward, decremented with every step backward
int tach_r_steps, tach_l_steps;

TachDirection_t tach_r_dir, tach_l_dir;

// Right Encoder B connected to P5.0 (J2.13)
// When high, increase r_steps and set dir forward
// When low, decrease r_steps and set dir reverse
void TachometerRightInt(uint16_t current_time) {
    tach_r_time1 = tach_r_time2;
    tach_r_time2 = current_time;
    
    tach_r_dir = (P5->IN & 0x01) ? Forward : Reverse;

    // int delta = tach_r_time2 - tach_r_time1;
    // int speed = 2000000 / delta;

    if (tach_r_dir == Forward)
        ++tach_r_steps;
    else
        --tach_r_steps;
    return;
}

// Left Encoder B connected to P5.2 (J2.12)
// When high, increase l_steps and set dir forward
// When low, decrease l_steps and set dir reverse
void TachometerLeftInt(uint16_t current_time) {
    tach_l_time1 = tach_l_time2;
    tach_l_time2 = current_time;
    
    tach_l_dir = (P5->IN & 0x04) ? Forward : Reverse;

    // int delta = tach_l_time2 - tach_l_time1;
    // int speed = 2000000 / delta;

    if (tach_l_dir == Forward)
        ++tach_l_steps;
    else
        --tach_l_steps;
    return;
}

// Initialize P5.0 and P5.2 and make them GPIO inputs, 
// which will be used to determine the direction of rotation.
// Initialize the input capture interface, which
// will be used to measure the speed of rotation.
void TachometerInit(void) {
    P5->SEL0 |= 0x05;   // Set SEL0 for bits 0 and 2
    P5->SEL1 &= ~0x05;  // Clear SEL1 for bits 0 and 2
    P5->DIR &= ~0x05;   // Set as inputs

    tach_r_dir = tach_l_dir = Stopped;

    TimerA3CaptureInit(&TachometerRightInt, &TachometerLeftInt);
}