#include <ETH.h>
#include <Wire.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <EmonLib.h>
#include <ZMPT101B.h>
#include <U8g2lib.h>
#include "SHT31.h"
#include "Arduino.h"

// Definiciones para Ethernet y MQTT
#define ETH_ADDR        0
#define ETH_POWER_PIN  -1
#define ETH_MDC_PIN    23
#define ETH_MDIO_PIN   18
#define ETH_TYPE       ETH_PHY_LAN8720
#define ETH_CLK_MODE   ETH_CLOCK_GPIO17_OUT

IPAddress local_ip(10, 0, 0, 149);
IPAddress gateway(10, 0, 0, 1);
IPAddress subnet(255, 255, 255, 0);
IPAddress dns(10, 0, 0, 1);

const char* server = "10.0.0.16";
const char* mqttuser = "";
const char* mqttpass = "";
const char* clientID = "m16v2";

// Pines del multiplexor
#define s0 32
#define s1 33
#define s2 13
#define s3 16
#define IN3 35

// Sensores y objetos
SHT31 sht;
U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, 5, 4, U8X8_PIN_NONE);
WiFiClient espClient;
PubSubClient client(espClient);
EnergyMonitor emon1;
ZMPT101B voltageSensor1(34, 60.0);
ZMPT101B voltageSensor2(39, 60.0);
ZMPT101B voltageSensor3(36, 60.0);

// Sensibilidades y variables de medición
#define SHT31_ADDRESS_1 0x44
#define SENSITIVITY1 501.00f
#define SENSITIVITY2 500.25f
#define SENSITIVITY3 501.25f
float temperature, humidity;
float ct1amps, ct2amps, ct3amps, ct4amps, ct5amps, ct6amps, ct7amps, ct8amps;
float ct1watts, ct2watts, ct3watts, ct4watts, ct5watts, ct6watts, ct7watts, ct8watts;
float p_pv, p_grid, p_load, p_home;
float l_pv, l_grid, l_load, l_home;
float voltage1, voltage2, voltage3, voltage_grid;

void setup() {
  Serial.begin(115200);
  pinMode(s0, OUTPUT);
  pinMode(s1, OUTPUT);
  pinMode(s2, OUTPUT);
  pinMode(s3, OUTPUT);
  pinMode(IN3, INPUT);

  u8g2.setI2CAddress(0x3C*2);
  u8g2.begin();
  u8g2.enableUTF8Print();

  Wire.begin(4,5);
  Wire.setClock(100000);
  sht.begin();

  setup_ethernet();
  client.setBufferSize(2048);
  client.setServer(server, 1883);
  client.setCallback(callback);

  emon1.current(IN3, 34.4820); // ADC_PIN is the pin where SCT013 is connected

  voltageSensor1.setSensitivity(SENSITIVITY1);
  voltageSensor2.setSensitivity(SENSITIVITY2);
  voltageSensor3.setSensitivity(SENSITIVITY3);
}

void loop() {
  if (!client.connected()) {
    reconnect();
  }

  client.loop();
  Serial.println("done loop");

  sht.read(); 
  temperature = sht.getTemperature();
  humidity = sht.getHumidity();

   u8g2.firstPage();
  do {
    page1();
  } while (u8g2.nextPage());

  voltage1 = voltageSensor1.getRmsVoltage();
  voltage2 = voltageSensor2.getRmsVoltage();
  voltage3 = voltageSensor3.getRmsVoltage();
  voltage_grid = voltage1 + voltage2;

  measureCurrent(LOW, LOW, LOW, LOW, ct1amps);
  measureCurrent(HIGH, LOW, LOW, LOW, ct2amps);
  measureCurrent(LOW, HIGH, LOW, LOW, ct3amps);
  measureCurrent(HIGH, HIGH, LOW, LOW, ct4amps);
  measureCurrent(LOW, LOW, HIGH, LOW, ct5amps);
  measureCurrent(HIGH, LOW, HIGH, LOW, ct6amps);
  measureCurrent(LOW, HIGH, HIGH, LOW, ct7amps);
  measureCurrent(HIGH, HIGH, HIGH, LOW, ct8amps);

/*
  measureCurrent(LOW, LOW, LOW, HIGH, ct9amps);
  measureCurrent(HIGH, LOW, LOW, HIGH, ct10amps);
  measureCurrent(LOW, HIGH, LOW, HIGH, ct11amps);
  measureCurrent(HIGH, HIGH, LOW, HIGH, ct12amps);
  measureCurrent(LOW, LOW, HIGH, HIGH, ct13amps);
  measureCurrent(HIGH, LOW, HIGH, HIGH, ct14amps);
  measureCurrent(LOW, HIGH, HIGH, HIGH, ct15amps);
  measureCurrent(HIGH, HIGH, HIGH, HIGH, ct16amps);
*/

  ct1watts = ct1amps * voltage1;
  ct2watts = ct2amps * voltage2;
  ct3watts = ct3amps * voltage1;
  ct4watts = ct4amps * voltage2;
  ct5watts = ct5amps * voltage1;
  ct6watts = ct6amps * voltage2;
  ct7watts = ct7amps * voltage3/2;
  ct8watts = ct8amps * voltage3/2;

  p_grid = ct1watts + ct2watts;
  p_pv   = ct3watts + ct4watts;
  p_load = ct5watts + ct6watts;
  p_home = ct7watts + ct8watts;

  if (p_pv > p_load){
    p_grid = p_grid * -1;
  }
  
  publishMQTT();
  delay(1000);
}

void publishMQTT() {
  client.publish("m16v2/sensor/voltage_ac_l1/state", String(voltage1).c_str());
  client.publish("m16v2/sensor/voltage_ac_l2/state", String(voltage2).c_str());
  client.publish("m16v2/sensor/voltage_ac_total/state", String(voltage_grid).c_str());
  client.publish("m16v2/sensor/voltage_ac_interno/state", String(voltage3).c_str());
  client.publish("m16v2/sensor/current_ct_01/state", String(ct1amps).c_str());
  client.publish("m16v2/sensor/current_ct_02/state", String(ct2amps).c_str());
  client.publish("m16v2/sensor/current_ct_03/state", String(ct3amps).c_str());
  client.publish("m16v2/sensor/current_ct_04/state", String(ct4amps).c_str());
  client.publish("m16v2/sensor/current_ct_05/state", String(ct5amps).c_str());
  client.publish("m16v2/sensor/current_ct_06/state", String(ct6amps).c_str());
  client.publish("m16v2/sensor/current_ct_07/state", String(ct7amps).c_str());
  client.publish("m16v2/sensor/current_ct_08/state", String(ct8amps).c_str());
  client.publish("m16v2/sensor/power_ct_01/state", String(ct1watts).c_str());
  client.publish("m16v2/sensor/power_ct_02/state", String(ct2watts).c_str());
  client.publish("m16v2/sensor/power_ct_03/state", String(ct3watts).c_str());
  client.publish("m16v2/sensor/power_ct_04/state", String(ct4watts).c_str());
  client.publish("m16v2/sensor/power_ct_05/state", String(ct5watts).c_str());
  client.publish("m16v2/sensor/power_ct_06/state", String(ct6watts).c_str());
  client.publish("m16v2/sensor/power_ct_07/state", String(ct7watts).c_str());
  client.publish("m16v2/sensor/power_ct_08/state", String(ct8watts).c_str());  
  client.publish("m16v2/sensor/p_grid/state", String(p_grid).c_str());
  client.publish("m16v2/sensor/p_pv/state", String(p_pv).c_str());
  client.publish("m16v2/sensor/p_load/state", String(p_load).c_str());
  client.publish("m16v2/sensor/p_home/state", String(p_home).c_str());
  client.publish("m16v2/sensor/humidity/state", String(humidity).c_str());
  client.publish("m16v2/sensor/temperature/state", String(temperature).c_str());
}

void page1() {
  u8g2.setFont(u8g2_font_timR12_tf); // Fuente 12

  // Mostrar título
  u8g2.setCursor(0, 15);
  u8g2.print("KC868-M16 V2");

  // Mostrar temperatura
  u8g2.setCursor(0, 35);
  u8g2.print("Temp: ");
  u8g2.print(temperature);
  u8g2.print(" C");

  // Mostrar humedad
  u8g2.setCursor(0, 55);
  u8g2.print("Humidity: ");
  u8g2.print(humidity);
  u8g2.print(" %");
}

void setup_ethernet() {
  ETH.begin(ETH_TYPE, ETH_ADDR, ETH_MDC_PIN, ETH_MDIO_PIN, ETH_POWER_PIN, ETH_CLK_MODE); // start with ETH
  if (ETH.config(local_ip, gateway, subnet, dns, dns) == false) {
    Serial.println("LAN8720 Configuration failed.");
  } else {
    Serial.println("LAN8720 Configuration success.");
  }
  Serial.println("Connected");
  Serial.print("IP Address:");
  Serial.println(ETH.localIP());
}

void reconnect() {
  while (!client.connected()) {
    if (client.connect(clientID, mqttuser, mqttpass)) {
      Serial.println("MQTT connected");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

void callback(char* topic, byte* payload, unsigned int length) {
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  Serial.println("");
}

String byteToHexString(uint8_t byte) {
  String hexStr = String(byte, HEX);
  if (hexStr.length() == 1) {
    hexStr = "0" + hexStr;
  }
  return hexStr;
}

float measureCurrent(bool s0_state, bool s1_state, bool s2_state, bool s3_state, float& ct) {
  digitalWrite(s0, s0_state);
  digitalWrite(s1, s1_state);
  digitalWrite(s2, s2_state);
  digitalWrite(s3, s3_state);
  if (analogRead(IN3) != 0) {
    ct = emon1.calcIrms(1480);
  }
  return ct;
}