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

namespace RobotNet10.RobotApp.Hubs;

/// <summary>
/// SignalR Hub cho Odometry - hiển thị pose và velocity từ OdometryService (wheel encoder + IMU).
/// Cùng nguồn OdometryService.CurrentOdometry được XlocIntegrationService dùng để DispatchOdometry sang XLOC
/// (khi EnableRawOdometry bật) và NavigationIntegrationService dùng để dispatch sang navigation.
/// </summary>
public class OdometryHub(OdometryService odometryService) : Hub
{
    /// <summary>
    /// Lấy odometry hiện tại (pose + velocity) để hiển thị trên UI
    /// </summary>
    public Task<OdometryDto?> GetCurrentOdometry()
    {
        var odom = odometryService.CurrentOdometry;
        var dto = MapToDto(odom);
        return Task.FromResult<OdometryDto?>(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 // Caller can compute if needed
        };
    }
}