#include <MPU6050.h>
#include <Arduino_BuiltIn.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>

int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z, acc_total_vector;
int temperature;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
long loop_timer;
int lcd_loop_counter;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output, calibrationValue;
bool calibrationState = true;
unsigned long calibrationTime;
unsigned long calibrationTimePrevious;
int sensitivitasPeringatan = 2;
int sensitivitasLarangan = 4;

LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  Wire.begin();
  pinMode(1, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);

  setup_mpu_6050_registers();

  lcd.begin();
  lcd.backlight();
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("  MPU-6050 IMU");
  lcd.setCursor(0, 1);
  lcd.print("     V1.0");

  delay(1500);
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("Calibrating gyro");
  lcd.setCursor(0, 1);
  for (int cal_int = 0; cal_int < 2000 ; cal_int ++) {
    if (cal_int % 125 == 0)lcd.print(".");
    read_mpu_6050_data();
    gyro_x_cal += gyro_x;
    gyro_y_cal += gyro_y;
    gyro_z_cal += gyro_z;
    delay(3);
  }
  gyro_x_cal /= 2000;
  gyro_y_cal /= 2000;
  gyro_z_cal /= 2000;

  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("Pitch:");
  lcd.setCursor(0, 1);
  lcd.print("Roll :");

  loop_timer = micros();
  calibrationTimePrevious = millis();
}

void loop() {
  calibrationTime = millis();
  readData();
  if ((calibrationState) && ((calibrationTime - calibrationTimePrevious) < 2000)) {
    lcd.setCursor(0, 0);
    lcd.print("Pitch:");
    lcd.setCursor(0, 1);
    lcd.print("Roll :");
  }
  if ((calibrationState) && ((calibrationTime - calibrationTimePrevious) > 2000)) {
    calibrationValue = angle_pitch_output;
    calibrationState = false;
  }
  (angle_pitch_output > (calibrationValue + sensitivitasPeringatan)) ? digitalWrite(10, HIGH) : digitalWrite(10, LOW);
  (angle_pitch_output > (calibrationValue + sensitivitasLarangan)) ? digitalWrite(9, HIGH), digitalWrite(10, LOW) : digitalWrite(9, LOW);
  ((angle_pitch_output < (calibrationValue + sensitivitasPeringatan)) && (angle_pitch_output > (calibrationValue - sensitivitasPeringatan))) ? digitalWrite(11, HIGH) : digitalWrite(11, LOW);
  (angle_pitch_output < (calibrationValue - sensitivitasPeringatan)) ? digitalWrite(12, HIGH) : digitalWrite(12, LOW);
  (angle_pitch_output < (calibrationValue - sensitivitasLarangan)) ? digitalWrite(13, HIGH), digitalWrite(12, LOW) : digitalWrite(13, LOW);

  if ((angle_pitch_output > (calibrationValue + sensitivitasLarangan)) || (angle_pitch_output < (calibrationValue - sensitivitasLarangan))) {
    digitalWrite(1, HIGH);
  }
  else {
    digitalWrite(1, LOW);
  }
}

void readData() {
  read_mpu_6050_data();

  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;

  //Gyro angle calculations
  //0.0000611 = 1 / (250Hz / 65.5)
  angle_pitch += gyro_x * 0.0000611;
  angle_roll += gyro_y * 0.0000611;

  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);

  //Accelerometer angle calculations
  acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z));
  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
  angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;
  angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;

  //Place the MPU-6050 spirit level and note the values in the following two lines for calibration
  angle_pitch_acc -= 0.0;
  angle_roll_acc -= 0.0;
  if (set_gyro_angles) {
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
  }
  else {
    angle_pitch = angle_pitch_acc;
    angle_roll = angle_roll_acc;
    set_gyro_angles = true;
  }

  //To dampen the pitch and roll angles a complementary filter is used
  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;
  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;

  write_LCD();
  while (micros() - loop_timer < 4000);
  loop_timer = micros();
}


void read_mpu_6050_data() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 14);
  while (Wire.available() < 14);
  acc_x = Wire.read() << 8 | Wire.read();
  acc_y = Wire.read() << 8 | Wire.read();
  acc_z = Wire.read() << 8 | Wire.read();
  temperature = Wire.read() << 8 | Wire.read();
  gyro_x = Wire.read() << 8 | Wire.read();
  gyro_y = Wire.read() << 8 | Wire.read();
  gyro_z = Wire.read() << 8 | Wire.read();

}

void write_LCD() {
  if (lcd_loop_counter == 14)lcd_loop_counter = 0;
  lcd_loop_counter ++;
  if (lcd_loop_counter == 1) {
    angle_pitch_buffer = angle_pitch_output * 10;
    lcd.setCursor(6, 0);
  }
  if (lcd_loop_counter == 2) {
    if (angle_pitch_buffer < 0)lcd.print("-");
    else lcd.print("+");
  }
  if (lcd_loop_counter == 3)lcd.print(abs(angle_pitch_buffer) / 1000);
  if (lcd_loop_counter == 4)lcd.print((abs(angle_pitch_buffer) / 100) % 10);
  if (lcd_loop_counter == 5)lcd.print((abs(angle_pitch_buffer) / 10) % 10);
  if (lcd_loop_counter == 6)lcd.print(".");
  if (lcd_loop_counter == 7)lcd.print(abs(angle_pitch_buffer) % 10);

  if (lcd_loop_counter == 8) {
    angle_roll_buffer = angle_roll_output * 10;
    lcd.setCursor(6, 1);
  }
  if (lcd_loop_counter == 9) {
    if (angle_roll_buffer < 0)lcd.print("-");
    else lcd.print("+");
  }
  if (lcd_loop_counter == 10)lcd.print(abs(angle_roll_buffer) / 1000);
  if (lcd_loop_counter == 11)lcd.print((abs(angle_roll_buffer) / 100) % 10);
  if (lcd_loop_counter == 12)lcd.print((abs(angle_roll_buffer) / 10) % 10);
  if (lcd_loop_counter == 13)lcd.print(".");
  if (lcd_loop_counter == 14)lcd.print(abs(angle_roll_buffer) % 10);
}

void setup_mpu_6050_registers() {
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  //Configure the accelerometer (+/-8g)
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission();
  //Configure the gyro (500dps full scale)
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();
}