#include <Wire.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <Adafruit_VL53L0X.h>
#include <math.h>?>
// Toggle Debug Messages
bool debugOn = true;

// Set Interval
uint16_t hz = 10;

// Replace with your network credentials
const char* ssid = "SSID";
const char* password = "SSID_Password";

// Our MQTT Broker IP address
const char* mqtt_server = "test.mosquitto.org";
const char* my_client_id = "ESP32Client";

const char* my_mqtt_topic_sensor = "gw/duese002";
const char* my_mqtt_topic_actuator = "gw/duese002-licht";


// Setup WiFi client and MQTT client
WiFiClient espClient;
PubSubClient client(espClient);

// Set up an instance for each sensor
Adafruit_VL53L0X tof1 = Adafruit_VL53L0X();
Adafruit_VL53L0X tof2 = Adafruit_VL53L0X();

// Shutdown pins
#define SHT_TOF1 35 //32 in our older system
#define SHT_TOF2 40 //33 in our older system

// Sensor readings
uint16_t x = 0;
uint16_t z = 0;

// Define the data object for sensor measurement
VL53L0X_RangingMeasurementData_t measure1;
VL53L0X_RangingMeasurementData_t measure2;

unsigned long lastMsg = 0;

// LED Pins
const int dueseLedPin = 4; 
const int OoRLedPin = 15; //25 in our older system

int sda_pin = 16; // GPIO16 as I2C SDA (Right_Blue Side on Board)
int scl_pin = 17; // GPIO17 as I2C SCL (Right_Red Side on Board)

void setup() {

  Wire.setPins(sda_pin, scl_pin); // Set the I2C pins before begin
  Wire.begin(); // join i2c bus (address optional for master)


  if (debugOn) Serial.begin(115200);

  setup_wifi();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);

  pinMode(dueseLedPin, OUTPUT);
  pinMode(OoRLedPin, OUTPUT);

  digitalWrite(dueseLedPin, LOW);
  digitalWrite(OoRLedPin, LOW);

  pinMode(SHT_TOF1, OUTPUT);
  pinMode(SHT_TOF2, OUTPUT);

  digitalWrite(SHT_TOF1, LOW);
  digitalWrite(SHT_TOF2, LOW);

  delay(10);
  digitalWrite(SHT_TOF1, HIGH);
  delay(10);

  // Initialize sensor 1
  if (!tof1.begin(0x30)) {
    debug(F("Failed to boot VL53L0X - 01 (Vertical)"));
    while (1)
      ;
  }

  delay(10);
  digitalWrite(SHT_TOF2, HIGH);
  delay(10);

  // Initialize sensor 2
  if (!tof2.begin()) {
    debug(F("Failed to boot VL53L0X - 02 (Horizontal)"));
    while (1);
  }

  digitalWrite(SHT_TOF1, HIGH);
  digitalWrite(SHT_TOF2, HIGH);

  delay(10);
}

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

  long now = millis();
  if (now - lastMsg > long(1000 / hz)) {  // Data transmission rate of 10Hz
    lastMsg = now;

    // Reading distance from both sensors
    tof1.getSingleRangingMeasurement(&measure1, false);
    if (measure1.RangeStatus != 4) {
      z = measure1.RangeMilliMeter;
       //Serial.print("Distance Vertical (mm): "); Serial.println(measure1.RangeMilliMeter);
    }
  
    tof2.getSingleRangingMeasurement(&measure2, false);
    if (measure2.RangeStatus != 4) {
      x = measure2.RangeMilliMeter;
      //Serial.print("Distance Horizontal (mm): "); Serial.println(measure2.RangeMilliMeter);

    }

    // Current time in milliseconds.
    long t = now;

    // Create data payload
    String payload = String("(") + String(t) + ", " + String(x) + ", " + String(z) + ")";

    // Publish data
    // Checks the readings noise-freeness; VL53L0X gives an output of 8191 by having so much noise by the environment
    if (x != 8191 && z != 8191 && x > 90 && z > 90) {
      digitalWrite(OoRLedPin, LOW);
      client.publish(my_mqtt_topic_sensor, (char*)payload.c_str());
      debug(payload);
    } else {
      digitalWrite(OoRLedPin, HIGH);        
    }
  }
}

void setup_wifi() {
  delay(10);
  debug("");
  debug("Connecting to ");
  debug(ssid);

  WiFi.begin(ssid, password);

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

  debug("");
  debug("WiFi connected");
}

void reconnect() {
  
  
  //debug(F("Connecing to the MQTT server ..."));
  //debug(mqtt_server);

  while (!client.connected()) {
    
    if (client.connect(my_client_id)) {
      // Once connected, resubscribe to the required topic
      client.subscribe(my_mqtt_topic_actuator);
      //Serial.println("MQTT connected"); //state when successfully connected to the MQTT server
    } 
    else {
      //Serial.print(client.state());
      //delay(5000);
    }

    debug(F(".."));
  }
}

void callback(char* topic, byte* message, unsigned int length) {
  String messageTemp;
  
  for (int i = 0; i < length; i++) {
    Serial.print((char)message[i]);
    messageTemp += (char)message[i];
  }

  // Feel free to add more if statements to control more GPIOs with MQTT

  // If a message is received on the topic esp32/output, you check if the message is either "on" or "off". 
  // Changes the output state according to the message
  if (String(topic) == my_mqtt_topic_actuator) {
    if(messageTemp == "on"){
      digitalWrite(dueseLedPin, HIGH);
    }
    else if(messageTemp == "off"){
      digitalWrite(dueseLedPin, LOW);
    }
  }
}

void debug(String msg) {
  if (debugOn) {
    Serial.println(msg);
  }
}