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

LiquidCrystal_I2C lcd(0x27, 20, 4);

// Stepper motor pins
#define STEP_X 22
#define DIR_X  23
#define STEP_Y1 24
#define DIR_Y1  25
#define STEP_Y2 26
#define DIR_Y2  27
#define STEP_Z1 28
#define DIR_Z1  29
#define STEP_Z2 30
#define DIR_Z2  31

// Button pins
#define BTN_START A0
#define BTN_STOP  A1
#define BTN_HOME  A2

// Limit switch pins
#define LIMIT_X_NEG A3
#define LIMIT_X_POS A4
#define LIMIT_Y_NEG A5
#define LIMIT_Y_POS A6
#define LIMIT_Z_NEG A7
#define LIMIT_Z_POS A8

#define LED_PIN 13

const int stepDelayMicros = 300;

// Pulse counters
long stepsX = 0, stepsY = 0, stepsZ = 0;
long targetX = 0, targetY = 0, targetZ = 0;

// States
bool running = false;
bool homing = false;
bool ledOn = false;

bool stopX = false;
bool stopY = false;
bool stopZ = false;

void setup() {
  // Motor pins
  pinMode(STEP_X, OUTPUT); pinMode(DIR_X, OUTPUT);
  pinMode(STEP_Y1, OUTPUT); pinMode(DIR_Y1, OUTPUT);
  pinMode(STEP_Y2, OUTPUT); pinMode(DIR_Y2, OUTPUT);
  pinMode(STEP_Z1, OUTPUT); pinMode(DIR_Z1, OUTPUT);
  pinMode(STEP_Z2, OUTPUT); pinMode(DIR_Z2, OUTPUT);

  // Input buttons
  pinMode(BTN_START, INPUT_PULLUP);
  pinMode(BTN_STOP, INPUT_PULLUP);
  pinMode(BTN_HOME, INPUT_PULLUP);

  // Limit switches
  pinMode(LIMIT_X_NEG, INPUT_PULLUP);
  pinMode(LIMIT_X_POS, INPUT_PULLUP);
  pinMode(LIMIT_Y_NEG, INPUT_PULLUP);
  pinMode(LIMIT_Y_POS, INPUT_PULLUP);
  pinMode(LIMIT_Z_NEG, INPUT_PULLUP);
  pinMode(LIMIT_Z_POS, INPUT_PULLUP);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  lcd.init(); lcd.backlight();
  lcd.setCursor(0, 0); lcd.print("System Ready");
  Serial.begin(9600);
}

void loop() {
  // START
  if (digitalRead(BTN_START) == LOW && !running && !homing) {
    running = true;
    stopX = stopY = stopZ = false;

    digitalWrite(DIR_X, HIGH);
    digitalWrite(DIR_Y1, HIGH); digitalWrite(DIR_Y2, LOW);
    digitalWrite(DIR_Z1, HIGH); digitalWrite(DIR_Z2, HIGH);

    lcd.setCursor(0, 0); lcd.print("Running...         ");
    Serial.println("START PRESSED");
    delay(300);
  }

  // STOP
  if (digitalRead(BTN_STOP) == LOW && running) {
    running = false;
    digitalWrite(LED_PIN, LOW);
    ledOn = false;
    lcd.setCursor(0, 0); lcd.print("Stopped            ");
    Serial.print("STOPPED at X="); Serial.print(stepsX);
    Serial.print(" Y="); Serial.print(stepsY);
    Serial.print(" Z="); Serial.println(stepsZ);
    delay(300);
  }

  // HOMING
  if (digitalRead(BTN_HOME) == LOW && !homing) {
    homing = true;
    running = false;
    digitalWrite(LED_PIN, LOW);
    ledOn = false;

    // Save target
    targetX = stepsX;
    targetY = stepsY;
    targetZ = stepsZ;

    stepsX = stepsY = stepsZ = 0;

    digitalWrite(DIR_X, LOW);
    digitalWrite(DIR_Y1, LOW); digitalWrite(DIR_Y2, HIGH);
    digitalWrite(DIR_Z1, LOW); digitalWrite(DIR_Z2, LOW);

    lcd.setCursor(0, 0); lcd.print("Homing...          ");
    Serial.println("HOMING STARTED");
    delay(300);
  }

  // NORMAL MOTION
  if (running) {
    // Check limits
    if (digitalRead(LIMIT_X_NEG) == LOW || digitalRead(LIMIT_X_POS) == LOW) stopX = true;
    if (digitalRead(LIMIT_Y_NEG) == LOW || digitalRead(LIMIT_Y_POS) == LOW) stopY = true;
    if (digitalRead(LIMIT_Z_NEG) == LOW || digitalRead(LIMIT_Z_POS) == LOW) stopZ = true;

    // Check if any motor still active
    bool anyMotorMoving = !stopX || !stopY || !stopZ;

    if (anyMotorMoving && !homing && !ledOn) {
      digitalWrite(LED_PIN, HIGH);
      ledOn = true;
    }

    if (!anyMotorMoving && ledOn) {
      digitalWrite(LED_PIN, LOW);
      ledOn = false;
    }

    if (!stopX) {
      digitalWrite(STEP_X, HIGH); delayMicroseconds(stepDelayMicros);
      digitalWrite(STEP_X, LOW); delayMicroseconds(stepDelayMicros);
      stepsX++;
    }

    if (!stopY) {
      digitalWrite(STEP_Y1, HIGH); digitalWrite(STEP_Y2, HIGH);
      delayMicroseconds(stepDelayMicros);
      digitalWrite(STEP_Y1, LOW); digitalWrite(STEP_Y2, LOW);
      delayMicroseconds(stepDelayMicros);
      stepsY++;
    }

    if (!stopZ) {
      digitalWrite(STEP_Z1, HIGH); digitalWrite(STEP_Z2, HIGH);
      delayMicroseconds(stepDelayMicros);
      digitalWrite(STEP_Z1, LOW); digitalWrite(STEP_Z2, LOW);
      delayMicroseconds(stepDelayMicros);
      stepsZ++;
    }

    lcd.setCursor(0, 1); lcd.print("X:"); lcd.print(stepsX);
    lcd.setCursor(0, 2); lcd.print("Y:"); lcd.print(stepsY);
    lcd.setCursor(0, 3); lcd.print("Z:"); lcd.print(stepsZ);
  }

  // HOMING MOTION
  if (homing) {
    bool done = true;

    if (stepsX < targetX) {
      digitalWrite(STEP_X, HIGH); delayMicroseconds(stepDelayMicros);
      digitalWrite(STEP_X, LOW); delayMicroseconds(stepDelayMicros);
      stepsX++;
      done = false;
    }

    if (stepsY < targetY) {
      digitalWrite(STEP_Y1, HIGH); digitalWrite(STEP_Y2, HIGH);
      delayMicroseconds(stepDelayMicros);
      digitalWrite(STEP_Y1, LOW); digitalWrite(STEP_Y2, LOW);
      delayMicroseconds(stepDelayMicros);
      stepsY++;
      done = false;
    }

    if (stepsZ < targetZ) {
      digitalWrite(STEP_Z1, HIGH); digitalWrite(STEP_Z2, HIGH);
      delayMicroseconds(stepDelayMicros);
      digitalWrite(STEP_Z1, LOW); digitalWrite(STEP_Z2, LOW);
      delayMicroseconds(stepDelayMicros);
      stepsZ++;
      done = false;
    }

    lcd.setCursor(0, 1); lcd.print("HX:"); lcd.print(stepsX);
    lcd.setCursor(0, 2); lcd.print("HY:"); lcd.print(stepsY);
    lcd.setCursor(0, 3); lcd.print("HZ:"); lcd.print(stepsZ);

    if (done) {
      homing = false;
      lcd.setCursor(0, 0); lcd.print("Homing Done        ");
      Serial.println("HOMING COMPLETE");
      delay(500);
    }
  }
}