using CartographerSharp.Transform;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Helper class for converting between Cartographer Rigid3d and RobotNet10 Pose
/// </summary>
public static class PoseConverter
{
    /// <summary>
    /// Convert Rigid3d to Pose
    /// </summary>
    public static Pose ToPose(Rigid3d rigid3d)
    {
        return new Pose
        {
            Position = new Vector3
            {
                X = rigid3d.Translation.X,
                Y = rigid3d.Translation.Y,
                Z = rigid3d.Translation.Z
            },
            Orientation = new Quaternion
            {
                X = rigid3d.Rotation.X,
                Y = rigid3d.Rotation.Y,
                Z = rigid3d.Rotation.Z,
                W = rigid3d.Rotation.W
            }
        };
    }

    /// <summary>
    /// Convert Pose to Rigid3d
    /// </summary>
    public static Rigid3d ToRigid3d(Pose pose)
    {
        var translation = new Vector3(
            (float)pose.Position.X,
            (float)pose.Position.Y,
            (float)pose.Position.Z);

        var rotation = new Quaternion(
            (float)pose.Orientation.X,
            (float)pose.Orientation.Y,
            (float)pose.Orientation.Z,
            (float)pose.Orientation.W);

        return new Rigid3d(translation, rotation);
    }
}