// using RobotNet.VDA5050.Type;
// using RobotNet10.RobotApp.Devices;
// using RobotNet10.RobotApp.Services.Exceptions;
// using System.Globalization;

// namespace RobotNet10.RobotApp.Services.Robot.Actions;

// /// <summary>
// /// Action đưa camera (lift) tới vị trí theo chiều cao (m).
// /// Gọi trực tiếp xuống động cơ CiA402 (giống device/hub), không qua LiftModule state machine.
// /// actionParameters: HEIGHT (unit: m), ví dụ "0.825".
// /// blockingType NONE: gửi lệnh di chuyển và kết thúc ngay, không chờ hoàn thành.
// /// </summary>
// [RobotAction(ActionType.LIFT_CAMERA_BY_HEIGHT,
//             [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
//             [BlockingType.NONE, BlockingType.HARD, BlockingType.SOFT],
//             "Lift camera with height (unit: m).",
//             "Lift camera move requested.")]
// public class LiftCameraByHeightAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
// {
//     private double _heightM;
//     private double _timeoutMs = 20 * 1000;

//     protected override void Initialize()
//     {
//         base.Initialize();

//         var heightParam = Action?.ActionParameters?.FirstOrDefault(p =>
//             string.Equals(p.Key, "HEIGHT", StringComparison.OrdinalIgnoreCase));
//         if (heightParam is null || string.IsNullOrWhiteSpace(heightParam.Value))
//         {
//             throw new ActionException("LiftCameraByHeight requires actionParameter HEIGHT (unit: m).");
//         }

//         if (!double.TryParse(heightParam.Value, NumberStyles.Any, CultureInfo.InvariantCulture, out _heightM))
//         {
//             throw new ActionException($"HEIGHT value '{heightParam.Value}' is not a valid number.");
//         }
//     }

//     protected override async Task StartAction()
//     {
//         try
//         {
//             var config = ServiceProvider.GetRequiredService<IConfiguration>();
//             var deviceProvider = ServiceProvider.GetRequiredService<IDeviceProvider>();

//             var deviceId = config.GetValue<string>("Modules:LiftModule:DeviceId") ?? "lift-motor";
//             var enable = config.GetValue<bool>("Modules:LiftModule:Enable", true);
//             if (!enable)
//             {
//                 SetStatus(ActionEvent.FAILED);
//                 ResultDescription = "Lift module is disabled in config.";
//                 return;
//             }

//             var device = deviceProvider.GetDevice(deviceId);
//             if (device is not ICiA402Servo servo)
//             {
//                 SetStatus(ActionEvent.FAILED);
//                 ResultDescription = $"Device '{deviceId}' not found or is not ICiA402Servo.";
//                 return;
//             }

//             // Đọc config để map height (m) -> position (encoder), giống LiftModuleService.GetPositionFromHeightM
//             var minHeightM = config.GetValue<double>("Modules:LiftModule:MinHeightM", 0);
//             var maxHeightM = config.GetValue<double>("Modules:LiftModule:MaxHeightM", 1);
//             var minPosition = config.GetValue<int>("Modules:LiftModule:MinPosition", 0);
//             var maxPosition = config.GetValue<int>("Modules:LiftModule:MaxPosition", 1000000);

//             int position;
//             if (maxHeightM <= minHeightM)
//             {
//                 position = minPosition;
//             }
//             else
//             {
//                 var t = Math.Clamp((_heightM - minHeightM) / (maxHeightM - minHeightM), 0, 1);
//                 position = minPosition + (int)(t * (maxPosition - minPosition));
//             }

//             var velocity = config.GetValue<uint>("Modules:LiftModule:ProfileVelocity", 100000);
//             var acceleration = config.GetValue<uint>("Modules:LiftModule:ProfileAcceleration", 50000);
//             var deceleration = config.GetValue<uint>("Modules:LiftModule:ProfileDeceleration", 50000);

//             Logger?.LogInformation("LiftCameraByHeight: calling servo directly (deviceId={DeviceId}), MoveToPositionAsync(position={Position}, height={HeightM} m).", deviceId, position, _heightM);
//             await servo.MoveToPositionAsync(position, velocity, acceleration, deceleration, CancellationToken.None);

//             // Timeout = (int)_timeoutMs;
//             SetStatus(ActionEvent.FINISHED);
//             ResultDescription = $"Lift camera move to height {_heightM} m requested (direct to drive).";    
//         }
//         catch (Exception ex)
//         {
//             SetStatus(ActionEvent.FAILED);
//             ResultDescription = $"Lift camera by height failed: {ex.Message}";
//         }

//         await base.StartAction();
//     }

//     // protected override async Task ExecuteAction()
//     // {
//     //     // to do: wait for the lift camera to reach the target height
        
//     // }

//     // protected override async Task CleanupAction()
//     // {
//     //     // to do: cleanup the lift camera
//     // }
// }

using RobotNet.VDA5050.Type;
using RobotNet10.CANOpen.CiA402.Enums;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Services.Exceptions;
using System.Globalization;

namespace RobotNet10.RobotApp.Services.Robot.Actions;

/// <summary>
/// Action đưa camera (lift) tới vị trí theo chiều cao (m).
/// Gọi trực tiếp xuống động cơ CiA402 (giống device/hub), không qua LiftModule state machine.
/// actionParameters: HEIGHT (unit: m), ví dụ "0.825".
/// blockingType NONE: gửi lệnh di chuyển và kết thúc ngay, không chờ hoàn thành.
/// </summary>
[RobotAction(ActionType.LIFT_CAMERA_BY_HEIGHT,
            [ActionScope.INSTANT, ActionScope.NODE, ActionScope.EDGE],
            [BlockingType.NONE, BlockingType.HARD],
            "Lift camera with height (unit: m).",
            "Lift camera move requested.")]
public class LiftCameraByHeightAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
    private double _heightM;

    protected override void Initialize()
    {
        base.Initialize();

        var heightParam = Action?.ActionParameters?.FirstOrDefault(p =>
            string.Equals(p.Key, "HEIGHT", StringComparison.OrdinalIgnoreCase));
        if (heightParam is null || string.IsNullOrWhiteSpace(heightParam.Value))
        {
            throw new ActionException("LiftCameraByHeight requires actionParameter HEIGHT (unit: m).");
        }

        if (!double.TryParse(heightParam.Value, NumberStyles.Any, CultureInfo.InvariantCulture, out _heightM))
        {
            throw new ActionException($"HEIGHT value '{heightParam.Value}' is not a valid number.");
        }
    }

    protected override async Task StartAction()
    {
        try
        {
            var config = ServiceProvider.GetRequiredService<IConfiguration>();
            var deviceProvider = ServiceProvider.GetRequiredService<IDeviceProvider>();

            var deviceId = config.GetValue<string>("Modules:LiftModule:DeviceId") ?? "lift-motor";
            var enable = config.GetValue<bool>("Modules:LiftModule:Enable", true);
            if (!enable)
            {
                SetStatus(ActionEvent.FAILED);
                ResultDescription = "Lift module is disabled in config.";
                return;
            }

            var device = deviceProvider.GetDevice(deviceId);
            if (device is not ICiA402Servo servo)
            {
                SetStatus(ActionEvent.FAILED);
                ResultDescription = $"Device '{deviceId}' not found or is not ICiA402Servo.";
                return;
            }

            // Đọc config để map height (m) -> position (encoder), giống LiftModuleService.GetPositionFromHeightM
            var minHeightM = config.GetValue<double>("Modules:LiftModule:MinHeightM", 0);
            var maxHeightM = config.GetValue<double>("Modules:LiftModule:MaxHeightM", 1);
            var minPosition = config.GetValue<int>("Modules:LiftModule:MinPosition", 0);
            var maxPosition = config.GetValue<int>("Modules:LiftModule:MaxPosition", 1000000);

            int position;
            if (maxHeightM <= minHeightM)
            {
                position = minPosition;
            }
            else
            {
                var t = Math.Clamp((_heightM - minHeightM) / (maxHeightM - minHeightM), 0, 1);
                position = minPosition + (int)(t * (maxPosition - minPosition));
            }

            var velocity = config.GetValue<uint>("Modules:LiftModule:ProfileVelocity", 100000);
            var acceleration = config.GetValue<uint>("Modules:LiftModule:ProfileAcceleration", 50000);
            var deceleration = config.GetValue<uint>("Modules:LiftModule:ProfileDeceleration", 50000);
            var tolerance = config.GetValue<int>("Modules:LiftModule:ActionTargetTolerance", 900);
            var checkIntervalMs = config.GetValue<int>("Modules:LiftModule:ActionStatusWordCheckIntervalMs", 100);
            var timeoutMs = config.GetValue<int>("Modules:LiftModule:ActionMoveTimeoutMs", 300000);

            Logger?.LogInformation("LiftCameraByHeight: calling servo directly (deviceId={DeviceId}), MoveToPositionAsync(position={Position}, height={HeightM} m).", deviceId, position, _heightM);
            await servo.MoveToPositionAsync(position, velocity, acceleration, deceleration, CancellationToken.None);
            await WaitForMovementCompletedAsync(servo, position, tolerance, checkIntervalMs, timeoutMs, CancellationToken.None);

            SetStatus(ActionEvent.FINISHED);
            ResultDescription = $"Lift camera reached height {_heightM} m (direct to drive).";
        }
        catch (Exception ex)
        {
            SetStatus(ActionEvent.FAILED);
            ResultDescription = $"Lift camera by height failed: {ex.Message}";
        }

        await base.StartAction();
    }

    private static async Task WaitForMovementCompletedAsync(
        ICiA402Servo servo,
        int targetPosition,
        int tolerance,
        int checkIntervalMs,
        int timeoutMs,
        CancellationToken ct)
    {
        var startedAt = DateTime.UtcNow;
        var pollInterval = Math.Max(10, checkIntervalMs);
        var maxWait = TimeSpan.FromMilliseconds(Math.Max(1000, timeoutMs));

        while (DateTime.UtcNow - startedAt < maxWait)
        {
            var statusword = await servo.GetStatuswordAsync(ct);
            if (statusword.GetState() == DriveState.Fault)
            {
                throw new ActionException("Lift movement failed: servo entered fault state.");
            }

            var currentPosition = await servo.GetActualPositionAsync(ct);
            if (statusword.TargetReached && Math.Abs(currentPosition - targetPosition) <= Math.Max(50, tolerance))
            {
                return;
            }

            await Task.Delay(pollInterval, ct);
        }

        throw new TimeoutException($"Lift movement timeout after {maxWait.TotalSeconds:F0}s.");
    }
}