// Basic demo for accelerometer readings from Adafruit MPU6050

// ESP32 Guide: https://RandomNerdTutorials.com/esp32-mpu-6050-accelerometer-gyroscope-arduino/
// ESP8266 Guide: https://RandomNerdTutorials.com/esp8266-nodemcu-mpu-6050-accelerometer-gyroscope-arduino/
// Arduino Guide: https://RandomNerdTutorials.com/arduino-mpu-6050-accelerometer-gyroscope/

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include "SPI.h"
#include "Adafruit_ILI9341.h"

#define DC 9
#define CS 10
Adafruit_ILI9341 tft = Adafruit_ILI9341(CS, DC);


float angle_x = 0;
float angle_y = 0;
float angle_z = 0;

Adafruit_MPU6050 mpu;

void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  tft.begin();
//  tft.setCursor(20, 50);
//  tft.setTextColor(ILI9341_WHITE);
//  tft.setTextSize(3);
//  tft.println("TUGAS AKHIR");

//  tft.setCursor(33, 90);
//  tft.setTextColor(ILI9341_WHITE);
//  tft.setTextSize(2);
//  tft.println("RIZKI TRI MUKTI");
//  tft.setCursor(75, 120);
//  tft.println("A021055");
//  delay(1000);
  tft.fillScreen(ILI9341_BLACK);
  

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  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_500_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_5_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() {
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  /* Print out the values */
  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");

  angle_x = atan(a.acceleration.x / sqrt(a.acceleration.y*a.acceleration.y + a.acceleration.z*a.acceleration.z))*1/(3.142/180);
  angle_y = atan(a.acceleration.y / sqrt(a.acceleration.x*a.acceleration.x + a.acceleration.z*a.acceleration.z))*1/(3.142/180);
  angle_z = atan(sqrt(a.acceleration.x*a.acceleration.x + a.acceleration.y*a.acceleration.y)/a.acceleration.z)*(180/3.142);
  
  Serial.print("angle_x :");
  Serial.print(angle_x);
  Serial.println(" deg");

  Serial.print("angle_y :");
  Serial.print(angle_y);
  Serial.println(" deg");

  // calibrate angle_z
  float cal_angle = 0;
  float m_angle = 1;
  float c_angle = 0;

  cal_angle = angle_z*m_angle + c_angle;
  Serial.print("angle_z :");
  Serial.print(cal_angle);
  Serial.println(" deg");

  Serial.println("");
  int pressure = analogRead(A0);
  float cal_pressure = 0;
  float m_pressure = 1;
  float c_pressure = 0;

  // print out the value you read:
  Serial.print("Pressure: ");
  pressure=map(pressure, 0, 1023, 0, 300);

  // calibrate pressure transducer
  cal_pressure = m_pressure*pressure + c_pressure;
  Serial.print(cal_pressure); //Voltage to pressure
  Serial.println(" bar");
  delay(500);

  tft.setCursor(30, 50);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(3);
  tft.println("PENGUKURAN");

  tft.setCursor(20, 95);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print("Sudut X :");
  tft.print(angle_x);
  tft.println(" deg");

  tft.setCursor(20, 115);
  tft.print("Sudut Y :");
  tft.print(angle_y);
  tft.println(" deg");

  tft.setCursor(20, 135);
  tft.print("Tekanan :");
  tft.print(cal_pressure);
  tft.println(" bar");
  delay(500); 
tft.fillScreen(ILI9341_BLACK);
  
}