#include <Wire.h>
#include <LCDI2C_Multilingual.h> //Added to manage LCD
#include <ESP32Servo.h>

/* This circuit is an automated landing gear system for a model aircraft, utilizing an ESP32 microcontroller to 
control two servos based on input from an ultrasonic sensor and a toggle switch. 
The system displays distance measurements and gear status on a 16x2 LCD screen via an I2C interface.*/


// Pin definitions
#define TRIG_PIN 5
#define ECHO_PIN 18
#define SWITCH_PIN 2
#define SERVO1_PIN 4
#define SERVO2_PIN 12
#define I2C_SDA 21
#define I2C_SCL 22

// Create objects
LCDI2C_Latin lcd =  LiquidCrystal_I2C_Latin(0x27, 16, 2); // LCDI2C_Generic
Servo servo1;
Servo servo2;

void start_measure()
{
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
}


void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Initialize LCD
  lcd.init();
  lcd.clear(); //LCD Clear
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Landing Gear");

  // Initialize ultrasonic sensor pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Initialize switch pin
  pinMode(SWITCH_PIN, INPUT);

  // Attach servos
  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);

  // Set initial servo positions
  servo1.write(0); // Retracted
  servo2.write(0); // Retracted
}

void loop() {
  // Read distance from ultrasonic sensor
  long duration, distance;
  start_measure(); // Start measure with SR04M, using trigPin

  duration = pulseIn(ECHO_PIN, HIGH);
  distance = (duration / 2) / 29.1; //0.0344

  // Display distance on LCD
  lcd.clear(); //LCD Clear
  lcd.setCursor(0, 0);
  lcd.print("Landing Gear");
  lcd.setCursor(0, 1);
  lcd.print("Dist: ");
  lcd.print(distance);
  lcd.print(" cm");
  delay(1500);
  Serial.print("Distance :" + String(distance));
  Serial.println(" cm.");

  // Check switch state
  int switchState = digitalRead(SWITCH_PIN);

  // Control servos based on distance and switch state
  if (switchState == HIGH || distance < 10) {
    // Extend landing gear
    servo1.write(90);
    servo2.write(90);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Landing Gear");
    lcd.setCursor(0, 1);
    lcd.print("Gear: Extended");
    Serial.println("Gear Extended!!!");
    delay (2000);
  } else {
    // Retract landing gear
    servo1.write(0);
    servo2.write(0);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Landing Gear");
    lcd.setCursor(0, 1);
    lcd.print("Gear: Retracted");
    Serial.println("Gear Retracted!!!");
    delay (2000);
  }

  delay(100);
}