/* interrupt routine for Rotary Encoders
   idea from: https://github.com/marcmerlin/IoTuz/blob/95f53be6569c68f7fbef491bb36082870f82d629/IoTuz.cpp

*/

#include "rotary.h"

#include "freertos/task.h"

//using namespace std;

Rotary* Rotary::instance = NULL;

Rotary::Rotary() {
  instance = this;
}

bool Rotary::begin(uint8_t pinA, uint8_t pinB, uint8_t pinButton) {
  this->pinA = pinA;
  this->pinB = pinB;
  this->pinButton = pinButton;
  pinMode(pinA, INPUT_PULLUP);
  pinMode(pinB, INPUT_PULLUP);
  pinMode(pinButton, INPUT_PULLUP);

  xTaskCreate(
                    &cTaskWrapper,          /* Task function. */
                    "encoderTask",        /* String with name of task. */
                    1024,            /* Stack size in words. */
                    this,             /* Parameter passed as input of the task */
                    tskIDLE_PRIORITY+2,                /* Priority of the task. */
                    &taskHandle);            /* Task handle. */

  attachInterrupt(digitalPinToInterrupt(pinA), doEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(pinB), doEncoder, CHANGE);

  return true;
}

void Rotary::cTaskWrapper(void* parameters) {
  static_cast<Rotary*>(parameters)->task(NULL);
}

void Rotary::task(void *pvParameters) {
  xTaskToNotify = xTaskGetCurrentTaskHandle();
  uint32_t ulNotificationValue;
  while(true) {
    ulNotificationValue = ulTaskNotifyTake( pdTRUE, portMAX_DELAY );
    delay(2);  // wait until bounce settled

    static uint8_t old_AB = 0;
    // grey code
    // http://hades.mech.northwestern.edu/index.php/Rotary_Encoder
    // also read up on 'Understanding Quadrature Encoded Signals'
    // https://www.pjrc.com/teensy/td_libs_Encoder.html
    // another interesting lib: https://github.com/0xPIT/encoder/blob/arduino/ClickEncoder.cpp
    static int8_t enc_states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};

    old_AB <<= 2;
    old_AB |= ((digitalRead(pinB))?(1<<1):0) | ((digitalRead(pinA))?(1<<0):0);
    encoderPos += ( enc_states[( old_AB & 0x0f )]);

    ulNotificationValue = ulTaskNotifyTake( pdTRUE, 0 ); // clear pending notifications

    debouncePulses++;
    if (debouncePulses > 3) { // update every 4 pulses
      debouncePulses = 0;
      if (encoderPos > encoderPosOld+1) value++;  // if the value has at least changed for 2
      else if (encoderPos < encoderPosOld-1) value--;
      else continue;   // otherwise skip
      encoderPosOld = encoderPos;
      if (callback) callback(value);
    }

  }
  vTaskDelete(NULL);
}

bool Rotary::registerCallback(std::function<void(int)> callback) {
  this->callback = callback;
  return true;
}

// Interrupt on changing state
void Rotary::doEncoder() {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    configASSERT( instance->xTaskToNotify != NULL );
    vTaskNotifyGiveFromISR( instance->xTaskToNotify, &xHigherPriorityTaskWoken );
    portYIELD_FROM_ISR(); //portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}