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

// LCD Configuration
LiquidCrystal_I2C lcd(0x27, 20, 4); // I2C address 0x27, 20x4 LCD

// Pin Definitions - TESTED AND WORKING
#define RAIL_STEP_PIN D6    // for feeder linear position motor
#define RAIL_DIR_PIN D7     // for feeder linear position motor
#define FEEDER_STEP_PIN D10  // for food extraction motor
#define FEEDER_DIR_PIN D8    // for food extraction motor
#define ENCODER_CLK D2
#define ENCODER_DT D1
#define ENCODER_SW D3
#define ENDSTOP_PIN D0      // for homing limit switch (pulled up)

// Stepper Motor Configuration
AccelStepper railStepper(AccelStepper::DRIVER, RAIL_STEP_PIN, RAIL_DIR_PIN);
AccelStepper feederStepper(AccelStepper::DRIVER, FEEDER_STEP_PIN, FEEDER_DIR_PIN);

// Motor Parameters - SIMPLIFIED 200 SPEED FOR ALL
#define MOTOR_SPEED 200           // All motors: 200 steps/sec
#define MOTOR_ACCELERATION 100    // Acceleration for rail motor
#define STEPS_PER_MM 80           // Steps per millimeter for rail motor
#define FEEDER_FEED_SPEED 200      // Feeder: 200 steps/sec (simplified, reliable)
#define STEPS_PER_REVOLUTION 800  // Both motors: 800 steps = 1 full revolution

// System Constants - UPDATED FOR 800 STEPS/REV
#define MAX_POSITIONS 3
#define MIN_FEED_TIME 1
#define MAX_FEED_TIME 60
#define DEBOUNCE_DELAY 50
#define LONG_PRESS_TIME 2000
#define MAX_RAIL_POSITION 8000    // Increased for 800 steps/rev (was 3000)
#define HOMING_SPEED 100          // Slower homing speed for safety
#define HOMING_TIMEOUT 30000      // Homing timeout in milliseconds

// EEPROM Addresses
#define EEPROM_POSITIONS 0
#define EEPROM_FEED_TIMES 20
#define EEPROM_SETTINGS_SAVED 50
#define EEPROM_HOME_POSITION 60

// System Variables
volatile int encoderPos = 0;
volatile bool encoderChanged = false;
int lastEncoderPos = 0;
bool lastButtonState = HIGH;
unsigned long buttonPressTime = 0;
unsigned long lastDebounceTime = 0;

// Homing Variables
bool isHomed = false;
long homePosition = 0;
bool homingInProgress = false;
unsigned long homingStartTime = 0;

// System State
enum SystemState {
  STARTUP,
  HOMING,
  MAIN_MENU,
  SET_POSITIONS,
  SET_FEED_TIMES,
  FEEDING_MODE,
  RUNNING_CYCLE,
  MANUAL_CONTROL
};

SystemState currentState = STARTUP;
int menuIndex = 0;
int editingPosition = 0;
bool isEditing = false;

// Feeding System Variables - UPDATED DEFAULT POSITIONS
long feedingPositions[MAX_POSITIONS] = {1600, 4000, 6400}; // Updated for 800 steps/rev
int feedingTimes[MAX_POSITIONS] = {5, 5, 5}; // Default feed times in seconds
int currentFeedingPosition = 0;
bool feedingActive = false;
unsigned long feederStartTime = 0;
bool feederRunning = false;
bool railMoving = false;

// Menu Structure
const char* mainMenuItems[] = {
  "1.Start Feeding",
  "2.Set Positions", 
  "3.Set Feed Times",
  "4.Manual Control",
  "5.Home System"
};

// Function declarations
void loadSettings();
void saveSettings();
void encoderISR();
void handleEncoder();
void handleButton();
void handleShortPress();
void handleLongPress();
void startHoming();
void handleHoming();
void displayMainMenu();
void selectMenuItem();
void displayHomingRequired();
void displaySetPositions();
void displaySetFeedTimes();
void displayFeedingMode();
void displayManualControl();
void displaySystemInfo();
void handleManualControl();
void handleManualMovement(int delta);
void toggleFeeder();
void startFeedingCycle();
void handleRunningCycle();
void displayFeedingCountdown();
void moveToFeedingPosition(long targetPosition);
void startFeeder();
void stopFeeder();
void stopAllMotors();
void stopFeeding();
void completeFeedingCycle();

void setup() {
  // Initialize serial FIRST
  Serial.begin(9600);
  delay(1000); // Give serial time to initialize
  Serial.println("Animal Feed System Starting...");
  
  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  
  // Initialize pins AFTER serial is stable
  pinMode(ENCODER_CLK, INPUT_PULLUP);
  pinMode(ENCODER_DT, INPUT_PULLUP);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  pinMode(ENDSTOP_PIN, INPUT_PULLUP);
  
  // Initialize motor pins
  pinMode(RAIL_STEP_PIN, OUTPUT);
  pinMode(RAIL_DIR_PIN, OUTPUT);
  pinMode(FEEDER_STEP_PIN, OUTPUT);
  pinMode(FEEDER_DIR_PIN, OUTPUT);
  
  // Set initial states - ALL LOW
  digitalWrite(RAIL_STEP_PIN, LOW);
  digitalWrite(RAIL_DIR_PIN, LOW);
  digitalWrite(FEEDER_STEP_PIN, LOW);
  digitalWrite(FEEDER_DIR_PIN, LOW);
  
  // Attach interrupts for encoder
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), encoderISR, FALLING);
  
  // Initialize stepper motors - ALL 200 SPEED
  railStepper.setMaxSpeed(MOTOR_SPEED);      // 200 max
  railStepper.setAcceleration(MOTOR_ACCELERATION);
  
  feederStepper.setMaxSpeed(FEEDER_FEED_SPEED); // 200 max  
  feederStepper.setAcceleration(MOTOR_ACCELERATION);
  
  Serial.println("Motors configured - ALL at 200 steps/sec");
  
  // Load settings from EEPROM
  loadSettings();
  
  // Display welcome message
  lcd.setCursor(0, 0);
  lcd.print("Animal Feed System");
  lcd.setCursor(0, 1);
  lcd.print("SIMPLIFIED - 200sps");
  lcd.setCursor(0, 2);
  lcd.print("Focus: MOTOR SPIN");
  lcd.setCursor(0, 3);
  lcd.print("Initializing...");
  delay(3000);
  
  // Start homing sequence
  currentState = HOMING;
  startHoming();
  
  Serial.println("Setup complete - Starting homing");
}

void loop() {
  handleEncoder();
  handleButton();
  
  // Always run stepper motors when needed
  if (homingInProgress) {
    railStepper.runSpeed(); // PRIORITY: Homing motor spinning
  } else {
    railStepper.run(); // Normal positioning
  }
  
  if (feederRunning) {
    feederStepper.runSpeed(); // Feeder motor spinning
  }
  
  switch (currentState) {
    case STARTUP:
      currentState = HOMING;
      startHoming();
      break;
      
    case HOMING:
      handleHoming();
      break;
      
    case MAIN_MENU:
      // Main menu handled by encoder and button functions
      break;
      
    case SET_POSITIONS:
      // Position setting handled by encoder and button functions
      break;
      
    case SET_FEED_TIMES:
      // Feed time setting handled by encoder and button functions
      break;
      
    case FEEDING_MODE:
      // Feeding mode handled by encoder and button functions
      break;
      
    case RUNNING_CYCLE:
      handleRunningCycle();
      break;
      
    case MANUAL_CONTROL:
      handleManualControl();
      break;
  }
  
  delay(10);
}

// Encoder interrupt service routine
void encoderISR() {
  static unsigned long lastInterruptTime = 0;
  unsigned long interruptTime = millis();
  
  if (interruptTime - lastInterruptTime > 5) {
    if (digitalRead(ENCODER_DT) == HIGH) {
      encoderPos++;
    } else {
      encoderPos--;
    }
    encoderChanged = true;
  }
  lastInterruptTime = interruptTime;
}

void handleEncoder() {
  if (encoderChanged) {
    encoderChanged = false;
    int encoderDelta = encoderPos - lastEncoderPos;
    
    switch (currentState) {
      case MAIN_MENU:
        menuIndex = constrain(menuIndex + encoderDelta, 0, 4);
        displayMainMenu();
        break;
        
      case SET_POSITIONS:
        if (!isEditing) {
          editingPosition = constrain(editingPosition + encoderDelta, 0, MAX_POSITIONS - 1);
        } else {
          // REAL-TIME POSITION ADJUSTMENT - Move motor as encoder turns
          long newPosition = feedingPositions[editingPosition] + encoderDelta * 100; // Increased step size
          newPosition = constrain(newPosition, 0, MAX_RAIL_POSITION);
          
          // Update position and move motor immediately
          feedingPositions[editingPosition] = newPosition;
          railStepper.moveTo(newPosition);
          
          Serial.print("Moving to position ");
          Serial.print(editingPosition + 1);
          Serial.print(": ");
          Serial.println(newPosition);
        }
        displaySetPositions();
        break;
        
      case SET_FEED_TIMES:
        if (!isEditing) {
          editingPosition = constrain(editingPosition + encoderDelta, 0, MAX_POSITIONS - 1);
        } else {
          feedingTimes[editingPosition] = constrain(
            feedingTimes[editingPosition] + encoderDelta, 
            MIN_FEED_TIME, MAX_FEED_TIME
          );
        }
        displaySetFeedTimes();
        break;
        
      case MANUAL_CONTROL:
        handleManualMovement(encoderDelta);
        break;
    }
    
    lastEncoderPos = encoderPos;
  }
}

void handleButton() {
  static bool buttonProcessed = false;
  static unsigned long pressStartTime = 0;
  bool currentButtonState = digitalRead(ENCODER_SW);
  
  // Detect button press start
  if (currentButtonState == LOW && lastButtonState == HIGH && !buttonProcessed) {
    pressStartTime = millis();
    buttonProcessed = true;
    Serial.println("Button press started");
  }
  
  // Detect button release and determine press type
  if (currentButtonState == HIGH && lastButtonState == LOW && buttonProcessed) {
    unsigned long pressDuration = millis() - pressStartTime;
    buttonProcessed = false;
    
    Serial.print("Button released after: ");
    Serial.print(pressDuration);
    Serial.println("ms");
    
    if (pressDuration > LONG_PRESS_TIME) {
      Serial.println("Long press detected");
      handleLongPress();
    } else if (pressDuration > 50) {
      Serial.println("Short press detected");
      handleShortPress();
    }
  }
  
  lastButtonState = currentButtonState;
}

void handleShortPress() {
  Serial.print("Short press in state: ");
  Serial.println(currentState);
  
  switch (currentState) {
    case MAIN_MENU:
      Serial.print("Selecting menu item: ");
      Serial.println(menuIndex);
      selectMenuItem();
      break;
      
    case SET_POSITIONS:
      if (!isEditing) {
        isEditing = true;
        Serial.print("Entering live position edit mode for position ");
        Serial.println(editingPosition + 1);
        // Move to current saved position when entering edit mode
        railStepper.moveTo(feedingPositions[editingPosition]);
      } else {
        isEditing = false;
        // Wait for motor to stop before saving
        if (railStepper.isRunning()) {
          lcd.setCursor(0, 3);
          lcd.print("Wait for motor...");
          return; // Don't save yet, motor still moving
        }
        saveSettings();
        Serial.print("Position ");
        Serial.print(editingPosition + 1);
        Serial.print(" saved at: ");
        Serial.println(feedingPositions[editingPosition]);
      }
      displaySetPositions();
      break;
      
    case SET_FEED_TIMES:
      if (!isEditing) {
        isEditing = true;
      } else {
        isEditing = false;
        saveSettings();
      }
      displaySetFeedTimes();
      break;
      
    case FEEDING_MODE:
      if (isHomed) {
        startFeedingCycle();
      } else {
        displayHomingRequired();
      }
      break;
      
    case RUNNING_CYCLE:
      // Emergency stop
      Serial.println("Emergency stop - button pressed during feeding");
      stopFeeding();
      currentState = FEEDING_MODE;
      displayFeedingMode();
      break;
      
    case MANUAL_CONTROL:
      // Toggle feeder on/off
      toggleFeeder();
      break;
  }
}

void handleLongPress() {
  Serial.print("Long press in state: ");
  Serial.println(currentState);
  
  // Long press goes back to previous menu or main menu
  switch (currentState) {
    case SET_POSITIONS:
    case SET_FEED_TIMES:
    case MANUAL_CONTROL:
      Serial.println("Going back to main menu");
      stopAllMotors(); // Stop any motor movement when exiting
      currentState = MAIN_MENU;
      isEditing = false;
      displayMainMenu();
      break;
      
    case FEEDING_MODE:
      Serial.println("Going back to main menu");
      currentState = MAIN_MENU;
      displayMainMenu();
      break;
      
    case RUNNING_CYCLE:
      Serial.println("Emergency stop - long press during feeding");
      stopFeeding();
      currentState = MAIN_MENU;
      displayMainMenu();
      break;
      
    case MAIN_MENU:
      // Long press in main menu shows system info
      displaySystemInfo();
      delay(3000);
      displayMainMenu();
      break;
      
    default:
      Serial.println("Long press not available in this state");
      break;
  }
}

// Homing Functions
void startHoming() {
  if (homingInProgress) return;
  
  homingInProgress = true;
  homingStartTime = millis();
  isHomed = false;
  
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("=== HOMING ===");
  lcd.setCursor(0, 1);
  lcd.print("Moving to home...");
  lcd.setCursor(0, 2);
  lcd.print("Direction: REVERSE");
  lcd.setCursor(0, 3);
  lcd.print("Speed: 100 steps/sec");
  
  // SIMPLE: Just set speed and start - SAME AS startFeeder()
  railStepper.setSpeed(-HOMING_SPEED); // NEGATIVE for reverse direction
  
  Serial.println("HOMING STARTED - moving REVERSE at 100 steps/sec");
  Serial.print("Verification - Speed set to: ");
  Serial.print(railStepper.speed());
  Serial.print(", MaxSpeed: ");
  Serial.println(railStepper.maxSpeed());
}

void handleHoming() {
  if (!homingInProgress) return;
  
  // PRIORITY: SPIN MOTOR CONTINUOUSLY - SAME AS feederStepper.runSpeed()
  railStepper.runSpeed();
  
  // Check endstop ONLY (minimal processing)
  if (digitalRead(ENDSTOP_PIN) == LOW) {
    // Endstop triggered - STOP MOTOR (same as stopFeeder)
    railStepper.setSpeed(0);
    railStepper.setCurrentPosition(0);
    homePosition = 0;
    isHomed = true;
    homingInProgress = false;
    
    Serial.println("HOMING COMPLETE - ENDSTOP TRIGGERED!");
    Serial.println("RAIL MOTOR STOPPED - Speed set to 0");
    
    // Simple completion display
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("HOMING COMPLETE!");
    lcd.setCursor(0, 1);
    lcd.print("System Ready");
    delay(2000);
    
    currentState = MAIN_MENU;
    displayMainMenu();
    return;
  }
  
  // Check timeout (minimal processing)
  if (millis() - homingStartTime > HOMING_TIMEOUT) {
    railStepper.setSpeed(0);
    homingInProgress = false;
    Serial.println("HOMING TIMEOUT - RAIL MOTOR STOPPED");
    currentState = MAIN_MENU;
    displayMainMenu();
    return;
  }
  
  // Update display ONLY every 3 seconds (minimal interruption)
  static unsigned long lastHomingDisplay = 0;
  if (millis() - lastHomingDisplay > 3000) {
    lcd.setCursor(0, 3);
    lcd.print("Rail: SPINNING...");
    lastHomingDisplay = millis();
  }
}

void displayMainMenu() {
  static int scrollOffset = 0;
  const int totalMenuItems = 5;
  const int visibleItems = 3;
  
  // Auto-scroll to keep selected item visible
  if (menuIndex < scrollOffset) {
    scrollOffset = menuIndex;
  } else if (menuIndex >= scrollOffset + visibleItems) {
    scrollOffset = menuIndex - visibleItems + 1;
  }
  
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("MENU");
  
  // Show homing status
  lcd.setCursor(15, 0);
  if (isHomed) {
    lcd.print("H:OK");
  } else {
    lcd.print("H:NO");
  }
  
  // Display visible menu items
  for (int i = 0; i < visibleItems; i++) {
    int itemIndex = scrollOffset + i;
    if (itemIndex < totalMenuItems) {
      lcd.setCursor(0, i + 1);
      
      // Show selection indicator
      if (itemIndex == menuIndex) {
        lcd.print(">");
      } else {
        lcd.print(" ");
      }
      
      // Display menu item (truncate if needed)
      String menuItem = String(mainMenuItems[itemIndex]);
      if (menuItem.length() > 18) {
        menuItem = menuItem.substring(0, 18);
      }
      lcd.print(menuItem);
    }
  }
  
  // Show scroll indicators if needed
  if (scrollOffset > 0) {
    lcd.setCursor(19, 1);
    lcd.print("^");
  }
  if (scrollOffset + visibleItems < totalMenuItems) {
    lcd.setCursor(19, 3);
    lcd.print("v");
  }
}

void selectMenuItem() {
  Serial.print("Selecting menu item: ");
  Serial.println(menuIndex);
  
  switch (menuIndex) {
    case 0: // Start Feeding
      Serial.println("Start Feeding selected");
      if (isHomed) {
        currentState = FEEDING_MODE;
        currentFeedingPosition = 0;
        displayFeedingMode();
      } else {
        displayHomingRequired();
      }
      break;
      
    case 1: // Set Positions
      Serial.println("Set Positions selected");
      if (isHomed) {
        currentState = SET_POSITIONS;
        editingPosition = 0;
        isEditing = false;
        displaySetPositions();
      } else {
        displayHomingRequired();
      }
      break;
      
    case 2: // Set Feed Times
      Serial.println("Set Feed Times selected");
      currentState = SET_FEED_TIMES;
      editingPosition = 0;
      isEditing = false;
      displaySetFeedTimes();
      break;
      
    case 3: // Manual Control
      Serial.println("Manual Control selected");
      if (isHomed) {
        currentState = MANUAL_CONTROL;
        displayManualControl();
      } else {
        displayHomingRequired();
      }
      break;
      
    case 4: // Home System
      Serial.println("Home System selected");
      currentState = HOMING;
      startHoming();
      break;
      
    default:
      Serial.println("Invalid menu selection");
      break;
  }
}

void displayHomingRequired() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("HOMING REQUIRED");
  lcd.setCursor(0, 1);
  lcd.print("Please home system");
  lcd.setCursor(0, 2);
  lcd.print("before operation");
  lcd.setCursor(0, 3);
  lcd.print("Press any key...");
  delay(3000);
  displayMainMenu();
}

void displaySetPositions() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("SET POSITIONS");
  
  lcd.setCursor(0, 1);
  lcd.print("Pos ");
  lcd.print(editingPosition + 1);
  lcd.print(": ");
  
  if (isEditing) {
    lcd.print("[");
    lcd.print(feedingPositions[editingPosition]);
    lcd.print("]");
    lcd.setCursor(15, 1);
    if (railStepper.isRunning()) {
      lcd.print("MOVE");
    } else {
      lcd.print("STOP");
    }
  } else {
    lcd.print(feedingPositions[editingPosition]);
  }
  
  lcd.setCursor(0, 2);
  lcd.print("Current: ");
  lcd.print(railStepper.currentPosition());
  if (isEditing) {
    lcd.print(" LIVE");
  } else {
    lcd.print(" steps");
  }
  
  lcd.setCursor(0, 3);
  if (isEditing) {
    lcd.print("Turn:MOVE Press:Save");
  } else {
    lcd.print("Press:Edit Hold:Back");
  }
}

void displaySetFeedTimes() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("SET FEED TIMES");
  
  lcd.setCursor(0, 1);
  lcd.print("Pos ");
  lcd.print(editingPosition + 1);
  lcd.print(": ");
  
  if (isEditing) {
    lcd.print("[");
    lcd.print(feedingTimes[editingPosition]);
    lcd.print("s]");
  } else {
    lcd.print(feedingTimes[editingPosition]);
    lcd.print("s");
  }
  
  lcd.setCursor(0, 2);
  lcd.print("Range: ");
  lcd.print(MIN_FEED_TIME);
  lcd.print("-");
  lcd.print(MAX_FEED_TIME);
  lcd.print("s");
  
  lcd.setCursor(0, 3);
  if (isEditing) {
    lcd.print("Turn:Adj Press:Save");
  } else {
    lcd.print("Press:Edit Hold:Back");
  }
}

void displayFeedingMode() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("=== FEEDING MODE ===");
  
  lcd.setCursor(0, 1);
  lcd.print("Ready to feed");
  lcd.setCursor(0, 2);
  lcd.print("Positions: ");
  lcd.print(MAX_POSITIONS);
  lcd.print(" Speed:200sps");
  lcd.setCursor(0, 3);
  lcd.print("Press: Start Cycle");
}

void displayManualControl() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("MANUAL CONTROL");
  
  lcd.setCursor(0, 1);
  lcd.print("Position: ");
  lcd.print(railStepper.currentPosition());
  
  lcd.setCursor(0, 2);
  lcd.print("Feeder: ");
  if (feederRunning) {
    lcd.print("SPINNING 200sps");
  } else {
    lcd.print("OFF");
  }
  
  lcd.setCursor(0, 3);
  lcd.print("Turn:Move Press:Feeder");
}

void displaySystemInfo() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("SYSTEM INFO");
  
  lcd.setCursor(0, 1);
  lcd.print("Homed: ");
  lcd.print(isHomed ? "YES" : "NO");
  lcd.print(" Speed:200");
  
  lcd.setCursor(0, 2);
  lcd.print("Position: ");
  lcd.print(railStepper.currentPosition());
  
  lcd.setCursor(0, 3);
  lcd.print("Max: ");
  lcd.print(MAX_RAIL_POSITION);
  lcd.print(" 800spr");
}

void handleManualControl() {
  // Update display periodically
  static unsigned long lastUpdate = 0;
  if (millis() - lastUpdate > 2000) {
    displayManualControl();
    lastUpdate = millis();
  }
}

void handleManualMovement(int delta) {
  if (!isHomed) return;
  
  long newPosition = railStepper.currentPosition() + (delta * 100); // Increased step size
  newPosition = constrain(newPosition, 0, MAX_RAIL_POSITION);
  
  railStepper.moveTo(newPosition);
  
  Serial.print("Manual move to position: ");
  Serial.println(newPosition);
}

void toggleFeeder() {
  if (feederRunning) {
    stopFeeder();
    Serial.println("Manual: FEEDER STOPPED");
  } else {
    startFeeder();
    Serial.println("Manual: FEEDER STARTED - should be spinning at 200sps");
  }
  displayManualControl();
}

// ===== FEEDING CYCLE FUNCTIONS =====
void startFeedingCycle() {
  currentState = RUNNING_CYCLE;
  currentFeedingPosition = 0;
  feedingActive = true;
  
  Serial.println("Starting feeding cycle at 200 steps/sec");
  Serial.print("Moving to position 1: ");
  Serial.println(feedingPositions[currentFeedingPosition]);
  
  // Show initial display
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("=== FEEDING ===");
  lcd.setCursor(0, 1);
  lcd.print("Starting cycle...");
  lcd.setCursor(0, 2);
  lcd.print("Moving to pos 1");
  
  moveToFeedingPosition(feedingPositions[currentFeedingPosition]);
}

void handleRunningCycle() {
  if (!feedingActive) return;
  
  // STEP 1: Move rail motor to position (rail motor working perfectly)
  if (railMoving) {
    // Rail motor keeps moving to position automatically in main loop
    
    // Check if rail reached the position
    if (!railStepper.isRunning()) {
      railMoving = false;
      Serial.print("Position ");
      Serial.print(currentFeedingPosition + 1);
      Serial.println(" reached - rail motor STOPPED");
      
      // Display feeding status
      displayFeedingCountdown();
      
      // STEP 2: Start feeder spinning at constant speed
      startFeeder();
      feederStartTime = millis();
      Serial.println("FEEDER: Spinning at constant 200 steps/sec");
    }
    return; // Don't do anything else while rail is moving
  }
  
  // STEP 3: Feeder spinning phase - feeder runs automatically in main loop
  if (feederRunning) {
    // Check if feeding time is complete
    unsigned long feederElapsed = millis() - feederStartTime;
    if (feederElapsed >= (unsigned long)feedingTimes[currentFeedingPosition] * 1000) {
      // STEP 4: Stop feeder, move to next position
      stopFeeder();
      Serial.println("FEEDER: Stopped - feeding time complete");
      
      currentFeedingPosition++;
      if (currentFeedingPosition >= MAX_POSITIONS) {
        // All positions done - cycle complete
        completeFeedingCycle();
      } else {
        // Move to next position
        Serial.print("RAIL: Moving to next position ");
        Serial.println(currentFeedingPosition + 1);
        
        // Show moving display
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("=== FEEDING ===");
        lcd.setCursor(0, 1);
        lcd.print("Moving to pos ");
        lcd.print(currentFeedingPosition + 1);
        lcd.setCursor(0, 2);
        lcd.print("Rail motor active...");
        
        // Start rail movement to next position
        moveToFeedingPosition(feedingPositions[currentFeedingPosition]);
      }
    }
    // During feeder spinning: NO rail motor, NO other processing
  }
}

void displayFeedingCountdown() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("=== FEEDING ===");
  
  lcd.setCursor(0, 1);
  lcd.print("Position: ");
  lcd.print(currentFeedingPosition + 1);
  lcd.print("/");
  lcd.print(MAX_POSITIONS);
  
  lcd.setCursor(0, 2);
  lcd.print("Feeder: ");
  lcd.print(feedingTimes[currentFeedingPosition]);
  lcd.print("s @ 200sps");
  
  lcd.setCursor(0, 3);
  lcd.print("Rail: STOPPED");
}

void moveToFeedingPosition(long targetPosition) {
  railStepper.moveTo(targetPosition);
  railMoving = true;
  Serial.print("Rail moving to position: ");
  Serial.println(targetPosition);
}

// PROVEN startFeeder() function from minimal test
void startFeeder() {
  feederStepper.setSpeed(FEEDER_FEED_SPEED); // 200 steps/sec
  feederRunning = true;
  Serial.println("FEEDER STARTED - 200 steps/sec - SPINNING NOW");
  
  // Immediate verification
  Serial.print("Verification - Speed set to: ");
  Serial.print(feederStepper.speed());
  Serial.print(", MaxSpeed: ");
  Serial.println(feederStepper.maxSpeed());
}

void stopFeeder() {
  feederStepper.setSpeed(0);
  feederRunning = false;
  Serial.println("FEEDER STOPPED - Speed set to 0");
}

void stopAllMotors() {
  railStepper.stop();
  stopFeeder();
  railMoving = false;
  Serial.println("All motors stopped");
}

void stopFeeding() {
  stopAllMotors();
  feedingActive = false;
  Serial.println("Feeding stopped - emergency stop");
}

void completeFeedingCycle() {
  stopFeeding();
  
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("=== CYCLE COMPLETE ===");
  lcd.setCursor(0, 1);
  lcd.print("All positions fed!");
  lcd.setCursor(0, 2);
  lcd.print("Press: Start Again");
  lcd.setCursor(0, 3);
  lcd.print("Hold: Main Menu");
  
  currentState = FEEDING_MODE;
  
  Serial.println("Feeding cycle complete - all positions fed");
}

void saveSettings() {
  // Save positions and feed times
  for (int i = 0; i < MAX_POSITIONS; i++) {
    EEPROM.put(EEPROM_POSITIONS + i * sizeof(long), feedingPositions[i]);
    EEPROM.put(EEPROM_FEED_TIMES + i * sizeof(int), feedingTimes[i]);
  }
  
  // Save home position
  EEPROM.put(EEPROM_HOME_POSITION, homePosition);
  
  // Mark settings as saved
  EEPROM.write(EEPROM_SETTINGS_SAVED, 0xAA);
  
  // Show confirmation
  lcd.setCursor(0, 3);
  lcd.print("Settings Saved!     ");
  delay(1000);
  
  Serial.println("Settings saved to EEPROM");
}

void loadSettings() {
  // Check if settings exist
  if (EEPROM.read(EEPROM_SETTINGS_SAVED) == 0xAA) {
    // Load positions and feed times
    for (int i = 0; i < MAX_POSITIONS; i++) {
      EEPROM.get(EEPROM_POSITIONS + i * sizeof(long), feedingPositions[i]);
      EEPROM.get(EEPROM_FEED_TIMES + i * sizeof(int), feedingTimes[i]);
      
      // Validate loaded values
      feedingPositions[i] = constrain(feedingPositions[i], 0, MAX_RAIL_POSITION);
      feedingTimes[i] = constrain(feedingTimes[i], MIN_FEED_TIME, MAX_FEED_TIME);
    }
    
    // Load home position
    EEPROM.get(EEPROM_HOME_POSITION, homePosition);
    
    Serial.println("Settings loaded from EEPROM");
  } else {
    Serial.println("Using default settings");
  }
}