// Shifter display for 6spd manual transmission car using
// 3D printed shifter mount with microswitches.

// Rev 0.2 (30.06.24)
// Github - @karledmnd

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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

  //The following is a bitmap to display the E46 outline, change this to whatever you
  //want or remove it from the code all together
const unsigned char e46Logo [] PROGMEM = 
{
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x01, 0x80, 0x00, 0x01, 0xf0, 0x00, 0x0f, 0x3f, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x0c, 0x00, 0x00, 0x07, 0xc0, 0x00, 0x0e, 0x00, 0x07, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x20, 0x00, 0x00, 0x0f, 0x80, 0x00, 0x0e, 0x00, 0x03, 0x87, 0xf8, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x80, 0x00, 0x00, 0x3f, 0x00, 0x00, 0x0e, 0x00, 0x01, 0xc0, 0x0e, 0x00, 0x00, 0x00, 0x00, 
0x02, 0x00, 0x00, 0x00, 0x7f, 0x00, 0x00, 0x0e, 0x00, 0x00, 0xe0, 0x01, 0xc0, 0x00, 0x00, 0x00, 
0x0f, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x1e, 0x00, 0x00, 0xf0, 0x00, 0x70, 0x00, 0x00, 0x00, 
0x1f, 0xff, 0xff, 0xff, 0x3f, 0xff, 0xff, 0xff, 0x00, 0x00, 0x78, 0x00, 0x1e, 0x00, 0x00, 0x00, 
0x1f, 0xff, 0xff, 0xfe, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x03, 0x80, 0x00, 0x00, 
0x07, 0xff, 0xff, 0xc0, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x01, 0xc0, 0x00, 0x00, 
0x07, 0xff, 0xff, 0x80, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 
0x3f, 0xff, 0xff, 0x80, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 
0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 
0x40, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0x80, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 
0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x1c, 
0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x7f, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x0c, 
0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0xff, 0xc7, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc7, 0xc6, 
0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc1, 0xff, 0xc7, 0xff, 0xff, 0xff, 0xff, 0xff, 0x8f, 0xe6, 
0x1c, 0x00, 0x00, 0x3f, 0xff, 0xff, 0x81, 0xff, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xff, 0x9f, 0xe4, 
0x00, 0x3c, 0x00, 0x00, 0x0f, 0xff, 0x81, 0xff, 0xc7, 0xff, 0xff, 0xff, 0xff, 0xff, 0x1f, 0xec, 
0x00, 0x1f, 0x00, 0x0f, 0x00, 0x00, 0x00, 0xff, 0xc7, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x0f, 0xe8, 
0x00, 0x0f, 0x80, 0x3f, 0xff, 0xff, 0x00, 0x7f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xc0, 
0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x08, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x3c, 
0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 
0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 
0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

const int switchPins[] = {9, 12, 8, 11, 7, 10, 6};
const char* gears[] = {"N", "1", "2", "3", "4", "5", "6", "R"};
const int numSwitches = sizeof(switchPins) / sizeof(switchPins[0]);

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

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Loop forever
  }

  display.display();
  delay(2000);
  display.clearDisplay();

  for(int i = 0; i < numSwitches; i++) {
    pinMode(switchPins[i], INPUT_PULLUP); // Using N/O switches
  }

  // Initial message
  display.setTextColor(SSD1306_WHITE);
  display.drawBitmap(0, 0, e46Logo, 128, 64, WHITE); //Loads the E46 logo
  display.display();
  delay(2000);
  display.clearDisplay();
  display.setTextSize(4);
  display.setCursor(31, 17);
  display.print("BPC");//Just a startup text
  display.display();
  delay(1000);
}

void loop() {
  bool gearEngaged = false;

  for(int i = 0; i < numSwitches; i++) {
    if(digitalRead(switchPins[i]) == LOW) {
      updateDisplay(gears[i + 1]); // Check gear
      gearEngaged = true;
      delay(200);
      break;
    }
  }

  if (!gearEngaged) {
    updateDisplay(gears[0]); // Display "Neutral" if no switch is pressed
    delay(200);
  }
}

void updateDisplay(const char* gear) {
  display.clearDisplay();
  display.setTextSize(6);
  display.setCursor(50, 10);
  display.print(gear);
  display.display();
}