// 9dof_IMU_6050&5883: https://wokwi.com/projects/474651744136909825
//
// Start the simulation,
// click on the MPU6050 part for accel/gyro
// click on the HMC5883 for magnetometer
//
// Based on https://wokwi.com/projects/474469330864562177
// See https://github.com/wokwi/wokwi-features/issues/760
// and https://forum.arduino.cc/t/wokwi-simulator-custom-chips/1292759

#include <Adafruit_MPU6050.h> // https://github.com/adafruit/Adafruit_MPU6050/tree/master
#include <Adafruit_HMC5883_U.h> // https://github.com/adafruit/Adafruit_HMC5883_Unified
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;
/* Assign a unique ID to this sensor at the same time */
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);


void displayMagSensorDetails(void)
{
  sensor_t sensor;
  mag.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("MagSensor:    "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" uT");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" uT");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" uT");
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}


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

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  if (!mag.begin())
  {
    /* There was a problem detecting the HMC5883 ... check your connections */
    Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
    while (1);
  }

  /* Display some basic information on this sensor */
  displayMagSensorDetails();


  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("");
  delay(100);
}

void loop() {

  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp, magEvent;
  mpu.getEvent(&a, &g, &temp);
  mag.getEvent(&magEvent);


  /* Print out the values in Serial monitor graphing format*/
  Serial.print("Ax:");
  Serial.print(a.acceleration.x);
  Serial.print(" Ay:");
  Serial.print(a.acceleration.y);
  Serial.print(" Az:");
  Serial.print(a.acceleration.z);
  Serial.print(" Rx:");
  Serial.print(g.gyro.x);
  Serial.print(" Ry:");
  Serial.print(g.gyro.y);
  Serial.print(" Rz:");
  Serial.print(g.gyro.z);
  Serial.print(" T:");
  Serial.print(temp.temperature);

  /* Display the results (magnetic vector values are in micro-Tesla (uT)) */
  //Serial.print(" Mx:"); Serial.print(magEvent.magnetic.x);
  //Serial.print(" My: "); Serial.print(magEvent.magnetic.y);
  //Serial.print(" Mz: "); Serial.print(magEvent.magnetic.z);

  // Hold the module so that Z is pointing 'up' and you can measure the heading with x&y
  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(magEvent.magnetic.y, magEvent.magnetic.x);

  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
  // Find yours here: http://www.magnetic-declination.com/
  // Mine is: -13* 2' W, which is ~13 Degrees, or (which we need) 0.22 radians
  // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
  float declinationAngle = 0.00 * M_PI / 180;
  heading += declinationAngle;

  // Correct for when signs are reversed.
  if (heading < 0)
    heading += 2 * PI;

  // Check for wrap due to addition of declination.
  if (heading > 2 * PI)
    heading -= 2 * PI;

  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180 / M_PI;

  Serial.print(" Hdg:"); Serial.print(headingDegrees);

  Serial.println("");

  delay(500);
}