#define PIN_HEADING_CLK D7
#define PIN_HEADING_DT D5
#define PIN_HEADING_SW D6

#define PIN_ALTITUDE_CLK D1
#define PIN_ALTITUDE_DT D2
#define PIN_ALTITUDE_SW D3

#define PIN_LED D4

#define VALUE_BUFFER 30

#define HEADING_DT_TRIGGER 1
#define HEADING_DT_HIGH 2
#define HEADING_SW_TRIGGER 4

#define ALTITUDE_DT_TRIGGER 8
#define ALTITUDE_DT_HIGH 16
#define ALTITUDE_SW_TRIGGER 32

#define MIN_DT_INPUT_DELAY 1000    // 1ms
#define MIN_SW_INPUT_DELAY 500000 // 500ms

#define MIN_ALTITUDE 1
#define MAX_ALTITUDE 60000

#define MIN_HEADING 1
#define MAX_HEADING 360

uint8_t lastClk = HIGH;
uint16_t heading = 360;
uint16_t altitude = 0;

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

long lastInput = 0;

// prevent bouncing input
bool useInput(long minInputDelay) {
  long now = micros();
  long diff = now - lastInput;

  // < 0 because of long overruns
  bool ret = diff < 0 || diff > minInputDelay;
  if (ret) {
    lastInput = now;
  }

  return ret;
}

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

void clkFalling(uint8_t PIN_DT, uint8_t DT_TRIGGER, uint8_t DT_HIGH) {
  if (useInput(MIN_DT_INPUT_DELAY)) {
    uint8_t dt = digitalRead(PIN_DT);
    uint8_t value = DT_TRIGGER;
    if (dt) {
      value |= DT_HIGH;
    } else {
      // value is already "DT_LOW"
    }

    addValue(value);
  }
}

void swFalling(uint8_t SW_TRIGGER) {
  if (useInput(MIN_SW_INPUT_DELAY)) {
    addValue(SW_TRIGGER);
  }
}

void ICACHE_RAM_ATTR headingClkFalling();
void headingClkFalling() {
  clkFalling(PIN_HEADING_DT, HEADING_DT_TRIGGER, HEADING_DT_HIGH);
}

void ICACHE_RAM_ATTR altitudeClkFalling();
void altitudeClkFalling() {
  clkFalling(PIN_ALTITUDE_DT, ALTITUDE_DT_TRIGGER, ALTITUDE_DT_HIGH);
}

void ICACHE_RAM_ATTR headingSwFalling();
void headingSwFalling() {
  swFalling(HEADING_SW_TRIGGER);
}

void ICACHE_RAM_ATTR altitudeSwFalling();
void altitudeSwFalling() {
  swFalling(ALTITUDE_SW_TRIGGER);
}

void dtTriggered(uint16_t &cnt, uint8_t high, uint8_t fast, uint16_t miV, uint16_t miVR, uint16_t maV, uint16_t mavR) {
  if (high) {
    cnt++;
  } else {
    cnt--;
  }

  if (cnt > maV) {
    cnt = mavR;
  }

  if (cnt < miV) {
    cnt = miVR;
  }
}

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  pinMode(PIN_HEADING_SW, INPUT_PULLUP);
  pinMode(PIN_HEADING_DT, INPUT_PULLUP);
  pinMode(PIN_HEADING_CLK, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(PIN_HEADING_CLK), headingClkFalling, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_HEADING_SW), headingSwFalling, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_ALTITUDE_CLK), altitudeClkFalling, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_ALTITUDE_SW), altitudeSwFalling, FALLING);
}

void loop() {
  while (writerPos != readerPos) {
    uint8_t value = valueBuffer[readerPos++];

    if (readerPos >= VALUE_BUFFER) {
      readerPos = 0;
    }

    if (value & HEADING_DT_TRIGGER) {
      uint8_t dtHigh = value & HEADING_DT_HIGH;
      dtTriggered(heading, dtHigh, 5, MIN_HEADING, MAX_HEADING, MAX_HEADING, MIN_HEADING);
      Serial.print("Heading: ");
      Serial.println(heading);
    } else if (value & HEADING_SW_TRIGGER) {
      Serial.println("Heading: pushed");
    } else if (value & ALTITUDE_DT_TRIGGER) {
      uint8_t dtHigh = value & ALTITUDE_DT_HIGH;
      dtTriggered(altitude, dtHigh, 5, MIN_ALTITUDE, MIN_ALTITUDE, MAX_ALTITUDE, MAX_ALTITUDE);
      Serial.print("Altitude: ");
      Serial.println(altitude);
    } else if (value & ALTITUDE_SW_TRIGGER) {
      Serial.println("Altitude: pushed");
    } else {
      // never happen error...
    }
  }
}