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

#define limitSwitch1 3
#define limitSwitch2 18
#define limitSwitch3 14
// #define limitSwitch4 19
// #define limitSwitch5 19

#define ENx 38
#define ENy 56
#define ENz 62
#define EN0 24
#define EN1 30
#define EN2 4

// Define the stepper motors and the pins the will use
// (Type:driver, STEP, DIR)
AccelStepper stepper1(1, 54, 55); 
AccelStepper stepper2(1, 60, 61);
AccelStepper stepper3(1, 46, 48);
AccelStepper stepper4(1, 26, 28);
AccelStepper stepper5(1, 36, 34);
AccelStepper stepper6(1, 5, 6);

// Servo gripperServo;  // create servo object to control a servo


double x = 10.0;
double y = 10.0;
double L1 = 228; // L1 = 228mm
double L2 = 136.5; // L2 = 136.5mm
double theta1, theta2, phi, z;

int stepper1Position, stepper2Position, stepper3Position, stepper4Position, stepper5Position, stepper6Position;

const float theta1AngleToSteps = 44.444444;
const float theta2AngleToSteps = 35.555555;
const float phiAngleToSteps = 10;
const float zDistanceToSteps = 100;
const float railDistanceToSteps = 100;
const float convDistanceToSteps = 100;
const int timeSlice = 3000;

byte inputValue[5];
int k = 0;

String content = "";
int data[12] = {0};

int theta1Array[50];
int theta2Array[50];
int phiArray[50];
int zArray[50];
int gripperArray[50];
int delays[50];
int railArray[50];
int convArray[50];
int positionsCounter = 0;

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

  // for(int i = 0 ; i < 11 ; i++)
  // {
  //   Serial.println(data[i]);
  // }

  pinMode(limitSwitch1, INPUT_PULLUP);
  pinMode(limitSwitch2, INPUT_PULLUP);
  pinMode(limitSwitch3, INPUT_PULLUP);
  // pinMode(limitSwitch4, INPUT_PULLUP);
  // pinMode(limitSwitch5, INPUT_PULLUP);

  pinMode(ENx, OUTPUT);
  pinMode(ENy, OUTPUT);
  pinMode(ENz, OUTPUT);
  pinMode(EN0, OUTPUT);
  pinMode(EN1, OUTPUT);
  pinMode(EN2, OUTPUT);
  digitalWrite(ENx, LOW); 
  digitalWrite(ENy, LOW); 
  digitalWrite(ENz, LOW); 
  digitalWrite(EN0, LOW); 
  digitalWrite(EN1, LOW);
  digitalWrite(EN2, LOW);

  // Stepper motors max speed
  stepper1.setMaxSpeed(4000);
  stepper1.setAcceleration(2000);
  stepper2.setMaxSpeed(4000);
  stepper2.setAcceleration(2000);
  stepper3.setMaxSpeed(4000);
  stepper3.setAcceleration(2000);
  stepper4.setMaxSpeed(4000);
  stepper4.setAcceleration(2000);
  stepper5.setMaxSpeed(4000);
  stepper5.setAcceleration(2000);
  stepper6.setMaxSpeed(4000);
  stepper6.setAcceleration(2000);

  // gripperServo.attach(A15, 600, 2500);
  // initial servo value - open gripper
  // data[6] = 180;
  // gripperServo.write(data[6]);
  delay(1000);
  Serial.println("Hello");
  for(int i = 0 ; i < 50 ; i++)
  {
    delays[i] = 0;
  }
  homing();
}

void loop() {
  if (Serial.available()) {
    int startTime = millis();
    content = Serial.readString(); // Read the incomding data from Processing
    // Extract the data from the string and put into separate integer variables (data[] array)
    for (int i = 0; i < 11; i++) {
      int index = content.indexOf(","); // locate the first ","
      data[i] = atol(content.substring(0, index).c_str()); //Extract the number from start to the ","
      content = content.substring(index + 1); //Remove the number from the string
      Serial.println(i + ":" + data[i]);
    }
    /*
     data[0] - SAVE button status
     data[1] - RUN button status
     data[2] - Joint 1 angle
     data[3] - Joint 2 angle
     data[4] - Joint 3 angle
     data[5] - Z position
     data[6] - Gripper value
     data[7] - Speed value
     data[8] - Delay value
     data[9] - Rail value
     data[10] - Conv value
    //  data[8] - Acceleration value
    */
    // If SAVE button is pressed, store the data into the appropriate arrays
    if (data[0] == 1) {
      theta1Array[positionsCounter] = data[2] * theta1AngleToSteps; //store the values in steps = angles * angleToSteps variable
      theta2Array[positionsCounter] = data[3] * theta2AngleToSteps;
      phiArray[positionsCounter] = data[4] * phiAngleToSteps;
      zArray[positionsCounter] = data[5] * zDistanceToSteps;
      gripperArray[positionsCounter] = data[6];
      delays[positionsCounter] = data[8] * timeSlice;
      railArray[positionsCounter] = data[9] * railDistanceToSteps;
      convArray[positionsCounter] = data[10] * convDistanceToSteps;
      positionsCounter++;
      // Serial.println("Elements : ");
      // Serial.println(theta1Array[positionsCounter - 1]);
      // Serial.println(theta2Array[positionsCounter - 1]);
      // Serial.println(phiArray[positionsCounter - 1]);
      // Serial.println(zArray[positionsCounter - 1]);
      // Serial.println(delays[positionsCounter - 1]);
      // Serial.println("");
    }
    // clear data
    if (data[0] == 2) {
      // Clear the array data to 0
      memset(theta1Array, 0, sizeof(theta1Array));
      memset(theta2Array, 0, sizeof(theta2Array));
      memset(phiArray, 0, sizeof(phiArray));
      memset(zArray, 0, sizeof(zArray));
      memset(gripperArray, 0, sizeof(gripperArray));
      memset(railArray, 0, sizeof(railArray));
      memset(convArray, 0, sizeof(convArray));
      memset(delays, 0, sizeof(delays));
      positionsCounter = 0;
    }
  }
  // If RUN button is pressed
  while (data[1] == 1) {
    stepper1.setSpeed(data[7]);
    stepper2.setSpeed(data[7]);
    stepper3.setSpeed(data[7]);
    stepper4.setSpeed(data[7]);
    stepper5.setSpeed(data[7]);
    stepper6.setSpeed(data[7]);
    stepper1.setAcceleration(500);
    stepper2.setAcceleration(500);
    stepper3.setAcceleration(500);
    stepper4.setAcceleration(500);
    stepper5.setAcceleration(500);
    stepper6.setAcceleration(500);

    // execute the stored steps
    for (int i = 0; i <= positionsCounter - 1; i++) 
    {
      // Serial.println("position is : ");
      // Serial.println(theta1Array[i]);
      // Serial.println(theta2Array[i]);
      // Serial.println(phiArray[i]);
      // Serial.println(zArray[i]);
      // Serial.println(delays[i]);
      // Serial.println("");
      int time = delays[i];
      if (data[1] == 0) {
        break;
      }
      if(digitalRead(limitSwitch1) != 1)
        stepper1.moveTo(theta1Array[i]);
      if(digitalRead(limitSwitch2) != 1)
        stepper2.moveTo(theta2Array[i]);
      if(digitalRead(limitSwitch3) != 1)
        stepper3.moveTo(phiArray[i]);
        
        stepper4.moveTo(zArray[i]);
        stepper5.moveTo(railArray[i]);
        stepper6.moveTo(convArray[i]);
      while (stepper1.currentPosition() != theta1Array[i] || stepper2.currentPosition() != theta2Array[i] || stepper3.currentPosition() != phiArray[i] || stepper4.currentPosition() != zArray[i] || stepper5.currentPosition() != railArray[i] || stepper6.currentPosition() != convArray[i]) 
      {
        stepper1.run();
        stepper2.run();
        stepper3.run();
        stepper4.run();
        stepper5.run();
        stepper6.run();
      }
      // delay(delays[i] * 3000);
      // if (i == 0) {
      //   gripperServo.write(gripperArray[i]);
      // }
      // else if (gripperArray[i] != gripperArray[i - 1]) {
      //   gripperServo.write(gripperArray[i]);
      //   delay(800); // wait 0.8s for the servo to grab or drop - the servo is slow
      // }

      //check for change in speed and acceleration or program stop
      if (Serial.available()) {
        content = Serial.readString(); // Read the incomding data from Processing
        // Extract the data from the string and put into separate integer variables (data[] array)
        for (int i = 0; i < 11; i++) {
          int index = content.indexOf(","); // locate the first ","
          data[i] = atol(content.substring(0, index).c_str()); //Extract the number from start to the ","
          content = content.substring(index + 1); //Remove the number from the string
          Serial.println(i + ":" + data[i]);
        }

        if (data[1] == 0) {
          break;
        }
        // change speed and acceleration while running the program
        stepper1.setSpeed(data[7]);
        stepper2.setSpeed(data[7]);
        stepper3.setSpeed(data[7]);
        stepper4.setSpeed(data[7]);
        stepper5.setSpeed(data[7]);
        stepper6.setSpeed(data[7]);
        stepper1.setAcceleration(500);
        stepper2.setAcceleration(500);
        stepper3.setAcceleration(500);
        stepper4.setAcceleration(500);
        stepper5.setAcceleration(500);
        stepper6.setAcceleration(500);
      }
      delay(time);
    }
  
  }

  if(data[0] == 0)
  {
    stepper1Position = data[2] * theta1AngleToSteps;
    stepper2Position = data[3] * theta2AngleToSteps;
    stepper3Position = data[4] * phiAngleToSteps;
    stepper4Position = data[5] * zDistanceToSteps;
    stepper5Position = data[9] * railDistanceToSteps;
    stepper6Position = data[10] * convDistanceToSteps;

    stepper1.setSpeed(data[7]);
    stepper2.setSpeed(data[7]);
    stepper3.setSpeed(data[7]);
    stepper4.setSpeed(data[7]);
    stepper5.setSpeed(data[7]);
    stepper6.setSpeed(data[7]);

    stepper1.setAcceleration(500);
    stepper2.setAcceleration(500);
    stepper3.setAcceleration(500);
    stepper4.setAcceleration(500);
    stepper5.setAcceleration(500);
    stepper6.setAcceleration(500);

    stepper1.moveTo(stepper1Position);
    stepper2.moveTo(stepper2Position);
    stepper3.moveTo(stepper3Position);
    stepper4.moveTo(stepper4Position);
    stepper5.moveTo(stepper5Position);
    stepper6.moveTo(stepper6Position);

    while (stepper1.currentPosition() != stepper1Position || stepper2.currentPosition() != stepper2Position || stepper3.currentPosition() != stepper3Position || stepper4.currentPosition() != stepper4Position || stepper5.currentPosition() != stepper5Position || stepper6.currentPosition() != stepper6Position) {
      stepper1.run();
      stepper2.run();
      stepper3.run();
      stepper4.run();
      stepper5.run();
      stepper6.run();
    }
    delay(100);
    // gripperServo.write(data[6]);
    // delay(300);
  }
}

void serialFlush() {
  while (Serial.available() > 0) {  //while there are characters in the serial buffer, because Serial.available is >0
    Serial.read();         // get one character
  }
}

void homing() {

  // Homing Stepper5
  // while (digitalRead(limitSwitch5) != 1) {
  //   stepper5.setSpeed(-500);
  //   stepper5.runSpeed();
  //   stepper5.setCurrentPosition(-800); // When limit switch pressed set position to 0 steps
  // }
  // delay(20);

  // // Homing Stepper4
  // while (digitalRead(limitSwitch4) != 0) {
  //   stepper4.setSpeed(1500);
  //   stepper4.runSpeed();
  //   stepper4.setCurrentPosition(0); // When limit switch pressed set position to 0 steps
  // }
  // delay(20);
  // stepper4.moveTo(0);
  // while (stepper4.currentPosition() != 0) {
  //   stepper4.run();
  // }

  // Homing Stepper3
  while (digitalRead(limitSwitch3) != 1) {
    stepper3.setSpeed(-500);
    stepper3.runSpeed();
    stepper3.setCurrentPosition(-800); // When limit switch pressed set position to 0 steps
  }
  delay(20);

  stepper3.moveTo(-800);
  while (stepper3.currentPosition() != -800) {
    stepper3.run();
  }

  // Homing Stepper2
  while (digitalRead(limitSwitch2) != 1) {
    stepper2.setSpeed(500);
    stepper2.runSpeed();
    stepper2.setCurrentPosition(800); // When limit switch pressed set position to -5440 steps
  }
  delay(20);

  stepper2.moveTo(100);
  while (stepper2.currentPosition() != 200) {
    stepper2.run();
  }

  // Homing Stepper1
  while (digitalRead(limitSwitch1) != 1) {
    stepper1.setSpeed(500);
    stepper1.runSpeed();
    stepper1.setCurrentPosition(800); // When limit switch pressed set position to 0 steps
  }
  delay(20);
  stepper1.moveTo(100);
  while (stepper1.currentPosition() != 200) {
    stepper1.run();
  }
}