using RobotNet10.RobotApp.Client.Shared.Motion;
using RobotNet10.Shared.Geometry;

namespace RobotNet10.RobotApp.Hubs;

/// <summary>
/// Extension methods for OdometryDto conversions (server-side only)
/// </summary>
public static class OdometryDtoExtensions
{
    /// <summary>
    /// Convert from Pose to OdometryDto (compatibility path when only raw pose is available).
    /// </summary>
    public static OdometryDto ToOdometryDto(this Pose pose, double updateFrequency, string frameId = "odom")
    {
        return new OdometryDto
        {
            Timestamp = DateTime.UtcNow,
            FrameId = frameId,
            PositionX = pose.Position.X,
            PositionY = pose.Position.Y,
            PositionZ = pose.Position.Z,
            OrientationX = pose.Orientation.X,
            OrientationY = pose.Orientation.Y,
            OrientationZ = pose.Orientation.Z,
            OrientationW = pose.Orientation.W,
            UpdateFrequency = updateFrequency
        };
    }

    /// <summary>
    /// Convert OdometryDto to Pose (for Reset operations)
    /// </summary>
    public static Pose ToPose(this OdometryDto dto)
    {
        return new Pose
        {
            Position = new Point(dto.PositionX, dto.PositionY, dto.PositionZ),
            Orientation = new Quaternion(dto.OrientationX, dto.OrientationY, dto.OrientationZ, dto.OrientationW)
        };
    }
}