/* interrupt routine for Rotary Encoders
   tested with Noble RE0124PVB 17.7FINB-24 http://www.nobleusa.com/pdf/xre.pdf - available at pollin.de
   and a few others, seems pretty universal

   The average rotary encoder has three pins, seen from front: A C B
   Clockwise rotation A(on)->B(on)->A(off)->B(off)
   CounterCW rotation B(on)->A(on)->B(off)->A(off)

   and may be a push switch with another two pins, pulled low at pin 8 in this case
   raf@synapps.de 20120107

*/

#include "rotary.h"

//using namespace std;

TaskHandle_t Rotary::xTaskToNotify = NULL;
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. */
                    "buttonTask",        /* String with name of task. */
                    2048,            /* Stack size in words. */
                    this,             /* Parameter passed as input of the task */
                    tskIDLE_PRIORITY+2,                /* Priority of the task. */
                    &buttonTaskHandle);            /* Task handle. */
  return true;
}

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

void Rotary::button_task(void *pvParameters) {
  xTaskToNotify = xTaskGetCurrentTaskHandle();

  attachInterrupt(digitalPinToInterrupt(pinA), doEncoderA, RISING);
  attachInterrupt(digitalPinToInterrupt(pinB), doEncoderB, RISING);
//  attachInterrupt(digitalPinToInterrupt(pinButton), doButton, FALLING);

  uint32_t ulNotificationValue;
  uint32_t ulStatusValue;

  while(true) {
    rotating = true;  // reset the debouncer

    ulNotificationValue = xTaskNotifyWait(0x00, ULONG_MAX, &ulStatusValue, portMAX_DELAY );

//    if ( rotating ) delay (1);  // wait a little until the bouncing is done

    if( ulStatusValue == 1 ) {
        // adjust counter + if A leads B
          encoderPos += 1;
          changed();
        rotating = false;  // no more debouncing until loop() hits again
    } else if ( ulStatusValue == 2 ) {
        // adjust counter -1 if B leads A
          encoderPos -= 1;
          changed();
        rotating = false;  // no more debouncing until loop() hits again
    } else if ( ulStatusValue == 4 ) {
        Button_set = digitalRead(pinButton);

        if (! Button_set ) {
          encoderPos = 0;
        }
    }

  }
}

void Rotary::changed() {
  if (lastReportedPos != encoderPos) {
    if (callback) callback(encoderPos);
    lastReportedPos = encoderPos;
  }
}

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

void Rotary::doButton() {
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;

  configASSERT( xTaskToNotify != NULL );
  uint32_t intrBits = 4;
  xTaskNotifyFromISR( xTaskToNotify, intrBits, eSetValueWithoutOverwrite, &xHigherPriorityTaskWoken );
  portYIELD_FROM_ISR(); //portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}

// Interrupt on A changing state
void Rotary::doEncoderA() {
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  if (instance->skipA) {
    instance->skipA = false;
    instance->debounceA = millis();
    return;
  }
  if (!digitalRead(instance->pinB) && ((millis() - instance->debounceA) > instance->debounceDelay)) {
    instance->debounceA = millis();
    instance->skipB = true;
    configASSERT( xTaskToNotify != NULL );
    uint32_t intrBits = 1;
    xTaskNotifyFromISR( xTaskToNotify, intrBits, eSetValueWithoutOverwrite, &xHigherPriorityTaskWoken );
    portYIELD_FROM_ISR(); //portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
  }
}

// Interrupt on B changing state, same as A above
void Rotary::doEncoderB() {
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  if (instance->skipB) {
    instance->skipB = false;
    instance->debounceB = millis();
    return;
  }
  if (!digitalRead(instance->pinA) && ((millis() - instance->debounceB) > instance->debounceDelay)) {
    instance->debounceB = millis();
    instance->skipA = true;
    configASSERT( xTaskToNotify != NULL );
    uint32_t intrBits = 2;
    xTaskNotifyFromISR( xTaskToNotify, intrBits, eSetValueWithoutOverwrite, &xHigherPriorityTaskWoken );
    portYIELD_FROM_ISR(); //portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
  }
}