//Билиотека для сервопривода
#include <Servo.h>

// Пины драйвера
#define PIN_ENABLE1 10
#define PIN_ENABLE2 12
#define PIN_OUTPUT1 6
#define PIN_OUTPUT2 7
#define PIN_OUTPUT3 8
#define PIN_OUTPUT4 9
#define PIN_OUTPUT5 3
#define TRIG_PIN 4
#define ECHO_PIN 5

const int correction = 0;
Servo rangeFinderServo;

void writeMotors(int speed1, int speed2) {
    // Определение направления вращения моторов
    bool dir1 = (speed1 >= 0);
    digitalWrite(PIN_OUTPUT1, dir1);
    digitalWrite(PIN_OUTPUT2, !dir1);

    bool dir2 = (speed2 >= 0);
    digitalWrite(PIN_OUTPUT3, dir2);
    digitalWrite(PIN_OUTPUT4, !dir2);

    // Если скорости нулевые - стопаем моторчики
    if (speed1 == 0 && speed2 == 0) {
        digitalWrite(PIN_ENABLE1, LOW);
        digitalWrite(PIN_ENABLE2, LOW);
        return;
    }

    // Установка скорости (с учетом коррекции)
    speed1 = map(abs(speed1), 0, 100, 0, 255);
    speed2 = map(abs(speed2), 0, 100, 0, 255);

    if (correction >= 0) {
        speed1 -= correction;
    } else {
        speed2 -= abs(correction);
    }

    analogWrite(PIN_ENABLE1, speed1);
    analogWrite(PIN_ENABLE2, speed2);
}

//Движение вперед
void moveForward() {
    writeMotors(100, 100);
}

//Движение назад
void moveBackward() {
    writeMotors(-50, -50);
}

long microsecondsToCentimeters(long microseconds) {
    return microseconds / 29 / 2;
}

//Датчик приближения
int getDistance() {
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    return microsecondsToCentimeters(pulseIn(ECHO_PIN, HIGH));
}

//Остановить моторы
void stopDrives() {
    writeMotors(0, 0);
}

void lookLeft() {
    rangeFinderServo.attach(3);
    rangeFinderServo.write(130);
    delay(250);
    rangeFinderServo.detach();
}

void lookRight() {
    rangeFinderServo.attach(3);
    rangeFinderServo.write(0);
    delay(250);
    rangeFinderServo.detach();
}

void lookForward() {
    rangeFinderServo.attach(3);
    rangeFinderServo.write(60);
    delay(250);
    rangeFinderServo.detach();
}

void setup() {
    pinMode(PIN_ENABLE1, OUTPUT);
    pinMode(PIN_ENABLE2, OUTPUT);
    pinMode(PIN_OUTPUT1, OUTPUT);
    pinMode(PIN_OUTPUT2, OUTPUT);
    pinMode(PIN_OUTPUT3, OUTPUT);
    pinMode(PIN_OUTPUT4, OUTPUT);
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    Serial.begin(9600);
    lookForward();

}

void turnRight() {
    writeMotors(100, -100);
    delay(250);
    stopDrives();
}

void turnLeft() {
    writeMotors(-100, 100);
    delay(250);
    stopDrives();
}

bool isNearWall() {
    int distance = getDistance();
    Serial.println(distance);
    if (distance < 7 && distance > 1)
        return true;

    return false;
}

bool checkLeft() {
    bool changedDirection = false;

    lookLeft();

    if (!isNearWall()) {
        turnLeft();
        changedDirection = true;
    }

    lookForward();
    return changedDirection;
}

bool checkRight() {
    bool changedDirection = false;

    lookRight();

    if (!isNearWall()) {
        turnRight();
        changedDirection = true;
    }

    lookForward();
    return changedDirection;
}

void goBack() {
    moveBackward();
    delay(500);
    stopDrives();
    delay(500);
    turnRight();
    turnRight();
}

void loop() {
    int distance = getDistance();
    Serial.println(distance);

    if (isNearWall()) {
      turnLeft();
      if(isNearWall()){
       turnRight();
       turnRight();
       if(isNearWall())
       turnRight();
      }

    }

    moveForward();
}