#include <Servo.h>
Servo servo;

#define SERVO_PIN 11
#define LED 13
#define BUZZER 10
#define PRZYCISK 2
#define L_DIR 4
#define L_VEL 5
#define R_DIR 9
#define R_VEL 6
#define TRIG_PIN 7
#define ECHO_PIN 8

#define LEFT 0
#define RIGHT 1

void setup() {
  pinMode(LED, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  pinMode(PRZYCISK, INPUT_PULLUP);
  digitalWrite(LED, LOW);
  digitalWrite(BUZZER, LOW);

  pinMode(L_DIR, OUTPUT);
  pinMode(L_VEL, OUTPUT);
  pinMode(R_DIR, OUTPUT);
  pinMode(R_VEL, OUTPUT);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  servo.attach(SERVO_PIN);

  digitalWrite(L_DIR, 0);
  analogWrite(L_VEL, 0);
  digitalWrite(R_DIR, 0);
  analogWrite(R_VEL, 0);

  digitalWrite(TRIG_PIN, LOW);

  delay(100);
  servo.write(90);
  delay(500);
}

static void Forward(void){
  digitalWrite(L_DIR, 0);
  analogWrite(L_VEL, 100);
  digitalWrite(R_DIR, 0);
  analogWrite(R_VEL, 100);

  delay(1000);

  analogWrite(L_VEL, 0);
  analogWrite(R_VEL, 0);
}

int Distance(){
  int czas, distance;

   digitalWrite(TRIG_PIN, LOW);
   delayMicroseconds(2);
   digitalWrite(TRIG_PIN, HIGH);
   delayMicroseconds(10);
   digitalWrite(TRIG_PIN, LOW);

   czas = pulseIn(ECHO_PIN, HIGH, 10000);
   if(czas == 0)
      czas = 10000;
   distance = czas / 58;
   return distance;
}

void Turnleft (void){

  digitalWrite(L_DIR, 1);
  analogWrite(L_VEL, 255-200);

  delay(500);

  digitalWrite(L_DIR, 0);
  analogWrite(L_VEL, 0);
}

void Turnright (void){

  digitalWrite(R_DIR, 1);
  analogWrite(R_VEL, 25-200);

  delay(500);

  digitalWrite(R_DIR, 0);
  analogWrite(R_VEL, 0);
}


bool Checkright(){
  servo.write(0);
  delay(500);
  bool ret = (Distance() > 30);
  servo.write(90);
  delay(400);
  return ret;
}

bool Checkleft(){
  servo.write(180);
  delay(500);
  bool ret = (Distance() > 30);
  servo.write(90);
  delay(400);
  return ret;
}

bool Checkforward() {
  servo.write(90);
  return (Distance() < 30);
}

void Back(int i, int j, int *For, int *Tur){
  int dire, times_for;
  Turnright(); Turnright();

  for( ; i>0 ; i--){
    times_for = For[i];
    for( ; times_for > 0; times_for--){
      Forward();
    }
    dire = Tur[j];
    if(dire == LEFT) Turnright();
    else Turnleft();
    j--;
  }
}

void loop() {
    static int For[200], Tur[50]; // tablice 1: ile kroków do przodu, 2: w którą strone skręcił
    static int times_for = 0, i = 0, j = 0;
    Tur[0] = 0;
    if(!Checkforward()){
      times_for++;
      Forward();
    }
    else {
      For[i] = times_for;
      times_for = 0;
      i++;
      if(Tur[j] == LEFT){
          if(Checkright()) { Tur[j] = RIGHT; j++; Turnright();
          }
          else {
            if(Checkleft()){ Tur[j] = LEFT;  j++; Turnleft();
            }
            else{ Back(i, j, &For[0], &Tur[0]);
            }
          }
      }
      else{
          if(Checkleft()){ Tur[j] = LEFT;  j++; Turnleft();
          }
          else {
              if(Checkright()) { Tur[j] = RIGHT; j++; Turnright();
              }
              else{ Back(i, j, &For[0], &Tur[0]);
              }
          }
      }
    }
  }