#include <Servo.h>
#include <Ultrasonic.h>
#include <NewPing.h>
Servo servomotor; //сервомотор - отвечает за поворот "головы", на которой стоит сонар
NewPing sonar(2, 4, 50); //собственно сонар, в параметрах вроде распиновка

//пины моторчиков на колесах
int leftWheel = 3;
int rightWheel = 6;


int moveDirectionPin = 8; //пин направления движения. Если на этот пин подан 0,
                          //то колеса будут крутиться вперёд, если 1 - то назад

boolean moveDirectionValue = true; //это мы не добавили по сути. Нужно чтобы ехать вперёд-назад, мы недоделали.
const int MS_PER_DEGREE = 6; //сколько миллисекунд нужно чтобы повернуть машинку на 1 градус


void setup() { //в самом начале настройка
  Serial.begin(9600); //это нужно для вывода значений в консоль, не используется
  servomotor.attach(9); //присоединяем сервомотор на девятый пин
  }

void loop() { //основной цикл, выполняется пока мы не выключим машинку
    rotate(findOptimalDirection()); //сначала машинка физически поворачивается туда, куда наиболее выгодно ехать
    servomotor.write(90);  //поворачиваем голову ровно вперёд
    while (getCurrentDistance() > 10 || getCurrentDistance()==0 && moveDirectionValue){ //и до тех пор пока дистанция больше 10
                                                                                         //(в случае если дистанция огромна - возвращается ноль)
      go(150); //собственно едем со скоростью 150 из 255
    }
    go(0); //останавливаемся
    if (getCurrentDistance() == -1) { //если нас занесло слишком близко
      goBack(); //отъезжаем
    }

}

void goBack() {
  digitalWrite(moveDirectionPin, HIGH); //выставляем на пин 1 => дальше будем ехать назад
  moveDirectionValue = false;
  go(150); //проезжаем со скоростью 150
  delay(1000); / в течение секунды
  go(0); //останавливаемся
  digitalWrite(moveDirectionPin, LOW); //выставляем 0 => дальше будем ехать вперёд
  moveDirectionValue = true;
}

int getCurrentDistance() {
  int distance = sonar.ping_cm(); //получаем дистанцию в сантиметра с сонара благодаря библиотеке NewPing
  if (distance < 5 && distance != 0) { //если мы слишком близко - возвращаем код "-1", типа "едь назад"
    return -1;
  }
  else return distance; // иначе просто дистанцию
}

void go(int speed) { //поскольку моторы на колесах поддерживают аналоговую запись, можем управлять скоростью от 0 до 255
  analogWrite(leftWheel, speed);
  analogWrite(rightWheel, speed);
}


void rotate(int absoluteDegree) {
  int relativeDegree = 90 - absoluteDegree; //поскольку сонар умеет поворачиваться от 0 до 180,
                                            //то такие же значения нужно использовать для поворота машинки
                                            //если мы вычитаем из 90 наш угол, то мы получаем настоящий угол поворота относительно
                                            //направления вперёд, но либо с плюсом, либо с минусом
  boolean left;                             //значение плюс или минус влияет на флажок left, означающий "будем ехать влево"
  if (relativeDegree < 0) left = true;      //если значение меньше нуля, то едем влево
  else left = false;                        //иначе вправо
  int rotationTime = abs(relativeDegree) * MS_PER_DEGREE;   //вычисляем сколько нужно поворачиваться - модуль угла * константу 6 мс
  if (left) { //и либо влево поворачиваемся
    analogWrite(rightWheel, 255);
    delay(rotationTime);  //столько сколько надо
    analogWrite(rightWheel, 0);
  } else {    //либо вправо
    analogWrite(leftWheel, 255);
    delay(rotationTime);  //столько сколько надо
    analogWrite(leftWheel, 0);
  }
}

int findOptimalDirection(){
  int maxDistance = 0;
  int maxDistanceAngle = 180;
  int currentDistance = 0;
  int minDistance = 999;

  for (int i = 30; i <= 150; i=i+10) { //для поиска направления мы вращаемся от 30 до 150 (потому что двигатель барахлит)
    servomotor.write(i);               //поворачиваем голову
    delay(50);
    currentDistance = getCurrentDistance(); //получаем расстояние
    if (currentDistance > maxDistance){     //сравниваем с предыдущим максимумом
      maxDistance = currentDistance;        //и если больше - обновляем максимум
      maxDistanceAngle=i;                   //и запоминаем куда лучше всего ехать
      }
    }
    if (currentDistance < minDistance && currentDistance != 0) {   //так же смотрит минимальную дистанцию
      minDistance = currentDistance;
    }
    servomotor.write(maxDistanceAngle);   //как только закончили наш цикл - поворачиваем голову туда, куда хотим ехать
    delay(1000);                          //зачем это тут? Машинка бы быстрее ехала без этого
    if (minDistance < 2) {                //если минимальная дистанция меньше 2 см, то машинка может застрять колесами об угол
      goBack();                           //поэтому отъезжаем
    }
    return maxDistanceAngle;
  }