#include <Wire.h>

#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Fonts/FreeMonoBold24pt7b.h>
#include <Fonts/FreeMonoBold18pt7b.h>
#include <Fonts/FreeMonoBold12pt7b.h>
#include <Adafruit_SSD1306.h>

#include "global.h"

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#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

#define INTERNAL_STATE_UNKNOWN 0
#define INTERNAL_STATE_INITIALIZED 1

#define PIN_ADDRESS_1 14
#define PIN_ADDRESS_2 15
#define PIN_ADDRESS_3 16

#define PIN_DISPLAY1_DC 8
#define PIN_DISPLAY1_RST 9
#define PIN_DISPLAY1_CS 10
#define PIN_DISPLAY2_DC 6
#define PIN_DISPLAY2_RST 7
#define PIN_DISPLAY2_CS 17

Adafruit_SSD1306 display1(SCREEN_WIDTH, SCREEN_HEIGHT,
  &SPI, PIN_DISPLAY1_DC, PIN_DISPLAY1_RST, PIN_DISPLAY1_CS);
Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT,
  &SPI, PIN_DISPLAY2_DC, PIN_DISPLAY2_RST, PIN_DISPLAY2_CS);

char name1[] = {'X', 'X', 'X'};
char name2[] = {'Y', 'Y', 'Y'};
int internalState = INTERNAL_STATE_UNKNOWN;

uint8_t lastClk = HIGH;

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

// ringbuffer of display data
struct {
  
}

void addValue(uint8_t value) {
  valueBuffer[writerPos++] = value;

  // check for writer position overflow
  if (writerPos >= VALUE_BUFFER) {
    writerPos = 0;
  }

  // check if writer overtooks reader
  // if so, increment reader and check
  // for reader position overflow
  if (writerPos == readerPos) {
    readerPos++;
    if (readerPos >= VALUE_BUFFER) {
      readerPos = 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, TRIGGER_BIT_1, HIGH_BIT_1);
}

void rotator2Falling() {
  falling(PIN_ROTATOR2_GREEN, TRIGGER_BIT_2, HIGH_BIT_2);
}

int eventCount = 0;
byte lastValue = 0;
// this method blocks the parallel use of rotators
// if parallel use should be possible
// lastValue and eventCount must be stored per rotator
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 (internalState == INTERNAL_STATE_UNKNOWN) {
    // looks like we were restarted
    // request initialization first
    Serial.println("Requesting init");
    Wire.write(DATA_RESET_BYTE);
    internalState = INTERNAL_STATE_INITIALIZED;
  } else {
    // 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) {
  while (Wire.available()) {
    byte data = Wire.read();
    if (data == DATA_RESET_BYTE) {
      Serial.println("Received reset request");
      writerPos = readerPos = 0;
    } else if (data == DATA_INIT_BYTE_1) {
      Serial.println("Received init 1");
      // next three bytes are the name of the first rotator
      for (int i = 0; i < 3 && Wire.available(); i++) {
        name1[i] = Wire.read();
      }
    } else if (data == DATA_INIT_BYTE_2) {
      Serial.println("Received init 2");
      // next three bytes are the name of the second rotator
      for (int i = 0; i < 3 && Wire.available(); i++) {
        name2[i] = Wire.read();
      }
    } else if (data == DATA_BYTE) {
      // next byte will be trigger bit
      byte triggerBit = Wire.read();
      
      // next byte will be length of data
      byte dataLength = Wire.read();
            
      // next byte(s) will be the data itself
      char dataBytes[dataLength];
      for (int i = 0; i < dataLength && Wire.available(); i++) {
        dataBytes[i] = Wire.read();
      }

      if (triggerBit & TRIGGER_BIT_1) {
        updateDisplay1(dataBytes, dataLength);
      } else if (triggerBit & TRIGGER_BIT_2) {
        updateDisplay2(dataBytes, dataLength);
      } else {
        Serial.println("Unknown trigger bit");
      }
    } else {
      // not yet implemented?
      Serial.println("Received unknown");
    }
  }
}

void displayTestScreen(Adafruit_SSD1306 display) {
  display.clearDisplay();
  for (int x = 0; x < SCREEN_WIDTH; x++) {
    for (int y = 0; y < SCREEN_HEIGHT; y++) {
      display.drawPixel(x, y, SSD1306_WHITE);
    }
  }
  display.display();
}

void updateDisplay1(char *data, byte len) {
  updateDisplay(&display1, name1, data, len);
}

void updateDisplay2(char *data, byte len) {
  updateDisplay(&display2, name2, data, len);
}

void updateDisplay(Adafruit_SSD1306 *display, char *name, char *data, byte len) {
  display->clearDisplay();
  display->setTextColor(SSD1306_WHITE); // Draw white text
  display->cp437(true);         // Use full 256 char 'Code Page 437' font

  display->setTextSize(1);      // Normal 1:1 pixel scale
  display->setCursor(0, 8);     // Start at top-left corner
  display->setFont(NULL);
  display->write(name[0]);
  display->write(name[1]);
  display->write(name[2]);
  
  display->setTextSize(2);      // Normal 1:1 pixel scale
  display->setCursor(0, 56);     // Start at top-left corner
  display->setFont(&FreeMonoBold18pt7b);
  for (byte i = 0; i < len; i++) {
    display->write(data[i]);
  }
  display->display();
}

uint8_t address = I2C_ADDRESS_START;
void setup() {
  // https://support.arduino.cc/hc/en-us/articles/4839084114460-If-your-board-runs-the-sketch-twice
  delay(150);
  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) << 1;
  address |= (digitalRead(PIN_ADDRESS_2) == LOW) << 2;
  address |= (digitalRead(PIN_ADDRESS_3) == LOW) << 3;

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

  Wire.begin(address);
  Wire.onRequest(i2cRequest);
  Wire.onReceive(i2cReceive);
  
  if(!display1.begin(SSD1306_SWITCHCAPVCC)) {
    Serial.println(F("SSD1306 allocation failed (1)"));
    for(;;); // Don't proceed, loop forever
  }
  displayTestScreen(display1);
  
  if(!display2.begin(SSD1306_SWITCHCAPVCC)) {
    Serial.println(F("SSD1306 allocation failed (2)"));
    for(;;); // Don't proceed, loop forever
  }
  displayTestScreen(display2);
  // make sure, test screen is at least displayed 2 seconds
  delay(2000);

  updateDisplay1("---", 3);
  updateDisplay2("---", 3);
}

void loop() {
  delay(1000);
}