#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>

#define SDA 25                    //Define SDA pins
#define SCL 26                    //Define SCL pins
#define PIN_LED    4
#define PIN_LED_2  5
#define trigPin 13 // define TrigPin
#define echoPin 14 // define EchoPin.
#define MAX_DISTANCE 700 // Maximum sensor distance is rated at 400-500cm.
//timeOut= 2*MAX_DISTANCE /100 /340 *1000000 = MAX_DISTANCE*58.8
float timeOut = MAX_DISTANCE * 60; 
int soundVelocity = 340; // define sound speed=340m/s
Servo myservo;  // create servo object to control a servo

int angulo;
int posVal = 90;    // variable to store the servo position
int servoPin = 2; // Servo motor pin
int contador=0;

const int pinPull=33;
const int lleno=10;

/*
 * note:If lcd1602 uses PCF8574T, IIC's address is 0x27,
 *      or lcd1602 uses PCF8574AT, IIC's address is 0x3F.
*/
LiquidCrystal_I2C lcd(0x27,16,2); 

void setup() {
  pinMode(trigPin,OUTPUT);// set trigPin to output mode
  pinMode(echoPin,INPUT); // set echoPin to input mode
  pinMode(pinPull,INPUT_PULLUP);
  Serial.begin(115200);   // Open serial monitor at 115200 baud to see ping results.



   // initialize digital pin LED_BUILTIN as an output.
  pinMode(PIN_LED, OUTPUT);
  pinMode(PIN_LED_2, OUTPUT);


   myservo.setPeriodHertz(50);           // standard 50 hz servo
  myservo.attach(servoPin, 500, 2500);  // attaches the servo on servoPin to the servo object
 myservo.write(posVal=90);  


  Wire.begin(SDA, SCL);           // attach the IIC pin
  if (!i2CAddrTest(0x27)) {
    lcd = LiquidCrystal_I2C(0x3F, 16, 2);
    }
  lcd.init();                     // LCD driver initialization
  lcd.backlight();                // Open the backlight
  lcd.setCursor(0,0);             // Move the cursor to row 0, column 0
  lcd.print("PARKING  "); lcd.print(contador);  lcd.print("/");lcd.print(lleno);     // The print content is displayed on the LCD
  
 
  lcd.setCursor(0,1);             // Move the cursor to row 0, column 0
  lcd.print("   Bienvenido   ");     // The print content is displayed on the LCD



}

void loop() {
  delay(100); // Wait 100ms between pings (about 20 pings/sec). 
  Serial.printf("Distance: ");
  Serial.print(getSonar()); // Send ping, get distance in cm and print result 
  Serial.println("");
 



 if(digitalRead(pinPull)==LOW) {
contador=0;

 digitalWrite(PIN_LED,HIGH);
 digitalWrite(PIN_LED_2,HIGH);
  myservo.write(180);       // tell servo to go to position in variable 'pos'
  delay(500);

lcd.setCursor(0,0);             // Move the cursor to row 0, column 0
  lcd.print("PARKING  "); lcd.print(contador);  lcd.print("/");lcd.print(lleno);  // The print content is displayed on the LCD

   myservo.write(90);       // tell servo to go to position in variable 'pos'
    delay(500);    
   
   

 digitalWrite(PIN_LED,LOW);
 digitalWrite(PIN_LED_2,LOW);

delay(500);

digitalWrite(PIN_LED,HIGH);
 digitalWrite(PIN_LED_2,HIGH);

delay(500);

digitalWrite(PIN_LED,LOW);
 digitalWrite(PIN_LED_2,LOW);

 }

}

float getSonar() {
  unsigned long pingTime;
  float distance;
 
  // make trigPin output high level lasting for 10μs to triger HC_SR04
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Wait HC-SR04 returning to the high level and measure out this waitting time
  pingTime = pulseIn(echoPin, HIGH, timeOut); 
  // calculate the distance according to the time
  distance = (float)pingTime * soundVelocity / 2 / 10000; 
 
 
 digitalWrite(PIN_LED,LOW);
 digitalWrite(PIN_LED_2,LOW);

if (distance < 10.00) { 
 
   
if (contador<lleno) {

 contador=contador+1;




       // The print content is displayed on the LCD
   lcd.setCursor(0,0);             // Move the cursor to row 0, column 0
  lcd.print("PARKING  "); lcd.print(contador); lcd.print("/");lcd.print(lleno);    // The print content is displayed on the LCD
  lcd.setCursor(0,1);             // Move the cursor to row 0, column 0
  lcd.print("Abriendo Barrera");     // The print content is displayed on the LCD

  digitalWrite(PIN_LED,HIGH);




 /*for (angulo=90;angulo<180;angulo++)
{

    myservo.write(angulo);   
  delay(5);
  
  myservo_2.write(angulo);  
}

 /* myservo.write(180);   */
 
    myservo.write(180);   
   
    delay(2000);    
 
            // tell servo to go to position in variable 'pos'

 
      // tell servo to go to position in variable 'pos'
 
  
  
    lcd.setCursor(0,1);             // Move the cursor to row 0, column 0
  lcd.print("Cerrando Barrera");     // The print content is displayed on the LCD

digitalWrite(PIN_LED,LOW);
 digitalWrite(PIN_LED_2, HIGH);   // turn the LED on (HIGH is the voltage level)
     delay(200);                       // wait for a second
     digitalWrite(PIN_LED_2, LOW);    // turn the LED off by making the voltage LOW
      delay(200);                       // wait for a second
        digitalWrite(PIN_LED_2, HIGH);   // turn the LED on (HIGH is the voltage level)
     delay(200);                       // wait for a second
     digitalWrite(PIN_LED_2, LOW);    // turn the LED off by making the voltage LOW
      delay(200);   
      digitalWrite(PIN_LED_2, HIGH);   // turn the LED on (HIGH is the voltage level)
     delay(200);                       // wait for a second
     digitalWrite(PIN_LED_2, LOW);    // turn the LED off by making the voltage LOW
      delay(200);                       // wait for a second
        digitalWrite(PIN_LED_2, HIGH);   // turn the LED on (HIGH is the voltage level)
     delay(200);                       // wait for a second
     digitalWrite(PIN_LED_2, LOW);    // turn the LED off by making the voltage LOW
      delay(200);   
       digitalWrite(PIN_LED_2, HIGH);   // turn the LED on (HIGH is the voltage level)
     delay(200);                       // wait for a second
     digitalWrite(PIN_LED_2, LOW);    // turn the LED off by making the voltage LOW
      delay(200);                       // wait for a second
    
      digitalWrite(PIN_LED_2, HIGH);   // turn the LED on (HIGH is the voltage level)
     delay(200);                       // wait for a second
     digitalWrite(PIN_LED_2, LOW);    // turn the LED off by making the voltage LOW
     delay(200);                       // wait for a second
      
    lcd.setCursor(0,1);             // Move the cursor to row 0, column 0
  lcd.print("");     // The print content is displayed on the LCD

 /*for (angulo=180;angulo>90;angulo--)
{

    myservo.write(angulo);   
  delay(7);

}*/

 myservo.write(90);       // tell servo to go to position in variable 'pos'
       // tell servo to go to position in variable 'pos'
 delay(500);

/*myservo_2.write(180);       // tell servo to go to position in variable 'pos'

delay(500);

myservo_2.write(0);       // tell servo to go to position in variable 'pos'*/

    /*myservo.write(90);  */ 

}






  }
  else{
    digitalWrite(PIN_LED,LOW);
    digitalWrite(PIN_LED_2,LOW);


  
  myservo.write(90);
  delay(5);
  


if (contador==lleno) 
{

 lcd.setCursor(0,1);             // Move the cursor to row 0, column 0
  lcd.print("    Completo    ");     // The print content is displayed on the LCD

digitalWrite(PIN_LED_2,HIGH);

}

else
{
 lcd.setCursor(0,1);             // Move the cursor to row 0, column 0
  lcd.print("   Bienvenido   ");     // The print content is displayed on the LCD
}  


  }
   
 return distance; // return the distance value

}

bool i2CAddrTest(uint8_t addr) {
  Wire.begin();
  Wire.beginTransmission(addr);
  if (Wire.endTransmission() == 0) {
    return true;
  }
  return false;
}