// using Appccelerate.StateMachine;
// using Appccelerate.StateMachine.Machine;
// using Microsoft.Extensions.Configuration;
// using Microsoft.Extensions.Hosting;
// using Microsoft.Extensions.Logging;
// using RobotNet10.CANOpen.CiA402.Enums;
// using RobotNet10.RobotApp.Devices;

// namespace RobotNet10.RobotApp.Modules;

// /// <summary>
// /// Triggers cho RotationModuleService state machine
// /// </summary>
// public enum RotationModuleTrigger
// {
//     StartInitialize,
//     InitializationCompleted,
//     StartHoming,
//     HomingCompleted,
//     HomingFailed,
//     StartMoving,
//     MovingCompleted,
//     MovingFailed,
//     ErrorOccurred,
//     Reset
// }

// /// <summary>
// /// Configuration cho RotationModuleService
// /// </summary>
// internal class RotationModuleConfiguration
// {
//     public bool Enable { get; set; }
//     public string DeviceId { get; set; } = string.Empty;
//     public byte HomingMethod { get; set; } = 35; // Default homing method
//     public int HomingSpeed { get; set; } = 1000; // encoder counts/s
//     public int HomingOffset { get; set; } = 0; // encoder counts
//     public uint ProfileVelocity { get; set; } = 2000; // encoder counts/s
//     public uint ProfileAcceleration { get; set; } = 5000; // encoder counts/s²
//     public uint ProfileDeceleration { get; set; } = 5000; // encoder counts/s²
//     public double Resolution { get; set; } = 360000.0; // encoder counts per 360 degrees (default: 360000 counts/360deg = 1000 counts/deg)
//     public bool IsReversed { get; set; } = false; // Đảo chiều động cơ (nếu true, góc dương sẽ quay ngược chiều)
//     public double? MinAngle { get; set; } // Giới hạn góc dưới (degrees, null = không giới hạn)
//     public double? MaxAngle { get; set; } // Giới hạn góc trên (degrees, null = không giới hạn)
//     public int StatusWordCheckIntervalMs { get; set; } = 50; // Interval để check status word khi moving
// }

// /// <summary>
// /// Service điều khiển module xoay
// /// Sử dụng CiA402Servo để điều khiển động cơ xoay
// /// </summary>
// public class RotationModuleService : IHostedService, IRotationModule, IDisposable
// {
//     private readonly PassiveStateMachine<RotationModuleState, RotationModuleTrigger> _stateMachine;
//     private readonly RotationModuleConfiguration _config;
//     private readonly IDeviceProvider _deviceProvider;
//     private readonly ILogger<RotationModuleService> _logger;
//     private readonly Lock _lock = new();

//     private ICiA402Servo? _servo;
//     private RotationModuleState _currentState = RotationModuleState.Uninitialized;
//     private bool _isHomed = false;
//     private bool _disposed = false;
    
//     public bool Enable => _config.Enable;
//     public bool IsReady => _currentState == RotationModuleState.Ready && _isHomed && _servo != null;
//     public RotationModuleState State => _currentState;

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

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

//         _config = new RotationModuleConfiguration();
//         configSection.Bind(_config);

//         // Validate configuration
//         ValidateConfiguration();

//         // Build state machine
//         _stateMachine = BuildStateMachine();
//         _stateMachine.Start();
//     }

//     private void ValidateConfiguration()
//     {
//         // Nếu Enable = false, không cần validate configuration
//         if (!_config.Enable)
//         {
//             _logger.LogInformation("RotationModule is disabled (Enable = false). Skipping configuration validation.");
//             return;
//         }

//         if (string.IsNullOrWhiteSpace(_config.DeviceId))
//             throw new InvalidOperationException("DeviceId is required in RotationModule configuration");

//         if (_config.HomingSpeed <= 0)
//             throw new InvalidOperationException("HomingSpeed must be > 0");

//         if (_config.ProfileVelocity <= 0)
//             throw new InvalidOperationException("ProfileVelocity must be > 0");

//         if (_config.ProfileAcceleration <= 0)
//             throw new InvalidOperationException("ProfileAcceleration must be > 0");

//         if (_config.ProfileDeceleration <= 0)
//             throw new InvalidOperationException("ProfileDeceleration must be > 0");

//         if (_config.Resolution <= 0)
//             throw new InvalidOperationException("Resolution must be > 0");

//         if (_config.MinAngle.HasValue && _config.MaxAngle.HasValue)
//         {
//             if (_config.MinAngle.Value >= _config.MaxAngle.Value)
//                 throw new InvalidOperationException("MinAngle must be less than MaxAngle");
//         }
//     }

//     private PassiveStateMachine<RotationModuleState, RotationModuleTrigger> BuildStateMachine()
//     {
//         var builder = new StateMachineDefinitionBuilder<RotationModuleState, RotationModuleTrigger>();

//         // Uninitialized state
//         builder.In(RotationModuleState.Uninitialized)
//             .ExecuteOnEntry(() =>
//             {
//                 lock (_lock)
//                 {
//                     _currentState = RotationModuleState.Uninitialized;
//                     _isHomed = false;
//                 }
//             })
//             .On(RotationModuleTrigger.StartInitialize)
//             .Goto(RotationModuleState.Initializing);

//         // Initializing state
//         builder.In(RotationModuleState.Initializing)
//             .ExecuteOnEntry(() =>
//             {
//                 lock (_lock)
//                 {
//                     _currentState = RotationModuleState.Initializing;
//                 }
//                 try
//                 {
//                     if (_servo == null)
//                         throw new InvalidOperationException("Servo is not available");

//                     // Set profile parameters (synchronous)
//                     _servo.SetProfileVelocity(_config.ProfileVelocity);
//                     _servo.SetProfileAcceleration(_config.ProfileAcceleration);
//                     _servo.SetProfileDeceleration(_config.ProfileDeceleration);
//                     _servo.SetHomingMethod(_config.HomingMethod);
//                     _servo.SetHomingSpeed(_config.HomingSpeed);
//                     _servo.SetHomingOffset(_config.HomingOffset);
//                     _stateMachine.Fire(RotationModuleTrigger.InitializationCompleted);
//                 }
//                 catch (Exception ex)
//                 {
//                     _logger.LogError(ex, "Error during initialization");
//                     _stateMachine.Fire(RotationModuleTrigger.ErrorOccurred);
//                 }
//             })
//             .On(RotationModuleTrigger.InitializationCompleted)
//             .Goto(RotationModuleState.Homing)
//             .On(RotationModuleTrigger.ErrorOccurred)
//             .Goto(RotationModuleState.Error);

//         // Homing state
//         builder.In(RotationModuleState.Homing)
//             .ExecuteOnEntry(() =>
//             {
//                 lock (_lock)
//                 {
//                     _currentState = RotationModuleState.Homing;
//                 }
//                 try
//                 {
//                     PerformHomingAsync();
//                     _stateMachine.Fire(RotationModuleTrigger.HomingCompleted);
//                 }
//                 catch (Exception ex)
//                 {
//                     _logger.LogError(ex, "Error during homing");
//                     _stateMachine.Fire(RotationModuleTrigger.HomingFailed);
//                 }
//             })
//             .On(RotationModuleTrigger.HomingCompleted)
//             .Goto(RotationModuleState.Ready)
//             .On(RotationModuleTrigger.HomingFailed)
//             .Goto(RotationModuleState.Error)
//             .On(RotationModuleTrigger.ErrorOccurred)
//             .Goto(RotationModuleState.Error);

//         // Ready state
//         builder.In(RotationModuleState.Ready)
//             .ExecuteOnEntry(() =>
//             {
//                 lock (_lock)
//                 {
//                     _currentState = RotationModuleState.Ready;
//                 }
//             })
//             .On(RotationModuleTrigger.StartMoving)
//             .Goto(RotationModuleState.Moving)
//             .On(RotationModuleTrigger.ErrorOccurred)
//             .Goto(RotationModuleState.Error);

//         // Moving state
//         builder.In(RotationModuleState.Moving)
//             .ExecuteOnEntry(() =>
//             {
//                 lock (_lock)
//                 {
//                     _currentState = RotationModuleState.Moving;
//                 }
//             })
//             .On(RotationModuleTrigger.MovingCompleted)
//             .Goto(RotationModuleState.Ready)
//             .On(RotationModuleTrigger.MovingFailed)
//             .Goto(RotationModuleState.Error)
//             .On(RotationModuleTrigger.ErrorOccurred)
//             .Goto(RotationModuleState.Error);

//         // Error state
//         builder.In(RotationModuleState.Error)
//             .ExecuteOnEntry(() =>
//             {
//                 lock (_lock)
//                 {
//                     _currentState = RotationModuleState.Error;
//                 }
//                 _logger.LogError("RotationModule state: Error");
//             })
//             .On(RotationModuleTrigger.Reset)
//             .Goto(RotationModuleState.Uninitialized);

//         return builder
//             .WithInitialState(RotationModuleState.Uninitialized)
//             .Build()
//             .CreatePassiveStateMachine();
//     }

//     #region IHostedService

//     public async Task StartAsync(CancellationToken cancellationToken)
//     {
//         try
//         {
//             // Nếu Enable = false, không khởi tạo
//             if (!_config.Enable)
//             {
//                 _logger.LogInformation("RotationModule is disabled (Enable = false). Service will not be initialized.");
//                 return;
//             }

//             // Đợi DeviceProvider kết nối xong tất cả devices
//             var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);

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

//             // Get CiA402Servo device
//             var device = _deviceProvider.GetDevice(_config.DeviceId);
//             if (device is not ICiA402Servo servo)
//             {
//                 _logger.LogError("Device '{DeviceId}' is not an ICiA402Servo", _config.DeviceId);
//                 return;
//             }

//             _servo = servo;

//             // Start initialization (will trigger homing automatically)
//             _stateMachine.Fire(RotationModuleTrigger.StartInitialize);
//         }
//         catch (Exception ex)
//         {
//             _logger.LogError(ex, "Error starting RotationModuleService");
//         }
//     }

//     public async Task StopAsync(CancellationToken cancellationToken)
//     {
//         try
//         {
//             // Disable servo if moving
//             if (_servo != null && _currentState == RotationModuleState.Moving)
//             {
//                 try
//                 {
//                     _servo.Disable();
//                 }
//                 catch (Exception ex)
//                 {
//                     _logger.LogWarning(ex, "Error disabling servo during stop");
//                 }
//             }

//             _stateMachine.Stop();
//         }
//         catch (Exception ex)
//         {
//             _logger.LogError(ex, "RotationModuleService: Error during StopAsync()");
//             throw;
//         }
//     }

//     #endregion

//     #region Private Methods

//     private void PerformHomingAsync()
//     {
//         if (_servo == null)
//             throw new InvalidOperationException("Servo is not available");

//         // Check if servo is in fault state
//         var isFault = _servo.IsInFaultState();
//         if (isFault)
//         {
//             _logger.LogWarning("Servo is in fault state, attempting fault reset...");
//             _servo.FaultReset();
//             Thread.Sleep(200);
//         }

//         // Enable servo
//         _servo.Enable();
//         Thread.Sleep(100);

//         // Perform homing
//         _servo.StartHoming();

//         // Wait for homing to complete
//         // Check statusword for homing completed
//         var maxWaitTime = TimeSpan.FromSeconds(120);
//         var startTime = DateTime.UtcNow;

//         while (DateTime.UtcNow - startTime < maxWaitTime)
//         {
//             var statusword = _servo.GetStatusword();
//             var state = statusword.GetState();
            
//             // Check for fault
//             if (state == DriveState.Fault)
//             {
//                 throw new InvalidOperationException("Homing failed: Servo entered fault state");
//             }

//             // Homing completion should be determined by CiA402 homing bits, not by drive state.
//             // During homing the drive is typically still OperationEnabled.
//             if (statusword.HomingAttained)
//             {
//                 lock (_lock)
//                 {
//                     _isHomed = true;
//                 }
//                 return;
//             }

//             // Check homing error bit
//             if (statusword.HomingError)
//             {
//                 throw new InvalidOperationException("Homing failed: Statusword indicates homing error");
//             }
//             Thread.Sleep(100);
//         }

//         throw new TimeoutException($"Homing timeout after {maxWaitTime.TotalSeconds} seconds");
//     }

//     /// <summary>
//     /// Convert angle (degrees) to encoder position (counts) relative to home position
//     /// Resolution is in counts per 360 degrees
//     /// </summary>
//     private int AngleToPosition(double angleDegrees)
//     {
//         // Convert angle to position: angleDegrees / 360 * Resolution
//         var position = (int)Math.Round(angleDegrees * _config.Resolution / 360.0);
        
//         // Apply reversal if needed
//         if (_config.IsReversed)
//         {
//             position = -position;
//         }
        
//         return position;
//     }

//     /// <summary>
//     /// Convert encoder position (counts) to angle (degrees) relative to home position
//     /// Resolution is in counts per 360 degrees
//     /// </summary>
//     private double PositionToAngle(int position)
//     {
//         // Calculate position relative to home
//         var relativePosition = position;
        
//         // Apply reversal if needed (before converting to angle)
//         if (_config.IsReversed)
//         {
//             relativePosition = -relativePosition;
//         }
        
//         // Convert position to angle: relativePosition / Resolution * 360
//         return relativePosition * 360.0 / _config.Resolution;
//     }

//     private async Task WaitForMovementCompletedAsync(CancellationToken cancellationToken = default)
//     {
//         if (_servo == null)
//             throw new InvalidOperationException("Servo is not available");

//         var maxWaitTime = TimeSpan.FromMinutes(5); // Long timeout for safety
//         var startTime = DateTime.UtcNow;
//         var checkInterval = TimeSpan.FromMilliseconds(_config.StatusWordCheckIntervalMs);

//         while (DateTime.UtcNow - startTime < maxWaitTime)
//         {
//             cancellationToken.ThrowIfCancellationRequested();

//             var statusword = _servo.GetStatusword();
//             var state = statusword.GetState();

//             // Check for fault
//             if (state == DriveState.Fault)
//             {
//                 throw new InvalidOperationException("Movement failed: Servo entered fault state");
//             }

//             // Check if target reached
//             if (statusword.TargetReached)
//             {
//                 return;
//             }

//             await Task.Delay(checkInterval, cancellationToken);
//         }

//         throw new TimeoutException($"Movement timeout after {maxWaitTime.TotalMinutes} minutes");
//     }

//     #endregion

//     #region IRotationModule Implementation
//     public async Task RotateToAngleAsync(double angleDegrees, CancellationToken cancellationToken = default)
//     {
//         if (!_config.Enable)
//         {
//             return;
//         }

//         // Check state before starting
//         var currentState = State;

//         if (currentState != RotationModuleState.Ready)
//         {
//             throw new InvalidOperationException($"RotationModule is not ready. Current state: {currentState}. Please wait until module is ready.");
//         }

//         if (_servo == null)
//             throw new InvalidOperationException("Servo is not available");

//         // Check limits
//         if (_config.MinAngle.HasValue && angleDegrees < _config.MinAngle.Value)
//         {
//             throw new ArgumentOutOfRangeException(nameof(angleDegrees), 
//                 $"Angle {angleDegrees}° is below MinAngle {_config.MinAngle.Value}°");
//         }

//         if (_config.MaxAngle.HasValue && angleDegrees > _config.MaxAngle.Value)
//         {
//             throw new ArgumentOutOfRangeException(nameof(angleDegrees), 
//                 $"Angle {angleDegrees}° is above MaxAngle {_config.MaxAngle.Value}°");
//         }

//         // Convert angle to encoder position relative to home
//         var targetPosition = AngleToPosition(angleDegrees);
        
//         // Fire trigger to enter Moving state
//         try
//         {
//             _stateMachine.Fire(RotationModuleTrigger.StartMoving);
//         }
//         catch (Exception ex)
//         {
//             _logger.LogError(ex, "Failed to fire StartMoving trigger");
//             throw new InvalidOperationException("Failed to start rotation", ex);
//         }

//         try
//         {
//             // MoveToPosition already handles: SetOperationMode, Enable, SetProfile parameters
//             _servo.MoveToPosition(
//                 targetPosition,
//                 _config.ProfileVelocity,
//                 _config.ProfileAcceleration,
//                 _config.ProfileDeceleration);

//             // Wait for movement to complete
//             await WaitForMovementCompletedAsync(cancellationToken);

//             // Return to Ready state
//             try
//             {
//                 _stateMachine.Fire(RotationModuleTrigger.MovingCompleted);
//             }
//             catch (Exception ex)
//             {
//                 _logger.LogError(ex, "Failed to fire MovingCompleted trigger. Current state: {State}", State);
//             }
//         }
//         catch (Exception ex)
//         {
//             _logger.LogError(ex, "Error during rotation to angle {Angle}°", angleDegrees);
//             try
//             {
//                 _stateMachine.Fire(RotationModuleTrigger.MovingFailed);
//             }
//             catch (Exception stateEx)
//             {
//                 _logger.LogError(stateEx, "Failed to fire MovingFailed trigger");
//             }
//             throw;
//         }
//     }

//     public async Task RotateOffsetAsync(double angleOffsetDegrees, CancellationToken cancellationToken = default)
//     {
//         if (!_config.Enable)
//         {
//             return;
//         }

//         // Get current angle
//         var currentAngle = await GetCurrentAngleAsync(cancellationToken);
//         var targetAngle = currentAngle + angleOffsetDegrees;

//         // Normalize angle to [-180, 180] range for better understanding (optional)
//         // But we don't restrict it here - let the absolute angle limits handle it
//         await RotateToAngleAsync(targetAngle, cancellationToken);
//     }

//     public Task<double> GetCurrentAngleAsync(CancellationToken cancellationToken = default)
//     {
//         if (!_config.Enable)
//         {
//             return Task.FromResult(0.0);
//         }

//         if (_servo == null)
//             throw new InvalidOperationException("Servo is not available");

//         var currentPosition = _servo.GetActualPosition();
//         return Task.FromResult(PositionToAngle(currentPosition));
//     }

//     public async Task<bool> IsHomedAsync(CancellationToken cancellationToken = default)
//     {
//         if (!_config.Enable)
//         {
//             return false;
//         }

//         lock (_lock)
//         {
//             return _isHomed;
//         }
//     }

//     #endregion

//     #region IDisposable

//     public void Dispose()
//     {
//         if (_disposed)
//             return;

//         _disposed = true;

//         try
//         {
//             _stateMachine.Stop();
//         }
//         catch (Exception ex)
//         {
//             _logger.LogError(ex, "Error disposing RotationModuleService");
//         }

//         GC.SuppressFinalize(this);
//     }

//     #endregion
// }