#include <Servo.h>

// Pin definition
const int trigPin = 9;  // HC-SR04 TRIG
const int echoPin = 8;  // HC-SR04 ECHO
const int stepPin = 13;  // A4988 STEP
const int dirPin = 12;   // A4988 DIR
const int buzzerPin = 10;  // Piezo bzučák

// Variables for distance measurement
long duration;
int distance;

void setup() {
  // HC-SR04 setup
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Stepper motor setup
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);

  // Set initial direction for stepper motor
  digitalWrite(dirPin, HIGH); // Motor bude točit jedním směrem

  // Initialize serial communication
  Serial.begin(9600);
}

void loop() {
  // Measure distance
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" mm");

  // Check if the object is closer than 55 mm
  if (distance < 55) {
    // Stop stepper motor
    Serial.println("Object detected! Stopping motor.");
    delay(1000);  // Wait for 1 second
    // Activate buzzer
    digitalWrite(buzzerPin, HIGH);  // Zapnutí bzučáku
    delay(500);                     // Bzučák zapnutý na 0,5 sekundy
    digitalWrite(buzzerPin, LOW);   // Vypnutí bzučáku
  } else {
    // Rotate stepper motor
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(800);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(800);
  }
}