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

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

#define FORWARD 0
#define BACKWARD 1

#define wheelCircumference 26.062
#define pulsesPerRotation 22

#include "driver/pcnt.h"

TaskHandle_t rpmTaskHandle;
float rpmRight = 0;
float rpmLeft = 0;

int lastState[2] = {1, 1};
int16_t counter[2] = {0, 0};
int direction[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
};

#define PCNT_UNIT_LEFT PCNT_UNIT_0
#define PCNT_UNIT_RIGHT PCNT_UNIT_1


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);

  // init counter
  pcnt_config_t pcnt_config_left = {
    .pulse_gpio_num = halBlueLeft,
    .ctrl_gpio_num = PCNT_PIN_NOT_USED,
    .lctrl_mode = PCNT_MODE_KEEP,
    .hctrl_mode = PCNT_MODE_KEEP,
    .pos_mode = PCNT_COUNT_INC,  // count both rising and falling edges
    .neg_mode = PCNT_COUNT_INC,
    .counter_h_lim = 32767,
    .counter_l_lim = -1,
    .unit = PCNT_UNIT_LEFT,
    .channel = PCNT_CHANNEL_0
  };

  pcnt_config_t pcnt_config_right = {
    .pulse_gpio_num = halBlueRight,
    .ctrl_gpio_num = PCNT_PIN_NOT_USED,
    .lctrl_mode = PCNT_MODE_KEEP,
    .hctrl_mode = PCNT_MODE_KEEP,
    .pos_mode = PCNT_COUNT_INC,  // count both rising and falling edges
    .neg_mode = PCNT_COUNT_INC,
    .counter_h_lim = 32767,
    .counter_l_lim = -1,
    .unit = PCNT_UNIT_RIGHT,
    .channel = PCNT_CHANNEL_0
  };

  pcnt_unit_config(&pcnt_config_left);
  pcnt_unit_config(&pcnt_config_right);


  pcnt_counter_pause(PCNT_UNIT_LEFT);
  pcnt_counter_pause(PCNT_UNIT_RIGHT);

  pcnt_counter_clear(PCNT_UNIT_LEFT);
  pcnt_counter_clear(PCNT_UNIT_RIGHT);

  pcnt_counter_resume(PCNT_UNIT_LEFT);
  pcnt_counter_resume(PCNT_UNIT_RIGHT);

}

void measureRpm(void * parameter) {

  while(true) {
    if(millis()-rpmTimer>250) {
      // read counters
      pcnt_get_counter_value(PCNT_UNIT_LEFT, &counter[1]);
      pcnt_get_counter_value(PCNT_UNIT_RIGHT, &counter[0]);

      pcnt_counter_clear(PCNT_UNIT_LEFT);
      pcnt_counter_clear(PCNT_UNIT_RIGHT);


      rpmRight = (counter[0]/pulsesPerRotation)*4;
      rpmLeft = (counter[1]/pulsesPerRotation)*4;
      kmh = (wheelCircumference/100) * (rpmRight+rpmLeft)/2 * 3600/1000;
      //Serial.print(kmh); Serial.print(" "); Serial.println(direction[0]);
      sendSpeed = abs(floor(kmh));
      sendSpeedDecimals = (kmh - sendSpeed) * 100;
      counter[true] = 0;
      counter[false] = 0;
      rpmTimer = millis();
    }

    int state = getState(0);
    if(lastState[0] != state) {
      if(relativeLookup[lastState[0]-1][state-1] > 0) {
        direction[0] = FORWARD;
      } else {
        direction[0] = BACKWARD;
      }
      lastState[0] = state;
    }

    state = getState(1);
    if(lastState[1] != state) {
      if(relativeLookup[lastState[1]-1][state-1] > 0) {
        direction[1] = FORWARD;
      } else {
        direction[1] = BACKWARD;
      }

      lastState[1] = state;
    }

    vTaskDelay(1 / portTICK_PERIOD_MS);
  }
}