#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <HX711.h>
#include <EEPROM.h>

// ===== KONFIGURACJA =====
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define OLED_ADDRESS 0x3C

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Piny HX711
#define HX711_DOUT 16
#define HX711_SCK 4

// Przyciski
#define BTN_TARA 25
#define BTN_CALIB 26

// HX711 - TYLKO JEDEN!
HX711 scale;

// Zmienne
float calibration_factor = 2000.0;  // Wartość startowa
float current_weight = 0.0;
bool calibration_mode = false;

// ZMIEŃ NA MASĘ SWOJEGO ODWAŻNIKA!
float known_weight = 800.0;  // 1000g = 1kg

// EEPROM
#define EEPROM_SIZE 64
#define ADDR_CALIBRATION 0

// Debouncing
unsigned long lastButtonPress = 0;
const unsigned long DEBOUNCE_DELAY = 300;

// Filtrowanie
const int NUM_READINGS = 5;
float readings[NUM_READINGS];
int readIndex = 0;
float total = 0;

void setup() {
  Serial.begin(115200);
  Serial.println("\n=== WAGA 4-CZUJNIKOWA ===");
  
  // EEPROM
  EEPROM.begin(EEPROM_SIZE);
  
  // Przyciski
  pinMode(BTN_TARA, INPUT_PULLUP);
  pinMode(BTN_CALIB, INPUT_PULLUP);
  
  // OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {
    Serial.println(F("BŁĄD: OLED nie odpowiada!"));
    Serial.println(F("Sprawdź połączenia SDA/SCL"));
    for(;;);
  }
  
  showMessage("WAGA ESP32", "Inicjalizacja...", 1000);
  
  // HX711 Init
  scale.begin(HX711_DOUT, HX711_SCK);
  
  if(!scale.is_ready()) {
    Serial.println(F("BŁĄD: HX711 nie odpowiada!"));
    showMessage("BLAD HX711!", "Sprawdz polacz.", 0);
    for(;;);
  }
  
  Serial.println("HX711 - OK");
  
  // Wczytaj kalibrację
  loadCalibration();
  scale.set_scale(calibration_factor);
  
  // Tarowanie początkowe
  showMessage("Tarowanie", "Oprozn wage!", 2000);
  
  scale.tare();
  delay(500);
  
  // Inicjalizacja filtra
  for(int i = 0; i < NUM_READINGS; i++) {
    readings[i] = 0;
  }
  
  showMessage("GOTOWE!", "", 1000);
  
  Serial.println("=== SYSTEM GOTOWY ===");
  Serial.print("Współczynnik kalibracji: ");
  Serial.println(calibration_factor, 2);
  Serial.print("Masa kalibracyjna: ");
  Serial.print(known_weight, 0);
  Serial.println("g");
  Serial.println("------------------------");
}

void loop() {
  unsigned long currentMillis = millis();
  
  // Przycisk TARA
  if(digitalRead(BTN_TARA) == LOW && (currentMillis - lastButtonPress > DEBOUNCE_DELAY)) {
    handleTara();
    lastButtonPress = currentMillis;
  }
  
  // Przycisk KALIBRACJA
  if(digitalRead(BTN_CALIB) == LOW && (currentMillis - lastButtonPress > DEBOUNCE_DELAY)) {
    handleCalibration();
    lastButtonPress = currentMillis;
  }
  
  // Odczyt wagi
  if(!calibration_mode) {
    readWeight();
    updateDisplay();
  }
  
  delay(100);
}

void readWeight() {
  if(scale.is_ready()) {
    // Odczyt z uśrednieniem
    float raw_weight = scale.get_units(3);
    
    // Filtr ruchomej średniej
    total = total - readings[readIndex];
    readings[readIndex] = raw_weight;
    total = total + readings[readIndex];
    readIndex = (readIndex + 1) % NUM_READINGS;
    
    current_weight = total / NUM_READINGS;
    
    // Eliminacja szumu dla małych wartości
    if(abs(current_weight) < 2.0) {
      current_weight = 0.0;
    }
    
    // Debug
    Serial.print("Waga: ");
    Serial.print(current_weight, 1);
    Serial.println(" g");
  } else {
    Serial.println("HX711 nie gotowy!");
  }
}

void updateDisplay() {
  display.clearDisplay();
  
  // Nagłówek
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(F("== WAGA 4-SENS =="));
  
  // Główny wynik - DUŻE CYFRY
  display.setTextSize(2);
  display.setCursor(0, 18);
  
  if(abs(current_weight) < 1000) {
    // Wyświetl w gramach
    display.print(current_weight, 1);
    display.setTextSize(1);
    display.println(" g");
  } else {
    // Wyświetl w kilogramach
    display.print(current_weight / 1000.0, 2);
    display.setTextSize(1);
    display.println(" kg");
  }
  
  // Dodatkowe info
  display.setTextSize(1);
  display.setCursor(0, 42);
  display.print(F("Cal: "));
  display.println(calibration_factor, 0);
  
  // Pasek statusu
  display.drawLine(0, 52, 127, 52, SSD1306_WHITE);
  display.setCursor(0, 55);
  display.print(F("T:Tara  C:Kalib"));
  
  display.display();
}

void handleTara() {
  Serial.println(">>> TAROWANIE <<<");
  
  showMessage("TAROWANIE", "Czekaj...", 0);
  
  scale.tare();
  
  // Reset filtra
  for(int i = 0; i < NUM_READINGS; i++) {
    readings[i] = 0;
  }
  total = 0;
  
  delay(500);
  
  showMessage("TARA OK!", "Wyzerowano", 1500);
  
  Serial.println("Tarowanie zakończone");
}

void handleCalibration() {
  calibration_mode = true;
  Serial.println("\n=== ROZPOCZYNAM KALIBRACJĘ ===");
  
  // Krok 1: Informacja
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 5);
  display.println(F("== KALIBRACJA =="));
  display.println();
  display.println(F("Potrzebny:"));
  display.setTextSize(2);
  display.print(known_weight, 0);
  display.println(F("g"));
  display.setTextSize(1);
  display.println();
  display.println(F("Za 3 sek"));
  display.println(F("oprozn wage!"));
  display.display();
  
  delay(3000);
  
  // Krok 2: Tarowanie
  showMessage("KROK 1/2", "Tarowanie...", 0);
  Serial.println("Tarowanie...");
  
  scale.tare();
  delay(1500);
  
  // Krok 3: Połóż ciężar
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println(F("KROK 2/2"));
  display.println();
  display.println(F("Poloz NA SRODKU:"));
  display.setTextSize(2);
  display.print(known_weight, 0);
  display.println(F(" g"));
  display.setTextSize(1);
  display.println();
  display.println(F("Czekaj 5 sek..."));
  display.display();
  
  Serial.print("Połóż ");
  Serial.print(known_weight, 0);
  Serial.println("g na środku platformy");
  
  delay(5000);
  
  // Krok 4: Pomiar
  showMessage("Pomiar...", "Nie ruszaj!", 0);
  Serial.println("Pomiar...");
  
  // Odczyt surowej wartości (bez kalibracji)
  scale.set_scale(1.0);  // Ustaw na 1 żeby dostać surowe dane
  
  long raw_value = 0;
  const int samples = 20;
  
  for(int i = 0; i < samples; i++) {
    if(scale.is_ready()) {
      raw_value += scale.get_units(1);
    }
    delay(100);
  }
  
  raw_value = raw_value / samples;
  
  Serial.print("Surowa wartość: ");
  Serial.println(raw_value);
  
  // Krok 5: Obliczenie współczynnika
  if(raw_value > 100) {
    calibration_factor = (float)raw_value / known_weight;
    
    Serial.print("Nowy współczynnik: ");
    Serial.println(calibration_factor, 2);
    
    // Zastosuj
    scale.set_scale(calibration_factor);
    
    // Zapisz
    saveCalibration();
    
    // Komunikat sukcesu
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(0, 10);
    display.println(F("=== SUKCES! ==="));
    display.println();
    display.print(F("Factor: "));
    display.println(calibration_factor, 1);
    display.println();
    display.println(F("Kalibracja"));
    display.println(F("zapisana!"));
    display.println();
    display.println(F("Zdejmij ciezar"));
    display.display();
    
    delay(4000);
    
    // Auto-tara
    showMessage("Auto-tara", "Czekaj...", 0);
    delay(2000);
    scale.tare();
    
    showMessage("GOTOWE!", "Mozna wazyc", 2000);
    
    Serial.println("=== KALIBRACJA ZAKOŃCZONA ===\n");
    
  } else {
    // Błąd
    Serial.println("BŁĄD: Brak odczytu lub za mała wartość!");
    showMessage("BLAD!", "Brak ciezaru", 3000);
    
    // Przywróć poprzedni współczynnik
    scale.set_scale(calibration_factor);
  }
  
  calibration_mode = false;
}

void saveCalibration() {
  EEPROM.put(ADDR_CALIBRATION, calibration_factor);
  EEPROM.commit();
  Serial.println("Współczynnik zapisany do EEPROM");
}

void loadCalibration() {
  float stored_value;
  EEPROM.get(ADDR_CALIBRATION, stored_value);
  
  // Walidacja
  if(stored_value > 1.0 && stored_value < 1000000.0) {
    calibration_factor = stored_value;
    Serial.print("Wczytano kalibrację z EEPROM: ");
    Serial.println(calibration_factor, 2);
  } else {
    Serial.println("EEPROM pusta - używam wartości domyślnej");
    calibration_factor = 2000.0;
  }
}

void showMessage(const char* line1, const char* line2, int delayTime) {
  display.clearDisplay();
  display.setTextSize(1);
  
  display.setCursor(0, 20);
  display.println(line1);
  
  display.setCursor(0, 35);
  display.println(line2);
  
  display.display();
  
  if(delayTime > 0) {
    delay(delayTime);
  }
}