#include <AccelStepper.h>
#include <Servo.h>
#include <SD.h>

File csvFile;
int repetitions = 1;
bool infiniteLoop = false;
bool stopCSV = false; 


#define STEPS_PER_REV 200 // Change this value based on your stepper motor specs

AccelStepper stepper1(1, 2, 5);  // Stepper 1 control pins: Step = 2, Direction = 5
AccelStepper stepper2(1, 3, 6);  // Stepper 2 control pins: Step = 3, Direction = 6
AccelStepper stepper3(1, 4, 7);  // Stepper 3 control pins: Step = 4, Direction = 7
AccelStepper stepper4(1, 12, 13); // Stepper 4 control pins: Step = 12, Direction = 13

Servo servoMotor;

const int limitSwitch1Pin = 11; // Limit switch 1 pin (adjust pin number as needed)
const int limitSwitch2Pin = 10; // Limit switch 2 pin (adjust pin number as needed)
const int limitSwitch3Pin = 9; // Limit switch 3 pin (adjust pin number as needed)
const int limitSwitch4Pin = A3; // Limit switch 4 pin (adjust pin number as needed)

bool steppersEnabled = true;

const int minSpeed = 100;
const int maxSpeed = 1200;
const int minAcceleration = 100;
const int maxAcceleration = 1200;

void setup() {

  

stepper1.setMaxSpeed(maxSpeed);
  stepper1.setAcceleration(maxAcceleration);
  
  stepper2.setMaxSpeed(maxSpeed);
  stepper2.setAcceleration(maxAcceleration);
  
  stepper3.setMaxSpeed(maxSpeed);
  stepper3.setAcceleration(maxAcceleration);
  
  stepper4.setMaxSpeed(maxSpeed);
  stepper4.setAcceleration(maxAcceleration);
  

  servoMotor.attach(A4); // Attach the servo motor to pin 11 (adjust as needed)

  Serial.begin(9600); // Initialize Serial communication
  pinMode(limitSwitch1Pin, INPUT_PULLUP); // Set the limit switch pin as input with internal pull-up resistor
  pinMode(limitSwitch2Pin, INPUT_PULLUP); // Set the limit switch pin as input with internal pull-up resistor
  pinMode(limitSwitch3Pin, INPUT_PULLUP); // Set the limit switch pin as input with internal pull-up resistor
  pinMode(limitSwitch4Pin, INPUT_PULLUP); // Set the limit switch pin as input with internal pull-up resistor

  home(); // Move steppers to home position at startup
   if (!SD.begin(53)) {
    Serial.println("SD card initialization failed!");
    return;
  }
  Serial.println("SD card initialized");

}

// Function to home stepper 1
void homeStepper1() {
  Serial.println("Homing stepper 1...");

  // Move stepper 1 to home position
  while (digitalRead(limitSwitch1Pin) != 1) {
    stepper1.setSpeed(100); // Set stepper 1 speed to move in the negative direction
    stepper1.runSpeed();
  }

  // Stop stepper 1 when the limit switch is triggered
  stepper1.stop();

  // Reset the stepper position to 0
  stepper1.setCurrentPosition(0);

  Serial.println("Stepper 1 homed.");
}

// Function to home stepper 2
void homeStepper2() {
  Serial.println("Homing stepper 2...");

  // Move stepper 2 to home position
  while (digitalRead(limitSwitch2Pin) != 1) {

    stepper2.setSpeed(100); // Set stepper 2 speed to move in the negative direction
    stepper2.runSpeed();
  }

  // Stop stepper 2 when the limit switch is triggered
  stepper2.stop();

  // Reset the stepper position to 0
  stepper2.setCurrentPosition(0);

  Serial.println("Stepper 2 homed.");
}

// Function to home stepper 3
void homeStepper3() {
  Serial.println("Homing stepper 3...");

  // Move stepper 3 to home position
  while (digitalRead(limitSwitch3Pin) != 1) {
    stepper3.setSpeed(-100); // Set stepper 3 speed to move in the negative direction
    stepper3.runSpeed();
  }

  // Stop stepper 3 when the limit switch is triggered
  stepper3.stop();

  // Reset the stepper position to 0
  stepper3.setCurrentPosition(0);

  Serial.println("Stepper 3 homed.");
}

// Function to home stepper 4
void homeStepper4() {
  Serial.println("Homing stepper 4...");

  // Move stepper 4 to home position
  while (digitalRead(limitSwitch4Pin) != 1) {
    stepper4.setSpeed(100); // Set stepper 4 speed to move in the negative direction
    stepper4.runSpeed();
  }

  // Stop stepper 4 when the limit switch is triggered
  stepper4.stop();

  // Reset the stepper position to 0
  stepper4.setCurrentPosition(0);

  Serial.println("Stepper 4 homed.");
}

void runCSVFile(const char* filename) {
  Serial.print("Running CSV file: ");
  Serial.println(filename);

  if (infiniteLoop) {
    while (true) { // Infinite loop
      runCSVOnce(filename);
      delay(1000); // Delay between repetitions
    }
  } else {
    for (int i = 0; i < repetitions; i++) {
      runCSVOnce(filename);
    }
  }
}

void runCSVOnce(const char* filename) {
  csvFile = SD.open(filename, FILE_READ);

  if (csvFile) {
    while (csvFile.available()) {
      String line = csvFile.readStringUntil('\n');
      int pos1, pos2, pos3, pos4;
      if (sscanf(line.c_str(), "%d,%d,%d,%d", &pos1, &pos2, &pos3, &pos4) == 4) {
        move0_1(pos1, pos2, pos3, pos4);
      }
      delay(1000);
    }

    csvFile.close();
    Serial.println("CSV file completed.");
  } else {
    Serial.println("Error opening CSV file.");
  }
}


// Function to home all stepper motors
void home() {
  Serial.println("Homing steppers...");

  // Home each stepper motor
  homeStepper1();
  homeStepper2();
  homeStepper3();
  homeStepper4();

  steppersEnabled = true; // Enable the steppers after homing
  Serial.println("Steppers homed.");
}

// Function to move the steppers to the desired positions
void move0_1(int pos0_1, int pos0_2, int pos0_3, int pos0_4) {
  if (!steppersEnabled) {
    Serial.println("Steppers are disabled. Please home the steppers first.");
    return;
  }
  Serial.print("Moving steppers to positions: ");
  Serial.print(pos0_1);
  Serial.print(", ");
  Serial.print(pos0_2);
  Serial.print(", ");
  Serial.print(pos0_3);
  Serial.print(", ");
  Serial.println(pos0_4);

  // Move each stepper to the desired position
  stepper1.moveTo(-pos0_1);
  stepper2.moveTo(-pos0_2);
  stepper3.moveTo(pos0_3);
  stepper4.moveTo(-pos0_4);

  // Run the steppers to their target positions
  while (stepper1.isRunning() || stepper2.isRunning() || stepper3.isRunning() || stepper4.isRunning()) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
    stepper4.run();
  }

  Serial.println("Steppers moved to desired positions.");
}



// Function to move the steppers to the desired positions
void move1(int pos1_1, int pos1_2, int pos1_3, int pos1_4) {
  if (!steppersEnabled) {
    Serial.println("Steppers are disabled. Please home the steppers first.");
    return;
  }
  Serial.print("Moving steppers to positions: ");
  Serial.print(pos1_1);
  Serial.print(", ");
  Serial.print(pos1_2);
  Serial.print(", ");
  Serial.print(pos1_3);
  Serial.print(", ");
  Serial.println(pos1_4);

  // Move each stepper to the desired position
  stepper1.moveTo(-pos1_1);
  stepper2.moveTo(-pos1_2);
  stepper3.moveTo(pos1_3);
  stepper4.moveTo(-pos1_4);

  // Run the steppers to their target positions
  while (stepper1.isRunning() || stepper2.isRunning() || stepper3.isRunning() || stepper4.isRunning()) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
    stepper4.run();
  }

  Serial.println("Steppers moved to desired positions.");
}


void move2(int pos2_1, int pos2_2, int pos2_3, int pos2_4) {
  if (!steppersEnabled) {
    Serial.println("Steppers are disabled. Please home the steppers first.");
    return;
  }
  Serial.print("Moving steppers to positions: ");
  Serial.print(pos2_1);
  Serial.print(", ");
  Serial.print(pos2_2);
  Serial.print(", ");
  Serial.print(pos2_3);
  Serial.print(", ");
  Serial.println(pos2_4);

  // Move each stepper to the desired position
  stepper1.moveTo(-pos2_1);
  stepper2.moveTo(-pos2_2);
  stepper3.moveTo(pos2_3);
  stepper4.moveTo(-pos2_4);

  // Run the steppers to their target positions
  while (stepper1.isRunning() || stepper2.isRunning() || stepper3.isRunning() || stepper4.isRunning()) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
    stepper4.run();
  }

  Serial.println("Steppers moved to desired positions.");
}



void move3(int pos3_1, int pos3_2, int pos3_3, int pos3_4) {
  if (!steppersEnabled) {
    Serial.println("Steppers are disabled. Please home the steppers first.");
    return;
  }
  Serial.print("Moving steppers to positions: ");
  Serial.print(pos3_1);
  Serial.print(", ");
  Serial.print(pos3_2);
  Serial.print(", ");
  Serial.print(pos3_3);
  Serial.print(", ");
  Serial.println(pos3_4);

  // Move each stepper to the desired position
  stepper1.moveTo(-pos3_1);
  stepper2.moveTo(-pos3_2);
  stepper3.moveTo(pos3_3);
  stepper4.moveTo(-pos3_4);

  // Run the steppers to their target positions
  while (stepper1.isRunning() || stepper2.isRunning() || stepper3.isRunning() || stepper4.isRunning()) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
    stepper4.run();
  }

  Serial.println("Steppers moved to desired positions.");
}

void move4(int pos4_1, int pos4_2, int pos4_3, int pos4_4) {
  if (!steppersEnabled) {
    Serial.println("Steppers are disabled. Please home the steppers first.");
    return;
  }
  Serial.print("Moving steppers to positions: ");
  Serial.print(pos4_1);
  Serial.print(", ");
  Serial.print(pos4_2);
  Serial.print(", ");
  Serial.print(pos4_3);
  Serial.print(", ");
  Serial.println(pos4_4);

  // Move each stepper to the desired position
  stepper1.moveTo(-pos4_1);
  stepper2.moveTo(-pos4_2);
  stepper3.moveTo(pos4_3);
  stepper4.moveTo(-pos4_4);

  // Run the steppers to their target positions
  while (stepper1.isRunning() || stepper2.isRunning() || stepper3.isRunning() || stepper4.isRunning()) {
    stepper1.run();
    stepper2.run();
    stepper3.run();
    stepper4.run();
  }

  Serial.println("Steppers moved to desired positions.");
}



// Function to move the servo motor to a specific position
void servoMove(int servoPosition) {
  Serial.print("Moving servo to position: ");
  Serial.println(servoPosition);

  // Move the servo motor to the desired position
  servoMotor.write(servoPosition);
}
// Function to save the current function to EEPROM (or other storage)
void saveCurrentFunction() {
  Serial.println("Saving current function...");
  // Implement the logic to save the current function (homed position and stepper positions) to EEPROM or another storage medium
  // Example: Save the homed position and the positions of each stepper motor to EEPROM addresses

  Serial.println("Current function saved.");
}

// Function to run the saved function
void runCurrentFunction() {
  for(int i=1;i<=2;i++){
  Serial.println("Running saved function...");

  int pos1_1 = 2200; // Example: Assign a value to pos1 (adjust as needed)
  int pos1_2 = 100; // Example: Assign a value to pos2 (adjust as needed)
  int pos1_3= 500;
  int pos1_4=2000;// Example: Assign a value to pos3 (adjust as needed)
  move1(pos1_1, pos1_2, pos1_3, pos1_4); // Call the movef1 function with the assigned positions
  
   int pos2_1 = 2200; // Example: Assign a value to pos1 (adjust as needed)
  int pos2_2 = 1000; // Example: Assign a value to pos2 (adjust as needed)
  int pos2_3= 500;
  int pos2_4=1000;// Example: Assign a value to pos3 (adjust as needed)
  move2(pos2_1, pos2_2, pos2_3, pos2_4); // Call the movef1 function with the assigned positions


   int pos3_1 = 300; // Example: Assign a value to pos1 (adjust as needed)
  int pos3_2 = 100; // Example: Assign a value to pos2 (adjust as needed)
  int pos3_3= 300;
  int pos3_4=1000;// Example: Assign a value to pos3 (adjust as needed)
  move3(pos3_1, pos3_2, pos3_3, pos3_4); // Call the movef1 function with the assigned positions

   int pos4_1 = 300; // Example: Assign a value to pos1 (adjust as needed)
  int pos4_2 = 500; // Example: Assign a value to pos2 (adjust as needed)
  int pos4_3= 300;
  int pos4_4=2500;// Example: Assign a value to pos3 (adjust as needed)
  move4(pos4_1, pos4_2, pos4_3, pos4_4); // Call the movef1 function with the assigned positions
  //delay(2000);
  

  
  }
  home();
  Serial.println("Saved function executed.");
}


void setStepperSpeed(int speed) {
  speed = constrain(speed, minSpeed, maxSpeed);
 

  stepper1.setMaxSpeed(speed);

  
  stepper2.setMaxSpeed(speed);
 
  
  stepper3.setMaxSpeed(speed);
 
  
  stepper4.setMaxSpeed(speed);
  
}

void setStepperAcceleration(int acceleration) {
  
  acceleration = constrain(acceleration, minAcceleration, maxAcceleration);

  
  stepper1.setAcceleration(acceleration);
  
 
  stepper2.setAcceleration(acceleration);
  
 
  stepper3.setAcceleration(acceleration);
  

  stepper4.setAcceleration(acceleration);
}
// Rest of the code...
int servoPosition = 90;

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

    if (input.startsWith("home")) {
      home();
    } else if (input.startsWith("move")) {
      int pos0_1, pos0_2, pos0_3, pos0_4;
      sscanf(input.c_str(), "move:%d,%d,%d,%d", &pos0_1, &pos0_2, &pos0_3, &pos0_4);
      move0_1(pos0_1, pos0_2, pos0_3, pos0_4);
    } else if (input.startsWith("loop:")) {
      String loopCommand = input.substring(5);
      if (loopCommand == "infinity") {
        infiniteLoop = true;
      } else {
        repetitions = loopCommand.toInt();
        infiniteLoop = false;
      }
    } else if (input.startsWith("runCSV:")) {
      const char* filename = input.substring(7).c_str();
      runCSVFile(filename);
    } else if (input == "stop") {
      stopCSV = true; // Set the flag to stop CSV execution
    }else if (input.startsWith("speed")) {
      int speed;
      sscanf(input.c_str(), "speed:%d", &speed);

      setStepperSpeed(speed);
      Serial.println("Stepper speed updated.");
    }

    
    else if (input.startsWith("accee")) {
      int acceleration;
      sscanf(input.c_str(), "accee:%d", &acceleration);

      setStepperAcceleration(acceleration);
      Serial.println("Stepperacceleration updated.");
    }else if (input == "j1+") {
      stepper1.move(10); // Move stepper 1 position by +10
      stepper1.runToPosition();
    } else if (input == "j1-") {
      stepper1.move(-10); // Move stepper 1 position by -10
      stepper1.runToPosition();
    }  else if(input.startsWith("j1:")) {
      // Extract the position value as a string after "j1:"
      String positionString = input.substring(3);
      
      // Convert the position string to an integer
      int targetPosition = positionString.toInt();
      
      // Move the stepper motor to the specified position
      stepper1.moveTo(targetPosition);
      stepper1.runToPosition();
    } 
  

    else if (input == "j2+") {
       stepper2.move(10); // Move stepper 1 position by +10
      stepper2.runToPosition();
    } else if (input == "j2-") {
      stepper2.move(-10); // Move stepper 1 position by -10
      stepper2.runToPosition();
    } else if(input.startsWith("j2:")) {
      // Extract the position value as a string after "j1:"
      String positionString = input.substring(3);
      
      // Convert the position string to an integer
      int targetPosition = positionString.toInt();
      
      // Move the stepper motor to the specified position
      stepper1.moveTo(targetPosition);
      stepper1.runToPosition();
    } 
  

    else if (input == "j3+") {
      stepper3.move(10); // Move stepper 1 position by +10
      stepper3.runToPosition();
    } else if (input == "j3-") {
      stepper3.move(-10); // Move stepper 1 position by -10
      stepper3.runToPosition();
    }  else if(input.startsWith("j3:")) {
      // Extract the position value as a string after "j1:"
      String positionString = input.substring(3);
      
      // Convert the position string to an integer
      int targetPosition = positionString.toInt();
      
      // Move the stepper motor to the specified position
      stepper1.moveTo(targetPosition);
      stepper1.runToPosition();
    } 
  

    else if (input == "j4+") {
      stepper4.move(10); // Move stepper 1 position by +10
      stepper4.runToPosition();
    } else if (input == "j4-") {
      stepper4.move(-10); // Move stepper 1 position by -10
      stepper4.runToPosition();
    }  else if(input.startsWith("j4:")) {
      // Extract the position value as a string after "j1:"
      String positionString = input.substring(3);
      
      // Convert the position string to an integer
      int targetPosition = positionString.toInt();
      
      // Move the stepper motor to the specified position
      stepper1.moveTo(targetPosition);
      stepper1.runToPosition();
    }
    
   else if (input == "g+") {
    servoPosition += 10; // Increment the servo position by 10 units
    servoMove(servoPosition); // Move the servo to the new position
} else if (input == "g-") {
    servoPosition -= 10; // Decrement the servo position by 10 units
    servoMove(servoPosition); // Move the servo to the new position
}



  
  }
  if (stopCSV) {
    stopCSV = false;
}
}