#include <AccelStepper.h>

// Define the pins:
// Stepper 0
#define STEP_PIN0 2
#define DIR_PIN0 30
#define ENABLE_PIN0 31
#define SENSOR_PIN0 32

// Stepper 1
#define STEP_PIN1 3
#define DIR_PIN1 33
#define ENABLE_PIN1 34
#define SENSOR_PIN1 35

// Stepper 2
#define STEP_PIN2 4
#define DIR_PIN2 36
#define ENABLE_PIN2 37
#define SENSOR_PIN2 38

// Stepper 3
#define STEP_PIN3 5
#define DIR_PIN3 39
#define ENABLE_PIN3 40
#define SENSOR_PIN3 41



// Define the steps per revolution of thr stepper motor
const float STEPS_PER_REVOLUTION = 200.0;
float i = 12;  //Gear Transmission

// Create myStepper Class and inherit AccelStepper
class myStepper : public AccelStepper {
public:
  myStepper(uint8_t interface = AccelStepper::FULL4WIRE, uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t sensorPin = 6, uint8_t enablePin = 7)
    : AccelStepper(interface, pin1, pin2, 9, 10, true), sensorPin(sensorPin), enablePin(enablePin) {}

  void rotateStepper(float degrees) {
    // Calculate the number of steps based on the desired angle
    float steps = degrees * (STEPS_PER_REVOLUTION / 360.0);

    // Move the stepper to the specified number of steps
    move(steps);

    // Disable the motor
    digitalWrite(enablePin, LOW);

    // Run the stepper until it reaches the target position
    while (distanceToGo() != 0) {
      run();
    }
    Serial.println(distanceToGo());

    // Enable the motor
    digitalWrite(enablePin, LOW);
  }

  void calibrateStepper() {
    while (digitalRead(sensorPin) == HIGH) {
      rotateStepper(1);
      delay(1);
    }
  }

private:
  int sensorPin;
  int enablePin;
};

myStepper stepper0(AccelStepper::DRIVER, STEP_PIN0, DIR_PIN0, SENSOR_PIN0, ENABLE_PIN0);
myStepper stepper1(AccelStepper::DRIVER, STEP_PIN1, DIR_PIN1, SENSOR_PIN1, ENABLE_PIN1);
myStepper stepper2(AccelStepper::DRIVER, STEP_PIN2, DIR_PIN2, SENSOR_PIN2, ENABLE_PIN2);
myStepper stepper3(AccelStepper::DRIVER, STEP_PIN3, DIR_PIN3, SENSOR_PIN3, ENABLE_PIN3);


const byte numChars = 32;
char receivedChars[numChars];
char tempChars[numChars];  // temporary array for use when parsing

// variables to hold the parsed data
float moveToAngels[4] = { 0, 0, 0, 0 };
int calibrateArm = 0;

boolean newData = false;

//============

void setup() {

  Serial.begin(9600);

  // Set the maximum speed and acceleration (you can adjust these values)
  stepper0.setMaxSpeed(400);
  stepper0.setAcceleration(400.0);

  stepper1.setMaxSpeed(400);
  stepper1.setAcceleration(400.0);

  stepper2.setMaxSpeed(400);
  stepper2.setAcceleration(400.0);

  stepper3.setMaxSpeed(400);
  stepper3.setAcceleration(400.0);

  // Set the enable pin as an output and disable the motor by default
  pinMode(ENABLE_PIN0, OUTPUT);
  digitalWrite(ENABLE_PIN0, LOW);
  pinMode(SENSOR_PIN0, INPUT);

  pinMode(ENABLE_PIN1, OUTPUT);
  digitalWrite(ENABLE_PIN1, LOW);
  pinMode(SENSOR_PIN1, INPUT);


  pinMode(ENABLE_PIN2, OUTPUT);
  digitalWrite(ENABLE_PIN2, LOW);
  pinMode(SENSOR_PIN2, INPUT);


  pinMode(ENABLE_PIN3, OUTPUT);
  digitalWrite(ENABLE_PIN3, LOW);
  pinMode(SENSOR_PIN3, INPUT);


  // Set the motor direction (0 for clockwise, 1 for counter-clockwise)
  stepper0.setPinsInverted(false, false, true);
  stepper1.setPinsInverted(false, false, true);
  stepper2.setPinsInverted(false, false, true);
  stepper3.setPinsInverted(false, false, true);
}

//============

void loop() {

  recvWithStartEndMarkers();
  if (newData == true) {
    strcpy(tempChars, receivedChars);
    // this temporary copy is necessary to protect the original data
    //   because strtok() used in parseData() replaces the commas with \0
    parseData();
    showParsedData();
    newData = false;

    //check received values
    Serial.println(moveToAngels[3]);
    if (calibrateArm == 1) {

      stepper0.calibrateStepper();
      stepper1.calibrateStepper();
      stepper2.calibrateStepper();
      stepper3.calibrateStepper();

    } else {
      stepper0.rotateStepper(moveToAngels[0] * i);
      stepper1.rotateStepper(moveToAngels[1] * i);
      stepper2.rotateStepper(moveToAngels[2] * i);
      stepper3.rotateStepper(moveToAngels[3] * i);
    }

    //reset recieved values
    moveToAngels[0] = 0;
    moveToAngels[1] = 0;
    moveToAngels[2] = 0;
    moveToAngels[3] = 0;
    calibrateArm = 0;
  }
}

//============

void recvWithStartEndMarkers() {
  static boolean recvInProgress = false;
  static byte ndx = 0;
  char startMarker = '<';
  char endMarker = '>';
  char rc;

  while (Serial.available() > 0 && newData == false) {
    rc = Serial.read();
    Serial.print(rc);

    if (recvInProgress == true) {
      if (rc != endMarker) {
        receivedChars[ndx] = rc;
        ndx++;
        if (ndx >= numChars) {
          ndx = numChars - 1;
        }
      } else {
        receivedChars[ndx] = '\0';  // terminate the string
        recvInProgress = false;
        ndx = 0;
        newData = true;
      }
    }

    else if (rc == startMarker) {
      recvInProgress = true;
    }
  }
}

//============

void parseData() {  // split the data into its parts

  char* strtokIndx;  // this is used by strtok() as an index

  strtokIndx = strtok(tempChars, ",");  // get the first part
  calibrateArm = atoi(strtokIndx);      // convert this part to an integer

  strtokIndx = strtok(NULL, ",");  // this continues where the previous call left off
  moveToAngels[0] = atoi(strtokIndx);

  strtokIndx = strtok(NULL, ",");
  moveToAngels[1] = atoi(strtokIndx);  // convert this part to a float

  strtokIndx = strtok(NULL, ",");
  moveToAngels[2] = atof(strtokIndx);  // convert this part to a float

  strtokIndx = strtok(NULL, ",");
  moveToAngels[3] = atof(strtokIndx);  // convert this part to a float
}


//============

// For debugging purposes
void showParsedData() {
  Serial.print("Calibration:  ");
  Serial.println(calibrateArm);
  Serial.print("Stepper1 ");
  Serial.println(moveToAngels[0]);
  Serial.print("Stepper2 ");
  Serial.println(moveToAngels[1]);
  Serial.print("Stepper3 ");
  Serial.println(moveToAngels[2]);
  Serial.print("Stepper4 ");
  Serial.println(moveToAngels[3]);
}