#include <Servo.h>

Servo myservo[12];
int LegsUD[12] = {98, 70, 64, 75, 105, 110, 58, 110, 104, 115, 65, 70};
float centerX[6] = {93, 93, 90, 92, 90, 80};
float centerX_forward[6] = {123, 123, 120, 122, 120, 110};
float centerX_backwards[6] = {63, 63, 60, 62, 60, 50};
int centerY[6] = {78, 90, 84, 95, 85, 90};
int pos = 60;
int opos = 130;

void setup() {
  for (int k = 0; k < 12; k++) {
    myservo[k].attach(2 + k);
  }
}
 void first_legs_up(int *LegsUD) {
   for (int i = 6; i < 9; i++) 
      myservo[i].write(LegsUD[i - 6]);
 }

 void other_legs_up(int *LegsUD) {
   for (int i = 9; i < 12; i++) 
      myservo[i].write(LegsUD[i - 6]);
 }

  void first_legs_down(int *LegsUD) {
   for (int i = 6; i < 9; i++) 
      myservo[i].write(LegsUD[i]);
 }

 void other_legs_down(int *LegsUD) {
   for (int i = 9; i < 12; i++) 
      myservo[i].write(LegsUD[i]);
 }


void first_legs_forward(float *centerX) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 0; i < 3; i++){
      if (i == 1) {
        myservo[i].write(centerX[i] + range);
      } else {
        myservo[i].write(centerX[i] - range);
      }
    }
  }
}
void first_legs_right(float *centerX) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 0; i < 3; i++){
        myservo[i].write(centerX[i] + range);
    }
  }
}
void first_legs_forward_2(float *centerX_forward , float *centerX_backwards) {
  for(float range = 1; range <= 60; range = range + 0.1) {
    for(int i = 0; i < 3; i++){
      if (i == 1) {
        myservo[i].write(centerX_backwards[i] + range);
      } else {
        myservo[i].write(centerX_forward[i] - range);
      }
    }
  }
}
void first_legs_right_2(float *centerX_backwards) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 0; i < 3; i++){
        myservo[i].write(centerX_backwards[i] + range);
    }
  }
}
void first_legs_backwards(float *centerX) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 0; i < 3; i++){
      if (i == 1) {
        myservo[i].write(centerX[i] - range);
      } else {
        myservo[i].write(centerX[i] + range);
      }
    }
  }
}

void first_legs_left(float *centerX) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 0; i < 3; i++){
        myservo[i].write(centerX[i] - range);
    }
  }
}

void first_legs_left_2(float *centerX_forward) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 0; i < 3; i++){
        myservo[i].write(centerX_forward[i] - range);
    }
  }
}

void first_legs_backwards_2(float *centerX_forward , float *centerX_backwards) {
  for(float range = 1; range <= 60; range = range + 0.1) {
    for(int i = 0; i < 3; i++){
      if (i == 1) {
        myservo[i].write(centerX_forward[i] - range);
      } else {
        myservo[i].write(centerX_backwards[i] + range);
      }
    }
  }
}
void other_legs_forward(float *centerX) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i < 6; i++){
      if (i == 4) {
        myservo[i].write(centerX[i] - range);
      } else {
        myservo[i].write(centerX[i] + range);
      }
    }
  }
}
void other_legs_right(float *centerX) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i < 6; i++){
        myservo[i].write(centerX[i] + range);
    }
  }
}
void other_legs_forward_2(float *centerX_forward , float *centerX_backwards) {
  for(float range = 1; range <= 60; range = range + 0.1) {
    for(int i = 3; i < 6; i++){
      if (i == 4) {
        myservo[i].write(centerX_forward[i] - range);
      } else {
        myservo[i].write(centerX_backwards[i] + range);
      }
    }
  }
}
void other_legs_right_2(float *centerX_backwards) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i < 6; i++){
        myservo[i].write(centerX_backwards[i] + range);
    }
  }
}

void other_legs_backwards(float *centerX) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i < 6; i++){
      if (i == 4) {
        myservo[i].write(centerX[i] + range);
      } else {
        myservo[i].write(centerX[i] - range);
      }
    }
  }
}
void other_legs_backwards_2(float *centerX_forward , float *centerX_backwards) {
  for(float range = 1; range <= 60; range = range + 0.1) {
    for(int i = 3; i < 6; i++){
      if (i == 4) {
        myservo[i].write(centerX_backwards[i] + range);
      } else {
        myservo[i].write(centerX_forward[i] - range);
      }
    }
  }
}

void first_side_legs_forward(float *centerX) {
 for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 1; i <= 6; i+5) {
        myservo[i].write(centerX[i] + range);
    }
  }
}

void first_side_legs_backwards(float *centerX) {
 for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 1; i <= 6; i+5) {
        myservo[i].write(centerX[i] - range);
    }
  }
}

void other_side_legs_forward(float *centerX) {
 for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i <= 4; i++) {
        myservo[i].write(centerX[i] + range);
    }
  }
}

void other_side_legs_backwards(float *centerX) {
 for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i <= 4; i++) {
        myservo[i].write(centerX[i] - range);
    }
  }
}

void other_legs_left(float *centerX) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i < 6; i++){
        myservo[i].write(centerX[i] - range);
    }
  }
}

void first_side_legs_forward_2(float *centerX_backwards) {
 for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 1; i <= 6; i+5) {
        myservo[i].write(centerX_backwards[i] + range);
    }
  }
}

void first_side_legs_backwards_2(float *centerX_backwards) {
 for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 1; i <= 6; i+5) {
        myservo[i].write(centerX_backwards[i] - range);
    }
  }
}

void other_legs_left_2(float *centerX_backwards) {
  for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i < 6; i++){
        myservo[i].write(centerX_backwards[i] - range);
    }
  }
}

void other_side_legs_forward_2(float *centerX_backwards) {
 for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i <= 4; i++) {
        myservo[i].write(centerX_backwards[i] + range);
    }
  }
}

void other_side_legs_backwards_2(float *centerX_backwards) {
 for(float range = 1; range <= 30; range = range + 0.1) {
    for(int i = 3; i <= 4; i++) {
        myservo[i].write(centerX_backwards[i] - range);
    }
  }
}

 void first_side_legs_up(int *LegsUD) {
   for (int i = 6; i < 12; i+5) 
      myservo[i].write(LegsUD[i - 6]);
 }

void first_side_legs_down(int *LegsUD) {
   for (int i = 6; i < 12; i+5) 
      myservo[i].write(LegsUD[i]);
 }

 void other_side_legs_up(int *LegsUD) {
   for (int i = 8; i < 10; i++) 
      myservo[i].write(LegsUD[i - 6]);
 }

void other_side_legs_down(int *LegsUD) {
  for (int i = 8; i < 10; i++) 
    myservo[i].write(LegsUD[i]);
 }



void forward() {
  other_legs_down(LegsUD);
  delay(400);
  first_legs_up(LegsUD);
  delay(400);
  first_legs_forward(centerX);
  other_legs_backwards(centerX);
  delay(400);
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  first_legs_backwards(centerX);
  other_legs_forward(centerX);
  delay(400);  
}

void forward_2(){
  other_legs_down(LegsUD);
  delay(400);
  first_legs_up(LegsUD);
  delay(400);
  first_legs_forward_2(centerX_backwards , centerX_forward);
  other_legs_backwards_2(centerX_backwards , centerX_forward);
  delay(400);
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  first_legs_backwards_2(centerX_backwards , centerX_forward);
  other_legs_forward_2(centerX_backwards , centerX_forward);
  delay(400);

}


void backward() {
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  first_legs_backwards(centerX);
  other_legs_forward(centerX);
  delay(400);
  first_legs_up(LegsUD);
  delay(400);
  other_legs_down(LegsUD);
  delay(400);
  first_legs_forward(centerX);
  other_legs_backwards(centerX);
  delay(400);
}

void backward_2() {
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  first_legs_backwards_2(centerX_backwards , centerX_forward);
  other_legs_forward_2(centerX_backwards , centerX_forward);
  delay(400);
  first_legs_up(LegsUD);
  delay(400);
  other_legs_down(LegsUD);
  delay(400);
  first_legs_forward_2(centerX_backwards , centerX_forward);
  other_legs_backwards_2(centerX_backwards , centerX_forward);
  delay(400);
}

void right(){
  myservo[10].write(centerY[4]);
  myservo[7].write(centerY[2]);
  first_side_legs_down(LegsUD);
  delay(400);
  other_side_legs_up(LegsUD);
  delay(400);
  first_side_legs_forward(centerX);
  other_side_legs_backwards(centerX);
  delay(400);
  other_side_legs_down(LegsUD);
  delay(400);
  first_side_legs_up(LegsUD);
  delay(400);
  first_side_legs_backwards(centerX);
  other_side_legs_forward(centerX);
  delay(400);

}
void right_2(){
  myservo[10].write(centerY[4]);
  myservo[7].write(centerY[2]);
  first_side_legs_down(LegsUD);
  delay(400);
  other_side_legs_up(LegsUD);
  delay(400);
  first_side_legs_forward_2(centerX);
  other_side_legs_backwards_2(centerX);
  delay(400);
  other_side_legs_down(LegsUD);
  delay(400);
  first_side_legs_up(LegsUD);
  delay(400);
  first_side_legs_backwards_2(centerX);
  other_side_legs_forward_2(centerX);
  delay(400);

}

void left(){
  myservo[10].write(centerY[4]);
  myservo[7].write(centerY[2]);
  other_side_legs_down(LegsUD);
  delay(400);
  first_side_legs_up(LegsUD);
  delay(400);
  other_side_legs_forward(centerX);
  first_side_legs_backwards(centerX);
  delay(400);
  first_side_legs_down(LegsUD);
  delay(400);
  other_side_legs_up(LegsUD);
  delay(400);
  other_side_legs_backwards(centerX);
  first_side_legs_forward(centerX);
  delay(400);

}


void left_2(){
  myservo[10].write(centerY[4]);
  myservo[7].write(centerY[2]);
  other_side_legs_down(LegsUD);
  delay(400);
  first_side_legs_up(LegsUD);
  delay(400);
  other_side_legs_forward_2(centerX_backwards);
  first_side_legs_backwards_2(centerX_forward);
  delay(400);
  first_side_legs_down(LegsUD);
  delay(400);
  other_side_legs_up(LegsUD);
  delay(400);
  other_side_legs_backwards_2(centerX_forward);
  first_side_legs_forward_2(centerX_backwards);
  delay(400);

}

void spin_left() {
  other_legs_down(LegsUD);
  delay(400);
  first_legs_up(LegsUD);
  delay(400);
  first_legs_right(centerX);
  other_legs_left(centerX);
  delay(400);
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  first_legs_left(centerX);
  other_legs_right(centerX);
  delay(400);

}

void spin_left_2() {
  other_legs_down(LegsUD);
  delay(400);
  first_legs_up(LegsUD);
  delay(400);
  first_legs_right_2(centerX_forward);
  other_legs_left_2(centerX_backwards);
  delay(400);
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  first_legs_left_2(centerX_forward);
  other_legs_right_2(centerX_backwards);
  delay(400);

}

void spin_right() {
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  other_legs_left(centerX);
  first_legs_right(centerX);
  delay(400);
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  other_legs_right(centerX);
  first_legs_left(centerX);
  delay(400);
}

void spin_right_2() {
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  other_legs_left(centerX_backwards);
  first_legs_right(centerX_forward);
  delay(400);
  first_legs_down(LegsUD);
  delay(400);
  other_legs_up(LegsUD);
  delay(400);
  other_legs_right(centerX_forward);
  first_legs_left(centerX_backwards);
  delay(400);
}



void loop() {
   int flag = 0;
  while(flag > 0) {
    spin_right_2();
  }
  spin_right(); 
  flag = flag + 1;

}