using Microsoft.Extensions.Configuration;
using Microsoft.Extensions.Hosting;
using Microsoft.Extensions.Logging;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Sensor;
using System;
using System.Threading;
using System.Threading.Tasks;

namespace RobotNet10.RobotApp.Motion;

/// <summary>
/// OdometryService - tính toán odometry từ encoder data của hai động cơ CiA402 Servo
/// Sử dụng động học thuận (forward kinematics) để tính toán vị trí, hướng và vận tốc của robot
/// Cấu trúc dữ liệu tương tự nav_msgs/Odometry.msg trong ROS
/// </summary>
public class OdometryService : IHostedService, IDisposable
{
    private readonly OdometryConfiguration _config;
    private readonly IDeviceProvider _deviceProvider;
    // private readonly IOdometryEstimator _odometryEstimator; // Unused
    private readonly ILogger<OdometryService> _logger;
    private readonly object _lock = new();

    // Servo devices
    private ICiA402Servo? _leftWheelServo;
    private ICiA402Servo? _rightWheelServo;
    
    // IMU device for orientation (theta) calculation
    private IInertialMeasurementUnit? _imu;
    private double DeltaTheta = 0.0;
    private double _imuYawZeroOffset = 0.0;
    private bool _imuYawZeroOffsetCaptured = false;

    private double PoseX = 0.0;
    private double PoseY = 0.0;
    private double PoseTheta = 0.0;

    // Odometry state
    private Odometry _currentOdometry = new();
    private int _lastLeftWheelPosition;
    private int _lastRightWheelPosition;
    private int _lastLeftWheelVelocity;
    private int _lastRightWheelVelocity;
    private DateTime _lastUpdateTime = DateTime.UtcNow;
    private bool _isFirstUpdate = true;

    // Sequence number for header
    private uint _sequenceNumber = 0;
    private int count = 0;

    // Timer for periodic updates
    private Timer? _updateTimer;
    private bool _disposed = false;
    private long _sourceUpdateCount = 0;
    private long _lastSourceUpdateCount = 0;
    private DateTime _lastSourceRateLogTime = DateTime.UtcNow;

    // Conversion factors (calculated from configuration)
    private double _leftWheelMetersPerCount;
    private double _rightWheelMetersPerCount;

    /// <summary>
    /// Fired when a new odometry sample is produced.
    /// </summary>
    public event EventHandler<OdometryUpdatedEventArgs>? OdometryUpdated;

    /// <summary>
    /// Gets the current odometry data
    /// </summary>
    public Odometry CurrentOdometry
    {
        get
        {
            lock (_lock)
            {
                return _currentOdometry;
            }
        }
    }

    public OdometryService(
        IConfiguration configuration,
        IDeviceProvider deviceProvider,
        ILogger<OdometryService> logger)
    {
        _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));

        // Load configuration
        var configSection = configuration.GetSection("Motion:Odometry");
        if (!configSection.Exists())
        {
            throw new InvalidOperationException("Configuration section 'Motion:Odometry' not found in appsettings.json");
        }

        _config = new OdometryConfiguration();
        configSection.Bind(_config);

        // Validate configuration
        ValidateConfiguration();

        // Calculate conversion factors
        CalculateConversionFactors();
    }

    private void ValidateConfiguration()
    {
        if (string.IsNullOrWhiteSpace(_config.LeftWheel.DeviceId))
            throw new InvalidOperationException("LeftWheel.DeviceId is required");

        if (string.IsNullOrWhiteSpace(_config.RightWheel.DeviceId))
            throw new InvalidOperationException("RightWheel.DeviceId is required");
        
        if (string.IsNullOrWhiteSpace(_config.ImuDeviceId))
            throw new InvalidOperationException("ImuDeviceId is required");

        if (_config.LeftWheel.WheelDiameter <= 0)
            throw new InvalidOperationException("LeftWheel.WheelDiameter must be greater than 0");

        if (_config.RightWheel.WheelDiameter <= 0)
            throw new InvalidOperationException("RightWheel.WheelDiameter must be greater than 0");

        if (_config.LeftWheel.PulsesPerRevolution <= 0)
            throw new InvalidOperationException("LeftWheel.PulsesPerRevolution must be greater than 0");

        if (_config.RightWheel.PulsesPerRevolution <= 0)
            throw new InvalidOperationException("RightWheel.PulsesPerRevolution must be greater than 0");

        if (string.IsNullOrWhiteSpace(_config.FrameId))
            _config.FrameId = "odom";

        if (string.IsNullOrWhiteSpace(_config.ChildFrameId))
            _config.ChildFrameId = "base_link";
    }

    private void CalculateConversionFactors()
    {
        // Calculate meters per encoder count
        var leftWheelCircumference = Math.PI * _config.LeftWheel.WheelDiameter;
        var rightWheelCircumference = Math.PI * _config.RightWheel.WheelDiameter;
        _leftWheelMetersPerCount = leftWheelCircumference / _config.LeftWheel.PulsesPerRevolution;
        _rightWheelMetersPerCount = rightWheelCircumference / _config.RightWheel.PulsesPerRevolution;

        _logger.LogInformation(
            "OdometryService conversion factors calculated: LeftMetersPerCount={LeftMetersPerCount}, RightMetersPerCount={RightMetersPerCount}",
            _leftWheelMetersPerCount, _rightWheelMetersPerCount);
    }

    /// <summary>
    /// Start OdometryService - đợi DeviceProvider kết nối xong tất cả devices rồi mới khởi tạo
    /// </summary>
    public async Task StartAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("Starting OdometryService...");

        try
        {
            // Đợi DeviceProvider kết nối xong tất cả devices
            _logger.LogInformation("Waiting for all devices to be connected...");
            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);

            if (!connected)
            {
                _logger.LogWarning("Timeout waiting for devices to connect. OdometryService will not be initialized.");
                return;
            }

            _logger.LogInformation("All devices connected. Initializing OdometryService...");

            // Get servo devices
            var leftDevice = _deviceProvider.GetDevice(_config.LeftWheel.DeviceId);
            var rightDevice = _deviceProvider.GetDevice(_config.RightWheel.DeviceId);
            var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);

            if (leftDevice == null)
            {
                _logger.LogError("Left wheel device '{DeviceId}' not found", _config.LeftWheel.DeviceId);
                return;
            }

            if (rightDevice == null)
            {
                _logger.LogError("Right wheel device '{DeviceId}' not found", _config.RightWheel.DeviceId);
                return;
            }
            
            if (imuDevice == null)
            {
                _logger.LogError("IMU device '{DeviceId}' not found", _config.ImuDeviceId);
                return;
            }

            if (leftDevice is not ICiA402Servo leftServo)
            {
                _logger.LogError("Left wheel device '{DeviceId}' is not an ICiA402Servo", _config.LeftWheel.DeviceId);
                return;
            }

            if (rightDevice is not ICiA402Servo rightServo)
            {
                _logger.LogError("Right wheel device '{DeviceId}' is not an ICiA402Servo", _config.RightWheel.DeviceId);
                return;
            }
            
            if (imuDevice is not IInertialMeasurementUnit imu)
            {
                _logger.LogError("IMU device '{DeviceId}' is not an IInertialMeasurementUnit", _config.ImuDeviceId);
                return;
            }

            // Check if devices are connected
            if (!leftDevice.IsConnected)
            {
                _logger.LogWarning("Left wheel device '{DeviceId}' is not connected. Status: {Status}. OdometryService will wait for connection.", 
                    _config.LeftWheel.DeviceId, leftDevice.Status);
                // Don't return - we'll retry later
            }

            if (!rightDevice.IsConnected)
            {
                _logger.LogWarning("Right wheel device '{DeviceId}' is not connected. Status: {Status}. OdometryService will wait for connection.", 
                    _config.RightWheel.DeviceId, rightDevice.Status);
                // Don't return - we'll retry later
            }
            
            if (!imuDevice.IsConnected)
            {
                _logger.LogWarning("IMU device '{DeviceId}' is not connected. Status: {Status}. OdometryService will wait for connection.", 
                    _config.ImuDeviceId, imuDevice.Status);
            }

            // Only initialize if all devices are connected
            if (!leftDevice.IsConnected || !rightDevice.IsConnected || !imuDevice.IsConnected)
            {
                _logger.LogWarning("One or more devices are not connected. OdometryService will not start until all devices are connected.");
                // Start a background task to retry initialization when devices connect
                _ = Task.Run(async () => await RetryInitializationWhenDevicesReadyAsync(cancellationToken));
                return;
            }

            _leftWheelServo = leftServo;
            _rightWheelServo = rightServo;
            _imu = imu;

            // Get initial positions
            _lastLeftWheelPosition = _leftWheelServo.CachedPosition;
            _lastRightWheelPosition = _rightWheelServo.CachedPosition;
            _lastLeftWheelVelocity = _leftWheelServo.CachedVelocity;
            _lastRightWheelVelocity = _rightWheelServo.CachedVelocity;
            _lastUpdateTime = DateTime.UtcNow;
            
            // Initialize theta integration
            DeltaTheta = 0.0;
            _imuYawZeroOffset = 0.0;
            _imuYawZeroOffsetCaptured = false;

            // Initialize odometry
            InitializeOdometry();

            // Start periodic update timer (update every 50ms = 20Hz)
            // var updateInterval = TimeSpan.FromMilliseconds(50);
            // _updateTimer = new Timer(OnTimerCallback, null, updateInterval, updateInterval);
            _ = Task.Run(async () => await OdometryLoopAsync(cancellationToken), cancellationToken);


            _logger.LogInformation("OdometryService started successfully with IMU integration for theta");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error starting OdometryService");
        }
    }

    /// <summary>
    /// Retry initialization when devices become ready
    /// </summary>
    private async Task RetryInitializationWhenDevicesReadyAsync(CancellationToken cancellationToken)
    {
        const int maxRetries = 60; // 5 minutes with 5 second intervals
        int retryCount = 0;

        while (retryCount < maxRetries && !cancellationToken.IsCancellationRequested)
        {
            await Task.Delay(TimeSpan.FromSeconds(5), cancellationToken);

            var leftDevice = _deviceProvider.GetDevice(_config.LeftWheel.DeviceId);
            var rightDevice = _deviceProvider.GetDevice(_config.RightWheel.DeviceId);
            var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);

            if (leftDevice != null && rightDevice != null && imuDevice != null &&
                leftDevice.IsConnected && rightDevice.IsConnected && imuDevice.IsConnected &&
                leftDevice is ICiA402Servo leftServo &&
                rightDevice is ICiA402Servo rightServo &&
                imuDevice is IInertialMeasurementUnit imu)
            {
                _logger.LogInformation("All devices are now connected. Initializing OdometryService...");

                lock (_lock)
                {
                    _leftWheelServo = leftServo;
                    _rightWheelServo = rightServo;
                    _imu = imu;

                    // Get initial positions
                    _lastLeftWheelPosition = _leftWheelServo.CachedPosition;
                    _lastRightWheelPosition = _rightWheelServo.CachedPosition;
                    _lastLeftWheelVelocity = _leftWheelServo.CachedVelocity;
                    _lastRightWheelVelocity = _rightWheelServo.CachedVelocity;
                    _lastUpdateTime = DateTime.UtcNow;
                    
                    // Initialize theta
                    DeltaTheta = 0.0;
                    _imuYawZeroOffset = 0.0;
                    _imuYawZeroOffsetCaptured = false;
                }

                // Initialize odometry
                InitializeOdometry();

                // Start periodic update timer (update every 50ms = 20Hz)
                // var updateInterval = TimeSpan.FromMilliseconds(50);
                // _updateTimer = new Timer(OnTimerCallback, null, updateInterval, updateInterval);
                _ = Task.Run(async () => await OdometryLoopAsync(cancellationToken), cancellationToken);

                _logger.LogInformation("OdometryService initialized successfully after device connection");
                return;
            }

            retryCount++;
            if (retryCount % 12 == 0) // Log every minute
            {
                _logger.LogInformation("Still waiting for devices to connect... (attempt {Attempt}/{MaxAttempts})", 
                    retryCount, maxRetries);
            }
        }

        if (retryCount >= maxRetries)
        {
            _logger.LogWarning("Timeout waiting for devices to connect. OdometryService will not be initialized.");
        }
    }

    /// <summary>
    /// Stop OdometryService
    /// </summary>
    public async Task StopAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("Stopping OdometryService...");

        try
        {
            _updateTimer?.Dispose();
            _updateTimer = null;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error stopping OdometryService");
        }

        await Task.CompletedTask;
    }

    // private void OnTimerCallback(object? state)
    // {
    //     try
    //     {
    //         UpdateOdometry();
    //     }
    //     catch (Exception ex)
    //     {
    //         _logger.LogError(ex, "Error updating odometry");
    //     }
    // }
    private async Task OdometryLoopAsync(CancellationToken cancellationToken)
    {
        while (!cancellationToken.IsCancellationRequested)
        {
            try
            {
                UpdateOdometry();
                await Task.Delay(1, cancellationToken); // 100Hz update rate
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error updating odometry");
            }
        }
    }

    private void InitializeOdometry()
    {
        lock (_lock)
        {
            // var currentPose = _odometryEstimator.CurrentPose;
            var currentTime = DateTime.UtcNow;

            _currentOdometry = new Odometry
            {
                Header = new Header
                {
                    Seq = _sequenceNumber++,
                    Stamp = DateTime.UtcNow,
                    FrameId = _config.FrameId
                },
                ChildFrameId = _config.ChildFrameId,
                Pose = new PoseWithCovariance
                {
                    // Pose = currentPose,
                    Pose = new Pose
                    {
                        Position = new Point(),
             
                        Orientation = new Quaternion(0, 0, 0, 1)  // Identity quaternion (valid)
                      
                    },
                    Covariance = new double[PoseWithCovariance.CovarianceSize]
                },
                Twist = new TwistWithCovariance
                {
                    Twist = new Twist
                    {
                        Linear = new Vector3(),
                        Angular = new Vector3()
                    },
                    Covariance = new double[TwistWithCovariance.CovarianceSize]
                }
            };
        }
    }

    /// <summary>
    /// Cập nhật odometry từ encoder data và IMU
    /// Sử dụng IMU orientation (yaw tuyệt đối) để tính theta
    /// Sử dụng encoder để tính linear displacement
    /// </summary>
    private void UpdateOdometry()
    {
        if (_leftWheelServo == null || _rightWheelServo == null || _imu == null)
            return;

        Odometry? updatedOdometry = null;

        // Check if devices are still connected
        var leftDevice = _deviceProvider.GetDevice(_config.LeftWheel.DeviceId);
        var rightDevice = _deviceProvider.GetDevice(_config.RightWheel.DeviceId);
        var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);

        if (leftDevice == null || rightDevice == null || imuDevice == null ||
            !leftDevice.IsConnected || !rightDevice.IsConnected || !imuDevice.IsConnected)
        {
            // Devices disconnected - stop updating but don't log every time
            return;
        }

        lock (_lock)
        {
            // Capture timestamp IMMEDIATELY when reading encoder (more accurate than later)
            var currentTime = DateTime.UtcNow;

            // Get current encoder positions and velocities
            var currentLeftPosition = -_leftWheelServo.CachedPosition;
            var currentRightPosition = _rightWheelServo.CachedPosition;
            var currentLeftVelocity = -_leftWheelServo.CachedVelocity;
            var currentRightVelocity = _rightWheelServo.CachedVelocity;
            // var currentTime = encoderTimestamp;  // Use captured timestamp

            // Calculate time delta
            var timeDelta = (currentTime - _lastUpdateTime).TotalSeconds;
            if (timeDelta <= 0 || _isFirstUpdate)
            {
                _lastLeftWheelPosition = currentLeftPosition;
                _lastRightWheelPosition = currentRightPosition;
                _lastLeftWheelVelocity = currentLeftVelocity;
                _lastRightWheelVelocity = currentRightVelocity;
                _lastUpdateTime = currentTime;
                // _lastThetaUpdateTime = currentTime;
                _isFirstUpdate = false;
                return;
            }

            // ===== LẤY GÓC YAW TUYỆT ĐỐI TỪ IMU ĐỂ TÍNH THETA =====
            var imuYawRaw = _imu.CachedOrientation.Vector.Z;

            // Transform from IMU frame to Odom frame (180° rotation around X-axis)
            // Yaw in odom frame is negated relative to raw IMU yaw.
            var imuYawInOdomFrame = NormalizeAngle(-imuYawRaw);

            // Capture startup yaw as zero reference so odom starts at 0 rad.
            if (!_imuYawZeroOffsetCaptured)
            {
                _imuYawZeroOffset = imuYawInOdomFrame;
                _imuYawZeroOffsetCaptured = true;
                _logger.LogInformation("Odometry IMU yaw zero-offset captured at {Offset:F6} rad", _imuYawZeroOffset);
            }

            // DeltaTheta = NormalizeAngle(imuYawInOdomFrame - _imuYawZeroOffset);
            DeltaTheta = imuYawInOdomFrame - _imuYawZeroOffset;
            while (DeltaTheta > Math.PI)  DeltaTheta -= 2.0 * Math.PI;
            while (DeltaTheta < -Math.PI) DeltaTheta += 2.0 * Math.PI;


            // Calculate wheel displacements (in encoder counts)
            var deltaLeft = currentLeftPosition - _lastLeftWheelPosition;
            var deltaRight = currentRightPosition - _lastRightWheelPosition;

            // Convert to meters
            var deltaLeftMeters = deltaLeft * _leftWheelMetersPerCount;
            var deltaRightMeters = deltaRight * _rightWheelMetersPerCount;
            // Console.WriteLine($"Delta Left: {deltaLeftMeters}, Delta Right: {deltaRightMeters}, deltaLeft: {deltaLeft}, deltaRight: {deltaRight}");

            // Calculate velocities (m/s)
            var leftVelocityMetersPerSec = currentLeftVelocity * _leftWheelMetersPerCount;
            var rightVelocityMetersPerSec = currentRightVelocity * _rightWheelMetersPerCount;

            // Calculate average velocity (không dùng angular velocity từ encoder)
            var linearVelocity = (leftVelocityMetersPerSec + rightVelocityMetersPerSec) / 2.0;
            var angularVelocityFromEncoders = (rightVelocityMetersPerSec - leftVelocityMetersPerSec) / _config.Wheelbase;

            // Tính độ dài
            var averageVelocity = (deltaLeftMeters + deltaRightMeters) / 2.0;
            var averageAngularVelocity = (-deltaRightMeters + deltaLeftMeters) / _config.Wheelbase;

            var previousTheta = PoseTheta;
            var currentTheta = DeltaTheta;
            var headingForIntegration = NormalizeAngle((previousTheta + currentTheta) * 0.5);

            // Use encoder position-delta displacement (averageVelocity) directly — no timeDelta needed
            PoseX = PoseX + linearVelocity * Math.Cos(DeltaTheta) * timeDelta;

            PoseY = PoseY + linearVelocity * Math.Sin(DeltaTheta) * timeDelta;

            // Integrate and normalize theta
            PoseTheta = DeltaTheta;

            if(count >= 200)
            {
                // logging
                Console.WriteLine($"linearVelocity: {linearVelocity}, headingForIntegration (rad): {headingForIntegration}, headingForIntegration (deg): {headingForIntegration * 180.0 / Math.PI}, timeDelta: {timeDelta}");
                Console.WriteLine($"PoseX: {PoseX}, PoseY: {PoseY}");
                Console.WriteLine($"PoseTheta (rad): {PoseTheta}, PoseTheta (deg): {PoseTheta * 180.0 / Math.PI}");
                count = 0;
            }
            else
            {
                count++;
            }
            // Build pose using integrated theta from IMU
            var pose = new Pose
            {
                Position = new Point
                {
                    X = PoseX,
                    Y = PoseY,
                    Z = 0.0
                },
                Orientation = EulerToQuaternion(0, 0, PoseTheta)
            };

            var twist = new Twist
            {
                Linear = new Vector3
                {
                    X = linearVelocity,
                    Y = 0.0,
                    Z = 0.0
                },
                Angular = new Vector3
                {
                    X = 0.0,
                    Y = 0.0,
                    // Z = wz  // Use IMU angular velocity
                    Z = angularVelocityFromEncoders  // Use encoder-based angular velocity for twist (for control)
                }
            };
            // Console.WriteLine($"Quaternion: {pose.Orientation.X}, {pose.Orientation.Y}, {pose.Orientation.Z}, {pose.Orientation.W}");
            // Console.WriteLine($"PoseTheta: {PoseTheta}, IMU wz: {wz}, Encoder Angular Vel: {angularVelocityFromEncoders}");
            // Build covariance matrices (simplified - can be improved with actual measurement uncertainty)
            var poseCovariance = new double[PoseWithCovariance.CovarianceSize];
            // Set diagonal values (position uncertainty: 0.01 m^2, orientation uncertainty: 0.01 rad^2)
            poseCovariance[0] = 0.0; // x
            poseCovariance[7] = 0.0; // y
            poseCovariance[14] = 0.0; // z
            poseCovariance[21] = 0.0; // roll
            poseCovariance[28] = 0.0; // pitch
            poseCovariance[35] = 0.0; // yaw

            var twistCovariance = new double[TwistWithCovariance.CovarianceSize];
            // Set diagonal values (linear velocity uncertainty: 0.1 m^2/s^2, angular velocity uncertainty: 0.1 rad^2/s^2)
            twistCovariance[0] = 0.0; // vx
            twistCovariance[7] = 0.0; // vy
            twistCovariance[14] = 0.0; // vz
            twistCovariance[21] = 0.0; // wx
            twistCovariance[28] = 0.0; // wy
            twistCovariance[35] = 0.0; // wz

            // Update odometry message
            _currentOdometry = new Odometry
            {
                Header = new Header
                {
                    Seq = _sequenceNumber++,
                    Stamp = DateTime.UtcNow,
                    FrameId = _config.FrameId
                },
                ChildFrameId = _config.ChildFrameId,
                Pose = new PoseWithCovariance
                {
                    Pose = pose,
                    Covariance = poseCovariance
                },
                Twist = new TwistWithCovariance
                {
                    Twist = twist,
                    Covariance = twistCovariance
                }
            };

            updatedOdometry = _currentOdometry;

            _sourceUpdateCount++;
            var elapsedSec = (currentTime - _lastSourceRateLogTime).TotalSeconds;
            if (elapsedSec >= 2.0)
            {
                var deltaCount = _sourceUpdateCount - _lastSourceUpdateCount;
                var sourceHz = deltaCount / elapsedSec;
                _logger.LogInformation(
                    "[ODOM-SOURCE-FREQ] rate={Rate:F1}Hz samples={Samples} window={Window:F2}s",
                    sourceHz,
                    deltaCount,
                    elapsedSec);
                _lastSourceRateLogTime = currentTime;
                _lastSourceUpdateCount = _sourceUpdateCount;
            }

            // Update last values
            _lastLeftWheelPosition = currentLeftPosition;
            _lastRightWheelPosition = currentRightPosition;
            _lastLeftWheelVelocity = currentLeftVelocity;
            _lastRightWheelVelocity = currentRightVelocity;
            _lastUpdateTime = currentTime;

            // DEBUG: Log raw odometry every 40 updates (~2 seconds at 20Hz)
            // if (_sequenceNumber % 40 == 0)
            // {
            //     var yaw = PoseTheta * 180.0 / Math.PI;
            //     _logger.LogInformation(
            //         "[RAW_ODOM] Pos: X={X:F3}m Y={Y:F3}m | Yaw={Yaw:F1}° | Vel: Linear={Vx:F3} Angular={Omega:F3} |Quaternion: {x},{y},{z},{w}" ,
            //         PoseX, PoseY, yaw, linearVelocity, wz, _currentOdometry.Pose.Pose.Orientation.X, _currentOdometry.Pose.Pose.Orientation.Y, _currentOdometry.Pose.Pose.Orientation.Z, _currentOdometry.Pose.Pose.Orientation.W
            //     );
            // }
        }

        if (updatedOdometry != null)
        {
            OdometryUpdated?.Invoke(this, new OdometryUpdatedEventArgs(updatedOdometry.Value));
        }
    }

    /// <summary>
    /// Chuyển đổi quaternion sang yaw angle (radians)
    /// </summary>
    private static double QuaternionToYaw(Quaternion q)
    {
        // Yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))
        var sinYaw = 2.0 * (q.W * q.Z + q.X * q.Y);
        var cosYaw = 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z);
        return Math.Atan2(sinYaw, cosYaw);
    }
    public static Quaternion EulerToQuaternion(double roll, double pitch, double yaw)
    {
        // Half angles
        double halfRoll  = roll  * 0.5;
        double halfPitch = pitch * 0.5;
        double halfYaw   = yaw   * 0.5;

        // Trigonometry
        double cr = Math.Cos(halfRoll);
        double sr = Math.Sin(halfRoll);
        double cp = Math.Cos(halfPitch);
        double sp = Math.Sin(halfPitch);
        double cy = Math.Cos(halfYaw);
        double sy = Math.Sin(halfYaw);

        // Quaternion (x, y, z, w)
        double w = cr * cp * cy + sr * sp * sy;
        double x = sr * cp * cy - cr * sp * sy;
        double y = cr * sp * cy + sr * cp * sy;
        double z = cr * cp * sy - sr * sp * cy;

        return new Quaternion(x, y, z, w);
    }
    private double NormalizeAngle(double angle)
    {
        while (angle > Math.PI)  angle -= 2.0 * Math.PI;
        while (angle < -Math.PI) angle += 2.0 * Math.PI;
        return angle;
    }
    public void Dispose()
    {
        if (_disposed)
            return;

        _disposed = true;

        try
        {
            _updateTimer?.Dispose();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disposing OdometryService");
        }
    }
}

/// <summary>
/// Cấu hình cho một bánh xe trong OdometryService
/// </summary>
public class OdometryWheelConfiguration
{
    /// <summary>
    /// Device ID của servo điều khiển bánh xe này
    /// </summary>
    public string DeviceId { get; set; } = string.Empty;

    /// <summary>
    /// Đường kính bánh xe (mét)
    /// </summary>
    public double WheelDiameter { get; set; }

    /// <summary>
    /// Số xung encoder trên 1 vòng quay của bánh xe
    /// </summary>
    public int PulsesPerRevolution { get; set; }

    /// <summary>
    /// Đảo chiều encoder/motor nếu hướng tăng xung ngược với chiều tiến của robot
    /// </summary>
    public bool IsReversed { get; set; } = false;
}

/// <summary>
/// Cấu hình cho OdometryService
/// </summary>
public class OdometryConfiguration
{
    /// <summary>
    /// Cấu hình bánh xe trái
    /// </summary>
    public OdometryWheelConfiguration LeftWheel { get; set; } = new();

    /// <summary>
    /// Cấu hình bánh xe phải
    /// </summary>
    public OdometryWheelConfiguration RightWheel { get; set; } = new();

    /// <summary>
    /// Khoảng cách giữa hai bánh xe (wheelbase) - mét
    /// </summary>
    public double Wheelbase { get; set; }

    /// <summary>
    /// Frame ID của odometry frame (thường là "odom")
    /// </summary>
    public string FrameId { get; set; } = "odom";

    /// <summary>
    /// Child frame ID (thường là "base_link")
    /// </summary>
    public string ChildFrameId { get; set; } = "base_link";
    
    /// <summary>
    /// IMU Device ID để lấy angular velocity
    /// </summary>
    public string ImuDeviceId { get; set; } = string.Empty;
}

public sealed class OdometryUpdatedEventArgs : EventArgs
{
    public OdometryUpdatedEventArgs(Odometry odometry)
    {
        Odometry = odometry;
    }

    public Odometry Odometry { get; }
}