#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Initialize the LCD library with the I2C address 0x27 (may vary depending on your module)
LiquidCrystal_I2C lcd(0x27, 16, 2);

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 LCD was updated
const long interval = 100;  // interval at which to update the LCD (milliseconds)

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!");
  // Initialize the LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("ESC loading...");
  delay(500);
  lcd.setCursor(0, 0);
  lcd.print("ESC initialized!");
  delay(200);
  lcd.clear();

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

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

  checkStartButton();
  checkCancelButton();

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

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

// Function to check if the start button is pressed
void checkStartButton() {
  if (digitalRead(BUTTON_START_PIN) == LOW) {
    Serial.println("Starting sweep!");
    lcd.setCursor(0, 0);
    lcd.print("Starting sweep!");
    sweeping = true;
    delay(1000); // Debounce delay
    lcd.clear();
  }
}

// Function to check if the cancel button is pressed
void checkCancelButton() {
  if (digitalRead(BUTTON_CANCEL_PIN) == LOW) {
    Serial.println("Cancelling sweep!");
    lcd.setCursor(0, 0);
    lcd.print("Cancelling sweep!");
    sweeping = false;
    delay(1000);
    lcd.clear();
  }
}

// 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();
    updateLCD();
  }
  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 LCD
void updateLCD() {
  if (lastPotValue != potValue || sweeping != lastSweeping) {
    if (sweeping) {
      lcd.setCursor(0, 0);
      lcd.print("Sweeping:      ");
      lcd.setCursor(0, 1);
      lcd.print("ESC Signal:    ");
      lcd.setCursor(11, 1);
      lcd.print(potValue);
    } else {
      lcd.setCursor(0, 0);
      lcd.print("Potentiometer: ");
      lcd.setCursor(0, 1);
      lcd.print("ESC Signal:    ");
      lcd.setCursor(11, 1);
      lcd.print(potValue);
    }
    lastPotValue = potValue;
    lastSweeping = sweeping;
  }
}