#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 1000

// Inicializace LCD – u některých modulů může být adresa 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);

// Proměnné pro správu nastavení
// screen = 1: nastavujeme rychlost motoru 1, screen = 2: nastavujeme rychlost motoru 2
int screen = 1; 

// Uložené (potvrzené) rychlosti motorů (steps/s)
long confirmedSpeed1 = 0;
long confirmedSpeed2 = 0;

// Aktuální hodnota, kterou uživatel upravuje pomocí encoderu
long currentSetting = 0;

// Proměnná pro čtení encoderu (kanál A)
int lastEncoderA = 0;

// Proměnné pro debouncing tlačítka encoderu
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 obnovu displeje
unsigned long lastDisplayUpdate = 0;
unsigned long displayInterval = 100; // aktualizace každých 100 ms

void setup() {
  Serial.begin(9600);
  
  // Inicializace vstupů pro encoder
  pinMode(ENCODER_A, INPUT_PULLUP);
  pinMode(ENCODER_B, INPUT_PULLUP);
  pinMode(ENCODER_BTN, INPUT_PULLUP);
  
  // Inicializace LCD
  lcd.init();
  lcd.backlight();
  
  // Inicializace motorů – start s rychlostí 0
  motor1.setSpeed(confirmedSpeed1);
  motor2.setSpeed(confirmedSpeed2);
  
  // Na začátku nastavujeme rychlost pro motor 1
  currentSetting = confirmedSpeed1;
  
  // Uložení počátečního stavu encoderu (kanál A)
  lastEncoderA = digitalRead(ENCODER_A);
}

void loop() {
  // Neustálé volání runSpeed() zajistí plynulý chod motorů
  motor1.runSpeed();
  motor2.runSpeed();
  
  // --- Zpracování otáčení rotary encoderu ---
  int encoderA = digitalRead(ENCODER_A);
  if (encoderA != lastEncoderA) { // detekce změny stavu kanálu A
    // Určíme směr otáčení porovnáním se stavem kanálu B
    if (digitalRead(ENCODER_B) != encoderA) {
      currentSetting++;  // otáčení jedním směrem zvýší hodnotu
    } else {
      currentSetting--;  // otáčení opačným směrem sníží hodnotu
    }
    // Omezení hodnoty mezi 0 a MAX_SPEED
    if (currentSetting < 0) currentSetting = 0;
    if (currentSetting > MAX_SPEED) currentSetting = MAX_SPEED;
    lastEncoderA = encoderA;
  }
  
  // --- Debouncing a zpracování stisku tlačítka encoderu ---
  int reading = digitalRead(ENCODER_BTN);
  
  // Pokud se nepřefiltrované čtení změnilo, resetujeme debounce časovač
  if (reading != lastButtonReading) {
    lastDebounceTime = millis();
  }
  
  // Po uplynutí debounce intervalu ověříme, zda se stav změnil
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != buttonState) {
      buttonState = reading;
      // Pokud bylo tlačítko stisknuto (přechod z HIGH na LOW)
      if (buttonState == LOW) {
        Serial.print("Encoder button pressed at: ");
        Serial.println(millis());
        
        if (screen == 1) {
          // Potvrzení rychlosti pro motor 1 a přepnutí na nastavení motoru 2
          confirmedSpeed1 = currentSetting;
          motor1.setSpeed(confirmedSpeed1);
          Serial.print("Motor1 speed set to: ");
          Serial.println(confirmedSpeed1);
          screen = 2;
          currentSetting = confirmedSpeed2; // načteme aktuální nastavenou rychlost motoru 2
        } else {
          // Potvrzení rychlosti pro motor 2 a přepnutí zpět na nastavení motoru 1
          confirmedSpeed2 = currentSetting;
          motor2.setSpeed(confirmedSpeed2);
          Serial.print("Motor2 speed set to: ");
          Serial.println(confirmedSpeed2);
          screen = 1;
          currentSetting = confirmedSpeed1; // načteme aktuální nastavenou rychlost motoru 1
        }
      }
    }
  }
  lastButtonReading = reading;
  
  // --- Aktualizace displeje každých displayInterval ms ---
  if (millis() - lastDisplayUpdate > displayInterval) {
    lcd.clear();
    if (screen == 1) {
      lcd.setCursor(0, 0);
      lcd.print("Motor1: ");
      lcd.print(currentSetting);
      lcd.setCursor(0, 1);
      lcd.print("Stisk pro nast.");
    } else {
      lcd.setCursor(0, 0);
      lcd.print("Motor2: ");
      lcd.print(currentSetting);
      lcd.setCursor(0, 1);
      lcd.print("Stisk pro nast.");
    }
    lastDisplayUpdate = millis();
  }
}