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

//Eigene zu trackende Entfernung festlegen
#define DISTANCE 155

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

// defines pins numbers
const int trigPin = D3;  //D4
const int echoPin1 = D4;  //D3
const int echoPin2 = D2; //D2

long duration1;
long duration2;
int distance1;
int distance2;
int bool1 = 0;
int bool2 = 0;
char msg[50];
int value = 0;
long lastMsg = 0;

void setup() {
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin1, INPUT); // Sets the echoPin as an Input
  pinMode(echoPin2, 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
  duration1 = pulseIn(echoPin1, HIGH);
  duration2 = pulseIn(echoPin2, HIGH);
  
  // Calculating the distance
  distance1 = getDistance(duration1);
  distance2 = getDistance(duration2);
  // Prints the distance1 on the Serial Monitor
  /* if ((distance1!= (distance_alt - 1)) && (distance1!= (distance_alt)) && (distance1!= (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);
    } */
  bool1 = presenceDetection(bool1, distance1);
  bool2 = presenceDetection(bool2, distance2);

  delay(80);
}

int presenceDetection(int bool1, int distance1){
    
  if (bool1 == 0) {
    //alternativ: if ((distance1!= (DISTANCE - 1)) && (distance1!= (DISTANCE)) && (distance1!= (DISTANCE + 1))) { //+-1
    
    if (((distance1 > (DISTANCE + 2)) || (distance1 < (DISTANCE - 2))) && (distance1 < (DISTANCE + 2))) { //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("/gso/bb/104/ultraschall/1", msg);
      //Serieller Monitor
      Serial.print("Motion detected! Distance: ");
      Serial.println(distance1);

      //Flag auf 1
       bool1 = 1;
       return bool1;
    }
    else {
      snprintf (msg, 50, "%d", 0);
      client.publish("/gso/bb/104/ultraschall/1", msg);
      return bool1;
    }

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

      return bool1;
    }
    else {
      //Meldung an PI, dass die Ausgangsdistanz wieder gemessen wird
      snprintf (msg, 50, "%d", 0);
      client.publish("/gso/bb/104/ultraschall/1", msg);

      //Flag wieder auf 0
      bool1 = 0;
      return bool1;

    }
  }
}

int getDistance(int duration){
  int distance = duration * 0.034 / 2;
  return distance;
}