#include <esp_now.h> 
#include <WiFi.h>
#include <PubSubClient.h>
#include <PZEM004Tv30.h>
#define PIN_RELAY_1 2
#define PIN_RELAY_2 4
#define PIN_RELAY_3 15
#define PIN_LED_HIJAU 32
#define PIN_LED_MERAH 33
#define PIN_BUZZER 25
#define PIN_RST 5
#if defined(ESP32)
PZEM004Tv30 pzem(Serial2, 16, 17);
#else
PZEM004Tv30 pzem(Serial2);
#endif
const char* ssid = "Ojan";
const char* password = "44332211";
const char *mqtt_server = "broker.hivemq.com";
const int mqtt_port = 1883;
const char *topic_pub1 = "current/nodered/pub";
const char *topic_pub2 = "voltage/nodered/pub";
const char *topic_pub3 = "temperature1/nodered/pub";
const char *topic_pub4 = "temperature2/nodered/pub";
const char *topic_pub5 = "Id1/pub";
const char *topic_pub6 = "Id2/pub";
const char *topic_pub7 = "readingId1/pub";
const char *topic_pub8 = "readingId2/pub";
const char *topic_pub9 = "rst/pub";
const char *topic_sub = "tugasakhir/sub";
String client_id = "ESP32_Client";
WiFiClient espClient;
PubSubClient client(espClient);
esp_now_peer_info_t slave;
int chan;
int counter = 0;
int pushButtonState;
int var;
int var_rst = 0;
float B_RST = 0.000;
float batasOverloadarus = 4.000;
float batasDropvoltage = 198.000;
float batasSuhu1 = 70.000;
float batasSuhu2 = 70.000;
float current;
float voltage;
float power;
float energy;
float frequency;
float pf;
unsigned long currentMillis = millis();
unsigned long previousMillis = 0;
unsigned long interval = 2000;
enum MessageType {PAIRING, DATA,};
MessageType messageType;
typedef struct struct_message{
  uint8_t msgType;
  uint8_t id;
  float temp;
  unsigned int readingId;
} struct_message;
typedef struct struct_pairing{
  uint8_t msgType;
  uint8_t id;
  uint8_t macAddr[6];
  uint8_t channel;
}struct_pairing;
struct_message incomingReadings;
struct_message board1;
struct_message board2;
struct_message boardStruct[2] = {board1, board2};
struct_pairing pairingData;
void kondisiAwalSistem();
void stateRST();
void errorInstalation_1();
void errorInstalation_2();
void errorInstalation_3();
void instalationChecking();
void printMAC(const uint8_t * mac_addr){
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x", mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  Serial.print(macStr);
}
bool addPeer(const uint8_t *peer_addr){
  memset(&slave, 0, sizeof(slave));
  const esp_now_peer_info_t *peer = &slave;
  memcpy(slave.peer_addr, peer_addr, 6);
  slave.channel = chan;
  slave.encrypt = 0;
  bool exists = esp_now_is_peer_exist(slave.peer_addr);
  if(exists){
    Serial.println("Already Paired");
    return true;
  }else{
    esp_err_t addStatus = esp_now_add_peer(peer);
    if(addStatus == ESP_OK){
      Serial.println("Pair Success");
      return true;
    }else{
      Serial.println("Pair Failed");
      return false;
    }
  }
}
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status){
  Serial.print("Last Packet Send Status: ");
  Serial.print(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success to" : "Delivery Fail to");
  printMAC(mac_addr);
  Serial.println();
}
void OnDataRecv(const uint8_t * mac_addr, const uint8_t *incomingData, int len){
  Serial.print(len);
  Serial.print(" Bytes of data received from: ");
  printMAC(mac_addr);
  Serial.println();
  uint8_t type = incomingData[0];
  switch (type) {
  case DATA :
    memcpy(&incomingReadings, incomingData, sizeof(incomingReadings));
    Serial.printf("Board ID %u: %u bytes\n", incomingReadings.id, len);
    boardStruct[incomingReadings.id-1].temp = incomingReadings.temp;
    boardStruct[incomingReadings.id-1].id = incomingReadings.id;
    boardStruct[incomingReadings.id-1].readingId = incomingReadings.readingId;
    Serial.printf("temp value: %d \n", boardStruct[incomingReadings.id-1].temp);
    Serial.printf("ID value: %d \n", boardStruct[incomingReadings.id-1].id);
    Serial.printf("readingId value: %d \n", boardStruct[incomingReadings.id-1].readingId);
    Serial.println();
    break;
  case PAIRING:
    memcpy(&pairingData, incomingData, sizeof(pairingData));
    Serial.println(pairingData.msgType);
    Serial.println(pairingData.id);
    Serial.print("Pairing request from: ");
    printMAC(mac_addr);
    Serial.println();
    Serial.println(pairingData.channel);
    if(pairingData.id > 0){
      if(pairingData.msgType == PAIRING){
        pairingData.id = 0;
        WiFi.softAPmacAddress(pairingData.macAddr);
        pairingData.channel = chan;
        Serial.println("send response");
        esp_err_t result = esp_now_send(mac_addr, (uint8_t *) &pairingData, sizeof(pairingData));
        addPeer(mac_addr);
      }
    }
    break;
  }
}
void initESP_NOW(){
  if(esp_now_init() != ESP_OK){
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  esp_now_register_send_cb(OnDataSent);
  esp_now_register_recv_cb(OnDataRecv);
}
void Callback(char *topic, byte *payload, unsigned int length){
  Serial.print("Dari Topik: ");
  Serial.println(topic);
  Serial.print("Pesannya: ");
  for(int i=0; i < length; i++){
    Serial.print((char) payload[i]);
  }
  Serial.println();
}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(PIN_RELAY_1, OUTPUT);
  pinMode(PIN_RELAY_2, OUTPUT);
  pinMode(PIN_RELAY_3, OUTPUT);
  pinMode(PIN_LED_HIJAU, OUTPUT);
  pinMode(PIN_LED_MERAH, OUTPUT);
  pinMode(PIN_RST, INPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  Serial.println();
  Serial.print("Server MAC Address: ");
  Serial.println(WiFi.macAddress());
  WiFi.mode(WIFI_AP_STA);
  WiFi.begin(ssid, password);
  while(WiFi.status() != WL_CONNECTED){
    delay(1000);
    Serial.println("Setting as a WiFi Station");
  }
  client.setServer(mqtt_broker, mqtt_port);
  client.setCallback(Callback);
  while(!client.connected()){
    if(client.connect(client_id.c_str())){
      Serial.println("Terhubung ke MQTT Broker");
    }else{
      Serial.print("Gagal terhubung");
      Serial.println(client.state());
      delay(2000);
    }
  }
  client.subscribe(topic_sub);
  Serial.print("Server SOFT AP MAC Address: ");
  Serial.println(WiFi.softAPmacAddress());
  chan = WiFi.channel();
  Serial.print("Station IP Address: ");
  Serial.println(WiFi.localIP());
  Serial.print("WiFi Channel: ");
  Serial.println(WiFi.channel());
  initESP_NOW();
}
void loop() {
  // put your main code here, to run repeatedly:
  client.loop();
  float boardTemp1 = boardStruct[0].temp;
  int boardId1 = boardStruct[0].id;
  int boardReadingId1 = boardStruct[0].readingId;
  float boardTemp2 = boardStruct[1].temp;
  int boardId2 = boardStruct[1].id;
  int boardReadingId2 = boardStruct[1].readingId;
  Serial.print("Nilai Suhu Ruangan 1: "); Serial.print(boardTemp1); Serial.print(" dengan Id: "); Serial.print(boardId1); Serial.print(" dan merupakan hasil bacaan ke-"); Serial.println(boardReadingId1);
  Serial.print("Nilai Suhu Ruangan 2: "); Serial.print(boardTemp2); Serial.print(" dengan Id: "); Serial.print(boardId2); Serial.print(" dan merupakan hasil bacaan ke-"); Serial.println(boardReadingId2); 
  kondisiAwalSistem();
  instalationChecking(current, voltage, boardTemp1, boardTemp2);
  Serial.print("Custom Address PZEM: ");
  Serial.println(pzem.readAddress(), HEX);
  current = pzem.current();
  String current_s = String(current);
  voltage = pzem.voltage();
  String voltage_s = String(voltage);
  power = pzem.power();
  String power_s = String(power);
  energy = pzem.energy();
  String energy_s = String(energy);
  frequency = pzem.frequency();
  String frequency_s = String(frequency);
  pf = pzem.pf();
  String pf_s = String(pf);
  if(isnan(voltage)){
    Serial.println("Error reading voltage");
  }else if(isnan(current)){
    Serial.println("Error reading current1");
  }else if(isnan(power)){
    Serial.println("Error reading power");
  }else if(isnan(energy)){
    Serial.println("Error reading energy");
  }else if(isnan(frequency)){
    Serial.println("Error reading frequency");
  }else if(isnan(pf)){
    Serial.println("Error reading power factor");
  }else{
    Serial.print("Voltage: ");    Serial.print(voltage);      Serial.println("V");
    Serial.print("Current: ");    Serial.print(current);      Serial.println("A");
    Serial.print("Power: ");      Serial.print(power);        Serial.println("W");
    Serial.print("Energy: ");     Serial.print(energy, 3);    Serial.println("kWh");
    Serial.print("Frequency: ");  Serial.print(frequency, 1); Serial.println("Hz");
    Serial.print("PF: ");         Serial.print(pf); 
  }
  Serial.println();
  String boardTemp1_s = String(boardTemp1);
  String boardId1_s = String(boardId1);
  String boardReadingId1_s = String(boardReadingId1);
  String boardTemp2_s = String(boardTemp2);
  String boardId2_s = String(boardId2);
  String boardReadingId2_s = String(boardReadingId2);
  if(currentMillis - previousMillis > interval){
    client.publish(topic_pub1, current_s.c_str());
    client.publish(topic_pub2, voltage_s.c_str());
    client.publish(topic_pub3, boardTemp1_s.c_str());
    client.publish(topic_pub4, boardTemp2_s.c_str());
    client.publish(topic_pub5, boardId1_s.c_str());
    client.publish(topic_pub6, boardId2_s.c_str());
    client.publish(topic_pub7, boardReadingId1_s.c_str());
    client.publish(topic_pub8, boardReadingId2_s.c_str());
    //readDataToSend();
    //esp_now_send(NULL, (uint8_t *) &outgoingSetpoints, sizeof(outgoingSetpoints));
  }
}
void kondisiAwalSistem(){
  digitalWrite(PIN_RELAY_1, HIGH);
  digitalWrite(PIN_RELAY_2, HIGH);
  digitalWrite(PIN_RELAY_3, HIGH);
  digitalWrite(PIN_LED_HIJAU, HIGH);
  digitalWrite(PIN_LED_MERAH, LOW);
  digitalWrite(PIN_BUZZER, LOW);
  digitalWrite(PIN_RST, LOW);
}
void errorInstalation_1(){
  digitalWrite(PIN_RELAY_1, LOW);
  digitalWrite(PIN_RELAY_2, LOW);
  digitalWrite(PIN_RELAY_3, LOW);
  digitalWrite(PIN_LED_HIJAU, LOW);
  digitalWrite(PIN_LED_MERAH, HIGH);
  digitalWrite(PIN_BUZZER, HIGH);
  digitalWrite(PIN_RST, LOW);
}
void errorInstalation_2(){
  digitalWrite(PIN_RELAY_1, HIGH);
  digitalWrite(PIN_RELAY_2, LOW);
  digitalWrite(PIN_RELAY_3, HIGH);
  digitalWrite(PIN_LED_HIJAU, LOW);
  digitalWrite(PIN_LED_MERAH, HIGH);
  digitalWrite(PIN_BUZZER, HIGH);
  digitalWrite(PIN_RST, LOW);
}
void errorInstalation_3(){
  digitalWrite(PIN_RELAY_1, HIGH);
  digitalWrite(PIN_RELAY_2, HIGH);
  digitalWrite(PIN_RELAY_3, LOW);
  digitalWrite(PIN_LED_HIJAU, LOW);
  digitalWrite(PIN_LED_MERAH, HIGH);
  digitalWrite(PIN_BUZZER, HIGH);
  digitalWrite(PIN_RST, LOW);
}
void stateRST(int pushButtonState){
  Serial.println("Silahkan tekan tombol RST untuk kembali ke kondisi awal alat :");
  while(pushButtonState != HIGH){
    if(var == 1 || var == 2){
      errorInstalation_1();
    }else if (var == 3){
      errorInstalation_2();
    }else if (var == 4){
      errorInstalation_3();
    }
  }
  var = var_rst;
  kondisiAwalSistem();
  Serial.println("Sistem telah di reset ke posisi awal");
  client.publish(topic_pub5, "Sistem Telah di Riset");
}
void instalationChecking(float current, float voltage, float suhu1, float suhu2){
  if(current > batasOverloadarus && voltage <= batasDropvoltage){
    var = 1;
    Serial.print("Nilai Arus Fasa :"); Serial.print(current); Serial.println("A");
    Serial.print("Nilai Tegangan :"); Serial.print(voltage); Serial.println("V");
    Serial.println("Terdapat Korsleting Listrik pada instalasi listrik anda");
    delay(10000);
    if(current != 0){
      Serial.println("Sistem Error Instalation_1 diaktifkan");
      errorInstalation_1();
      delay(1000);
      pushButtonState = digitalRead(PIN_RST);
      stateRST(pushButtonState);
    }
  }else if(current > batasOverloadarus){
    var = 2;
    Serial.print("Nilai Arus Fasa :"); Serial.print(current); Serial.println("A");
    Serial.println("Terdapat Overload Arus Listrik pada instalasi listrik anda");
    delay(10000);
    if(current != 0){
      Serial.println("Sistem Error Instalation_1 diaktifkan");
      errorInstalation_1();
      delay(1000);
      pushButtonState = digitalRead(PIN_RST);
      stateRST(pushButtonState);
    }
  }else if(suhu1 > batasSuhu1){
    var = 3;
    Serial.print("Nilai suhu penghantar pada ruangan 1 :"); Serial.print(suhu1); Serial.println("C");
    Serial.println("Terdapat peningkatan suhu pada ruangan 1");
    Serial.println("Sistem Error Instalation_2 diaktifkan");
    errorInstalation_2();
    delay(1000);
    pushButtonState = digitalRead(PIN_RST);
    stateRST(pushButtonState);
  }else if(suhu2 > batasSuhu2){
    var = 4;
    Serial.print("Nilai suhu penghantar pada ruangan 2 :"); Serial.print(suhu2); Serial.println("C");
    Serial.println("Terdapat peningkatan suhu pada ruangan 2");
    Serial.println("Sistem Error Instalation_3 diaktifkan");
    errorInstalation_3();
    delay(1000);
    pushButtonState = digitalRead(PIN_RST);
    stateRST(pushButtonState);
  }else{
    kondisiAwalSistem();
  }
}