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

*/

#include "rotary.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( pdFALSE, portMAX_DELAY );
    int newPos = encoderPos/2;
    if (lastPos != newPos)
      if (callback) callback(newPos);
  }
  vTaskDelete(NULL);
}

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

// Interrupt on changing state
void Rotary::doEncoder() {
  if ((millis() - instance->debounceMillis) > instance->debounceDelay) {
    instance->debounceMillis = millis();

    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    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(instance->pinB))?(1<<1):0) | ((digitalRead(instance->pinA))?(1<<0):0);
    instance->encoderPos += ( enc_states[( old_AB & 0x0f )]);

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