#include <WiFi.h>
#include <PubSubClient.h>

#define TRIGGER_PIN 12
#define ECHO_PIN 14

void callback(char* subscribetopic, byte* payload, unsigned int payloadLength);

#define ORG "17athy"
#define DEVICE_TYPE "Britto"  
#define DEVICE_ID "hifi_britto"
#define TOKEN "12345678"

char server[] = ORG ".messaging.internetofthings.ibmcloud.com";
char publishTopic[] = "iot-2/evt/Data/fmt/json";
char subscribetopic[] = "iot-2/cmd/command/fmt/String";
char authMethod[] = "use-token-auth";
char token[] = TOKEN;
char clientId[] = "d:" ORG ":" DEVICE_TYPE ":" DEVICE_ID;

WiFiClient wifiClient;
PubSubClient client(server, 1883, callback, wifiClient);

void setup()
{
  Serial.begin(115200);
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  delay(10);
  Serial.println();
  wificonnect();
  mqttconnect();
}

void loop()
{
  if (!client.connected()) {
    mqttconnect();
  }
  client.loop();

  int distance = calculateDistance();
  if (distance < 20) {
    PublishData(20);
    delay(2000);
  }
}

void PublishData(int distance)
{
  String payload = "{\"distance\":";
  payload += distance;
  payload += "}";

  Serial.print("Sending payload: ");
  Serial.println(payload);

  if (client.publish(publishTopic, (char*)payload.c_str())) {
    Serial.println("Publish ok");
  } else {
    Serial.println("Publish failed");
  }
}

void mqttconnect()
{
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    if (client.connect(clientId, authMethod, token)) {
      Serial.println("connected");
      initManagedDevice();
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" retrying in 5 seconds");
      delay(5000);
    }
  }
}

void wificonnect()
{
  Serial.print("Connecting to WiFi...");
  WiFi.begin("Wokwi-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

void initManagedDevice()
{
  if (client.subscribe(subscribetopic)) {
    Serial.println(subscribetopic);
    Serial.println("Subscribe to cmd OK");
  }
}

int calculateDistance()
{
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  int distance = duration * 0.034 / 2;

  return distance;
}

void callback(char* subscribetopic,byte* payload, unsigned int payloadLength)
{
String data = "";
for (int i = 0; i < payloadLength; i++) {
data += (char)payload[i];
}
Serial.print("Callback invoked for topic: ");
Serial.println(subscribetopic);
Serial.println("Data: " + data);
}