#define trigger 8
#define echo 7
#define zumbador 9
#include "TimerThree.h"

const int viaCM = 125, pin_pulsador[3] = {18, 19, 20}, LED[3] = {22,23,24};

volatile int distancia, distancia_ant, sensor, x = 0, y, e, pse_e = 5;
volatile bool pulsador[3] = {false, false, false}, pulsador_anterior[3] = {false, false, false}, salida[3] = {false, false, false};

bool password = 0, notificacion = 0;


void setup() {
  pinMode(trigger, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(zumbador, OUTPUT);
  for(int i = 0; i < 3; i++)
  {
    pinMode(pin_pulsador[i], INPUT);
    pinMode(LED[i], OUTPUT);
  }
  /*
  attachInterrupt(digitalPinToInterrupt(18), pulsador1, RISING);
  attachInterrupt(digitalPinToInterrupt(19), pulsador1, RISING);
  attachInterrupt(digitalPinToInterrupt(20), pulsador1, RISING);
  */
  Timer3.initialize(1500000);
  Timer3.attachInterrupt(timer_general);



  Serial.begin(9600);
}
void timer_general(void) {
  pse_e--;
  e = 0;
  distancia = sensor;
}

void pulsadores(void) {

  for(int i = 0; i < 3; i++)
  {
    pulsador_anterior[i] = pulsador[i];
    pulsador[i] = digitalRead(pin_pulsador[i]);
    if(pulsador[i] > pulsador_anterior[i])
    {
      salida[i] = true;
    }
    else salida[i] = false;
  }

}



/*
void pulsador1()  {              CÓDIGO ACTUALMENTE INUTILIZADO
  x++;
  if((x == 1)||(x == 4)) y++;
}
void pulsador2()  {
  x++;
  if(x == 3) y++;
}
void pulsador3()  {
  x++;
  if(x == 2) y++;
}
*/

int distancia_() { //Devuelve en cm la distancia a la que se encuentra el objeto del sensor
  long respuesta_echo;
   digitalWrite(trigger, LOW);
   delayMicroseconds(4);
   digitalWrite(trigger, HIGH);
   delayMicroseconds(10);
   digitalWrite(trigger, LOW);
   respuesta_echo = pulseIn(echo, HIGH);
   return (respuesta_echo * 0.01712);
}


void loop() {
  e = 1;

  noInterrupts();
  sensor = distancia_(); //Evita enviar datos corruptos
  interrupts();

  /*
   * El PIN actualmente es de cuatro dígitos con tres pulsadores (1321)
   */
if(pse_e < 0)//Para que únicamente haga la lectura cada 0,05s
{
  pse_e = 5;
  if(salida[0]) {
    if((x == 0)||(x == 3)) y++;
    x++;
  }
  if(salida[1]) {
    if(x == 2) y++;
    x++;
  }
  if(salida[2]) {
    if(x == 1) y++;
    x++;
  }
  pulsadores();
}
  //Comprueba si se ha introducido la contraseña correctamente
  if(x == 4)
  {
    if(y == 4) password = 1;
    x = 0;
    y = 0;
  }
  else password = 0;

  if(password)
  {
    while(password)
    {
      if(viaCM > distancia)
      {
        password = 0;
        delay(5000);
      }
    }
  }
  else if(viaCM > distancia)
  {
    notificacion = 1;
  }
  else notificacion = 0;






  //Impresiones
  switch(x) {
    case 1: {
      digitalWrite(LED[0], HIGH);
      break;
    }
    case 2: {
      digitalWrite(LED[1], HIGH);
      break;
    }
    case 3: {
      digitalWrite(LED[2], HIGH);
      break;
    }
    default: {
      x = 0;
      for(int i = 0; i < 4; i++)  {
        digitalWrite(LED[i], LOW);
      }
      break;
    }
  }
  digitalWrite(zumbador, notificacion);

  //Debug
  if(Serial.available() > 0)
  {
    salida[0] = Serial.read() - 48;
  }
  Serial.println(pse_e);
  Serial.println(salida[0]);
  Serial.println(x);
  Serial.println(y);
  Serial.println("");
  /* -FIN DEL PROGRAMA- */

  while(e)
  {

  }
}