#include <HardwareSerial.h>
#include <LiquidCrystal_I2C.h>
#include <PZEM004Tv30.h>

#define CONSOLE_SERIAL    Serial   // Текстовий вивід
#define I2C_ADDR          0x27     // адреса дисплея
#define LCD_COLUMNS       20       // Кількість стовпців
#define LCD_LINES         4        // Кількість рядків
#define PZEM_SERIAL       Serial2  // Послідовний інтерфейс комунікації сенсору з контролером
#define PZEM_DEFAULT_ADDR 0xF8     // Aдреса сенсору за замовчуванням
#define PZEM_ADDR         0x55     // Адреса сенсору
#define PZEM_ADDR_INC     true     // Флаг інкременту адреси сенсору кожен цикл
#define PZEM_RX_PIN       16       // Пін RX фізичного послідовного інтерфейсу контролера 
#define PZEM_TX_PIN       17       // Пін TX фізичного послідовного інтерфейсу контролера

// Ініціалізуємо сенсор
// https://github.com/mandulaj/PZEM-004T-v30/blob/eec09ab4574db567306a3d335e5131c766e3333e/src/PZEM004Tv30.h#L82 
// Не оновлена документація - наявна зміна API, згідно якої ця функція тепер приймає посилання на послідовний порт замість вказівника
// Раніше: PZEM004Tv30(Serial2);
// Тепер: PZEM004Tv30(&Serial2);
PZEM004Tv30 pzem(&PZEM_SERIAL, PZEM_RX_PIN, PZEM_TX_PIN); 
// Ініціалізуємо дисплей
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES); 

void setup() {
  CONSOLE_SERIAL.begin(115200);

  lcd.init();
  lcd.backlight();
  // Щоб зкинути внутрішній лічильник енергії - розкоментувати рядок нижче
  // pzem.resetEnergy();
}

void loop() {
  static uint8_t addr = PZEM_ADDR;
  CONSOLE_SERIAL.print("Previous address: 0x");
  CONSOLE_SERIAL.println(pzem.readAddress(), HEX);
  CONSOLE_SERIAL.print("Setting address to 0x");
  CONSOLE_SERIAL.println(addr, HEX);

  if(!pzem.setAddress(addr)) {
    CONSOLE_SERIAL.println("ADDRESS set error!");
  } else {
    CONSOLE_SERIAL.print("Current address: 0x");
    CONSOLE_SERIAL.println(pzem.readAddress(), HEX);  
  }

  if(PZEM_ADDR_INC) {
    addr++;

    if (addr >= PZEM_DEFAULT_ADDR) {
      addr = 0x01;
    }
  }
  delay(5000);
  // lcd.setCursor(0, 0);
  // lcd.print("Custom Address: ");
  // lcd.setCursor(16, 0);
  // lcd.print(pzem.readAddress(), HEX);
  // delay(3500);
  // lcd.clear();

  // Отримуємо дані від сенсора
  float voltage = pzem.voltage();
  float current = pzem.current();
  float power = pzem.power();
  float energy = pzem.energy();
  float frequency = pzem.frequency();
  float pf = pzem.pf();

  // Перевіряємо валідність даних
  if(isnan(voltage)){
    lcd.setCursor(0, 0);
    lcd.print("VOLTAGE read error!");
    delay(3500);
    lcd.clear();
  } else if (isnan(current)) {
    lcd.setCursor(0, 0);
    lcd.print("CURRENT read error!");
    delay(3500);
    lcd.clear();
  } else if (isnan(power)) {
    lcd.setCursor(0, 0);
    lcd.print("POWER read error!");
    delay(3500);
    lcd.clear();
  } else if (isnan(energy)) {
    lcd.setCursor(0, 0);
    lcd.print("ENERGY read error!");
    delay(3500);
    lcd.clear();
  } else if (isnan(frequency)) {
    lcd.setCursor(0, 0);
    lcd.print("FREQ read error!");
    delay(3500);
    lcd.clear();
  } else if (isnan(pf)) {
    lcd.setCursor(0, 0);
    lcd.print("PF read error!");
    delay(3500);
    lcd.clear();
  } else {
        // Всі дані валідні; послідовно виводимо на екран
        lcd.setCursor(0, 0);
        lcd.print("Voltage: ");
        lcd.write(voltage);
        lcd.print("V");
        lcd.setCursor(0, 1);
        lcd.print("Current: ");
        lcd.write(current);
        lcd.print("A");
        lcd.setCursor(0, 2);
        lcd.print("Power: ");
        lcd.write(power);
        lcd.print("W");
        lcd.setCursor(0, 3);
        lcd.print("Energy: ");
        lcd.write(energy);
        lcd.print("kWh");
        delay(3500);
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Frequency: ");
        lcd.write(frequency);
        lcd.print("Hz");
        lcd.setCursor(0, 1);
        lcd.print("PF: ");
        lcd.write(pf);
        delay(3500);
        lcd.clear();
    }
}