#include <Servo.h>

// Pin definicije
const int uz1TrigPin = 5;
const int uz1EchoPin = 4;
const int uz2TrigPin = 7;
const int uz2EchoPin = 6;

const int srv1Pin = 9;
const int srv2Pin = 10;

const int btn1Pin = 13;

const int blueLedPin = 12;
const int greenLedPin = 11;

// Globalne spremenljivke
Servo srv1;
Servo srv2;

int uz1Distance;
int uz2Distance;
int srv1Angle = 0;  // Začetni kot za servo motor 1
int srv2Angle = 0;  // Začetni kot za servo motor 2

void setup() {
  Serial.begin(9600);

  pinMode(uz1TrigPin, OUTPUT);
  pinMode(uz1EchoPin, INPUT);
  pinMode(uz2TrigPin, OUTPUT);
  pinMode(uz2EchoPin, INPUT);

  pinMode(srv1Pin, OUTPUT);
  pinMode(srv2Pin, OUTPUT);

  pinMode(btn1Pin, INPUT);

  pinMode(blueLedPin, OUTPUT);
  pinMode(greenLedPin, OUTPUT);

  srv1.attach(srv1Pin);
  srv2.attach(srv2Pin);
}

void loop() {
  // Merjenje razdalje za ultrazvočne senzorje
  uz1Distance = getDistance(uz1TrigPin, uz1EchoPin);
  uz2Distance = getDistance(uz2TrigPin, uz2EchoPin);

  // Premik servo motorjev
  srv1Angle = map(uz1Distance, 0, 100, 0, 180);
  srv2Angle = map(uz1Distance, 0, 100, 180, 0);

  // Stabilizacija za servo motorje
  stabilizeServos();

  // Upravljanje LED lučk
  if (uz1Distance < uz2Distance) {
    digitalWrite(greenLedPin, HIGH);
    digitalWrite(blueLedPin, LOW);
  } else {
    digitalWrite(greenLedPin, LOW);
    digitalWrite(blueLedPin, HIGH);
  }

  // Izpis na Serial Monitor
  Serial.print("uz1:");
  Serial.print(uz1Distance);
  Serial.print("cm ");
  Serial.print("srv1:");
  Serial.print(srv1Angle);
  Serial.print("° ");
  Serial.print("uz2:");
  Serial.print(uz2Distance);
  Serial.print("cm ");
  Serial.print("srv2:");
  Serial.println(srv2Angle);

  // Preverjanje stanja gumba
  if (digitalRead(btn1Pin) == HIGH) {
    srv2.write(srv2Angle);
    delay(500);  // Dopolnite glede na potrebe
  }

  delay(500);  // Dopolnite glede na potrebe
}

int getDistance(int trigPin, int echoPin) {
  // Koda za merjenje razdalje z ultrazvočnim senzorjem
  // Dopolnite glede na vaš senzor
}

void stabilizeServos() {
  // Dodajte kodo za stabilizacijo servo motorjev, glede na nihanje razdalje
  // Ena možnost je, da uporabite povprečje zadnjih nekaj meritev in to uporabite za nastavitev kota servo motorja.
}