#include <Wire.h>

#define PIN_ROTATOR1_WHITE 2
#define PIN_ROTATOR1_GREEN 4

#define PIN_ROTATOR2_WHITE 3
#define PIN_ROTATOR2_GREEN 5

#define VALUE_BUFFER 30

#define SKIP_ROTARY_INPUTS 50

// needs global define
#define DATA_STOP_BYTE 0x00
#define DATA_RESET_BYTE 0xFF
#define ROTATOR1_TRIGGER_BIT 1
#define ROTATOR1_HIGH_BIT 2
#define ROTATOR2_TRIGGER_BIT 4
#define ROTATOR2_HIGH_BIT 8
#define PIN_ADDRESS_1 11
#define PIN_ADDRESS_2 12
#define PIN_ADDRESS_3 13

int lastClk = HIGH;

// ringbuffer of trigger and direction values
byte valueBuffer[VALUE_BUFFER] = { 0 };
uint8_t readerPos, writerPos = 0;

void addValue(uint8_t value) {
  valueBuffer[writerPos++] = value;
  if (writerPos >= VALUE_BUFFER) {
    writerPos = 0;
  }  
}

void falling(uint8_t pin, byte triggerBit, byte highBit) {
  uint8_t dt = digitalRead(pin);
  byte value = triggerBit;

  // read direction of pin
  if (dt) {
    value |= highBit;
  } else {
    // value is already "lowBit"
  }

  if (useInput(value)) {
    addValue(value);
  }
}

void rotator1Falling() {
  falling(PIN_ROTATOR1_GREEN, ROTATOR1_TRIGGER_BIT, ROTATOR1_HIGH_BIT);
}

void rotator2Falling() {
  falling(PIN_ROTATOR2_GREEN, ROTATOR2_TRIGGER_BIT, ROTATOR2_HIGH_BIT);
}

int eventCount = 0;
byte lastValue = 0;
bool useInput(byte value) {
  if (lastValue == value) {
    // same event as last event
    // check if already SKIP_ROTARY_INPUTS happend
    if (eventCount > SKIP_ROTARY_INPUTS) {
      eventCount = 0;
      return true;
    } else {
      eventCount++;
      return false;
    }
  } else {
    // not same event as last event
    // reset counter
    lastValue = value;
    eventCount = 0;
    return false;
  }  
}


void i2cRequest() {
  // if write is ahead, send data
  if (writerPos != readerPos) {
    byte value = valueBuffer[readerPos++];
 
    if (readerPos >= VALUE_BUFFER) {
      readerPos = 0;
    }

    Wire.write(value);
  } else {
    Wire.write(DATA_STOP_BYTE);
  }
}

void i2cReceive(int bytes) {
  for (int i = 0; i < bytes && Wire.available(); i++) {
    byte data = Wire.read();
    if (data == DATA_RESET_BYTE) {
      writerPos = readerPos = 0;
    } else {
      // nothing else yet implemented
    }
  }
}

uint8_t address = 0;
void setup() {
  Serial.begin(115200);

  // setup rotator GPIOs
  pinMode(PIN_ROTATOR1_WHITE, INPUT_PULLUP);
  pinMode(PIN_ROTATOR1_GREEN, INPUT_PULLUP);
  pinMode(PIN_ROTATOR2_WHITE, INPUT_PULLUP);
  pinMode(PIN_ROTATOR2_GREEN, INPUT_PULLUP);
  
  // setup address selector GPIOs
  pinMode(PIN_ADDRESS_1, INPUT_PULLUP);
  pinMode(PIN_ADDRESS_2, INPUT_PULLUP);
  pinMode(PIN_ADDRESS_3, INPUT_PULLUP);

  // calculate address by LOW GPIOs
  address = digitalRead(PIN_ADDRESS_1) == LOW;
  address |= (digitalRead(PIN_ADDRESS_2) == LOW) << 1;
  address |= (digitalRead(PIN_ADDRESS_3) == LOW) << 2;

  attachInterrupt(digitalPinToInterrupt(PIN_ROTATOR1_WHITE), rotator1Falling, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_ROTATOR2_WHITE), rotator2Falling, FALLING);

  Wire.begin(address);
  Wire.onRequest(i2cRequest);
  Wire.onReceive(i2cReceive);
}

void loop() {
  delay(10000);
}