#define halBlueLeft 2
#define halYellowLeft 22
#define halGreenLeft 19

#define halBlueRight 23
#define halYellowRight 18
#define halGreenRight 5

#define wheelCircumference 26.062

TaskHandle_t rpmTaskHandle;
int rpmRight = 0;
int rpmLeft = 0;

int lastState[2] = {1, 1};
long lastTime[2] = {0, 0};
int counter[2] = {0, 0};

long rpmTimer = 0;

// Zero states are non existant
int motorPosition[2][2][2] {
  {
    {0, 1},
    {3, 2}
  },
  {
    {5, 6},
    {4, 0}
  }
};
// old state, new state
int relativeLookup[6][6] {
  { 0, 1, 2, 3,-2,-1}, // 1
  {-1, 0, 1, 2, 3,-1}, // 2
  {-2,-1, 0, 1, 2, 3}, // 3
  {-3,-2,-1, 0, 1, 2}, // 4
  { 1,-3,-2,-1, 0, 1}, // 5
  { 1, 2,-3,-2,-1, 0}  // 6
};

int getState(bool isLeft) {
  if(isLeft) {
    return motorPosition[digitalRead(halBlueLeft)][digitalRead(halYellowLeft)][digitalRead(halGreenLeft)];
  } else {
    return motorPosition[digitalRead(halBlueRight)][digitalRead(halYellowRight)][digitalRead(halGreenRight)];
  }
}

void initRPMPins() {
  pinMode(halBlueLeft, INPUT_PULLUP);
  pinMode(halYellowLeft, INPUT_PULLUP);
  pinMode(halGreenLeft, INPUT_PULLUP);

  pinMode(halBlueRight, INPUT_PULLUP);
  pinMode(halYellowRight, INPUT_PULLUP);
  pinMode(halGreenRight, INPUT_PULLUP);
}

void measureRpm(void * parameter) {
  while(true) {
    if(millis()-rpmTimer>250) {
      rpmRight = constrain(counter[0], 0, 350)*4;
      rpmLeft = constrain(counter[1], 0, 350)*4;
      kmh = (wheelCircumference/100) * (rpmRight+rpmLeft)/2 * 60/1000;

      sendSpeed = abs(floor(kmh));
      sendSpeedDecimals = (kmh - sendSpeed) * 100;
      Serial.print("remote "); Serial.println(kmh);
      counter[true] = 0;
      counter[false] = 0;
      rpmTimer = millis();
    }

    int state = getState(0);
    if(lastState[0] != state) {
      counter[0] = counter[0] + relativeLookup[lastState[0]-1][state-1];
      lastState[0] = state;
    }

    state = getState(1);
    if(lastState[1] != state) {
      counter[1] = counter[1] + relativeLookup[lastState[1]-1][state-1];
      lastState[1] = state;
    }

    vTaskDelay(pdMS_TO_TICKS(1));
  }
}