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

// Define pins
#define SCL_DRV 2
#define TRIG_SENSOR_ABAJO 4
#define ECHO_SENSOR_ABAJO 16 // RX2
#define ELECTROIMAN 17 // TX2
#define SERVO_PIN 5
#define SDA_MPU 19
#define SCL_MPU 21
#define TRIG_SENSOR_IZQUIERDA 14
#define ECHO_SENSOR_IZQUIERDA 27
#define TRIG_SENSOR_DERECHA 26
#define ECHO_SENSOR_DERECHA 25
#define LED_PIN 2

// Create objects
Servo myServo;
MPU9250 mpu;

// Function to measure distance
long measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2;

  return distance;
}

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    delay(10); // Wait for serial initialization
  }

  Serial.println("Initializing components...");

  // Initialize Servo
  myServo.attach(SERVO_PIN);

  // Initialize pins
  pinMode(TRIG_SENSOR_ABAJO, OUTPUT);
  pinMode(ECHO_SENSOR_ABAJO, INPUT);
  pinMode(TRIG_SENSOR_IZQUIERDA, OUTPUT);
  pinMode(ECHO_SENSOR_IZQUIERDA, INPUT);
  pinMode(TRIG_SENSOR_DERECHA, OUTPUT);
  pinMode(ECHO_SENSOR_DERECHA, INPUT);
  pinMode(ELECTROIMAN, OUTPUT);
  pinMode(LED_PIN, OUTPUT); // Initialize LED pin

  // Initialize I2C for MPU9250
  Wire.begin(SDA_MPU, SCL_MPU);
  if (!mpu.setup(0x68)) {
    Serial.println("MPU9250 initialization failed.");
    while (1);
  }
  Serial.println("MPU9250 initialized.");

  Serial.println("Vehicle Control Initialized.");
  displayMenu();
}

void loop() {
  handleUserInput();
}

/*
 * Display the interactive menu
 */
void displayMenu() {
  Serial.println("Choose an option:");
  Serial.println("1. Measure Distance (Abajo)");
  Serial.println("2. Measure Distance (Izquierda)");
  Serial.println("3. Measure Distance (Derecha)");
  Serial.println("4. Read MPU9250 Data");
  Serial.println("5. Control Electromagnet");
  Serial.println("6. Control Servo");
  Serial.println("7. Exit");
  Serial.print("Enter your choice: ");
}

/*
 * Handle user input and control the vehicle accordingly
 */
void handleUserInput() {
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    input.trim(); // Remove any whitespace characters
    int choice = input.toInt();

    switch (choice) {
      case 1:
        Serial.print("Distance (Abajo): ");
        Serial.print(measureDistance(TRIG_SENSOR_ABAJO, ECHO_SENSOR_ABAJO));
        Serial.println(" cm");
        break;
      case 2:
        Serial.print("Distance (Izquierda): ");
        Serial.print(measureDistance(TRIG_SENSOR_IZQUIERDA, ECHO_SENSOR_IZQUIERDA));
        Serial.println(" cm");
        break;
      case 3:
        Serial.print("Distance (Derecha): ");
        Serial.print(measureDistance(TRIG_SENSOR_DERECHA, ECHO_SENSOR_DERECHA));
        Serial.println(" cm");
        break;
      case 4:
        if (mpu.update()) {
          Serial.print("Accel X: "); Serial.print(mpu.getAccX());
          Serial.print(" Y: "); Serial.print(mpu.getAccY());
          Serial.print(" Z: "); Serial.println(mpu.getAccZ());
        } else {
          Serial.println("Failed to read MPU9250 data.");
        }
        break;
      case 5:
        controlElectromagnet();
        break;
      case 6:
        controlServo();
        break;
      case 7:
        Serial.println("Exiting...");
        while (true) {} // Stop the program
        break;
      default:
        Serial.println("Invalid choice. Please select again.");
        break;
    }

    delay(2000); // Wait 2 seconds before showing the menu again
    displayMenu(); // Display the menu again after handling the input
  }
}

/*
 * Control Electromagnet
 */
void controlElectromagnet() {
  Serial.println("Electromagnet Control:");
  Serial.println("1. Turn ON");
  Serial.println("2. Turn OFF");

  while (Serial.available() == 0) {} // Wait for user input
  String input = Serial.readStringUntil('\n');
  input.trim(); // Remove any whitespace characters
  int electromagnetChoice = input.toInt();

  if (electromagnetChoice == 1) {
    digitalWrite(ELECTROIMAN, HIGH);
    Serial.println("Electromagnet is ON");
  } else if (electromagnetChoice == 2) {
    digitalWrite(ELECTROIMAN, LOW);
    Serial.println("Electromagnet is OFF");
  } else {
    Serial.println("Invalid choice. Returning to main menu.");
  }
}

/*
 * Control Servo
 */
void controlServo() {
  Serial.println("Enter Servo angle (0 to 180):");
  while (Serial.available() == 0) {} // Wait for user input
  String input = Serial.readStringUntil('\n');
  input.trim(); // Remove any whitespace characters
  int angle = input.toInt();

  if (angle >= 0 && angle <= 180) {
    myServo.write(angle);
    Serial.print("Servo moved to ");
    Serial.print(angle);
    Serial.println(" degrees");
  } else {
    Serial.println("Invalid angle. Please enter a value between 0 and 180.");
  }
}