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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Encoder encoder(2, 3); // Connect the encoder outputs to digital pins 2 and 3
const int buttonPin = 4; // Connect the button pin to digital pin 4

int menuIndex = 0; // Variable to store the current menu index
int lastEncoderValue = 0; // Variable to store the last encoder value
bool buttonState = LOW; // Variable to store the current state of the button
bool lastButtonState = LOW; // Variable to store the previous state of the button
unsigned long lastDebounceTime = 0; // Variable to store the time of the last debounce

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

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

    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("SSD1306 allocation failed"));
        for (;;)
            ;
    }

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

void loop() {
     int encoderValue = encoder.read(); // Read the encoder value

    // Check if the encoder value has changed and debounce it
    if (encoderValue != lastEncoderValue) {
        if (millis() - lastDebounceTime > 50) {
            if (encoderValue > lastEncoderValue) {
                menuIndex = (menuIndex + 1) % 9; // Assuming you have 9 menu options
            } else {
                menuIndex = (menuIndex - 1 + 9) % 9; // Ensure non-negative index
            }
            lastDebounceTime = millis(); // Update last debounce time
        }
        lastEncoderValue = encoderValue; // Update last encoder value
    }

    // Read the state of the button
    buttonState = digitalRead(buttonPin);

    // Check if the button state has changed from high to low (pressed)
    if (buttonState == LOW && lastButtonState == HIGH) {
        // Perform an action when the button is pressed
        // For example, display some information on the OLED screen
        display.clearDisplay();
        display.setTextSize(1);
        display.setTextColor(SSD1306_WHITE);
        display.setCursor(0, 0);
        display.println("Button Pressed!");
        display.display();
        delay(1000); // Delay to stabilize the display
    }

    lastButtonState = buttonState; // Update last button state
    displayMenu(); // Display menu options
}

void displayMenu() {
    display.clearDisplay();

    // Display menu options based on menuIndex
    switch (menuIndex) {
        case 0:
            displayMenuOption("BRAKE");
            break;
        case 1:
            displayMenuOption("LEFT");
            break;
        case 2:
            displayMenuOption("RIGHT");
            break;
        case 3:
            displayMenuOption("TAIL");
            break;
        case 4:
            displayMenuOption("REVERSE");
            break;
        case 5:
            displayMenuOption("AUX");
            break;
        case 6:
            displayMenuOption("ALL FLASH");
            break;
        case 7:
            displayMenuOption("SEQUENCE");
            break;
        case 8:
            displayMenuOption("SETTINGS");
            break;
    }

    display.display();
    delay(100); // Adjust as needed
}

void displayMenuOption(String option) {
    // Calculate the starting position to center the text horizontally
    int16_t x = (SCREEN_WIDTH - option.length() * 6) / 2; // Assuming each character width is 6 pixels

    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(x, 0);
    display.println(option);
}