namespace RobotNet10.NavigationTune.Shared.Models;

/// <summary>
/// 2D Pose (position + heading)
/// </summary>
public struct Pose2D
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Theta { get; set; } // radians

    public Pose2D(double x, double y, double theta)
    {
        X = x;
        Y = y;
        Theta = theta;
    }

    public static double Distance(Pose2D a, Pose2D b)
    {
        double dx = b.X - a.X;
        double dy = b.Y - a.Y;
        return Math.Sqrt(dx * dx + dy * dy);
    }
}

/// <summary>
/// 2D Twist (linear + angular velocity)
/// </summary>
public struct Twist2D
{
    public double Linear { get; set; }   // m/s
    public double Angular { get; set; }  // rad/s

    public Twist2D(double linear, double angular)
    {
        Linear = linear;
        Angular = angular;
    }
}