#include <AccelStepper.h>

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

// Stepper 1
#define STEP_PIN1 3
#define DIR_PIN1 32
#define ENABLE_PIN1 33

// Stepper 2
#define STEP_PIN2 4
#define DIR_PIN2 34
#define ENABLE_PIN2 35

// Stepper 3
#define STEP_PIN3 5
#define DIR_PIN3 36
#define ENABLE_PIN3 37



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

// Create myStepper Class and inherrt AccelStepper
class myStepper: public AccelStepper{
  public: 
  myStepper(uint8_t interface = AccelStepper::FULL4WIRE, uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t pin3 = 4, uint8_t pin4 = 5, bool enable = true)
    :AccelStepper(interface, pin1, pin2, pin3, pin4, enable){}

  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);

    // Enable the motor
    digitalWrite(ENABLE_PIN3, LOW);

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

    // Disable the motor
    //digitalWrite(ENABLE_PIN3, HIGH);
  }
};
myStepper stepper3(AccelStepper::DRIVER, STEP_PIN3, DIR_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 isCalibrated = 0;

boolean newData = false;

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

void setup() {
  Serial.begin(9600);
  
  // Set the maximum speed and acceleration (you can adjust these values)
  stepper3.setMaxSpeed(400);
  stepper3.setAcceleration(400.0);

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

  // Set the motor direction (0 for clockwise, 1 for counter-clockwise)
  stepper3.setPinsInverted(false, false, true);  // Adjust the last parameter if needed
}

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

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]);
    
    stepper3.rotateStepper(moveToAngels[3] * i);
    
    
    //reset recieved values
    moveToAngels[0] = 0;
    moveToAngels[1] = 0;
    moveToAngels[2] = 0;
    moveToAngels[3] = 0;
    isCalibrated = 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
  isCalibrated = 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
}

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

/* 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
  stepper.move(steps);

  // Enable the motor
  digitalWrite(ENABLE_PIN1, LOW);

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

  // Disable the motor
  //digitalWrite(ENABLE_PIN1, HIGH);
} 
**/

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

// For debugging purposes
void showParsedData() {
  Serial.print("Calibration:  ");
  Serial.println(isCalibrated);
  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]);
}