#include <Encoder.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Inicializace I2C LCD (adresa 0x27, 16 sloupců, 2 řady)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Enkodér připojený na piny 2 (CLK) a 3 (DT)
Encoder myEnc(2, 3);

// Definice tlačítek
const int buttonPin = 4;      // Tlačítko pro přechod vpřed (encoder)
const int backButtonPin = 9;  // Tlačítko zpět

// Motor 1 (A4988): STEP pin 5, DIR pin 6
const int m1StepPin = 5;
const int m1DirPin  = 6;

// Motor 2 (A4988): STEP pin 7, DIR pin 8
const int m2StepPin = 7;
const int m2DirPin  = 8;

// Režimy (screeny)
enum Mode { SPLASH, SETTING_M1, SETTING_M2, RUNNING };
Mode mode = SPLASH;

// Uložené hodnoty rychlosti (steps/s)
long motor1SpeedSetting = 0;
long motor2SpeedSetting = 0;

// Baseline hodnoty enkodéru pro relativní nastavení
long baselineM1 = 0;
long baselineM2 = 0;

// Aktivní rychlosti pro generování pulzů – se aktualizují při přechodu do RUNNING
long activeMotor1Speed = 0;
long activeMotor2Speed = 0;

// Proměnné pro časování pulzů u obou motorů
unsigned long lastStepTimeM1 = 0;
unsigned long lastStepTimeM2 = 0;

void setup() {
  Serial.begin(9600);
  
  // Nastavení tlačítek s interním pull-up rezistorem
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(backButtonPin, INPUT_PULLUP);
  
  // Inicializace pinů motorů
  pinMode(m1StepPin, OUTPUT);
  pinMode(m1DirPin, OUTPUT);
  digitalWrite(m1DirPin, HIGH); // Nastavte směr dle potřeby
  
  pinMode(m2StepPin, OUTPUT);
  pinMode(m2DirPin, OUTPUT);
  digitalWrite(m2DirPin, HIGH); // Nastavte směr dle potřeby
  
  // Inicializace I2C LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  
  // Zahájení v režimu SPLASH – úvodní obrazovka
  lcd.setCursor(0, 0);
  lcd.print("filament maker");
}

void loop() {
  // ----------------- Zpracování tlačítka ZPĚT (pin 9) -----------------
  if (digitalRead(backButtonPin) == LOW) {
    delay(200); // debounce
    if (mode == RUNNING) {
      mode = SETTING_M2;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Set M2 Speed:");
      baselineM2 = myEnc.read();
    } else if (mode == SETTING_M2) {
      mode = SETTING_M1;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Set M1 Speed:");
      baselineM1 = myEnc.read();
    } else if (mode == SETTING_M1) {
      mode = SPLASH;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("filament maker");
    }
    while (digitalRead(backButtonPin) == LOW);
    delay(50);
  }
  
  // ----------------- Zpracování tlačítka VPŘED (pin 4) -----------------
  if (digitalRead(buttonPin) == LOW) {
    delay(200); // debounce
    if (mode == SPLASH) {
      // Přejít do nastavení motoru 1
      mode = SETTING_M1;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Set M1 Speed:");
      baselineM1 = myEnc.read();
    } else if (mode == SETTING_M1) {
      // Uložit novou efektivní hodnotu pro motor 1 a přejít do nastavení motoru 2
      long effectiveM1 = motor1SpeedSetting + (myEnc.read() - baselineM1);
      effectiveM1 = constrain(effectiveM1, 0, 200);
      motor1SpeedSetting = effectiveM1;
      
      mode = SETTING_M2;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Set M2 Speed:");
      baselineM2 = myEnc.read();
    } else if (mode == SETTING_M2) {
      // Uložit novou efektivní hodnotu pro motor 2 a přejít do RUNNING režimu
      long effectiveM2 = motor2SpeedSetting + (myEnc.read() - baselineM2);
      effectiveM2 = constrain(effectiveM2, 0, 200);
      motor2SpeedSetting = effectiveM2;
      
      mode = RUNNING;
      activeMotor1Speed = motor1SpeedSetting;
      activeMotor2Speed = motor2SpeedSetting;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Motors Running");
    } else if (mode == RUNNING) {
      // Přejít z RUNNING do SETTING_M1 pro další úpravy
      mode = SETTING_M1;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Set M1 Speed:");
      baselineM1 = myEnc.read();
    }
    while (digitalRead(buttonPin) == LOW);
    delay(50);
  }
  
  // ----------------- Aktualizace zobrazení dle režimu -----------------
  if (mode == SPLASH) {
    // Udržujeme úvodní obrazovku
    lcd.setCursor(0, 0);
    lcd.print("filament maker");
    lcd.setCursor(0, 1);
    lcd.print("                ");
  }
  else if (mode == SETTING_M1) {
    long effectiveM1 = motor1SpeedSetting + (myEnc.read() - baselineM1);
    effectiveM1 = constrain(effectiveM1, 0, 200);
    lcd.setCursor(0, 1);
    lcd.print("M1: ");
    lcd.print(effectiveM1);
    lcd.print(" st/s   ");
    printEncoderValue("M1", effectiveM1);
  }
  else if (mode == SETTING_M2) {
    long effectiveM2 = motor2SpeedSetting + (myEnc.read() - baselineM2);
    effectiveM2 = constrain(effectiveM2, 0, 200);
    lcd.setCursor(0, 1);
    lcd.print("M2: ");
    lcd.print(effectiveM2);
    lcd.print(" st/s   ");
    printEncoderValue("M2", effectiveM2);
  }
  else if (mode == RUNNING) {
    lcd.setCursor(0, 0);
    lcd.print("M1: ");
    lcd.print(activeMotor1Speed);
    lcd.print(" st/s ");
    lcd.setCursor(0, 1);
    lcd.print("M2: ");
    lcd.print(activeMotor2Speed);
    lcd.print(" st/s ");
  }
  
  // ----------------- Generování motorových pulzů (běží nepřetržitě) -----------------
  unsigned long currentTime = micros();
  
  if (activeMotor1Speed > 0) {
    unsigned long interval1 = 1000000UL / activeMotor1Speed;
    if (currentTime - lastStepTimeM1 >= interval1) {
      digitalWrite(m1StepPin, HIGH);
      delayMicroseconds(2);
      digitalWrite(m1StepPin, LOW);
      lastStepTimeM1 = currentTime;
    }
  }
  
  if (activeMotor2Speed > 0) {
    unsigned long interval2 = 1000000UL / activeMotor2Speed;
    if (currentTime - lastStepTimeM2 >= interval2) {
      digitalWrite(m2StepPin, HIGH);
      delayMicroseconds(2);
      digitalWrite(m2StepPin, LOW);
      lastStepTimeM2 = currentTime;
    }
  }
}

// Funkce pro vypisování hodnoty enkodéru do Serial monitoru pouze při změně
void printEncoderValue(const char* motorLabel, long value) {
  static long oldValueM1 = -1;
  static long oldValueM2 = -1;
  
  if (strcmp(motorLabel, "M1") == 0) {
    if (value != oldValueM1) {
      Serial.print("Encoder M1 effective value: ");
      Serial.println(value);
      oldValueM1 = value;
    }
  } else if (strcmp(motorLabel, "M2") == 0) {
    if (value != oldValueM2) {
      Serial.print("Encoder M2 effective value: ");
      Serial.println(value);
      oldValueM2 = value;
    }
  }
}