#include <LiquidCrystal_I2C.h>
#include <AccelStepper.h>

// Definice pinů pro encoder
#define ENCODER_A 2
#define ENCODER_B 3
#define ENCODER_BTN 4

// Definice pinů pro motor 1 (A4988 driver)
#define MOTOR1_STEP 5
#define MOTOR1_DIR 6

// Definice pinů pro motor 2 (A4988 driver)
#define MOTOR2_STEP 7
#define MOTOR2_DIR 8

// Maximální rychlost (steps/s) – upravte dle potřeby
#define MAX_SPEED 100000

// Konstantní násobitel pro převod nastavení na skutečnou rychlost motoru
#define SPEED_MULTIPLIER 100

// Inicializace LCD – adresa 0x27 (u některých modulů může být 0x3F)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Inicializace motorů pomocí režimu DRIVER (používá STEP a DIR)
AccelStepper motor1(AccelStepper::DRIVER, MOTOR1_STEP, MOTOR1_DIR);
AccelStepper motor2(AccelStepper::DRIVER, MOTOR2_STEP, MOTOR2_DIR);

// ----------------- Filtrace enkodéru -----------------
// Definice struktury pro filtraci
typedef struct {
  uint8_t state;
  bool output;
} T_filtr;

// Globální proměnná pro filtr pro kanál A
T_filtr filtrA;

// Uložená hodnota filtrovaného stavu kanálu A
bool lastFilteredA = false;

// Funkce filtr – pokud je vstupní hodnota 1 načtena dvakrát za sebou, nastaví output na HIGH.
void filtr(bool input, T_filtr *filtr) {
  switch(filtr->state) {
    case 0: // počáteční stav
      if(input == 1){
        filtr->state = 1;
      } else {
        filtr->state = 3;
      }
      break;
      
    case 1:
      if(input == 1){
        filtr->state = 2;
      } else {
        filtr->state = 3;
      }
      break;
      
    case 2:
      if(input == 1){
        filtr->state = 2;
        filtr->output = 1;
      } else {
        filtr->state = 3;
      }
      break;
      
    case 3:
      if(input == 0){
        filtr->state = 4;
      } else {
        filtr->state = 1;
      }
      break;
      
    case 4:
      if(input == 0){
        filtr->state = 4;
        filtr->output = 0;
      } else {
        filtr->state = 1;
      }
      break;
  }
}
// ----------------- Konec filtrace -----------------

// Globální proměnné pro správu nastavení:
// screen 1: nastavujeme motor1, screen 2: nastavujeme motor2, screen 3: přehled obou motorů
int screen = 1;
long confirmedSpeed1 = 0;
long confirmedSpeed2 = 0;
long currentSetting = 0;

// Debounce proměnné pro tlačítko enkodéru
int buttonState = HIGH;         // ustálený stav tlačítka
int lastButtonReading = HIGH;    // poslední nepřefiltrované čtení tlačítka
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50; // 50 ms debounce

// Proměnné pro sledování posledního zobrazeného stavu displeje
int lastScreen = -1;
long lastDisplayedValue = -1;

void setup() {
  Serial.begin(9600);
  
  // Inicializace vstupů – pro ENCODER_A a ENCODER_B používáme pouze INPUT (potřebné externí pull-up rezistory)
  pinMode(ENCODER_A, INPUT);
  pinMode(ENCODER_B, INPUT);
  // Tlačítko enkodéru s interním pullup
  pinMode(ENCODER_BTN, INPUT_PULLUP);
  
  // Inicializace LCD
  lcd.init();
  lcd.backlight();
  
  // Inicializace motorů – start s rychlostí 0
  motor1.setSpeed(confirmedSpeed1);
  motor2.setSpeed(confirmedSpeed2);
  
  // Nastavení aktuální hodnoty na rychlost motoru 1
  currentSetting = confirmedSpeed1;
  
  // Inicializace filtru pro kanál A
  filtrA.state = 0;
  filtrA.output = false;
  lastFilteredA = false;
  
  // Počáteční kompletní zobrazení displeje
  updateDisplay(true);
}

void loop() {
  // Plynulý chod motorů
  motor1.runSpeed();
  motor2.runSpeed();
  
  // --- Zpracování otáčení rotary enkodéru s filtrováním ---  
  // Rotace zpracováváme pouze na screenech 1 a 2 (kde se nastavuje rychlost)
  if(screen == 1 || screen == 2) {
    bool rawA = digitalRead(ENCODER_A);
    filtr(rawA, &filtrA);
    bool filteredA = filtrA.output;
    
    if (filteredA != lastFilteredA) {
      // Podle stavu kanálu B určíme směr otáčení
      if (digitalRead(ENCODER_B) != filteredA) {
        currentSetting++;
      } else {
        currentSetting--;
      }
      if (currentSetting < 0) currentSetting = 0;
      if (currentSetting > MAX_SPEED) currentSetting = MAX_SPEED;
      lastFilteredA = filteredA;
    }
  }
  
  // --- Debouncing a zpracování stisku tlačítka enkodéru ---
  int reading = digitalRead(ENCODER_BTN);
  if (reading != lastButtonReading) {
    lastDebounceTime = millis();
  }
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != buttonState) {
      buttonState = reading;
      // Přechod z HIGH na LOW – tlačítko bylo stisknuto
      if (buttonState == LOW) {
        Serial.print("Encoder button pressed at: ");
        Serial.println(millis());
        if (screen == 1) {
          // Nastavení rychlosti motoru 1
          confirmedSpeed1 = currentSetting;
          motor1.setSpeed(confirmedSpeed1 * SPEED_MULTIPLIER);
          Serial.print("Motor1 speed set to: ");
          Serial.println(confirmedSpeed1);
          screen = 2;
          currentSetting = confirmedSpeed2;
          updateDisplay(true);
        } else if (screen == 2) {
          // Nastavení rychlosti motoru 2
          confirmedSpeed2 = currentSetting;
          motor2.setSpeed(confirmedSpeed2 * SPEED_MULTIPLIER);
          Serial.print("Motor2 speed set to: ");
          Serial.println(confirmedSpeed2);
          screen = 3;
          updateDisplay(true);
        } else if (screen == 3) {
          // Na overview screenu se tlačítkem vracíme zpět k nastavení motoru 1
          screen = 1;
          currentSetting = confirmedSpeed1;
          updateDisplay(true);
        }
      }
    }
  }
  lastButtonReading = reading;
  
  // --- Aktualizace displeje pouze při změně hodnoty ---
  if((screen == 1 || screen == 2) && (currentSetting != lastDisplayedValue)) {
    updateDisplay(false);
  }
}

//
// updateDisplay:
// fullUpdate == true => kompletní obnova displeje (při změně screenu)
// fullUpdate == false => aktualizace pouze čísla (aktuální nastavení) pro screeny 1 a 2,
// a na screenu 3 je zobrazen přehled obou motorů
//
void updateDisplay(bool fullUpdate) {
  if (fullUpdate) {
    lcd.clear();
    if (screen == 1) {
      lcd.setCursor(0, 0);
      lcd.print("Motor1: ");
      lcd.setCursor(0, 1);
      lcd.print("Stisk pro nast.");
    } else if (screen == 2) {
      lcd.setCursor(0, 0);
      lcd.print("Motor2: ");
      lcd.setCursor(0, 1);
      lcd.print("Stisk pro nast.");
    } else if (screen == 3) {
      lcd.setCursor(0, 0);
      lcd.print("M1:");
      lcd.setCursor(4, 0);
      lcd.print(confirmedSpeed1);
      lcd.setCursor(8, 0);
      lcd.print("spm");
      lcd.setCursor(0, 1);
      lcd.print("M2:");
      lcd.setCursor(4, 1);
      lcd.print(confirmedSpeed2);
      lcd.setCursor(8, 1);
      lcd.print("spm");
    }
    lastScreen = screen;
    lastDisplayedValue = -1;
  }
  if (screen == 1 || screen == 2) {
    if (currentSetting != lastDisplayedValue) {
      lcd.setCursor(8, 0);
      lcd.print("       ");
      lcd.setCursor(8, 0);
      lcd.print(currentSetting);
      lastDisplayedValue = currentSetting;
    }
  }
}