#include <Stepper.h>

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

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

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

// Наперед називємо необхідні нам піни для роботи Датчиками колізії
#define LEFT_COL_PIN 1
#define FORWARD_LEFT_COL_PIN 0
#define FORWARD_RIGHT_COL_PIN 19
#define RIGHT_COL_PIN 18

// Рухатись суворо вперед, заставивши обидві сторони крутитись в напрямку руху уперед.
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.println(frontDistance);
}


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

  digitalWrite(RIGHT_STEP_PIN, LOW);
  digitalWrite(LEFT_STEP_PIN, LOW);
}


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

  int collisionsStatuses[4] = {0,0,0,0};
  collisionsStatuses[0] = digitalRead(LEFT_COL_PIN);
  collisionsStatuses[1] = digitalRead(FORWARD_LEFT_COL_PIN);
  collisionsStatuses[2] = digitalRead(FORWARD_RIGHT_COL_PIN);
  collisionsStatuses[3] = digitalRead(RIGHT_COL_PIN);

  for(int i = 0; i < 4; i++)
  {
    if (collisionsStatuses[i] == LOW) {   // check if the collision key is locked
    Serial.println("Collision detected!");
    }
  }
  
  delay(500);
}