// 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/

/* Build server in ESP */
#include <Arduino.h>
#if defined(ESP8266)
 /* ESP8266 Dependencies */
  #include <ESP8266WiFi.h>
  #include <ESPAsyncTCP.h>
  #include <ESPAsyncWebServer.h>
#elif defined(ESP32)
  /* ESP32 Dependencies */
  #include <WiFi.h>
  #include <AsyncTCP.h>
  #include <ESPAsyncWebServer.h>
#endif
#include <ESPDash.h>

/* Your SoftAP WiFi Credentials */
const char* ssid = "UwU"; // SSID
const char* password = ""; // Password

/* Start Webserver */
AsyncWebServer server(80);

/* Attach ESP-DASH to AsyncWebServer */
ESPDash dashboard(&server);

/* 
  Dashboard Cards 
  Format - (Dashboard Instance, Card Type, Card Name, Card Symbol(optional) )
*/
Card sliderSetPoint1(&dashboard, SLIDER_CARD, "Set Point 1", "", 0, 255);
Card sliderKp1(&dashboard, SLIDER_CARD, "KP 1", "", 0, 255);
Card sliderKi1(&dashboard, SLIDER_CARD, "KI 1", "", 0, 255);
Card sliderKd1(&dashboard, SLIDER_CARD, "KD 1", "", 0, 255);
Card sliderSetPoint2(&dashboard, SLIDER_CARD, "Set Point 2", "", 0, 255);
Card sliderKp2(&dashboard, SLIDER_CARD, "KP 2", "", 0, 255);
Card sliderKi2(&dashboard, SLIDER_CARD, "KI 2", "", 0, 255);
Card sliderKd2(&dashboard, SLIDER_CARD, "KD 2", "", 0, 255);
Card sliderSetPoint3(&dashboard, SLIDER_CARD, "Set Point 3", "", 0, 255);
Card sliderKp3(&dashboard, SLIDER_CARD, "KP 3", "", 0, 255);
Card sliderKi3(&dashboard, SLIDER_CARD, "KI 3", "", 0, 255);
Card sliderKd3(&dashboard, SLIDER_CARD, "KD 3", "", 0, 255);
Card Acceleration_X(&dashboard, GENERIC_CARD, "Acc X", "m/s²");
Card Acceleration_Y(&dashboard, GENERIC_CARD, "Acc Y", "m/s²");
Card Acceleration_Z(&dashboard, GENERIC_CARD, "Acc Z", "m/s²");
Card Gyro_X(&dashboard, GENERIC_CARD, "Gyro X", "rad/s");
Card Gyro_Y(&dashboard, GENERIC_CARD, "Gyro Y", "rad/s");
Card Gyro_Z(&dashboard, GENERIC_CARD, "Gyro Z", "rad/s");
Chart bar(&dashboard, BAR_CHART, "Respon PID");
String XAxis[] = {"Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat"};
int YAxis[] = {0, 0, 0, 0, 0, 0, 0};

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

Adafruit_MPU6050 mpu;

long AccX, AccY, AccZ;
double accAngleX, accAngleY, accAngleZ;
double accelPitch, accelRoll
int GyroX, GyroY, GyroZ;
double gyroRoll, gyroPitch, gyroYaw;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
//int16_t gyroRateX, gyroRateY, gyroRateZ;
//float gyroAngleX = 0, gyroAngleY = 0, gyroAngleZ = 0;
float rotation_x, rotation_y, rotation_z;
double freq, dt;
double tau = 0.98, roll = 0, pitch = 0;
long loopTimer, loopTimer2;

/*
// 250 deg/s --> 131.0, 500 deg/s --> 65.5, 1000 deg/s --> 32.8, 2000 deg/s --> 16.4
long scaleFactorGyro = 65.5;

// 2g --> 16384 , 4g --> 8192 , 8g --> 4096, 16g --> 2048
long scaleFactorAccel = 8192;
*/

int Kp1, Ki1, Kd1, Kp2, Ki2, Kd2, Kp3, Ki3, Kd3, set1, set2, set3;

/* Servo settings */
Servo servo1, servo2, servo3;

static const int servoPin1 = 12;
static const int servoPin2 = 13;
static const int servoPin3 = 14;

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

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

  // Setup the registers of the MPU-6050 and start up
  setup_mpu_6050_registers();

  /* Calibration */
  Serial.println("Calibrating gyro, place on level surface and do not move.");

  // Take 3000 readings for each coordinate and then find average offset
  for (int cal_int = 0; cal_int < 3000; cal_int ++){
    if(cal_int % 200 == 0)Serial.print(".");
    read_mpu_6050_data();
    gyro_x_cal += GyroX;
    gyro_y_cal += GyroY;
    gyro_z_cal += GyroZ;
    delay(3);
  }

  // Average the values
  gyro_x_cal /= 3000;
  gyro_y_cal /= 3000;
  gyro_z_cal /= 3000;

  // Reset the loop timer
  loopTimer = micros();
  loopTimer2 = micros();

  /* Start Access Point */
  WiFi.mode(WIFI_AP);
  WiFi.softAPConfig(IPAddress(192, 168, 4, 1), IPAddress(192, 168, 4, 1), IPAddress(255, 255, 255, 0));
  WiFi.softAP(ssid, password);
  Serial.print("IP Address: ");
  Serial.println(WiFi.softAPIP());

  /* Start AsyncWebServer */
  server.begin();

  /*
  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_250_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("");

  /* Servo pins */
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);

  delay(100);
}

void loop() {
  freq = 1/((micros() - loopTimer2) * 1e-6);
  loopTimer2 = micros();
  dt = 1/freq;

  // Read the raw acc data from MPU-6050
  read_mpu_6050_data();

  
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  /* Read 6 register total. Accelerometer x, y, z and Gyroscope x, y, z */
  /* Get the accelerometer x, y, z */
  AccX = a.acceleration.x;
  AccY = a.acceleration.y;
  AccZ = a.acceleration.z;

  /* Print out the values */
  Serial.print("Acceleration X: ");
  Serial.print(AccX);
  Serial.print(", Y: ");
  Serial.print(AccY);
  Serial.print(", Z: ");
  Serial.print(AccZ);
  Serial.println(" m/s^2");

  /* Get the Angle of Accelerometer x, y, z */
  accAngleX = atan2(AccY, AccZ)*180/PI;
  accAngleY = atan2(AccX, AccZ)*180/PI;
  accAngleZ = atan2(AccX, AccY)*180/PI;

  /* Print out the Accelerometer of Angle */
  if(isnan(accAngleX) || isnan(accAngleY) || isnan(accAngleZ));
  else{
    Serial.print("accAngle X: ");
    Serial.print(accAngleX);
    Serial.print(", Y: ");
    Serial.print(accAngleY);
    Serial.print(", Z: ");
    Serial.println(accAngleZ);
  }

  currTime = millis();
  loopTime = currTime - prevTime;
  prevTime = currTime;

  /* Get the Gyroscope x, y, z */  
  GyroX = g.gyro.x;
  GyroY = g.gyro.y;
  GyroZ = g.gyro.z;

  /* Calculating Yaw Roll and Pitch from the gyro data */
  Serial.print("Rotation X: ");
  Serial.print(GyroX);
  Serial.print(", Y: ");
  Serial.print(GyroY);
  Serial.print(", Z: ");
  Serial.print(GyroZ);
  Serial.println(" rad/s");

  /* Change the value of range from (-32768 ~ 32767) to (-250 ~ 250) */
  /*
  gyroRateX = map(GyroX, -32768, 32767, -250, 250);
  gyroRateY = map(GyroY, -32768, 32767, -250, 250);
  gyroRateZ = map(GyroZ, -32768, 32767, -250, 250);
  */

  /* Find the Angle of Gyro x, y, z */
  /*
  gyroAngleX = gyroAngleX + (float)gyroRateX*loopTime/1000;
  gyroAngleY = gyroAngleY + (float)gyroRateY*loopTime/1000;
  gyroAngleZ = gyroAngleZ + (float)gyroRateZ*loopTime/1000; 
  */ 

  /*
  Serial.print("GyroRate X: ");
  Serial.print((float)gyroRateX);
  Serial.print(" Y: ");
  Serial.print((float)gyroRateY);
  Serial.print(" Z: ");
  Serial.println((float)gyroRateZ);
  */
  
  /*
  Serial.print("GyroAngle X: ");
  Serial.print(gyroAngleX);
  Serial.print(" Y: ");
  Serial.print(gyroAngleY);
  Serial.print(" Z: ");
  Serial.println(gyroAngleZ);
  */
  
      
  /*
  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");
  */
  
  Serial.println("");
  
  /* Display Acceleration in the server */
  Acceleration_X.update(AccX);
  Acceleration_Y.update(AccY);
  Acceleration_Z.update(AccZ);  

  /* Display Gyroscope in the server */  
  Gyro_X.update(GyroX);
  Gyro_Y.update(GyroY);
  Gyro_Z.update(GyroZ);  
    
  /* Send Updates to our Dashboard (realtime) */
  dashboard.sendUpdates();

  /* Servo control */
  /*
  for (int pos = 0; pos <= 180; pos += 1) { // sweep from 0 degrees to 180 degrees
		// in steps of 1 degree
		servo1.write(pos);
		delay(20);             // waits 20ms for the servo to reach the position
	}
	for (int pos = 180; pos >= 0; pos -= 1) { // sweep from 180 degrees to 0 degrees
		servo1.write(pos);
		delay(20);
	}

  for (int pos = 0; pos <= 180; pos += 1) { // sweep from 0 degrees to 180 degrees
		// in steps of 1 degree
		servo2.write(pos);
		delay(20);             // waits 20ms for the servo to reach the position
	}
	for (int pos = 180; pos >= 0; pos -= 1) { // sweep from 180 degrees to 0 degrees
		servo2.write(pos);
		delay(20);
	}

  for (int pos = 0; pos <= 180; pos += 1) { // sweep from 0 degrees to 180 degrees
		// in steps of 1 degree
		servo3.write(pos);
		delay(20);             // waits 20ms for the servo to reach the position
	}
	for (int pos = 180; pos >= 0; pos -= 1) { // sweep from 180 degrees to 0 degrees
		servo3.write(pos);
		delay(20);
	}
  */
  
  delay(100);
}

void read_mpu_6050_data() {
  // Subroutine for reading the raw data
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 14);

  // Read data --> Temperature falls between acc and gyro registers
  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 setup_mpu_6050_registers() {
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  // Configure the accelerometer
  // Wire.write(0x__);
  // Wire.write; 2g --> 0x00, 4g --> 0x08, 8g --> 0x10, 16g --> 0x18
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x08);
  Wire.endTransmission();

  // Configure the gyro
  // Wire.write(0x__);
  // 250 deg/s --> 0x00, 500 deg/s --> 0x08, 1000 deg/s --> 0x10, 2000 deg/s --> 0x18
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();
}