using Microsoft.AspNetCore.Authorization;
using Microsoft.AspNetCore.SignalR;
using RobotNet10.CANOpen.CiA402.Enums;
using RobotNet10.CANOpen.CiA402.Models;
using RobotNet10.CANOpen.Exceptions;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;

namespace RobotNet10.RobotApp.Hubs;

/// <summary>
/// SignalR Hub cho CiA402Servo device - cung cấp real-time servo data
/// </summary>
[Authorize]
public class CiA402ServoHub(IDeviceProvider deviceProvider) : Hub
{
    /// <summary>
    /// Lấy thông tin device (DeviceName) theo device ID
    /// </summary>
    public async Task<DeviceInfoDto?> GetDeviceInfo(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo)
        {
            return null;
        }

        return new DeviceInfoDto
        {
            DeviceId = device.DeviceId,
            DeviceName = device.DeviceName
        };
    }

    /// <summary>
    /// Lấy dữ liệu servo theo device ID
    /// </summary>
    public async Task<CiA402ServoDataDto?> GetServoData(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
        {
            return null;
        }

        return await MapToDtoAsync(servo);
    }

    /// <summary>
    /// Map ICiA402Servo sang CiA402ServoDataDto. Các lệnh đọc SDO (GetOperationMode, GetLatestErrorCode, GetProfile*) có thể timeout
    /// khi bus bận hoặc drive không phản hồi — bắt exception và trả về dữ liệu từ cache + giá trị mặc định.
    /// </summary>
    private static async Task<CiA402ServoDataDto> MapToDtoAsync(ICiA402Servo servo)
    {
        var statusword = servo.CachedStatusword;
        var driveState = statusword.GetState();
        var position = servo.CachedPosition;
        var velocity = servo.CachedVelocity;
        var torque = servo.CachedTorque;

        OperationMode operationMode = OperationMode.ProfilePosition;
        try
        {
            operationMode = await servo.GetOperationModeAsync();
        }
        catch (CanOpenTimeoutException)
        {
            // SDO timeout (vd. 0x6061) — dùng mặc định, tránh fail hub
        }
        catch (Exception)
        {
            // Lỗi khác khi đọc mode — giữ mặc định
        }

        ushort errorCode = 0;
        try
        {
            errorCode = await servo.GetLatestErrorCodeAsync();
        }
        catch (CanOpenTimeoutException)
        {
            // SDO timeout — giữ 0
        }
        catch (Exception)
        {
            // Bỏ qua
        }

        uint profileSpeed = 0, profileAcceleration = 0, profileDeceleration = 0;
        try
        {
            profileSpeed = await servo.GetProfileSpeedAsync();
            profileAcceleration = await servo.GetProfileAccelerationAsync();
            profileDeceleration = await servo.GetProfileDecelerationAsync();
        }
        catch (CanOpenTimeoutException)
        {
            // Timeout — giữ 0
        }
        catch (Exception)
        {
            // Nếu drive chưa sẵn sàng đọc, giữ 0
        }

        return new CiA402ServoDataDto
        {
            Statusword = statusword.Value,
            DriveState = driveState.ToString(),
            OperationMode = operationMode.ToString(),
            Position = position,
            Velocity = velocity,
            Torque = torque,
            ErrorCode = errorCode,
            IsConnected = servo is not DeviceBase deviceBase || deviceBase.IsConnected,
            ProfileSpeed = profileSpeed,
            ProfileAcceleration = profileAcceleration,
            ProfileDeceleration = profileDeceleration
        };
    }

    // State Machine Control Methods
    public async Task EnableOperationAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.EnableOperationAsync();
    }

    public async Task DisableOperationAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.DisableOperationAsync();
    }

    public async Task QuickStopAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.QuickStopAsync();
    }

    public async Task FaultResetAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.FaultResetAsync();
    }

    public async Task ShutdownAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.ShutdownAsync();
    }

    public async Task SwitchOnAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SwitchOnAsync();
    }

    public async Task EnableAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.EnableAsync();
    }

    public async Task DisableAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.DisableAsync();
    }

    // Operation Mode
    public async Task SetOperationModeAsync(string deviceId, string modeString)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        if (!Enum.TryParse<OperationMode>(modeString, ignoreCase: true, out var mode))
            throw new ArgumentException($"Invalid operation mode: {modeString}");

        await servo.SetOperationModeAsync(mode);
    }

    // Position Control
    public async Task SetTargetPositionAsync(string deviceId, int position)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetTargetPositionAsync(position);
    }

    public async Task MoveToPositionAsync(string deviceId, int position, uint velocity = 1000, uint acceleration = 5000, uint deceleration = 5000)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.MoveToPositionAsync(position, velocity, acceleration, deceleration);
    }

    // Velocity Control
    public async Task SetTargetVelocityAsync(string deviceId, int velocity)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetTargetVelocityAsync(velocity);
    }

    public async Task TargetVelocityAsync(string deviceId, int targetVelocity, uint acceleration = 5000, uint deceleration = 5000)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.TargetVelocityAsync(targetVelocity, acceleration, deceleration);
    }

    public async Task ProfileVelocityAsync(string deviceId, int targetVelocity, uint acceleration = 5000, uint deceleration = 5000)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.ProfileVelocityAsync(targetVelocity, acceleration, deceleration);
    }

    // Torque Control
    public async Task SetTargetTorqueAsync(string deviceId, short torque)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetTargetTorqueAsync(torque);
    }

    public async Task RunTorqueAsync(string deviceId, short torque)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.RunTorqueAsync(torque);
    }

    // Profile Settings
    public async Task SetProfileAccelerationAsync(string deviceId, uint acceleration)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetProfileAccelerationAsync(acceleration);
    }

    public async Task SetProfileDecelerationAsync(string deviceId, uint deceleration)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetProfileDecelerationAsync(deceleration);
    }

    public async Task SetProfileVelocityAsync(string deviceId, uint velocity)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetProfileVelocityAsync(velocity);
    }

    public async Task SetProfileSpeedAsync(string deviceId, uint velocity)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetProfileSpeedAsync(velocity);
    }

    /// <summary>
    /// Ghi cùng lúc cả 3 profile (speed, acceleration, deceleration) xuống drive qua SDO.
    /// </summary>
    public async Task SetProfileSettingsAsync(string deviceId, uint profileSpeed, uint profileAcceleration, uint profileDeceleration)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetProfileSpeedAsync(profileSpeed);
        await servo.SetProfileAccelerationAsync(profileAcceleration);
        await servo.SetProfileDecelerationAsync(profileDeceleration);
    }

    // Homing
    public async Task SetHomingMethodAsync(string deviceId, byte method)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetHomingMethodAsync(method);
    }

    public async Task SetHomingSpeedAsync(string deviceId, int speed)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetHomingSpeedAsync(speed);
    }

    public async Task SetHomingOffsetAsync(string deviceId, int offset)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.SetHomingOffsetAsync(offset);
    }

    /// <summary>
    /// Đọc lại homing method từ drive để kiểm tra đã ghi xuống chưa.
    /// </summary>
    public async Task<byte> GetHomingMethodAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
        return await servo.GetHomingMethodAsync();
    }

    /// <summary>
    /// Đọc lại homing speed từ drive để kiểm tra đã ghi xuống chưa.
    /// </summary>
    public async Task<int> GetHomingSpeedAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
        return await servo.GetHomingSpeedAsync();
    }

    /// <summary>
    /// Đọc lại homing offset từ drive để kiểm tra đã ghi xuống chưa.
    /// </summary>
    public async Task<int> GetHomingOffsetAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");
        return await servo.GetHomingOffsetAsync();
    }

    public async Task StartHomingAsync(string deviceId, byte method, int speed)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.StartHomingAsync(method, speed);
    }

    // Position Control - Additional Methods
    public async Task StartPositionMoveAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.StartPositionMoveAsync();
    }

    public async Task WaitUntilAtTargetAsync(string deviceId, int tolerance = 100, bool useStatusword = true, int checkIntervalMs = 10)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        await servo.WaitUntilAtTargetAsync(tolerance, useStatusword, checkIntervalMs);
    }

    // Error and Status Information
    public async Task<bool> IsInFaultStateAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.IsInFaultStateAsync();
    }

    public async Task<byte> GetErrorRegisterAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.GetErrorRegisterAsync();
    }

    public async Task<ushort[]> GetErrorHistoryAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.GetErrorHistoryAsync();
    }

    public async Task<ushort> GetLatestErrorCodeAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.GetLatestErrorCodeAsync();
    }

    public async Task<bool> TryFaultResetAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.TryFaultResetAsync();
    }

    public async Task<bool> IsEnabledAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.IsEnabledAsync();
    }

    public async Task<bool> IsReadyAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.IsReadyAsync();
    }

    // Statusword & Controlword
    public async Task<ushort> GetStatuswordAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        var statusword = await servo.GetStatuswordAsync();
        return statusword.Value;
    }

    public async Task SetControlwordAsync(string deviceId, ushort controlwordValue)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        var controlword = new Controlword(controlwordValue);
        await servo.SetControlwordAsync(controlword);
    }

    public async Task<string> GetStateAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        var state = await servo.GetStateAsync();
        return state.ToString();
    }

    public async Task<string> GetOperationModeAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        var mode = await servo.GetOperationModeAsync();
        return mode.ToString();
    }

    // Position, Velocity, Torque
    public async Task<int> GetActualPositionAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.GetActualPositionAsync();
    }

    public async Task<int> GetActualVelocityAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.GetActualVelocityAsync();
    }

    public async Task<short> GetActualTorqueAsync(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return await servo.GetActualTorqueAsync();
    }

    // Target Position Checking
    public Task<bool> IsAtTarget(string deviceId, int tolerance = 100, bool useStatusword = true)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ICiA402Servo servo)
            throw new InvalidOperationException($"Device {deviceId} is not a CiA402Servo");

        return Task.FromResult(servo.IsAtTarget(tolerance, useStatusword));
    }
}