#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
const int servoPin = 11;    // Servo motor

// Servo setup
Servo myServo;

// 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);

  // Buzzer setup
  pinMode(buzzerPin, OUTPUT);

  // Servo setup
  myServo.attach(servoPin);
  myServo.write(90); // Start in the center position (90°)

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

  // Set initial direction for stepper motor
  digitalWrite(dirPin, HIGH);
}

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) {
    Serial.println("Object detected! Taking action.");

    // Step 1: Buzzer alert
    digitalWrite(buzzerPin, HIGH);  // Activate buzzer
    delay(500);                     // Wait 0.5 seconds
    digitalWrite(buzzerPin, LOW);   // Deactivate buzzer

    // Step 3: Move stepper motor back
    digitalWrite(dirPin, LOW);     // Set direction to back
    for (int i = 0; i < 200; i++) { // Move 200 steps back
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(800);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(800);
    }

    // Step 2: Turn servo to simulate turning
    myServo.write(45);              // Turn servo to 45 degrees (left)
    delay(1000);                    // Hold position for 1 second

    // Step 3: Move stepper motor forward
    digitalWrite(dirPin, HIGH);     // Set direction to forward
    for (int i = 0; i < 200; i++) { // Move 200 steps forward
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(800);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(800);
    }

    // Step 4: Turn servo to the opposite side
    myServo.write(135);             // Turn servo to 135 degrees (right)
    delay(1000);                    // Hold position for 1 second

    // Step 5: Move stepper motor forward again
    for (int i = 0; i < 200; i++) { // Move 200 steps forward
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(800);
      digitalWrite(stepPin, LOW);
      delayMicroseconds(800);
    }

    // Step 6: Align servo back to the center
    myServo.write(90);              // Reset servo to 90 degrees (center)
    delay(500);                     // Wait 0.5 seconds for stabilization
  } else {
    // Normal operation: keep the motor rotating forward
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(800);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(800);
  }

  delay(100); // Small delay before the next loop
}