#include <WiFi.h>
#include "ThingsBoard.h"
#include <Servo.h>

#define CURRENT_FIRMWARE_TITLE "TEST"
#define CURRENT_FIRMWARE_VERSION "1.0.0"

#define SSID "Wokwi-GUEST"
#define PASS ""

#define TOKEN "vjh4hzuNcIyGOf9BEK6V"
#define THINGSBOARD_SERVER "thingsboard.cloud"

#define SERIAL_DEBUG_BAUD 115200

#define ECHO_PIN 4
#define TRIG_PIN 2
#define pinBuzzer 5
int status = WL_IDLE_STATUS;

WiFiClient client;
ThingsBoard tb(client);

Servo myservo;  
  

float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

void reconnect(){
  status = WiFi.status();
  if(status!=WL_CONNECTED){
    WiFi.begin(SSID, PASS);
    while(WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.println(".");
    }
    Serial.println("Connected to AP");
  }
}

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  Serial.begin(115200);
  pinMode(pinBuzzer, OUTPUT);
  myservo.attach(13);  
  WiFi.disconnect();
  WiFi.begin("Wokwi-GUEST", "");
  while ((!(WiFi.status() == WL_CONNECTED))) {
    delay(300);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

void loop() {
   if(WiFi.status() != WL_CONNECTED){
    reconnect();
  }

  if(!tb.connected()) {
      Serial.println("Connecting to: ");
      Serial.print(THINGSBOARD_SERVER);
      Serial.print(" with token ");
      Serial.println(TOKEN);
      if(!tb.connect(THINGSBOARD_SERVER, TOKEN)){
        Serial.println("Failed to connect");
        return;
      }
  }
  Serial.println("Sending data...");

  float distance = readDistanceCM();

  Serial.print("Measured distance: ");
  Serial.println(distance);
  tb.sendTelemetryFloat("Jarak", distance);
  if(distance <= 200){    
    myservo.write(90);
    tb.sendTelemetryString("Servo", "Terbuka");
    delay(15);
   tone(pinBuzzer, 1000);
    delay(200);     
    tone(pinBuzzer, 500);
    delay(200);
   tb.sendTelemetryString("Buzzer", "Berbunyi");
  }
  else{
    myservo.write(0);
    noTone(pinBuzzer);
    tb.sendTelemetryString("Servo", "Terbuka"); 
    tb.sendTelemetryString("Buzzer", "Terbuka");
  } 
  tb.loop();                          
}