#include <ESP32Servo.h>

//Botones
const int Btn1 = 4;
const int Btn2 = 2;

//Ultrasonico
const int ECHO = 18;
const int TRIG = 19;
float duration_us, distance_cm;

//Servomotor
Servo servoMotor;

//LDR
#define ldrPin 14
const float gama = 0.7;
const float rl10 = 50;
// Se coloca en divisor de tensión con resistencia de 10K
// La resistencia a Voltaje, y el LDR va a tierra

//Leds
const int Led1 = 25;
const int Led2 = 26;
const int Led3 = 27;
const int Led4 = 33;

void setup()
{
  Serial.begin(9600);
  
  //Botones
  pinMode(Btn1, INPUT_PULLUP);
  pinMode(Btn2, INPUT_PULLUP);
  
  //Leds
  pinMode(Led1, OUTPUT);
  pinMode(Led2, OUTPUT);
  pinMode(Led3, OUTPUT);
  pinMode(Led4, OUTPUT);

  servoMotor.attach(21);
  servoMotor.write(90);

  //Ultrasonico
  pinMode(ECHO, INPUT);
  pinMode(TRIG, OUTPUT);
}

void loop()
{
  //Ultrasonico
  digitalWrite(TRIG, HIGH);//Generación de pulso por TRigger de 10microseg
  delayMicroseconds(10);//10 microsegundos
  digitalWrite(TRIG, LOW);
  duration_us= pulseIn(ECHO,HIGH);//Medición duración del pulso en ECHO
  distance_cm=0.017* duration_us;//Calculando la distancia en centimetros

  //LDR
  int valor_LDR = analogRead(ldrPin);
  valor_LDR = map(valor_LDR, 4095, 0, 1024, 0);
  float voltaje = valor_LDR / 1024.*5;
  float resistencia = 2000 * voltaje / (1-voltaje/5);
  float iluminacion = pow(rl10 * 1e3 * pow(10,gama) / resistencia,(1/gama));

  //Botones
  int estd_btn1 =! digitalRead(Btn1);
  int estd_btn2 =! digitalRead(Btn2);

  Serial.print("D=");
  Serial.print(distance_cm);
  Serial.print(" cm ");
  Serial.print("L=");
  Serial.print(iluminacion);
  Serial.print(" B1=");
  Serial.print(estd_btn1);
  Serial.print(" B2:");
  Serial.print(estd_btn2);
  Serial.print(" ");

  //Ejercicio
  if (distance_cm > 5 && distance_cm < 10)
  {
    servoMotor.write(45);
    digitalWrite(Led1, HIGH);
    digitalWrite(Led2, LOW);
    digitalWrite(Led3, LOW);
    digitalWrite(Led4, LOW);
    Serial.println(" C1");
    delay(100);
  }

  else if (iluminacion > 500)
  {
    servoMotor.write(60);
    digitalWrite(Led1, LOW);
    digitalWrite(Led2, HIGH);
    digitalWrite(Led3, LOW);
    digitalWrite(Led4, LOW);
    Serial.println(" C2");
    delay(100);
  }

  else if (distance_cm >= 10 && distance_cm < 20 && estd_btn2 == HIGH)
  {
    servoMotor.write(150);
    digitalWrite(Led1, LOW);
    digitalWrite(Led2, LOW);
    digitalWrite(Led3, LOW);
    digitalWrite(Led4, HIGH);
    Serial.println(" C4");
    delay(100);
  }

  else if (estd_btn1 == HIGH || estd_btn2 == HIGH)
  {
    servoMotor.write(120);
    digitalWrite(Led1, LOW);
    digitalWrite(Led2, LOW);
    digitalWrite(Led3, HIGH);
    digitalWrite(Led4, LOW);
    Serial.println(" C3");
    delay(100);
  }

  else
  {
    servoMotor.write(90);
    digitalWrite(Led1, LOW);
    digitalWrite(Led2, LOW);
    digitalWrite(Led3, LOW);
    digitalWrite(Led4, LOW);
    Serial.println(" RE");
    delay(100);
  }

  delay(1000);
}