#define IN1_1 14  // Motor 1 - vstupy
#define IN2_1 12
#define IN3_1 27  // Motor 2 - vstupy
#define IN4_1 26

#define TRIG 23  // Ultrazvukový senzor - Trig
#define ECHO 22  // Ultrazvukový senzor - Echo

int delayTime = 5;  // Rýchlosť otáčania
long duration;
int distance;
bool motor2_active = false; // Flag pre aktiváciu motora 2

void stepMotor(int step, int in1, int in2, int in3, int in4) {
  switch (step) {
    case 0: digitalWrite(in1, HIGH); digitalWrite(in2, LOW); digitalWrite(in3, LOW); digitalWrite(in4, LOW); break;
    case 1: digitalWrite(in1, LOW); digitalWrite(in2, HIGH); digitalWrite(in3, LOW); digitalWrite(in4, LOW); break;
    case 2: digitalWrite(in1, LOW); digitalWrite(in2, LOW); digitalWrite(in3, HIGH); digitalWrite(in4, LOW); break;
    case 3: digitalWrite(in1, LOW); digitalWrite(in2, LOW); digitalWrite(in3, LOW); digitalWrite(in4, HIGH); break;
  }
  delay(delayTime);
}

void setup() {
  pinMode(IN1_1, OUTPUT);
  pinMode(IN2_1, OUTPUT);
  pinMode(IN3_1, OUTPUT);
  pinMode(IN4_1, OUTPUT);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  Serial.begin(115200);
}

void loop() {
  // Meranie vzdialenosti
  digitalWrite(TRIG, LOW);
  delay(2);
  digitalWrite(TRIG, HIGH);
  delay(10);
  digitalWrite(TRIG, LOW);
  duration = pulseIn(ECHO, HIGH);
  distance = duration * 0.034 / 2;
  Serial.print("Vzdialenosť: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Motor 1 sa neustále otáča
  static int step1 = 0;
  stepMotor(step1 % 4, IN1_1, IN2_1, IN3_1, IN4_1);
  step1++;

  // Aktivácia a deaktivácia motora 2 na základe vzdialenosti
  static int step2 = 0;
  if (distance < 10) {
    motor2_active = false;
  } else {
    motor2_active = true;
  }

  if (motor2_active) {
    Serial.println("Motor 2 spustený");
    stepMotor(step2 % 4, IN3_1, IN4_1, IN1_1, IN2_1);
    step2++;
  } else {
    Serial.println("Motor 2 zastavený");
    step2 = 0; // Reset step counter when motor 2 stops
  }
}