// === Pin Definitions ===
const int in1Pin = 33;
const int in2Pin = 25;
const int enAPin = 32;
const int encoderPinA = 34;
const int encoderPinB = 35;
volatile long pulseCount = 0;
int lastEncoded = 0;

// Stepper
const int stepPin = 12;
const int dirPin = 14;
const int ENABLE_PIN = 13;
int pwmValue = 180;
const int limitSwitchPin = 26;  // Limit switch connected to pin 26

void updateEncoder() {
  int MSB = digitalRead(encoderPinA);
  int LSB = digitalRead(encoderPinB);
  int encoded = (MSB << 1) | LSB;
  int sum = (lastEncoded << 2) | encoded;

  static const int8_t encoderLookup[16] = {
     0, -1,  1,  0,
     1,  0,  0, -1,
    -1,  0,  0,  1,
     0,  1, -1,  0
  };

  pulseCount += encoderLookup[sum];
  lastEncoded = encoded;
}

void setup() {
  Serial.begin(115200);
  pinMode(limitSwitchPin, INPUT_PULLUP);  // Assuming the limit switch is active low

  // DC motor
  pinMode(in1Pin, OUTPUT);
  pinMode(in2Pin, OUTPUT);
  pinMode(enAPin, OUTPUT);
  analogWrite(enAPin, pwmValue);

  pinMode(encoderPinA, INPUT_PULLUP);
  pinMode(encoderPinB, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(encoderPinA), updateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPinB), updateEncoder, CHANGE);

  // Stepper motor
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(ENABLE_PIN, OUTPUT);
  Serial.println("Ready. Enter commands like: DC 2000 or DC -2000 or STEP 400 or STEP -400");
  digitalWrite(ENABLE_PIN, LOW);  // Habilita el driver (activo bajo)
}

void loop() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();

    if (cmd.startsWith("DC")) {
      long target = cmd.substring(3).toInt();
      moveDC(target);
    } else if (cmd.startsWith("STEP")) {
      int steps = cmd.substring(5).toInt();
      moveStepper(steps);
    }
  }
}

void moveDC(long target) {
  Serial.print("Starting DC move to: ");
  Serial.println(target);

  long startPos = pulseCount;
  long targetAbs = abs(target);

  // Set direction
  if (target > 0) {
    digitalWrite(in1Pin, HIGH);
    digitalWrite(in2Pin, LOW);
  } else if (target < 0) {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, HIGH);
  } else {
    Serial.println("Target is 0. No movement.");
    return;
  }

  analogWrite(enAPin, pwmValue);

  while (true) {
    long delta = abs(pulseCount - startPos);
    if (delta >= targetAbs) break;
    delay(1); // optional smoothing
  }

  // Stop motor
  digitalWrite(in1Pin, LOW);
  digitalWrite(in2Pin, LOW);
  analogWrite(enAPin, 0);

  Serial.print("Final pulse count: ");
  Serial.println(pulseCount);
  Serial.println("DC move complete.\n");
}

void moveStepper(int steps) {
  Serial.print("Starting Stepper move: ");
  Serial.println(steps);

  // Check the direction: if the steps are positive, the motor moves forward, if negative, the motor moves backward
  bool movingForward = (steps > 0);
  
  // Set the direction of the motor based on the input steps
  digitalWrite(dirPin, (steps > 0) ? HIGH : LOW);
  steps = abs(steps);

  // Start the motor movement
  for (int i = 0; i < steps; i++) {
    // Check if the limit switch is pressed
    if (digitalRead(limitSwitchPin) == LOW && movingForward) {
      Serial.println("Limit switch triggered! Stopping stepper motor.");
      break;  // Stop if moving forward and limit switch is pressed
    }
    
    // Continue the motor movement
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(500);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(500);
  }

  // If we are at the limit switch, and the user sends a negative value, reverse direction
  if (digitalRead(limitSwitchPin) == LOW && !movingForward) {
    Serial.println("Limit switch triggered while moving forward. Reversing direction.");
    // Reverse direction and allow the motor to move away from the limit switch
    digitalWrite(dirPin, LOW);  // Set direction to reverse
    for (int i = 0; i < steps; i++) {
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(500);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(500);
    }
  }

  Serial.println("Stepper move complete.\n");
}