#include <MPU9250_asukiaaa.h>
#include <ArduinoJson.h>
#ifdef _ESP32_HAL_I2C_H_
#define SDA_PIN 4
#define SCL_PIN 5
#endif

MPU9250_asukiaaa mySensor;
float aX, aY, aZ, aSqrt, gX, gY, gZ, mDirection, mX, mY, mZ;
String magnometer;
String accelarator;
String gyroscope;
String sensorID;

void setup() {
  Serial.begin(9600);
  while(!Serial);
  Serial.println("started");
 
 
#ifdef _ESP32_HAL_I2C_H_ // For ESP32
  Wire.begin(SDA_PIN, SCL_PIN);
  mySensor.setWire(&Wire);
#endif

  mySensor.beginAccel();
  mySensor.beginGyro();
  mySensor.beginMag();

  // You can set your own offset for mag values
  // mySensor.magXOffset = -50;
  // mySensor.magYOffset = -55;
  // mySensor.magZOffset = -10;
}

void loop() {
  StaticJsonDocument<200> ID;
  StaticJsonDocument<200> accelarator;
  StaticJsonDocument<200> gyroscope;
  StaticJsonDocument<200> magnometer;
  uint8_t sensorId;
  if (mySensor.readId(&sensorId) == 0) {
     ID["Sensor"] = "=== 10 DOF IMU Sensor V2 ===";
     ID["ID"] = sensorId;
   //serializeJson(ID, Serial);
   // Serial.println("sensorId: " + String(sensorId));
  } else {
    Serial.write("Cannot read sensorId");
  }

  if (mySensor.accelUpdate() == 0) {
    aX = mySensor.accelX();
    aY = mySensor.accelY();
    aZ = mySensor.accelZ();
    aSqrt = mySensor.accelSqrt();
     accelarator["sensor"] = "accelarator";
     accelarator["X"] = aX;
     accelarator["Y"] = aY;
     accelarator["Z"] = aZ;
     //serializeJsonPretty(accelarator, Serial);
  } else {
    Serial.write("Cannod read accel values");
  }

  if (mySensor.gyroUpdate() == 0) {
    gX = mySensor.gyroX();
    gY = mySensor.gyroY();
    gZ = mySensor.gyroZ();
     gyroscope["sensor"] = "gyroscope";
     gyroscope["X"] = gX;
     gyroscope["Y"] = gY;
     gyroscope["Z"] = gZ;
     //serializeJsonPretty(gyroscope, Serial);
 
  } else {
    //Serial.println("Cannot read gyro values");
    Serial.write("Cannot read gyro values");
  }

  if (mySensor.magUpdate() == 0) {
    mX = mySensor.magX();
    mY = mySensor.magY();
    mZ = mySensor.magZ();
    mDirection = mySensor.magHorizDirection();
     magnometer["sensor"] = "magnometer";
     magnometer["X"] = gX;
     magnometer["Y"] = gY;
     magnometer["Z"] = gZ;
     magnometer["Direction"] = mDirection;
    serializeJsonPretty(magnometer, Serial);
   
  } else {
    Serial.write("Cannot read mag values");
   
  }
  //Serial.write("Hello World!");   
  delay(2000);
}