#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>  // Dodanie biblioteki EEPROM

// Definicje wyświetlacza I2C
LiquidCrystal_I2C lcd(0x27, 20, 4);  // Adres I2C (0x27) i rozmiar wyświetlacza

// Stałe i piny
#define ENCODER_PIN_A 2
#define ENCODER_PIN_B 3
#define RESET_BUTTON 4
#define CALIB_UP_BUTTON 5
#define CALIB_DOWN_BUTTON 6
#define CALIB_001_BUTTON 13  // Nowy przycisk do kalibracji o 0,01 mm
#define CALIB_DIRECTION_BUTTON 14  // Nowy przycisk do zmiany kierunku
#define ZERO_BUTTON A1  // Nowy przycisk do ustawienia zerowego punktu

volatile long encoderPosition = 0;
float wheelDiameter = 10.0; // Początkowa średnica koła w mm
bool calibrationMode = false; // Flaga dla trybu kalibracji
unsigned long resetPressTime = 0; // Czas przytrzymania przycisku Reset
bool resetTriggered = false;     // Flaga dla obsługi krótkiego i długiego naciśnięcia
unsigned long debounceDelay = 50; // Czas debouncing dla przycisków
unsigned long calib001PressTime = 0;  // Czas przytrzymania przycisku kalibracji 0,01 mm
bool calib001Triggered = false;  // Flaga do resetu średnicy do domyślnej
unsigned long lastButtonPress = 0;  // Ostatni czas naciśnięcia przycisku
unsigned long calibUpButtonPressTime = 0;  // Czas przytrzymania przycisku "kalibracja w górę"
unsigned long calibDownButtonPressTime = 0;  // Czas przytrzymania przycisku "kalibracja w dół"

bool encoderDirection = true; // Flaga kierunku enkodera (true = normalny, false = odwrócony)

// Adresy pamięci EEPROM
#define EEPROM_WHEEL_DIAMETER_ADDR 0
#define EEPROM_ENCODER_POSITION_ADDR 4
#define EEPROM_ENCODER_DIRECTION_ADDR 8
#define EEPROM_ZERO_POINT_ADDR 12  // Adres zerowego punktu

long zeroPoint = 0;  // Zerowy punkt (pozycja enkodera)

void readEncoder() {
  static int lastA = LOW;
  int A = digitalRead(ENCODER_PIN_A);
  int B = digitalRead(ENCODER_PIN_B);

  if (A != lastA && A == HIGH) {
    if (B == LOW) {
      encoderPosition += (encoderDirection ? 1 : -1);
    } else {
      encoderPosition -= (encoderDirection ? 1 : -1);
    }
  }
  lastA = A;
}

void setup() {
  lcd.init();
  lcd.backlight();
  lcd.print("DRO Start...");

  pinMode(ENCODER_PIN_A, INPUT);
  pinMode(ENCODER_PIN_B, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCODER_PIN_A), readEncoder, CHANGE);

  pinMode(RESET_BUTTON, INPUT_PULLUP);
  pinMode(CALIB_UP_BUTTON, INPUT_PULLUP);
  pinMode(CALIB_DOWN_BUTTON, INPUT_PULLUP);
  pinMode(CALIB_001_BUTTON, INPUT_PULLUP); // Nowy przycisk do kalibracji 0,01 mm
  pinMode(CALIB_DIRECTION_BUTTON, INPUT_PULLUP); // Nowy przycisk do zmiany kierunku
  pinMode(ZERO_BUTTON, INPUT_PULLUP); // Nowy przycisk do ustawienia zerowego punktu

  // Odczytanie zapisanych ustawień z EEPROM
  wheelDiameter = EEPROM.read(EEPROM_WHEEL_DIAMETER_ADDR); // Odczyt średnicy
  encoderPosition = EEPROM.read(EEPROM_ENCODER_POSITION_ADDR); // Odczyt pozycji enkodera
  encoderDirection = EEPROM.read(EEPROM_ENCODER_DIRECTION_ADDR); // Odczyt kierunku
  zeroPoint = EEPROM.read(EEPROM_ZERO_POINT_ADDR); // Odczyt zerowego punktu

  // Jeśli odczytana wartość jest nieprawidłowa (np. 255, co jest wartością domyślną dla pustych komórek EEPROM),
  // ustaw domyślne wartości
  if (wheelDiameter == 255) {
    wheelDiameter = 10.0;  // Domyślna średnica
  }
  if (encoderPosition == 255) {
    encoderPosition = 0;  // Domyślna pozycja
  }
  if (encoderDirection == 255) {
    encoderDirection = true;  // Domyślny kierunek
  }
  if (zeroPoint == 255) {
    zeroPoint = encoderPosition;  // Jeśli brak zerowego punktu, ustaw aktualną pozycję jako zero
  }

  delay(2000);
  lcd.clear();
}

void loop() {
  unsigned long currentMillis = millis();

  // Obsługa przycisku Reset
  if (digitalRead(RESET_BUTTON) == LOW) {
    if (resetPressTime == 0) {
      resetPressTime = currentMillis; // Start czasu
    } else if (currentMillis - resetPressTime > 2000 && !resetTriggered) {  // Zmiana z 3s na 2s
      calibrationMode = !calibrationMode; // Tryb kalibracji po 2 sekundach
      resetTriggered = true;
      lcd.clear();
    }
  } else {
    if (resetPressTime > 0 && !resetTriggered && (currentMillis - resetPressTime < 2000)) {
      encoderPosition = 0; // Krótkie naciśnięcie resetuje pozycję
      lcd.clear();
    }
    resetPressTime = 0;
    resetTriggered = false;
  }

  // Obsługa przycisku kalibracji 0,01 mm
  if (digitalRead(CALIB_001_BUTTON) == LOW) {
    if (calib001PressTime == 0) {
      calib001PressTime = currentMillis; // Start czasu przytrzymania
    } else if (currentMillis - calib001PressTime > 2000 && !calib001Triggered) {  // Jeśli przytrzymano 2 sekundy
      wheelDiameter = 10.0;  // Reset do domyślnej średnicy koła (10 mm)
      calib001Triggered = true;
      lcd.clear();
      lcd.print("Reset do 10mm!");
      delay(1000);  // Krótkie opóźnienie na komunikat
      lcd.clear();
    }
  } else {
    calib001PressTime = 0;
    calib001Triggered = false;
  }

  // Obsługa zmiany kierunku
  if (digitalRead(CALIB_DIRECTION_BUTTON) == LOW) {
    encoderDirection = !encoderDirection;  // Zmiana kierunku obrotu
    delay(200); // Debouncing
    EEPROM.write(EEPROM_ENCODER_DIRECTION_ADDR, encoderDirection); // Zapis kierunku do EEPROM
  }

  // Obsługa przycisku ustawienia zerowego punktu
  if (digitalRead(ZERO_BUTTON) == LOW) {
    zeroPoint = encoderPosition;  // Ustawienie zerowego punktu na bieżącą pozycję enkodera
    EEPROM.write(EEPROM_ZERO_POINT_ADDR, zeroPoint); // Zapis zerowego punktu do EEPROM
    delay(500); // Debouncing
  }

  // Obsługa trybu kalibracji
  if (calibrationMode) {
    // Tryb kalibracji
    lcd.setCursor(0, 0);
    lcd.print("Kalibracja Lin:");
    lcd.setCursor(0,1);
    lcd.print("Wartosc: ");
    lcd.print(wheelDiameter, 3); // Wyświetlanie z 3 miejscami po przecinku
    

    // Obsługa przycisków kalibracji
    if (digitalRead(CALIB_UP_BUTTON) == LOW) {
      if (millis() - calibUpButtonPressTime > debounceDelay) {
        wheelDiameter += 1.0;  // Zwiększ średnicę o 0,1 mm
        calibUpButtonPressTime = millis();
        delay(200); // Debouncing
        EEPROM.write(EEPROM_WHEEL_DIAMETER_ADDR, wheelDiameter); // Zapis średnicy do EEPROM
      }
    }

    if (digitalRead(CALIB_DOWN_BUTTON) == LOW) {
      if (millis() - calibDownButtonPressTime > debounceDelay) {
        wheelDiameter -= 0.1;  // Zmniejsz średnicę o 0,1 mm
        calibDownButtonPressTime = millis();
        delay(200); // Debouncing
        EEPROM.write(EEPROM_WHEEL_DIAMETER_ADDR, wheelDiameter); // Zapis średnicy do EEPROM
      }
    }

    // Przycisk kalibracji o 0,01 mm
    if (digitalRead(CALIB_001_BUTTON) == LOW && millis() - lastButtonPress > debounceDelay) {
      wheelDiameter += 0.01;  // Zwiększ średnicę o 0,01 mm
      lastButtonPress = millis();
      delay(200); // Debouncing
      EEPROM.write(EEPROM_WHEEL_DIAMETER_ADDR, wheelDiameter); // Zapis średnicy do EEPROM
    }

  } else {
    // Tryb normalny
    float circumference = wheelDiameter * 3.14159; // Obwód koła w mm
    float displacementDiameter = (encoderPosition * circumference) / 1000.0; // Przesunięcie na średnicy
    float displacementRadius = displacementDiameter / 2; // Przesunięcie na promieniu

    lcd.setCursor(0, 0);
    lcd.print("Osi X:");

    lcd.setCursor(0, 1);
    lcd.print("SRED: ");
    lcd.print(displacementDiameter, 3);
    lcd.print(" mm");

    lcd.setCursor(0, 2);
    lcd.print("PROM: ");
    lcd.print(displacementRadius, 4);
    lcd.print(" mm");

    // Wyświetlanie bezwzględnego przesunięcia względem zerowego punktu
    long absoluteDisplacement = encoderPosition - zeroPoint;
    lcd.setCursor(0, 3);
    lcd.print("Abs: ");
    lcd.print(absoluteDisplacement * circumference / 1000.0, 3);  // Bezwzględne przesunięcie w mm

    // Informacja o kierunku
    lcd.setCursor(12, 0);
    lcd.print(encoderDirection ? "DO OSI+" : "OD OSI-");
  }
}