//character for commands
/*
     'C' : Prints all the commands and their functions.
     'E' : Expand the X-motor in positive (CW) direction, absolute.
     'S' : Shrink the X-motor in negative (CCW) direction, absolute.
     'U' : drive up the Y-motor (cw) direction, absolute.
     'D' : drive down the Y-motor (CCW) direction, absolute.
     'R' : Rotates the motor to an absolute positive position (+).
     'r' : Rotates the motor to an absolute negative position (-).
     's' : Stops the motor immediately.
     'A' : Sets an acceleration value.
     'L' : Prints the current position/location of the motor.
     'H' : Goes back to 0 position from the current position (homing).
     'u' : Updates the position current position and makes it as the new 0 position.
 */

 #include <AccelStepper.h>
 
//User-defined values
long receivedSteps = 0; //Number of steps
long receivedSpeed = 0; //Steps / second
long receivedAcceleration = 0; //Steps / second^2
float receivedRotationAngle = 0.0; // desired angle of rotation
long received_n_Pic = 0; // desired numbre of taken Pictures
long receivedRadiusX = 0; // lengh of x-axis
char receivedCommand;


const int x_dirPin = 3;
const int x_stepPin = 2;
const int y_dirPin = 5;
const int y_stepPin = 4;
const int rotate_dirPin = 7;
const int rotate_stepPin = 6;

#define motorInterfaceType 1

int directionMultiplier = 1; // = 1: positive direction, = -1: negative direction
bool newData, runXallowed,runYallowed, rotate_allowed = false; // booleans for new data from serial, and runallowed flag
AccelStepper x_stepper(1,x_stepPin, x_dirPin);// direction Digital 3 (CCW), pulses Digital 2 (CLK)
AccelStepper y_stepper(1,y_stepPin, y_dirPin);
AccelStepper rotate_stepper(1,rotate_stepPin, rotate_dirPin);


void setup()
{
  Serial.begin(9600);
  Serial.println("Testing of stepper motors"); //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: 200 steps/s, default acceleration: 400 steps/s^2.");
  x_stepper.setMaxSpeed(200); //SPEED = Steps / second
  x_stepper.setAcceleration(400); //ACCELERATION = Steps /(second)^2
  x_stepper.disableOutputs(); //disable outputs
  y_stepper.setMaxSpeed(200); //SPEED = Steps / second
  y_stepper.setAcceleration(400); //ACCELERATION = Steps /(second)^2
  y_stepper.disableOutputs(); //disable outputs
  rotate_stepper.setAcceleration(400); //ACCELERATION = Steps /(second)^2
  rotate_stepper.disableOutputs(); //disable outputs
}

void loop()
{
  //Constantly looping through these 3 functions.
  //We only use non-blocking commands, so something else (should also be non-blocking) can be done during the movement of the motor
 
  checkSerial(); //check serial port for new commands
  RunThe_x_Motor(); //function to handle the x_motor
  RunThe_y_Motor(); //function to handle the y_motor
  RunThe_rotation_Motor(); //function to handle the rotation_motor

}

void RunThe_x_Motor() //function for the motor
{
    if (runXallowed == true)
    {
        x_stepper.enableOutputs(); //enable pins
        x_stepper.run(); //step the motor (this will step the motor by 1 step at each loop)  
    }
    else //program enters this part if the runallowed is FALSE, we do not do anything
    {
        x_stepper.disableOutputs(); //disable outputs
        return;
    }
}
void RunThe_y_Motor() //function for the motor
{
    if (runYallowed == true)
    {
        y_stepper.enableOutputs(); //enable pins
        y_stepper.run(); //step the motor (this will step the motor by 1 step at each loop)  
    }
    else //program enters this part if the runallowed is FALSE, we do not do anything
    {
        y_stepper.disableOutputs(); //disable outputs
        return;
    }
}
void RunThe_rotation_Motor() //function for the motor
{
    if (rotate_allowed == true)
    {
        rotate_stepper.enableOutputs(); //enable pins
        rotate_stepper.run(); //step the motor (this will step the motor by 1 step at each loop)  
    }
    else //program enters this part if the runallowed is FALSE, we do not do anything
    {
        rotate_stepper.disableOutputs(); //disable outputs
        return;
    }
}

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 'E': //E 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
            if(receivedSteps < 50)
            {
               Serial.println("please entre a numbre > 50 cm");
                x_stepper.stop();
                x_stepper.disableOutputs();
            }
            else 
            {
              directionMultiplier = 1; //We define the direction
              Serial.print("expanding to ");
              Serial.print(receivedSteps);
              Serial.println(" cm"); //print the action
              Rotate_x_Ab(); //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 'S': //S uses the move() function of the AccelStepper library, which means that it moves relatively to the current position.      
            if(receivedSteps < 50)
            {
              Serial.println("this movement isn't allowed!!");
              x_stepper.stop();
              x_stepper.disableOutputs();
            } 
            else
            {
              receivedSteps = Serial.parseFloat(); //value for the steps
              receivedSpeed = Serial.parseFloat(); //value for the speed 
              directionMultiplier = -1; //We define the direction
              Serial.print("Shrinking to ");
              Serial.print(receivedSteps);
              Serial.println(" cm"); //print action
              NegativRotate_x_Ab(); //Run the function
            } 
                 //example: N2000 400 - 2000 steps (5 revolution with 400 step/rev microstepping) and 500 steps/s speed; will rotate in the other direction
                //In theory, this movement should take 5 seconds
            break;
          
          case 'U':

            receivedSteps = Serial.parseFloat();
            receivedSpeed = Serial.parseFloat();
            if (receivedSteps <= 40)
            {
                Serial.println("please entre a numbre > 40 cm");
                y_stepper.stop();
                y_stepper.disableOutputs();
            }
            else 
            {
              directionMultiplier = 1;
              Serial.print("driving up to ");
              Serial.print(receivedSteps);
              Serial.println(" cm");
              Rotate_y_Ab();
            }

            break;
          
          case 'D':
            if(receivedSteps <= 40)
            {
              Serial.println("this movement isn't allowed!!");
              y_stepper.stop();
              y_stepper.disableOutputs();
            }
            else
            {
              receivedSteps = Serial.parseFloat();
              receivedSpeed = Serial.parseFloat();
              directionMultiplier = -1;
              Serial.print("driving down to ");
              Serial.print(receivedSteps);
              Serial.println(" cm");
              NegativRotate_y_Ab();
            }
            
            break;
            
 
          case 'R': //R uses the moveTo() function of the AccelStepper library, 
                    //which means that it rotates the structure in the posotive 
                    //direction around the Objekt. 
            Serial.println("Example: R + # pics + angle + radius ");                   
            received_n_Pic = Serial.parseInt();
            receivedRotationAngle = Serial.parseFloat();
            receivedRadiusX = Serial.parseFloat();
            directionMultiplier = 1;
            rotate_z_positive();
            
            /*receivedSteps = Serial.parseFloat(); //value for the steps
            receivedSpeed = Serial.parseFloat(); //value for the speed     
            directionMultiplier = 1; //We define the direction
            Serial.println("Absolute position (+)."); //print the action
            RotateAbsolute(); //Run the function*/
 
                //example: R800 400 - It moves to the position which is located at +800 steps away from 0.
            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 
            directionMultiplier = -1; //We define the direction
            Serial.println("Absolute position (-)."); //print the action
            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
               
            x_stepper.stop(); //stop motor
            y_stepper.stop();
            x_stepper.disableOutputs(); //disable power
            y_stepper.disableOutputs();
            Serial.println("Stopped."); //print action
            runXallowed = false;
            runYallowed = false; //disable running
            break;
 
          case 'A': // Updates acceleration
 
            runXallowed = false; //we still keep running disabled, since we just update a variable
            x_stepper.disableOutputs(); //disable power
            receivedAcceleration = Serial.parseFloat(); //receive the acceleration from serial
            x_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 'L': //L: Location
 
            runXallowed = false; //we still keep running disabled
            x_stepper.disableOutputs(); 
            runYallowed = false; //we still keep running disabled
            y_stepper.disableOutputs();//disable power
            Serial.print("Current location of the X motor: steps: ");//Print the message
            Serial.print(x_stepper.currentPosition()); //Printing the current position in steps.
            Serial.print(", position in cm: ");
            Serial.println((x_stepper.currentPosition()/1000)+50);
            Serial.print("Current location of the y motor: steps: ");
            Serial.print(y_stepper.currentPosition());
            Serial.print(", poisition in cm: ");
            Serial.println((y_stepper.currentPosition()/2000)+40);
            break;
               
          case 'H': //H: Homing
 
            runXallowed = true;
            runYallowed = true;    
            Serial.println("Homing"); //Print the message
            GoHome();// Run the function
            break;
 
          case 'u':
 
            runXallowed = false;
            runYallowed = false; //we still keep running disabled
            x_stepper.disableOutputs(); //disable power
            x_stepper.setCurrentPosition(0); //Reset current position. "new home"   
            y_stepper.disableOutputs(); //disable power
            y_stepper.setCurrentPosition(0); //Reset current position. "new home"          
            Serial.print("The current position is updated to: "); //Print message
            Serial.println(x_stepper.currentPosition()); //Check position after reset.
            Serial.println(y_stepper.currentPosition());
            break; 
 
          case 'C':
 
            PrintCommands(); //Print the commands for controlling the motor
            break;  
 
          default:  

            break;
        }
      }
        //after we went through the above tasks, newData is set to false again, so we are ready to receive new commands again.
    newData = false;       
  }
 
 
} 
void GoHome()
{  
    if (x_stepper.currentPosition() == 0 && y_stepper.currentPosition() == 0)
    {
        Serial.println("We are at the home position.");
        x_stepper.disableOutputs();
        y_stepper.disableOutputs(); //disable power
    }
    else
    {
        x_stepper.setMaxSpeed(200);
        y_stepper.setMaxSpeed(200); //set speed manually to 400. In this project 400 is 400 step/sec = 1 rev/sec.
        x_stepper.moveTo(0); 
        y_stepper.moveTo(0);//set abolute distance to move
    }
}
 
void Rotate_x_Ab()
{
  //We move X steps from the current position of the stepper motor in a given direction.
  //The direction is determined by the multiplier (+1 or -1)
   
  runXallowed = true; //allow running - this allows entering the RunTheMotor() function.
  x_stepper.setMaxSpeed(receivedSpeed); //set speed
  x_stepper.move(directionMultiplier * ((receivedSteps-50)*1000)-x_stepper.currentPosition()); //set relative distance and direction
    
}

void NegativRotate_x_Ab()
{
  float sum;
  runXallowed =true;
  x_stepper.setMaxSpeed(receivedSpeed);
  sum = directionMultiplier*(receivedSteps-50)*1000;
  float endsum = -1*(x_stepper.currentPosition() + sum);
  Serial.println(x_stepper.currentPosition());
  Serial.println(sum);
  Serial.println(endsum);
  if(sum <= 0)
  {
    //x_stepper.move(-1*(x_stepper.currentPosition()-(x_stepper.currentPosition() + sum)));
    x_stepper.move(endsum);
  }
  else
  {
    Serial.println("this movement isn't allowed!!");
    x_stepper.stop(); //stop motor 
    x_stepper.disableOutputs();
  }
  //x_stepper.move((directionMultiplier*x_stepper.currentPosition())+((receivedSteps-50)*1000));
  //x_stepper.move(directionMultiplier * (receivedSteps-50)*1000);
}

void Rotate_y_Ab()
{
  runYallowed =true;
  y_stepper.setMaxSpeed(receivedSpeed);
  y_stepper.move(directionMultiplier*((receivedSteps-40)*2000)-y_stepper.currentPosition());
}

void NegativRotate_y_Ab()
{
  runYallowed = true;
  float sum = directionMultiplier*(receivedSteps-40)*2000;
  float endsum = -1 *(y_stepper.currentPosition()+sum);
  y_stepper.setMaxSpeed(receivedSpeed);
  //y_stepper.move((directionMultiplier*y_stepper.currentPosition())+((receivedSteps-40)*2000));
  Serial.println(y_stepper.currentPosition());
  Serial.println(sum);
  Serial.println(endsum);
  if(sum <= 0)
  {
    y_stepper.move(endsum);
  }
  else
  {
    Serial.println("this movement isn't allowed!!");
    y_stepper.stop(); //stop motor 
    y_stepper.disableOutputs();
  }
}

float degToRad(float number)
{
  float division = 3.14/180;
  return number*division;
}

float rotate_angle(long n_pics, float angle)
{
  float result = angle/n_pics;
  return result;
}

void rotate_z_positive()
{
  rotate_allowed = true;
  rotate_stepper.setMaxSpeed(100);

  float angleToMove = rotate_angle(received_n_Pic, receivedRotationAngle);
  //Serial.print("angle to move: ");
  //Serial.println(angleToMove);
  float radAngle = degToRad(angleToMove);
  //Serial.print("rotation angle in Rad: ");
  //Serial.println(radAngle);
  float arcToMove = receivedRadiusX * radAngle;
  //Serial.print("calculated arc: ");
  //Serial.println(arcToMove);
  float wheelNumbreOfRevolution = arcToMove / (6.28*4);
  //Serial.print("Wheel's numbre of Rotation: ");
  //Serial.println(wheelNumbreOfRevolution);
  float revolutionToSteps = wheelNumbreOfRevolution * 200;
  Serial.print("Rotation Steps between pics: ");
  Serial.println(revolutionToSteps);
  float delaytime = revolutionToSteps*100;
  

  for ( int i = 0; i < received_n_Pic-1; i++)
  {
    Serial.println("taking a photo");

    int dir = (revolutionToSteps > 0)? HIGH:LOW;
    revolutionToSteps = abs(revolutionToSteps);

    digitalWrite(rotate_dirPin,dir);
    
    for (int j = 0; j < revolutionToSteps; j++)
    {
      digitalWrite(rotate_stepPin, HIGH); 
      delayMicroseconds(delaytime); 

      digitalWrite(rotate_stepPin, LOW); 
      delayMicroseconds(delaytime);
    }
    Serial.println("done");
    delay(2000);
  }
   Serial.println("taking a photo");  
}
 
 
void RotateAbsolute()
{
    //We move to an absolute position.
    //The AccelStepper library keeps track of the position.
    //The direction is determined by the multiplier (+1 or -1)
    //Why do we need negative numbers? - If you drive a threaded rod and the zero position is in the middle of the rod...
 
    runXallowed = true; //allow running - this allows entering the RunTheMotor() function.
    x_stepper.setMaxSpeed(receivedSpeed); //set speed
    x_stepper.moveTo(directionMultiplier * receivedSteps); //set relative distance   
}
 
void PrintCommands()
{  
    //Printing the commands
    Serial.println(" 'C' : Prints all the commands and their functions.");
    Serial.println(" 'E' : Expand the X-motor in positive (CW) direction, Absolute.");
    Serial.println(" 'S' : Shrink the X-motor in negative (CCW) direction, Absolute.");
    Serial.println(" 'U' : drive up the Y-motor (cw) direction, Absolute.");
    Serial.println(" 'D' : drive down the Y-motor (CCW) direction, Absolute.");
    Serial.println(" 'R' : Rotates the motor to an absolute positive position (+).");
    Serial.println(" 'r' : Rotates the motor to an absolute negative position (-).");
    Serial.println(" 's' : Stops the motor immediately."); 
    Serial.println(" 'A' : Sets an acceleration value.");
    Serial.println(" 'L' : Prints the current position/location of the motor.");
    Serial.println(" 'H' : Goes back to 0 position from the current position (homing).");
    Serial.println(" 'u' : Updates the position current position and makes it as the new 0 position. ");   
}