// Constants (adjust as needed)
const float mmPerStep = 0.0488;  // example value
const float tolerance = 0.5;     // mm tolerance

void setup() {
  Serial.begin(9600);

  // Motor pins setup
  pinMode(8, OUTPUT);  // motorForwardPin
  pinMode(9, OUTPUT);  // motorBackwardPin
}

void loop() {
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n'); // Read line
    input.trim(); // remove leading/trailing spaces

    int commaIndex = input.indexOf(',');
    if (commaIndex == -1) {
      Serial.println("Invalid input! Use: <number>,<state>");
      return;
    }

    String targetStr = input.substring(0, commaIndex);
    String state = input.substring(commaIndex + 1);
    state.trim();
    targetStr.trim();

    float targetMM = targetStr.toFloat();

    moveActuatorToPosition("Motor1", A0, 8, 9, targetMM, state);
  }
}

// Updated function with state
void moveActuatorToPosition(String motor_name, int potPin, int motorForwardPin, int motorBackwardPin, float targetMM, String state) {
  // Clamp target range
  if (targetMM < 0) targetMM = 0;
  if (targetMM > 50) targetMM = 50;

  while (true) {
    // Allow immediate stop
    if (state == "stp") {
      digitalWrite(motorForwardPin, LOW);
      digitalWrite(motorBackwardPin, LOW);
      Serial.println("Motor stopped due to 'stp' command.");
      break;
    }

    int potValue = analogRead(potPin);
    float currentMM = potValue * mmPerStep;
    float error = targetMM - currentMM;

    Serial.print(motor_name);
    Serial.print(" Target: ");
    Serial.print(targetMM);
    Serial.print(" Current: ");
    Serial.print(currentMM);
    Serial.print(" Error: ");
    Serial.print(error);

    if (abs(error) <= tolerance) {
      digitalWrite(motorForwardPin, LOW);
      digitalWrite(motorBackwardPin, LOW);
      Serial.println(" | Target reached.");
      break;
    }

    if (error > 0) {
      digitalWrite(motorForwardPin, HIGH);
      digitalWrite(motorBackwardPin, LOW);
    } else {
      digitalWrite(motorForwardPin, LOW);
      digitalWrite(motorBackwardPin, HIGH);
    }

    Serial.println();
    delay(10);
  }

  digitalWrite(motorForwardPin, LOW);
  digitalWrite(motorBackwardPin, LOW); 
}