#include <Servo.h>
#include <LiquidCrystal.h>

const int rs = 2, en = 4, d4 = 9, d5 = 10, d6 = 11, d7 = 12;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

Servo sv11;
Servo sv12;
Servo sv13;
Servo sv14;
Servo sv15;
Servo sv16;
Servo sv17;

Servo sv21;
Servo sv22;
Servo sv23;
Servo sv24;
Servo sv25;
Servo sv26;
Servo sv27;

Servo sv31;
Servo sv32;
Servo sv33;
Servo sv34;
Servo sv35;
Servo sv36;
Servo sv37;

Servo sv41;
Servo sv42;
Servo sv43;
Servo sv44;
Servo sv45;
Servo sv46;
Servo sv47;

//label all servo motors
//first digit is number placement (1, 2, 3, 4)
//second digit is location (top left is 1)

//number 1
int pin11 = 22;
int pin12 = 24;
int pin13 = 26;
int pin14 = 28;
int pin15 = 30;
int pin16 = 32;
int pin17 = 34;

//number 2
int pin21 = 23;
int pin22 = 25;
int pin23 = 27;
int pin24 = 29;
int pin25 = 31;
int pin26 = 33;
int pin27 = 35;

//number 3
int pin31 = 37;
int pin32 = 39;
int pin33 = 41;
int pin34 = 43;
int pin35 = 45;
int pin36 = 47;
int pin37 = 49;

//number 4
int pin41 = 36;
int pin42 = 38;
int pin43 = 40;
int pin44 = 42;
int pin45 = 44;
int pin46 = 46;
int pin47 = 48;

int time = 500;
int n1 = 2;
int n2 = 3;
int n3 = 4;
int n4 = 0;

void setup() {

  lcd.begin(16,2);

  // attach pins to the servos

  sv11.attach(pin11);
  sv12.attach(pin12);
  sv13.attach(pin13);
  sv14.attach(pin14);
  sv15.attach(pin15);
  sv16.attach(pin16);
  sv17.attach(pin17);

  sv21.attach(pin21);
  sv22.attach(pin22);
  sv23.attach(pin23);
  sv24.attach(pin24);
  sv25.attach(pin25);
  sv26.attach(pin26);
  sv27.attach(pin27);

  sv31.attach(pin31);
  sv32.attach(pin32);
  sv33.attach(pin33);
  sv34.attach(pin34);
  sv35.attach(pin35);
  sv36.attach(pin36);
  sv37.attach(pin37);

  sv41.attach(pin41);
  sv42.attach(pin42);
  sv43.attach(pin43);
  sv44.attach(pin44);
  sv45.attach(pin45);
  sv46.attach(pin46);
  sv47.attach(pin47);

}

void loop() {
  if (n1 == 0) {
    sv11.write(0);
    sv12.write(0);
    sv13.write(0);
    sv14.write(90);
    sv15.write(0);
    sv16.write(0);
    sv17.write(0);
  }
  else if (n1 == 1) {
    sv11.write(90);
    sv12.write(90);
    sv13.write(0);
    sv14.write(90);
    sv15.write(90);
    sv16.write(0);
    sv17.write(90);
    }

  else if (n1 == 2) {
    sv11.write(0);
    sv12.write(90);
    sv13.write(0);
    sv14.write(0);
    sv15.write(0);
    sv16.write(90);
    sv17.write(0);
  }

  n1 + 1; 

  number2();
}

void number2() {
  if (n2 == 0) {
    sv21.write(0);
    sv22.write(0);
    sv23.write(0);
    sv24.write(90);
    sv25.write(0);
    sv26.write(0);
    sv27.write(0);
    }

  else if (n2 == 1) {
    sv21.write(90);
    sv22.write(90);
    sv23.write(0);
    sv24.write(90);
    sv25.write(90);
    sv26.write(0);
    sv27.write(90);
  }

  else if (n2 == 2) {
    sv21.write(0);
    sv22.write(90);
    sv23.write(0);
    sv24.write(0);
    sv25.write(0);
    sv26.write(90);
    sv27.write(0);
  }

  else if (n2 == 3) {
    sv21.write(0);
    sv22.write(90);
    sv23.write(0);
    sv24.write(0);
    sv25.write(90);
    sv26.write(0);
    sv27.write(0);
  }

  else if (n2 == 4) {
    sv21.write(90);
    sv22.write(0);
    sv23.write(0);
    sv24.write(0);
    sv25.write(90);
    sv26.write(0);
    sv27.write(90);
  }

  else if (n2 == 5) {
    sv21.write(0);
    sv22.write(0);
    sv23.write(90);
    sv24.write(0);
    sv25.write(90);
    sv26.write(0);
    sv27.write(0);
  }

  else if (n2 == 6) {
    sv21.write(0);
    sv22.write(0);
    sv23.write(90);
    sv24.write(0);
    sv25.write(0);
    sv26.write(0);
    sv27.write(0);
  }

  else if (n2 == 7) {
    sv21.write(0);
    sv22.write(90);
    sv23.write(0);
    sv24.write(90);
    sv25.write(90);
    sv26.write(0);
    sv27.write(90);
  }

  else if (n2 == 8) {
    sv21.write(0);
    sv22.write(0);
    sv23.write(0);
    sv24.write(0);
    sv25.write(0);
    sv26.write(0);
    sv27.write(0);
  }

  else if (n2 == 9) {
    sv21.write(0);
    sv22.write(0);
    sv23.write(0);
    sv24.write(0);
    sv25.write(90);
    sv26.write(0);
    sv27.write(0);
  }

  n2 += 1;

  number3();
}

void number3() {
  if (n3 == 0) {
    sv31.write(0);
    sv32.write(0);
    sv33.write(0);
    sv34.write(90);
    sv35.write(0);
    sv36.write(0);
    sv37.write(0);
  }

  else if (n3 == 1) {
    sv31.write(90);
    sv32.write(90);
    sv33.write(0);
    sv34.write(90);
    sv35.write(90);
    sv36.write(0);
    sv37.write(90);
  }

  else if (n3 == 2) {
    sv31.write(0);
    sv32.write(90);
    sv33.write(0);
    sv34.write(0);
    sv35.write(0);
    sv36.write(90);
    sv37.write(0);
  }

  else if (n3 == 3) {
    sv31.write(0);
    sv32.write(90);
    sv33.write(0);
    sv34.write(0);
    sv35.write(90);
    sv36.write(0);
    sv37.write(0);
  }

  else if (n3 == 4) {
    sv31.write(90);
    sv32.write(0);
    sv33.write(0);
    sv34.write(0);
    sv35.write(90);
    sv36.write(0);
    sv37.write(90);
  }

  else if (n3 == 5) {
    sv31.write(0);
    sv32.write(0);
    sv33.write(90);
    sv34.write(0);
    sv35.write(90);
    sv36.write(0);
    sv37.write(0);
  }

  n3 += 1;

  number4();
}

void number4() {
  lcd.clear();

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  //number 0 
  sv41.write(0);
  sv42.write(0);
  sv43.write(0);
  sv44.write(90);
  sv45.write(0);
  sv46.write(0);
  sv47.write(0);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);
  
  // number 1
  sv41.write(90);
  sv42.write(90);
  sv43.write(0);
  sv44.write(90);
  sv45.write(90);
  sv46.write(0);
  sv47.write(90);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  // number 2
  sv41.write(0);
  sv42.write(90);
  sv43.write(0);
  sv44.write(0);
  sv45.write(0);
  sv46.write(90);
  sv47.write(0);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  //number 3
  sv41.write(0);
  sv42.write(90);
  sv43.write(0);
  sv44.write(0);
  sv45.write(90);
  sv46.write(0);
  sv47.write(0);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  //number 4
  sv41.write(90);
  sv42.write(0);
  sv43.write(0);
  sv44.write(0);
  sv45.write(90);
  sv46.write(0);
  sv47.write(90);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  //number 5
  sv41.write(0);
  sv42.write(0);
  sv43.write(90);
  sv44.write(0);
  sv45.write(90);
  sv46.write(0);
  sv47.write(0);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  //number 6
  sv41.write(0);
  sv42.write(0);
  sv43.write(90);
  sv44.write(0);
  sv45.write(0);
  sv46.write(0);
  sv47.write(0);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  //number 7
  sv41.write(0);
  sv42.write(90);
  sv43.write(0);
  sv44.write(90);
  sv45.write(90);
  sv46.write(0);
  sv47.write(90);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  //number 8
  sv41.write(0);
  sv42.write(0);
  sv43.write(0);
  sv44.write(0);
  sv45.write(0);
  sv46.write(0);
  sv47.write(0);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  //number 9
  sv41.write(0);
  sv42.write(0);
  sv43.write(0);
  sv44.write(0);
  sv45.write(90);
  sv46.write(0);
  sv47.write(0);

  n4 += 1;
  delay(time);

  lcd.setCursor(0,0);
  lcd.print("n1: ");
  lcd.print(n1);
  lcd.setCursor(8,0);
  lcd.print("n2: ");
  lcd.print(n2-1);
  lcd.setCursor(0,1);
  lcd.print("n3: ");
  lcd.print(n3-1);
  lcd.setCursor(8,1);
  lcd.print("n4: ");
  lcd.print(n4);

  cases();
}

void cases() {
  if (n1 < 2) {
    if (n2 < 10) {
      if (n3 < 6) {
        n4 = 0;
        number3();
      }
      else {
        n3 = 0;
        n4 = 0;
        number2();
      }
    }

    else {
      n1 += 1;
      n2 = 0;
      n3 = 0;
      n4 = 0;
    }

  }

  else {
    if (n2 < 4) {
      if (n3 < 6) {
        n4 = 0;
        number3();
      }
      else {
        n3 = 0;
        n4 = 0;
        number2();
      }
    }

    else {
      if (n3 < 6) {
        n4 = 0;
        number3();
      }
      else {
        n1 = 0;
        n2 = 0;
        n3 = 0;
        n4 = 0;
        loop();
      }
    }
  }
}