#ifdef DEBUG    // Macros are usually in all capital letters.
  #define DPRINT(...)    Serial.print(__VA_ARGS__)     // DPRINT is a macro, debug print.
  #define DPRINTLN(...)  Serial.println(__VA_ARGS__)   // DPRINTLN is a macro, debug print with new line.
#else
  #define DPRINT(...)     // Now defines a blank line.
  #define DPRINTLN(...)   // Now defines a blank line.
#endif

#if defined(ESP8266)
#define HARDWARE "ESP8266"
  #include "ESP8266WiFi.h"
  #include <ESP8266mDNS.h>
#elif defined(ESP32)
  #define HARDWARE "ESP32"
  #include "WiFi.h"
#endif
#include <PubSubClient.h>
#include <ArduinoOTA.h>
#include <ArduinoJson.h>
#include <ESP32Servo.h>
#include <WiFi.h>
#include "functions.h"
#define TIME_BETWEEN_MEASUREMENTS 5000


// añadidos ************************************************
enum State { CLOSED, OPEN, CONTROL_ACTIVE };
State state = CLOSED;
bool controlState = true;
unsigned long currentPos = 0;
bool wifi_connected = false;
int pos = 0;
//*********************************************************

// Declaring function
//bool reconnectToServer(PubSubClient&, const char*);

// Global constants
const char* codeVersion = "1.1.1";
const char* info_topic  = "stadium/parking/barrier/state"; // Topic to publish the current state of parking.
const char* cmd_topic   = "stadium/parking/barrier/cmd";   // Topic to send commands to the parking HW.
const char* debug_topic = "debug/stadium/parking/barrier";

// Network and WiFi settings
extern const char* SSID;
extern const char* PASSWORD;

// Parking spot settings
extern const char* ID_a;
extern const char* ID_e;

// MQTT server connection settings :)
// this is used only for function mqttSetup();
// check later if this declaration is still neeeded here (TO-DO)
extern const char* MQTT_SERVER;
extern const char* MQTT_PORT;

// LED and Button connection settings
const int controlLEDPin = 25;
const int controlButtonPin = 26;
const int barrierButtonPin = 27;
const int barrierLEDPin = 14;
const int barrierPin = 4;

// Time we want the barrier to stay open for, in milliseconds.
const unsigned long openBarrierInterval = 15000;

// Variables for barrier servo control
const int minPos = 0; // Limits for barrier position 
const int maxPos = 90;          // Barrier position
Servo barrier;        // create servo object to control a servo

int goodIterations = 0; // Variable for debugging
int goodDataSends = 0; // same


// Initialize WiFi client and MQTT client
WiFiClient espClient;
PubSubClient mqtt_client(espClient);

//↓↓ Commented out for now, will add later ↓↓//
//vector<sniffd> vecQueue; //Global queue for sniffed data (stored in a vector due to format limitations)

//Global variable, timestamp for sending data over MQTT
unsigned long lastSendTime = 0;

void setup() {
  // Initialise serial
  Serial.begin(115200);
  while (!Serial) continue;
  Serial.println("Beginning setup()...");
  // Initialise WiFi
    ("   Initializing WiFi...");
  bool wifi_connected = connectWiFi(SSID, PASSWORD, ID_e);
  //WiFi.mode(WIFI_STA);
  Serial.println("   ...done!");
  // Setup pins for input and output. INPUT_PULLUP es para activar la resistencia de pull up interna de la esp32
  pinMode(controlLEDPin, OUTPUT);
  pinMode(controlButtonPin, INPUT_PULLUP);
  pinMode(barrierButtonPin, INPUT_PULLUP);
  pinMode(barrierLEDPin, OUTPUT);
  // Configure servo
  barrier.attach(barrierPin);
  barrier.write(minPos);
  // Connecting to WiFi
  // MQTT settings
  //mqttSetup();
  
  Serial.println("Setup complete!");
}

//State variables for red (control) and green (barrier) LEDs
int lastControlButtonState = HIGH;
int lastBarrierButtonState = HIGH;
int redLEDstate = LOW;
int greenLEDstate = LOW;
unsigned long lastOpenSignal = 0;//variable for the timestamp of the last green button press

//void purgeQueue(queue<sniffd>& queue, int max_time)


void loop() {
  // MQTT Connection check
  if (!mqtt_client.connected()) {
    reconnect(mqtt_client, cmd_topic);
  }
  if(mqtt_client.connected()){
    mqtt_client.loop();
    delay(1);
  }
  //Serial.println("looping is ok"); // <- Sanity check

  //Sending current barrier status every once in a while
  unsigned long currentMillis = millis();
  goodIterations++;
  //Serial.printf("iteracions bones: %i\n", goodIterations);

  if (millis() - lastSendTime > TIME_BETWEEN_MEASUREMENTS) {
      goodDataSends++;
      Serial.printf("greenLEDstate: %i\nredLEDstate: %i\n", greenLEDstate, redLEDstate);
      //sendBarrierState(greenLEDstate, redLEDstate, pos, lastSendTime);
      //Serial.printf("sendBarrierState OK \n");
      //Serial.printf("   dataSends bons: %i\n", goodDataSends);
      lastSendTime = millis();
  }

  // Check signal from control button
  int controlButtonValue = digitalRead((controlButtonPin));
  if (lastControlButtonState != controlButtonValue) {
    // If change in control button signal, check if button has been pressed or released
    lastControlButtonState = controlButtonValue;
    if (controlButtonValue == LOW) {
      // Red button pressed
      // Toggle red LED state:
      if (redLEDstate == LOW) {
        redLEDstate = HIGH;
      }
      else {
        redLEDstate = LOW;
      }
      digitalWrite(controlLEDPin, redLEDstate);
    }
  }

  if(redLEDstate == LOW)
  {
    int barrierButtonValue = digitalRead((barrierButtonPin));
    if (lastBarrierButtonState != barrierButtonValue) {
      // Change of state in green button detected
      lastBarrierButtonState = barrierButtonValue;
      if (barrierButtonValue == LOW) {
        // Green button pressed
        lastOpenSignal = millis();
        // Toggle green LED state:
        if (greenLEDstate == LOW) {
          greenLEDstate = HIGH;
          digitalWrite(barrierLEDPin, greenLEDstate);
          if (pos < maxPos)
          {
            for (int i=0; i<=maxPos; i++)
            {
              pos = i;
              barrier.write(pos);
            }
          }
        }
      }
    }
  }
  

  // Comprovem si barrera és oberta
  if (greenLEDstate == HIGH)
  {
    // Revisem si porta oberta 15 segons:
    unsigned long timeToShutdown = openBarrierInterval - (millis()-lastOpenSignal);
    if (millis() - lastOpenSignal > openBarrierInterval)
    {
      //tanquem barrera:
      greenLEDstate = LOW;
      digitalWrite(barrierLEDPin, greenLEDstate);
      if (pos > minPos)
      {
        for (int i=0; i>=minPos; i--)
        {
          pos = i;
          barrier.write(pos);
        }
      }
    }
  }
  delay(100);
}