#include <CheapStepper.h>
#include <Servo.h>

CheapStepper stepper (8, 9, 10, 11);
Servo myservo;

// here we declare our stepper using default pins:
// arduino pin <--> pins on ULN2003 board:
// 8 <--> IN1
// 9 <--> IN2
// 10 <--> IN3
// 11 <--> IN4

// let's create a boolean variable to save the direction of our rotation
//int verticleAngle = 75;
boolean moveClockwise = true;
boolean moveAntiClockwise = false;
int servoAngle = 30 ;
const int maxAngle = 75;
const int minAngle = 30;

void setup() {



  // let's just set up a serial connection and test print to the console
  Serial.begin(9600);
  myservo.attach(6);
}

void loop() {

  // let's move a full rotation (4096 mini-steps)
  // we'll go step-by-step using the step() function
  //  myservo.write(45);
  //  for(int i = 0;i < 1024;i++){
  //    stepper.step(moveClockwise);
  //  }
  //  myservo.write(90);
  //  for(int i = 0;i < 1024;i++){
  //    stepper.step(moveAntiClockwise);
  //  }
  int topLeft = analogRead(A2);
  int topRight = analogRead(A3);
  int botLeft = analogRead(A0);
  int botRight = analogRead(A1);

  int valueReadedMean = (topLeft + topRight + botLeft + botRight) / 4;
  int threshold = valueReadedMean + 50;

  if ((topLeft > threshold) | (topRight > threshold) | (botLeft > threshold) | (botRight > threshold)) {
    if (((topLeft > threshold) | (topRight > threshold)) & servoAngle > minAngle) {
      servoAngle -= 5;
      myservo.write(servoAngle);
    }

    else if (((botLeft > threshold) | (botRight > threshold)) & servoAngle < maxAngle ) {
      servoAngle += 5;
      myservo.write(servoAngle);
    }

    if ((topLeft > threshold) | (botLeft > threshold)) {
      for (int i = 0; i < 256; i++) {
        stepper.step(moveAntiClockwise);
      }
    }

    else if ((topRight > threshold) | (botRight > threshold)) {
      for (int i = 0; i < 256; i++) {
        stepper.step(moveClockwise);
      }
    }

    Serial.print("topLeft: ");
    Serial.println(topLeft);

    Serial.print("topRight: ");
    Serial.println(topRight);

    Serial.print("botLeft: ");
    Serial.println(botLeft);

    Serial.print("botRight: ");
    Serial.println(botRight);

    Serial.println(servoAngle);
    Serial.println("=====================");

  }
}




//      if(topLeft >= 150){
//        if (servoAngle == 75){
//          servoAngle = 30;
//          myservo.write(30);
//        }
//  // VVV this only works if we do it UP DOWN LEFT RIGHT
//  //      servoAngle+=5;
//  //      myservo.write(verticleAngle);
//        for(int i=0;i<256;i++){
//          stepper.step(moveAntiClockwise);
//        }
//      }
//      else if(topRight >= 150){
//        if (servoAngle == 75){
//          servoAngle = 30;
//          myservo.write(30);
//        }
//  //      servoAngle+=5;
//  //      myservo.write(verticleAngle);
//        for(int i=0;i<256;i++){
//          stepper.step(moveClockwise);
//        }
//      }
//      else if(botLeft >= 150){
//        if (servoAngle == 30){
//          servoAngle = 75;
//          myservo.write(75);
//        }
//  //      servoAngle-=5
//  //      myservo.write(verticleAngle);
//        for(int i=0;i<256;i++){
//          stepper.step(moveAntiClockwise);
//        }
//      }
//      else if(botRight >= 150){
//        if (servoAngle == 30){
//          servoAngle = 75;
//          myservo.write(75);
//        }
//  //      servoAngle-=5
//  //      myservo.write(verticleAngle);
//        for(int i=0;i<256;i++){
//          stepper.step(moveClockwise);
//        }
//      }
//  //
//  //  delay(200);
//}