#include <Wire.h>
#include <Adafruit_LSM6DSOX.h>
#include <Adafruit_LIS3MDL.h>

// Define the pins for the IMU
#define LSM6DSOX_CS 5
#define LIS3MDL_CS 6

// Create instances of the IMU sensors
Adafruit_LSM6DSOX lsm6dsox;
Adafruit_LIS3MDL lis3mdl;

// Declare global variables
float hx = 0.0;
float hy = 0.0;
float hz = 0.0;
float sx = 1.0;
float sy = 1.0;
float sz = 1.0;

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

  // Initialize the IMU sensors
  if (!lsm6dsox.begin_I2C()) {
    Serial.println("Failed to initialize LSM6DSOX");
    while (1);
  }
  if (!lis3mdl.begin_I2C()) {
    Serial.println("Failed to initialize LIS3MDL");
    while (1);
  }

  // Set the magnetometer range to 16 Gauss (default is 4 Gauss)
  lis3mdl.setRange(LIS3MDL_RANGE_16_GAUSS);
}

void loop() {
  // Read the accelerometer and gyroscope data
  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temp;
  lsm6dsox.getEvent(&accel, &gyro, &temp);

  // Read the magnetometer data
  sensors_event_t mag;
  lis3mdl.getEvent(&mag);

  // Apply hard-iron offsets to the magnetometer data
  float mx = mag.magnetic.x - hx;
  float my = mag.magnetic.y - hy;
  float mz = mag.magnetic.z - hz;

  // Apply soft-iron offsets to the magnetometer data
  mx *= sx;
  my *= sy;
  mz *= sz;

  // Calculate the tilt-compensated magnetometer data
  float roll = atan2(accel.acceleration.y, accel.acceleration.z);
  float pitch = atan2(-accel.acceleration.x, sqrt(accel.acceleration.y * accel.acceleration.y + accel.acceleration.z * accel.acceleration.z));
  float cosRoll = cos(roll);
  float sinRoll = sin(roll);
  float cosPitch = cos(pitch);
  float sinPitch = sin(pitch);

  float mx_comp = mx * cosPitch + my * sinRoll * sinPitch - mz * cosRoll * sinPitch;
  float my_comp = my * cosRoll - mz * sinRoll;
  float mz_comp = mx * sinPitch + my * sinRoll * cosPitch + mz * cosRoll * cosPitch;

  // Calculate the heading
  float heading = atan2(-my_comp, mx_comp);

  // Convert the heading to degrees
  heading *= 180.0 / PI;

  // Ensure the heading is between 0 and 360 degrees
  if (heading < 0) {
    heading += 360;
  }

  // Print the heading to the serial output
  Serial.print("Heading: ");
  Serial.print(heading);
  Serial.println(" degrees");

  delay(50);
}