#include <WiFi.h>
#include <PubSubClient.h>
#include "Ultrasonic.h"
Ultrasonic ultrasonic(2, 4);
float distance;


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



#define ORG "pjo9g2"
#define DEVICE_TYPE "dustbin"
#define DEVICE_ID "12345"
#define TOKEN "123456789"
String data3;




char server[] = ORG ".messaging.internetofthings.ibmcloud.com";
char publishTopic[] = "iot-2/evt/Data/fmt/json";
char subscribetopic[] = "iot-2/cmd/test/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);


  delay(10);
  Serial.println();
  wificonnect();
  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, authMethod, token)) {
      Serial.print(".");
      delay(500);
    }

     initManagedDevice();
     Serial.println();
  }
}
void wificonnect()
{
  Serial.println();
  Serial.print("Connecting to ");
  
  WiFi.begin("wowki-GUEST", "", 6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.println("");
  }
  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");
 } else {
  Serial.println("subscribe to cd FAILED");
 }
}

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

  Serial.print("callback invoked for topic: ");
  Serial.println(subscribetopic);

  for (int i = 0; i < payloadLength; i++) {

    data3 += (char)payload[i];
  }

  Serial.println("data: "+ data3);

  data3="";


}