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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() {
  Wire.begin(8, 9);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();

  // Print out Welcome message
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor((SCREEN_WIDTH/2) - 38,(SCREEN_HEIGHT/2) - 10);
  display.println("Welcome");

  display.display();

  Serial.begin(115200);
  Serial.println("Serial comunication initialized");

  delay(1000);
  display.clearDisplay();

  // Initialize display values for gear showing
  display.setTextSize(6);
  display.setCursor((SCREEN_WIDTH/2) - 12,(SCREEN_HEIGHT/2) - 22);
}

void loop() {
  char gear = (char)Serial.read();
  
  switch (gear) {
    case 'N':
      display.println("N");
      display.setCursor((SCREEN_WIDTH/2) - 12,(SCREEN_HEIGHT/2) - 22);
      display.display();
    break;
    case '1':
      display.println("1");
      display.setCursor((SCREEN_WIDTH/2) - 12,(SCREEN_HEIGHT/2) - 22);
      display.display();
    break;
    case '2':
      display.println("2");
      display.setCursor((SCREEN_WIDTH/2) - 12,(SCREEN_HEIGHT/2) - 22);
      display.display();
    break;
    case '3':
      display.println("3");
      display.setCursor((SCREEN_WIDTH/2) - 12,(SCREEN_HEIGHT/2) - 22);
      display.display();
    break;
    case '4':
      display.println("4");
      display.setCursor((SCREEN_WIDTH/2) - 12,(SCREEN_HEIGHT/2) - 22);
      display.display();
    break;
    case '5':
      display.println("5");
      display.setCursor((SCREEN_WIDTH/2) - 12,(SCREEN_HEIGHT/2) - 22);
      display.display();
    break;
    case 'R':
      display.println("R");
      display.setCursor((SCREEN_WIDTH/2) - 12,(SCREEN_HEIGHT/2) - 22);
      display.display();
    break;
    default:
      display.println("N");
      display.setCursor((SCREEN_WIDTH/2) - 12,(SCREEN_HEIGHT/2) - 22);
      display.display();
    break;
  }

  delay(10); // this speeds up the simulation
}