#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;

int redLED = 13;
int blueLED = 12;
int greenLED = 8;
int yellowLED = 7;
int pulseWidthMid = 1000;
int redLEDPeriod = pulseWidthMid;
int blueLEDPeriod = pulseWidthMid;
int greenLEDPeriod = pulseWidthMid;
int yellowLEDPeriod = pulseWidthMid;
int pulseScaler = 100;
int maxPeriod = 2000;
int currentTime = 0;

void setup(void) {
  Serial.begin(9600);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 Initializing");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (true) {
      delay(1000);
    }
  }
  Serial.println("MPU6050 initialized");
  delay(1000);

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  //mpu.getAccelerometerRange() is a two letter string
  
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG); 
  //250, 500, 1000, 2000

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  //5, 10, 21, 44, 34, 184, 260


  Serial.println("");
  delay(100);

  pinMode(redLED, OUTPUT);
  pinMode(blueLED, OUTPUT);
  pinMode(greenLED, OUTPUT);
  pinMode(yellowLED, OUTPUT);
}

void loop() {

  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  /* Print out the values */
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");

  Serial.println("");


  if (temp.temperature > 60) {
    Serial.println("Temperature too high. Emergencylanding");
  }

  blueLEDPeriod = pulseWidthMid + int(a.acceleration.x * pulseScaler);
  redLEDPeriod = pulseWidthMid + int(a.acceleration.x * pulseScaler);
  greenLEDPeriod = pulseWidthMid - int(a.acceleration.x * pulseScaler);
  yellowLEDPeriod = pulseWidthMid - int(a.acceleration.x * pulseScaler);

  int newTime = micros();
  int deltaTime = newTime - currentTime;
  if (deltaTime < maxPeriod) {
    delay(maxPeriod - deltaTime);
  }
  currentTime = micros();

  digitalWrite(redLED, HIGH);
  digitalWrite(blueLED, HIGH);
  digitalWrite(greenLED, HIGH);
  digitalWrite(yellowLED, HIGH);

  delay(redLEDPeriod);
  digitalWrite(redLED, LOW);

  
  delay(blueLEDPeriod);
  digitalWrite(blueLED, LOW);

  
  delay(greenLEDPeriod);
  digitalWrite(greenLED, LOW);

  
  delay(yellowLEDPeriod);
  digitalWrite(yellowLED, LOW);
}