const int dirPin1 = 47;
const int dirPin2 = 29;
const int stepPin_F = 49; //all pins will need to be checked
const int stepPin_B = 27;
const int stepPin_R = 51;
const int stepPin_L = 25;
const int stepPin_U = 53;
const int stepPin_D = 23;

const int disable_Pin = 39;

int letter;


//default speed
int acc_decel1 = 700;
int acc_decel2 = 650;
int acc_decel3 = 600;
int acc_decel4 = 550;
int acc_decel5 = 500;


unsigned long timeA = 0;
unsigned long timeB = 0;
unsigned long solveTime = 0;
unsigned long looptime;
bool motors_on = false;

void setup()
{
  pinMode(disable_Pin, OUTPUT);
  digitalWrite(disable_Pin, HIGH);
  
  Serial . begin ( 115200 ) ;
  // Declare pins as Outputs
  digitalWrite(disable_Pin, HIGH);
  pinMode(dirPin1, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  pinMode(stepPin_F, OUTPUT);
  pinMode(stepPin_B, OUTPUT);
  pinMode(stepPin_R, OUTPUT);
  pinMode(stepPin_L, OUTPUT);
  pinMode(stepPin_U, OUTPUT);
  pinMode(stepPin_D, OUTPUT);
  
  
  
}
void double_move_accel(int stepPin1, int stepPin2, int pulseSpeed, int steps)
{
           for(int x = 0; x < steps; x++)
           {
              digitalWrite(stepPin1, HIGH);
              digitalWrite(stepPin2, HIGH);
              delayMicroseconds(pulseSpeed);
              digitalWrite(stepPin1, LOW);
              digitalWrite(stepPin2, LOW);
              delayMicroseconds(pulseSpeed);
           }
}


void double_90(int stepPin1, int stepPin2, int dirHeight1, int dirHeight2)
{

           // Set motor directions
           digitalWrite(dirPin1, dirHeight1);
           digitalWrite(dirPin2, dirHeight2);

           double_move_accel(stepPin1, stepPin2, acc_decel1, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel2, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel3, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel4, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel5, 10);
           double_move_accel(stepPin1, stepPin2, acc_decel4, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel3, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel2, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel1, 5);

          
}

void double_180(int stepPin1, int stepPin2, int dirHeight1, int dirHeight2)
{

           // Set motor directions
           digitalWrite(dirPin1, dirHeight1);
           digitalWrite(dirPin2, dirHeight2);

           double_move_accel(stepPin1, stepPin2, acc_decel1, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel2, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel3, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel4, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel5, 60);
           double_move_accel(stepPin1, stepPin2, acc_decel4, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel3, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel2, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel1, 5);

          
}

void double_180_90(int stepPin1, int stepPin2, int dirHeight1, int dirHeight2)
{

           // Set motor directions
           digitalWrite(dirPin1, dirHeight1);
           digitalWrite(dirPin2, dirHeight2);
            //steppin 1 is the one that does a 180
           accel(stepPin1, acc_decel1, 5);
           accel(stepPin1, acc_decel2, 5);
           accel(stepPin1, acc_decel3, 5);
           accel(stepPin1, acc_decel4, 5);
           accel(stepPin1, acc_decel5, 10);
           accel(stepPin1, acc_decel4, 5);
           accel(stepPin1, acc_decel3, 5);
           accel(stepPin1, acc_decel2, 5);
           accel(stepPin1, acc_decel1, 5);


           double_move_accel(stepPin1, stepPin2, acc_decel1, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel2, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel3, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel4, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel5, 10);
           double_move_accel(stepPin1, stepPin2, acc_decel4, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel3, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel2, 5);
           double_move_accel(stepPin1, stepPin2, acc_decel1, 5);
           

          
}

             

void accel (int stepPin, int pulseSpeed, int steps){
  for(int x = 0; x < steps; x++)
           {
              digitalWrite(stepPin, HIGH);
              delayMicroseconds(pulseSpeed);
              digitalWrite(stepPin, LOW);
              delayMicroseconds(pulseSpeed);
           }
                                                    }


void motorMove90 (int stepPin, int height){

           
           digitalWrite(dirPin1, height);
           digitalWrite(dirPin2, height);
           accel(stepPin, acc_decel1, 5);
           accel(stepPin, acc_decel2, 5);
           accel(stepPin, acc_decel3, 5);
           accel(stepPin, acc_decel4, 5);
           accel(stepPin, acc_decel5, 10);
           accel(stepPin, acc_decel4, 5);
           accel(stepPin, acc_decel3, 5);
           accel(stepPin, acc_decel2, 5);
           accel(stepPin, acc_decel1, 5);           
           
           

           
                                             }


void motorMove180 (int stepPin){

           accel(stepPin, acc_decel1, 5);
           accel(stepPin, acc_decel2, 5);
           accel(stepPin, acc_decel3, 5);
           accel(stepPin, acc_decel4, 5);
           accel(stepPin, acc_decel5, 60);
           accel(stepPin, acc_decel4, 5);
           accel(stepPin, acc_decel3, 5);
           accel(stepPin, acc_decel2, 5);
           accel(stepPin, acc_decel1, 5);
             
                              }
void loop()
{
  
  delay(10);
    if ( Serial . available ( ) > 0 ) 
    { 
      looptime = millis();
      
      if (motors_on == false){ digitalWrite(disable_Pin, LOW); motors_on = true; delay(1); }

      // read the oldest byte in the serial buffer: 
      letter = Serial . read ( ) ; 

 //****************Front Motor*******************     
        if ( letter == 'F' )  {  motorMove90(stepPin_F, HIGH);}
  
       if ( letter == 'f' ) { motorMove90(stepPin_F, LOW);}
//***************Back Motor***************************
        if ( letter == 'B' ) { motorMove90(stepPin_B, HIGH); }
  
       if ( letter == 'b' )  {motorMove90(stepPin_B, LOW); }

//***************Right Motor************************
        if ( letter == 'R' )  { motorMove90(stepPin_R, HIGH);}
        if ( letter == 'r' )  { motorMove90(stepPin_R, LOW);}

//***************Left Motor*************************
        if ( letter == 'L' ) {  motorMove90(stepPin_L, HIGH);}
  
       if ( letter == 'l' )  {   motorMove90(stepPin_L, LOW); }

//*************** Up Motor**************************
        if ( letter == 'U' ) 
        {  motorMove90(stepPin_U, HIGH);}
  
       if ( letter == 'u' )  {motorMove90(stepPin_U, LOW);}

//***************Down Motor***************************
        if ( letter == 'D' ) {motorMove90(stepPin_D, HIGH);}
  
       if ( letter == 'd' ) {motorMove90(stepPin_D, LOW);}
//***************  U2  ***************************
        if ( letter == 'Y' || letter =='y') {motorMove180(stepPin_U);}
//***************  D2  ***************************
        if ( letter == 'S' || letter == 's') {motorMove180(stepPin_D); }
//***************  F2  ***************************
        if ( letter == 'G' || letter == 'g') {motorMove180(stepPin_F); }
//***************  B2  ***************************
        if ( letter == 'V' || letter == 'v') {motorMove180(stepPin_B);}
//***********************************************
//***************  R2  ***************************
        if ( letter == 'E' || letter == 'e') { motorMove180(stepPin_R); }
//***********************************************
//***************  L2  ***************************
        if ( letter == 'K' || letter == 'k') {motorMove180(stepPin_L);}

//***************Enable/Disable Motors*****************
        if ( letter == ']' ) 
          {
           // Set disable_pin high to Disable motors
           motors_on = false;
           delay(500);
           digitalWrite(disable_Pin, HIGH);
          }
  
        if ( letter == '[' ) 
           {
           // Set disable_Pin Low to Enable Motors
          digitalWrite(disable_Pin, LOW);
          motors_on = true;
          delay(10);
           }
//*****************************************************
//***************  Pause  ***************************
        if ( letter == '>') 
        {
           //Serial.print("3 ");
           delay(2000);  
        }
//************************************************
//****************Timer Start  ***************************
        if ( letter == '+') 
        {
           //stepSpeed = solveSpeed;
           //Serial.println(" Timer Started ");
           timeA = millis();
        }

//**********************************************
//****************Timer end*************
      if ( letter == '-')
      {
        
        timeB = millis();
        //Serial.println("!");
        
        solveTime = (timeB - timeA);
        Serial.println(solveTime);
        //stepSpeed = scrambleSpeed;
      }

//  This is FB Double
     if (letter == 'A'){double_90(stepPin_F, stepPin_B, HIGH, HIGH);}

     if (letter == 'C'){double_180_90(stepPin_F, stepPin_B, HIGH, HIGH);}

     if (letter == 't'){double_90(stepPin_F, stepPin_B, LOW, HIGH);}

     if (letter == 'H'){double_180_90(stepPin_B, stepPin_F, HIGH, HIGH);}

      if (letter == 'I'){double_180(stepPin_F, stepPin_B, HIGH, LOW);}

     if (letter == 'J'){double_180_90(stepPin_B, stepPin_F, LOW, LOW);}

     if (letter == 'M'){double_90(stepPin_F, stepPin_B, HIGH, LOW);}
     
     if (letter == 'N'){double_180_90(stepPin_F, stepPin_B, HIGH, LOW);}

     if (letter == 'O'){double_90(stepPin_F, stepPin_B, LOW, LOW);}

     //this is LR Double ###########################################
     if (letter == 'P'){double_90(stepPin_L, stepPin_R, HIGH, HIGH);}

     if (letter == 'Q'){double_180_90(stepPin_L, stepPin_R, HIGH, HIGH);}

     if (letter == 'w'){double_90(stepPin_L, stepPin_R, HIGH, LOW);}

     if (letter == 'T'){double_180_90(stepPin_R, stepPin_L, HIGH, HIGH);}

      if (letter == 'W'){double_180(stepPin_L, stepPin_R, HIGH, LOW);}

     if (letter == 'X'){double_180_90(stepPin_R, stepPin_L, HIGH, LOW);}

     if (letter == 'Z'){double_90(stepPin_L, stepPin_R, LOW, HIGH);}
     
     if (letter == 'a'){double_180_90(stepPin_L, stepPin_R, LOW, LOW);}

     if (letter == 'c'){double_90(stepPin_L, stepPin_R, LOW, LOW);}

     //this is UD Double ###########################################
     if (letter == 'x'){double_90(stepPin_U, stepPin_D, HIGH, HIGH);}

     if (letter == 'h'){double_180_90(stepPin_U, stepPin_D, HIGH, HIGH);  }

     if (letter == 'i'){double_90(stepPin_U, stepPin_D, LOW, HIGH);}

     if (letter == 'j'){double_180_90(stepPin_D, stepPin_U, HIGH, HIGH);}

      if (letter == 'm'){double_180(stepPin_U, stepPin_D, HIGH, LOW);}

     if (letter == 'n'){double_180_90(stepPin_D, stepPin_U, HIGH, LOW);}

     if (letter == 'o'){double_90(stepPin_U, stepPin_D, HIGH, LOW);}
     
     if (letter == 'p'){double_180_90(stepPin_U, stepPin_D, HIGH, LOW);}

     if (letter == 'q') {double_90(stepPin_U, stepPin_D, LOW, LOW);}

    
     //speed adjustments 
     
     if (letter == '1')
     {
      acc_decel1 = 600;
      acc_decel2 = 550;
      acc_decel3 = 500;
      acc_decel4 = 450;
      acc_decel5 = 400;
     }

     if (letter == '2')
     {
      acc_decel1 = 650;
      acc_decel2 = 600;
      acc_decel3 = 550;
      acc_decel4 = 500;
      acc_decel5 = 450;
     }

     if (letter == '3')
     {
      acc_decel1 = 700;
      acc_decel2 = 650;
      acc_decel3 = 600;
      acc_decel4 = 550;
      acc_decel5 = 500;
     }

     if (letter == '4')
     {
      acc_decel1 = 800;
      acc_decel2 = 750;
      acc_decel3 = 700;
      acc_decel4 = 650;
      acc_decel5 = 500;
     }

     if (letter == '5')
     {
      acc_decel1 = 1000;
      acc_decel2 = 950;
      acc_decel3 = 900;
      acc_decel4 = 850;
      acc_decel5 = 800;
     }

     if (letter == '6')
     {
      acc_decel1 = 1500;
      acc_decel2 = 1400;
      acc_decel3 = 1300;
      acc_decel4 = 1200;
      acc_decel5 = 1000;
     }
 
}
else{
  if (millis() - looptime > 10000 && motors_on)
  {
    digitalWrite(disable_Pin, HIGH);
    motors_on = false;

    //Serial.println("Motors off");

  }

  

 
}

}