using System;
using System.Numerics;

namespace LLib.Movement.Unstuck;

public sealed class StallDetector(StallDetectorOptions options)
{
	private bool _hasAnchor;

	private long _anchorMs;

	private Vector3 _anchorPosition;

	private DriveMode _anchorMode;

	private bool _anchorUsesProgress;

	private float _referenceBestDistance;

	private float _bestDistance;

	private long _lastDrivenMs;

	private long _cooldownUntilMs;

	public StallVerdict? Observe(in StallSample sample)
	{
		if (!sample.Driving)
		{
			return null;
		}
		bool flag = sample.Mode != DriveMode.Ground && sample.DistanceToTarget.HasValue;
		if (!_hasAnchor || sample.TimestampMs - _lastDrivenMs > options.ContinuityGapMs || sample.Mode != _anchorMode || flag != _anchorUsesProgress)
		{
			Anchor(in sample, flag);
			return null;
		}
		_lastDrivenMs = sample.TimestampMs;
		if (!flag)
		{
			return ObserveDisplacement(in sample);
		}
		return ObserveProgress(in sample);
	}

	public void Reset()
	{
		_hasAnchor = false;
		_cooldownUntilMs = 0L;
	}

	private StallVerdict? ObserveProgress(in StallSample sample)
	{
		float value = sample.DistanceToTarget.Value;
		if (value < _bestDistance)
		{
			_bestDistance = value;
		}
		float num = _referenceBestDistance - _bestDistance;
		if (num >= options.ProgressImprovementThreshold)
		{
			_referenceBestDistance = _bestDistance;
			_anchorMs = sample.TimestampMs;
			_anchorPosition = sample.Position;
			return null;
		}
		long num2 = sample.TimestampMs - _anchorMs;
		if (num2 >= options.ProgressWindowMs)
		{
			return Trip(in sample, num2, num);
		}
		return null;
	}

	private StallVerdict? ObserveDisplacement(in StallSample sample)
	{
		float num = ((sample.Mode == DriveMode.Ground) ? DistanceXZ(_anchorPosition, sample.Position) : Vector3.Distance(_anchorPosition, sample.Position));
		if (num >= options.GroundDistanceThreshold)
		{
			_anchorMs = sample.TimestampMs;
			_anchorPosition = sample.Position;
			return null;
		}
		long num2 = sample.TimestampMs - _anchorMs;
		if (num2 >= options.GroundWindowMs)
		{
			return Trip(in sample, num2, num);
		}
		return null;
	}

	private StallVerdict? Trip(in StallSample sample, long elapsedMs, float measured)
	{
		bool num = sample.TimestampMs < _cooldownUntilMs;
		Anchor(in sample, _anchorUsesProgress);
		if (num)
		{
			return null;
		}
		_cooldownUntilMs = sample.TimestampMs + options.CooldownMs;
		return new StallVerdict(elapsedMs, measured, sample.Mode);
	}

	private void Anchor(in StallSample sample, bool usesProgress)
	{
		_hasAnchor = true;
		_anchorMs = sample.TimestampMs;
		_anchorPosition = sample.Position;
		_anchorMode = sample.Mode;
		_anchorUsesProgress = usesProgress;
		_lastDrivenMs = sample.TimestampMs;
		_referenceBestDistance = (_bestDistance = sample.DistanceToTarget.GetValueOrDefault());
	}

	private static float DistanceXZ(Vector3 a, Vector3 b)
	{
		float num = a.X - b.X;
		float num2 = a.Z - b.Z;
		return MathF.Sqrt(num * num + num2 * num2);
	}
}