#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  
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define JOYSTICK_X A0
#define JOYSTICK_Y A1

#define SERVO_1 5  // Servo for Chili
#define SERVO_2 6  // Servo for Pepper
#define SERVO_3 7  // Servo for Ginger

Servo servo1, servo2, servo3;
int selectedSpice = 0;

void setup() {
    Serial.begin(9600);

    // Initialize OLED
    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
        Serial.println("SSD1306 allocation failed");
        for (;;);
    }

    // Attach Servos
    servo1.attach(SERVO_1);
    servo2.attach(SERVO_2);
    servo3.attach(SERVO_3);

    updateDisplay();  // Show initial menu
}

void loop() {
    int yValue = analogRead(JOYSTICK_Y);  // Read Y-axis for UP/DOWN movement
    int xValue = analogRead(JOYSTICK_X);  // Read X-axis for LEFT/RIGHT (for dispensing)

    Serial.print("X: "); Serial.print(xValue);
    Serial.print(" | Y: "); Serial.println(yValue);  // For debugging

    // Set stronger dead zone (movement detected only when joystick is pushed far enough)
    if (yValue < 300) {  // Move UP
        if (selectedSpice > 0) {
            selectedSpice--;
            updateDisplay();
            delay(500); // Debounce delay
        }
    } 
    else if (yValue > 700) { // Move DOWN
        if (selectedSpice < 2) {
            selectedSpice++;
            updateDisplay();
            delay(500); // Debounce delay
        }
    }

    // Dispense spice when moving joystick Left/Right
    if (xValue < 300 || xValue > 700) {  // Detect LEFT/RIGHT movement
        displayDispensingMessage();
        dispenseSpice(selectedSpice);  // Dispense spice based on selection
        updateDisplay();  // Return to main menu
        delay(500);  // Wait a bit before returning to menu
    }
}

void updateDisplay() {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);

    // Display Spice Options
    display.setCursor(10, 10);
    display.println((selectedSpice == 0) ? "> Chili" : "  Chili");

    display.setCursor(10, 30);
    display.println((selectedSpice == 1) ? "> Pepper" : "  Pepper");

    display.setCursor(10, 50);
    display.println((selectedSpice == 2) ? "> Ginger" : "  Ginger");

    display.display();  // Update the OLED screen
}

void displayDispensingMessage() {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(20, 30);
    display.println("Dispensing...");
    display.display();
    delay(1000);  // Wait 1 second before moving servo
}

void dispenseSpice(int spice) {
    Servo *selectedServo;
    
    // Determine which servo to control based on the selected spice
    switch (spice) {
        case 0: 
            Serial.println("Dispensing Chili...");
            selectedServo = &servo1; 
            break;
        case 1: 
            Serial.println("Dispensing Pepper...");
            selectedServo = &servo2; 
            break;
        case 2: 
            Serial.println("Dispensing Ginger...");
            selectedServo = &servo3; 
            break;
    }

    selectedServo->write(60);  // Rotate servo 60°
    delay(1000);               // Wait for 1 second
    selectedServo->write(0);   // Reset servo position
}