#include "MPU9250.h"
#include "MPU9250_asukiaaa.h"
#include "ArduinoJson.h"

// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
float aX, aY, aZ, gX, gY, gZ, mX, mY, mZ;
String magnometer;
String accelarator;
String gyroscope;
String sensorID;

void setup() {
  // serial to display data
  Serial.begin(9600);
  while(!Serial) {}

  // start communication with IMU 
  status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {}
  }
  // setting the accelerometer full scale range to +/-8G 
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_8G);
  // setting the gyroscope full scale range to +/-500 deg/s
  IMU.setGyroRange(MPU9250::GYRO_RANGE_500DPS);
  // setting DLPF bandwidth to 20 Hz
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  // setting SRD to 19 for a 50 Hz update rate
  IMU.setSrd(19);
}

void loop() {

  StaticJsonDocument<200> accelarator;
  StaticJsonDocument<200> gyroscope;
  StaticJsonDocument<200> magnometer;
  // read the sensor
  IMU.readSensor();

  // display the data
  
//  Serial.print("Accel X "); Serial.print(IMU.getAccelX_mss());
//  Serial.print("\t");
//  Serial.print("Accel Y "); Serial.print(IMU.getAccelY_mss());
//  Serial.print("\t");
//  Serial.print("Accel Z ");  Serial.print(IMU.getAccelZ_mss());
//  Serial.print("\t");

    aX = IMU.getAccelX_mss();
    aY = IMU.getAccelY_mss();
    aZ = IMU.getAccelZ_mss();
    accelarator["sensor"] = "accelarator";
    accelarator["X"] = aX;
    accelarator["Y"] = aY;
    accelarator["Z"] = aZ;
    serializeJson(accelarator, Serial);
  
//  Serial.print("Gyro X ");  Serial.print(IMU.getGyroX_rads());
//  Serial.print("\t");
//  Serial.print("Gyro Y ");  Serial.print(IMU.getGyroY_rads());
//  Serial.print("\t");
//  Serial.print("Gyro Z ");  Serial.print(IMU.getGyroZ_rads());
//  Serial.print("\t");
//  Serial.print("Direction "); Serial.print(atan2(IMU.getMagX_uT(), IMU.getMagY_uT()) * 180 / PI);

     gX = IMU.getGyroX_rads();
     gY = IMU.getGyroY_rads();
     gZ = IMU.getGyroZ_rads();
     gyroscope["sensor"] = "gyroscope";
     gyroscope["X"] = gX;
     gyroscope["Y"] = gY;
     gyroscope["Z"] = gZ;
     serializeJson(gyroscope, Serial);
  
//  Serial.print("Magno X "); Serial.print(IMU.getMagX_uT(),6);
//  Serial.print("\t");
//  Serial.print("Magno Y "); Serial.print(IMU.getMagY_uT(),6);
//  Serial.print("\t");
//  Serial.print("Magno Z "); Serial.print(IMU.getMagZ_uT(),6);
//  Serial.println("\t");

     mX = IMU.getMagX_uT();
     mY = IMU.getMagY_uT();
     mZ = IMU.getMagZ_uT();
     magnometer["sensor"] = "magnometer";
     magnometer["X"] = gX;
     magnometer["Y"] = gY;
     magnometer["Z"] = gZ;
    serializeJson(magnometer, Serial);
    Serial.write("\n");
  //Serial.println(IMU.getTemperature_C(),6);
  delay(100);
}