#include <Adafruit_ICM20X.h>
#include <Adafruit_ICM20948.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_AHRS.h>
#include <Wire.h>

Adafruit_ICM20948 icm;

// Try Mahony first — change to Madgwick if you want to experiment
Adafruit_Mahony filter;  
// Adafruit_Madgwick filter;

float roll, pitch, yaw;

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);

  Wire.begin();  // D4=SDA, D5=SCL on XIAO RP2040

  if (!icm.begin_I2C()) {
    Serial.println("Failed to find ICM20948 chip");
    while (1) delay(10);
  }

  Serial.println("ICM20948 Found!");

  icm.setAccelRange(ICM20948_ACCEL_RANGE_4_G);     // good compromise
  icm.setGyroRange(ICM20948_GYRO_RANGE_1000_DPS);
  // icm.setMagDataRate(AK09916_MAG_DATARATE_100_HZ);  // optional

  filter.begin(80);   // 80 Hz — realistic & stable on RP2040

  Serial.println("Roll,Pitch,Yaw_deg");
}

void loop() {
  sensors_event_t accel, gyro, mag, temp;
  icm.getEvent(&accel, &gyro, &temp, &mag);

  // ─────────────────────────────────────────────
  // Try these remapping options one by one if directions feel wrong
  // Option 1: Flip magnetometer signs (very common for ICM-20948 breakouts)
  // filter.update(gyro.gyro.x, gyro.gyro.y, gyro.gyro.z,
  //               accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
  //               -mag.magnetic.x, -mag.magnetic.y, -mag.magnetic.z);

  // Option 2: Swap mag X/Y + flip some signs
  filter.update(gyro.gyro.x, gyro.gyro.y, gyro.gyro.z,
                accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
                mag.magnetic.y, -mag.magnetic.x, mag.magnetic.z);

  // Default (no remap) — start here, then try options above
  // filter.update(gyro.gyro.x, gyro.gyro.y, gyro.gyro.z,
  //               accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
  //               mag.magnetic.x, mag.magnetic.y, mag.magnetic.z);
  // ─────────────────────────────────────────────

  roll = filter.getRoll();
  pitch = filter.getPitch();
  yaw = filter.getYaw();

  Serial.print(roll, 2);   Serial.print(",");
  Serial.print(pitch, 2);  Serial.print(",");
  Serial.print(yaw, 2);    Serial.println();

  delay(10);   // ≈ 80 Hz
}