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

Adafruit_MPU6050 mpu;

// inisialisasi variabel
float velX, velY = 0;
float posX, posY = 0;
float gyroX, gyroY = 0;
int turnTime;
bool miring = false;
unsigned long currentTime = 0;
float samplingPeriod = 0.01;
float consoleDelay = 1;//Dalam Satuan Sekon

// inisialisasi untuk membaca data dari sensor
sensors_event_t accel, gyro, temp;

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

//rumus penghitung jarak dan kecepatan
void positionX(){
  velX = velX + accel.acceleration.x * samplingPeriod;
  posX = posX + velX * samplingPeriod;
}
void positionY(){
  velY = velY + accel.acceleration.y * samplingPeriod;
  posY = posY + velY * samplingPeriod;
}

void loop(void) {
  // Ambil data untuk Percepatan
  mpu.getAccelerometerSensor()->getEvent(&accel);
  // Ambil data untuk Kemiringan
  mpu.getGyroSensor()->getEvent(&gyro);
  // Ambil data untuk Suhu
  mpu.getTemperatureSensor()->getEvent(&temp);
  delay(samplingPeriod*1100); //kompenasai delay kalkulasi serta
  //sensor agar hasil mendekati analitik
  positionX();
  positionY();

  // formula untuk mendapatkan nilai kemiringan
  gyroX = gyro.gyro.x / 3.14 * 180;
  gyroY = gyro.gyro.y / 3.14 * 180;

  if( (posY > 1 || posY < -1) && !miring){
    logData();
    miring = true;
  }
  if(millis() > currentTime){
    logData();
    currentTime += (1000*consoleDelay);
  }
}

// fungsi untuk menampilkan data
void logData() {
  Serial.println();
  Serial.print("===== Data Reactics E-Car =====\n");

  // tampilkan data waktu
  Serial.print("Waktu: ");
  Serial.print(millis());
  Serial.print(" ms\n");

  // tampilkan data akselerasi
  Serial.print("Akselerasi X: ");
  Serial.print(accel.acceleration.x);
  Serial.print(", Y: ");
  Serial.println(accel.acceleration.y);

  // tampilkan data suhu
  Serial.print("Suhu: ");
  Serial.print(temp.temperature);
  Serial.println(" Celcius");

  // tampilkan data keceparan
  Serial.print("Kecepatan X: ");
  Serial.print(velX);
  Serial.print(", Y: ");
  Serial.println(velY);

  // tampilkan data posisi
  Serial.print("Posisi X: ");
  Serial.print(posX);
  Serial.print(", Y: ");
  Serial.println(posY);

  // tampilkan data kemiringan
  Serial.print("Kemiringan X: ");
  Serial.print(gyroX);
  Serial.print(", Y: ");
  Serial.println(gyroY);

  // warning message apabila mobil berjalan miring
  if(posY > 0.005 || posY < -0.005) {
    Serial.print("WARNING!!! Mobil berjalan melenceng!!!\n");
    Serial.print("TIME AT: ");
    Serial.print(millis());
    Serial.print(" milliseconds");
    Serial.println();
  }

  // warning message apabila wahana mobil dalam posisi miring terhadap sumbu X
  if((gyroX > 7.5 || gyroX < -7.5))  {
    Serial.print("WARNING!! Mobil dalam keadaaan miring!!!");
    Serial.print("Dengan kemiringan: ");
    Serial.print(gyroX);
    Serial.print(" dalam sumbu X");
    Serial.println();
  }

  // warning message apabila wahana mobil dalam posisi miring terhadap sumbu X
  if((gyroY > 7.5 || gyroY < -7.5))  {
    Serial.print("WARNING!! Mobil dalam keadaaan miring!!!");
    Serial.print("Dengan kemiringan: ");
    Serial.print(gyroY);
    Serial.print(" dalam sumbu Y");
    Serial.println();
  }
}