#include <LiquidCrystal_I2C.h>    // Integrace knihovny pro displej
#include <EEPROM.h>               // Integrace knohivny pro ukladani stavu nastaveni

LiquidCrystal_I2C lcd(0x27, 20, 4); // Vytvoreni objektu pro LCD displej

// Zapojeni dle MOTOR shield:
#define EN 8 		 // stepper motor enable, active low

#define X_DIR 5  // X -axis stepper motor direction control
#define Y_DIR 6  // Y -axis stepper motor direction control
#define Z_DIR 7  // Z -axis stepper motor direction control
#define A_DIR 13 // A -axis stepper motor direction control

#define X_STP 2  // X -axis stepper control
#define Y_STP 3  // Y -axis stepper control
#define Z_STP 4  // Z -axis stepper control
#define A_STP 12 // A -axis stepper control

// Zapojeni enkoderu mimo shield
#define CLK 23
#define DT 25
#define SW 27

// Zapojeni limit switchu
#define limit_nahore 51

#define limit_dole1 45
#define limit_dole2 47
#define limit_dole3 49
#define limit_dole4 43

// Promenne pro enkoder:
int currentStateCLK, lastStateCLK;
unsigned long lastButtonPress = 0;
unsigned long enkoderTlacitko;

// Promenne pro nastaveni a logiku programu:
int pocetMotoru = 4, rychlost = 2000;
int pocetPlus = 1, pocetMinus = 1, pocetCyklu = 1, hotovoCyklu, status, menu, cursor, tlacstav, pocetkroku, mereniproudu, proudzmeren, presnostKroku = 10;
bool setSpeedorCNT, work, detekceProudu = true, blinking;

int repozice = 600; // v krocich ---
int rychlostrepozice = 8000;  // rychlost nulovani pozice

// Promenne pro pocitani porudu a historie
const int maxSize = 5;
int currentIndex = 0;
bool arrayFull;

void setup() {
  // Nastaveni pinu
  pinMode(CLK, INPUT);
  pinMode(DT, INPUT);
  pinMode(SW, INPUT_PULLUP);

  pinMode(X_DIR, OUTPUT);
  pinMode(Y_DIR, OUTPUT);
  pinMode(Z_DIR, OUTPUT);
  pinMode(A_DIR, OUTPUT);

  pinMode(X_STP, OUTPUT);
  pinMode(Y_STP, OUTPUT);
  pinMode(Z_STP, OUTPUT);
  pinMode(A_STP, OUTPUT);

  pinMode(limit_nahore, INPUT_PULLUP);

  pinMode(limit_dole1, INPUT_PULLUP);
  pinMode(limit_dole2, INPUT_PULLUP);
  pinMode(limit_dole3, INPUT_PULLUP);
  pinMode(limit_dole4, INPUT_PULLUP);

  pinMode(EN, OUTPUT);
  digitalWrite(EN, LOW);    // Povoleni rizeni motoru

  // Cteni posledni pozice pro enkoder
  lastStateCLK = digitalRead(CLK);

  // Inicializace LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();

  // Nacteni nastaveni z pameti
  EEPROM.get(0, pocetMotoru);
  EEPROM.get(20, rychlost);
  EEPROM.get(40, detekceProudu);
  EEPROM.get(60, repozice);

  // Pokud je nastaveni neplatne, pouzit vychozi nastaveni
  if (pocetMotoru > 4 || pocetMotoru < 1) pocetMotoru = 4;
  if (rychlost < 100 || rychlost > 10000) rychlost = 2000;
  if (repozice < 0 || repozice > 10000) repozice = 600;
  if (detekceProudu != true || detekceProudu != false) detekceProudu = true;

  // Vypis zakladni obrazovku
  drawScreenStatus();

  Serial.begin(9600);
}


void loop() {
  // Pokud se rotacni enkoder pohnul, posun kurzor a aktualizuj nastaveni
  currentStateCLK = digitalRead(CLK);

  if (currentStateCLK != lastStateCLK  && currentStateCLK == 1) {

    if (digitalRead(DT) != currentStateCLK) {
      if (cursor > 0) cursor--;
    } else {
      if (cursor < 2 && menu == 2 || menu != 2 && menu != 4 && cursor < 3 || menu == 4 && cursor < 2) cursor++;
    }

    drawScreenStatus();
  }

  lastStateCLK = currentStateCLK;

  int btnState = digitalRead(SW);

  // Pokud je zmacknute tlacitko na enkoderu po dobu delsi nez 50ms
  if (btnState == LOW) {

    if (millis() - lastButtonPress > 50) {
      // Naviguj do ruznych menu/podmenu
      if (menu == 0 && cursor == 0) menu = 1;   // mod 1
      else if (menu == 0 && cursor == 1) menu = 2;  // mod 2
      else if (menu == 0 && cursor == 2) menu = 4;  // kalibrace
      else if (menu == 0 && cursor == 3) menu = 3;  // nastaveni
      else if (menu == 4 && cursor == 2) menu = 0;  // zpet do hl.menu
      else if (menu == 3 && cursor == 3) {    // zpet do hl.menu
        menu = 0;

        EEPROM.put(0, pocetMotoru);
        EEPROM.put(20, rychlost);
        EEPROM.put(40, detekceProudu);
      }
      else if (menu == 1 && cursor == 3 || menu == 2 && cursor == 2) {  // spusteni programu

        if (menu == 2) {
          pocetCyklu = 0;
        }

        hotovoCyklu = 0;

        delay(200);
        lcd.clear();
        work = true;

        currentIndex = 0;
        arrayFull = false;

        status = 1;
        drawWorkStatus();
        pocetkroku = 0;

        mereniproudu = millis();
        proudzmeren = false;

        digitalWrite(X_DIR, HIGH);
        digitalWrite(Y_DIR, HIGH);
        digitalWrite(Z_DIR, HIGH);
        digitalWrite(A_DIR, HIGH);
      }
      else if (menu == 3 && cursor == 0 || cursor == 1 && menu == 3 || cursor == 0 && menu == 1 ||
               cursor == 1 && menu == 1 || cursor == 2 && menu == 1 || cursor == 0 && menu == 2 ||
               cursor == 1 && menu == 2 || cursor == 2 && menu == 2 || menu == 4 && cursor == 1) {    // nastaveni specifickych sisel, integrace pro blikani
        while (digitalRead(SW) == LOW) setSpeedorCNT = true;
      }
      else if (menu == 3 && cursor == 2) detekceProudu = !detekceProudu;    // povoleni/zakazani mereni proudu
      else if (menu == 4 && cursor == 0) {  // repozice cyklus

        bool repozicecyklus1 = true;
        bool motorynaswitchi[4] = {false, false, false, false};

        digitalWrite(X_DIR, LOW);
        digitalWrite(Y_DIR, LOW);
        digitalWrite(Z_DIR, LOW);
        digitalWrite(A_DIR, LOW);

        if (pocetMotoru == 1) {
          motorynaswitchi[1] = true;
          motorynaswitchi[2] = true;
          motorynaswitchi[3] = true;
        }
        else if (pocetMotoru == 2) {
          motorynaswitchi[2] = true;
          motorynaswitchi[3] = true;
        }
        else if (pocetMotoru == 3) {
          motorynaswitchi[3] = true;
        }

        while (repozicecyklus1) {
          if (!digitalRead(limit_dole1)) motorynaswitchi[0] = true;
          if (!digitalRead(limit_dole2)) motorynaswitchi[1] = true;
          if (!digitalRead(limit_dole3)) motorynaswitchi[2] = true;
          if (!digitalRead(limit_dole4)) motorynaswitchi[3] = true;

          if (!motorynaswitchi[0]) digitalWrite(X_STP, HIGH);
          if (!motorynaswitchi[1]) digitalWrite(Y_STP, HIGH);
          if (!motorynaswitchi[2]) digitalWrite(Z_STP, HIGH);
          if (!motorynaswitchi[3]) digitalWrite(A_STP, HIGH);
          delayMicroseconds(rychlostrepozice);
          if (!motorynaswitchi[0]) digitalWrite(X_STP, LOW);
          if (!motorynaswitchi[1]) digitalWrite(Y_STP, LOW);
          if (!motorynaswitchi[2]) digitalWrite(Z_STP, LOW);
          if (!motorynaswitchi[3]) digitalWrite(A_STP, LOW);

          if (motorynaswitchi[0] && motorynaswitchi[1] && motorynaswitchi[2] && motorynaswitchi[3]) repozicecyklus1 = false;
        }

        digitalWrite(X_DIR, HIGH);
        digitalWrite(Y_DIR, HIGH);
        digitalWrite(Z_DIR, HIGH);
        digitalWrite(A_DIR, HIGH);

        delay(100);

        for (int x = 0; x < repozice; x++) {
          if (pocetMotoru == 1) {
            digitalWrite(X_STP, HIGH);
            delayMicroseconds(rychlost*2);
            digitalWrite(X_STP, LOW);
          }
          else if (pocetMotoru == 2) {
            digitalWrite(X_STP, HIGH);
            digitalWrite(Y_STP, HIGH);
            delayMicroseconds(rychlost*2);
            digitalWrite(X_STP, LOW);
            digitalWrite(Y_STP, LOW);
          }
          else if (pocetMotoru == 3) {
            digitalWrite(X_STP, HIGH);
            digitalWrite(Y_STP, HIGH);
            digitalWrite(Z_STP, HIGH);
            delayMicroseconds(rychlost*2);
            digitalWrite(X_STP, LOW);
            digitalWrite(Y_STP, LOW);
            digitalWrite(Z_STP, LOW);
          }
          else {
            digitalWrite(X_STP, HIGH);
            digitalWrite(Y_STP, HIGH);
            digitalWrite(Z_STP, HIGH);
            digitalWrite(A_STP, HIGH);
            delayMicroseconds(rychlost*2);
            digitalWrite(X_STP, LOW);
            digitalWrite(Y_STP, LOW);
            digitalWrite(Z_STP, LOW);
            digitalWrite(A_STP, LOW);
          }
        }
      }

      int hodnotyProudu[maxSize];

      while (work) {  // Smycka pro hl.praci

        if (status == 1 || status == 2) {   // Zde se ridime dle statusu, ktery je vyobrazen na LCD displeji, pokud neni dosazen dany pocet kroku na motoru, toc s motorem dale
          krokDopredu();

          if (status == 1 && pocetkroku > (pocetPlus * 200) || status == 1 && !digitalRead(limit_nahore)) {
            status = 2;
            drawWorkStatus();
            pocetkroku = 0;
            mereniproudu = millis();
            proudzmeren = false;

            digitalWrite(X_DIR, LOW);
            digitalWrite(Y_DIR, LOW);
            digitalWrite(Z_DIR, LOW);
            digitalWrite(A_DIR, LOW);
          }

          else if (status == 2 && pocetkroku > (pocetPlus * 200) || status == 2 & (!digitalRead(limit_dole1) || !digitalRead(limit_dole2) || !digitalRead(limit_dole3) || !digitalRead(limit_dole4))) {
            status = 3;
            drawWorkStatus();
            pocetkroku = 0;
            mereniproudu = millis();
            proudzmeren = false;
          }
        }

        if (status == 3 || status == 4 ) {
          krokDopredu();

          if (status == 3 && pocetkroku > (pocetMinus * 200) || status == 3 & (!digitalRead(limit_dole1) || !digitalRead(limit_dole2) || !digitalRead(limit_dole3) || !digitalRead(limit_dole4))) {
            status = 4;
            drawWorkStatus();
            pocetkroku = 0;
            mereniproudu = millis();
            proudzmeren = false;

            digitalWrite(X_DIR, HIGH);
            digitalWrite(Y_DIR, HIGH);
            digitalWrite(Z_DIR, HIGH);
            digitalWrite(A_DIR, HIGH);
          }

          else if (status == 4 && pocetkroku > (pocetMinus * 200) || status == 4 && !digitalRead(limit_nahore)) {    // Pri dosazeni posledniho kroku cyklu zknotrolujeme postup, pripadne ukoncime a vypiseme dokonceno
            hotovoCyklu++;
            if (hotovoCyklu + 1 > pocetCyklu && pocetCyklu != 0) {
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("Dokonceno!");
              lcd.setCursor(0, 1);
              if (pocetCyklu != 0) lcd.print(String(hotovoCyklu) + "/" + String(pocetCyklu) + "cyklu");
              else lcd.print(String(hotovoCyklu) + "cyklu");

              lcd.setCursor(0, 3);
              lcd.print("Stiskni enkoder.");

              while (digitalRead(SW) == HIGH) { }

              work = false;
              if (pocetCyklu == 0) pocetCyklu = 1;
              lcd.clear();
              menu = 0;
            }
            else {
              status = 1;
              drawWorkStatus();
              pocetkroku = 0;
              mereniproudu = millis();
              proudzmeren = false;
            }
          }
        }

        if (detekceProudu && !proudzmeren && mereniproudu + 300 < millis()) {   // Mereni proudu
          proudzmeren = true;

          int analogValue = analogRead(A8);     // pridani hodnoty do pole s poslednimi 5ti hodnoty
          hodnotyProudu[currentIndex] = analogValue;
          currentIndex = (currentIndex + 1) % maxSize;

          if (currentIndex == 0) {
            arrayFull = true;
          }

          if (arrayFull) {    // Pokud jsme jiz ziskali prumer z poslednich 5ti hodnot
            int sum = 0;
            for (int i = 0; i < maxSize; i++) {
              sum += hodnotyProudu[i];
            }
            int average = sum / maxSize;

            if (analogValue >= (average - 3) && analogValue <= (average + 3)) {}
            else {
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("ERR - skok v proudu!");
              lcd.setCursor(0, 2);
              lcd.print("stiskni enkoder pro");
              lcd.setCursor(0, 3);
              lcd.print("vyp. mereni proudu");

              while (digitalRead(SW) == HIGH) {}

              lcd.clear();
              detekceProudu = false;
              drawWorkStatus();
            }
          }
        }

        btnState = digitalRead(SW); // Kontrola zmacknuti tlacitka pro exit na dobu delsi nez 1s

        if (btnState == LOW && tlacstav == false) {
          tlacstav = true;
          enkoderTlacitko = millis();
        }
        else if (btnState == HIGH && tlacstav == true) {
          tlacstav = false;
        }
        else if (btnState == LOW && tlacstav == true && enkoderTlacitko + 1000 < millis()) {    // pokud detekovano:
          work = false;
          EEPROM.put(60, repozice);
          if (pocetCyklu == 0) pocetCyklu = 1;
          lcd.clear();
          menu = 0;
          while (digitalRead(SW) == LOW) {}
        }
      }

      unsigned long blinkingMs = millis();

      while (setSpeedorCNT) {   // Smycka pro upravu danych promennych (blikani na LCD)

        if (blinkingMs + 200 < millis()) {    // Vytvoreni nezavislosti na rotacnim enkoderu

          blinking = !blinking;
          blinkingMs = millis();

          lcd.setCursor(0, cursor);

          if (menu == 3) {    // Dle dane polozky blikej s konkretnim textem
            if (cursor == 0 && blinking) {
              lcd.print("                    ");
            }
            else if (cursor == 0 && !blinking) {
              lcd.print(">Pocet motoru   ");
              lcd.print(pocetMotoru);
            }
            else if (cursor == 1 && blinking) {
              lcd.print("                    ");
            }
            else if (cursor == 1 && !blinking) {
              lcd.print(">Rychlost       ");
              lcd.print(rychlost);
            }
          }
          else if (menu == 1) {
            if (cursor == 0 && blinking) {
              lcd.print("                    ");
            }
            else if (cursor == 0 && !blinking) {
              lcd.print(">Pocet otoceni +");
              lcd.print(pocetPlus);
              lcd.print("o");
            }
            else if (cursor == 1 && blinking) {
              lcd.print("                    ");
            }
            else if (cursor == 1 && !blinking) {
              lcd.print(">Pocet otoceni -");
              lcd.print(pocetMinus);
              lcd.print("o");
            }
            else if (cursor == 2 && blinking) {
              lcd.print("                    ");
            }
            else if (cursor == 2 && !blinking) {
              lcd.print(">Pocet cyklu ");
              lcd.print(pocetCyklu);
              lcd.print("x");
            }
          }
          else if (menu == 2) {
            if (cursor == 0 && blinking) {
              lcd.print("                    ");
            }
            else if (cursor == 0 && !blinking) {
              lcd.print(">Pocet otoceni +");
              lcd.print(pocetPlus);
              lcd.print("o");
            }
            else if (cursor == 1 && blinking) {
              lcd.print("                    ");
            }
            else if (cursor == 1 && !blinking) {
              lcd.print(">Pocet otoceni -");
              lcd.print(pocetMinus);
              lcd.print("o");
            }
          }
          else if (menu == 4) {
            if (cursor == 1 && blinking) {
              lcd.setCursor(0, 2);
              lcd.print("                    ");
            }
            else if (cursor == 1 && !blinking) {
              lcd.setCursor(0, 2);
              lcd.print(" Od konce: " + String(repozice) + "kroku");
            }
          }
        }

        currentStateCLK = digitalRead(CLK);

        if (currentStateCLK != lastStateCLK  && currentStateCLK == 1) {   // Hlidani pozice rotacniho enkoderu, dle konkretni polozky a limitaci pridej nebo uber do promenne

          if (digitalRead(DT) != currentStateCLK) {
            if (menu == 3) {
              if (cursor == 0 && pocetMotoru > 1) pocetMotoru--;
              else if (cursor == 1 && rychlost > 0) rychlost = rychlost - 20;
            }
            else if (menu == 1) {
              if (cursor == 0 && pocetPlus > 0) pocetPlus--;
              else if (cursor == 1 && pocetMinus > 0) pocetMinus--;
              else if (cursor == 2 && pocetCyklu > 0) pocetCyklu--;
            }
            else if (menu == 2) {
              if (cursor == 0 && pocetPlus > 0) pocetPlus--;
              else if (cursor == 1 && pocetMinus > 0) pocetMinus--;
            }
            else if (menu == 4) {   // Kalibrace
              if (cursor == 1) {
                if (repozice > 10) repozice = repozice - 10;
              }
            }
          } else {
            if (menu == 3) {
              if (cursor == 0 && pocetMotoru < 4) pocetMotoru++;
              else if (cursor == 1 && rychlost < 5000) rychlost = rychlost + 20;
            }
            if (menu == 1) {
              if (cursor == 0 && pocetPlus < 99) pocetPlus++;
              else if (cursor == 1 && pocetMinus < 99) pocetMinus++;
              else if (cursor == 2 && pocetCyklu < 99) pocetCyklu++;
            }
            else if (menu == 2) {
              if (cursor == 0 && pocetPlus < 99) pocetPlus++;
              else if (cursor == 1 && pocetMinus < 99) pocetMinus++;
            }
            else if (menu == 4) {
              if (cursor == 1) repozice = repozice + 10;
            }
          }
        }

        lastStateCLK = currentStateCLK;

        if (digitalRead(SW) == LOW) {   // pokud je tlacitko zmacknuto, vyskoc z upravy polozky (blikani)
          if (millis() - lastButtonPress > 50) {
            setSpeedorCNT = false;
          }
          lastButtonPress = millis();
        }
        delay(1);
      }
      lcd.clear();
      cursor = 0;
      drawScreenStatus();
      lastButtonPress = millis();
    }
  }
  delay(1);
}

void drawScreenStatus() {   // Funkce pro vyobrazeni konkretniho menu na displeji
  lcd.setCursor(0, 0);

  if (menu == 0) {
    lcd.print(" Mod 1 -pocet cyklu");
    lcd.setCursor(0, 1);
    lcd.print(" Mod 2 -endless");
    lcd.setCursor(0, 2);
    lcd.print(" Reset pozice motoru");
    lcd.setCursor(0, 3);
    lcd.print(" Nastaveni");

    lcd.setCursor(0, cursor);

    lcd.print(">");
  }

  else if (menu == 1) {
    lcd.print(" Pocet otoceni +");
    lcd.print(pocetPlus);
    lcd.print("o");
    lcd.setCursor(0, 1);
    lcd.print(" Pocet otoceni -");
    lcd.print(pocetMinus);
    lcd.print("o");
    lcd.setCursor(0, 2);
    lcd.print(" Pocet cyklu ");
    lcd.print(pocetCyklu);
    lcd.print("x");
    lcd.setCursor(0, 3);
    lcd.print(" Spustit");

    lcd.setCursor(0, cursor);
    lcd.print(">");
  }

  else if (menu == 2) {
    lcd.print(" Pocet otoceni +");
    lcd.print(pocetPlus);
    lcd.print("o");
    lcd.setCursor(0, 1);
    lcd.print(" Pocet otoceni -");
    lcd.print(pocetMinus);
    lcd.print("o");

    lcd.setCursor(0, 3);
    lcd.print(" Spustit");

    switch (cursor) {
      case 0:
        lcd.setCursor(0, 0);
        break;
      case 1:
        lcd.setCursor(0, 1);
        break;
      case 2:
        lcd.setCursor(0, 3);
        break;
    }

    lcd.print(">");
  }

  else if (menu == 3) {
    lcd.print(" Pocet motoru   ");
    lcd.print(pocetMotoru);
    lcd.setCursor(0, 1);
    lcd.print(" Rychlost       ");
    lcd.print(rychlost);
    lcd.setCursor(0, 2);
    lcd.print(" Detekce proudu ");

    if (detekceProudu) lcd.print("A");
    else lcd.print("N");

    lcd.setCursor(0, 3);
    lcd.print(" Zpet");

    lcd.setCursor(0, cursor);
    lcd.print(">");
  }

  else if (menu == 4) {
    lcd.print(" Vyresetovat pozici");
    lcd.setCursor(0, 2);
    lcd.print(" Od konce: " + String(repozice) + "kroku");
    lcd.setCursor(0, 3);
    lcd.print(" Zpet");

    switch (cursor)  {
      case 0:
        lcd.setCursor(0, 0);
        lcd.print(">");
        break;
      case 1:
        lcd.setCursor(0, 2);
        lcd.print(">");
        break;
      case 2:
        lcd.setCursor(0, 3);
        lcd.print(">");
    }
  }
}

void drawWorkStatus() {   // Funkce pro vyobrazeni stavu prace na displeji
  lcd.setCursor(0, 0);

  lcd.print(hotovoCyklu);
  if (pocetCyklu != 0) {
    lcd.print("/");
    lcd.print(pocetCyklu);
  }
  lcd.print(" cyklu");

  lcd.setCursor(0, 1);

  switch (status) {
    case 0:
      lcd.print("status: stop");
      break;
    case 1:
      lcd.print("status: dopredu     ");
      break;
    case 2:
      lcd.print("status: dopredu-zpet");
      break;
    case 3:
      lcd.print("status: dozadu      ");
      break;
    case 4:
      lcd.print("status: dozadu-zpet");
      break;
  }

  lcd.setCursor(0, 2);
  lcd.print("konf: " + String(pocetMotoru) + "m, " + String(rychlost) + "mcs");

  lcd.setCursor(0, 3);
  lcd.print("podrz tlac. pro exit");
}

void krokDopredu() {      // Funkce pro poslani kroku do motoru (zalezi na orientaci)
  if (pocetMotoru == 1) {
    digitalWrite(X_STP, HIGH);
    delayMicroseconds(rychlost);
    digitalWrite(X_STP, LOW);
    delayMicroseconds(rychlost);
  }
  else if (pocetMotoru == 2) {
    digitalWrite(X_STP, HIGH);
    digitalWrite(Y_STP, HIGH);
    delayMicroseconds(rychlost);
    digitalWrite(X_STP, LOW);
    digitalWrite(Y_STP, LOW);
    delayMicroseconds(rychlost);
  }
  else if (pocetMotoru == 3) {
    digitalWrite(X_STP, HIGH);
    digitalWrite(Y_STP, HIGH);
    digitalWrite(Z_STP, HIGH);
    delayMicroseconds(rychlost);
    digitalWrite(X_STP, LOW);
    digitalWrite(Y_STP, LOW);
    digitalWrite(Z_STP, LOW);
    delayMicroseconds(rychlost);
  }
  else if (pocetMotoru == 4) {
    digitalWrite(X_STP, HIGH);
    digitalWrite(Y_STP, HIGH);
    digitalWrite(Z_STP, HIGH);
    digitalWrite(A_STP, HIGH);
    delayMicroseconds(rychlost);
    digitalWrite(X_STP, LOW);
    digitalWrite(Y_STP, LOW);
    digitalWrite(Z_STP, LOW);
    digitalWrite(A_STP, LOW);
    delayMicroseconds(rychlost);
  }
  pocetkroku++;
}