#include <Wire.h>

#define PIN_HEADING_WHITE PD3
#define PIN_HEADING_RED PD6

#define PIN_ALTITUDE_WHITE PD2
#define PIN_ALTITUDE_RED PD5

#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

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);
  }
}

void setup() {
  Serial.begin(115200);

  pinMode(PIN_HEADING_WHITE, INPUT_PULLUP);
  pinMode(PIN_HEADING_RED, INPUT_PULLUP);
  pinMode(PIN_ALTITUDE_WHITE, INPUT_PULLUP);
  pinMode(PIN_ALTITUDE_RED, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(PIN_ALTITUDE_WHITE), altitudeFalling, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_HEADING_WHITE), headingFalling, FALLING);

  Wire.begin(0x01);
  Wire.onRequest(i2cRequest);
}

void loop() {
  delay(10000);
}