#include <WiFi.h>
//*********DHT22, library****************************************
#include "DHTesp.h"
#define lampuM 4
#define lampuK 0
#define lampuH 2
#define pump 27
const char* ssid = "Wokwi-GUEST";
const char* password = "";

const char* mqtt_username = "avisha";
const char* mqtt_password = "12345678";



//*********Ultrasonics Sensor, deklarasi pin**********************************
const int trigPin = 5;
const int echoPin = 18;

//define sound speed in cm/uS
#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701

long duration;
int distanceCm,distanceCM;


String data_topic;
String stMessage;

bool Switch= false;
bool Manual = false;


void wifiConnect() {
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("Terhubung ke WiFi");
  Serial.println("Alamat IP: ");
  Serial.println(WiFi.localIP());
}
WiFiClient espClient;

#include "PubSubClient.h"
PubSubClient client(espClient);
const char* mqttServer = "broker.emqx.io";
int port = 1883;
char clientId[] = "MDN_ENG_PP#00110022024adfasdsfad&&%%$$##";
char topic_switch[] = "mqtt/MDN_ENG^782112321hj9179rw54rwrw2712hjshj/switch";
char topic_manual[] = "mqtt/MDN_ENG^782112321hj9179rw54rwrw2712hjshj/manual";
char topic_distanceCM[] = "mqtt/MDN_ENG^782112321hj9179rw54rwrw2712hjshj54rwrw2712hjshjasdfgjkl/distanceCM";
char topic_RedLamp[] = "mqtt/MDN_ENG^782112321hj9179rw54rwrw2712hjshj54rwrw2712hjshjasdfgjkl/redLampState";
char topic_OrangeLamp[] = "mqtt/MDN_ENG^782112321hj9179rw54rwrw2712hjshj54rwrw2712hjshjasdfgjkl/orangeLampState";
char topic_GreenLamp[] = "mqtt/MDN_ENG^782112321hj9179rw54rwrw2712hjshj54rwrw2712hjshjasdfgjkl/greenLampState";
char topic_pumpstatus[] = "mqtt/MDN_ENG^782112321hj9179rw54rwrw2712hjshj54rwrw2712hjshjasdfgjkl/pumpState";

#define MSG_BUFFER_SIZE (50)
char msg[MSG_BUFFER_SIZE];

void mqttReconnect() {
  while (!client.connected()) {
    Serial.print("Menghubungkan ke MQTT Server... ");
    if (client.connect(clientId, mqtt_username, mqtt_password)) {
      Serial.print(clientId);
      Serial.println(" Terhubung");
      client.subscribe(topic_switch);
      client.subscribe(topic_manual);
    } else {
      Serial.print("Gagal Terhubung, stataus=");
      Serial.print(client.state());
      Serial.println(" tunggu 2 detik");
      delay(2000);
    }
  }
}

void callback(char* topic, byte* message, unsigned int length) {
  Serial.print("Pesan Baru, Topik: ");
  Serial.print(topic);
  Serial.print(" Pesan: ");
  data_topic = topic;
  stMessage;
  for (int i = 0; i < length; i++) {
    stMessage += (char)message[i];
  }
  Serial.print(data_topic);
  Serial.println(stMessage);

  

  if(data_topic == topic_switch)
  {
    if(stMessage == "1")
    {
      Switch = true;
    }
    else
    {
      Switch = false;
      stMessage="";
    }
    
  }
  else if(data_topic == topic_manual)
  {
    if(stMessage == "1")
    {
      Manual = true;
    }
    else
    {
      Manual = false;
      stMessage="";
    }
  }

}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  pinMode(lampuM, OUTPUT);
  pinMode(lampuK, OUTPUT);
  pinMode(lampuH, OUTPUT);
  pinMode(pump,OUTPUT);
  wifiConnect();
  client.setServer(mqttServer, port);
  client.setCallback(callback);
 
  
}
unsigned const long jeda = 1000;
unsigned long zero = 0;

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  
  // Calculate the distance
  distanceCm = duration * SOUND_SPEED/2;
  distanceCM = map(distanceCm, 0,400,400,0);

  if (!client.connected()) {
    mqttReconnect();
  }
  client.loop();
  if(millis() - zero >= jeda){

    Serial.println("Mqtt Publish..." +String(distanceCM)+"   "+String(Switch));
    snprintf (msg, MSG_BUFFER_SIZE, "%ld", distanceCM);
    client.publish(topic_distanceCM, msg);
    
    if(Switch==0)
    {
      if(distanceCM > 398)
      {
        digitalWrite(lampuH, HIGH);
        digitalWrite(lampuK, LOW);
        digitalWrite(lampuM, LOW);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_GreenLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_OrangeLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_RedLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", Manual);
        client.publish(topic_pumpstatus, msg);
      }
      else if(distanceCM > 200 && distanceCM <= 399)
      {
        digitalWrite(lampuH, HIGH);
        digitalWrite(lampuK, LOW);
        digitalWrite(lampuM, LOW);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_GreenLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_OrangeLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_RedLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", Manual);
        client.publish(topic_pumpstatus, msg);
      }
      else if(distanceCM > 5 && distanceCM <= 200)
      {
        digitalWrite(lampuH, LOW);
        digitalWrite(lampuK, HIGH);
        digitalWrite(lampuM, LOW);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_GreenLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_OrangeLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_RedLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", Manual);
        client.publish(topic_pumpstatus, msg);
      }
      else if(distanceCM < 5)
      {
        digitalWrite(lampuH, LOW);
        digitalWrite(lampuK, LOW);
        digitalWrite(lampuM, HIGH);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_GreenLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_OrangeLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_RedLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", Manual);
        client.publish(topic_pumpstatus, msg);
      }

      if(Manual == 1)
      {
        digitalWrite(pump, HIGH);
      }
      else
      {
        digitalWrite(pump, LOW);
      }
    
    }
    else
    {
      if(distanceCM > 398)
      {
        digitalWrite(lampuH, HIGH);
        digitalWrite(lampuK, LOW);
        digitalWrite(lampuM, LOW);
        digitalWrite(pump, LOW);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_GreenLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_OrangeLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_RedLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_pumpstatus, msg);

      }
      else if(distanceCM > 200 && distanceCM <= 399)
      {
        digitalWrite(lampuH, HIGH);
        digitalWrite(lampuK, LOW);
        digitalWrite(lampuM, LOW);
        digitalWrite(pump, LOW);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_GreenLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_OrangeLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_RedLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_pumpstatus, msg);
      }
      else if(distanceCM > 5 && distanceCM <= 200)
      {
        digitalWrite(lampuH, LOW);
        digitalWrite(lampuK, HIGH);
        digitalWrite(lampuM, LOW);
        digitalWrite(pump, LOW);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_GreenLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_OrangeLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_RedLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_pumpstatus, msg);
      }
      else if(distanceCM < 5)
      {
        digitalWrite(lampuH, LOW);
        digitalWrite(lampuK, LOW);
        digitalWrite(lampuM, HIGH);
        digitalWrite(pump, HIGH);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_GreenLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 0);
        client.publish(topic_OrangeLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_RedLamp, msg);
        snprintf (msg, MSG_BUFFER_SIZE, "%ld", 1);
        client.publish(topic_pumpstatus, msg);
      }
    }
    zero = millis();
  }
}