#include <Servo.h>
Servo Leg1F;  
Servo Leg1B;  
Servo Leg2F;  
Servo Leg2B;  
Servo Leg3F;  
Servo Leg3B;  
Servo Leg4F;  
Servo Leg4B;  
boolean stringComplete = false;
boolean stopcmd = false;
String inputString;
int selservo;
int rotate1;
int rotate2;
int rotate3;
int rotate4;


//int walkF[][7]={{124,146,177,150,132,115,115},{94,132,178,139,112,84,84},{37,112,179,139,95,42,42},{22,95,150,115,78,30,30},{11,78,124,92,59,13,13},{13,59,92,58,36,2,2}};
//int walkB[][7]={{3,34,56,65,48,30,30},{2,48,86,96,68,41,41},{1,68,143,138,85,41,41},{30,85,158,150,102,65,65},{56,102,169,167,121,88,88},{88,121,167,178,144,122,122}};

int walkF[][7]={{124,146,177,150,132,115,115},{124,146,177,139,112,84,84},{94,132,178,139,95,42,42},{37,112,179,115,78,30,30},{22,95,150,92,59,13,13},{11,78,124,58,36,2,2}};
int walkB[][7]={{3,34,56,65,48,30,30},{3,34,56,96,68,41,41},{2,48,86,138,85,41,41},{1,68,143,150,102,65,65},{30,85,158,167,121,88,88},{56,102,169,178,144,122,122}};

int walkH[]={3,4,5,6,7,8};
int Fheight;
int Bheight;
int heightchange;

int walkstep;
int walkstep2;

int Twalkstep;
int Twalkstep2;

String S1;
String S2;
String Splitstr;
int Sstr1;
int Sstr2;
int findcomma;
int previouscomma;
String reccmd;
String othercmd;

void setup() {
  // put your setup code here, to run once:
  Leg1F.attach(2);
  Leg1B.attach(3);  
  Leg2F.attach(4);
  Leg2B.attach(5);  
  Leg3F.attach(6);  
  Leg3B.attach(7);  
  Leg4F.attach(8);  
  Leg4B.attach(9);  
  delay(1000);
  Leg1F.write(80);
  Leg1B.write(100);  
  Leg2F.write(100);
  Leg2B.write(80);  
  Leg3F.write(80);  
  Leg3B.write(100);  
  Leg4F.write(100);  
  Leg4B.write(80);
  Serial.begin(9600);        
  walkstep=1;
  Fheight=5;
  Bheight=5;
  reccmd="S";
}

void loop() {
//  if(stringComplete == true)
//  {
//    findcomma=0;
//    previouscomma=0;
//    findcomma=inputString.indexOf(",",findcomma);
//    if (findcomma>0)
//    {
//      Splitstr=inputString.substring(0,findcomma);
//      rotate1=Splitstr.toInt();
//    }
//    previouscomma=findcomma+1;
//    inputString = inputString.substring(previouscomma);
//
//    findcomma=0;
//    findcomma=inputString.indexOf(",",findcomma);
//    if (findcomma>0)
//    {
//      Splitstr=inputString.substring(0,findcomma);
//      rotate2=Splitstr.toInt();
//    }
//    Leg1F.write(rotate1);
//    Leg1B.write(rotate2);  
//    Leg2F.write(rotate2);
//    Leg2B.write(rotate1);  
//    Leg3F.write(rotate1);  
//    Leg3B.write(rotate2);  
//    Leg4F.write(rotate2);  
//    Leg4B.write(rotate1);
//
////    Serial.print(rotate1);
////    Serial.print("-");
////    Serial.print(rotate2);
//    
//    inputString="";
//    stringComplete = false;
//  }

  if (reccmd=="F" || reccmd=="B" || reccmd=="L" || reccmd=="R" || reccmd=="G" || reccmd=="I" || reccmd=="H" || reccmd=="J" )
  {
    if(reccmd=="F" || reccmd=="L" || reccmd=="G" || reccmd=="I" )
    {
      walkstep=walkstep+1;
      if (walkstep>7)
      {
        walkstep=1;
      }
      walkstep2 = walkstep+3;
      if(walkstep2>7)
      {
        walkstep2=walkstep2-7;
      }
    }
    else if(reccmd=="B" || reccmd=="R" || reccmd=="H" || reccmd=="J" )
    {
      walkstep=walkstep-1;
      if (walkstep<1)
      {
        walkstep=7;
      }
      walkstep2 = walkstep-4;
      if(walkstep2<1)
      {
        walkstep2=walkstep2+7;
      }      
    }    

     if(reccmd=="F" || reccmd=="B" )
     {
      //Walking Full correct***********************************************************
      rotate1=walkF[Fheight][walkstep-1];
      rotate2=walkB[Fheight][walkstep-1];        
      rotate3=walkF[Bheight][walkstep-1];
      rotate4=walkB[Bheight][walkstep-1];        

      Leg1F.write(rotate1);
      Leg1B.write(rotate2);  
      Leg4F.write(180-rotate3);  
      Leg4B.write(180-rotate4);    

      rotate1=walkF[Fheight][walkstep2-1];
      rotate2=walkB[Fheight][walkstep2-1];        
      rotate3=walkF[Bheight][walkstep2-1];
      rotate4=walkB[Bheight][walkstep2-1];        

      
      Leg2F.write(180-rotate1);
      Leg2B.write(180-rotate2);        
      Leg3F.write(rotate3);  
      Leg3B.write(rotate4);    
//      Serial.print(walkstep-1);      
//      Serial.print("-");
//      Serial.println(walkstep2-1);      
      //********************************************************************************
     }else if(reccmd=="G" || reccmd=="I" || reccmd=="H" || reccmd=="J")
     {
        if (reccmd=="I" and walkstep>=4)
        {
          rotate1=walkF[Fheight][4];
          rotate2=walkB[Fheight][4];                  
        }else
        {
          rotate1=walkF[Fheight][walkstep-1];
          rotate2=walkB[Fheight][walkstep-1];        
        }

        if (reccmd=="H" and walkstep>=4)
        {
          rotate3=walkF[Bheight][4];
          rotate4=walkB[Bheight][4];                 
        }
        else
        {
          rotate3=walkF[Bheight][walkstep-1];
          rotate4=walkB[Bheight][walkstep-1];                           
        }
        
        Leg1F.write(rotate1);
        Leg1B.write(rotate2);  
        Leg4F.write(180-rotate3);  
        Leg4B.write(180-rotate4);    

        if (reccmd=="G" and walkstep2>=4)
        {
          rotate1=walkF[Fheight][4];
          rotate2=walkB[Fheight][4];          
        }
        else
        {
          rotate1=walkF[Fheight][walkstep2-1];
          rotate2=walkB[Fheight][walkstep2-1];                
        }

        if (reccmd=="J" and walkstep2>=4)
        {
          rotate3=walkF[Bheight][4];
          rotate4=walkB[Bheight][4];        
        }
        else
        {
          rotate3=walkF[Bheight][walkstep2-1];
          rotate4=walkB[Bheight][walkstep2-1];         
        }
  
        Leg2F.write(180-rotate1);
        Leg2B.write(180-rotate2);        
        Leg3F.write(rotate3);  
        Leg3B.write(rotate4);                  
     }      
     else if(reccmd=="L" || reccmd=="R" )
     {
      rotate1=walkF[Fheight][walkstep-1];
      rotate2=walkB[Fheight][walkstep-1];   
      rotate3=walkF[Bheight][walkstep-1];
      rotate4=walkB[Bheight][walkstep-1];   
                 
      Leg1F.write(rotate1);
      Leg1B.write(rotate2);  
      Leg4F.write(rotate4);  
      Leg4B.write(rotate3);    
  
  
      rotate1=walkF[Fheight][(8-walkstep)-1];
      rotate2=walkB[Fheight][(8-walkstep)-1];    
      rotate3=walkF[Bheight][(8-walkstep)-1];
      rotate4=walkB[Bheight][(8-walkstep)-1];          
      Leg2F.write(180-rotate1);
      Leg2B.write(180-rotate2);        
      Leg3F.write(180-rotate4);  
      Leg3B.write(180-rotate3);               
     }

    delay(100);   

    if (stopcmd==true)
    {
      if(walkstep==4)
      {
        reccmd="S";
        stopcmd=false;         
      }
    }     
    if(heightchange!=0 && walkstep==4)
    {
      changeheight();
    }    
  }else if(heightchange!=0)
  {   
      changeheight();
  }else if(reccmd=="C")
  {
    selfcheck();
    reccmd="S";
  }else if(reccmd=="V")
  {
    sayhai();
    reccmd="S";
  }
}

void sayhai(){
  Leg1F.write(0);
  Leg1B.write(180);  
  Leg2F.write(180);
  Leg2B.write(0);  
  
  Leg3F.write(180);  
  Leg3B.write(0);  
  Leg4F.write(0);  
  Leg4B.write(180);

  for(int i=1;i<=5;i++)
  {
  delay(500);
  Leg1F.write(60);    
  delay(500);  
  Leg1F.write(100);   
  } 
  Leg1F.write(0);     
}

void selfcheck()
{
  Leg1F.write(0);
  Leg1B.write(180);  
  Leg2F.write(180);
  Leg2B.write(0);  
  
  Leg3F.write(0);  
  Leg3B.write(180);  
  Leg4F.write(180);  
  Leg4B.write(0);
  
  delay(500);
  Leg1F.write(180);
  Leg1B.write(0);  
  delay(500);
  Leg1F.write(0);
  Leg1B.write(180);

  delay(500);
  Leg2F.write(0);
  Leg2B.write(180);  
  delay(500);
  Leg2F.write(180);
  Leg2B.write(0);        

  delay(500);
  Leg3F.write(180);  
  Leg3B.write(0);
  delay(500);  
  Leg3F.write(0);  
  Leg3B.write(180);    

  delay(500);
  Leg4F.write(0);  
  Leg4B.write(180);
  delay(500);  
  Leg4F.write(180);  
  Leg4B.write(0);    
}

void changeheight(){
    if(othercmd=="O")
    {
      Fheight=5;
      Bheight=5;
    }
    else if(othercmd=="P")
    {
      Fheight=0;
      Bheight=0;
    }
    
    if(othercmd=="D" || othercmd=="U"  ||othercmd=="W" || othercmd=="Y")
    {
      Fheight=Fheight+heightchange;
      if(Fheight>5)
      {
        Fheight=5;      
      }
      if(Fheight<0)
      {
        Fheight=0;             
      }        
    }
    if(othercmd=="D" || othercmd=="U" || othercmd=="X" || othercmd=="Z")
    {
      Bheight=Bheight+heightchange;
      if(Bheight>5)
      {
        Bheight=5;      
      }
      if(Bheight<0)
      {
        Bheight=0;             
      }
    }      
    heightchange=0;  

    rotate1=walkF[Fheight][4];
    rotate2=walkB[Fheight][4];
    Leg1F.write(rotate1);
    Leg1B.write(rotate2);  
    Leg2F.write(180-rotate1);
    Leg2B.write(180-rotate2); 
    rotate1=walkF[Bheight][4];
    rotate2=walkB[Bheight][4];        
    Leg3F.write(rotate1);  
    Leg3B.write(rotate2);          
    Leg4F.write(180-rotate1);  
    Leg4B.write(180-rotate2);
}
void serialEvent() {
  while (Serial.available()) {
    char inChar = (char)Serial.read();   
//        if (inChar == 'Q') {
//          stringComplete = true;
//          reccmd="S";
//         }
        if (inChar == 'F' || inChar == 'B' || inChar == 'L' || inChar == 'R' || inChar == 'G' ) 
        {
          inputString ="";          
          reccmd=inChar;
         }         
        else if (inChar == 'S') {
          inputString = "";
          stopcmd=true;
         }                  
       else if (inChar == 'C' || inChar == 'V') {
          inputString = "";
          reccmd=inChar;
         }                           
        else if (inChar == 'U' || inChar == 'O' ) {
          inputString = "";          
          othercmd=inChar;
          heightchange=1;
         }              
        else if (inChar == 'D' || inChar == 'P' ) {
          inputString = "";
          othercmd=inChar;
          heightchange=-1;
         }                        
        else if (inChar == 'W' || inChar == 'X' ) {
          inputString = "";
          othercmd=inChar;          
          heightchange=-1;
         }         
        else if (inChar == 'Y' || inChar == 'Z' ) {
          inputString = "";
          othercmd=inChar;          
          heightchange=1;
         }                  
//        else
//         {
//          //inputString += inChar; 
//         }
  }  
}