#include <Arduino.h>

const int MOTOR_COUNT = 3;  // We're using 3 motors (X, Y, Z)

// Pin definitions for motor control
const int EN[MOTOR_COUNT] = {13, 10, 7};    // Enable pins
const int STEP[MOTOR_COUNT] = {12, 9, 6};   // Step pins
const int DIR[MOTOR_COUNT] = {11, 8, 5};    // Direction pins

// Pin definitions for limit switches
#define LIMITX 22
#define LIMITY 24 
#define LIMITZ 26

// Pin definition for stop button
#define STOP 23

// Motor movement parameters
const int stepDelay = 800;  // Microseconds between steps (controls speed)

// Motor state variables
bool motorRunning[MOTOR_COUNT] = {true, true, true};  // All motors start running
bool motorDirection[MOTOR_COUNT] = {true, true, true}; // Default direction (true = clockwise)

void setup() {
  // Initialize motor control pins
  for (int i = 0; i < MOTOR_COUNT; i++) {
    pinMode(EN[i], OUTPUT);
    pinMode(STEP[i], OUTPUT);
    pinMode(DIR[i], OUTPUT);
    
    digitalWrite(EN[i], LOW);  // Enable all motors initially (LOW = enabled)
    digitalWrite(DIR[i], motorDirection[i] ? LOW : HIGH);  // Set initial direction
  }
  
  // Initialize limit switches with pull-up resistors
  pinMode(LIMITX, INPUT_PULLUP);
  pinMode(LIMITY, INPUT_PULLUP);
  pinMode(LIMITZ, INPUT_PULLUP);
  
  // Initialize stop button with pull-up resistor
  pinMode(STOP, INPUT_PULLUP);
  
  Serial.begin(9600);
  Serial.println("Stepper Motor Control System Ready");
  Serial.println("All motors rotating continuously until limit or stop");
}

// Function to move a specific motor one step in its current direction
void moveMotorOneStep(int motorIndex) {
  // Enable motor (in case it was disabled)
  digitalWrite(EN[motorIndex], LOW);
  
  // Make one step
  digitalWrite(STEP[motorIndex], HIGH);
  delayMicroseconds(stepDelay);
  digitalWrite(STEP[motorIndex], LOW);
  delayMicroseconds(stepDelay);
}

// Function to stop a specific motor
void stopMotor(int motorIndex) {
  digitalWrite(EN[motorIndex], HIGH);  // Disable motor
  motorRunning[motorIndex] = false;    // Update state
}

// Function to stop all motors
void stopAllMotors() {
  for (int i = 0; i < MOTOR_COUNT; i++) {
    stopMotor(i);
  }
}

void loop() {
  // Check for stop button first - this stops all motors
  if (digitalRead(STOP) == LOW) {
    stopAllMotors();
    Serial.println("EMERGENCY STOP - All motors stopped");
    delay(100); // Small debounce
    return; // Skip the rest of the loop
  }
  
  // Check limit switches
  if (digitalRead(LIMITX) == LOW) {
    stopMotor(0);
    Serial.println("X limit switch triggered - X motor stopped");
  }
  
  if (digitalRead(LIMITY) == LOW) {
    stopMotor(1);
    Serial.println("Y limit switch triggered - Y motor stopped");
  }
  
  if (digitalRead(LIMITZ) == LOW) {
    stopMotor(2);
    Serial.println("Z limit switch triggered - Z motor stopped");
  }
  
  // Move all motors that are still running
  for (int i = 0; i < MOTOR_COUNT; i++) {
    if (motorRunning[i]) {
      moveMotorOneStep(i);
    }
  }
}