const int enPin = 8;
const int stepXPin = 2; //X.STEP
const int dirXPin = 5; // X.DIR
const int stepYPin = 3; //Y.STEP
const int dirYPin = 6; // Y.DIR
const int stepZPin = 4; //Z.STEP
const int dirZPin = 7; // Z.DIR

// Added pins for stop control
const int stopControlOutPin = 12; // Output pin
const int stopControlInPin = 13; // Input pin
const int testControlInPin = A0; // Input pin
const int testControlInPin2 = A1; // Input pin

bool currentDirection = HIGH; // Start with clockwise
bool previousDirection = !currentDirection; // Initialize to the opposite of currentDirection

int pulseWidthMicros = 50; // microseconds
int millisBtwnSteps = 2000; // Time between steps in microseconds

unsigned long lastStopDetectionTime = 0; // Tracks the last time a stop was detected
const unsigned long debounceDelay = 1000; // 1 second delay to debounce the stop detection

void setup() {
    Serial.begin(9600);
    pinMode(enPin, OUTPUT);
    digitalWrite(enPin, LOW);
    pinMode(stepYPin, OUTPUT);
    pinMode(dirYPin, OUTPUT);
    pinMode(stepZPin, OUTPUT);
    pinMode(dirZPin, OUTPUT);

    // Setup stop control pins
    pinMode(stopControlOutPin, OUTPUT);
    pinMode(stopControlInPin, INPUT_PULLUP); // Use internal pull-up resistor
    pinMode(testControlInPin, INPUT_PULLUP); // Use internal pull-up resistor
    pinMode(testControlInPin2, INPUT_PULLUP); // Use internal pull-up resistor

    Serial.println(F("CNC Shield Initialized"));
}

void loop() {
    if (currentDirection != previousDirection) {
        // Only update direction and log if there has been a change
        digitalWrite(dirYPin, currentDirection); // Set the current direction
        Serial.println(currentDirection == HIGH ? F("Running clockwise") : F("Running counter-clockwise"));
        previousDirection = currentDirection; // Update previous direction to current
    }

    unsigned long currentTime = millis();
    
    // Check if stop condition is met
    if (digitalRead(stopControlInPin) == LOW && (currentTime - lastStopDetectionTime > debounceDelay)) {
        Serial.println(F("Stop detected, changing direction..."));
        currentDirection = !currentDirection; // Change direction
        lastStopDetectionTime = currentTime; // Update the last stop detection time
    }
    unsigned long currentTime2 = millis();

    if (digitalRead(testControlInPin) == LOW && (currentTime2 - lastStopDetectionTime > debounceDelay)) {
      Serial.println(F("Awe Bra..."));
      digitalWrite(dirZPin, HIGH); // Starting direction for Z
      for (int i = 0; i < 5; i++) {
        digitalWrite(stepZPin, HIGH);
        delayMicroseconds(pulseWidthMicros);
        digitalWrite(stepZPin, LOW);
        delayMicroseconds(millisBtwnSteps); // Adjust for pulse width time
      }
      lastStopDetectionTime = currentTime2; // Update the last stop detection time
    }

    const int stepsPerRev = 200;
    int pulseWidthMicros = 50; // microseconds
    int millisBtwnSteps = 4000; // Time between steps in microseconds
    unsigned long currentTime3 = millis();

    if (digitalRead(testControlInPin2) == LOW && (currentTime3 - lastStopDetectionTime > debounceDelay)) {
      digitalWrite(dirZPin, LOW);
      Serial.println(F("Going Home!"));
      for (int j = 0; j < stepsPerRev * 2; j++) {
        digitalWrite(stepZPin, HIGH);
        delayMicroseconds(pulseWidthMicros);
        digitalWrite(stepZPin, LOW);
        delayMicroseconds(millisBtwnSteps); // Adjust for pulse width time
      }
      lastStopDetectionTime = currentTime2; // Update the last stop detection time
    }
    int stepPerRev = 500;
    // Send step instruction Y
    for(int i = 0; i < stepPerRev; i++) {
      digitalWrite(stepYPin, HIGH);
      delayMicroseconds(pulseWidthMicros);
      digitalWrite(stepYPin, LOW);
      delayMicroseconds(millisBtwnSteps); // Adjust for pulse width time
    }
    currentDirection = !currentDirection;
}