using System.Diagnostics;
using System.Threading;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Sensor;

namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
{
    [Device(DeviceType.Imu, "WheeltecN100IMU", "WheeltecN100IMU", "1.0.0", Description = "IMU Simulation Driver")]
    public class WheeltecN100IMU : DeviceBase, IInertialMeasurementUnit
    {
        private readonly WheeltecReader IMU;

        // Cached data
        private AccelStamped _cachedAcceleration;
        private Vector3Stamped _cachedAngularVelocity;
        private Vector3Stamped _cachedMagnetometer;
        private Vector3Stamped _cachedOrientation;
        private QuaternionStamped _cachedQuaternion;
        private double _cachedTemperature = 25.0;
        private bool _isCalibrated = false;
        private DateTime _lastUpdateTime = DateTime.UtcNow;
        private double _sampleRate = 0.0;
        private readonly string _portName;
        private readonly int _timeOut;
        private readonly int _baudRate;
        private readonly bool _printDataEnabled;
        private readonly TimeSpan _printDataInterval;
        private Timer? _printDataTimer;

        // Sample rate calculation
        private int _sampleCount = 0;
        private DateTime _sampleRateStartTime = DateTime.UtcNow;
        private readonly TimeSpan _sampleRateWindow = TimeSpan.FromSeconds(1.0);

        // Calibration: 2 giay dau sau lan nhan du lieu dau tien de thu thap bias (robot dung yen tuyet doi)
        private static readonly TimeSpan CalibrationDuration = TimeSpan.FromSeconds(2.0);
        private static readonly TimeSpan CalibrationWaitTimeout = TimeSpan.FromSeconds(5.0);
        private const double GravityMps2 = 9.81;
        private const double GravityToleranceMps2 = 2.0;

        // IMU outlier validation thresholds
        private const double MaxValidAccelerationMps2 = 50.0;
        private const double MaxValidAngularVelocityRadS = 20.0;

        // Thread-safety: Lock for cached sensor data (struct assignments are NOT atomic)
        private readonly object _dataLock = new();

        // High-precision timestamp using Stopwatch (DateTime.UtcNow has ~10-15ms precision)
        private readonly Stopwatch _highPrecisionTimer = Stopwatch.StartNew();
        private DateTime _timerStartUtc = DateTime.UtcNow;
        private DateTime? _calibrationStartUtc;
        private bool _calibrationDone;
        private TaskCompletionSource<bool>? _calibrationCompletedTcs;
        private double _calibrationSumAccX, _calibrationSumAccY, _calibrationSumAccZ;
        private double _calibrationSumGx, _calibrationSumGy, _calibrationSumGz;
        private double _calibrationSumRoll, _calibrationSumPitch, _calibrationSumYaw;
        private int _calibrationCount;
        private double _accBiasX, _accBiasY, _accBiasZ;
        private double _gyroBiasX, _gyroBiasY, _gyroBiasZ;
        private double _orientationBiasRoll, _orientationBiasPitch, _orientationBiasYaw;

        // Yaw integration: tich phan CalibratedGz thay vi dung firmware AHRS Yaw (firmware drift ~0.009 rad/s)
        // Roll/Pitch van dung firmware AHRS vi co gravity reference (khong drift)
        private double _integratedYaw;
        private DateTime _lastIntegrationTime;

        // Diagnostic: log drift moi 5 giay
        private DateTime _lastDiagnosticLog = DateTime.MinValue;
        private static readonly TimeSpan DiagnosticLogInterval = TimeSpan.FromSeconds(5.0);

        // Events (interface)
        public event EventHandler<AccelerationChangedEventArgs>? AccelerationChanged;
        public event EventHandler<AngularVelocityChangedEventArgs>? AngularVelocityChanged;
        public event EventHandler<MagnetometerChangedEventArgs>? MagnetometerChanged;
        public event EventHandler<OrientationChangedEventArgs>? OrientationChanged;

        // Event thong nhat cho SensorPipeline
        public event EventHandler<ImuDataChangedEventArgs>? ImuDataChanged;

        public WheeltecN100IMU(string deviceId, string deviceName, IConfigurationSection connection)
            : base(deviceId, deviceName, DeviceType.Imu)
        {
            _portName = connection.GetValue<string>("Port") ?? throw new Exception("Port is required");
            _baudRate = connection.GetValue<int?>("BaudRate") ?? throw new Exception("BaudRate is required");
            _timeOut = connection.GetValue<int?>("TimeOut") ?? throw new Exception("Timeout is required");

            IMU = new WheeltecReader(_portName, _baudRate, _timeOut);

            // Continuous IMU data printing (optional)
            _printDataEnabled = connection.GetValue<bool?>("DebugEnabled") ?? false;
            _printDataInterval = TimeSpan.FromMilliseconds(connection.GetValue<int?>("PrintDataIntervalMs") ?? 200);

            // Doc cau hinh neu co
            var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
            var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
            var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");

            if (autoReconnectEnabled.HasValue)
                AutoReconnectEnabled = autoReconnectEnabled.Value;
            else
                AutoReconnectEnabled = true;

            if (reconnectDelayMs.HasValue)
                ReconnectDelayMs = reconnectDelayMs.Value;

            if (maxReconnectAttempts.HasValue)
                MaxReconnectAttempts = maxReconnectAttempts.Value;

            _cachedAcceleration = CreateAccelStamped(0, 0, 9.81, DateTime.UtcNow);
            _cachedAngularVelocity = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
            _cachedMagnetometer = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
            _cachedOrientation = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
            _cachedQuaternion = CreateQuaternionStamped(1, 0, 0, 0, DateTime.UtcNow);

            // Khoi tao gia tri properties
            UpdateProperties();
        }

        protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
        {
            yield return new PropertyDescription("IsCalibrated", "Calibrated", "Trang thai calibrate")
            {
                DataType = "boolean",
                IsReadOnly = true,
                DisplayOrder = 1,
                Category = "Trang thai",
                DefaultValue = "true"
            };

            yield return new PropertyDescription("SampleRate", "Sample Rate (Hz)", "Tan so lay mau (Hz)")
            {
                DataType = "number",
                IsReadOnly = true,
                DisplayOrder = 2,
                Category = "Cau hinh",
                DefaultValue = "100"
            };

            yield return new PropertyDescription("Acceleration", "Acceleration (m/s²)", "Gia toc 3 truc (m/s²)")
            {
                DataType = "string",
                IsReadOnly = true,
                DisplayOrder = 3,
                Category = "Du lieu",
                DefaultValue = "0, 0, 9.81"
            };

            yield return new PropertyDescription("AngularVelocity", "Angular Velocity (rad/s)", "Van toc goc 3 truc (rad/s)")
            {
                DataType = "string",
                IsReadOnly = true,
                DisplayOrder = 4,
                Category = "Du lieu",
                DefaultValue = "0, 0, 0"
            };

            yield return new PropertyDescription("Orientation", "Orientation (rad)", "Huong Euler angles (rad)")
            {
                DataType = "string",
                IsReadOnly = true,
                DisplayOrder = 5,
                Category = "Du lieu",
                DefaultValue = "0, 0, 0"
            };

            yield return new PropertyDescription("Temperature", "Temperature (°C)", "Nhiet do cam bien (°C)")
            {
                DataType = "number",
                IsReadOnly = true,
                DisplayOrder = 6,
                Category = "Du lieu",
                DefaultValue = "25"
            };
        }

        protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
        {
            IMU.Connect();
            await Task.CompletedTask;
        }

        protected override async Task OnConnectAsync(CancellationToken cancellationToken)
        {
            // Dam bao port da mo (sau Disconnect can Connect lai)
            if (!IMU.IsConnected)
                IMU.Connect();

            if (_printDataEnabled)
            {
                _printDataTimer?.Dispose();
                _printDataTimer = new Timer(_ =>
                {
                    try
                    {
                        var acc = CachedAcceleration.Accel.Linear;
                        var gyro = CachedAngularVelocity.Vector;
                        var ori = CachedOrientation.Vector;
                        var temp = CachedTemperature;
                        var t = ((IInertialMeasurementUnit)this).LastUpdateTime;
                        var tempStr = temp.HasValue ? temp.Value.ToString("F2") : "NA";

                        Console.WriteLine(
                            $"{DateTime.Now:HH:mm:ss.ffffff} [IMU_DATA] " +
                            $"t={t:HH:mm:ss.fff} " +
                            $"acc=({acc.X:F3},{acc.Y:F3},{acc.Z:F3}) " +
                            $"gyro=({gyro.X:F5},{gyro.Y:F5},{gyro.Z:F5}) " +
                            $"rpy=({ori.X:F5},{ori.Y:F5},{ori.Z:F5}) " +
                            $"temp={tempStr}");
                    }
                    catch { }
                }, null, dueTime: TimeSpan.Zero, period: _printDataInterval);
            }

            // Reset high-precision timer for accurate timestamps
            _timerStartUtc = DateTime.UtcNow;
            _highPrecisionTimer.Restart();

            // Reset calibration de moi lan connect thu thap lai 2 giay dau
            ResetCalibrationState();
            _calibrationCompletedTcs = new TaskCompletionSource<bool>();

            // Dang ky event handler cho DataReceived tu WheeltecReader
            IMU.DataReceived += IMU_DataReceived;

            // Doi xu ly _calibrationDone (toi da CalibrationWaitTimeout), de connect chi hoan tat sau khi da calibrate
            try
            {
                await Task.WhenAny(
                    _calibrationCompletedTcs.Task,
                    Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
            }
            catch (OperationCanceledException)
            {
                // cancellationToken bi huy
            }
            _calibrationCompletedTcs = null;
        }

        protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
        {
            // Huy dang ky event handler
            IMU.DataReceived -= IMU_DataReceived;

            // Disconnect IMU de dung processing thread va serial port
            IMU.Disconnect();

            _printDataTimer?.Dispose();
            _printDataTimer = null;

            await Task.CompletedTask;
        }

        protected override async Task OnResetAsync(CancellationToken cancellationToken)
        {
            // Huy event va disconnect
            IMU.DataReceived -= IMU_DataReceived;
            IMU.Disconnect();

            _printDataTimer?.Dispose();
            _printDataTimer = null;

            // Reset high-precision timer
            _timerStartUtc = DateTime.UtcNow;
            _highPrecisionTimer.Restart();

            lock (_dataLock)
            {
                _cachedAcceleration = CreateAccelStamped(0, 0, 9.81, DateTime.UtcNow);
                _cachedAngularVelocity = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
                _cachedMagnetometer = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
                _cachedOrientation = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
                _cachedQuaternion = CreateQuaternionStamped(1, 0, 0, 0, DateTime.UtcNow);
                _cachedTemperature = 25.0;
                _lastUpdateTime = DateTime.UtcNow;
            }

            // Reset calibration de sau khi connect lai thu thap 2 giay dau
            ResetCalibrationState();

            // Reset sample rate
            _sampleCount = 0;
            _sampleRate = 0.0;
            _sampleRateStartTime = DateTime.UtcNow;

            // Reconnect va bat dau calibration lai
            IMU.Connect();
            _calibrationCompletedTcs = new TaskCompletionSource<bool>();
            IMU.DataReceived += IMU_DataReceived;

            // Doi calibration hoan tat
            try
            {
                await Task.WhenAny(
                    _calibrationCompletedTcs.Task,
                    Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
            }
            catch (OperationCanceledException)
            {
                // cancellationToken bi huy
            }
            _calibrationCompletedTcs = null;
            UpdateProperties();
        }

        protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
        {
            return Task.FromResult(IMU.IsConnected);
        }

        /// <summary>
        /// Reset toan bo trang thai calibration ve gia tri ban dau
        /// </summary>
        private void ResetCalibrationState()
        {
            _calibrationStartUtc = null;
            _calibrationDone = false;
            _isCalibrated = false;
            _calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
            _calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
            _calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
            _calibrationCount = 0;
            _accBiasX = _accBiasY = _accBiasZ = 0;
            _gyroBiasX = _gyroBiasY = _gyroBiasZ = 0;
            _orientationBiasRoll = _orientationBiasPitch = _orientationBiasYaw = 0;
            _integratedYaw = 0;
            _lastIntegrationTime = DateTime.MinValue;
        }

        /// <summary>
        /// Event handler cho DataReceived tu WheeltecReader.
        /// 2 giay dau ke tu lan nhan du lieu dau tien: chi thu thap mau de tinh bias (robot dung yen tuyet doi).
        /// Sau 2 giay: ap dung bias de calibrate, roi moi cap nhat _cached*, fire events, _sampleCount.
        /// </summary>
        private void IMU_DataReceived(object? sender, EventArgs e)
        {
            try
            {
                var snapshot = IMU.GetSnapshot();
                // Use high-precision timer instead of DateTime.UtcNow (which has ~10-15ms precision)
                var timestamp = _timerStartUtc + _highPrecisionTimer.Elapsed;

                // Bat dau cua so calibration khi nhan du lieu lan dau
                if (_calibrationStartUtc == null)
                {
                    _calibrationStartUtc = timestamp;
                    _calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
                    _calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
                    _calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
                    _calibrationCount = 0;
                }

                if (!_calibrationDone)
                {
                    var calElapsed = timestamp - _calibrationStartUtc.Value;
                    if (calElapsed < CalibrationDuration)
                    {
                        // Trong 2 giay dau: chi tich luy mau, khong cap nhat cache / fire events / sampleCount
                        _calibrationSumAccX += snapshot.AccX;
                        _calibrationSumAccY += snapshot.AccY;
                        _calibrationSumAccZ += snapshot.AccZ;
                        _calibrationSumGx += snapshot.Gx;
                        _calibrationSumGy += snapshot.Gy;
                        _calibrationSumGz += snapshot.Gz;
                        _calibrationSumRoll += snapshot.Roll;
                        _calibrationSumPitch += snapshot.Pitch;
                        _calibrationSumYaw += snapshot.Yaw;
                        _calibrationCount++;
                        return;
                    }

                    // Het 2 giay: tinh bias va danh dau da calibrate
                    if (_calibrationCount > 0)
                    {
                        double n = _calibrationCount;
                        double meanAccX = _calibrationSumAccX / n;
                        double meanAccY = _calibrationSumAccY / n;
                        double meanAccZ = _calibrationSumAccZ / n;

                        // Validate gravity magnitude — neu lech qua xa 9.81 thi robot bi rung/di chuyen
                        double gravityMagnitude = Math.Sqrt(meanAccX * meanAccX + meanAccY * meanAccY + meanAccZ * meanAccZ);
                        if (Math.Abs(gravityMagnitude - GravityMps2) > GravityToleranceMps2)
                        {
                            Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecN100IMU] Calibration REJECTED: " +
                                $"GravityMag={gravityMagnitude:F4} (expected ~{GravityMps2}, tolerance ±{GravityToleranceMps2}). " +
                                $"Robot co the dang rung/di chuyen. Thu lai...");
                            _calibrationStartUtc = null;
                            _calibrationCount = 0;
                            _calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
                            _calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
                            _calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
                            return;
                        }

                        // FIXED: Khong tru gravity khoi acceleration data!
                        // ImuTracker trong Cartographer CAN gravity de estimate orientation.
                        // Khi dung yen (Z up): acc ~ (0, 0, +9.81) — day la luc phan ung tu mat dat
                        //
                        // Chi calibrate bias nho (sensor offset) cho X va Y.
                        // Voi Z: tinh bias = meanAccZ - expected_gravity
                        double expectedGravityZ = meanAccZ > 0 ? GravityMps2 : -GravityMps2;

                        // Bias X,Y: offset khi dung yen (nen ~ 0 neu robot dat phang)
                        _accBiasX = meanAccX;
                        _accBiasY = meanAccY;
                        // Bias Z: chi tru phan offset, GIU NGUYEN gravity
                        _accBiasZ = meanAccZ - expectedGravityZ;

                        // Gyro bias: dung — khi dung yen angular velocity = 0
                        _gyroBiasX = _calibrationSumGx / n;
                        _gyroBiasY = _calibrationSumGy / n;
                        _gyroBiasZ = _calibrationSumGz / n;

                        // Orientation bias: giu lai cho display purposes
                        _orientationBiasRoll = _calibrationSumRoll / n;
                        _orientationBiasPitch = _calibrationSumPitch / n;
                        _orientationBiasYaw = _calibrationSumYaw / n;

                        _isCalibrated = true;
                        Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecN100IMU] Calibration done (n={_calibrationCount}): " +
                            $"MeanAcc=({meanAccX:F4},{meanAccY:F4},{meanAccZ:F4}), GravityMag={gravityMagnitude:F4}, " +
                            $"AccBias=({_accBiasX:F4},{_accBiasY:F4},{_accBiasZ:F4}), " +
                            $"GyroBias=({_gyroBiasX:F6},{_gyroBiasY:F6},{_gyroBiasZ:F6})");
                    }
                    _calibrationDone = true;
                    _calibrationCompletedTcs?.TrySetResult(true);
                    _calibrationCompletedTcs = null;
                    // Khoi tao yaw integration tu thoi diem calibration xong
                    _integratedYaw = 0;
                    _lastIntegrationTime = timestamp;
                    // Bat dau dem sample rate tu sau calibration
                    _sampleRateStartTime = timestamp;
                    _sampleCount = 0;
                }

                // Ap dung bias: du lieu da calibrate (sau 2 giay moi chay toi day)
                double accX = snapshot.AccX - _accBiasX;
                double accY = snapshot.AccY - _accBiasY;
                double accZ = snapshot.AccZ - _accBiasZ;
                double gx = snapshot.Gx - _gyroBiasX;
                double gy = snapshot.Gy - _gyroBiasY;
                double gz = snapshot.Gz - _gyroBiasZ;
                double roll = snapshot.Roll - _orientationBiasRoll;
                double pitch = snapshot.Pitch - _orientationBiasPitch;

                // Tich phan CalibratedGz de tinh Yaw thay vi dung firmware AHRS Yaw
                // Firmware AHRS Yaw drift ~0.009 rad/s do tich phan gyro noi bo khong chinh xac
                // CalibratedGz sau khi tru bias chi con ~0.00006 rad/s trung binh → giam drift 150 lan
                double yaw;
                if (_lastIntegrationTime != DateTime.MinValue)
                {
                    double dt = (timestamp - _lastIntegrationTime).TotalSeconds;
                    _integratedYaw += gz * dt;
                    yaw = _integratedYaw;
                }
                else
                {
                    yaw = 0;
                }
                _lastIntegrationTime = timestamp;

                // Diagnostic log moi 5 giay: theo doi drift
                if (timestamp - _lastDiagnosticLog >= DiagnosticLogInterval)
                {
                    _lastDiagnosticLog = timestamp;
                    var elapsedSec = (timestamp - _timerStartUtc).TotalSeconds;
                    double firmwareYaw = snapshot.Yaw - _orientationBiasYaw;
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [IMU_DIAG] t={elapsedSec:F1}s | " +
                        $"CalibratedGz={gz:F6} | " +
                        $"IntegratedYaw={yaw:F6} FirmwareYaw={firmwareYaw:F6} | " +
                        $"Temp={snapshot.Temp:F2}");
                }

                // Outlier validation: reject data with unrealistic values
                // This prevents ImuTracker corruption from EMI spikes or communication errors
                double accMagnitude = Math.Sqrt(accX * accX + accY * accY + accZ * accZ);
                double gyroMagnitude = Math.Sqrt(gx * gx + gy * gy + gz * gz);
                if (accMagnitude > MaxValidAccelerationMps2 || gyroMagnitude > MaxValidAngularVelocityRadS)
                {
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecN100IMU] OUTLIER REJECTED: " +
                        $"accMag={accMagnitude:F2} m/s² (max={MaxValidAccelerationMps2}), " +
                        $"gyroMag={gyroMagnitude:F2} rad/s (max={MaxValidAngularVelocityRadS})");
                    return;
                }

                // Quaternion tu goc Euler da calibrate
                var cosRoll = Math.Cos(roll / 2);
                var sinRoll = Math.Sin(roll / 2);
                var cosPitch = Math.Cos(pitch / 2);
                var sinPitch = Math.Sin(pitch / 2);
                var cosYaw = Math.Cos(yaw / 2);
                var sinYaw = Math.Sin(yaw / 2);
                var qw = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
                var qx = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
                var qy = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
                var qz = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;

                var newAcceleration = CreateAccelStamped(accX, accY, accZ, timestamp);
                var newAngularVelocity = CreateVector3Stamped(gx, gy, gz, timestamp);
                var newMagnetometer = CreateVector3Stamped(snapshot.MagX, snapshot.MagY, snapshot.MagZ, timestamp);
                var newOrientation = CreateVector3Stamped(roll, pitch, yaw, timestamp);
                var newQuaternion = CreateQuaternionStamped(qw, qx, qy, qz, timestamp);

                AccelStamped previousAcceleration;
                Vector3Stamped previousOrientation;

                // Thread-safe update of cached data using lock
                // Struct assignments are NOT atomic — without lock, readers could get partially updated data
                lock (_dataLock)
                {
                    previousAcceleration = _cachedAcceleration;
                    previousOrientation = _cachedOrientation;

                    _cachedAcceleration = newAcceleration;
                    _cachedAngularVelocity = newAngularVelocity;
                    _cachedMagnetometer = newMagnetometer;
                    _cachedOrientation = newOrientation;
                    _cachedQuaternion = newQuaternion;
                    _cachedTemperature = snapshot.Temp;
                    _lastUpdateTime = timestamp;
                }

                // Fire unified event (SensorPipeline)
                ImuDataChanged?.Invoke(this, new ImuDataChangedEventArgs(
                    newAcceleration,
                    newAngularVelocity,
                    newMagnetometer,
                    newOrientation,
                    timestamp));

                // Fire individual events (interface — XlocIntegrationService, etc.)
                if (Math.Abs(previousAcceleration.Accel.Linear.X - newAcceleration.Accel.Linear.X) > 0.1 ||
                    Math.Abs(previousAcceleration.Accel.Linear.Y - newAcceleration.Accel.Linear.Y) > 0.1 ||
                    Math.Abs(previousAcceleration.Accel.Linear.Z - newAcceleration.Accel.Linear.Z) > 0.1)
                {
                    AccelerationChanged?.Invoke(this, new AccelerationChangedEventArgs(newAcceleration));
                }

                // Xloc requires a continuous IMU stream even when the robot is stationary.
                // Emit angular velocity updates every sample instead of threshold-based changes.
                AngularVelocityChanged?.Invoke(this, new AngularVelocityChangedEventArgs(newAngularVelocity));

                MagnetometerChanged?.Invoke(this, new MagnetometerChangedEventArgs(newMagnetometer));

                if (Math.Abs(previousOrientation.Vector.X - newOrientation.Vector.X) > 0.01 ||
                    Math.Abs(previousOrientation.Vector.Y - newOrientation.Vector.Y) > 0.01 ||
                    Math.Abs(previousOrientation.Vector.Z - newOrientation.Vector.Z) > 0.01)
                {
                    OrientationChanged?.Invoke(this, new OrientationChangedEventArgs(newOrientation));
                }

                _sampleCount++;
                var elapsed = timestamp - _sampleRateStartTime;
                if (elapsed >= _sampleRateWindow)
                {
                    _ = Task.Run(() =>
                    {
                        _sampleRate = _sampleCount / elapsed.TotalSeconds;
                        _sampleCount = 0;
                        _sampleRateStartTime = timestamp;
                        UpdateProperties();
                    });
                }
            }
            catch (Exception ex)
            {
                LastError = ex;
                OnErrorOccurred(ex, "Error updating data from IMU");
            }
        }

        /// <summary>
        /// Cap nhat properties hien thi
        /// </summary>
        private void UpdateProperties()
        {
            var accel = _cachedAcceleration;
            var angularVel = _cachedAngularVelocity;
            var orientation = _cachedOrientation;
            var temp = _cachedTemperature;
            var sampleRate = _sampleRate;
            var isCalibrated = _isCalibrated;

            SetProperty("IsCalibrated", isCalibrated.ToString());
            SetProperty("SampleRate", sampleRate.ToString("F1"));
            SetProperty("Acceleration", $"{accel.Accel.Linear.X:F2}, {accel.Accel.Linear.Y:F2}, {accel.Accel.Linear.Z:F2}");
            SetProperty("AngularVelocity", $"{angularVel.Vector.X:F3}, {angularVel.Vector.Y:F3}, {angularVel.Vector.Z:F3}");
            SetProperty("Orientation", $"{orientation.Vector.X:F3}, {orientation.Vector.Y:F3}, {orientation.Vector.Z:F3}");
            SetProperty("Temperature", temp.ToString("F2"));
        }

        private static AccelStamped CreateAccelStamped(double x, double y, double z, DateTime timestamp)
        {
            return new AccelStamped
            {
                Header = new Header
                {
                    Stamp = timestamp,
                    FrameId = "imu_frame"
                },
                Accel = new Accel
                {
                    Linear = new Vector3(x, y, z),
                    Angular = new Vector3(0, 0, 0)
                }
            };
        }

        private static Vector3Stamped CreateVector3Stamped(double x, double y, double z, DateTime timestamp)
        {
            return new Vector3Stamped
            {
                Header = new Header
                {
                    Stamp = timestamp,
                    FrameId = "imu_frame"
                },
                Vector = new Vector3(x, y, z)
            };
        }

        private static QuaternionStamped CreateQuaternionStamped(double w, double x, double y, double z, DateTime timestamp)
        {
            return new QuaternionStamped
            {
                Header = new Header
                {
                    Stamp = timestamp,
                    FrameId = "imu_frame"
                },
                Quaternion = new Quaternion(x, y, z, w)
            };
        }

        #region IInertialMeasurementUnit Implementation

        bool IInertialMeasurementUnit.IsConnected => base.IsConnected;

        public bool IsCalibrated
        {
            get { return _isCalibrated; }
        }

        public double SampleRate
        {
            get
            {
                Thread.MemoryBarrier();
                return _sampleRate;
            }
        }

        public AccelStamped CachedAcceleration
        {
            get { lock (_dataLock) { return _cachedAcceleration; } }
        }

        public Vector3Stamped CachedAngularVelocity
        {
            get { lock (_dataLock) { return _cachedAngularVelocity; } }
        }

        public Vector3Stamped? CachedMagnetometer
        {
            get { lock (_dataLock) { return _cachedMagnetometer; } }
        }

        public Vector3Stamped CachedOrientation
        {
            get { lock (_dataLock) { return _cachedOrientation; } }
        }

        public QuaternionStamped? CachedQuaternion
        {
            get { lock (_dataLock) { return _cachedQuaternion; } }
        }

        public double? CachedTemperature
        {
            get { lock (_dataLock) { return _cachedTemperature; } }
        }

        DateTime IInertialMeasurementUnit.LastUpdateTime
        {
            get { lock (_dataLock) { return _lastUpdateTime; } }
        }

        public async Task<AccelStamped> ReadAccelerationAsync(CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
            return CachedAcceleration;
        }

        public async Task<Vector3Stamped> ReadAngularVelocityAsync(CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
            return CachedAngularVelocity;
        }

        public async Task<Vector3Stamped?> ReadMagnetometerAsync(CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
            return CachedMagnetometer;
        }

        public async Task<Vector3Stamped> ReadOrientationAsync(CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
            return CachedOrientation;
        }

        public async Task<QuaternionStamped?> ReadQuaternionAsync(CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
            return CachedQuaternion;
        }

        public async Task<Imu> ReadAllDataAsync(CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
            lock (_dataLock)
            {
                return CreateImuFromCachedData();
            }
        }

        private Imu CreateImuFromCachedData()
        {
            var timestamp = _lastUpdateTime != default ? _lastUpdateTime : DateTime.UtcNow;

            var orientation = _cachedQuaternion.Quaternion;

            var orientationCovariance = new double[Imu.OrientationCovarianceSize];
            var angularVelocityCovariance = new double[Imu.AngularVelocityCovarianceSize];
            var linearAccelerationCovariance = new double[Imu.LinearAccelerationCovarianceSize];

            double gyroVariance = 1e-4;
            angularVelocityCovariance[0] = gyroVariance;
            angularVelocityCovariance[4] = gyroVariance;
            angularVelocityCovariance[8] = gyroVariance;

            double accelVariance = 1e-3;
            linearAccelerationCovariance[0] = accelVariance;
            linearAccelerationCovariance[4] = accelVariance;
            linearAccelerationCovariance[8] = accelVariance;

            orientationCovariance[0] = 0.001;
            orientationCovariance[4] = 0.001;
            orientationCovariance[8] = 0.002;

            return new Imu(
                header: new Header
                {
                    Stamp = timestamp,
                    FrameId = "imu_frame"
                },
                orientation: orientation,
                orientationCovariance: orientationCovariance,
                angularVelocity: _cachedAngularVelocity.Vector,
                angularVelocityCovariance: angularVelocityCovariance,
                linearAcceleration: _cachedAcceleration.Accel.Linear,
                linearAccelerationCovariance: linearAccelerationCovariance
            );
        }

        public async Task<double?> ReadTemperatureAsync(CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
            return CachedTemperature;
        }

        public async Task CalibrateAsync(CancellationToken cancellationToken = default)
        {
            // Huy event, reset calibration, dang ky lai event va doi calib
            IMU.DataReceived -= IMU_DataReceived;
            ResetCalibrationState();

            _timerStartUtc = DateTime.UtcNow;
            _highPrecisionTimer.Restart();

            _calibrationCompletedTcs = new TaskCompletionSource<bool>();
            IMU.DataReceived += IMU_DataReceived;

            try
            {
                await Task.WhenAny(
                    _calibrationCompletedTcs.Task,
                    Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
            }
            catch (OperationCanceledException)
            {
                // cancellationToken bi huy
            }
            _calibrationCompletedTcs = null;
            UpdateProperties();
        }

        public async Task CalibrateMagnetometerAsync(CancellationToken cancellationToken = default)
        {
            await Task.Delay(2000, cancellationToken);
        }

        public async Task ResetCalibrationAsync(CancellationToken cancellationToken = default)
        {
            // Huy event, reset state, dang ky lai va doi calib moi
            IMU.DataReceived -= IMU_DataReceived;
            ResetCalibrationState();

            _timerStartUtc = DateTime.UtcNow;
            _highPrecisionTimer.Restart();

            _calibrationCompletedTcs = new TaskCompletionSource<bool>();
            IMU.DataReceived += IMU_DataReceived;

            try
            {
                await Task.WhenAny(
                    _calibrationCompletedTcs.Task,
                    Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
            }
            catch (OperationCanceledException)
            {
                // cancellationToken bi huy
            }
            _calibrationCompletedTcs = null;
            UpdateProperties();
        }

        public async Task SetSampleRateAsync(double sampleRate, CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
            _sampleRate = Math.Max(1, Math.Min(1000, sampleRate));
            UpdateProperties();
        }

        public async Task SetAccelerometerRangeAsync(double range, CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
        }

        public async Task SetGyroscopeRangeAsync(double range, CancellationToken cancellationToken = default)
        {
            await Task.CompletedTask;
        }

        #endregion

        protected override void Dispose(bool disposing)
        {
            if (disposing)
            {
                IMU.DataReceived -= IMU_DataReceived;
                IMU.Disconnect();
                IMU.Dispose();
                _printDataTimer?.Dispose();
                _printDataTimer = null;
            }
            base.Dispose(disposing);
        }
    }
}