/*
  Forum: https://forum.arduino.cc/t/servo-motor-help/1364849
  Wokwi: https://wokwi.com/projects/425766774512205825

  Original function triggerServo() is outcommented and replaced by ahsrabrifat's suggestion

  ec2021

*/

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

Servo mainServo;
Servo servo1;
Servo servo2;

LiquidCrystal_I2C lcd(0x27, 16, 2);

const int selectButton = 2;
const int increaseButton = 3;
const int startButton = 4;

int selectedTime = 0; // 0 = hours, 1 = minutes, 2 = seconds
int countdownHours = 0;
int countdownMinutes = 0;
int countdownSeconds = 0;

bool lastSelectState = HIGH;
bool lastIncreaseState = HIGH;
bool lastStartState = HIGH;

int mainServoPos1 = 45;
int mainServoPos2 = 135;
int mainServoStart = 180;

void setup() {
    Serial.begin(9600);
    mainServo.attach(9);
    servo1.attach(10);
    servo2.attach(11);

    lcd.init();
    lcd.backlight();

    pinMode(selectButton, INPUT_PULLUP);
    pinMode(increaseButton, INPUT_PULLUP);
    pinMode(startButton, INPUT_PULLUP);

    // Reset servo positions
    mainServo.write(mainServoStart);
    servo1.write(0);
    servo2.write(0);
    
    displayTimeSelection();
}

void loop() {
    if (buttonPressed(selectButton, lastSelectState)) {
        selectedTime = (selectedTime + 1) % 3; // Cycles between 0, 1, 2
        displayTimeSelection();
    }

    if (buttonPressed(increaseButton, lastIncreaseState)) {
        increaseTime();
        displayTimeSelection();
    }

    if (buttonPressed(startButton, lastStartState)) {
        startCountdown();
    }
}

void increaseTime() {
    if (selectedTime == 0 && countdownHours < 23) {
        countdownHours++;
    } else if (selectedTime == 1 && countdownMinutes < 59) {
        countdownMinutes++;
    } else if (selectedTime == 2 && countdownSeconds < 59) {
        countdownSeconds++;
    }
}

void displayTimeSelection() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Set Time:");
    lcd.setCursor(0, 1);
    
    lcd.print(selectedTime == 0 ? "*H " : "H ");
    lcd.print(countdownHours); // No leading zero
    lcd.print(":");
    lcd.print(selectedTime == 1 ? "*M " : "M ");
    lcd.print(countdownMinutes); // No leading zero
    lcd.print(":");
    lcd.print(selectedTime == 2 ? "*S " : "S ");
    lcd.print(countdownSeconds); // No leading zero
}

void startCountdown() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Counting down...");
    
    while (countdownHours > 0 || countdownMinutes > 0 || countdownSeconds > 0) {
        lcd.setCursor(0, 1);
        printDoubleDigit(countdownHours);
        lcd.print(":");
        printDoubleDigit(countdownMinutes);
        lcd.print(":");
        printDoubleDigit(countdownSeconds);
        delay(1000);

        if (countdownSeconds > 0) {
            countdownSeconds--;
        } else if (countdownMinutes > 0) {
            countdownMinutes--;
            countdownSeconds = 59;
        } else if (countdownHours > 0) {
            countdownHours--;
            countdownMinutes = 59;
            countdownSeconds = 59;
        }
    }
    triggerServo();
}


/*

// This is the Orginal function  

void triggerServo() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Dispensing...");

    // Move main servo to position 1
    mainServo.write(mainServoPos1);
    delay(2000);

    // Open and close first servo
    servo1.write(90);
    delay(1000);
    servo1.write(0);
    delay(1000);

    // Move main servo to position 2
    mainServo.write(mainServoPos2);
    delay(2000);

    // Open and close second servo
    servo2.write(90);
    delay(1000);
    servo2.write(0);
    delay(1000);

    // Reset main servo to start position
    mainServo.write(mainServoStart);
    delay(2000);

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Done!");
}
*/

// Function as provided by https://forum.arduino.cc/u/ahsrabrifat
void triggerServo() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Dispensing...");

    // Move main servo to position 1
    mainServo.write(180); // Move forward
    delay(1000); // Adjust timing to control movement
    mainServo.write(90); // Stop
    delay(500);

    // Open and close first servo
    servo1.write(90);
    delay(1000);
    servo1.write(0);
    delay(1000);

    // Move main servo to position 2
    mainServo.write(0); // Move backward
    delay(1000); // Adjust timing to control movement
    mainServo.write(90); // Stop
    delay(500);

    // Open and close second servo
    servo2.write(90);
    delay(1000);
    servo2.write(0);
    delay(1000);

    // Reset main servo to start position
    mainServo.write(180); // Move forward
    delay(1000); 
    mainServo.write(90);  // Stop

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Done!");
}

bool buttonPressed(int button, bool &lastState) {
    bool buttonGotPressed = false;
    bool currentState = digitalRead(button);

    if (currentState != lastState) {  
        if (currentState == LOW) {   
            buttonGotPressed = true; 
        }
        lastState = currentState; 
        delay(50); 
    }

    return buttonGotPressed;
}

// Helper function to print numbers as two digits (01, 02, ... 09)
void printDoubleDigit(int number) {
    if (number < 10) {
        lcd.print("0");  
    }
    lcd.print(number);
}