#include <Servo.h>     //zahrnutí knihovny pro ovládání servo motoru


class Blikacka{
public:
  Blikacka(int pin); //kosntruktor 
  void blikej(int interval);
  void neblikej();
  private:
  int pin;
  bool stav = false;
  long posledniZmena;
};

Blikacka::Blikacka(int pin){
  if(pin<0 || pin>13) pin=13;
  this->pin=pin;
  pinMode(pin, OUTPUT); //nepotrebuji inicialiyovat v setupu
}

void Blikacka::blikej(int interval){
 if (interval <=0) interval=250;

 if ((millis()-posledniZmena)>interval){
 stav=!stav;
 posledniZmena=millis();
 }
 digitalWrite(pin, stav);
}

/*void Blikacka::souhra(){
  digitalWrite(pin, HIGH);
  digitalWrite(pin, LOW);
  

 }*/

void Blikacka::neblikej() {
digitalWrite(pin, LOW);
} //Zkopírováno z předchozí úlohy (Úloha blikačka)

Blikacka X(3); // bila
Blikacka Y(5); // leva modra
Blikacka Z(6); // prava modra



Servo myservo;         //každý motor má svou instanci třídy Servo
Servo myservo1;         //každý motor má svou instanci třídy Servo

int sensor1 = 2;
int sensor2 = 4;

int hodnotaSenzoru1 = 0;
int hodnotaSenzoru2 = 0;

int pos = 0;           //proměnná obsahující pozici motoru (úhel natočení)

long previousMillis;  // Proměnná pro ukládání času poslední akce
const long interval = 5000;        // Interval v milisekundách
const long interval2 = 10000;  
bool stavZ = false;

void setup()
{
  myservo.attach(9);   //tento motor je připojen na pin 9
  myservo1.attach(10);
  Serial.begin(9600);

  pinMode(sensor1, INPUT);
  pinMode(sensor2, INPUT);
}

bool stavL = false;
bool stavLL = false;

void loop()
{ 

  hodnotaSenzoru1 = digitalRead(sensor1);
  hodnotaSenzoru2 = digitalRead(sensor2);

  /*

  Pirsenzor v simulátoru detekuje pouze 5 sekund, prosím brát ohledy na tento fakt 


  */

if (hodnotaSenzoru1  == HIGH || hodnotaSenzoru2  == HIGH) 
{
  if(!stavL){
   previousMillis = millis();
   stavL = true;
  }
  
  X.neblikej();
  Z.neblikej();
  Y.blikej(1);
  delay(500);
  Z.blikej(1);
  Y.neblikej();
  delay(500);
  Serial.println(millis() - previousMillis);
  if (millis() - previousMillis >= interval) {
    // ulož aktuální čas jako poslední akci
    previousMillis = millis();
  stavL = false;
  pos = 90;
  myservo.write(pos);  //natočení motoru na aktuální úhel
  myservo1.write(pos);
  delay(500);
  stavLL = true;;
}
}

else{
  if (millis() - previousMillis >= interval) {
    Serial.println("test");
    if(stavLL == true){
    X.neblikej();
  Z.neblikej();
  Y.blikej(1);
  delay(500);
  Z.blikej(1);
  Y.neblikej();
  delay(500);
    }
    pos = 0; 
    myservo.write(pos);  //natočení motoru na aktuální úhel
    myservo1.write(pos);
  }

 
  if (millis() - previousMillis >= interval2) {
    stavLL = false;
    if(stavLL == false){
  X.blikej(1000); 
  Y.neblikej();
  Z.neblikej();
    }
  }
  
  }
    
}


/*if (hodnotaSenzoru1  == LOW && hodnotaSenzoru2  == LOW) {
  switch(hodnotaSenzoru1 && hodnotaSenzoru1 ){
    case HIGH:
      X.neblikej();pos = 90;    
  Y.blikej(250);
  Z.blikej(500);
    myservo.write(pos);  //natočení motoru na aktuální úhel
    myservo1.write(pos);
      break;

    default:
    X.blikej(1000);
    pos = 0;    
    Y.neblikej();
    Z.neblikej();
    myservo.write(pos);  //natočení motoru na aktuální úhel
    myservo1.write(pos);
    break;
  }
}*/