/***   Included libraries  ***/
#include <Servo.h>




/***   Global variables and function definition  ***/
const int BOTON_SEGUNDO = 8;
const int BOTON_PRIMERO = 9;
const int BOTON_BAJO = 10;
const int IR_BAJO = 5;
const int IR_PRIMERO = 4;
const int IR_SEGUNDO = 3;
Servo servoRC;
const int led_bajo = 11;
const int led_primero = 12;
const int led_segundo = 13;
const int zumbador = 6;
const int led_bajo_azul = 7;

float estado = 6;
void S0() {
    servoRC.write(90);
    ApagarLedsPisos();
    digitalWrite(led_bajo, HIGH);
}
void S1() {
    servoRC.write(90);
    ApagarLedsPisos();
    digitalWrite(led_primero, HIGH);
}
void S2() {
    servoRC.write(90);
    ApagarLedsPisos();
    digitalWrite(led_segundo, HIGH);
}
void S3() {
    servoRC.write(0);
    ApagarLedsPisos();
    while (digitalRead(IR_PRIMERO) == 0) {
        digitalWrite(led_primero, HIGH);
        delay(100);
        digitalWrite(led_primero, LOW);
        delay(100);
    }
}
void S4() {
    servoRC.write(0);
    ApagarLedsPisos();
    while (digitalRead(IR_SEGUNDO) == 0) {
        digitalWrite(led_segundo, HIGH);
        delay(100);
        digitalWrite(led_segundo, LOW);
        delay(100);
    }
}
void S5() {
    servoRC.write(180);
    ApagarLedsPisos();
    while (digitalRead(IR_PRIMERO) == 0) {
        digitalWrite(led_primero, HIGH);
        delay(100);
        digitalWrite(led_primero, LOW);
        delay(100);
    }
}
void S6() {
    servoRC.write(180);
    ApagarLedsPisos();
    while (digitalRead(IR_BAJO) == 0) {
        digitalWrite(led_bajo, HIGH);
        delay(100);
        digitalWrite(led_bajo, LOW);
        delay(100);
    }
}
void ApagarLedsPisos() {
    digitalWrite(led_bajo, LOW);
    digitalWrite(led_primero, LOW);
    digitalWrite(led_segundo, LOW);
}
void Parpadeo_Encender() {
    for (int bitbloqForCounter = 0; bitbloqForCounter < 3; bitbloqForCounter += 1) {
        digitalWrite(led_bajo_azul, HIGH);
        delay(70);
        digitalWrite(led_bajo_azul, LOW);
        delay(70);
    }
}
void SonidoLlegarPiso() {
    tone(zumbador, 370, 20);
    delay(20);
    tone(zumbador, 466, 40);
    delay(40);
    noTone(zumbador);
}
void SonidoEncender() {
    tone(zumbador, 261, 100);
    delay(100);
    tone(zumbador, 293, 100);
    delay(100);
    tone(zumbador, 329, 100);
    delay(100);
    tone(zumbador, 349, 100);
    delay(100);
    tone(zumbador, 392, 100);
    delay(100);
    tone(zumbador, 349, 100);
    delay(100);
    tone(zumbador, 329, 100);
    delay(100);
    tone(zumbador, 293, 100);
    delay(100);
    tone(zumbador, 261, 100);
    delay(100);
    noTone(zumbador);
}



/***   Setup  ***/
void setup() {
    pinMode(BOTON_SEGUNDO, INPUT);
    pinMode(BOTON_PRIMERO, INPUT);
    pinMode(BOTON_BAJO, INPUT);
    pinMode(IR_BAJO, INPUT);
    pinMode(IR_PRIMERO, INPUT);
    pinMode(IR_SEGUNDO, INPUT);
    servoRC.attach(2);
    pinMode(led_bajo, OUTPUT);
    pinMode(led_primero, OUTPUT);
    pinMode(led_segundo, OUTPUT);
    pinMode(zumbador, OUTPUT);
    pinMode(led_bajo_azul, OUTPUT);

    SonidoEncender();
    delay(1000);
    Parpadeo_Encender();
}


/***   Loop  ***/
void loop() {
    /*
    Cambio de Estados
    */
    if (estado == 0) {
        if (digitalRead(BOTON_PRIMERO) == 1) {
            estado = 3;
            delay(400);
        } else if (digitalRead(BOTON_SEGUNDO) == 1) {
            estado = 4;
            delay(400);
        }
    }
    if (estado == 1) {
        if (digitalRead(BOTON_SEGUNDO) == 1) {
            estado = 4;
            delay(400);
        }
        if (digitalRead(BOTON_BAJO) == 1) {
            estado = 6;
            delay(400);
        }
    }
    if (estado == 2) {
        if (digitalRead(BOTON_PRIMERO) == 1) {
            estado = 5;
            delay(400);
        }
        if (digitalRead(BOTON_BAJO) == 1) {
            estado = 6;
            delay(400);
        }
    }
    if (estado == 3) {
        if (digitalRead(IR_PRIMERO) == 1) {
            estado = 1;
            SonidoLlegarPiso();
        }
    }
    if (estado == 4) {
        if (digitalRead(IR_SEGUNDO) == 1) {
            estado = 2;
        }
        SonidoLlegarPiso();
    }
    if (estado == 5) {
        if (digitalRead(IR_PRIMERO) == 1) {
            estado = 1;
        }
        SonidoLlegarPiso();
    }
    if (estado == 6) {
        if (digitalRead(IR_BAJO) == 1) {
            estado = 0;
        }
        SonidoLlegarPiso();
    }
    /*
    Ejecución de Funciones
    */
    switch (int(estado)) {
        case 0:
            S0();
            break;
        case 1:
            S1();
            break;
        case 2:
            S2();
            break;
        case 3:
            S3();
            break;
        case 4:
            S4();
            break;
        case 5:
            S5();
            break;
        case 6:
            S6();
            break;
    }
}