#include <Wire.h>

#define PIN_HEADING_WHITE 3
#define PIN_HEADING_RED 5

#define PIN_ALTITUDE_WHITE 2
#define PIN_ALTITUDE_RED 4

#define VALUE_BUFFER 30

#define SKIP_ROTARY_INPUTS 50

// needs global define
#define DATA_STOP_BYTE 0x00
#define HEADING_TRIGGER_BIT 1
#define HEADING_HIGH_BIT 2
#define ALTITUDE_TRIGGER_BIT 4
#define ALTITUDE_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 headingFalling() {
  falling(PIN_HEADING_RED, HEADING_TRIGGER_BIT, HEADING_HIGH_BIT);
}

void altitudeFalling() {
  falling(PIN_ALTITUDE_RED, ALTITUDE_TRIGGER_BIT, ALTITUDE_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);
  }
}

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

  // setup rotator GPIOs
  pinMode(PIN_HEADING_WHITE, INPUT_PULLUP);
  pinMode(PIN_HEADING_RED, INPUT_PULLUP);
  pinMode(PIN_ALTITUDE_WHITE, INPUT_PULLUP);
  pinMode(PIN_ALTITUDE_RED, 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_ALTITUDE_WHITE), altitudeFalling, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_HEADING_WHITE), headingFalling, FALLING);

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

void loop() {
  delay(10000);
}