#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;
    // write this code
}

// 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;
    // write this code
}

// 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) {
    // write this code
}