#include <Servo.h>  //servomotory

int hpPin = A5;     //fotorezistor HP
int hlPin = A4;     //fotorezistor HL
int dlPin = A2;     //fotorezistor DL
int dpPin = A3;     //fotorezistor DP
int PinX0 = 3;      //piny motoru nataceni (osa x)
int PinY0 = 7;      //piny motoru naklonu (osa y)

float necitlivost_f = 0.08 * 256; //tolerance fotorezistoru

//nastaveni motoru
Servo motorX;
Servo motorY;

int naklon = 180;       //vychozi uhel naklonu
int natoceni = 180;     //vychozi uhel natoceni
int naklon_min = 120;   //minimalni uhel naklonu
int naklon_max = 180;   //maximalni uhel naklonu
int natoceni_min = 45;  //minimalni uhel natoceni
int natoceni_max = 180; //maximalni uhel natoceni
int hp = 0;             //hodnota fotorezistoru HP
int hl = 0;             //hodnota fotorezistoru HL
int dp = 0;             //hodnota fotorezistoru DP
int dl = 0;             //hodnota fotorezistoru DL
int nahore = 0;         //osvetleni horni casti
int dole = 0;           //osvetleni dolni casti
int vlevo = 0;          //osvetleni leve casti
int vpravo = 0;         //osvetleni prave casti
int rozdilX = 0;        //rozdil osvetleni v ose x
int rozdilY = 0;        //rozdil osvetleni v ose y
int pauza = 15;         //pomocna promenna

void setup() {
  pinMode(hpPin, INPUT);
  pinMode(hlPin, INPUT);
  pinMode(dpPin, INPUT);
  pinMode(dlPin, INPUT);

  motorX.attach(3);
  motorY.attach(7);
  motorX.write(natoceni);
  motorY.write(naklon);

  delay(3000);
}

void loop() {
  //ziskani hodnot z fotorezistoru a vypocet prumernych hodnot
  hp = analogRead(hpPin);
  hl = analogRead(hlPin);
  dl = analogRead(dlPin);
  dp = analogRead(dpPin);
  nahore = (hp + hl) / 2;
  dole = (dp + dl) / 2;
  vlevo = (dl + hl) / 2;
  vpravo = (hp + dp) / 2;
  rozdilX = vlevo - vpravo;
  rozdilY = nahore - dole;

  if (rozdilX > necitlivost_f){     //kontrola proti toleranci
    if (natoceni < natoceni_max){   //kontrola limitu
      natoceni = natoceni + 1;      //zvetseni uhlu natoceni
      motorX.write(natoceni);       //pokyn k pohybu motoru
       delay(pauza);                //pauza
    }
  }else if (rozdilX < -necitlivost_f){
    if (natoceni > natoceni_min){   //kontrola limitu
      natoceni = natoceni - 1;      //zmenseni uhlu natoceni
      motorX.write(natoceni);       //pokyn k pohybu motoru
       delay(pauza);                //pauza
    }
  }
  if (rozdilY > necitlivost_f){     //kontrola proti toleranci
    if (naklon < naklon_max){       //kontrola limitu
      naklon = naklon + 1;          //zvetseni uhlu natoceni
      motorY.write(naklon);         //pokyn k pohybu motoru
       delay(pauza);                //pauza
    }
  }else if (rozdilY < -necitlivost_f){
    if (naklon > naklon_min){       //kontrola limitu
      naklon = naklon - 1;          //zmenseni uhlu natoceni
      motorY.write(naklon);         //pokyn k pohybu motoru
       delay(pauza);                //pauza
    }
  }
}
