#include "MPU6050.h"
#include "I2Cdev.h"
#include "Wire.h"
// #include "ServoESP32.h"
#include <Servo.h>

MPU6050 sensor;
Servo miServo;


int Trigger = 5;
int Echo = 18;
int pin_cuatro = 4;
int pin_dos = 2;
int16_t ax, ay, az;
int16_t gx, gy, gz;


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  pinMode(Trigger, OUTPUT); 
  pinMode(pin_dos, OUTPUT);
  pinMode(pin_cuatro, OUTPUT);//pin como salida
  pinMode(Echo, INPUT);  //pin como entrada
  digitalWrite(Trigger, LOW);
  //Inicializamos el pin con 0
  Wire.begin();           //Iniciando I2C  
  sensor.initialize();    //Iniciando el sensor

  if (sensor.testConnection()) Serial.println("Sensor iniciado correctamente");
  else Serial.println("Error al iniciar el sensor");

  miServo.attach(33);  // El número de pin puede variar dependiendo del modelo exacto de tu ESP32.

  // Ajusta la frecuencia de actualización del Servo
  // miServo.setPWM(50);  // 50 Hz (ajusta según sea necesario)
}

void loop() {
  long t; //timepo que demora en llegar el eco
  long d; //distancia en centimetros

  sensor.getAcceleration(&ax, &ay, &az);
  sensor.getRotation(&gx, &gy, &gz);

  Serial.print("a[x y z] g[x y z]:\t");
  Serial.print(ax); Serial.print("\t");
  Serial.print(ay); Serial.print("\t");
  Serial.print(az); Serial.print("\t");
  Serial.print(gx); Serial.print("\t");
  Serial.print(gy); Serial.print("\t");
  Serial.println(gz);

  digitalWrite(Trigger, HIGH);
  delayMicroseconds(10);          //Enviamos un pulso de 10us
  digitalWrite(Trigger, LOW);
  
  t = pulseIn(Echo, HIGH); //obtenemos el ancho del pulso
  d = t/59;             //escalamos el tiempo a una distancia en cm
  
  Serial.print("Distancia: ");
  Serial.print(d);      //Enviamos serialmente el valor de la distancia
  Serial.print("cm");
  Serial.println();       

  if (d < 100){
     //Encendemos el led
    digitalWrite(pin_cuatro, LOW);
    digitalWrite(pin_dos, HIGH);
  }else{
    digitalWrite(pin_dos, LOW);
    digitalWrite(pin_cuatro, HIGH);
  }


  int servoValue = map(ay, -16384, 16383, 0, 180);
  miServo.write(servoValue);

  delay(10);  
}