#include <ModbusRTUMaster.h>  // https://github.com/CMB27/ModbusRTUMaster/
#include <Wire.h>
#include <Adafruit_GFX.h>      // https://github.com/adafruit/Adafruit-GFX-Library
#include <Adafruit_SSD1306.h>  // https://github.com/adafruit/Adafruit_SSD1306
#include <Arduino.h>
#include <math.h>

#include <WiFi.h>
#include "ESPAsyncWebServer.h"
#include <Arduino_JSON.h>     // http://github.com/arduino-libraries/Arduino_JSON
#include "LittleFS.h"

const char* ssid = "PWR_METER";
const char* password = "123456";

// Configure Modbus device to 9600 Baud, SERIAL_8N1
uint8_t unitId = 8;  // Modubus device address


// Define RS485 pins
#define TX_PIN 5  // RS485 TX
#define RX_PIN 6  // RS485 RX
#define DE_PIN 8  // Driver Enable (tie RE & DE together)

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);


// --- Configure Modbus Interface ---

// -- Code valid for usage with Arduino Nano ---
// #include <SoftwareSerial.h>
// SoftwareSerial mySerial(RX_PIN, TX_PIN);
// #define MODBUS_SERIAL mySerial

// --- Code valid for usage with ESP32
#include <HardwareSerial.h>
HardwareSerial MySerial1(1);  // Define Serial device mapped to the on of the internal UARTs
#define MODBUS_SERIAL MySerial1
ModbusRTUMaster modbus(MODBUS_SERIAL, DE_PIN);

// Webserver
AsyncWebServer server(80);
JSONVar readings_json;


// --- Configure Peak Hold ---
const int peakHoldTime = 500;  // Peak hold duration (ms)


// --- Configure low pass filter ---
float cutoff_freq = 0.1;  // Cutoff frequency in Hz
float dt = 0.1;           // Sampling interval in seconds


// Other variable definitions
float filter_y_prev = 0.0;
float RC = 1.0 / (2.0 * M_PI * cutoff_freq);
float filter_alpha = dt / (RC + dt);

uint8_t error = 0;
uint16_t value[6];  // Buffer for multiple 16-bit registers
uint16_t address = 0x2000;

float peakValue = 0;
int peakIndex = 0;           // Stores the highest LED level
unsigned long peakTime = 0;  // Stores when peak LED was last updated
int dt_delay = dt * 1000;

const uint8_t numErrorStrings = 12;
const char* errorStrings[numErrorStrings] = {
  "Response timeout",
  "Frame error in response",
  "CRC error in response",
  "Unknown communication error",
  "Unexpected unit ID in response",
  "Exception response ",
  "Unexpected function code in response",
  "Unexpected response length",
  "Unexpected byte count in response",
  "Unexpected data address in response",
  "Unexpected value in response",
  "Unexpected quantity in response"
};

const uint8_t numExceptionResponseStrings = 4;
const char* exceptionResponseStrings[] = { "illegal function", "illegal data address", "illegal data value", "server device failure" };

union {
  uint16_t words[2];
  float value;
} U_V;

union {
  uint16_t words[2];
  float value;
} I_A;

union {
  uint16_t words[2];
  float value;
} P_W;


// Get Sensor Readings and return JSON object
String format_readings_json() {
  readings_json["P_W"] = String(P_W.value, 1);
  readings_json["U_V"] = String(U_V.value, 1);
  readings_json["I_A"] = String(I_A.value, 3);
  String jsonString = JSON.stringify(readings_json);
  return jsonString;
}


void setup() {

  Wire.begin(2, 3);
  Serial.begin(115200);
  Serial.println("\n\nModbusRTUMasterProbe");
  delay(1000);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {  // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;  // Don't proceed, loop forever
  }
  Serial.println(F("SSD1306 allocation OK"));

  if (!LittleFS.begin(true)) {
    Serial.println("An error has occurred while mounting LittleFS");
  }
  Serial.println("LittleFS mounted successfully");


  MODBUS_SERIAL.begin(9600, SERIAL_8N1, RX_PIN, TX_PIN);
  modbus.begin(9600);  // SERIAL_8N1

  // Setup Wifi
  WiFi.softAP(ssid);  //, password);
  Serial.print("AP IP address: ");
  Serial.println(WiFi.softAPIP());

  // ----------- Spalsh Screen  ---------------
  display.clearDisplay();
  display.setRotation(2);
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Powermeter");
  display.setTextSize(1);
  display.setCursor(0, 10);
  display.println("by dustinbrun, 2026");
  display.setCursor(0, 20);
  display.println(WiFi.softAPIP());
  display.display();
  delay(2000);
  display.clearDisplay();


  // main webpage handler
  server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
    request->send(LittleFS, "/index.html", "text/html");
  });

  server.serveStatic("/", LittleFS, "/");

  // readings webpage handler
  server.on("/readings", HTTP_GET, [](AsyncWebServerRequest* request) {
    request->send(200, "application/json", format_readings_json());
  });
  server.begin();
}

void update_display() {
  display.clearDisplay();

  display.setRotation(2);
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  // Power value
  display.setCursor(10, 0);
  if (P_W.value < 10)
    display.print("000");
  else if (P_W.value < 100)
    display.print("00");
  else if (P_W.value < 1000)
    display.print("0");
  display.print(P_W.value, 1);
  display.setCursor(95, 0);
  display.print("W");

  // Voltage value
  display.setCursor(10, 15);
  if (U_V.value < 10)
    display.print("0");
  else if (U_V.value < 10)
    display.print("00");
  display.print(U_V.value, 1);
  display.setCursor(95, 15);
  display.print("V");

  // Current value
  display.setRotation(3);
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print(I_A.value, 2);

  // error code (if possible)
  if (error) {
    display.setTextSize(1);
    display.setCursor(115, 0);
    display.print("ERR");
    display.print(error);
  }
  //display.setRotation(0);
  display.setRotation(2);

  // Bar graph display

  // Low pass filtering of the bar graph scale maximum value
  float new_target = P_W.value * 2;  // Target is double of the current value -> stable power value should end up as a line filling half of the screen width
  filter_y_prev = filter_alpha * new_target + (1 - filter_alpha) * filter_y_prev;
  // Serial.print(new_target);
  // Serial.print("\t");
  // Serial.println(filter_y_prev);

  // map value to screen width
  int percent = map(P_W.value, 0, int(filter_y_prev), 0, SCREEN_WIDTH);
  display.fillRect(1, 29, percent, 3, SSD1306_WHITE);  // x Top left corner x coordinate, y Top left corner y coordinate, w Width in pixels, h Height in pixels

  // Peak hold and decay
  if (P_W.value > peakValue) {
    peakValue = P_W.value;
    peakTime = millis();
  } else if (millis() - peakTime > peakHoldTime) {
    peakValue = max(peakValue - 1, P_W.value);
    peakTime = millis();
  }
  // Draw peak value on bar graph display
  peakIndex = peakValue * SCREEN_WIDTH / filter_y_prev;  // map peak value to current index on display
  if (peakIndex > SCREEN_WIDTH-2)
    peakIndex = SCREEN_WIDTH-2;
  display.fillRect(peakIndex, 30, 2, 1, SSD1306_WHITE);
  display.fillRect(peakIndex+2, 29, 2, 3, SSD1306_WHITE);


  display.display();
}



void loop() {

  // Query modbus device registers
  error = modbus.readHoldingRegisters(unitId, address, value, 6);

  if (!error) {

    // 0x2000 = Voltage [V]
    U_V.words[0] = value[1];
    U_V.words[1] = value[0];

    // 0x2002 = Current [A]
    I_A.words[0] = value[3];
    I_A.words[1] = value[2];

    // 0x2004 = active Power [kW]
    P_W.words[0] = value[5];
    P_W.words[1] = value[4];
    P_W.value *= 1000;  // convert to [W]

    // Debug output to console
    Serial.print(U_V.value, 1);
    Serial.print("\t");
    Serial.print(I_A.value, 3);
    Serial.print("\t");
    Serial.print(P_W.value, 1);
    Serial.println();

  } else {

    Serial.print("Error: ");
    if (error >= 4 && error < (numErrorStrings + 4)) {
      Serial.print(errorStrings[error - 4]);
      if (error == MODBUS_RTU_MASTER_EXCEPTION_RESPONSE) {
        uint8_t exceptionResponse = modbus.getExceptionResponse();
        Serial.print(exceptionResponse);
        if (exceptionResponse >= 1 && exceptionResponse <= numExceptionResponseStrings) {
          Serial.print(" - ");
          Serial.print(exceptionResponseStrings[exceptionResponse - 1]);
        }
      }
    } else Serial.print("Unknown error");
    Serial.println();
  }

  update_display();

  delay(dt_delay);
}