#include <ESP32Servo.h>

const int DIR_PIN_1 = 2;    // Direction pin for the first stepper motor
const int STEP_PIN_1 = 0;   // Step pin for the first stepper motor

const int DIR_PIN_2 = 12;   // Direction pin for the second stepper motor
const int STEP_PIN_2 = 13;  // Step pin for the second stepper motor

const int DIR_PIN_1 = 2;    // Direction pin for the third stepper motor
const int STEP_PIN_1 = 0;   // Step pin for the third stepper motor

const int DIR_PIN_1 = 2;    // Direction pin for the fourth stepper motor
const int STEP_PIN_1 = 0;   // Step pin for the fouth stepper motor

const int steps_per_rev = 200;

const int servoPin_tilting = 14;
const int servoPin_feeding = 27;

Servo servo_tilting;
Servo servo_feeding;

String data = "9;5;-9;-13;1\n2;6;11;7;-1\n9;5;-9;-13;1\n2;6;11;7;-1\n-22;-22;11;7;-1\n220;-220;11;7;-1\n"; // Add more lines if needed

void setup() {
  pinMode(DIR_PIN_1, OUTPUT);
  pinMode(STEP_PIN_1, OUTPUT);
  pinMode(DIR_PIN_2, OUTPUT);
  pinMode(STEP_PIN_2, OUTPUT);

  servo_tilting.attach(servoPin_tilting, 500, 2400);
  servo_feeding.attach(servoPin_feeding, 500, 2400);

  processData(data);
}

void moveStepper(int stepPin1, int dirPin1, int stepsToMove1, int stepPin2, int dirPin2, int stepsToMove2) {
  digitalWrite(dirPin1, stepsToMove1 > 0 ? HIGH : LOW);
  digitalWrite(dirPin2, stepsToMove2 > 0 ? HIGH : LOW);

  int steps1 = abs(stepsToMove1);
  int steps2 = abs(stepsToMove2);
  int steps = max(steps1, steps2);

  for (int i = 0; i < steps; i++) {
    if (i < steps1) {
      digitalWrite(stepPin1, HIGH);
    }
    if (i < steps2) {
      digitalWrite(stepPin2, HIGH);
    }
    delayMicroseconds(2000);
    digitalWrite(stepPin1, LOW);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(2000);
  }
}

void activateServo(int angle) {
  servo_tilting.write(angle); // Set servo_tilting angle
  delay(10); // Adjust the delay for the servo to move to its position
}

void processData(String data) {
  String delimiter = "\n";
  int pos = 0;
  String line;
  while ((pos = data.indexOf(delimiter)) != -1) {
    line = data.substring(0, pos);
    data = data.substring(pos + delimiter.length());
    
    String values[5];
    int i = 0;
    int startPos = 0;
    int endPos = line.indexOf(";");
    while (endPos != -1) {
      values[i++] = line.substring(startPos, endPos);
      startPos = endPos + 1;
      endPos = line.indexOf(";", startPos);
    }
    values[i] = line.substring(startPos, line.length());

    int stepsMotor1 = values[0].toInt();
    int stepsMotor2 = values[1].toInt();

    moveStepper(STEP_PIN_1, DIR_PIN_1, stepsMotor1, STEP_PIN_2, DIR_PIN_2, stepsMotor2);

    int servoControl = values[4].toInt();
    if (servoControl < 0) {
      activateServo(60); // Activate the servo
    } else {
      activateServo(0); // Deactivate the servo
    }

    delay(1000); // Adjust delay as needed after completing each line
  }
}

void loop() {
  // Your code here, if needed
}