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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
#define OLED_I2C_ADDRESS 0x3C  // I2C address for the OLED display
Adafruit_SSD1306 display = Adafruit_SSD1306(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Servo ESC;  // create servo object to control the ESC

int potValue;  // value from the analog pin
int lastPotValue = -1;  // store the last potentiometer value to detect changes
const int ESC_PIN = 9;  // pin for ESC
const int BUTTON_START_PIN = 2;  // button to start sweep
const int BUTTON_CANCEL_PIN = 3;  // button to cancel sweep
bool sweeping = false;
bool lastSweeping = false;  // store the last sweeping status to detect changes

unsigned long previousMillis = 0;  // will store last time OLED was updated
const long interval = 100;  // interval at which to update the OLED (milliseconds)

// Functions for right alignment of integers
int align2(int q) { // Space in 100s and 10s
  if (q < 100) {display.print(" ");}
  if (q < 10) {display.print(" ");}
}
int align3(int q) { // Space in 1000s
  if (q < 1000) {display.print(" ");}
  align2(q);
}

void setup() {
  // Attach the ESC on pin 9
  ESC.attach(ESC_PIN, 1000, 2000);  // (pin, min pulse width, max pulse width in microseconds)
  ESC.write(20);
  Serial.begin(9600);
  Serial.println("ESC initialized!");

  pinMode(BUTTON_START_PIN, INPUT_PULLUP);
  pinMode(BUTTON_CANCEL_PIN, INPUT_PULLUP);

  // Initialize the OLED display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  display.display();
  delay(2000);  // Pause for 2 seconds
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.println("ESC Controller v0.0.1");
  // display.setCursor(80,34);
  // display.println("Turn Pot");
  display.setTextSize(1.5);             // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(80,5);
  display.drawRect(0, 10, 70, 35, SSD1306_WHITE);  // RAW pot value box
  display.println("Mode:");
  display.println("Manual");
  // display.clearDisplay();
  // display.setTextSize(1);
  // display.setTextColor(SSD1306_WHITE);
  // display.setCursor(0, 0);
  // display.println("ESC Controller v0.0.1 - Initializing...");
  // display.display();
  // delay(2000);
  // display.clearDisplay();
}

void loop() {
  unsigned long currentMillis = millis();

  checkStartButton();
  checkCancelButton();

  if (sweeping) {
    performSweep();
  } else {
    readPotentiometer();
  }

  // Update the OLED at a non-blocking interval
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    updateOLED();
  }
}

// Function to check if the start button is pressed
void checkStartButton() {
  if (digitalRead(BUTTON_START_PIN) == LOW) {
    Serial.println("Starting sweep!");
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println("ESC Controller v0.0.1");
    display.setCursor(0, 12);
    display.println("Starting sweep!");
    display.display();
    sweeping = true;
    delay(1000); // Debounce delay
  }
}

// Function to check if the cancel button is pressed
void checkCancelButton() {
  if (digitalRead(BUTTON_CANCEL_PIN) == LOW) {
    Serial.println("Cancelling sweep!");
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println("ESC Controller v0.0.1");
    display.setCursor(0, 12);
    display.println("Cancelling sweep!");
    display.display();
    sweeping = false;
    delay(1000); // Debounce delay
  }
}

// Function to perform the sweep operation
void performSweep() {
  unsigned long startTime = millis();
  unsigned long elapsedTime = 0;

  while (elapsedTime < 10000) { // 10 seconds
    if (digitalRead(BUTTON_CANCEL_PIN) == LOW) {
      sweeping = false;
      return;
    }
    
    elapsedTime = millis() - startTime;
    float phase = (float)elapsedTime / 10000.0;  // normalized phase (0.0 to 1.0)
    if (phase <= 0.5) {
      potValue = map(phase * 2000, 0, 1000, 0, 180);  // 0 to 180 in first half
    } else {
      potValue = map((phase - 0.5) * 2000, 0, 1000, 180, 0);  // 180 to 0 in second half
    }
    ESC.write(potValue);
    Serial.print("ESC_Signal:");
    Serial.print(potValue);
    Serial.println();
    updateOLED();
  }
  sweeping = false;
}

// Function to read the potentiometer value and update the ESC
void readPotentiometer() {
  potValue = analogRead(A0);                  // reads the value of the potentiometer (value between 0 and 1023)
  potValue = map(potValue, 0, 1023, 0, 180);  // scale it to use it with the servo library (value between 0 and 180)
  ESC.write(potValue);                        // Send the signal to the ESC
  Serial.print("ESC_Signal:");
  Serial.print(potValue);
  Serial.println();
}

// Function to update the OLED
void updateOLED() {
  if (lastPotValue != potValue || sweeping != lastSweeping) {
    // display.setCursor(0, 0);
    // display.setTextSize(1.4);
    // display.println("ESC Controller v0.0.1");
    // display.setCursor(0, 12);
    // if (sweeping) {
    //   display.println("Mode: Sweeping");
    // } else {
    //   display.println("Mode: Manual");
    // }
    // display.setCursor(0, 24);
    // display.print("ESC Signal: ");
    // display.print(potValue);
    // Read pot and display values
    int perCent = float(analogRead(A0) * 100.0/1018);
    display.drawLine(0, 47, 0, 62, SSD1306_WHITE);
    display.fillRect(1, 50, perCent, 10, SSD1306_WHITE);
    display.setTextSize(1);  //Normal 1:1 Pixel Scale
    display.setTextColor(SSD1306_WHITE); // Draw white text
    display.setCursor(102,50);
    align2(perCent);
    display.print(perCent);
    display.println("$");
    display.fillRect(1, 11, 68, 33, SSD1306_BLACK); // Clear box
    display.setCursor(10,20);
    display.setTextSize(2);
    align3(potValue);
    display.println(potValue);
    display.setTextSize(1);
    display.display();
    delay(1);
    display.fillRect(1, 50, perCent, 10, SSD1306_BLACK);
    display.setTextColor(SSD1306_BLACK);
    display.setCursor(102,50);
    align2(perCent);
    display.print(perCent);
    display.println("%");
    display.display();
    lastPotValue = potValue;
    lastSweeping = sweeping;
  }
}