#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
// #incdlue <Wire.h>
Adafruit_MPU6050 mpu;

#include <Servo.h>
Servo servoX, servoY, servoZ;
long sensorValueX = 0, sensorValueY = 0, sensorValueZ = 0;
long xAngle = 0, yAngle = 0, zAngle = 0;
long countAngle = 60;

byte ServoX = 7, ServoY = 6, ServoZ = 5;

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

  lcd.init();
  lcd.backlight();

  servoX.attach(ServoX);
  servoY.attach(ServoY);
  servoZ.attach(ServoZ);

  while(!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(10);
  }
  Serial.println("Adafruit MPU6050 test!");

  if(!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while(1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch(mpu.getAccelerometerRange()) {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
  }

  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  Serial.print("Gyro range set to: ");
  switch(mpu.getGyroRange()) {
    case MPU6050_RANGE_250_DEG:
      Serial.println("+- 250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+- 500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+- 1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+- 2000 deg/s");
      break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch(mpu.getFilterBandwidth()) {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 HZ");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 HZ");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 HZ");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 HZ");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 HZ");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 HZ");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 HZ");
      break;
  }

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

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  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");

  if(xAngle >= 180) xAngle = 0;
  if(yAngle >= 180) yAngle = 0;
  if(zAngle >= 180) zAngle = 0;

  // 進行積分
  xAngle = xAngle + g.gyro.x * 0.5 * 57;
  yAngle = yAngle + g.gyro.y * 0.5 * 57;
  zAngle = zAngle + g.gyro.z * 0.5 * 57;
  // servo顯示
  servoX.write(xAngle);
  servoY.write(yAngle);
  servoZ.write(zAngle);
  // 計算角度
  countAngle = countAngle + xAngle + yAngle + zAngle;

  lcd.setCursor(0, 0); //第一排
  lcd.print("X: ");
  lcd.print(xAngle);
  lcd.print(" deg");

  lcd.setCursor(0, 1); //第二排
  lcd.print("Y: ");
  lcd.print(yAngle);
  lcd.print(" deg");

  lcd.setCursor(0, 2); //第三排
  lcd.print("Z: ");
  lcd.print(zAngle);
  lcd.print(" deg");

  lcd.setCursor(0, 3); //第四排
  lcd.print("total degree: ");
  lcd.print(countAngle);
  lcd.print(" deg");

  Serial.println("");
  delay(500);
}