#include <Servo.h>

const int NUM_SERVOS = 5;

// Povezivanje pinova servoa (D3, D5, D6, D9, D10)
const int servoPins[NUM_SERVOS] = {3, 5, 6, 9, 10};

// Povezivanje tastera (A0 do A4)
const int buttonPins[NUM_SERVOS] = {A0, A1, A2, A3, A4};

// Prekidač za smer (A5)
const int directionSwitchPin = A5;

// Servo objekti
Servo servos[NUM_SERVOS];

// Trenutni ugao svakog serva (početno 90 stepeni)
int angles[NUM_SERVOS] = {90, 90, 90, 90, 90};

void setup() {
  // Inicijalizuj servo motore i tastere
  for (int i = 0; i < NUM_SERVOS; i++) {
    servos[i].attach(servoPins[i]);
    servos[i].write(angles[i]);
    pinMode(buttonPins[i], INPUT_PULLUP); // Tasteri kao INPUT_PULLUP
  }

  // Prekidač za smer
  pinMode(directionSwitchPin, INPUT);
}

void loop() {
  // Čitanje smera iz prekidača
  bool direction = digitalRead(directionSwitchPin); // HIGH = desno, LOW = levo

  // Prolazak kroz sve tastere i motore
  for (int i = 0; i < NUM_SERVOS; i++) {
    if (digitalRead(buttonPins[i]) == LOW) {
      // Taster pritisnut – pomeri servo u zadatom smeru
      if (direction && angles[i] < 180) {
        angles[i]++;
      } else if (!direction && angles[i] > 0) {
        angles[i]--;
      }

      // Ažuriraj poziciju servoa
      servos[i].write(angles[i]);
      delay(15); // Kontrola brzine kretanja
    }
  }
}