BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Hubs/MotionHub.cs

using Microsoft.AspNetCore.Authorization;
using Microsoft.AspNetCore.SignalR;
using RobotNet10.RobotApp.Client.Shared.Motion;
using RobotNet10.RobotApp.Client.Shared.Modules;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.Modules;

namespace RobotNet10.RobotApp.Hubs;

/// <summary>
/// SignalR Hub thống nhất cho tất cả các chức năng Motion:
/// - ManualControl (điều khiển thủ công qua RF Handle)
/// - Odometry (ước lượng vị trí robot)
/// - LiftModule (điều khiển nâng hạ)
/// - RotationModule (điều khiển xoay)
/// </summary>
[AllowAnonymous]
public class MotionHub(
    ManualControlService manualControlService,
    IOdometryEstimator odometryEstimator,
    ILiftModule liftModule,
    IRotationModule rotationModule) : Hub
{
    #region Manual Control

    /// <summary>
    /// Lấy trạng thái hiện tại của ManualControlService
    /// </summary>
    public Task<ManualControlStatusDto> GetStatus()
    {
        var status = manualControlService.CurrentRfHandleStatus;
        var twist = manualControlService.CurrentTwist;

        return Task.FromResult(new ManualControlStatusDto
        {
            State = manualControlService.State.ToString(),
            IsEnabled = manualControlService.State == ManualControlState.Maintenance ||
                        manualControlService.State == ManualControlState.Override,
            CurrentLinearVelocity = twist.Linear.X,
            CurrentAngularVelocity = twist.Angular.Z,
            RfHandleStatus = status != null ? new RfHandleStatusDto
            {
                Heartbeat = status.Heartbeat,
                Ready = status.Ready,
                Locked = status.Locked,
                EStop = status.EStop,
                Enable = status.Enable,
                Speed = status.Speed,
                Linear = status.Linear,
                Angular = status.Angular,
                Mode = status.Mode,
                LastUpdateTime = status.LastUpdateTime
            } : null
        });
    }

    /// <summary>
    /// Start manual control update loop
    /// </summary>
    public Task Enable()
    {
        manualControlService.Start();
        return Task.CompletedTask;
    }

    /// <summary>
    /// Stop manual control update loop
    /// </summary>
    public Task Disable()
    {
        manualControlService.Stop();
        return Task.CompletedTask;
    }

    #endregion

    #region Odometry

    /// <summary>
    /// Lấy pose hiện tại của robot (odometry)
    /// </summary>
    public OdometryDto GetCurrentPose()
    {
        var pose = odometryEstimator.CurrentPose;
        var frequency = odometryEstimator.UpdateFrequency;
        return pose.ToOdometryDto(frequency);
    }

    /// <summary>
    /// Reset odometry về vị trí ban đầu
    /// </summary>
    public void ResetOdometry() => odometryEstimator.ResetOdometry();

    /// <summary>
    /// Reset odometry về một pose cụ thể
    /// </summary>
    public void ResetOdometryPose(OdometryDto dto)
    {
        if (dto != null)
        {
            odometryEstimator.ResetOdometry(dto.ToPose());
        }
    }

    #endregion

    #region Lift Module

    /// <summary>
    /// Lấy trạng thái hiện tại của LiftModule
    /// </summary>
    public async Task<LiftModuleStatusDto> GetLiftStatus()
    {
        var position = await liftModule.GetCurrentPositionAsync();
        return new LiftModuleStatusDto
        {
            State = liftModule.State.ToString(),
            IsReady = liftModule.IsReady,
            CurrentPosition = position
        };
    }

    /// <summary>
    /// Lấy vị trí hiện tại của Lift
    /// </summary>
    public async Task<int> GetLiftCurrentPosition()
    {
        return await liftModule.GetCurrentPositionAsync();
    }

    /// <summary>
    /// Nâng lên
    /// </summary>
    public async Task LiftUp()
    {
        await liftModule.LiftUpAsync();
    }

    /// <summary>
    /// Hạ xuống
    /// </summary>
    public async Task LiftDown()
    {
        await liftModule.LiftDownAsync();
    }

    /// <summary>
    /// Dừng nâng/hạ (khi đang di chuyển)
    /// </summary>
    public async Task LiftStop()
    {
        await liftModule.LiftStopAsync();
    }

    /// <summary>
    /// Di chuyển đến vị trí cụ thể
    /// </summary>
    public async Task LiftToPosition(int position)
    {
        await liftModule.LiftToPositionAsync(position);
    }

    /// <summary>
    /// Kiểm tra Lift module đã homed chưa
    /// </summary>
    public async Task<bool> IsLiftHomed()
    {
        return await liftModule.IsHomedAsync();
    }

    /// <summary>
    /// Thực hiện homing thủ công cho LiftModule
    /// </summary>
    public async Task LiftHome()
    {
        await liftModule.HomeAsync();
    }

    #endregion

    #region Rotation Module

    /// <summary>
    /// Lấy trạng thái hiện tại của RotationModule
    /// </summary>
    public async Task<RotationModuleStatusDto> GetRotationStatus()
    {
        var angle = await rotationModule.GetCurrentAngleAsync();
        return new RotationModuleStatusDto
        {
            State = rotationModule.State.ToString(),
            IsReady = rotationModule.IsReady,
            CurrentAngle = angle
        };
    }

    /// <summary>
    /// Lấy góc hiện tại
    /// </summary>
    public async Task<double> GetRotationCurrentAngle()
    {
        return await rotationModule.GetCurrentAngleAsync();
    }

    /// <summary>
    /// Xoay đến góc cụ thể (absolute)
    /// </summary>
    public async Task RotateToAngle(double angleDegrees)
    {
        await rotationModule.RotateToAngleAsync(angleDegrees);
    }

    /// <summary>
    /// Xoay một góc offset (relative)
    /// </summary>
    public async Task RotateOffset(double angleOffsetDegrees)
    {
        await rotationModule.RotateOffsetAsync(angleOffsetDegrees);
    }

    /// <summary>
    /// Kiểm tra Rotation module đã homed chưa
    /// </summary>
    public async Task<bool> IsRotationHomed()
    {
        return await rotationModule.IsHomedAsync();
    }

    #endregion
}