#include <Servo.h>

// Definícia registrov
#define TLACIDLA_REG PORTD // Port pre tlačidlá
#define TLACIDLA_DDR DDRD  // DDR pre tlačidlá
#define SERVO_REG PORTB    // Port pre servo motory
#define SERVO_DDR DDRB     // DDR pre servo motory

// Bitové masky pre tlačidlá
#define MODRE_BIT 0 // Prvé tlačidlo, bit 0
#define CERVENE_BIT 1 // Druhé tlačidlo, bit 1
#define ZELENE_BIT 2 // Tretie tlačidlo, bit 2

// Pin pre servo motory
const int servo1Pin = 0; // Pin pre servo 1 (PB0)
const int servo2Pin = 1; // Pin pre servo 2 (PB1)
const int servo3Pin = 2; // Pin pre servo 3 (PB2)

// Objekty pre servo motory
Servo servo1;
Servo servo2;
Servo servo3;

void setup() {
  // Nastavenie tlačidiel ako vstupov
  TLACIDLA_DDR &= ~(1 << MODRE_BIT); // Modré tlačidlo
  TLACIDLA_DDR &= ~(1 << CERVENE_BIT); // Červené tlačidlo
  TLACIDLA_DDR &= ~(1 << ZELENE_BIT); // Zelené tlačidlo

  // Nastavenie servo motorov ako výstupov
  SERVO_DDR |= (1 << servo1Pin); // Servo 1
  SERVO_DDR |= (1 << servo2Pin); // Servo 2
  SERVO_DDR |= (1 << servo3Pin); // Servo 3

  // Inicializácia servo motorov
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  servo3.attach(servo3Pin);
}

void loop() {
  // Kontrola stlačenia tlačidiel
  if (!(TLACIDLA_REG & (1 << MODRE_BIT))) {
    otocServo(servo1, 90); // Otoc servo 1 o 90 stupňov
    delay(1000); // Počkaj 1 sekundu
  } else if (!(TLACIDLA_REG & (1 << CERVENE_BIT))) {
    otocServo(servo2, 90); // Otoc servo 2 o 90 stupňov
    delay(3000); // Počkaj 3 sekundy
  } else if (!(TLACIDLA_REG & (1 << ZELENE_BIT))) {
    otocServo(servo3, 90); // Otoc servo 3 o 90 stupňov
    delay(3000); // Počkaj 3 sekundy
  }
}

// Funkcia na otáčanie servo motora na daný uhol
void otocServo(Servo servo, int uhol) {
  servo.write(uhol);
}