#include <Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;

int cervena = 0;
int zelena = 0;
int modra = 0;

bool test_c = true;
bool test_z = true;
bool test_m = true;




void setup() 
{
  DDRD |= (1 << 2) | (1 << 3) | (1 << 4);  // Nastaví piny 2, 3, 4 ako výstupy (RGB LED)
  PORTD &= ~((1 << 2) | (1 << 3) | (1 << 4));  // Všetky farby nastaví na LOW

  //TLACITKA
  DDRB &= ~(1 << 0); // D8 ako vstup
  DDRB &= ~(1 << 1); // D9 ako vstup
  DDRB &= ~(1 << 2); // D10 ako vstup
  
  //PULLUP
  PORTB |= (1 << 0) | (1 << 1) | (1 << 2);
  
  Serial.begin(9600);
  
  
  servo1.attach(5);
  servo2.attach(6);
  servo3.attach(7);
  
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  
}

void loop() 
{
  if (PINB & (1<<5)){
    
    //MOTOR	
  	PORTB |= (1 << 3); // IN1 HIGH
  	PORTB &= ~(1 << 4); // IN2 LOW
    
    //CERVENA
    if ((PINB & (1 << 0)) == 0 && test_c == true) {
      	test_c = false;
      
      	cervena++;
      
       	PORTD |= (1 << 4);  // Zapne červenú
  		delay(500);
  		PORTD &= ~(1 << 4); // Vypne červenú
      	
      	servo1.write(90);
      	delay(2000);
      	servo1.write(0);
      	
      	Serial.print("Cervena stlacena = ");
  		Serial.println(cervena);
    
    	Serial.print("zelena stlacena = ");
  		Serial.println(zelena);
    
    	Serial.print("Modra stlacena = ");
  		Serial.println(modra);
      
      	Serial.println("---------------------------------");
    }
    
    //ZELENA
    else if ((PINB & (1 << 1)) == 0 && test_z == true) {
      	test_z = false;
      
      	zelena++;
      
      	PORTD |= (1 << 3);  // Zapne zelenú
  		delay(500);
  		PORTD &= ~(1 << 3); // Vypne zelenú
      
      	servo2.write(90);
      	delay(2000);
      	servo2.write(0);
      
      	Serial.print("Cervena stlacena = ");
  		Serial.println(cervena);
    
    	Serial.print("zelena stlacena = ");
  		Serial.println(zelena);
    
    	Serial.print("Modra stlacena = ");
  		Serial.println(modra);
      
      	Serial.println("---------------------------------");
    }
    
    //MODRA
    else if ((PINB & (1 << 2)) == 0 && test_m == true) {
      	test_m = false;
      
      	modra++;
      
      	PORTD |= (1 << 2);  // Zapne modrú
  		delay(500);
  		PORTD &= ~(1 << 2); // Vypne modrú
      
      	servo3.write(90);
      	delay(2000);
      	servo3.write(0);
      
      	Serial.print("Cervena stlacena = ");
  		Serial.println(cervena);
    
    	Serial.print("zelena stlacena = ");
  		Serial.println(zelena);
    
    	Serial.print("Modra stlacena = ");
  		Serial.println(modra);
      
      	Serial.println("---------------------------------");
    }
    
    	
    
    //TESTY
    
    if ((PINB & (1<<0)) != 0) { 
    test_c= true; 
  }
    if ((PINB & (1<<1)) != 0) { 
    test_z = true;
  }
    if ((PINB & (1<<2)) != 0) {
    test_m = true;
  }
    
    
    
  }
  
  else{
    PORTB |= (1 << 4); // IN1 HIGH
  	PORTB &= ~(1 << 7); // IN2 LOW
  }
  
}