#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_I2CDevice.h>

Adafruit_SSD1306 display(128, 64, &Wire, -1, 3000000); // Define Display

// Backspace Char (6x7 Resolution, 7 Bytes in flash)
static const unsigned char PROGMEM backspaceChar[] = {
  0b00000000,
  0b00111100,
  0b01000100,
  0b10000100,
  0b01000100,
  0b00111100,
  0b00000000,
};

// Enter Char (6x7 Resolution, 7 Bytes in flash)
static const unsigned char PROGMEM enterChar[] = {
  0b00000000,
  0b00000100,
  0b00000100,
  0b01000100,
  0b11111100,
  0b01000000,
  0b00000000,
};

// Shift Char (6x7 Resolution, 7 Bytes in flash)
static const unsigned char PROGMEM shiftChar[] = {
  0b00000000,
  0b00110000,
  0b01001000,
  0b10000100,
  0b11001100,
  0b01001000,
  0b01111000,
};

// Shift Char Filled (6x7 Resolution, 7 Bytes in flash)
static const unsigned char PROGMEM shiftCharFilled[] = {
  0b00000000,
  0b00110000,
  0b01111000,
  0b11111100,
  0b11111100,
  0b01111000,
  0b01111000,
};

// Space Char (16x3 Resolution, 6 Bytes in flash)
static const unsigned char PROGMEM spaceChar[] = {
  0b10000000, 0b00000001,
  0b10000000, 0b00000001,
  0b11111111, 0b11111111,
};

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);        // Begin I2C Communication With Display

  display.clearDisplay();                           // Clear Display Buffer
  display.setTextSize(1);                           // Text size should be 1px
  display.setTextColor(SSD1306_INVERSE);            // Toggle pixels while drawing instead of setting to one specific color
  display.setCursor(0, 0);                          // Reset Cursor
  display.print("Booting...");
  display.display();                                // Display On Screen

  pinMode(4, INPUT);  // Up
  pinMode(18, INPUT); // Down
  pinMode(16, INPUT); // Left
  pinMode(17, INPUT); // Right
  pinMode(5, INPUT);  // Select

  Serial.begin(115200);
}

String str = "QWERTYUIOP ASDFGHJKL' ZXCVBNM,.- ";
const int columns = 11;
const int rows = 3; 
/*const */int space = 6;
/*const */int xOffset = 3;
/*const */int yOffset = 18;

uint8_t u, pu; // Up     (GPIO 4)
uint8_t d, pd; // Down   (GPIO 18)
uint8_t l, pl; // Left   (GPIO 16)
uint8_t r, pr; // Right  (GPIO 17)
uint8_t s, ps; // Select (GPIO 5)

int8_t selection = 0;
bool shift = true;
String output = "";

void loop() {
  display.clearDisplay();

  updateInput();

  if (l>pl) selection--;
  if (r>pr) selection++;
  if (u>pu) selection -= columns;
  if (d>pd) selection += columns;
  selection = constrain(selection, 0, columns * rows);
  
  if (s>ps) {
    if (selection%columns != columns - 1) output = output + str.charAt(selection);
    else {
      switch (selection / columns) {
        case 0:
          output.remove(output.length() - 1);
          break;
          
        case 1:
          // code for Enter key
          if (shift) output = output + "\n";
          break;
          
        case 2:
          shift = !shift;
          if (shift) str.toUpperCase();
          else str.toLowerCase();
          break;
      }
    }
  }
  
  for (int i = 0; i<rows; i++) {
    for (int j = 0; j<columns; j++) {
      uint8_t x = j*(5 + space) + xOffset;
      uint8_t y = i*(7 + space) + yOffset;

      if (j==10) {
        switch (i) {
          case 0:
            display.drawBitmap(x, y, backspaceChar, 6, 7, SSD1306_WHITE);
            break;
          case 1:
            display.drawBitmap(x, y, enterChar, 6, 7, SSD1306_WHITE);
            break;
          case 2:
            display.drawBitmap(x, y, shift? shiftCharFilled : shiftChar, 6, 7, SSD1306_WHITE);
            break;
        }
      } else display.drawChar(x, y, str.charAt(i*columns + j), SSD1306_WHITE, SSD1306_BLACK, 1);

      if (y == yOffset && x != xOffset) display.drawFastVLine(x - (space / 2), y - 1, 20 + (space*rows) - ((int)round((float)space / 2) - 2), SSD1306_WHITE);
      else if (x == xOffset) display.drawFastHLine(0, y + 7 + (space / 2), 128, SSD1306_WHITE);
    }
  }
  display.drawBitmap(56, ((61 - ((rows - 1)*(7 + space) + yOffset + 7 + (space / 2))) / 2), spaceChar, 16, 3, SSD1306_WHITE);
  display.drawRect(0, yOffset - 2, 128, 66 - yOffset, SSD1306_WHITE);

  if (selection < columns * rows) {
    display.fillRoundRect(
      (selection % columns)*(5 + space) + xOffset - (space / 2) + 1 - (selection%columns == 0? (int)-round(((float)space / 2) - 3.1) : 0),
      (selection / columns)*(7 + space) + yOffset - (selection<columns ? 1 : (int)round((float)space / 2) - 1),
      (selection%columns == columns - 1) ? 126 - ((columns - 1)*(5 + space) + xOffset - (space/2)) : (selection%columns == 0 ? (xOffset + space + 1 + (int)-round(((float)space / 2) - 3.1)) : 4+space),
      8 + (space / 2) + (selection<columns ? 0 : ((int)round((float)space / 2) - 2)),
      0,
      SSD1306_INVERSE
    );
  } else {
    display.fillRoundRect(
      (selection % columns)*(5 + space) + xOffset - (space / 2) + 1 - (int)-round(((float)space / 2) - 3.1),
      (selection / columns)*(7 + space) + yOffset - (int)round((float)space / 2) + 1,
      126,
      62 - ((rows - 1)*(7 + space) + yOffset + 7 + (space / 2)),
      0,
      SSD1306_INVERSE
    );
  }

  display.setCursor(0, 0);
  display.print(output);  // Fix this so it can't overwrite keyboard grapgics
  
  /*display.drawLine(0, 64, 128, 0, SSD1306_WHITE);
  display.drawLine(0, 0, 128, 64, SSD1306_WHITE);*/

  display.display();
}

void updateInput() {
  pu = u;
  pd = d;
  pl = l;
  pr = r;
  ps = s;

  u = digitalRead(4);
  d = digitalRead(18);
  l = digitalRead(16);
  r = digitalRead(17);
  s = digitalRead(5);

  if (Serial.available() > 0) {
    String recString = Serial.readStringUntil('\n');
    Serial.println(recString);
    int recInt = recString.substring(1).toInt();

    if (recString.startsWith("x")) xOffset = recInt;
    else if (recString.startsWith("y")) yOffset = recInt;
    else if (recString.startsWith("s")) space = recInt;

    Serial.println();
  }
}