#include <ESP8266WiFi.h>
#include <PubSubClient.h>

//Eigene zu trackende Entfernung festlegen
#define DISTANCE 15

const char* SSID = "smartroom";
const char* PSK = "smarthome";
const char* MQTT_BROKER = "192.168.4.1";
WiFiClient espClient;
PubSubClient client(espClient);

// defines pins numbers
const int trigPin = 2;  //D4
const int echoPin = 0;  //D3

long duration;
int distance;
int bool1 = 0;
char msg[50];
int value = 0;
long lastMsg = 0;

void setup() {
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input

  Serial.begin(115200);
  setup_wifi();
  client.setServer(MQTT_BROKER, 1883);
}

void setup_wifi() {
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(SSID);
  WiFi.mode(WIFI_STA);
  WiFi.begin(SSID, PSK);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

void reconnect() {
  while (!client.connected()) {
    Serial.print("Reconnecting...");
    if (!client.connect("ESP8266Client")) {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" retrying in 5 seconds");
      delay(5000);
    }
  }
}


void loop() {

  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  // Clears the trigPin
  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);

  // Calculating the distance
  distance = duration * 0.034 / 2;
  // Prints the distance on the Serial Monitor
 /* if ((distance != (distance_alt - 1)) && (distance != (distance_alt)) && (distance != (distance_alt + 1))) { //+-1 um störungen herauszufiltern
    snprintf (msg, 50, "%d", distance);
    Serial.print("Publish Motion: ");
    Serial.println(msg);
    client.publish("/home/data", msg);
    delay(200);
  } */
  if (bool1 == 0) {
    //alternativ: if ((distance != (DISTANCE - 1)) && (distance != (DISTANCE)) && (distance != (DISTANCE + 1))) { //+-1
    if (((DISTANCE + 2) < distance) || ((DISTANCE - 2) > distance)) { //darf +- 2 um festgelegte entfernung schwanken, um störungen herauszufiltern
      //Meldung an PI, dass die Distanz gestört ist
      snprintf (msg, 50, "%d", 1);
      client.publish("/home/data", msg);
      //Serieller Monitor
      Serial.print("Motion detected! Distance: ");
      Serial.println(msg);

      //Flag auf 1
      bool1 = 1;

    }
  }
  else if (bool1 == 1) {
    if (((DISTANCE + 2) < distance) || ((DISTANCE - 2) > distance)) { //darf +- 2 um festgelegte entfernung schwanken, um störungen herauszufiltern
      Serial.print("Still motion detected! Distance: ");
      Serial.println(distance);
    }
    else {
      //Meldung an PI, dass die Ausgangsdistanz wieder gemessen wird
      snprintf (msg, 50, "%d", 0);
      client.publish("/home/data", msg);
      
      //Flag wieder auf 0
      bool1 = 0;

    }
  }
  delay(100);
}