#include <Stepper.h>

const int stepsPerRevolution = 100; // За одну операцію ,Біполярний кроковий моторчик робитиме 2000 обертів
int frontDistance, leftDistance;

// Наперед називаю необхідні мені піни для роботи з Дальноміром HC-SR04
#define LEFT_ECHO_PIN 8  
#define LEFT_TRIG_PIN 9
#define FORWARD_ECHO_PIN 6
#define FORWARD_TRIG_PIN 7

// Наперед називаю необхідні мені піни для роботи з Короковими моторчиками через драйвер A4988
#define RIGHT_DIR_PIN 18
#define RIGHT_STEP_PIN 19
#define LEFT_DIR_PIN 2
#define LEFT_STEP_PIN 3


// Рухатись суворо вперед, заставивши обидві сторони крутитись в напрямку руху уперед.
void FullForward() 
{
  digitalWrite(RIGHT_DIR_PIN, HIGH);
  digitalWrite(LEFT_DIR_PIN, LOW);
  for (int i = 0; i < stepsPerRevolution; i++) {
    digitalWrite(RIGHT_STEP_PIN, HIGH);
    digitalWrite(RIGHT_STEP_PIN, LOW);
    digitalWrite(LEFT_STEP_PIN, HIGH);
    digitalWrite(LEFT_STEP_PIN, LOW);
    delay(10); // 5 ms * 200 = 1 second
  }
}

// Для повороту направо, ми заставляємо ліві колеса рухатись вперед, а праві - назад. Таким чином, наш автомобіль повертатиметься на місці.
void TurnRight()
{
  digitalWrite(RIGHT_DIR_PIN, LOW);
  digitalWrite(LEFT_DIR_PIN, LOW);
  for (int i = 0; i < stepsPerRevolution; i++) {
    digitalWrite(RIGHT_STEP_PIN, HIGH);
    digitalWrite(RIGHT_STEP_PIN, LOW);
    digitalWrite(LEFT_STEP_PIN, HIGH);
    digitalWrite(LEFT_STEP_PIN, LOW);
    delay(10); // 5 ms * 200 = 1 second
  }
}

// Ідентична ситуація до повороту праворуч.
void TurnLeft()
{
  digitalWrite(RIGHT_DIR_PIN, HIGH);
  digitalWrite(LEFT_DIR_PIN, HIGH);
  for (int i = 0; i < stepsPerRevolution; i++) {
    digitalWrite(RIGHT_STEP_PIN, HIGH);
    digitalWrite(RIGHT_STEP_PIN, LOW);
    digitalWrite(LEFT_STEP_PIN, HIGH);
    digitalWrite(LEFT_STEP_PIN, LOW);
    delay(10); // 5 ms * 200 = 1 second
  }
}

// Даний метод використовується для вимірювання дистанції спереду, справа та зліва від безпілотника. Для цього він використовує три модуля HC-SR04.
void CalculateDistances()
{
  digitalWrite(FORWARD_TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(FORWARD_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(FORWARD_TRIG_PIN, LOW);
  frontDistance = (pulseIn(FORWARD_ECHO_PIN, HIGH) * 0.034 / 2); // Спершу він надсилає імпульс, та засікає час, поки він отримає цей ж самий сигнал, відбитий від стіни.
  Serial.print(" Distance in front: ");
  Serial.print(frontDistance);

  digitalWrite(LEFT_TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(LEFT_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(LEFT_TRIG_PIN, LOW);
  leftDistance = (pulseIn(LEFT_ECHO_PIN, HIGH) * 0.034 / 2);
  Serial.print(" Distance on the left: ");
  Serial.println(leftDistance);
}


void setup() {
  // Налагоджуємо роботу усіх модулів, та виставляємо їх у необхідні нам режими.
  Serial.begin(9600);
  // Підключаємо піни HC-SR04
  pinMode(FORWARD_TRIG_PIN, OUTPUT);
  pinMode(FORWARD_ECHO_PIN, INPUT);
  pinMode(LEFT_TRIG_PIN, OUTPUT);
  pinMode(LEFT_ECHO_PIN, INPUT);
  // Підключаємо піни драйверів
  pinMode(RIGHT_STEP_PIN, OUTPUT);
  pinMode(RIGHT_DIR_PIN, OUTPUT);
  pinMode(LEFT_STEP_PIN, OUTPUT);
  pinMode(LEFT_DIR_PIN, OUTPUT);
  digitalWrite(RIGHT_STEP_PIN, LOW);
  digitalWrite(LEFT_STEP_PIN, LOW);
}

void loop() {
  // Раз за разом вимірюємо дистанцію до стін навколо безпілотника
  CalculateDistances();
  // Далі - в залежності від того, яка дистанція до стіни попереду ,ми вирішуємо, що робити. Чи рухатись прямо, чи визначити, в якому напрямку нам повертати.
  if(frontDistance >= 10)
  {
    FullForward();
    Serial.println("FullForward ");
  }
  else
  {
    // якщо ми до однієї з стін відстань буде більше 10 см, то значить - в нас є простір для подальшого маневру в цьому напрямку. Після повороту рухаємось прямо, аби знову отримати стіни у своє поле зору.
    if(leftDistance >= 10)
    {
      TurnRight();
      Serial.println("Turn Right ");
      FullForward();
    }
    else
    {
      TurnLeft();
      Serial.println("Turn Left ");
      FullForward();
    }
  }
  delay(500);
}