#include "Arduino.h"
#include <WiFiMulti.h>
#include <Wire.h> //Import the required libraries
#include <PubSubClient.h>



#define DISCHARGE_PIN 23
#define CHARGE_PIN 5
#define MEASURING_PIN 36
#define RESISTOR_VALUE 1e6
#define MQTT_SERVER "test.mosquitto.org"
#define MQTT_PORT 1883
#define MQTT_TOPIC_CAPCITANCE "pipe_system/capacitance"
#define DEVICE_ID "PIPE_01"



WiFiMulti wifiMulti;
WiFiClient espClient;
PubSubClient client(espClient);

void setup()
{
  // initialize LED digital pin as an output.
  Serial.begin(9600);

  pinMode(CHARGE_PIN, OUTPUT);
  digitalWrite(CHARGE_PIN, LOW);

  setup_wifi();
  setup_mqtt();

}

// =============== LOOP ============ 
void loop()
{
  float cap;

  // Check WiFi connection and reconnect if needed
  if (wifiMulti.run() != WL_CONNECTED) {
    Serial.println("Wifi connection lost");
  }

  if (!client.connected()) {
    reconnect_mqtt();
  }


  cap = capacitance(DISCHARGE_PIN, CHARGE_PIN, MEASURING_PIN);
  publish_mqtt("capacitance", cap);

}
// =============== LOOP END ============ 


float capacitance(int discharge_pin, int charge_pin, int measuring_pin) {
  unsigned int start_time, elapsed_time;
  float nanoFarads;

  // discharge:
  digitalWrite(charge_pin, LOW);
  pinMode(discharge_pin, OUTPUT);
  digitalWrite(discharge_pin, LOW);
  // wait until the capacitor is fully discharged
  while(analogRead(measuring_pin) > 0) {
    //Serial.println(analogRead(measuring_pin));
    //Serial.print("discharging\n");
  };
  // stop discharging
  pinMode(discharge_pin, INPUT);

  // Start charging
  digitalWrite(charge_pin, HIGH);
  start_time = millis();
  while(analogRead(measuring_pin) < 2588){// 2588 is 63.2% of 4096, which corresponds to full-scale voltage
  }

  elapsed_time = millis() - start_time;
  nanoFarads = ((float)elapsed_time / RESISTOR_VALUE) * 1e6;
  Serial.println((long) nanoFarads);
  delay(1000);

  return nanoFarads;
};

void setup_wifi() {
  WiFi.mode(WIFI_STA); // Setup wifi connection
  wifiMulti.addAP(WIFI_SSID, WIFI_PASSWORD);

  Serial.print("Connecting to wifi"); // Connect to WiFi
  while (wifiMulti.run() != WL_CONNECTED) {
    Serial.print(".");
    delay(100);
  }
  Serial.print("Connected to ");
  Serial.print(WIFI_SSID);
  Serial.println("  Successfully");
  Serial.print(WiFi.localIP());
  Serial.println("  is the local IP");
}

void setup_mqtt() {
  client.setServer(MQTT_SERVER, MQTT_PORT);
  client.setCallback(callback);
}

void callback(char *topic, byte *message, unsigned int length) {
  Serial.print("Message arrived on topic: ");
  Serial.print(topic);
  Serial.print(". Message: ");
  String messageTemp;

  for (int i = 0; i < length; i++) {
    Serial.print((char)message[i]);
    messageTemp += (char)message[i];
  }
  Serial.println();

  if (String(topic) == "SUBSCRIBE_STRING") {
    Serial.print("a message!");
  }
}

void reconnect_mqtt() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect(DEVICE_ID)) {
      Serial.println("connected");
      // Subscribe
      client.subscribe("SUB_STRING");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

//void publish_mqtt(const char *topic, const char *sensor, float value) {
void publish_mqtt(const char *sensor, float value) {
  char buffer[40];
  sprintf(buffer, "%s %s=%.0f", DEVICE_ID, sensor, value);
  Serial.print("Publishing ");
  Serial.println(buffer);
  client.publish(MQTT_TOPIC_CAPCITANCE, buffer);
}