//AccelStepperSendCommandMoveComplete
#include <AccelStepper.h>

long receivedSteps = 0; //Number of steps
long receivedSpeed = 0; //Steps / second
long receivedMaxSpeed = 0; //Steps / second
long receivedAcceleration = 0; //Steps / second^2
long CurrentPosition = 0;
char receivedCommand; //a letter sent from the terminal
long StartTime = 0;
long PreviousTime = 0;
//-------------------------------------------------------------------------------
bool newData, runallowed = false; // booleans for new data from serial, and runallowed flag
bool lastStepPosition = false; //follows the steps to see if the last step was preformed
bool pingpong_CW = true;
bool pingpong_CCW = true;
bool pingpongAllowed = false;
int pingpongTimes = 10;


AccelStepper stepper(AccelStepper::DRIVER, 4, 5);

void setup() {
  Serial.begin(115200);
  Serial.println("Demonstration of AccelStepper Library"); //print a messages
  Serial.println("Send 'C' for printing the commands.");

  //setting up some default values for maximum speed and maximum acceleration
  Serial.println("Default speed: 400 steps/s, default acceleration: 800 steps/s^2.");
  
  stepper.setMaxSpeed(400); //SPEED = Steps / second
  stepper.setAcceleration(800); //ACCELERATION = Steps /(second)^2
  //tempstepper.disableOutputs(); //disable outputs
  StartTime = millis(); //start the timer

  stepper.enableOutputs();
}

void loop() 
{
checkSerial();
  RunTheMotor(); //function to handle the motor   
  SendPosition();
  PingPong();
}


void RunTheMotor() //function for the motor
{
  if (stepper.distanceToGo() != 0)
  {
    //tempstepper.enableOutputs(); //enable pins
    stepper.run(); //step the motor (this will step the motor by 1 step at each loop) 
    lastStepPosition = true;
  }
  else //program enters this part if the runallowed is FALSE, we do not do anything
  {

     //tempstepper.disableOutputs(); //disable outputs
     if(lastStepPosition == true)
     {
      Serial.print("L");
      Serial.println(stepper.currentPosition());//Print the message 
      //Serial.print("V"); //You can do the same with speed, but it slows down the arduino
      //Serial.println(stepper.speed());//Print the message 
      lastStepPosition = false;
     }
    return;
  }
}

void SendPosition()
{
  if (stepper.distanceToGo() != 0)
  {    
    //The larger this number (300) the better. Multiple Serial.println interferes with the stepper motor
    if((millis()-StartTime) >= 400) 
    {
      StartTime = millis();            
      Serial.print("L");
      Serial.println(stepper.currentPosition());//Print the message 
      //Serial.print("V"); //Alternatively, we can print speed too, but it can interfere with the motor at high speeds
      //Serial.println(stepper.speed());//Print the message 
    }    
  }
  else
  {
  // skip
  }  
}

void checkSerial() //function for receiving the commands
{ 
  if (Serial.available() > 0) //if something comes from the computer
  {
    receivedCommand = Serial.read(); // pass the value to the receivedCommad variable
    newData = true; //indicate that there is a new data by setting this bool to true

    if (newData == true) //we only enter this long switch-case statement if there is a new command from the computer
    {
      switch (receivedCommand) //we check what is the command
      {
        case 'P': //P uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.   
        receivedSteps = Serial.parseFloat(); //value for the steps
        receivedSpeed = Serial.parseFloat(); //value for the speed
        Serial.print("received Steps = ");
        Serial.println(receivedSteps);
        Serial.print("received Speed = ");
        Serial.println(receivedSpeed);
        Serial.println("Rotate Relative."); //print the action
        RotateRelative(); //Run the function
        //example: P2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 400 steps/s speed
        //In theory, this movement should take 5 seconds
        break;

        case 'R': //R uses the moveTo() function of the AccelStepper library, which means that it moves absolutely to the current position.     
        receivedSteps = Serial.parseFloat(); //value for the steps
        receivedSpeed = Serial.parseFloat(); //value for the speed   
        Serial.print("received Steps = ");
        Serial.println(receivedSteps);
        Serial.print("received Speed = ");
        Serial.println(receivedSpeed);
        Serial.println("Rotate Absolute.");
        RotateAbsolute(); //Run the function
        //example: R800 400 - It moves to the position which is located at +800 steps away from 0.
        break;

        case 'S': // Stops the motor
        stepper.stop(); //stop motor
        //tempstepper.disableOutputs(); //disable power
        Serial.println("Stopped."); //print action
        runallowed = false; //disable running
        pingpongAllowed = false; //disable pingpong
        break;

        case 'A': // Updates acceleration        
        //runallowed = false; //we still keep running disabled, since we just update a variable
        ////tempstepper.disableOutputs(); //disable power
        receivedAcceleration = Serial.parseFloat(); //receive the acceleration from serial
        stepper.setAcceleration(receivedAcceleration); //update the value of the variable
        Serial.print("New acceleration value: "); //confirm update by message
        Serial.println(receivedAcceleration); //confirm update by message
        break;





        

        
        case 'V': // Updates speed
        //runallowed = false; //we still keep running disabled, since we just update a variable
        ////tempstepper.disableOutputs(); //disable power
        receivedSpeed = Serial.parseFloat(); //receive the acceleration from serial
        stepper.setSpeed(receivedSpeed); //update the value of the variable
        //Serial.print("New speed value: "); //confirm update by message
        //Serial.println(receivedSpeed); //confirm update by message
        break;
        
        case 'v': // Updates Max speed
        //runallowed = false; //we still keep running disabled, since we just update a variable
        ////tempstepper.disableOutputs(); //disable power
        receivedMaxSpeed = Serial.parseFloat(); //receive the acceleration from serial
        stepper.setMaxSpeed(receivedMaxSpeed); //update the value of the variable
        //Serial.print("New Max speed value: "); //confirm update by message
        //Serial.println(receivedMaxSpeed); //confirm update by message
        break;
        
        case 'L': //L: Location
        runallowed = false; //we still keep running disabled
        //tempstepper.disableOutputs(); //disable power
        Serial.print("L");//Print the message
        Serial.println(stepper.currentPosition()); //Printing the current position in steps.
        break;
        
        case 'U':
        runallowed = false; //we still keep running disabled
        //tempstepper.disableOutputs(); //disable power
        stepper.setCurrentPosition(0); //Reset current position. "new home"       
        stepper.setSpeed(receivedSpeed); //We have to reupdate this, because the above function resets it.
        Serial.print("L"); //Print message
        Serial.println(stepper.currentPosition()); //Check position after reset.
        break;
        
        case 'C':
        PrintCommands(); //Print the commands for controlling the motor
        break;
        
        case 'K':
        runallowed = true;
        receivedSteps = Serial.parseFloat(); //value for the steps
        receivedSpeed = Serial.parseFloat(); //value for the speed 
        pingpongTimes = Serial.parseFloat();
        //tempstepper.enableOutputs(); //enable pins
        stepper.setMaxSpeed(receivedSpeed);
        stepper.setAcceleration(receivedSpeed*2);
        Serial.println("PingPong");
        pingpong_CW = false;
        pingpong_CCW = false;
        pingpongAllowed = true;
        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.
   
  }
}


void PingPong()
{       
    if(pingpongAllowed == true) //If the pingpong function is allowed we enter
    {
      if(pingpong_CW == false) //CW rotation is not yet done
      {  
        stepper.moveTo(receivedSteps); //set a target position, it should be an absolute. relative (move()) leads to "infinite loop"
        
        if(stepper.distanceToGo() == 0) //When the above number of steps are completed, we manipulate the variables
        {
            pingpong_CW = true; //CW rotation is now done
            pingpong_CCW = false; //CCW rotation is not yet done - this allows the code to enter the next ifs
                pingpongTimes--;
                Serial.print("ping....");
                Serial.print("L");//Print the message
                Serial.println(stepper.currentPosition()); //Printing the current position in steps.
                //Serial.println(stepper.currentPosition());
                delay(2000);
        }      
      }
        
      if(pingpong_CW == true && pingpong_CCW == false) //CW is completed and CCW is not yet done
      {
        stepper.moveTo(0); //Absolute position 
        
        if(stepper.distanceToGo() == 0) //When the number of steps are completed
          {
            pingpong_CCW = true; //CCW is now done
            pingpong_CW = false; //CW is not yet done. This allows the code to enter the first if again!
                pingpongTimes--;
                Serial.print("pong....");
                Serial.print("L");//Print the message
                Serial.println(stepper.currentPosition()); //Printing the current position in steps.
                //Serial.println(stepper.currentPosition());
                delay(2000);
          }     
      }
    }


    if(pingpongTimes == 0)
    {
        stepper.stop(); //stop motor
        Serial.println("pingpongTimes Ended."); //print action
        runallowed = false; //disable running
        pingpongAllowed = false; //disable pingpong
        pingpongTimes = 10;
    }
}

void RotateRelative()
{
  //We move X steps from the current position of the stepper motor in a given direction (+/-).  
  runallowed = true; //allow running - this allows entering the RunTheMotor() function.
  stepper.setMaxSpeed(receivedSpeed); //set speed 
  stepper.move(receivedSteps); //set relative distance and direction
}

void RotateAbsolute()
{
  //We move to an absolute position. 
  //The AccelStepper library keeps track of the position.
  runallowed = true; //allow running - this allows entering the RunTheMotor() function.
  stepper.setMaxSpeed(receivedSpeed); //set speed
  stepper.moveTo(receivedSteps); //set relative distance  
}

void PrintCommands()
{ 
  //Printing the commands
  Serial.println(" 'C' : Prints all the commands and their functions.");
  Serial.println(" 'P' : Rotates the motor - relative using move().");
  Serial.println(" 'R' : Rotates the motor - absolute using moveTo().");
  Serial.println(" 'S' : Stops the motor immediately.");
  Serial.println(" 'A' : Sets an acceleration value.");
  Serial.println(" 'V' : Sets a speed value using setSpeed().");
  Serial.println(" 'v' : Sets a speed value using setMaxSpeed().");
  Serial.println(" 'L' : Prints the current position/location of the motor using currentPosition().");
  Serial.println(" 'U' : Updates the current position and makes it as the new 0 position using setCurrentPosition().");
  Serial.println(" 'K' : Demonstrates an oscillating motion.");
}