#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)
const int homingSpeed = 1200; // Slower speed for homing (microseconds)

// Home directions (1 = positive direction, 0 = negative direction)
const bool HOME_DIR[MOTOR_COUNT] = {0, 0, 0};  // Assuming homing in negative direction

// Limit switch pins in order X, Y, Z
const int limitSwitches[MOTOR_COUNT] = {LIMITX, LIMITY, LIMITZ};

String inputString = "";      // String to hold incoming serial data
boolean stringComplete = false;  // Whether the string is complete

void setup() {
  Serial.begin(115200);
  inputString.reserve(50);
  
  // Initialize motor control pins
  for (int i = 0; i < MOTOR_COUNT; i++) {
    pinMode(EN[i], OUTPUT);
    pinMode(STEP[i], OUTPUT);
    pinMode(DIR[i], OUTPUT);
    
    // Enable motors (LOW = enabled for most drivers)
    digitalWrite(EN[i], LOW);
  }
  
  // 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.println("System initialized. Send G28 to home all axes.");
}

void loop() {
  // Process complete commands
  if (stringComplete) {
    processCommand(inputString);
    inputString = "";
    stringComplete = false;
  }
  
  // Check for stop button press
  if (digitalRead(STOP) == LOW) {
    emergencyStop();
  }
}

void serialEvent() {
  while (Serial.available()) {
    char inChar = (char)Serial.read();
    inputString += inChar;
    
    // If the incoming character is a newline, set a flag
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}

void processCommand(String command) {
  command.trim();  // Remove any whitespace
  
  if (command.startsWith("G28")) {
    Serial.println("Executing homing sequence (G28)");
    homeAllAxes();
  } else {
    Serial.println("Unknown command: " + command);
  }
}

void homeAllAxes() {
  // Home in sequence: X, Y, then Z
  for (int axis = 0; axis < MOTOR_COUNT; axis++) {
    homeAxis(axis);
    delay(500); // Pause between axes
  }
  
  Serial.println("Homing complete - machine at home position");
}

void homeAxis(int axis) {
  char axisName;
  switch(axis) {
    case 0: axisName = 'X'; break;
    case 1: axisName = 'Y'; break;
    case 2: axisName = 'Z'; break;
  }
  
  Serial.print("Homing ");
  Serial.print(axisName);
  Serial.println(" axis...");
  
  // Set direction for homing
  digitalWrite(DIR[axis], HOME_DIR[axis]);
  
  // Move until limit switch is triggered
  while (digitalRead(limitSwitches[axis]) == HIGH) { // Assuming active LOW switches
    // Check for stop button
    if (digitalRead(STOP) == LOW) {
      emergencyStop();
      return;
    }
    
    // Step the motor
    digitalWrite(STEP[axis], HIGH);
    delayMicroseconds(homingSpeed / 2);
    digitalWrite(STEP[axis], LOW);
    delayMicroseconds(homingSpeed / 2);
  }
  
  Serial.print(axisName);
  Serial.println(" axis homed successfully");
}

void emergencyStop() {
  Serial.println("EMERGENCY STOP ACTIVATED");
  
  // Disable all motors
  for (int i = 0; i < MOTOR_COUNT; i++) {
    digitalWrite(EN[i], HIGH); // HIGH = disabled for most drivers
  }
  
  // Wait until stop button is released
  while (digitalRead(STOP) == LOW) {
    delay(100);
  }
  
  Serial.println("Emergency stop cleared. Re-enabling motors.");
  
  // Re-enable motors
  for (int i = 0; i < MOTOR_COUNT; i++) {
    digitalWrite(EN[i], LOW);
  }
}