namespace Ehwrj.Core.Models;

public sealed class ObjectTracker
{
    private readonly Dictionary<string, TrackPoint> _last = new(StringComparer.Ordinal);

    public MotionEstimate Update(string id, double? x, double? y)
    {
        if (!x.HasValue || !y.HasValue)
        {
            return MotionEstimate.Empty;
        }

        var now = DateTimeOffset.UtcNow;
        if (!_last.TryGetValue(id, out var prior))
        {
            _last[id] = new TrackPoint(x.Value, y.Value, now, null, null);
            return MotionEstimate.Empty;
        }

        var seconds = Math.Max(0.001, (now - prior.UpdatedAt).TotalSeconds);
        var distance = Math.Sqrt(Math.Pow(x.Value - prior.X, 2) + Math.Pow(y.Value - prior.Y, 2));
        var pseudoMetersPerSecond = distance * 100_000 / seconds;
        var rawMach = Math.Clamp(pseudoMetersPerSecond / 343.0, 0, 5);
        var mach = prior.Mach.HasValue ? prior.Mach.Value * 0.72 + rawMach * 0.28 : rawMach;
        var closure = prior.ClosureSpeed.HasValue ? prior.ClosureSpeed.Value * 0.65 + pseudoMetersPerSecond * 0.35 : pseudoMetersPerSecond;

        _last[id] = new TrackPoint(x.Value, y.Value, now, mach, closure);
        return new MotionEstimate(mach, closure);
    }

    private sealed record TrackPoint(double X, double Y, DateTimeOffset UpdatedAt, double? Mach, double? ClosureSpeed);
}

public readonly record struct MotionEstimate(double? Mach, double? ClosureSpeed)
{
    public static MotionEstimate Empty { get; } = new(null, null);
}
