#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 "project_12488"
#define AIO_KEY "aio_QPFX76Hv9PYlF3gN9BRIYsYxhJdW"

int maxDistance = 50;
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 Sensor_5_trig_Pin = 21;
int Sensor_5_echo_Pin = 19;

int Sensor_6_trig_Pin = 18;
int Sensor_6_echo_Pin = 5;

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


long duration;
int distance;

WiFiClientSecure client;
Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT, AIO_USERNAME, AIO_KEY);
// io.adafruit.com root CA
const char* adafruitio_root_ca = \
                                 "-----BEGIN CERTIFICATE-----\n"
                                 "MIIEjTCCA3WgAwIBAgIQDQd4KhM/xvmlcpbhMf/ReTANBgkqhkiG9w0BAQsFADBh\n"
                                 "MQswCQYDVQQGEwJVUzEVMBMGA1UEChMMRGlnaUNlcnQgSW5jMRkwFwYDVQQLExB3\n"
                                 "d3cuZGlnaWNlcnQuY29tMSAwHgYDVQQDExdEaWdpQ2VydCBHbG9iYWwgUm9vdCBH\n"
                                 "MjAeFw0xNzExMDIxMjIzMzdaFw0yNzExMDIxMjIzMzdaMGAxCzAJBgNVBAYTAlVT\n"
                                 "MRUwEwYDVQQKEwxEaWdpQ2VydCBJbmMxGTAXBgNVBAsTEHd3dy5kaWdpY2VydC5j\n"
                                 "b20xHzAdBgNVBAMTFkdlb1RydXN0IFRMUyBSU0EgQ0EgRzEwggEiMA0GCSqGSIb3\n"
                                 "DQEBAQUAA4IBDwAwggEKAoIBAQC+F+jsvikKy/65LWEx/TMkCDIuWegh1Ngwvm4Q\n"
                                 "yISgP7oU5d79eoySG3vOhC3w/3jEMuipoH1fBtp7m0tTpsYbAhch4XA7rfuD6whU\n"
                                 "gajeErLVxoiWMPkC/DnUvbgi74BJmdBiuGHQSd7LwsuXpTEGG9fYXcbTVN5SATYq\n"
                                 "DfbexbYxTMwVJWoVb6lrBEgM3gBBqiiAiy800xu1Nq07JdCIQkBsNpFtZbIZhsDS\n"
                                 "fzlGWP4wEmBQ3O67c+ZXkFr2DcrXBEtHam80Gp2SNhou2U5U7UesDL/xgLK6/0d7\n"
                                 "6TnEVMSUVJkZ8VeZr+IUIlvoLrtjLbqugb0T3OYXW+CQU0kBAgMBAAGjggFAMIIB\n"
                                 "PDAdBgNVHQ4EFgQUlE/UXYvkpOKmgP792PkA76O+AlcwHwYDVR0jBBgwFoAUTiJU\n"
                                 "IBiV5uNu5g/6+rkS7QYXjzkwDgYDVR0PAQH/BAQDAgGGMB0GA1UdJQQWMBQGCCsG\n"
                                 "AQUFBwMBBggrBgEFBQcDAjASBgNVHRMBAf8ECDAGAQH/AgEAMDQGCCsGAQUFBwEB\n"
                                 "BCgwJjAkBggrBgEFBQcwAYYYaHR0cDovL29jc3AuZGlnaWNlcnQuY29tMEIGA1Ud\n"
                                 "HwQ7MDkwN6A1oDOGMWh0dHA6Ly9jcmwzLmRpZ2ljZXJ0LmNvbS9EaWdpQ2VydEds\n"
                                 "b2JhbFJvb3RHMi5jcmwwPQYDVR0gBDYwNDAyBgRVHSAAMCowKAYIKwYBBQUHAgEW\n"
                                 "HGh0dHBzOi8vd3d3LmRpZ2ljZXJ0LmNvbS9DUFMwDQYJKoZIhvcNAQELBQADggEB\n"
                                 "AIIcBDqC6cWpyGUSXAjjAcYwsK4iiGF7KweG97i1RJz1kwZhRoo6orU1JtBYnjzB\n"
                                 "c4+/sXmnHJk3mlPyL1xuIAt9sMeC7+vreRIF5wFBC0MCN5sbHwhNN1JzKbifNeP5\n"
                                 "ozpZdQFmkCo+neBiKR6HqIA+LMTMCMMuv2khGGuPHmtDze4GmEGZtYLyF8EQpa5Y\n"
                                 "jPuV6k2Cr/N3XxFpT3hRpt/3usU/Zb9wfKPtWpoznZ4/44c1p9rzFcZYrWkj3A+7\n"
                                 "TNBJE0GmP2fhXhP1D/XVfIW/h0yCJGEiV9Glm/uGOa3DXHlmbAcxSyCRraG+ZBkA\n"
                                 "7h4SeM6Y8l/7MBRpPCz6l8Y=\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");
Adafruit_MQTT_Publish Parking_5 = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/Parking_5");
Adafruit_MQTT_Publish Parking_6 = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/Parking_6");

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

int period = 15000;
unsigned long prevTime = 0;
void loop() {
  MQTT_connect();
  readAllSensorsAndDisplayOnSerial();

  while (millis() - prevTime >= period) {
    prevTime = millis();
    publishParking_1_status();
    publishParking_2_status();
    publishParking_3_status();
    publishParking_4_status();
    publishParking_5_status();
    publishParking_6_status();
  }
  // wait a couple seconds to avoid rate limit 30 Data/Min
  //delay(15000);
}

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);
  showDistanceSerial(readSensor(Sensor_5_trig_Pin, Sensor_5_echo_Pin), 5);
  showDistanceSerial(readSensor(Sensor_6_trig_Pin, Sensor_6_echo_Pin), 6);
  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);
  distance = (duration * 0.034 / 2) + 3;
  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!"));
  }
}
void publishParking_5_status() {
  if (!Parking_5.publish(Parking_status[4])) {
    Serial.println(F("[5] Failed"));
  } else {
    Serial.println(F("[5] OK!"));
  }
}
void publishParking_6_status() {
  if (!Parking_6.publish(Parking_status[5])) {
    Serial.println(F("[6] Failed"));
  } else {
    Serial.println(F("[6] OK!"));
  }
}