//Agrega las librerias necesarias para que el codigo funcione

#include "Wire.h"
#include <MPU6050_light.h>

MPU6050 mpu(Wire);
unsigned long timer = 0;
#define ESPERA 250  //Tiempo de espera entre actualizaciones, en milisegundos

//Se definen los pines usados por el rele y los leds

#define PINRELAYA 8 //Pin 1 Relay Motor
#define PINRELAYB 9 //Pin 2 Relay Motor
#define PINRELAYC 10 //Pin 3 Relay Motor

#define PINLEDA 4 //Led Superior Izq
#define PINLEDB 3 //Led Superior Der
#define PINLEDC 5 //Led Inferior Izq
#define PINLEDD 6 //Led Inferior Der

double angulox = 0;
double anguloy = 0;
double anguloz = 0;

void setup() {

  Serial.begin(9600); //Inicia la comunicacion para leer valores por el puerto serie/usb
  Wire.begin();

  byte status = mpu.begin();
  Serial.print(F("Estado MPU6050: ")); //Indica el estado del sensor, 0 significa sin errores
  Serial.println(status);
  while (status != 0) { }

  Serial.println(F("Calibrando Sensor, No mover el dron"));
  delay(1000);
  //mpu.upsideDownMounting = true; //Descomentar si el sensor esta colocado al revez
  mpu.calcOffsets();
  Serial.println("Listo");

  pinMode(PINRELAYA, OUTPUT);
  pinMode(PINRELAYB, OUTPUT);
  pinMode(PINRELAYC, OUTPUT);

  pinMode(PINLEDA, OUTPUT);
  pinMode(PINLEDB, OUTPUT);
  pinMode(PINLEDC, OUTPUT);
  pinMode(PINLEDD, OUTPUT);

}

void loop() {

  mpu.update(); //Actualiza los datos del sensor MPU6050

  if ((millis() - timer) > ESPERA) {

    //Actualiza los valores recibidos del sensor y los almacena en las variables
    angulox = mpu.getAngleX();
    anguloy = mpu.getAngleY();
    anguloz = mpu.getAngleZ();

    //Escribe por el puerto serie los valores de cada eje

    Serial.print("X : ");
    Serial.print(angulox);
    Serial.print("\tY : ");
    Serial.print(anguloy);
    Serial.print("\tZ : ");
    Serial.println(anguloz);

    //Controla la inclinacion del dron
    //y enciende o apaga los leds segun sea necesario

    if ((anguloy > 3 && angulox < -3) || (anguloy > 4) || (angulox < -4)) {
      digitalWrite(PINLEDA, HIGH);
    } else {
      digitalWrite(PINLEDA, LOW);
    }

    if ((anguloy > 3 && angulox > 3) || (anguloy > 4) || (angulox > 4)) {
      digitalWrite(PINLEDB, HIGH);
    } else {
      digitalWrite(PINLEDB, LOW);
    }

    if ((anguloy < -3 && angulox < -3) || (anguloy < -4) || (angulox < -4)) {
      digitalWrite(PINLEDC, HIGH);
    } else {
      digitalWrite(PINLEDC, LOW);
    }

    if ((anguloy < -3 && angulox > 3) || (anguloy < -4) || (angulox > 4)) {
      digitalWrite(PINLEDD, HIGH);
    } else {
      digitalWrite(PINLEDD, LOW);
    }

    // Si el dron esta nivelado, todos los leds estan apagados
    // y los motores se encienden.

    if ((digitalRead(PINLEDA) + digitalRead(PINLEDB) + digitalRead(PINLEDC) + digitalRead(PINLEDD)) == 0) {
      digitalWrite(PINRELAYA, LOW);
      digitalWrite(PINRELAYB, LOW);
      digitalWrite(PINRELAYC, LOW);
    }

    else {
      digitalWrite(PINRELAYA, HIGH);
      digitalWrite(PINRELAYB, HIGH);
      digitalWrite(PINRELAYC, HIGH);
    }

    timer = millis();

  }
}