private double CalculatePitch(Vector3 acceleration) =>
     Math.Atan2(acceleration.X, Math.Sqrt(acceleration.Y * acceleration.Y + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateRoll(Vector3 acceleration) =>
        Math.Atan2(acceleration.Y, Math.Sqrt(acceleration.X * acceleration.X + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateYaw(Vector3 angularVelocity, double deltaTime, ref double previousYaw)
    {
        previousYaw += angularVelocity.Z * deltaTime;
        return previousYaw;
    }

    private double CalculatePitch(Vector3 acceleration) =>
     Math.Atan2(acceleration.X, Math.Sqrt(acceleration.Y * acceleration.Y + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateRoll(Vector3 acceleration) =>
        Math.Atan2(acceleration.Y, Math.Sqrt(acceleration.X * acceleration.X + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateYaw(Vector3 angularVelocity, double deltaTime, ref double previousYaw)
    {
        previousYaw += angularVelocity.Z * deltaTime;
        return previousYaw;
    }

public async Task<AccelerometerData> GetDataAsync()
{
    if (_icm20948 == null)
    {
        throw new InvalidOperationException("ICM20948 is not initialized.");
    }

    int numReadings = 10;

    Vector3 accelTotal = new Vector3(0, 0, 0);
    Vector3 gyroTotal = new Vector3(0, 0, 0);

    for (int i = 0; i < numReadings; i++)
    {
        var acceleration = _icm20948.GetAccelerometer();
        var angularVelocity = _icm20948.GetGyroscope();

        accelTotal += acceleration;
        gyroTotal += angularVelocity;

        await Task.Delay(10); // Small delay between readings
    }

    Vector3 accelAverage = accelTotal / numReadings;
    Vector3 gyroAverage = gyroTotal / numReadings;

    var pitch = CalculatePitch(accelAverage);
    var roll = CalculateRoll(accelAverage);

    var currentTime = DateTime.Now;
    var deltaTime = (currentTime - _lastUpdate).TotalSeconds;
    _lastUpdate = currentTime;

    _previousYaw += gyroAverage.Z * deltaTime;
    var yaw = _previousYaw;

    _logger.LogInformation($"Data from ICM20948 is Pitch:{pitch}, Roll:{roll}, Yaw:{yaw}.");

    return new AccelerometerData
    {
        Pitch = pitch,
        Roll = roll,
        Yaw = yaw
    };
}

public async Task<AccelerometerData> GetDataAsync()
{
    if (_icm20948 == null)
    {
        throw new InvalidOperationException("ICM20948 is not initialized.");
    }

    int numReadings = 10;

    Vector3 accelTotal = new Vector3(0, 0, 0);
    Vector3 gyroTotal = new Vector3(0, 0, 0);

    for (int i = 0; i < numReadings; i++)
    {
        var acceleration = _icm20948.GetAccelerometer();
        var angularVelocity = _icm20948.GetGyroscope();

        accelTotal += acceleration;
        gyroTotal += angularVelocity;

        await Task.Delay(10); // Small delay between readings
    }

    Vector3 accelAverage = accelTotal / numReadings;
    Vector3 gyroAverage = gyroTotal / numReadings;

    var pitch = CalculatePitch(accelAverage);
    var roll = CalculateRoll(accelAverage);

    var currentTime = DateTime.Now;
    var deltaTime = (currentTime - _lastUpdate).TotalSeconds;
    _lastUpdate = currentTime;

    _previousYaw += gyroAverage.Z * deltaTime;
    var yaw = _previousYaw;

    _logger.LogInformation($"Data from ICM20948 is Pitch:{pitch}, Roll:{roll}, Yaw:{yaw}.");

    return new AccelerometerData
    {
        Pitch = pitch,
        Roll = roll,
        Yaw = yaw
    };
}

var i2cSettings = new I2cConnectionSettings(i2cBusId, i2cAddress);
var i2cDevice = I2cDevice.Create(i2cSettings);
_icm20948 = new Icm20948(i2cDevice);  // Initialize the ICM20948

// Set bandwidth for accelerometer and gyroscope
var result = _icm20948.FindOptimalBandwidthAsync().Result;

_icm20948.SetBandwidth(result.bestBandwidth, result.bestBandwidth);  

var i2cSettings = new I2cConnectionSettings(i2cBusId, i2cAddress);
var i2cDevice = I2cDevice.Create(i2cSettings);
_icm20948 = new Icm20948(i2cDevice);  // Initialize the ICM20948

// Set bandwidth for accelerometer and gyroscope
var result = _icm20948.FindOptimalBandwidthAsync().Result;

_icm20948.SetBandwidth(result.bestBandwidth, result.bestBandwidth);