#include <WiFi.h>
#include "WiFiClientSecure.h"
#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"

#define WLAN_SSID "network"
#define WLAN_PASS "123456789"
#define AIO_SERVER "io.adafruit.com"
#define AIO_SERVERPORT 8883
#define AIO_USERNAME "gobavi2999"
#define AIO_KEY "aio_VmeQ97kt1AKUit5SYRTw1dmYVtkN"

int maxDistance = 50;// 50 cm < x
int Sensor_1_trig_Pin = 32;
int Sensor_1_echo_Pin = 33;

int Sensor_2_trig_Pin = 26;
int Sensor_2_echo_Pin = 25;

int Sensor_3_trig_Pin = 27;
int Sensor_3_echo_Pin = 14;

int Sensor_4_trig_Pin = 22;
int Sensor_4_echo_Pin = 23;

int Parking_status[4] = {0,0,0,0};


long duration;
int distance;

WiFiClientSecure client;
Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT, AIO_USERNAME, AIO_KEY);
const char* adafruitio_root_ca =
  "-----BEGIN CERTIFICATE-----\n"
  "MIIDrzCCApegAwIBAgIQCDvgVpBCRrGhdWrJWZHHSjANBgkqhkiG9w0BAQUFADBh\n"
  "MQswCQYDVQQGEwJVUzEVMBMGA1UEChMMRGlnaUNlcnQgSW5jMRkwFwYDVQQLExB3\n"
  "d3cuZGlnaWNlcnQuY29tMSAwHgYDVQQDExdEaWdpQ2VydCBHbG9iYWwgUm9vdCBD\n"
  "QTAeFw0wNjExMTAwMDAwMDBaFw0zMTExMTAwMDAwMDBaMGExCzAJBgNVBAYTAlVT\n"
  "MRUwEwYDVQQKEwxEaWdpQ2VydCBJbmMxGTAXBgNVBAsTEHd3dy5kaWdpY2VydC5j\n"
  "b20xIDAeBgNVBAMTF0RpZ2lDZXJ0IEdsb2JhbCBSb290IENBMIIBIjANBgkqhkiG\n"
  "9w0BAQEFAAOCAQ8AMIIBCgKCAQEA4jvhEXLeqKTTo1eqUKKPC3eQyaKl7hLOllsB\n"
  "CSDMAZOnTjC3U/dDxGkAV53ijSLdhwZAAIEJzs4bg7/fzTtxRuLWZscFs3YnFo97\n"
  "nh6Vfe63SKMI2tavegw5BmV/Sl0fvBf4q77uKNd0f3p4mVmFaG5cIzJLv07A6Fpt\n"
  "43C/dxC//AH2hdmoRBBYMql1GNXRor5H4idq9Joz+EkIYIvUX7Q6hL+hqkpMfT7P\n"
  "T19sdl6gSzeRntwi5m3OFBqOasv+zbMUZBfHWymeMr/y7vrTC0LUq7dBMtoM1O/4\n"
  "gdW7jVg/tRvoSSiicNoxBN33shbyTApOB6jtSj1etX+jkMOvJwIDAQABo2MwYTAO\n"
  "BgNVHQ8BAf8EBAMCAYYwDwYDVR0TAQH/BAUwAwEB/zAdBgNVHQ4EFgQUA95QNVbR\n"
  "TLtm8KPiGxvDl7I90VUwHwYDVR0jBBgwFoAUA95QNVbRTLtm8KPiGxvDl7I90VUw\n"
  "DQYJKoZIhvcNAQEFBQADggEBAMucN6pIExIK+t1EnE9SsPTfrgT1eXkIoyQY/Esr\n"
  "hMAtudXH/vTBH1jLuG2cenTnmCmrEbXjcKChzUyImZOMkXDiqw8cvpOp/2PV5Adg\n"
  "06O/nVsJ8dWO41P0jmP6P6fbtGbfYmbW0W5BjfIttep3Sp+dWOIrWcBAI+0tKIJF\n"
  "PnlUkiaY4IBIqDfv8NZ5YBberOgOzW6sRBc4L0na4UU+Krk2U886UAb3LujEV0ls\n"
  "YSEY1QSteDwsOoBrp+uvFRTp2InBuThs4pFsiv9kuXclVzDAGySj4dzp30d8tbQk\n"
  "CAUw7C29C79Fv1C5qfPrmAESrciIxpg0X40KPMbp1ZWVbd4=\n"
  "-----END CERTIFICATE-----\n";

Adafruit_MQTT_Publish Parking_1 = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/Parking_1");
Adafruit_MQTT_Publish Parking_2 = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/Parking_2");
Adafruit_MQTT_Publish Parking_3 = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/Parking_3");
Adafruit_MQTT_Publish Parking_4 = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/Parking_4");

void setup() {
  Serial.begin(115200);
  readAllSensorsAndDisplayOnSerial();
  delay(10);
  connectToWiFiAndAdafruitIO();
}

void loop() {
  MQTT_connect();
  readAllSensorsAndDisplayOnSerial();
  publishParking_1_status();
  publishParking_2_status();
  publishParking_3_status();
  publishParking_4_status();
  // wait a couple seconds to avoid rate limit 30 Data/Min
  delay(8000);
}

void MQTT_connect() {
  int8_t ret;
  // Stop if already connected.
  if (mqtt.connected()) {
    return;
  }

  Serial.print("Connecting to MQTT... ");

  uint8_t retries = 3;
  while ((ret = mqtt.connect()) != 0) {  // connect will return 0 for connected
    Serial.println(mqtt.connectErrorString(ret));
    Serial.println("Retrying MQTT connection in 5 seconds...");
    mqtt.disconnect();
    delay(5000);  // wait 5 seconds
    retries--;
    if (retries == 0) {
      // basically die and wait for WDT to reset me
      while (1)
        ;
    }
  }

  Serial.println("MQTT Connected!");
}


void readAllSensorsAndDisplayOnSerial() {
  showDistanceSerial(readSensor(Sensor_1_trig_Pin, Sensor_1_echo_Pin), 1);
  showDistanceSerial(readSensor(Sensor_2_trig_Pin, Sensor_2_echo_Pin), 2);
  showDistanceSerial(readSensor(Sensor_3_trig_Pin, Sensor_3_echo_Pin), 3);
  showDistanceSerial(readSensor(Sensor_4_trig_Pin, Sensor_4_echo_Pin), 4);
  Serial.println("########################################################");
}

int readSensor(int a_trig, int b_echo) {
  pinMode(a_trig, OUTPUT);
  pinMode(b_echo, INPUT);
  digitalWrite(a_trig, LOW);
  delayMicroseconds(5);
  digitalWrite(a_trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(a_trig, LOW);
  duration = pulseIn(b_echo, HIGH);//1000 milli second = 1 second
  //speed of sound = 0.034 [cm]/[milli second]
  distance = (duration * 0.034 / 2) + 3;// cm
  return distance;
}
void showDistanceSerial(int showDistane, int number) {
  Serial.print("Distance_");
  Serial.print(number);
  Serial.print(" = ");
  Serial.print(showDistane);
  Serial.println(" cm");
  if (showDistane < maxDistance) {
    Parking_status[number - 1] = 1;
  } else {
    Parking_status[number - 1] = 0;
  }
  Serial.print(number);
  Serial.print(" = ");
  Serial.println(Parking_status[number - 1]);
  delay(50);
}

void connectToWiFiAndAdafruitIO() {
  Serial.println(F("Adafruit IO MQTTS (SSL/TLS) Example"));
  // Connect to WiFi access point.
  Serial.println();
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(WLAN_SSID);
  delay(1000);
  WiFi.begin(WLAN_SSID, WLAN_PASS);
  delay(2000);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println();
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  // Set Adafruit IO's root CA
  client.setCACert(adafruitio_root_ca);
}

void publishParking_1_status() {
  if (!Parking_1.publish(Parking_status[0])) {
    Serial.println(F("[1] Failed"));
  } else {
    Serial.println(F("[1] OK!"));
  }
}
void publishParking_2_status() {
  if (!Parking_2.publish(Parking_status[1])) {
    Serial.println(F("[2] Failed"));
  } else {
    Serial.println(F("[2] OK!"));
  }
}
void publishParking_3_status() {
  if (!Parking_3.publish(Parking_status[2])) {
    Serial.println(F("[3] Failed"));
  } else {
    Serial.println(F("[3] OK!"));
  }
}
void publishParking_4_status() {
  if (!Parking_4.publish(Parking_status[3])) {
    Serial.println(F("[4] Failed"));
  } else {
    Serial.println(F("[4] OK!"));
  }
}