//V4, selfmade
#include <Servo.h>

// Constants and pins
#define PUMP_PIN 7
#define NUM_GLASSES 3
#define GLASS_BUTTON_PINS {2, 3, 4} // Update with your glass button pins
#define MASTER_BUTTON_PIN 8
#define SERVO_PIN 9

// Glass positions and debounce settings
int glassPositions[NUM_GLASSES] = {120, 150, 180}; // Servo angles
int restPosition = 90;
int glassButtonPins[NUM_GLASSES] = GLASS_BUTTON_PINS;
//bool fillState[NUM_GLASSES] = {false, false, false}; // Glass fill states
unsigned long lastDebounceTime[NUM_GLASSES] = {0, 0, 0}; // Last debounce times for each button
unsigned long masterButtonDebounceTime = 50;
unsigned long debounceDelay = 50; // Debounce delay in milliseconds

Servo augerServo;

void setup() {
  // Initialize pump, servo, and buttons
  pinMode(PUMP_PIN, OUTPUT);
  augerServo.attach(SERVO_PIN);

  for (int i = 0; i < NUM_GLASSES + 1; i++) {
    pinMode(glassButtonPins[i], INPUT_PULLUP); // Use internal pull-up resistors for glass buttons
  }

  pinMode(MASTER_BUTTON_PIN, INPUT_PULLUP); // Use internal pull-up resistor for master button

  // Start with the auger at a neutral position
  augerServo.write(restPosition);
  Serial.begin(9600);
  Serial.println("Start program");
}

void loop() {
  // Check for a debounced press of the master button to start the sequence
  if (debouncedRead(MASTER_BUTTON_PIN, masterButtonDebounceTime)) {
    Serial.println("Master button pressed, starting sequence...");
    executeSequence();
  }
}

// Execute the sequence of checking and filling glasses
void executeSequence() {
  for (int i = 0; i < NUM_GLASSES + 1; i++) {
    processGlass(i);
  }

  // Return the auger to the neutral position after all glasses are processed
  augerServo.write(restPosition);
  Serial.println("Sequence complete, returning auger to neutral position.");
}

// Process a specific glass position
void processGlass(int index) {
  // Check if the glass button is debounced and pressed
  if (debouncedRead(glassButtonPins[index], lastDebounceTime[index])) {
    Serial.println("Glass detected at position " + String(index));


    // Move auger to the glass position
    augerServo.write(glassPositions[index]);
    Serial.println("Moving auger to position " + String(glassPositions[index]));
    delay(2000); // Wait for the auger to move

    // Dispense smoothie
    dispenseSmoothie();
  }
}

// Activate the pump for dispensing
void dispenseSmoothie() {
  digitalWrite(PUMP_PIN, HIGH); // Turn on pump
  Serial.println("Dispensing smoothie...");
  delay(2000);                  // Adjust dispensing time
  digitalWrite(PUMP_PIN, LOW);  // Turn off pump
  Serial.println("Smoothie dispensed.");
}

// Debounced read for buttons
bool debouncedRead(int pin, unsigned long &lastTime) {
  static bool lastState[NUM_GLASSES + 1] = {HIGH}; // Add +1 for the master button
  bool currentState = digitalRead(pin);

  // Check if the state has changed
  if (currentState != lastState[pin]) {
    lastTime = millis(); // Update the last time the state changed
  }

  // Only consider the state valid if the debounce delay has passed
  if ((millis() - lastTime) > debounceDelay) {
    lastState[pin] = currentState;
    if (currentState == LOW) { // Button pressed
      return true;
    }
  }

  return false;
}