#include <Servo.h>



// constants won't change
const int BUTTON_01_PIN = A0; // Arduino pin connected to button's pin
const int SERVO_01_PIN  = 9; // Arduino pin connected to servo motor's pin

const int BUTTON_02_PIN = A1; // Arduino pin connected to button's pin
const int SERVO_02_PIN  = 3; // Arduino pin connected to servo motor's pin

const int BUTTON_03_PIN = A2; // Arduino pin connected to button's pin
const int SERVO_03_PIN  = 6; // Arduino pin connected to servo motor's pin

const int BUTTON_04_PIN = A3; // Arduino pin connected to button's pin
const int SERVO_04_PIN  = 11; // Arduino pin connected to servo motor's pin


//** deklarácie závora

int sensePin1=1; // analog Pin A0 on the arduino
int sensePin2=0; // analog Pin A1 on the arduino
int RED1= A6; // light on crossing signal
int RED2= A7;  // light on crossing signal
int POWER=8; // POWER pin 8
int flashcount;

//** deklarácie závora



Servo servo_vyhybka_01; // create servo object to control a servo
Servo servo_vyhybka_02; // create servo object to control a servo
Servo servo_vyhybka_03; // create servo object to control a servo
Servo servo_vyhybka_04; // create servo object to control a servo


// variables will change:
int angle_01 = 58;          // the current angle of servo motor
int lastButtonState_01;    // the previous state of button
int currentButtonState_01; // the current state of button

int angle_02 = 58;          // the current angle of servo motor
int lastButtonState_02;    // the previous state of button
int currentButtonState_02; // the current state of button

int angle_03 = 58;          // the current angle of servo motor
int lastButtonState_03;    // the previous state of button
int currentButtonState_03; // the current state of button

int angle_04 = 58;          // the current angle of servo motor
int lastButtonState_04;    // the previous state of button
int currentButtonState_04; // the current state of button


unsigned long lasttime_button_01_change = millis();
unsigned long lasttime_button_02_change = millis();
unsigned long lasttime_button_03_change = millis();
unsigned long lasttime_button_04_change = millis();

unsigned long debounceDuration = 500;

void setup() {
  Serial.begin(9600);                // initialize serial
  
  pinMode(BUTTON_01_PIN, INPUT_PULLUP); // set arduino pin to input pull-up mode
  servo_vyhybka_01.attach(SERVO_01_PIN);           // attaches the servo on pin 9 to the servo object

  pinMode(BUTTON_02_PIN, INPUT_PULLUP); // set arduino pin to input pull-up mode
  servo_vyhybka_02.attach(SERVO_02_PIN);           // attaches the servo on pin 9 to the servo object
  
  pinMode(BUTTON_03_PIN, INPUT_PULLUP); // set arduino pin to input pull-up mode
  servo_vyhybka_03.attach(SERVO_03_PIN);           // attaches the servo on pin 9 to the servo object

 pinMode(BUTTON_04_PIN, INPUT_PULLUP); // set arduino pin to input pull-up mode
  servo_vyhybka_04.attach(SERVO_04_PIN);           // attaches the servo on pin 9 to the servo object



  
  pinMode(0, INPUT);
  pinMode(1, INPUT);



   pinMode(4, OUTPUT);
   pinMode(2, OUTPUT);
   
   pinMode(10, OUTPUT);
   pinMode(8, OUTPUT);

   pinMode(7, OUTPUT);
   pinMode(13, OUTPUT);
  
   pinMode(A4, OUTPUT);
   pinMode(A5, OUTPUT);

   pinMode(A6, OUTPUT);
   pinMode(A7, OUTPUT);

  

   


  
  
  servo_vyhybka_01.write(angle_01);
  currentButtonState_01 = digitalRead(BUTTON_01_PIN);
     
  servo_vyhybka_02.write(angle_02);
  currentButtonState_02 = digitalRead(BUTTON_02_PIN);
  
  servo_vyhybka_03.write(angle_03);
  currentButtonState_03 = digitalRead(BUTTON_03_PIN);

  servo_vyhybka_04.write(angle_04);
  currentButtonState_04 = digitalRead(BUTTON_04_PIN);
  
  
   digitalWrite (2,HIGH); // uvodne rozsvietenie zelenych diod
    delay(300); // Wait for 300 millisecond(s)
  digitalWrite (8,HIGH); // uvodne rozsvietenie zelenych diod
     delay(300); // Wait for 300 millisecond(s)
  digitalWrite (7,HIGH); // uvodne rozsvietenie zelenych diod
     delay(300); // Wait for 300 millisecond(s)
  digitalWrite (A4,HIGH); // uvodne rozsvietenie zelenych diod
     delay(300); // Wait for 300 millisecond(s)
  
  
}

void loop() {

if (millis() - lasttime_button_01_change >= debounceDuration) {

  lastButtonState_01    = currentButtonState_01;     // save the last state
  currentButtonState_01 = digitalRead(BUTTON_01_PIN); // read new state

  if(lastButtonState_01 == HIGH && currentButtonState_01 == LOW) {
    lasttime_button_01_change = millis();
        Serial.println("The button_1 is pressed");

    // change angle of servo motor
    if(angle_01 == 58)
      angle_01 = 100, digitalWrite (4,HIGH), digitalWrite (2,LOW);
    else
    if(angle_01 == 100)
      angle_01 = 58, digitalWrite (2,HIGH), digitalWrite (4,LOW);

    // control servo motor arccoding to the angle
    servo_vyhybka_01.write(angle_01);
    
    }
    }
    

  if (millis() - lasttime_button_02_change >= debounceDuration) {
  lastButtonState_02    = currentButtonState_02;     // save the last state
  currentButtonState_02 = digitalRead(BUTTON_02_PIN); // read new state

  if(lastButtonState_02 == HIGH && currentButtonState_02 == LOW) {
    lasttime_button_02_change = millis();
    Serial.println("The button_2 is pressed");

    // change angle of servo motor
    if(angle_02 == 58)
      angle_02 = 100, digitalWrite (10,HIGH), digitalWrite (8,LOW);
    else
    if(angle_02 == 100)
      angle_02 = 58, digitalWrite (8,HIGH), digitalWrite (10,LOW);

    // control servo motor arccoding to the angle
    servo_vyhybka_02.write(angle_02);
    
   }
   } 
     
  
  
  if (millis() - lasttime_button_03_change >= debounceDuration) {  
    
  lastButtonState_03    = currentButtonState_03;     // save the last state
  currentButtonState_03 = digitalRead(BUTTON_03_PIN); // read new state

  if(lastButtonState_03 == HIGH && currentButtonState_03 == LOW) {
    lasttime_button_03_change = millis();
    Serial.println("The button_3 is pressed");

    // change angle of servo motor
    if(angle_03 == 58)
      angle_03 = 100, digitalWrite (13,HIGH), digitalWrite (7,LOW);
    else
    if(angle_03 == 100)
      angle_03 = 58, digitalWrite (7,HIGH), digitalWrite (13,LOW);

    // control servo motor arccoding to the angle
    servo_vyhybka_03.write(angle_03);
  
  
  }
}
  
  if (millis() - lasttime_button_04_change >= debounceDuration) {  
    
  lastButtonState_04    = currentButtonState_04;     // save the last state
  currentButtonState_04 = digitalRead(BUTTON_04_PIN); // read new state

  if(lastButtonState_04 == HIGH && currentButtonState_04 == LOW) {
    lasttime_button_04_change = millis();
    Serial.println("The button_4 is pressed");

    // change angle of servo motor
    if(angle_04 == 58)
      angle_04 = 100, digitalWrite (A5,HIGH), digitalWrite (A4,LOW);
    else
    if(angle_04 == 100)
      angle_04 = 58, digitalWrite (A4,HIGH), digitalWrite (A5,LOW);

    // control servo motor arccoding to the angle
    servo_vyhybka_04.write(angle_04);
  }

}

}