#include <Servo.h>
unsigned long int future = 0;
Servo myservo;
int stav = 0;
#define set1(r,b) r|=(1<<b)
#define set0(r,b) r&=~(1<<b)
#define test(r,b) (r&(1<<b))
bool test1 = true;
bool test2 = true;
bool test3 = true;
bool test4 = true;

void setup() {
future = millis();
myservo.attach(3);
myservo.write(90);
set0(DDRD,7); 
set1(PORTD,7); 
set0(DDRB,0); 
set1(PORTB,0); 
set0(DDRB,1); 
set1(PORTB,1); 
set0(DDRC,3); 
set1(PORTC,3); 

set1(DDRD,4); 
set1(PORTD,4); 
set1(DDRD,5); 
set1(PORTD,5); 
set1(DDRD,6); 
set1(PORTD,6); 
set1(SREG,7);

set1(PCICR,PCIE0);
set1(PCMSK0,PCINT0);

set1(PCICR,PCIE0);
set1(PCMSK0,PCINT1);

set1(PCICR,PCIE2);
set1(PCMSK2,PCINT23);

set1(PCICR,PCIE1);
set1(PCMSK1,PCINT11);

switch(stav)
{
  case 0: (set0(DDRD,4)); break;
  case 1: (set1(DDRD,4)); (set1(DDRD,5)); (set1(DDRD,6));break;
  case 2: (set0(DDRD,4)); (set0(DDRD,5)); (set0(DDRD,6));break;
  case 3: (set1(DDRD,4)); (set1(DDRD,5)); (set1(DDRD,6));break;
  default: break;
}

}

void loop() {
  // put your main code here, to run repeatedly:

}

ISR(PCINT0_vect)
{
  if((test(PINB,0)==0) && test1 == true){
    myservo.write(180);
    test1 = false;
  }else if((test(PINB,0)!=0) && test1 == false){
    myservo.write(90);
    test1 = true;
  }

  if((test(PINB,1)==0) && test2 == true){
    myservo.write(0);
    test2 = false;
  }else if((test(PINB,1)!=0) && test2 == false){
    myservo.write(90);
    test2 = true;
  }
}

ISR(PCINT2_vect)
{
  if((test(PIND,7)==0) && test3 == true)
  {
    if(millis() > future)
    {
      stav++; // kod, ktory sa vykona raz za pol sekundy
      if(stav == 2)
      stav = 0;
    }
    future = millis() + 500;
    test3 == false;
  }
  else if((test(PIND,7)!=0) && test3 == false)
  {
    stav = 0;
    test3 = true;
  }
}

ISR(PCINT1_vect)
{
  if((test(PINC,3)==0) && test4 == true)
  {
    if(millis() > future)
    {
      stav++;
      if(stav == 4)
      stav = 2;
    }
    future = millis() + 500;
    test4 == false;
  }
  else if((test(PIND,7)!=0) && test3 == false)
  {
    stav = 0;
    test4 = true;
  }
}