#include <WiFi.h> // library for wifi
#include <PubSubClient.h> // library for MQTT
#include "Ultrasonic.h"

Ultrasonic ultrasonic(2, 4);
float distance;
String data3;

#define ORG "ynes81"
#define DEVICE_TYPE "abcde"
#define DEVICE_ID "12345"
#define TOKEN "dXOWZMIB212obhhLJY"

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

WiFiClient wifiClient;
PubSubClient client(wifiClient);

void callback(char* topic, byte* payload, unsigned int payloadLength) {
  Serial.println("callback invoked for topic");
  Serial.println(topic);

  for (int i = 0; i < payloadLength; i++) {
    data3 += (char)payload[i];
  }
  Serial.println("data: " + data3);

  data3 = "";
}

void setup() {
  Serial.begin(115200);
  delay(10);
  Serial.println();
  wificonnect();
  client.setServer(server, 1883);
  client.setCallback(callback);
  mqttconnect();
}

void loop() {
  distance = ultrasonic.read(CM);
  Serial.print("Distance in CM: ");
  Serial.println(distance);
  delay(1000);
  publishData(distance);
  delay(1000);
  if (!client.loop()) {
    mqttconnect();
  }
}

void publishData(float distance) {
  mqttconnect();

  String payload = "{\"distance\":";
  payload += distance;
  payload += "}";

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

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

void mqttconnect() {
  if (!client.connected()) {
    Serial.print("Reconnecting client to ");
    Serial.println(server);
    while (!client.connect(clientId, token, "")) {
      Serial.print(".");
      delay(500);
    }
    initManagedDevice();
    Serial.println();
  }
}

void wificonnect() {
  Serial.println();
  Serial.print("Connecting to ");

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

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