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

Servo servo1;
Servo servo2;
LiquidCrystal_I2C lcd(0x27, 16, 2);

const int button1Pin = 6;
const int button2Pin = 7;
bool isRunning = false;
int level = 1;
const int maxLevel = 5;
const int baseDelay = 2000; // Base delay in milliseconds
unsigned long startTime = 0; // Start time
bool button1Pressed = false; // To control debounce for button 1
bool button2Pressed = false; // To control debounce for button 2
const int duration = 60; // Duration of 60 seconds for the system to stay on
void setup() {
  Serial.begin(9600);
  servo1.attach(3); // Connects servo 1 to pin 3
  servo2.attach(5); // Connects servo 2 to pin 5
  pinMode(button1Pin, INPUT_PULLUP);
  pinMode(button2Pin, INPUT_PULLUP);
  
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("System Off");
  Serial.println("System Off"); // Displays initial message
  servo1.write(0);
  servo2.write(0);
}
void loop() {
  // Checks the on/off button with debounce
  if (digitalRead(button1Pin) == LOW && !button1Pressed) {
    isRunning = !isRunning; // Toggles the system state
    delay(200);
    if (isRunning) {
      level = 1; // Resets the level to 1 when turned on
      startTime = millis(); // Stores the current time
      lcd.clear(); // Clears the LCD
      lcd.print("System On");
      Serial.println("System ON");
      delay(1000); // Waits 1 second
      lcd.clear();
      Serial.print("Speed: ");
      Serial.println(level); // Displays the current level
    } else {
      servo1.write(90);
      servo2.write(90);
      lcd.clear();
      Serial.println("System OFF"); // Displays the off message
      delay(1000); // Waits 1 second
    }
  } else if (digitalRead(button1Pin) == HIGH && button1Pressed) {
    button1Pressed = false; // Allows the next button press
  }
  // Checks the button to adjust speed with debounce
  if (digitalRead(button2Pin) == LOW && isRunning && !button2Pressed) {
    button2Pressed = true; // Prevents multiple quick presses
    level = (level % maxLevel) + 1; // Increments the speed and resets at level 5
    lcd.clear();
    lcd.setCursor(0, 0); // Moves the cursor to the first line
    lcd.print("Speed: ");
    lcd.print(level);
    Serial.print("Speed: ");
    Serial.println(level); // Displays the current speed level
    delay(100); // Anti-bounce
  } else if (digitalRead(button2Pin) == HIGH && button2Pressed) {
    button2Pressed = false; // Allows the next button press
  }
  // If the system is running, control the servos
  if (isRunning) {
    int delayTime = baseDelay / level; // Sets the delay based on the speed level
    // Rotate the servos in opposite directions
    servo1.write(360); // Rotates servo1 to 360 degrees (on tinkercad it's limited to 180 :D)
    servo2.write(0); // Rotates servo2 to 0 degrees (opposite direction of servo 1)
    delay(delayTime); // Waits based on the speed level
    // Return the servos to opposite positions
    servo1.write(0); // Rotates servo1 to 0 degrees
    servo2.write(180); // Rotates servo2 to 180 degrees (opposite direction)
    delay(delayTime); // Waits based on the speed level (the higher the level, the shorter the delay)
    // Countdown
    unsigned long elapsedTime = (millis() - startTime) / 1000;
    int remainingTime = duration - elapsedTime; // Remaining time
    lcd.setCursor(0, 1);
    lcd.print("Time: ");
    Serial.print("Time: ");
    lcd.print(remainingTime);
    Serial.print(remainingTime);
    lcd.print(" s");
    Serial.println(" s ");
    if (remainingTime <= 0) {
      isRunning = false;
      servo1.write(90);
      servo2.write(90);
      lcd.clear();
      lcd.print("System OFF");
      Serial.println("System OFF");
      delay(1000);
    }
  }
}