#include <Servo.h>
#include <Wire.h>
#include <I2Cdev.h>
#include <SPI.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include <SoftwareSerial.h>

SoftwareSerial BTSerial(12,13);

///////////////////////////////////MPU 6050 DMP 에서 추출 코드///////////////////////////////////////////
MPU6050 mpu;                           // mpu interface object

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

Quaternion q;
VectorFloat gravity;

float ypr[3];
float yprLast[3];
int16_t gyro[3];

volatile bool mpuInterrupt = false;
void dmpDataReady() {
  mpuInterrupt = true;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////

Servo front, back, left, right;
double T = 0.0045; // Loop time.

////////////////////////////X,Y 축 오프셋///////////////////////////////////////////////////////////////

double Yoffset =  8.45;
double Xoffset = -3.75;

int throttle = 0;
int throttle_real = 0;
int front_output = 0, back_output = 0, left_output = 0, right_output = 0;

//////////////////////////////////// PID 게인값 셋팅 ////////////////////////////////////////////////////

double P_angle_gain_y = 4; double P_gain_y = 1.255; double I_gain_y =0.012; double D_gain_y = 0.020;
double P_angle_gain_x = 4; double P_gain_x = 1.255; double I_gain_x =0; double D_gain_x = 0.00;

////////////////////////////////////이중 PID 선언////////////////////////////////////////////////////////

double error_y;
double error_pid_y, error_pid_y1;
double P_angle_pid_y;
double P_y, I_y, D_y, PID_y;
double desired_angle_y = 0 + Yoffset;

double error_x;
double error_pid_x, error_pid_x1;
double P_angle_pid_x;
double P_x, I_x, D_x, PID_x;
double desired_angle_x = 0 + Xoffset;

////////////////////////////////MPU 6050 dmp 라이브러리에서 추출한 코드/////////////////////////////////////////////////////////

void dmpsetup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
  Serial.begin(115200);
  while (!Serial);
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();
  mpu.setXGyroOffset(92);
  mpu.setYGyroOffset(21);
  mpu.setZGyroOffset(0);
  mpu.setZAccelOffset(1931);

  if (devStatus == 0) {
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  }
  else {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
}

void setup() {
  Serial.begin(115200);
  dmpsetup();

  servoset(); //initialize settings for each motors.
  BTSerial.begin(115200);
  Serial.setTimeout(15);

}
//////////////////MPU 6050 dmp 라이브러리에서 추출한 코드/////////////////////////

void dmploop() {
  if (!dmpReady)
    return;
  while (!mpuInterrupt && fifoCount < packetSize)
  {
  }
 
  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.dmpGetGyro(gyro, fifoBuffer);
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
   
    ypr[1] = (ypr[1] * 180 / M_PI); //x
    ypr[2] = (ypr[2] * 180 / M_PI); //y
    ypr[0] = (ypr[0] * 180 / M_PI); //z
    //gyro[0],gyro[1],gyro[2]//x,y,z 각속도값
  }
}

//////////////////////////////////이중 PID X축 연산 수식 코드/////////////////////////////////

void PID_control_y() {

  error_y = desired_angle_y - ypr[1]; //angle def
  P_angle_pid_y = P_angle_gain_y * error_y; //angle def + outer P control

  error_pid_y = P_angle_pid_y + gyro[1]; // Pcontrol_angle - angle rate = PID Goal

  P_y = error_pid_y * P_gain_y; // Inner P control
  D_y = (error_pid_y - error_pid_y1) / T * D_gain_y; // Inner D control
  I_y += (error_pid_y) * T * I_gain_y; // Inner I control
  I_y = constrain(I_y, -100, 100); // I control must be limited to prevent being jerk.

  PID_y = P_y + D_y + I_y;
  front_output = +PID_y + throttle;
  back_output =  -PID_y + throttle;

  front_output = constrain(front_output, 800, 1600);
  back_output = constrain(back_output, 800, 1600);

  front.writeMicroseconds(front_output);
  back.writeMicroseconds(back_output);

  error_pid_y1 = error_pid_y;
 
}

/////////////////////// 이중 PID X축 연산 수식 코드//////////////////////////////////////////////

void PID_control_x() {
  error_x = desired_angle_x - ypr[2]; //angle def
  P_angle_pid_x = P_angle_gain_x * error_x; //angle def + outer P control

  error_pid_x = P_angle_pid_x - gyro[0]; // Pcontrol_angle - angle rate = PID Goal

  P_x = error_pid_x * P_gain_x; // Inner P control
  D_x = (error_pid_x - error_pid_x1) / T * D_gain_x; // Inner D control
  I_x += (error_pid_x) * T * I_gain_x; // Inner I control
  I_x = constrain(I_x, -100, 100); // I control must be limited to prevent being jerk.

  PID_x = P_x + D_x + I_x;
 left_output =  +PID_x + throttle;
 right_output =  -PID_x + throttle;

 left_output = constrain(left_output, 800, 1600);
 right_output = constrain(right_output, 800, 1600);

 left.writeMicroseconds((left_output));
 right.writeMicroseconds((right_output));

  error_pid_x1 = error_pid_x;
 
}

////////////////////////////문제의 통신 코드/////////////////////////////////////////////

void BTSerialEvent()
{  
     if(BTSerial.available()){
        throttle_real = BTSerial.parseInt();
        BTSerial.setTimeout(15);
        
    }
    if (throttle_real > 1600 || throttle_real < 800 ) {
        throttle_real = throttle;
    }

    if (throttle_real > 900 && throttle_real < 1600) {
        throttle = throttle_real;
    }
  
  }

///////////////////////////////////////////////////////////////////////////////////////////

void loop() {
 
  dmploop(); // MPU6050 센서 값 읽어오는거 반복
 
  BTSerialEvent();

  if (throttle_real == 801 ) {
    stopped();
  }

  if (throttle_real > 810 && throttle_real < 900 )
    stopped();
  }

  if (throttle_real == 802 ) { // up
   desired_angle_x = 0 + Xoffset; //initialize other direction angle
   desired_angle_y = 5 + Yoffset;
   PID_control_x();
   PID_control_y();
  }
  if (throttle_real == 803 ) { // down
   desired_angle_x = 0 + Xoffset; //initialize other direction angle
   desired_angle_y = -5 + Yoffset;
   PID_control_x();
   PID_control_y();
  }
  if (throttle_real == 804 ) { // right
   desired_angle_y = 0 + Yoffset; //initialize other direction angle
   desired_angle_x = -5 + Xoffset;
   PID_control_x();
   PID_control_y();
  }
  if (throttle_real == 805 ) { // left
    desired_angle_y = 0 + Yoffset; //initialize other direction angle
    desired_angle_x = 5 + Xoffset;
    PID_control_x();
    PID_control_y();
  }

  if (throttle_real == 806 ) { // hovering
    desired_angle_x = 0 + Xoffset;
    desired_angle_y = 0 + Yoffset;
    PID_control_x();
    PID_control_y();
  }

  if (throttle_real >= 900) { // throttle_real 값이 900 넘으면 pid 제어 실행
    PID_control_x();
    PID_control_y();
  }

    Serial.print(throttle);
    Serial.print('\t');
    Serial.print(throttle_real);

    Serial.print('\t');
    Serial.print(ypr[1]); //ypr[1]이 y고, 2가 x다.
    Serial.print('\t');
    Serial.print(ypr[2]);
    Serial.print('\t');
    Serial.print(ypr[3]);
    Serial.print('\t');
    Serial.print(front_output);
    Serial.print('\t');
    Serial.print(back_output);
    Serial.print('\t');
    Serial.print(right_output);
    Serial.print('\t');
    Serial.print(left_output);
    Serial.print('\t');
    Serial.print(-gyro[0]);
    Serial.print('\t');
    Serial.print(-gyro[1]);
    Serial.println('\t');

  
 
}

////////////////////서보모터 멈추게할 설정 ///////////////////////////////////

void stopped() {
  front.writeMicroseconds(800);
  back.writeMicroseconds(800);
  right.writeMicroseconds(800);
  left.writeMicroseconds(800);
}

//////////// 서보모터 사용을 위한 설정 ////////////////////////////////////////

void servoset() {
  front.attach(5);
  back.attach(10);
  right.attach(11);
  left.attach(6);

 front.writeMicroseconds(800);
  back.writeMicroseconds(800);
 right.writeMicroseconds(800);
  left.writeMicroseconds(800);
}