using Microsoft.AspNetCore.SignalR;
using RobotNet10.RobotApp.Client.Shared.Motion;
using RobotNet10.RobotApp.Motion;
using RobotNet10.Shared.Sensor;

namespace RobotNet10.RobotApp.Hubs;

/// <summary>
/// HubContext để broadcast odometry đến tất cả clients (realtime hiển thị odom)
/// </summary>
public class OdometryHubContext
{
    private readonly IHubContext<OdometryHub> _hubContext;
    private readonly OdometryService _odometryService;

    public OdometryHubContext(IHubContext<OdometryHub> hubContext, OdometryService odometryService)
    {
        _hubContext = hubContext;
        _odometryService = odometryService;
    }

    /// <summary>
    /// Broadcast odometry hiện tại đến tất cả clients
    /// </summary>
    public async Task BroadcastOdometryAsync()
    {
        var odom = _odometryService.CurrentOdometry;
        var dto = MapToDto(odom);
        await _hubContext.Clients.All.SendAsync("ReceiveOdometry", dto);
    }

    private static OdometryDto MapToDto(Odometry odom)
    {
        var p = odom.Pose.Pose.Position;
        var q = odom.Pose.Pose.Orientation;
        var linear = odom.Twist.Twist.Linear;
        var angular = odom.Twist.Twist.Angular;

        return new OdometryDto
        {
            Timestamp = odom.Header.Stamp,
            FrameId = odom.Header.FrameId ?? "odom",
            ChildFrameId = odom.ChildFrameId ?? "base_link",
            PositionX = p.X,
            PositionY = p.Y,
            PositionZ = p.Z,
            OrientationX = q.X,
            OrientationY = q.Y,
            OrientationZ = q.Z,
            OrientationW = q.W,
            LinearVelocityX = linear.X,
            LinearVelocityY = linear.Y,
            LinearVelocityZ = linear.Z,
            AngularVelocityX = angular.X,
            AngularVelocityY = angular.Y,
            AngularVelocityZ = angular.Z,
            UpdateFrequency = 0
        };
    }
}