/*
Anggota:
1. 21/474701/TK/52375 Herbert Imanuel Purba
2. 21/481223/TK/53116 Mushab Husein Isnan
3. 21/482715/TK/53347 Afif Irfan Zein
4. 21/474195/TK/52298 Dzaky Ahmad S
5. 21/474299/TK/52322 Wildan Abizar Muhammad
*/

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

/* Deklarasi Global Variabel dan Class */
Adafruit_MPU6050 mpu;

float yaw = 0;
float vx = 0, vy = 0;
float s = 0, sx = 0, sy = 0;

void setup(void) {
  /* Set Up Sensor MPU6050 */
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens
  Serial.println("Adafruit MPU6050 test!");

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

  Serial.println("MPU6050 Found!");

  //setting sensor akselerometer MPU6050
  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;
  }

  //setting sensor gyro MPU6050
  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;
  }

  //setting filter MPU6050
  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;
  }
  
  delay(100);
}

void loop() {
  /* Pengambilan Data */
  //ambil data dari sensor
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  //periode pengambilan data (ms)
  float dt = 1000;

  /* Print Data yang Telah Diambil */
  Serial.println("===========================================================");
  Serial.print("Acceleration \tX: ");
  Serial.print(a.acceleration.x);
  Serial.print(", \tY: ");
  Serial.print(a.acceleration.y);
  Serial.print(", \tZ: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation \tX: ");
  Serial.print(g.gyro.x);
  Serial.print(", \tY: ");
  Serial.print(g.gyro.y);
  Serial.print(", \tZ: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");


  //kalkulasi yaw
  yaw = yaw + 180/PI*g.gyro.z*dt/1000;

  //kalkulasi kecepatan menggunakan percepatan
  vx = vx + a.acceleration.x*dt/(9.81*1000);
  vy = vy + a.acceleration.y*dt/(9.81*1000);
  float vtot = sqrt(vx*vx+vy*vy);

  /* Kalkulasi Perpindahan dan Jarak */
  //variabel data sementara untuk perhitungan
  float sxtemp = sx;
  float sytemp = sy;

  //kalkulasi perpindahan dan jarak
  sx = sx + vx*dt/1000;
  sy = sy + vy*dt/1000;
  float perpindahan = sqrt(sx*sx+sy*sy);
  s = s + sqrt((sx-sxtemp)*(sx-sxtemp) + (sy-sytemp)*(sy-sytemp));

  /* Print Data yang Telah Dikalkulasi */
  Serial.print("Yaw: \t");
  Serial.print(yaw);
  Serial.println(" deg");

  Serial.print("Velocity \tX: ");
  Serial.print(vx);
  Serial.print(" m/s");

  Serial.print(", \tY: ");
  Serial.print(vy);
  Serial.print(" m/s");

  Serial.print(", \tTotal: ");
  Serial.print(vtot);
  Serial.println(" m/s");
  
  Serial.print("Position \tX: ");
  Serial.print(sx);
  Serial.print(" m");

  Serial.print(", \tY: ");
  Serial.print(sy);
  Serial.println(" m");

  Serial.print("Distance from initial point (Displacement): \t");
  Serial.print(perpindahan);
  Serial.println(" m");

  Serial.print("Distance Traveled from initial point: \t");
  Serial.print(s);
  Serial.println(" m");

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

  Serial.println("===========================================================");



  delay(dt);
}