#define BLYNK_PRINT Serial
#define BLYNK_TEMPLATE_ID "TMPLdEZleqT_"
#define BLYNK_TEMPLATE_NAME "Quickstart Template"
#define BLYNK_AUTH_TOKEN "4_Q-jLWBTRqIjbbpGvLTb__lFuBQZPDu"
// kumpulan library yang dibutuhkan
#include <WiFi.h>               //library wifi
#include <WiFiClient.h>         //library wifi client
#include <BlynkSimpleEsp32.h>   //library blynk
#include <ESP32Servo.h>         //library servo
#include <PubSubClient.h>
//-------------------------------------------------


char ssid[] = "Wokwi-GUEST";        //ssid wifi
char pass[] = "";        //password wifi

const char* mqtt_server = "test.mosquitto.org";  //broker lain : "broker.hivemq.com";
WiFiClient espClient;
PubSubClient client(espClient);
unsigned long lastMsg = 0;
#define MSG_BUFFER_SIZE  (50)
char msg[MSG_BUFFER_SIZE];

//https://wokwi.com/projects/400317592385009665 ->link wokwi
//https://testclient-cloud.mqtt.cool/    -> Tes mqtt online

WidgetLCD lcdV(V4);   
Servo servoControl;         //deklarasi servo


int ledNotif = 12; //deklarasi led pada pin led default esp32 
int trigerPinSensor1 = 23;  //deklarasi pin triger sensor srf04  sensor jark manusia
int echoPinSensor1 = 21;    //deklarasi pin echo sensor srf04 sensor jark manusia
int trigerPinSensor2 = 19;   // pin triger srf04 sensor jarak untuk level sampah
int echoPinSensor2 = 18;    // pin echo srf04 sensor jarak untuk level sampah
int buzzer = 26;
int servoPin = 13;          //pin servo (gpio13)




long duration,distance;     
bool controlBuka = false;


int minimumDistance = 30;   //jarak minimum sensor deteksi orang (cm)
int maxVolumeDistance = 22; //volume sampah maksimal (cm)



int peopleDistance;         //variabel jarak orang
bool statusOpen=false;      //status tempat sampah (terbuka/tertutup)
int levelLimit = 80;        //batas level sampah (%)
int level=0;                //variabel level isi sampah




int openPosition = 0;      //set derajat servo posisi tutup sampah terbuka
int closePosition = 200;    //set derajat servo posisi tutup sampah tertutup
String idHardware = "TRASH001"; //id tempat sampah
String infoStatus = "";       //variabel informasi status tempat sampah


const int numReadings = 10;     //jumlah data untuk dirata-ratakan = 10
int readings[numReadings];      //variabel penampung baca data sensor
int readIndex = 0;              //variabel penampung 
int total = 0;                  //total data
int average = 0;                //rata-rata

unsigned long previousMillis = 0;         //variabel penampung millis/waktu 
const long interval = 5000;               // intervarl (second)
float volumeDistance = 0;


void setup()      //konfigurasi pin/library/dll
{
  Serial.begin(115200);             //set baudrate serial 115200
  Serial.println("Start program");  //menampilkan pesan start program
  ESP32PWM::allocateTimer(0);       //set timmer pwm servo
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);
  pinMode(trigerPinSensor1, OUTPUT);   //konfigurasi pin triger sensor srf04 sebagai output
  pinMode(echoPinSensor1, INPUT);      //konfigurasi pin echo sensor sebagai input
  pinMode(trigerPinSensor2, OUTPUT);    //konfigurasi pin triger sensor srf04 sebagai output
  pinMode(echoPinSensor2, INPUT);     //konfigurasi pin echo sensor sebagai input
  pinMode(ledNotif, OUTPUT);            //konfigurasi pin ledNotif sebagai output
  pinMode(buzzer, OUTPUT);            // konfigurasi pin buzzer sebagai output
  servoControl.setPeriodHertz(50);      //konfigurasi frekwensi servo
  servoControl.attach(servoPin, 1000, 2000);  //attach pin servo 
  servoControl.write(closePosition);    //set posisi awal servo dengan posisi tertutup
  Serial.println("Authentication");
  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);        //memulai blynk dengan autentikasi key blynk dan settingan wifi
  Serial.println("Connected");
  infoStatus=" Selamat Datang";

  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
    readings[thisReading] = 0;   // mengosongkan nilai variabel penampung rata-rata
  }

  client.setServer(mqtt_server, 1883);   //koneksi mqtt server
  client.setCallback(callback);
  
}


BLYNK_WRITE(V6) //menunggu perintah dari virtual pin blynk yang di kontrol dari aplikasi blynk
{
  int pinValue = param.asInt(); // menampung nilai dari virtual pin blynk 
  if (pinValue == 1){    // jika nilai==1 maka kontrol servo membuka 
    infoStatus="Status: Terbuka ";
     //servo terbuka
    for(int i=closePosition;i>openPosition; i--){
      servoControl.write(i);
      Serial.println(i);
      delay(5);
    }
    controlBuka = true;

  } else if (pinValue == 0) {   //jika nilai 0 kontrol servo menutup
    infoStatus="Status: Tertutup";
    //servoControl.write(closePosition);
    for(int i = openPosition; i<closePosition; i++){
      Serial.println(i);
      servoControl.write(i);
      delay(8);
    }
    controlBuka = false;

    
  }
  
  
}

void loop()   //perulangan
{
  unsigned long currentMillis = millis(); //menampung millis/waktu
  Blynk.run();
  float peopleDistance = distanceSensor(trigerPinSensor1, echoPinSensor1);  //membaca jarak orang dengan sensor srf04
  if(peopleDistance<minimumDistance){ //jika jarak orang lebih kecil dari jarak minimum
    previousMillis = currentMillis;
    if(statusOpen==false){
      infoStatus="Status: Terbuka ";    //status terbuka
      statusOpen=true;
      //servoControl.write(openPosition); //kontrol buka servo

      for(int i=closePosition;i>openPosition; i--){  //looping untuk menggerakan servo secara perlahan
        servoControl.write(i); //menggerakan servo dengan nilai i
        Serial.println(i);
        delay(5);
      }
    }
    
  }
  else{
    if(statusOpen==true){
      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        //servoControl.write(closePosition);
        for(int i = openPosition; i<closePosition; i++){ //looping untuk menutup servo
          Serial.println(i);
          servoControl.write(i);
          delay(8);
        }
        infoStatus="Status: Tertutup";
        statusOpen=false;
      }
      
    }
    
  }

  if(statusOpen==false){
    volumeDistance = distanceSensor(trigerPinSensor2, echoPinSensor2); //membaca jarak volume sampah
    if(volumeDistance>=maxVolumeDistance){   //jika jarak sampah lebih besar dari jarak maksimal
      volumeDistance = maxVolumeDistance-1;
    }
    volumeDistance = maxVolumeDistance-1-volumeDistance;
    level = map(volumeDistance,0, maxVolumeDistance, 0, 100);   //konversi jarak satuan cm ke dalam bentuk level % (0 s/d 100%)
    level = countAverage(level);  //hitung rata-rata sampahnya
    if(level>=levelLimit){          //jika persentase sampah lebih besar dari batas
      digitalWrite(ledNotif, HIGH); //hidupkan led   
      alarm();   
    }
    else{
      digitalWrite(ledNotif, LOW);
    }
  }
  
  Serial.println();
//  Serial.print("level(avg):");
//  Serial.println(average);
  Serial.print("Sensor Jarak Manusia:");
  Serial.println(peopleDistance);
  Serial.print("level (%):");
  Serial.println(level);
  Serial.print("level(cm):");
  Serial.println(volumeDistance);
  
  lcdV.print(0,0,infoStatus);
  if(!controlBuka){
    lcdV.print(0,1,"   Level : "+String(level)+"%  ");
  }
  else{
    lcdV.print(0,1,"Melalui Control!");
  }
  Blynk.virtualWrite(V5, level);
  if(Serial.available()>0){ //jika ada data serial
    
  }

  if (!client.connected()) {
    reconnect();
  }
  client.loop();
  unsigned long now = millis();
  if (now - lastMsg > 2000) {
    lastMsg = now;
    snprintf (msg, MSG_BUFFER_SIZE, "Level Sampah : %d", level);
    Serial.print("Publish message: ");
    Serial.println(msg);
    client.publish("tempatSampah001", msg);  //publish data mqtt pada topich tempatSampah001
  }

  
}


float distanceSensor(int trigerPin, int echoPin){   //fungsi untuk kalkulasi gelombang suara menjadi jarak
  digitalWrite(trigerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigerPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration * 0.0343)/2; //satuan cm, didapat dari konversi kecepatan gelombang ultrasonic menjadi jarak
  return distance;
}

int countAverage(int levelSampah){    //fungsi hitung rata-rata level sampah
  total = total - readings[readIndex];
  readings[readIndex] = levelSampah;
  total = total + readings[readIndex];
  readIndex = readIndex + 1;
  if (readIndex >= numReadings) {
    readIndex = 0;
  }
  average = total / numReadings;
  return average;
}


void alarm(){  //fungsi alarm
  digitalWrite(buzzer, HIGH);
  delay(100);
  digitalWrite(buzzer, LOW);
  delay(150);
}


void callback(char* topic, byte* payload, unsigned int length) {
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  for (int i = 0; i < length; i++) {
    Serial.print((char)payload[i]);
  }
  Serial.println();
  if ((char)payload[0] == '1') {
    //eksekusi callback jika payload 1
    
  } else {
    
  }

}

void reconnect() {
  // mencoba reconnect ke broker mqtt
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    String clientId = "ESP32Client-";
    clientId += String(random(0xffff), HEX);
    if (client.connect(clientId.c_str())) {
      Serial.println("connected");
      client.subscribe("inTopic");
      snprintf (msg, MSG_BUFFER_SIZE, "Level Sampah : %d", level);
      Serial.print("Publish message: ");
      Serial.println(msg);
      client.publish("tempatSampah001", msg);
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      //delay 5 second
      delay(5000);
    }
  }
}