int emgVal1 = 0;
int emgVal2 = 0; //Od 0 do 1023
int tresh = 750;
int motNapred[] = {11,12,13}; //palec pokazalec ostali
int motNazad[] = {17,14,13};
int lev[] = {0,0,0};
int numLevels = 1;
int vreme = 2000; //ima x leveli,najdi timing!
int glavenDelay = 50;
int niza[][3] = { {1,0,1}, {1,1,1}, {1,1,0}, {0,0,0} };
bool vlezeni[3] = { false, false, false};

void setup() {
  //Turi pinMode

  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(motNapred[0], OUTPUT);
  pinMode(motNapred[1], OUTPUT);
  pinMode(motNapred[2], OUTPUT);
  pinMode(motNazad[0], OUTPUT);
  pinMode(motNazad[1], OUTPUT);
  pinMode(motNazad[2], OUTPUT);

  Serial.begin(9600);
}

int level(int val) {
  //Od tresh do 1,2,3

  return ceil((val-tresh)/double(1023-tresh)*(numLevels));
}

void mrdni(int smeni[3]) {
  int razLeveli[3];

  for (int i = 0; i<3; i++) {
     razLeveli[i] = smeni[i]-lev[i];

     if (vlezeni[i] == false) { digitalWrite(motNazad[i], HIGH); Serial.println("Teraj!"); }
     if (vlezeni[i] == true) { digitalWrite(motNapred[i], HIGH); Serial.println("Vrakjaj!"); }
  }

  int dileji[3] = {0,0,0};

  for (int i = 0; i<3; i++) dileji[i] = abs(razLeveli[i])*(vreme/numLevels);

  //Najdi raspored na delays

  int a = dileji[0], b = dileji[1], c = dileji[2];
  int a2,b2,c2;

  if (a <= b & a <= c) {
     if (b <= c) a2 = 0, b2 = 1, c2 = 2;
     else a2 = 0, b2 = 2, c2 = 1;
  } else if (b <= a && b <= c) {
     if (a <= c) a2 = 1, b2 = 0, c2 = 2;
     else a2 = 1, b2 = 2, c2 = 0;
  } else if (c <= a && c <= b) {
     if (a <= b) a2 = 2, b2 = 0, c2 = 1;
     else a2 = 2, b2 = 1, c2 = 0;
  }

  int rasp[3] = {a2,b2,c2};
  int dileji2[3];

  dileji2[0] = dileji[rasp[0]];
  dileji2[1] = dileji[rasp[1]]-dileji2[0];
  dileji2[2] = dileji[rasp[2]]-dileji2[1];

  //Gotovo

  for (int i = 0; i<3; i++) {
     lev[i] = smeni[i];

     if (razLeveli[i] <= 0) continue;

     delay(dileji2[i]);

     Serial.println(dileji2[i]);

    // if (vlezeni[i] == false) digitalWrite(motNazad[i], LOW);
    // if (vlezeni[i] == true) digitalWrite(motNapred[i], LOW);

     vlezeni[i] = !vlezeni[i];
  }
}

void loop() {
   emgVal1 = analogRead(A1);
  // emgVal2 = analogRead(A2);
   emgVal2 = 0;

   bool prv = emgVal1 >= tresh;
   bool vtor = emgVal2 >= tresh;

   int lev1 = 0, lev2 = 0, levP = 0;

   //samo pokazalec (inverse), site 3, palec i pokazalec

   if (prv && !vtor) {
       lev1 = level(emgVal1);

     //  Serial.println("Palec, posledni 3!");
    //   Serial.println(lev1, DEC);

       for (int i = 0; i<3; i++) if (niza[0][i]) niza[0][i] = lev1;

       mrdni(niza[0]);
   } else if (!prv && vtor) {
       lev2 = level(emgVal2);

     //  Serial.println("Palec, pokazalec, posledni 3!");
     //  Serial.println(lev2, DEC);

       for (int i = 0; i<3; i++) if (niza[1][i]) niza[1][i] = lev2;

       mrdni(niza[1]);
   } else if (prv && vtor) {
       levP = level( (emgVal1+emgVal2) / 2);

    //   Serial.println("Palec, pokazalec!");
     //  Serial.println(levP, DEC);

       for (int i = 0; i<3; i++) if (niza[2][i]) niza[2][i] = levP;

       mrdni(niza[2]);
   }

   delay(glavenDelay);
}