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

#define UP_PIN 2 // Up button pin
#define DOWN_PIN 3 // Down button pin

#define BATTERY_PIN A0 // Battery charge input pin

#define MODE_UPDATE_DELAY 5000  //Mode is updatted after 5s of select mode
#define BATTERY_CHECK_DELAY 20000 // Battery is chacked in 20s interval

uint8_t runningMode;

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C

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

uint32_t prev0, prev1, prev2, prev3;
int8_t currPosition = 1;
int8_t mode = -1;
bool modeUpdate;

void selectMenu(uint8_t currPos) {
  display.drawRoundRect(2, 15, 124, 12, 2, BLACK);
  display.drawRoundRect(2, 27, 124, 12, 2, BLACK);
  display.drawRoundRect(2, 39, 124, 12, 2, BLACK);
  display.drawRoundRect(2, 50, 124, 12, 2, BLACK);

  if (currPos == 1) {
    display.drawRoundRect(2, 15, 124, 12, 2, WHITE);
  } else if (currPos == 2) {
    display.drawRoundRect(2, 27, 124, 12, 2, WHITE);
  } else if (currPos == 3) {
    display.drawRoundRect(2, 39, 124, 12, 2, WHITE);
  } else if (currPos == 4) {
    display.drawRoundRect(2, 50, 124, 12, 2, WHITE);
  }
}

void menuUpdate() {
  display.fillRect(0, 12, 128, 52, BLACK);
  display.fillRect(2, 2, 73, 10, WHITE);
  display.setTextSize(0);
  display.setTextColor(BLACK);
  display.setCursor(3, 3);
  //Main Page
  display.println("Select Mode");
  display.setTextColor(WHITE);
  display.setCursor(20, 17);
  display.print("1. Automatic");
  display.setCursor(20, 29);
  display.print("2. Handheld High");
  display.setCursor(20, 41);
  display.print("3. Handheld Low");
  display.setCursor(20, 53);
  display.print("4. Stop");
}


void batteryLevelStatus(int battery_percentage) {
  display.drawRect(105, 0, 20, 10, WHITE);  // Battery outline
  display.fillRect(104, 2, 1, 6, WHITE);    // Battery icon

  int batteryFillWidth = map(battery_percentage, 0, 100, 0, 20);
  int batteryFillX = 125 - batteryFillWidth; // Calculate X-coordinate for fill

  display.fillRect(batteryFillX, 1, batteryFillWidth, 8, WHITE); // Battery fill

  // battery level in percentage
  display.setTextSize(0);
  display.setTextColor(WHITE);
  display.setCursor(77, 2);
  display.print(battery_percentage);
  if (battery_percentage > 9 && battery_percentage <= 99) {
    display.setCursor(91, 2);
    display.print("%");
  }
  else if (battery_percentage <= 9) {
    display.setCursor(88, 2);
    display.print("%");
  }
  else {
    display.setCursor(96, 2);
    display.print("%");
  }
}



void UP_PRESSED() {
  if (millis() - prev1 > 500 && currPosition != -1) {
    mode = -1;
    prev1 = millis();
    currPosition = (currPosition - 1);
    if (currPosition < 0) currPosition = 3;
    selectMenu(currPosition + 1);
    mode = currPosition + 1;
    modeUpdate = true;
    prev3 = millis();
  }
}

void DOWN_PRESSED() {
  if (millis() - prev2 > 500 && currPosition != -1) {
    mode = -1;
    prev2 = millis();
    currPosition = (currPosition + 1) % 4;
    selectMenu(currPosition + 1);
    mode = currPosition + 1;
    modeUpdate = true;
    prev3 = millis();
  }
}

void configDisplay() {
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;
  }
  delay(10);
  display.clearDisplay();

  /*
    display.clearDisplay();

    display.setTextSize(0);
    display.setTextColor(WHITE);
    display.setCursor(10, 25);
    display.print("Gizmo by Christine!");

    display.display();

    delay(2000);
    display.clearDisplay();

    batteryUpdate(56);
    menuUpdate();
  */
}


void setup() {
  Serial.begin(115200);
  configDisplay(); // Configure display

}

void loop() {
  display.clearDisplay();

  // Select position using push button
  if (digitalRead(2) == LOW && digitalRead(3) == HIGH) {
     if(currPosition > 5 || currPosition < 0){
      currPosition = 0;
    }
    currPosition++;
    delay(1000);
  }
  if (digitalRead(2) == HIGH && digitalRead(3) == LOW) {
    currPosition--;
    if(currPosition > 5 || currPosition < 0){
      currPosition = 0;
    }
    delay(1000);
  }
  // Battery Level
  int battery_voltage = analogRead(A0);
  int battery_percentage = map(battery_voltage, 0, 1023, 0, 100);
  batteryLevelStatus(battery_percentage);

  // Menu List
  menuUpdate();

  // Select Menu
  selectMenu(currPosition);

  display.display();
}