#include <AFMotor.h>
int StepSensor1 = 20;
int count1 = 0;
int turn = 0;
int new_val;
int old_val;
int iss = 1;
int iss2 = 1;


AF_DCMotor motorHor(2, MOTOR12_64KHZ);
AF_DCMotor motorVer(4, MOTOR34_64KHZ);

void setup() {
  // put your setup code here, to run once:
  pinMode(StepSensor1, INPUT);
  Serial.begin(9600);

  motorHor.setSpeed(255);
  motorHor.run(RELEASE);
  motorVer.setSpeed(255);
  motorVer.run(RELEASE);

  Up();
  delay(1000);
  backward();
  delay(1000);


}

void loop() {
  Box10();
  delay(1000);
  defaultPos();
  delay(1000);
  Box20();
  delay(1000);
  defaultPos();
  delay(1000);
  Box30();
  delay(1000);
  defaultPos();
  delay(1000);
  Box11();
  delay(1000);
  defaultPos();
  delay(1000);
  Box21();
  delay(1000);
  defaultPos();
  delay(1000);
  Box31();
  delay(1000);
  defaultPos();
  delay(1000);
}

void Box10()
{
  StepSense(2); //goes 2 step from default position
}

void Box20()
{
  StepSense(6); //goes 6 step from default position
}

void Box30()
{
  StepSense(12); //goes 12 step from default position
}

void Box11()
{
  NextLevel();  // Goes Up
  StepSense(2); //goes 2 step from default position
}

void Box21()
{
  NextLevel();   // Goes Up
  StepSense(6); //goes 6 step from default position
}

void Box31()
{
  NextLevel();    // Goes Up
  StepSense(12); //goes 12 step from default position
}

void defaultPos()
{
  Down();
  delay(1000);
  backward();
  delay(1000);
}

void forward()
{
  motorHor.run(FORWARD);
  delay(1000);
  motorHor.run(RELEASE);
}

void backward()
{
  motorHor.run(BACKWARD);
  delay(2000);
  motorHor.run(RELEASE);
}

void Up()
{
  motorVer.run(FORWARD);
  delay(700);
  motorVer.run(RELEASE);
}

void Down()
{
  motorVer.run(BACKWARD);
  delay(650);
  motorVer.run(RELEASE);
}

void NextLevel()
{
  if (iss2 < 2) {
    Down();
    iss2++;
  }
}



void StepSense(int step)
{
  if (turn != step) {
    motorHor.run(FORWARD);
    new_val = digitalRead(StepSensor1);


    if (new_val != old_val)
    {
      count1++;
      if (count1 == 10)
      {
        motorHor.run(RELEASE);
        turn++;
        count1 = 0;
        Serial.print("TURN ");
        Serial.print(turn);
        Serial.print(" Complete ");

      }
      else

        Serial.println(count1);
      old_val = new_val;

    }
  }

}