#define BTN_1ra 4
#define BTN_2da 5
#define BTN_3ra 6
#define BTN_4ta 7
#define BTN_5ta 8
#define BTN_Ret 9
#define Servo_1 10
#define Servo_2 11
#define BTN_1_2 12
#define BTN_5_R 13

#include <Servo.h>

#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo servo1;
Servo servo2;

int Angulo_1 = 90;
int Angulo_2 = 90;

int Ultimo_estado = 0;

bool BTN_st_1 = true;
bool BTN_st_2 = true;
bool BTN_st_3 = true;
bool BTN_st_4 = true;
bool BTN_st_5 = true;
bool BTN_st_R = true;
bool BTN_st_N_1_2 = true;
bool BTN_st_N_5_R = true;

void setup() {

  servo1.attach(Servo_1);
  servo2.attach(Servo_2);    

  pinMode(BTN_1ra, INPUT_PULLUP);
  pinMode(BTN_2da, INPUT_PULLUP);
  pinMode(BTN_3ra, INPUT_PULLUP);
  pinMode(BTN_4ta, INPUT_PULLUP);
  pinMode(BTN_5ta, INPUT_PULLUP);
  pinMode(BTN_Ret, INPUT_PULLUP);
  pinMode(BTN_1_2, INPUT_PULLUP);
  pinMode(BTN_5_R, INPUT_PULLUP);

  servo1.write(Angulo_1);
  servo2.write(Angulo_2);

  lcd.init();
  lcd.begin(16,2);
  lcd.backlight();

  lcd.setCursor(0, 0);
  lcd.print("SELECTOR");
  lcd.setCursor(0, 1);
  lcd.print("ELECTROMECANICO");
  delay (1000);
  lcd.clear();
}


void loop() {

  BTN_st_1 = digitalRead(BTN_1ra);
  BTN_st_2 = digitalRead(BTN_2da);
  BTN_st_3 = digitalRead(BTN_3ra);
  BTN_st_4 = digitalRead(BTN_4ta);
  BTN_st_5 = digitalRead(BTN_5ta);
  BTN_st_R = digitalRead(BTN_Ret);
  BTN_st_N_1_2 = digitalRead(BTN_1_2);
  BTN_st_N_5_R = digitalRead(BTN_5_R);

  Activar_Servos();
}

void Activar_Servos()
{
  if(BTN_st_1==LOW)
  {
    if(Ultimo_estado != 1)
    {
      Activar_LCD(1);
    }
    Angulo_1 = 45;
    Angulo_2 = 25;    
    
    Ultimo_estado = 1;
  }

  if(BTN_st_2==LOW)
  {
    if(Ultimo_estado != 2)
    {
      Activar_LCD(2);
    }
    Angulo_1 = 45;
    Angulo_2 = 160;

    Ultimo_estado = 2;
  }

  if(BTN_st_3==LOW)
  {
    if(Ultimo_estado != 3)
    {
      Activar_LCD(3);
    }
    Angulo_1 = 90;
    Angulo_2 = 25;
    
    Ultimo_estado = 3;
  }
  
  if(BTN_st_4==LOW)
  {
    if(Ultimo_estado != 4)
    {
      Activar_LCD(4);
    }
    Angulo_1 = 90;
    Angulo_2 = 160;

    Ultimo_estado = 4;
  }

  if(BTN_st_5==LOW)
  {
    if(Ultimo_estado != 5)
    {
      Activar_LCD(5);
    }
    Angulo_1 = 145;
    Angulo_2 = 25;    

    Ultimo_estado = 5;
  }

  if(BTN_st_R==LOW)
  {
    if(Ultimo_estado != 6)
    {
      Activar_LCD(6);
    }
    Angulo_1 = 145;
    Angulo_2 = 160;

    Ultimo_estado = 6;
  }

  if(BTN_st_N_1_2==LOW && BTN_st_1==HIGH && BTN_st_2==HIGH)
  {
    if(Ultimo_estado != 7)
    {
      Activar_LCD(7);
    }
    Angulo_1 = 45;
    Angulo_2 = 90;

    Ultimo_estado = 7;
  }

  if(BTN_st_N_1_2==HIGH && BTN_st_N_5_R==HIGH && BTN_st_3==HIGH && BTN_st_4==HIGH)
  {
    if(Ultimo_estado != 8)
    {
      Activar_LCD(8);
    }
    Angulo_1 = 90;
    Angulo_2 = 90;

    Ultimo_estado = 8;
  }

  if(BTN_st_N_5_R==LOW && BTN_st_5==HIGH && BTN_st_R==HIGH)
  {
    if(Ultimo_estado != 9)
    {
      Activar_LCD(9);
    }
    Angulo_1 = 145;
    Angulo_2 = 90;
    
    Ultimo_estado = 9;
  }

  servo1.write(Angulo_1);
  servo2.write(Angulo_2);
}


void Activar_LCD(int x)
{
  lcd.clear();
  switch (x)
  {
    case 1:
      lcd.setCursor(0, 0);
      lcd.print("MARCHA:");
      lcd.setCursor(0, 1);
      lcd.print("PRIMERA");
      break;
    case 2:
      lcd.setCursor(0, 0);
      lcd.print("MARCHA:");
      lcd.setCursor(0, 1);
      lcd.print("SEGUNDA");
      break;
    case 3:
      lcd.setCursor(0, 0);
      lcd.print("MARCHA:");
      lcd.setCursor(0, 1);
      lcd.print("TERCERA");
      break;
    case 4:
      lcd.setCursor(0, 0);
      lcd.print("MARCHA:");
      lcd.setCursor(0, 1);
      lcd.print("CUARTA");
      break;
    case 5:
      lcd.setCursor(0, 0);
      lcd.print("MARCHA:");
      lcd.setCursor(0, 1);
      lcd.print("QUINTA");
      break;
    case 6:
      lcd.setCursor(0, 0);
      lcd.print("MARCHA:");
      lcd.setCursor(0, 1);
      lcd.print("RETRO");
      break;
    case 7:
      lcd.setCursor(0, 0);
      lcd.print("MARCHA:");
      lcd.setCursor(0, 1);
      lcd.print("NEUTRO");
      break;
    case 8:
      lcd.setCursor(0, 0);
      lcd.print("MARCHA:");
      lcd.setCursor(0, 1);
      lcd.print("NEUTRO");
      break;
    case 9:
      lcd.setCursor(0, 0);
      lcd.print("MARCHA:");
      lcd.setCursor(0, 1);
      lcd.print("NEUTRO");
      break;
  }

}