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


#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

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

// "UIC Logo", 128x64px
static const unsigned char PROGMEM logo_bmp [] =
{
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 
0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 
0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 
0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 
0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 
0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 
0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 
0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 
0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 
0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 
0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 
0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 
0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 
0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 
0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x01, 0xff, 0xff, 0xf8, 
0x1f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xff, 0xff, 0x00, 0x00, 0x07, 0xff, 0xf8, 
0x3f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xff, 0xf8, 0x00, 0x00, 0x00, 0xff, 0xfc, 
0x3f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x7f, 0xfc, 
0x3f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xff, 0x80, 0x00, 0x00, 0x00, 0x1f, 0xfc, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xff, 0x00, 0x01, 0xf8, 0x00, 0x0f, 0xfe, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xfe, 0x00, 0x07, 0xff, 0x00, 0x3f, 0xfe, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xfe, 0x00, 0x1f, 0xff, 0x87, 0xff, 0xfe, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xfc, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xfe, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xfc, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xfe, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xfc, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xfe, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xfc, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xfe, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xfc, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xfe, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xfc, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xfe, 
0x7f, 0xfc, 0x00, 0x1f, 0xfe, 0x00, 0x0f, 0xe0, 0x00, 0xfe, 0x00, 0x0f, 0xff, 0x9f, 0xff, 0xfe, 
0x3f, 0xfe, 0x00, 0x0f, 0xfc, 0x00, 0x1f, 0xe0, 0x00, 0xfe, 0x00, 0x07, 0xff, 0x00, 0x7f, 0xfc, 
0x3f, 0xfe, 0x00, 0x03, 0xf0, 0x00, 0x1f, 0xe0, 0x00, 0xff, 0x00, 0x00, 0xf8, 0x00, 0x0f, 0xfc, 
0x3f, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xe0, 0x00, 0xff, 0x80, 0x00, 0x00, 0x00, 0x1f, 0xfc, 
0x1f, 0xff, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xe0, 0x00, 0xff, 0xe0, 0x00, 0x00, 0x00, 0x3f, 0xf8, 
0x1f, 0xff, 0xc0, 0x00, 0x00, 0x00, 0xff, 0xe0, 0x00, 0xff, 0xf8, 0x00, 0x00, 0x00, 0xff, 0xf8, 
0x0f, 0xff, 0xf8, 0x00, 0x00, 0x07, 0xff, 0xe0, 0x00, 0xff, 0xff, 0x00, 0x00, 0x07, 0xff, 0xf0, 
0x07, 0xff, 0xff, 0xe0, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x01, 0xff, 0xff, 0xe0, 
0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 
0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 
0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 
0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 
0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 
0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 
0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 
0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 
0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 
0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 
0x00, 0x00, 0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 
0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 
0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xff, 0xff, 0xff, 0xff, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

// Encoder pins
#define CLK 2
#define DT 3
#define SW 4

Encoder myEnc(CLK, DT);
int lastPos = -999;
int pos;

int modeSelected = -1;  // No mode selected initially
boolean confirmed = false; 
int lastMode = -1; 


void setup() {
  Serial.begin(9600);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.drawBitmap(0, 0, logo_bmp, 128, 64, WHITE);
  display.display();
  Serial.println("OLED Display Setup complete");

  pinMode(SW, INPUT_PULLUP); // Set the button pin as input with pull-up

  delay(3000);                // Displays logo for 3 seconds 
  display.clearDisplay();
  display.display();
  Serial.println("Camera Slider");
  Serial.print("Enter mode: 0 = Pan, 1 = Rotate, 2 = Pan & Rotate or use encoder:\n");
}

void loop() {
  static long oldPosition  = -999;
  long newPosition = myEnc.read() / 4; // Encoder position

  // Handle encoder input
  if (!confirmed) {
    if (newPosition != oldPosition) {
      oldPosition = newPosition;
      if (newPosition < 0) newPosition = 2; // Wrap around logic
      if (newPosition > 2) newPosition = 0;
      updateDisplay(newPosition);
    }

    if (digitalRead(SW) == LOW) {
      modeSelected = newPosition;
      confirmed = true;
      Serial.print("Mode selected via Encoder: ");
      Serial.println(modeSelected);
      displayModeSelection(modeSelected);
    }
  }

  // Handle serial input
  if (!confirmed && Serial.available() > 0) {
    int serialInput = Serial.parseInt();
    if (serialInput >= 0 && serialInput <= 2) {
      modeSelected = serialInput;
      confirmed = true;
      Serial.print("Mode selected via Serial: ");
      Serial.println(modeSelected);
      displayModeSelection(serialInput);
    } else {
      Serial.println("Invalid input. Please enter a value between 0 and 2");
      // Optionally, you can add a delay or reset the Serial input buffer here
    }
  }

  // Execute the selected mode
  if (confirmed && modeSelected != lastMode) {
    lastMode = modeSelected;
    executeMode(modeSelected);
  }
}

void updateDisplay(int pos) {
  display.clearDisplay();
  display.setTextSize(1);

  display.setCursor(28, 4);
  display.print(F("Camera Slider"));

  display.setCursor(25, 20);
  if (pos == 0) {
    display.print(F("> "));
  } else {
    display.print(F("  "));
  }
  display.print(F("Pan"));

  display.setCursor(25, 30);
  if (pos == 1) {
    display.print(F("> "));
  } else {
    display.print(F("  "));
  }
  display.print(F("Rotate"));

  display.setCursor(25, 40);
  if (pos == 2) {
    display.print(F("> "));
  } else {
    display.print(F("  "));
  }
  display.print(F("Pan & Rotate"));

  display.display();
}

const char* getModeName(int pos) {
  switch (pos) {
    case 0: return "Pan";
    case 1: return "Rotate";
    case 2: return "Pan & Rotate";
    default: return "";
  }
}

void displayModeSelection(int mode) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(15, 20);
  display.print(F("Mode selected: \n"));
  display.print(getModeName(mode));
  display.display();
}

void executeMode(int mode) {
  switch (mode) {
    case 0:
      runPan();
      break;
    case 1:
      runRotate();
      break;
    case 2:
      runPanAndRotate();
      break;
  }
}

void runPan() {
  // Code to run Pan mode
  Serial.println("Running Pan");
}

void runRotate() {
  // Code to run Rotate mode
  Serial.println("Running Rotate");
}

void runPanAndRotate() {
  // Code to run Pan & Rotate mode
  Serial.println("Running Pan and Rotate");
}