// Define pin connections & motor's steps per revolution for each motor
const int dirPinMotor1 = 2;
const int stepPinMotor1 = 3;
const int stepsPerRevolutionMotor1 = 200;

const int dirPinMotor2 = 4;
const int stepPinMotor2 = 5;
const int stepsPerRevolutionMotor2 = 200;

const int dirPinMotor3 = 12;
const int stepPinMotor3 = 13;
const int stepsPerRevolutionMotor3 = 200;

// Define current positions for each motor
int currentPositionMotor1 = 0;
int currentPositionMotor2 = 0;
int currentPositionMotor3 = 0;

// Define coordinates of two opposite corners of the rectangle
const int rectX1 = 0;
const int rectY1 = 0;
const int rectZ1 = 0;
const int rectX2 = 2500;
const int rectY2 = 1500;
const int rectZ2 = 100;

void setup()
{
    // Declare pins as Outputs for each motor
    pinMode(stepPinMotor1, OUTPUT);
    pinMode(dirPinMotor1, OUTPUT);
    pinMode(stepPinMotor2, OUTPUT);
    pinMode(dirPinMotor2, OUTPUT);
    pinMode(stepPinMotor3, OUTPUT);
    pinMode(dirPinMotor3, OUTPUT);

    // Move motors to initial points
    moveMotorsToInitialPoints();
}

void loop()
{
    // Move motors to specified coordinates to form a rectangle
    moveMotorsToCoordinates(rectX1, rectY1, rectZ1);
    moveMotorsToCoordinates(rectX2, rectY1, rectZ1);
    moveMotorsToCoordinates(rectX2, rectY2, rectZ1);
    moveMotorsToCoordinates(rectX1, rectY2, rectZ1);
    moveMotorsToCoordinates(rectX1, rectY1, rectZ1);
   

    // Stop program execution
    while (true) {
        // Do nothing, program stops here
    }
}

void moveMotorsToInitialPoints() {
    moveMotorToPosition(dirPinMotor1, stepPinMotor1, stepsPerRevolutionMotor1, currentPositionMotor1, 0);
    moveMotorToPosition(dirPinMotor2, stepPinMotor2, stepsPerRevolutionMotor2, currentPositionMotor2, 0);
    moveMotorToPosition(dirPinMotor3, stepPinMotor3, stepsPerRevolutionMotor3, currentPositionMotor3, 0);
}

void moveMotorsToCoordinates(int x, int y, int z) {
    // Calculate steps required to move each motor
    int stepsX = x - currentPositionMotor1;
    int stepsY = y - currentPositionMotor2;
    int stepsZ = z - currentPositionMotor3;

    // Set direction based on movement direction
    int dirX = (stepsX > 0) ? HIGH : LOW;
    int dirY = (stepsY > 0) ? HIGH : LOW;
    int dirZ = (stepsZ > 0) ? HIGH : LOW;

    // Make steps positive
    stepsX = abs(stepsX);
    stepsY = abs(stepsY);
    stepsZ = abs(stepsZ);

    // Set motor directions
    digitalWrite(dirPinMotor1, dirX);
    digitalWrite(dirPinMotor2, dirY);
    digitalWrite(dirPinMotor3, dirZ);

    // Move motors simultaneously
    for (int i = 0; i < max(max(stepsX, stepsY), stepsZ); i++) {
        if (i < stepsX) {
            digitalWrite(stepPinMotor1, HIGH);
        }
        if (i < stepsY) {
            digitalWrite(stepPinMotor2, HIGH);
        }
        if (i < stepsZ) {
            digitalWrite(stepPinMotor3, HIGH);
        }
        delayMicroseconds(2000); // Adjust this delay as needed for your motor's speed

        digitalWrite(stepPinMotor1, LOW);
        digitalWrite(stepPinMotor2, LOW);
        digitalWrite(stepPinMotor3, LOW);
        delayMicroseconds(2000); // Adjust this delay as needed for your motor's speed
    }

    // Update current positions
    currentPositionMotor1 = x;
    currentPositionMotor2 = y;
    currentPositionMotor3 = z;
}

void moveMotorToPosition(int dirPin, int stepPin, int stepsPerRevolution, int &currentPosition, int targetPosition) {
    int stepsToMove = targetPosition - currentPosition;
    int direction = (stepsToMove > 0) ? HIGH : LOW; // Set direction based on movement direction
    stepsToMove = abs(stepsToMove); // Make sure stepsToMove is positive

    // Set motor direction
    digitalWrite(dirPin, direction);

    // Spin motor to move to the target position
    for(int x = 0; x < stepsToMove; x++)
    {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(2000); // Adjust this delay as needed for your motor's speed
        digitalWrite(stepPin, LOW);
        delayMicroseconds(2000); // Adjust this delay as needed for your motor's speed
    }

    currentPosition = targetPosition; // Update current position
}