#include <SPI.h>
#include <MFRC522.h>
#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// ---------------- LCD ----------------
LiquidCrystal_I2C lcd(0x27, 16, 2);

// ---------------- RFID ----------------
#define SS_PIN 53
#define RST_PIN 8
MFRC522 rfid(SS_PIN, RST_PIN);

// ---------------- SERVO ENTRADA ----------------
Servo servoEntrada;
const int pinServoEntrada = 9;

// ---------------- SERVO SALIDA ----------------
Servo servoSalida;
const int pinServoSalida = 10;

// ---------------- BOTONES SALIDA ----------------
const int botonAbrirSalida = 22;
const int botonCerrarSalida = 23;

// ---------------- ULTRASONICO ENTRADA ----------------
const int trigPin = 6;
const int echoPin = 7;

// ---------------- VARIABLES SERVO ----------------
int posEntradaActual = 0;
int posEntradaDestino = 0;

int posSalidaActual = 0;
int posSalidaDestino = 0;

unsigned long tiempoAnterior = 0;
const unsigned long intervaloMovimiento = 15;

// ---------------- INFRAROJO ----------------

int ledPrueba = 45;
int sensorInf1 = 47;
int estadoInf = 0;

// ---------------- ESPACIOS ----------------
int espaciosDisponibles = 2;

// ---------------- MENSAJES TEMPORALES ----------------
bool mensajeTemporalActivo = false;
unsigned long mensajeInicio = 0;
const unsigned long mensajeDuracion = 2000;

// ---------------- TARJETAS ----------------
byte tarjetasAutorizadas[][4] = {
  {0x04, 0x11, 0x22, 0x33},
  {0x01, 0x02, 0x03, 0x04}
};
const int totalTarjetas = 2;

// ---------------- ESTADOS ----------------
enum EstadoEntrada {
  CERRADA,
  ABRIENDO,
  ABIERTA_ESPERANDO_CARRO,
  CARRO_DETECTADO,
  CERRANDO
};

enum EstadoSalida {
  SALIDA_CERRADA,
  SALIDA_ABRIENDO,
  SALIDA_ABIERTA,
  SALIDA_CERRANDO
};

EstadoEntrada estadoEntrada = CERRADA;
EstadoSalida estadoSalida = SALIDA_CERRADA;

// ---------------- INFRAROJOS ----------------
const int infrarojos[] = {37, 39, 41, 43, 45};

const int infrarojosTotales = 5;
int contadorParqueo;

// ------------------------------------------------------

void setup() {

  Serial.begin(9600);

  SPI.begin();
  rfid.PCD_Init();

  servoEntrada.attach(pinServoEntrada);
  servoSalida.attach(pinServoSalida);

  servoEntrada.write(0);
  servoSalida.write(0);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(botonAbrirSalida, INPUT_PULLUP);
  pinMode(botonCerrarSalida, INPUT_PULLUP);

  lcd.init();
  lcd.backlight();
  lcd.print("Sistema listo");
  delay(1000);
  lcd.clear();

  pinMode(ledPrueba, OUTPUT);
  pinMode(sensorInf1, INPUT);

  for (int i = 0; i < infrarojosTotales; i++) {
    pinMode(infrarojos[i], INPUT_PULLUP);
  }
}

// ------------------------------------------------------

void loop() {

  unsigned long tiempoActual = millis();

  if (mensajeTemporalActivo) {
    if (millis() - mensajeInicio >= mensajeDuracion) {
      mensajeTemporalActivo = false;
    } else {
      return;
    }
  }

  moverServos(tiempoActual);

  controlarEntrada();
  controlarSalida();
  pruebaINFled();
  contarParqueos();
}

// ------------------------------------------------------

void controlarSalida() {

  if (estadoSalida == SALIDA_CERRADA && digitalRead(botonAbrirSalida) == LOW) {
    mostrarMensajeTemporal("Salida abierta", "");
    posSalidaDestino = 90;
    estadoSalida = SALIDA_ABRIENDO;
  }

  if (estadoSalida == SALIDA_ABRIENDO && posSalidaActual == 90) {
    estadoSalida = SALIDA_ABIERTA;
  }

  if (estadoSalida == SALIDA_ABIERTA && digitalRead(botonCerrarSalida) == LOW) {
    posSalidaDestino = 0;
    estadoSalida = SALIDA_CERRANDO;
  }

  if (estadoSalida == SALIDA_CERRANDO && posSalidaActual == 0) {
    estadoSalida = SALIDA_CERRADA;
  }
}

// ------------------------------------------------------

void controlarEntrada() {

  long distancia = medirDistancia();

  if (estadoEntrada == CERRADA &&
      rfid.PICC_IsNewCardPresent() &&
      rfid.PICC_ReadCardSerial()) {

    if (contadorParqueo == 0) {
      mostrarMensajeTemporal("Parqueo lleno", "");
      delay(2500);
    }
    else if (validarTarjeta(rfid.uid.uidByte, rfid.uid.size)) {
      mostrarMensajeTemporal("Acceso permitido", "Espacios: " + String(contadorParqueo));
      posEntradaDestino = 90;
      estadoEntrada = ABRIENDO;
    }
    else {
      mostrarMensajeTemporal("Acceso denegado", "");
    }

    rfid.PICC_HaltA();
  }

  if (estadoEntrada == CERRADA) {
    mostrarEstado("Pase tarjeta", "Espacios: " + String(contadorParqueo));
  }

  if (estadoEntrada == ABRIENDO && posEntradaActual == 90) {
    estadoEntrada = ABIERTA_ESPERANDO_CARRO;
  }

  if (estadoEntrada == ABIERTA_ESPERANDO_CARRO && distancia < 10) {
    estadoEntrada = CARRO_DETECTADO;
  }

  if (estadoEntrada == CARRO_DETECTADO && distancia > 15) {
    posEntradaDestino = 0;
    estadoEntrada = CERRANDO;
  }

  if (estadoEntrada == CERRANDO && posEntradaActual == 0) {
    estadoEntrada = CERRADA;
  }
}

// ------------------------------------------------------

void moverServos(unsigned long tiempoActual) {

  static unsigned long tiempoAnteriorMovimiento = 0;

  if (tiempoActual - tiempoAnteriorMovimiento >= intervaloMovimiento) {

    tiempoAnteriorMovimiento = tiempoActual;

    if (posEntradaActual < posEntradaDestino) {
      posEntradaActual++;
      servoEntrada.write(posEntradaActual);
    }
    else if (posEntradaActual > posEntradaDestino) {
      posEntradaActual--;
      servoEntrada.write(posEntradaActual);
    }

    if (posSalidaActual < posSalidaDestino) {
      posSalidaActual++;
      servoSalida.write(posSalidaActual);
    }
    else if (posSalidaActual > posSalidaDestino) {
      posSalidaActual--;
      servoSalida.write(posSalidaActual);
    }
  }
}

// ------------------------------------------------------

long medirDistancia() {

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duracion = pulseIn(echoPin, HIGH);
  return duracion * 0.034 / 2;
}

// ------------------------------------------------------

bool validarTarjeta(byte *uid, byte size) {

  for (int i = 0; i < totalTarjetas; i++) {

    bool coincide = true;

    for (int j = 0; j < size; j++) {
      if (uid[j] != tarjetasAutorizadas[i][j]) {
        coincide = false;
        break;
      }
    }

    if (coincide) return true;
  }

  return false;
}

// ------------------------------------------------------

void mostrarEstado(String linea1, String linea2) {

  static String ultimaLinea1 = "";
  static String ultimaLinea2 = "";

  if (linea1 != ultimaLinea1 || linea2 != ultimaLinea2) {

    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print(linea1);
    lcd.setCursor(0,1);
    lcd.print(linea2);

    ultimaLinea1 = linea1;
    ultimaLinea2 = linea2;
  }
}

// ------------------------------------------------------

void mostrarMensajeTemporal(String linea1, String linea2) {

  mostrarEstado(linea1, linea2);

  mensajeTemporalActivo = true;
  mensajeInicio = millis();
}

void pruebaINFled (){
  estadoInf = digitalRead(sensorInf1);
  //sensor manda 1 si no hay obstaculo
  //y 0 si hay obstaculo
  if (estadoInf) {               
    digitalWrite(ledPrueba, LOW);
  } else {
    digitalWrite(ledPrueba, HIGH);
  }
}

// ------------------------------------------------------
 void contarParqueos(){
  contadorParqueo = 5;

  for (int i = 0; i < infrarojosTotales; i++) {

    int estado = digitalRead(infrarojos[i]);

    //0 detecta
    //1 no hay obstaculo
    if (estado == LOW) {      
      contadorParqueo--;
    } else {
    }
  }


 }