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

TaskHandle_t Task1;
TaskHandle_t Task2;
TaskHandle_t Task3;
TaskHandle_t Task4;
TaskHandle_t Task5;
TaskHandle_t Task6;
TaskHandle_t Task7;
TaskHandle_t Task8;
TaskHandle_t Task9;
TaskHandle_t Task10;

#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);

#define wifiLed 2

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

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

#define Bypass 32
#define KBypass 15
#define Buzzer 14
#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 UnderVoltPotVal;
int UnderVolt;
int OverVoltPotVal;
int OverVolt;
int VdelaytimePotVal;
int delaytime;
int Vdelaytime;
int OCS;  //Over current setting 
int OCSPotVal;
int Idelaytime = 500; //Over current trip time delay
int LoopItr;

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

bool BypassState = LOW;
bool KBypassState = LOW;
bool FaultState = LOW;
bool FanState = LOW;

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

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

int dataloopduration;

//ESP Core Temperature
#ifdef __cplusplus
  extern "C" {
 #endif

  uint8_t temprature_sens_read();

#ifdef __cplusplus
}
#endif

uint8_t temprature_sens_read();

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(VRK, OUTPUT);
  pinMode(VYK, OUTPUT);
  pinMode(VBK, OUTPUT);
  pinMode(IRK, OUTPUT);
  pinMode(IYK, OUTPUT);
  pinMode(IBK, OUTPUT);
  pinMode(MainsRelay, OUTPUT);

digitalWrite(MainsRelay, HIGH);
  //During Starting all Relays should TURN OFF
  digitalWrite(VRK, LOW);
  digitalWrite(VYK, LOW);
  digitalWrite(VBK, 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);

  delay(1000);
  
  //create a task that will be executed in the Task1code() function, with priority 1 and executed on core 0/1
  //xTaskCreatePinnedToCore(Task function, "name of task", Stack size of task, parameter of the task-NULL, priority of the task-1, Task handle to keep track of created task, pin task to core 1/0)//
  xTaskCreatePinnedToCore(Task1code, "DataReadLoop", 10000, NULL, 1, &Task1, 1); 
   
  xTaskCreatePinnedToCore(Task2code, "RphaseLoop", 10000, NULL, 1, &Task2, 1);
   
  xTaskCreatePinnedToCore(Task3code, "YphaseLoop", 10000, NULL, 1, &Task3, 1);
   
  xTaskCreatePinnedToCore(Task4code, "BphaseLoop", 10000, NULL, 1, &Task4, 1);
  
  xTaskCreatePinnedToCore(Task5code, "RphaseCurrentLoop", 10000, NULL, 1, &Task5, 1);
   
  xTaskCreatePinnedToCore(Task6code, "YphaseCurrentLoop", 10000, NULL, 1, &Task6, 1);
  
  xTaskCreatePinnedToCore(Task7code, "BphaseCurrentLoop", 10000, NULL, 1, &Task7, 1);
  
  xTaskCreatePinnedToCore(Task8code, "FaultledLoop", 10000, NULL, 1, &Task8, 1);
  
  xTaskCreatePinnedToCore(Task9code, "TempLoop", 10000, NULL, 1, &Task9, 1);
  
  xTaskCreatePinnedToCore(Task10code, "OLEDLoop", 10000, NULL, 1, &Task10, 1);
  
  
}

//Task1code: DataReadLoop
void Task1code( void * pvParameters ){
  
  for(;;){
    
   unsigned long dataloopstart = millis();
 
   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);
  Vdelaytime = map(LoopItr, 0, 4095, 10, 10000);
  
  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;
  }
  
//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(OKled, HIGH);
    FaultState = LOW;
  }
  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(OKled, LOW);
    FaultState = HIGH;
  }
    delay(200);
    dataloopduration = millis() - dataloopstart;  
    Serial.print("DataloopDuration:");
    Serial.println(dataloopduration);
  } 
}

//Task2code: RphaseLoop
void Task2code( void * pvParameters ){

  for(;;){

   if (((vrout < UnderVolt) || (vrout > OverVolt) || isnan(vrout)) && BypassState == LOW && VRKState == LOW) {
   delay(Vdelaytime);
   digitalWrite(VRK, HIGH);
   VRKState = HIGH;
  }
  
  if (((vrout >= UnderVolt) && (vrout <= OverVolt) || BypassState == HIGH) && VRKState == HIGH) {
   delay(Vdelaytime);
   digitalWrite(VRK, LOW);
   VRKState = LOW;
  }
delay(10);
  }
}

//Task3code: YphaseLoop
void Task3code( void * pvParameters ){

  for(;;){
    
   if (((vyout < UnderVolt) || (vyout > OverVolt) || isnan(vyout)) && BypassState == LOW && digitalRead(VYK) == LOW) {
   delay(Vdelaytime);
   digitalWrite(VYK, HIGH);
   VYKState = HIGH;
  }
  
  if (((vyout >= UnderVolt) && (vyout <= OverVolt) || BypassState == HIGH) && digitalRead(VYK) == HIGH) {
   delay(Vdelaytime);
   digitalWrite(VYK, LOW);
   VYKState = LOW;
  }
delay(10);
  }
}

//Task4code: BphaseLoop
void Task4code( void * pvParameters ){
  
  for(;;){
    
   if (((vbout < UnderVolt) || (vbout > OverVolt) || isnan(vbout)) && BypassState == LOW && digitalRead(VBK) == LOW) {
   delay(Vdelaytime);
   digitalWrite(VBK, HIGH);
   VBKState = HIGH;
  }
  
  if (((vbout >= UnderVolt) && (vbout <= OverVolt) || BypassState == HIGH) && digitalRead(VBK) == HIGH) {
   delay(Vdelaytime);
   digitalWrite(VBK, LOW);
   VBKState = LOW;
  }
delay(10);
  }
}

//Task5code: Rphase Currentloop
void Task5code( void * pvParameters ){

  for(;;){
    
  if (CTRVal > OCS && IRKState == LOW) {
  delay(Idelaytime);
   digitalWrite(IRK, HIGH);
   IRKState = HIGH;
  }
  
  if (CTRVal < OCS && IRKState == HIGH) {
      digitalWrite(IRK, LOW);
      IRKState = LOW;
    }
delay(50);
  }
}

//Task6code: Yphase Currentloop
void Task6code( void * pvParameters ){

  for(;;){
    
  if (CTYVal > OCS && IYKState == LOW) {
  delay(Idelaytime);
   digitalWrite(IYK, HIGH);
   IYKState = HIGH;
  }
  
  if (CTYVal < OCS && IYKState == HIGH) {
      digitalWrite(IYK, LOW);
      IYKState = LOW;
    }
delay(50);
  }
}

//Task7code: Bphase Currentloop
void Task7code( void * pvParameters ){

  for(;;){
    
  if (CTBVal > OCS && IBKState == LOW) {
  delay(Idelaytime);
   digitalWrite(IBK, HIGH);
   IBKState = HIGH;
  }
  
  if (CTBVal < OCS && IBKState == HIGH) {
      digitalWrite(IBK, LOW);
      IBKState = LOW;
    }
delay(50);
  }
}

//Task8code: FaultledLoop
void Task8code( void * pvParameters ){

  for(;;){

  digitalWrite(Faultled, FaultState);
  delay(300);
  digitalWrite(Faultled, LOW);
  delay(300);

  }
}

//Task9code: Temperature CPU/Panel Loop
void Task9code( void * pvParameters ){

  for(;;){
    
  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;
    
  }

  CPUTemp = (temprature_sens_read() - 32) / 1.8;

  if (Temperature > 38 || CPUTemp > 40) {
    digitalWrite(Fan, HIGH);
    FanState = HIGH;
    
  } else if (Temperature < 38 || CPUTemp < 40) {
    digitalWrite(Fan, LOW);
    FanState = LOW;
    }
delay(2000);
  }
}

//Task10code: OLED Display
void Task10code( void * pvParameters ){

  for(;;){
    
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 5);
  display.println("BYP:");

  if(BypassState == HIGH){
    display.setCursor(35, 5);
    display.println("* Bypass ON *");
    display.display();
  }
  else{
    display.setCursor(35, 5);
    display.println(" Bypass OFF ");
    display.display();
  }

  display.setCursor(0, 22);
  display.println("CPU Temp:      *C");
  display.setCursor(55, 22);
  display.println(CPUTemp);

  display.setCursor(0, 37);
  display.println("Panel Temp:    *C");
  display.setCursor(67, 37);
  display.println(Temperature);
  display.setCursor(0, 55);
  display.println("Cooling Fan:");
  
  if(FanState == HIGH){
    display.setCursor(70, 55);
    display.println(" ON ");
    display.display();
  }
  else{
    display.setCursor(70, 55);
    display.println(" OFF ");
    display.display();
  }
  display.display();
  delay(1000);
  
  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(Vdelaytime + dataloopduration);
  display.setCursor(90, 35);
  display.println("~ms");
  display.setCursor(0, 55);
  display.println("I.DelayT:  ");
  display.setCursor(54, 55);
  display.println(Idelaytime + dataloopduration);
  display.setCursor(90, 55);
  display.println("~ms");
  display.display();
  delay(1000);
  
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.drawCircle(8, 15, 8, WHITE);
  display.drawCircle(8, 38, 8, WHITE);
  display.setCursor(6, 12);
  display.println("V");
  display.setCursor(6, 34);
  display.println("A");
  display.setCursor(0, 0);
  display.println("      R     Y     B");
  display.setCursor(29, 12);
  display.println(vrout);
  display.setCursor(65, 12);
  display.println(vyout);
  display.setCursor(101, 12);
  display.println(vbout);
  display.setCursor(29, 34);
  display.println(CTRVal);
  display.setCursor(65, 34);
  display.println(CTYVal);
  display.setCursor(101, 34);
  display.println(CTBVal);
  display.setCursor(0, 55);
  display.println("STS:");

  if(FaultState == LOW){
    display.setCursor(35, 55);
    display.println(" --Normal-- ");
    display.display();
  }
  else{
    display.setCursor(35, 55);
    display.println(" ** Fault ** ");
    display.display();
  }
  display.display();
  delay(1000);
  }
}

void loop() {
unsigned long CurrentTime = millis();

delay(2000);
digitalWrite(MainsRelay, LOW);
 // Buzzer
 if(CurrentTime - Buzzerloop > Buzzerint){
    Buzzerloop = CurrentTime;
    
      digitalWrite(Buzzer, FaultState);
      delay(500);
      digitalWrite(Buzzer, LOW);
 } 

}