#include <PZEM004Tv30.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <DHT.h>

#if !defined(PZEM_RX_PIN) && !defined(PZEM_TX_PIN)
#define PZEM_RX_PIN 16
#define PZEM_TX_PIN 17
#endif

#if !defined(PZEM_SERIAL)
#define PZEM_SERIAL Serial2
#endif

#define NUM_PZEMS 3

PZEM004Tv30 pzems[NUM_PZEMS];

#if defined(USE_SOFTWARE_SERIAL) && defined(ESP32)
#error "Can not use SoftwareSerial with ESP32"
#elif defined(USE_SOFTWARE_SERIAL)

SoftwareSerial pzemSWSerial(PZEM_RX_PIN, PZEM_TX_PIN);
#endif

#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 64  // OLED display height, in pixels
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

unsigned long DataReadloop = millis();
long DataReadloopint = 250;

unsigned long RPhaseloop = millis();
unsigned long YPhaseloop = millis();
unsigned long BPhaseloop = millis();
long Phaseloopint = 300;

unsigned long Temploop = millis();
long Temploopint = 2000;

unsigned long vr1currenttime = millis();
unsigned long vr2currenttime = millis();
unsigned long vr1previoustime = millis();
unsigned long vr2previoustime = millis();

unsigned long vy1currenttime = millis();
unsigned long vy2currenttime = millis();
unsigned long vy1previoustime = millis();
unsigned long vy2previoustime = millis();

unsigned long vb1currenttime = millis();
unsigned long vb2currenttime = millis();
unsigned long vb1previoustime = millis();
unsigned long vb2previoustime = millis();

unsigned long Ir1currenttime = millis();
unsigned long Ir1previoustime = millis();
unsigned long Iy1currenttime = millis();
unsigned long Iy1previoustime = millis();
unsigned long Ib1currenttime = millis();
unsigned long Ib1previoustime = millis();

unsigned long Buzzerloop = millis();
long Buzzerint = 5000;

long ChangeScreen = 0;
unsigned long ScreenLastTime = 0;
long ScreenSelect = 0;

#define wifiLed 2

#define UnderVoltPot 36
#define OverVoltPot 39
#define VdelaytimePot 34
#define OCSPot 35

#define VRK 18
#define VYK 19
#define VBK 23
#define IRK 25
#define IYK 26
#define IBK 27
#define MainsRelay 14

#define Bypass 32
#define KBypass 15
#define Buzzer 12
#define Faultled 4
#define OKled 5
#define Fan 13
#define DHTPIN 33

float vrout;
float vyout;
float vbout;
float CTRVal;
float CTYVal;
float CTBVal;
float RPower;
float YPower;
float BPower;

int VTimeloopR1 = 0;
int VTimeloopR2 = 0;
int VTimeloopY1 = 0;
int VTimeloopY2 = 0;
int VTimeloopB1 = 0;
int VTimeloopB2 = 0;

int VsecR1 = 0;
int VsecR2 = 0;
int VsecY1 = 0;
int VsecY2 = 0;
int VsecB1 = 0;
int VsecB2 = 0;

int UnderVoltPotVal;
int UnderVolt;
int OverVoltPotVal;
int OverVolt;
int VdelaytimePotVal;
int delaytime;
int Vdelaytime;
int OCS;  //Over current setting
int OCSPotVal;
int Idelaytime = 1000; //Over current trip time delay
int LoopItr;

int ITimeloopR1 = 0;
int ITimeloopY1 = 0;
int ITimeloopB1 = 0;
int IsecR1 = 0;
int IsecY1 = 0;
int IsecB1 = 0;

int tripstart;
int triped;
int tripduration;

bool VRKState = LOW;
bool VYKState = LOW;
bool VBKState = LOW;
bool IRKState = LOW;
bool IYKState = LOW;
bool IBKState = LOW;
bool MainsRelayState = LOW;

bool WifiState = LOW;
bool BypassState = LOW;
bool KBypassState = LOW;
bool BuzzerState = LOW;
bool FanState = LOW;

#define DHTTYPE DHT22  // DHT 22, AM2302, AM2321
int Temperature = 0;
int Humidity = 0;
DHT dht(DHTPIN, DHTTYPE);



void setup() {
  Serial.begin(115200);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  // For each PZEM, initialize it
  for (int i = 0; i < NUM_PZEMS; i++) {

#if defined(USE_SOFTWARE_SERIAL)
    // Initialize the PZEMs with Software Serial
    pzems[i] = PZEM004Tv30(pzemSWSerial, 0x10 + i);
#elif defined(ESP32)
    // Initialize the PZEMs with Hardware Serial2 on RX/TX pins 16 and 17
    pzems[i] = PZEM004Tv30(PZEM_SERIAL, PZEM_RX_PIN, PZEM_TX_PIN, 0x10 + i);
#else
    // Initialize the PZEMs with Hardware Serial2 on the default pins

    /* Hardware Serial2 is only available on certain boards.
       For example the Arduino MEGA 2560
    */
    pzems[i] = PZEM004Tv30(PZEM_SERIAL, 0x10 + i);
#endif
  }

  dht.begin();
  // Clear the buffer
  display.clearDisplay();
  // Display Text
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 28);
  display.println("Hello... Hashva!");
  display.display();

  pinMode(UnderVoltPot, INPUT);
  pinMode(OverVoltPot, INPUT);
  pinMode(VdelaytimePot, INPUT);
  pinMode(OCSPot, INPUT);
  pinMode(DHTPIN, INPUT);
  pinMode(Bypass, INPUT_PULLUP);

  pinMode(wifiLed, OUTPUT);

  pinMode(MainsRelay, OUTPUT);
  pinMode(VRK, OUTPUT);
  pinMode(VYK, OUTPUT);
  pinMode(VBK, OUTPUT);
  pinMode(IRK, OUTPUT);
  pinMode(IYK, OUTPUT);
  pinMode(IBK, OUTPUT);

  //During Starting all Relays should TURN OFF
  digitalWrite(VRK, LOW);
  digitalWrite(VYK, LOW);
  digitalWrite(VBK, LOW);
  digitalWrite(MainsRelay, LOW);
  digitalWrite(IRK, LOW);
  digitalWrite(IYK, LOW);
  digitalWrite(IBK, LOW);

  pinMode(KBypass, OUTPUT);
  pinMode(Faultled, OUTPUT);
  pinMode(OKled, OUTPUT);
  pinMode(Buzzer, OUTPUT);
  pinMode(Fan, OUTPUT);

  display.clearDisplay();
  delay(1000);
}

void loop() {

  unsigned long CurrentTime = millis();

  // Data Read loop PZEM sensor, UV, OV, Timedelay, OCS pots and Bypass switch
  if (CurrentTime - DataReadloop > DataReadloopint) {
    DataReadloop = CurrentTime;

    vrout = pzems[0].voltage();
    CTRVal = pzems[0].current();
    RPower = pzems[0].power();



    vyout = pzems[1].voltage();
    CTYVal = pzems[1].current();
    YPower = pzems[1].power();



    vbout = pzems[2].voltage();
    CTBVal = pzems[2].current();
    BPower = pzems[2].power();



    // POT Value for UV, OV, VT, and OCS
    UnderVoltPotVal = analogRead(UnderVoltPot);
    UnderVolt = map(UnderVoltPotVal, 0, 4095, 80, 200);

    OverVoltPotVal = analogRead(OverVoltPot);
    OverVolt = map(OverVoltPotVal, 0, 4095, 240, 270);

    LoopItr = analogRead(VdelaytimePot);
    delaytime = map(LoopItr, 0, 4095, 0, 10000);
    Vdelaytime = (delaytime + Phaseloopint) / Phaseloopint;
    Serial.print("Vdelaytime: ");
    Serial.println(Vdelaytime);

    OCSPotVal = analogRead(OCSPot);
    OCS = map(OCSPotVal, 0, 4095, 1, 63);

    // Bypass Switch
    if (digitalRead(Bypass) == LOW && BypassState == LOW) {
      digitalWrite(KBypass, HIGH);
      BypassState = HIGH;
      KBypassState = HIGH;

    }
    if (digitalRead(Bypass) == HIGH && BypassState == HIGH) {
      digitalWrite(KBypass, LOW);
      BypassState = LOW;
      KBypassState = LOW;
      tripstart = millis();

    }

    //Fault and OK led Indication
    if (vrout > UnderVolt && vrout < OverVolt && vyout > UnderVolt && vyout < OverVolt && vbout > UnderVolt && vbout < OverVolt && IRKState == 0 && IYKState == 0 && IBKState == 0) {

      digitalWrite(Faultled, LOW);
      digitalWrite(OKled, HIGH);

    }
    if (vrout < UnderVolt || vrout > OverVolt || vyout < UnderVolt || vyout > OverVolt || vbout < UnderVolt || vbout > OverVolt || IRKState == 1 || IYKState == 1 || IBKState == 1 || isnan(vrout) || isnan(vyout) || isnan(vbout)) {

      digitalWrite(Faultled, HIGH);
      digitalWrite(OKled, LOW);

    }

  }

  // R Phase Loop
  if (CurrentTime - RPhaseloop > Phaseloopint) {
    RPhaseloop = CurrentTime;


    if (((vrout < UnderVolt) || (vrout > OverVolt) || isnan(vrout)) && BypassState == LOW) {
      vr1currenttime = CurrentTime;
      
    }

    if (((vrout >= UnderVolt) && (vrout <= OverVolt)) || BypassState == HIGH) {
      vr2currenttime = CurrentTime;
      VTimeloopR1 = 0;
    }

    // R Phase Ondelay Timer for VRK Relay pickup
    if (((vrout < UnderVolt) || (vrout > OverVolt) || isnan(vrout)) && BypassState == LOW && VTimeloopR1 == 0 && digitalRead(VRK) == LOW) {
      VTimeloopR1 = 1;
      VsecR1 = Vdelaytime;
    }
    if (VTimeloopR1 == 1) {
      if ((vr1currenttime - vr1previoustime) > 1) {
        VsecR1--;
        vr1previoustime = vr1currenttime;

        if (VsecR1 <= 0) {
          VTimeloopR1 = 0;
          digitalWrite(VRK, HIGH);

          VRKState = HIGH;
          VsecR1 = 0;
          triped = millis();
          tripduration = triped - tripstart;
          Serial.print("Trip Duration: ");
          Serial.println(tripduration);
        }
      }
    }
    // R Phase Ondelay Timer for VRK Relay to restore
    if (((vrout >= UnderVolt) && (vrout <= OverVolt) || BypassState == HIGH) && VTimeloopR2 == 0 && digitalRead(VRK) == HIGH) {
      VTimeloopR2 = 1;
      VsecR2 = Vdelaytime;
    }

    if (VTimeloopR2 == 1) {
      if ((vr2currenttime - vr2previoustime) > 1) {
        VsecR2--;
        vr2previoustime = vr2currenttime;

        if (VsecR2 <= 0) {
          VTimeloopR2 = 0;
          digitalWrite(VRK, LOW);
          VRKState = LOW;
          VsecR2 = 0;

        }
      }
    }

    // R Phase loop for Current
    if (CTRVal > OCS) {
      Ir1currenttime = CurrentTime;
    }

    if (CTRVal < OCS) {
      ITimeloopR1 = 0;
    }
    // R Phase Ondelay Timer for IRK Relay ON/OFF
    if (CTRVal > OCS && ITimeloopR1 == 0 && IRKState == LOW) {
      ITimeloopR1 = 1;
      IsecR1 = Idelaytime;
    }
    if (ITimeloopR1 == 1) {
      if ((Ir1currenttime - Ir1previoustime) > 1) {
        IsecR1--;
        Ir1previoustime = Ir1currenttime;
        if (IsecR1 <= 0) {
          ITimeloopR1 = 0;
          digitalWrite(IRK, HIGH);
          IRKState = HIGH;
          IsecR1 = 0;

        }
      }
    }
    if (CTRVal < OCS) {
      digitalWrite(IRK, LOW);
      IRKState = LOW;

    }

  }

  // Y Phase Loop
  if (CurrentTime - YPhaseloop > Phaseloopint) {
    YPhaseloop = CurrentTime;


    if (((vyout < UnderVolt) || (vyout > OverVolt) || isnan(vyout)) && BypassState == LOW) {
      vy1currenttime = CurrentTime;
    }

    if (((vyout >= UnderVolt) && (vyout <= OverVolt)) || BypassState == HIGH) {
      vy2currenttime = CurrentTime;
      VTimeloopY1 = 0;
    }

    // Y Phase Ondelay Timer for VYK Relay pickup
    if (((vyout < UnderVolt) || (vyout > OverVolt) || isnan(vyout)) && BypassState == LOW && VTimeloopY1 == 0 && digitalRead(VYK) == LOW) {
      VTimeloopY1 = 1;
      VsecY1 = Vdelaytime;
    }
    if (VTimeloopY1 == 1) {
      if ((vy1currenttime - vy1previoustime) > 1) {
        VsecY1--;
        vy1previoustime = vy1currenttime;

        if (VsecY1 <= 0) {
          VTimeloopY1 = 0;
          digitalWrite(VYK, HIGH);
          VYKState = HIGH;
          VsecY1 = 0;

        }
      }
    }
    // Y Phase Ondelay Timer for VRK Relay to restore
    if (((vyout >= UnderVolt) && (vyout <= OverVolt) || BypassState == HIGH) && VTimeloopY2 == 0 && digitalRead(VYK) == HIGH) {
      VTimeloopY2 = 1;
      VsecY2 = Vdelaytime;
    }

    if (VTimeloopY2 == 1) {
      if ((vy2currenttime - vy2previoustime) > 1) {
        VsecY2--;
        vy2previoustime = vy2currenttime;

        if (VsecY2 <= 0) {
          VTimeloopY2 = 0;
          digitalWrite(VYK, LOW);
          VYKState = LOW;
          VsecY2 = 0;

        }
      }
    }

    // Y Phase loop for Current
    if (CTYVal > OCS) {
      Iy1currenttime = CurrentTime;
    }

    if (CTYVal < OCS) {
      ITimeloopY1 = 0;
    }
    // Y Phase Ondelay Timer for IYK Relay ON/OFF
    if (CTYVal > OCS && ITimeloopY1 == 0 && IYKState == LOW) {
      ITimeloopY1 = 1;
      IsecY1 = Idelaytime;
    }
    if (ITimeloopY1 == 1) {
      if ((Iy1currenttime - Iy1previoustime) > 1) {
        IsecY1--;
        Iy1previoustime = Iy1currenttime;
        if (IsecY1 <= 0) {
          ITimeloopY1 = 0;
          digitalWrite(IYK, HIGH);
          IYKState = HIGH;
          IsecY1 = 0;

        }
      }
    }
    if (CTYVal < OCS) {
      digitalWrite(IYK, LOW);
      IYKState = LOW;

    }


  }

  // B Phase Loop
  if (CurrentTime - BPhaseloop > Phaseloopint) {
    BPhaseloop = CurrentTime;


    if (((vbout < UnderVolt) || (vbout > OverVolt) || isnan(vbout)) && BypassState == LOW) {
      vb1currenttime = CurrentTime;
    }

    if (((vbout >= UnderVolt) && (vbout <= OverVolt)) || BypassState == HIGH) {
      vb2currenttime = CurrentTime;
      VTimeloopB1 = 0;
    }

    // B Phase Ondelay Timer for VBK Relay pickup
    if (((vbout < UnderVolt) || (vbout > OverVolt) || isnan(vbout)) && BypassState == LOW && VTimeloopB1 == 0 && digitalRead(VBK) == LOW) {
      VTimeloopB1 = 1;
      VsecB1 = Vdelaytime;
    }
    if (VTimeloopB1 == 1) {
      if ((vb1currenttime - vb1previoustime) > 1) {
        VsecB1--;
        vb1previoustime = vb1currenttime;

        if (VsecB1 <= 0) {
          VTimeloopB1 = 0;
          digitalWrite(VBK, HIGH);
          VBKState = HIGH;
          VsecB1 = 0;

        }
      }
    }
    // B Phase Ondelay Timer for VBK Relay to restore
    if (((vbout >= UnderVolt) && (vbout <= OverVolt) || BypassState == HIGH) && VTimeloopB2 == 0 && digitalRead(VBK) == HIGH) {
      VTimeloopB2 = 1;
      VsecB2 = Vdelaytime;
    }

    if (VTimeloopB2 == 1) {
      if ((vb2currenttime - vb2previoustime) > 1) {
        VsecB2--;
        vb2previoustime = vb2currenttime;

        if (VsecB2 <= 0) {
          VTimeloopB2 = 0;
          digitalWrite(VBK, LOW);
          VBKState = LOW;
          VsecB2 = 0;

        }
      }
    }

    // B Phase loop for Current
    if (CTBVal > OCS) {
      Ib1currenttime = CurrentTime;
    }

    if (CTBVal < OCS) {
      ITimeloopB1 = 0;
    }
    // B Phase Ondelay Timer for IBK Relay ON/OFF
    if (CTBVal > OCS && ITimeloopB1 == 0 && IBKState == LOW) {
      ITimeloopB1 = 1;
      IsecB1 = Idelaytime;
    }
    if (ITimeloopB1 == 1) {
      if ((Ib1currenttime - Ib1previoustime) > 1) {
        IsecB1--;
        Ib1previoustime = Ib1currenttime;
        if (IsecB1 <= 0) {
          ITimeloopB1 = 0;
          digitalWrite(IBK, HIGH);
          IBKState = HIGH;
          IsecB1 = 0;

        }
      }
    }
    if (CTBVal < OCS) {
      digitalWrite(IBK, LOW);
      IBKState = LOW;

    }


  }

  // Buzzer
  if (CurrentTime - Buzzerloop > Buzzerint) {
    Buzzerloop = CurrentTime;

    if (vrout < UnderVolt || vrout > OverVolt || vyout < UnderVolt || vyout > OverVolt || vbout < UnderVolt || vbout > OverVolt || IRKState == 1 || IYKState == 1 || IBKState == 1 || isnan(vrout) || isnan(vyout) || isnan(vbout)) {

      if (BuzzerState == HIGH) {
        BuzzerState = LOW;
      }
      else {
        BuzzerState = HIGH;
      }
      digitalWrite(Buzzer, BuzzerState);

    }


  }

  // Panel Temperature control
  if (CurrentTime - Temploop > Temploopint) {
    Temploop = CurrentTime;


    float h = dht.readHumidity();
    float t = dht.readTemperature();  // or dht.readTemperature(true) for Fahrenheit

    if (isnan(h) || isnan(t)) {
      Serial.println("Failed to read from DHT sensor!");
      return;
    }
    else {
      Humidity = h;
      Temperature = t;

    }

    if (Temperature > 38) {
      digitalWrite(Fan, HIGH);
      FanState = HIGH;


    } else if (Temperature < 38) {
      digitalWrite(Fan, LOW);
      FanState = LOW;

    }

  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 5);
  display.println("Set:  UV   OV   OCS");
  display.setCursor(32, 17);
  display.println(UnderVolt);
  display.setCursor(65, 17);
  display.println( OverVolt);
  display.setCursor(101, 17);
  display.println(OCS);
  display.setCursor(0, 35);
  display.println("V.DelayT:  ");
  display.setCursor(54, 35);
  display.println(delaytime);
  display.setCursor(90, 35);
  display.println("~ms");
  display.setCursor(0, 55);
  display.println("I.DelayT:  ");
  display.setCursor(54, 55);
  display.println(Idelaytime);
  display.setCursor(90, 55);
  display.println("~ms");
  display.display();
  
  int loopduration = millis() - CurrentTime;
  Serial.print("Loop Duration: ");
          Serial.println(loopduration);
}