#include <Wire.h>
#include <SPI.h>
#include "SparkFun_BNO08x_Arduino_Library.h" // Matches your BNO08x library folder

#define DEBUG 

BNO08x myIMU;

// Your 8-wire configuration (Note: WAK pin is ignored by the library logic now)
const byte imu_CS = 5;
const byte imu_INT = 12; 
const byte imu_RST = 2;

HardwareSerial ESP1_Serial(2);
const int TX2_PIN = 17;

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  Serial.println("\n=============================================");
  Serial.println("ESP2: Initializing IMU Co-Processor (BNO08x SPI)...");
  Serial.println("=============================================");

  ESP1_Serial.begin(115200, SERIAL_8N1, -1, TX2_PIN); 

  // FIXED: Corrected argument parameters and order for the BNO08x library
  if (myIMU.beginSPI(imu_CS, imu_INT, imu_RST, 3000000, SPI) == false) {
    Serial.println("❌ ERROR: BNO086 not detected over SPI!");
    while (1); 
  }
  Serial.println("✅ BNO086 connected successfully over SPI.");

  // Activate Game Rotation Vector at 10ms (100Hz)
  myIMU.enableGameRotationVector(10); 

  // FIXED: New calibration layout for the BNO08x library
  myIMU.setCalibrationConfig(SH2_CAL_ACCEL | SH2_CAL_GYRO); // Dynamic cal for Gyro + Accel only
  
  Serial.println("🚀 Setup complete. Streaming data to ESP1...");
}

void loop() {
  // Check if a new sensor event has been captured over SPI
  if (myIMU.getSensorEvent() == true) {
    
    // Check if the event that just came in is our Game Rotation Vector data
    if (myIMU.getSensorEventID() == SH2_GAME_ROTATION_VECTOR) {
      
      // Extract data (the library automatically updates these when getSensorEvent runs)
      float roll = (myIMU.getRoll()) * 180.0 / PI;   
      float pitch = (myIMU.getPitch()) * 180.0 / PI; 
      float yaw = (myIMU.getYaw()) * 180.0 / PI;     

      // Package into our lightweight production string for ESP1
      String packet = String(yaw, 2) + "," + String(pitch, 2) + "," + String(roll, 2);

      // Send to ESP1 down the TX wire
      ESP1_Serial.println(packet);

      #ifdef DEBUG
        Serial.print("▼ DATA RECEIVED | ");
        Serial.print("Yaw: ");   Serial.print(yaw, 2);   Serial.print("°\t");
        Serial.print("Pitch: "); Serial.print(pitch, 2); Serial.print("°\t");
        Serial.print("Roll: ");  Serial.print(roll, 2);  Serial.println("°");
      #endif
    }
  }
}