// Motor pins setup
#define Am 8
#define Ap 9
#define Bp 10
#define Bm 11

// Buttons pins
#define lv1 2
#define lv2 3
#define lv3 4

// Functions for moving the motors
void move1(int t){
  for(int k=0; k<50*t;k++){
    delay(20);
    digitalWrite(Bm, LOW); digitalWrite(Ap, HIGH);
    delay(20);
    digitalWrite(Ap, LOW); digitalWrite(Bp,HIGH);
    delay(20);
    digitalWrite(Bp,LOW); digitalWrite(Am,HIGH);
    delay(20);
    digitalWrite(Am,LOW); digitalWrite(Bm,HIGH);
  }
}

void move2(int n){
  for(int i=0;i<50*n;i++){
    delay(20);
    digitalWrite(Ap, LOW); digitalWrite(Bm, HIGH);
    delay(20);
    digitalWrite(Bm, LOW); digitalWrite(Am,HIGH);
    delay(20);
    digitalWrite(Am,LOW); digitalWrite(Bp,HIGH);
    delay(20);
    digitalWrite(Bp,LOW); digitalWrite(Ap,HIGH);
  }
}

// Variables for the level infos
bool stg1,stg2,stg3;
int currentState=0;
int nextState=0;
int stage=0;

void setup() {
  // put your setup code here, to run once:
  pinMode(2,INPUT);
  pinMode(3,INPUT);
  pinMode(4, INPUT);
  pinMode(8,OUTPUT);
  pinMode(9,OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11,OUTPUT);
}

void loop() {
  
  stg1=digitalRead(lv1);
  stg2=digitalRead(lv2);
  stg3=digitalRead(lv3);

  // Initial movement

  if(stg1==HIGH){
    currentState=1;
    move1(currentState);
  }
  else if(stg2==HIGH){
    currentState=2;
    move1(currentState);
  }
  else if(stg3==HIGH){
    currentState=3;
    move1(currentState);
  }
  
  // Testing the movement
  if(stg1==HIGH){
    nextState=1;
    switch(currentState){
      case 2:
        move2(currentState-nextState);
      case 3:
        move2(currentState-nextState);
    }
  }
  if(stg2==HIGH){
    nextState=2;
    switch(currentState){
      case 1: 
        move1(nextState-currentState);
      case 3:
        move2(currentState-nextState);
    }
  }
  if(stg3==HIGH){
    switch(currentState){
      case 1: 
        move1(currentState+2);
      case 2:
        move1(currentState+1);
    }
  }
  
}