/*
Next steps: add in mooveDegrees function to change inputs from steps to degrees for input
*/

#include <AccelStepper.h>
#include <MultiStepper.h>


//Defining stepper objects for each motor
AccelStepper stepper1(1,2,3); 
AccelStepper stepper2(1,4,5);
AccelStepper stepper3(1,8,9);

MultiStepper steppersControl;  // Create instance of MultiStepper

int stepsPerFullTurn = 200*16; //How many steps are in a full turn

long gotoposition[3]; // An array to store the target positions for each stepper motor

char receivedCommand; //A character to determine what motor to turn
bool newData; //Boolean to determine if a new command has been received

void setup() {

  Serial.begin(9600);

  stepper1.setMaxSpeed(1000); // Set maximum speed value for the stepper
  stepper2.setMaxSpeed(1000);
  stepper3.setMaxSpeed(1000);

  // Adding the 3 steppers to the steppersControl instance for multi stepper control
  steppersControl.addStepper(stepper1);
  steppersControl.addStepper(stepper2);
  steppersControl.addStepper(stepper3);
}

//Converting movement in degrees to steps
double moveDegrees(double degrees){
  double stepPos = stepsPerFullTurn * degrees / 360.0 ;
  return stepPos;
}

void loop() {
  checkSerial(); //Check to see if data is available in the Serial


}

void checkSerial(){
  if(Serial.available()>0)                  //Check if there is actual data to read
  {
    receivedCommand = Serial.read();        //Read the data that has come in
    newData = true;                         //Indicate that there is a new data by setting this bool to true
    if (newData == true)                    //Only enter this long switch-case statement if there is a new command from the computer
    {
      switch (receivedCommand)              //Creates cases based on the value of received command (seems like a compact if statement)
      {
        case 'A': //Command for moving motor number 1 
          gotoposition[0]=Serial.parseFloat(); //Add in moveDegrees here at some point****
          Serial.println(gotoposition[0]);
          break;

        case 'B': //Command for moving motor number 2
          gotoposition[1]=Serial.parseFloat(); //Add in moveDegrees here at some point****
          Serial.println(gotoposition[1]);
          break;

        case 'C': //Command for moving motor number 3
          gotoposition[2]=Serial.parseFloat(); //Add in moveDegrees here at some point****
          Serial.println(gotoposition[2]);
          break;

        case 'G': //Run positions
            steppersControl.moveTo(gotoposition); //Caclulates the required speed for all motors
            steppersControl.runSpeedToPosition(); //Blocks until all steppers are in position

        default:
          break;
      }
    }
    newData=false;        //after we went through the above tasks, newData is set to false again, so we are ready to receive new commands again.
  }
}

/* Code for attempting to make the motors run unsynchronized 
   add below code to case 'G' section and comment out other code
      // stepper1.moveTo(gotoposition[0]);
      // stepper1.setSpeed(1000);
      // stepper2.moveTo(gotoposition[1]);
      // stepper2.setSpeed(1000);
      // stepper3.moveTo(gotoposition[2]);
      // stepper3.setSpeed(1000);
      // while(stepper1.currentPosition()!=gotoposition[0] || stepper2.currentPosition()!=gotoposition[1] || stepper3.currentPosition()!=gotoposition[2])
        // { //Using a while loop to see if they all still run at the same speed.
        //   stepper1.run();
        //   stepper2.run();
        //   stepper3.run();
        // }
*/

/* Code for attempting to remove blocking. Put back in case 'G' section.
            //Uncomment below and comment out runSpeedToPosition line to remove blocking 
            // while(stepper1.currentPosition()!=gotoposition[0] || stepper2.currentPosition()!=gotoposition[1] || stepper3.currentPosition()!=gotoposition[2])
            // { //Using a while loop to see if they all still run at the same speed.
            //   steppersControl.run();
            // }
*/


/* //Code from HowToMechatronics for reference
void loop() {
  // Store the target positions in the "gotopostion" array
  gotoposition[0] = 800;  // 800 steps - full rotation with quater-step resolution
  gotoposition[1] = 1600;
  gotoposition[2] = 3200;

  steppersControl.moveTo(gotoposition); // Calculates the required speed for all motors
  steppersControl.runSpeedToPosition(); // Blocks until all steppers are in position

  delay(1000);

  gotoposition[0] = 0;
  gotoposition[1] = 0;
  gotoposition[2] = 0;

  steppersControl.moveTo(gotoposition);
  steppersControl.runSpeedToPosition();

  delay(1000);
}
*///*************************

//Code for serial inputs to be modified

// void setup() {

//   Serial.begin(9600);

//   Serial.println("begin");

//   stepper1.setMaxSpeed(1000); // Set maximum speed value for the stepper
//   stepper1.setAcceleration(500); // Set acceleration value for the stepper
//   stepper1.setCurrentPosition(0); // Set the current position to 0 steps

//   stepper2.setMaxSpeed(1000);
//   stepper2.setAcceleration(500);
//   stepper2.setCurrentPosition(0);
// }

// void moveDegrees(double degrees){
//   double stepPos = stepsPerFullTurn * degrees / 360.0 ;
//   pos = stepPos;
// }

// void loop() {
//   checkSerial();
// }

// void checkSerial(){
//   if(Serial.available()>0){
//     double degrees = Serial.parseInt();
//     moveDegrees(degrees);
//     stepper1.moveTo(pos);
//     stepper2.moveTo(-pos);
//     while(stepper1.currentPosition()!=pos || stepper2.currentPosition()!=-pos){
//       stepper1.run();
//       stepper2.run();
//     }

//     Serial.println("After move:");
//     Serial.println(pos);
//   }
// }