/*
  Arduino test code for a 16x4 I2C LCD Display.
  This sketch provides an INTERACTIVE menu for the robotic arm controller.

  ** REQUIRES **
  1. An Arduino board (Uno, Nano, etc.)
  2. A 16x4 I2C LCD Display
  3. A 2-axis joystick module with a switch
  4. The "LiquidCrystal_I2C" library.

  ** WIRING **
  - LCD GND -> Arduino GND
  - LCD VCC -> Arduino 5V
  - LCD SDA -> Arduino A4 (or SDA)
  - LCD SCL -> Arduino A5 (or SCL)
  
  - JOYSTICK GND  -> Arduino GND
  - JOYSTICK +5V  -> Arduino 5V
  - JOYSTICK VRy  -> Arduino A0 (Y-axis for Up/Down)
  - JOYSTICK SW   -> Arduino D2 (Switch for Select)
*/

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

// --- Joystick Pins ---
#define JOY_Y_PIN A0
#define JOY_SW_PIN 2

// !! IMPORTANT !!
// You may need to change 0x27 to 0x3F.
// If it doesn't work, run an "I2C Scanner" sketch to find the correct address.
LiquidCrystal_I2C lcd(0x27, 16, 4); // Address, Columns, Rows

// --- Menu State Machine ---
enum State {
  STATE_MAIN_MENU,
  STATE_LIVE_MONITOR,
  STATE_RECORD_MENU,
  STATE_PLAY_MENU,
  STATE_RECORDING,
  STATE_PLAYING
};
State currentState = STATE_MAIN_MENU;

int menuCursor = 0;           // Current line selected
int maxMainMenuOptions = 3;   // "Live", "Record", "Play"
int maxSlotMenuOptions = 5; // "Slot 1-4", "Back"

bool needsRedraw = true; // Flag to update the
long lastInputTime = 0;  // For debouncing joystick

void setup() {
  lcd.init();      // Initialize the LCD
  lcd.backlight(); // Turn on the backlight

  // Set up joystick switch pin
  pinMode(JOY_SW_PIN, INPUT_PULLUP); // Use internal pull-up resistor

  // --- Splash Screen ---
  lcd.setCursor(2, 0);
  lcd.print("Robotic Arm");
  lcd.setCursor(4, 1);
  lcd.print("Controller");
  lcd.setCursor(5, 2);
  lcd.print("v1.0");
  delay(2500); // Show splash screen
}

void loop() {
  // 1. Check for user input
  handleInput();

  // 2. Update the display ONLY if needed (prevents flickering)
  if (needsRedraw) {
    updateDisplay();
    needsRedraw = false; // We've redrawn, so clear the flag
  }
}

// --- Input Handling ---
void handleInput() {
  // A simple delay to "debounce" inputs
  if (millis() - lastInputTime < 200) {
    return; // Not enough time has passed
  }

  int joyY = analogRead(JOY_Y_PIN);
  bool joySelect = (digitalRead(JOY_SW_PIN) == LOW); // LOW because of PULLUP

  bool inputDetected = false;

  // Check which state we are in to decide what to do
  switch (currentState) {
    case STATE_MAIN_MENU:
      if (joyY > 900) { // Joystick UP
        menuCursor--;
        if (menuCursor < 0) menuCursor = maxMainMenuOptions - 1;
        inputDetected = true;
      } else if (joyY < 100) { // Joystick DOWN
        menuCursor++;
        if (menuCursor >= maxMainMenuOptions) menuCursor = 0;
        inputDetected = true;
      } else if (joySelect) { // Joystick SELECT
        if (menuCursor == 0) currentState = STATE_LIVE_MONITOR;
        if (menuCursor == 1) currentState = STATE_RECORD_MENU;
        if (menuCursor == 2) currentState = STATE_PLAY_MENU;
        menuCursor = 0; // Reset cursor for the next menu
        inputDetected = true;
      }
      break;

    case STATE_RECORD_MENU:
    case STATE_PLAY_MENU:
      if (joyY > 900) { // Joystick UP
        menuCursor--;
        if (menuCursor < 0) menuCursor = maxSlotMenuOptions - 1;
        inputDetected = true;
      } else if (joyY < 100) { // Joystick DOWN
        menuCursor++;
        if (menuCursor >= maxSlotMenuOptions) menuCursor = 0;
        inputDetected = true;
      } else if (joySelect) { // Joystick SELECT
        if (menuCursor == 4) { // 4 is the "Back" option
          currentState = STATE_MAIN_MENU;
        } else if (currentState == STATE_RECORD_MENU) {
          // Here you would set the slot (menuCursor 0-3)
          currentState = STATE_RECORDING;
        } else {
          // Here you would set the slot (menuCursor 0-3)
          currentState = STATE_PLAYING;
        }
        menuCursor = 0; // Reset cursor
        inputDetected = true;
      }
      break;

    case STATE_LIVE_MONITOR:
    case STATE_RECORDING:
    case STATE_PLAYING:
      if (joySelect) { // Press Select to stop/go back
        currentState = STATE_MAIN_MENU;
        menuCursor = 0;
        inputDetected = true;
      }
      break;
  }

  if (inputDetected) {
    lastInputTime = millis(); // Reset debounce timer
    needsRedraw = true;     // Flag that we need to update the screen
  }
}

// --- Display Functions ---
void updateDisplay() {
  lcd.clear(); // Clear screen before drawing
  switch (currentState) {
    case STATE_MAIN_MENU:
      showMainMenu();
      break;
    case STATE_LIVE_MONITOR:
      showLiveMonitor();
      break;
    case STATE_RECORD_MENU:
      showRecordMenu();
      break;
    case STATE_PLAY_MENU:
      showPlayMenu();
      break;
    case STATE_RECORDING:
      showRecordingScreen();
      break;
    case STATE_PLAYING:
      showPlayingScreen();
      break;
  }
}

void showMainMenu() {
  lcd.setCursor(0, 0);
  lcd.print(menuCursor == 0 ? "> Live Control" : "  Live Control");
  lcd.setCursor(0, 1);
  lcd.print(menuCursor == 1 ? "> Record Move" : "  Record Move");
  lcd.setCursor(0, 2);
  lcd.print(menuCursor == 2 ? "> Play Move" : "  Play Move");
  lcd.setCursor(0, 3);
  lcd.print("  (Joy: Up/Down)");
}

void showLiveMonitor() {
  // In the real code, these numbers would be variables
  lcd.setCursor(0, 0);
  lcd.print("S1: 90  S2: 120"); // S1 = Servo 1 angle
  lcd.setCursor(0, 1);
  lcd.print("S3: 45  S4: 80");
  lcd.setCursor(0, 3);
  lcd.print("Press Select...");
}

void showRecordMenu() {
  lcd.setCursor(0, 0);
  lcd.print("Record to Slot:");
  lcd.setCursor(0, 1);
  lcd.print(menuCursor == 0 ? "> Slot 1" : "  Slot 1");
  lcd.setCursor(0, 2);
  lcd.print(menuCursor == 1 ? "> Slot 2" : "  Slot 2");
  lcd.setCursor(0, 3);
  lcd.print(menuCursor == 2 ? "> Slot 3" : (menuCursor == 3 ? "> Slot 4" : (menuCursor == 4 ? "> Back" : "  Slot 3")));
  
  // This logic is tricky, let's simplify the 3rd row
  lcd.setCursor(10, 2); // Put Slot 4 on 2nd row
  lcd.print(menuCursor == 2 ? " Slot 3" : " Slot 3");
  lcd.setCursor(10, 3); // Put Back on 3rd row
  lcd.print(menuCursor == 3 ? " Slot 4" : " Slot 4");
  
  // Re-do layout for 4 slots + back
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(menuCursor == 0 ? "> Slot 1" : "  Slot 1");
  lcd.setCursor(9, 0);
  lcd.print(menuCursor == 1 ? "> Slot 2" : "  Slot 2");
  lcd.setCursor(0, 1);
  lcd.print(menuCursor == 2 ? "> Slot 3" : "  Slot 3");
  lcd.setCursor(9, 1);
  lcd.print(menuCursor == 3 ? "> Slot 4" : "  Slot 4");
  lcd.setCursor(0, 3);
  lcd.print(menuCursor == 4 ? "> Back to Menu" : "  Back to Menu");
}

void showPlayMenu() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(menuCursor == 0 ? "> Play Slot 1" : "  Play Slot 1");
  lcd.setCursor(0, 1);
  lcd.print(menuCursor == 1 ? "> Play Slot 2" : "  Play Slot 2");
  lcd.setCursor(0, 2);
  lcd.print(menuCursor == 2 ? "> Play Slot 3" : "  Play Slot 3");
  lcd.setCursor(0, 3);
  lcd.print(menuCursor == 3 ? "> Play Slot 4" : (menuCursor == 4 ? "> Back" : "  Play Slot 4"));

  // Re-do layout for 4 slots + back (same as record)
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(menuCursor == 0 ? "> Slot 1" : "  Slot 1");
  lcd.setCursor(9, 0);
  lcd.print(menuCursor == 1 ? "> Slot 2" : "  Slot 2");
  lcd.setCursor(0, 1);
  lcd.print(menuCursor == 2 ? "> Slot 3" : "  Slot 3");
  lcd.setCursor(9, 1);
  lcd.print(menuCursor == 3 ? "> Slot 4" : "  Slot 4");
  lcd.setCursor(0, 3);
  lcd.print(menuCursor == 4 ? "> Back to Menu" : "  Back to Menu");
}

void showRecordingScreen() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("!! RECORDING !!");
  lcd.setCursor(0, 1);
  // In real code, menuCursor would hold the selected slot (0-3)
  lcd.print("...to Slot ");
  lcd.print(menuCursor + 1); 
  lcd.setCursor(0, 3);
  lcd.print("Press Select Stop");
}

void showPlayingScreen() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(">> PLAYING <<");
  lcd.setCursor(0, 1);
  lcd.print("...from Slot ");
  lcd.print(menuCursor + 1);
  lcd.setCursor(0, 3);
  lcd.print("Press Select Stop");
}