#include <Servo.h>
#include <Wire.h>
// Gy 521 Start
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}

//#define yprAnglesAndAcc
// GY521 end

//servo
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;

//DEFINE VARIABLES
int m1; //motor 1-2 = ROLL
int m2;
int m3; //motor 3-4 = PITCH
int m4;
int counter = 0;

bool first = true; //is this the first time looping?
//Variables for ROLL
int X_eV;             // Angle error (for roll)
float X_gV;           // For storing accelerometer data
float X_gVArray [5];  // Put accelerometer data in array
float X_oVH;          // Desiered angular velocity
float X_gVH;          // For storing Gyro data
float X_gVHArray [5]; // Put gyro data in array
float X_eVH;          // Angualar velocity error
float X_dKraft;       // For storing thrust difference between propellers
float X_gVHmed;       // Average angular velcoity
float X_gVmed;        // Average angle
//Variables for PITCH
int Y_eV;             // Angle error (for pitch)
float Y_gV;
float Y_gVArray [5];
float Y_oVH;
float Y_gVH;
float Y_gVHArray [5];
float Y_eVH;
float Y_dKraft;
float Y_gVHmed;
float Y_gVmed;

//Settings
float kp1 = 0.6;    //Set P-term to limit desired angular velocity
float kp2 = 1.5;    //Set P-term to limit difference between m1 and m2
int thrust = 1400;  //Set thrust
int maxVal = 1650;
int minVal = 1250;
int X_oV = 0;       //Set desired angle for ROLL
int Y_oV = 0;       //Set desired angle for PITCH


void setup()
{
  Serial.begin(250000);
  // GY521 Start
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
/*
  // wait for ready
  Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available());                 // wait for data
  while (Serial.available() && Serial.read()); // empty buffer again*/

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
  // GY521 end




  motor1.attach(3); // ESC pin
  motor2.attach(5); // ESC pin
  motor3.attach(6); // ESC pin !!!!WILL PROBABLY NEED TO CHANGE!!!
  motor4.attach(9); // ESC pin      <-- !!!!HERE TO!!!!
}


void loop() {
  //add starting frequence
  //init (current start freq)
  if (first) {
    for (int i = 1000; i <  1700; i += 1) {
      motor1.writeMicroseconds(i);
      motor2.writeMicroseconds(i);
      motor3.writeMicroseconds(i);
      motor4.writeMicroseconds(i);
      Serial.println(i);
      delay(25);
    }
    for (int j = 1700; j > 1390 ; j -= 1) {
      motor1.writeMicroseconds(j);
      motor2.writeMicroseconds(j);
      motor3.writeMicroseconds(j);
      motor4.writeMicroseconds(j);
      Serial.println(j);
      delay(25);
    }
    first = false;
  }
  while (true) {

    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
      // other program behavior stuff here
      // .
      // .
      // .
      // if you are really paranoid you can frequently test in between other
      // stuff to see if mpuInterrupt is true, and if so, "break;" from the
      // while() loop to immediately process the MPU data
      // .
      // .
      // .
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      mpu.resetFIFO();
      Serial.println(F("FIFO overflow!"));
    } else if (mpuIntStatus & 0x02) {
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      fifoCount -= packetSize;




      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      mpu.dmpGetAccel(&aa, fifoBuffer);
      mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
      mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
      //Read data from accelerometer and gyro
      X_gV = ypr[1] * 180 / M_PI; // roll
      Y_gV = ypr[2] * 180 / M_PI;  // Pitch
      //Save all accelerometer data in a array
      X_gVArray[counter % 5] = X_gV;
      Y_gVArray[counter % 5] = Y_gV;

      X_gVH = aaWorld.x / 10;   // Acc x
      Y_gVH = aaWorld.y / 10;   // acc y
      //Save all gyro data in a different array
      X_gVHArray[counter % 5] = X_gVH;
      Y_gVHArray[counter % 5] = Y_gVH;

      counter++;

      if (counter % 5 == 0) {
        //Compute for every five readings
        //Find average angle
        X_gVmed = (X_gVArray[0] + X_gVArray[1] + X_gVArray[2] + X_gVArray[3] + X_gVArray[4]) / 5;
        Y_gVmed = (Y_gVArray[0] + Y_gVArray[1] + Y_gVArray[2] + Y_gVArray[3] + Y_gVArray[4]) / 5;
        //Find avarage angular velocity
        X_gVHmed = (X_gVHArray[0] + X_gVHArray[1] + X_gVHArray[2] + X_gVHArray[3] + X_gVHArray[4]) / 5;
        Y_gVHmed = (Y_gVHArray[0] + Y_gVHArray[1] + Y_gVHArray[2] + Y_gVHArray[3] + Y_gVHArray[4]) / 5;
        //Error angle
        X_eV = X_oV - X_gVmed;
        Y_eV = Y_oV - Y_gVmed;
        //Set desired angular velocity
        X_oVH = X_eV * kp1;
        Y_oVH = Y_eV * kp1;

        //To avoid sign error:
        if (X_oVH >= 0) {
          X_eVH = X_oVH - X_gVHmed;
        }
        else {
          X_eVH = X_oVH + X_gVHmed;
        }
        if (Y_oVH >= 0) {
          Y_eVH = Y_oVH - Y_gVHmed;
        }
        else {
          Y_eVH = Y_oVH + Y_gVHmed;
        }

        //Thrust differential
        X_dKraft = X_eVH * kp2;
        Y_dKraft = Y_eVH * kp2;
        //Final thrust
        m1 = thrust + X_dKraft;
        m2 = thrust - X_dKraft;
        m3 = thrust + Y_dKraft;
        m4 = thrust - Y_dKraft;

        //Check if m1 exceeds the limit
        if (m1 > maxVal) {
          m1 = maxVal;
        }
        else if (m1 < minVal) {
          m1 = minVal;
        }
        //Check if m2 exceeds the limit
        if (m2 > maxVal) {
          m2 = maxVal;
        }
        else if (m2 < minVal) {
          m2 = minVal;
        }

        //Send PWM signals
        motor1.writeMicroseconds(m1);
        motor2.writeMicroseconds(m2);
        motor3.writeMicroseconds(m3);
        motor4.writeMicroseconds(m4);

      }
      if (counter % 10 == 0) {
        //Print pwm signals for motors, acc and gyro data for every 20 loop
        Serial.print("X : ");
        Serial.print(m1);
        Serial.print(" - ");
        Serial.print(m2);
        Serial.print(" - ");
        Serial.print(X_gV);
        Serial.print(" - ");
        Serial.print(X_gVH);
        Serial.print(" ");
        Serial.print("Y : ");
        Serial.print(m3);
        Serial.print(" - ");
        Serial.print(m4);
        Serial.print(" - ");
        Serial.print(Y_gV);
        Serial.print(" - ");
        Serial.print(Y_gVH);
        Serial.println(" ");
      }
    }
  }
}