using System;

/// <summary>
/// Integrates one SoftBone link in renderer-local space.
/// </summary>
public static class BoBiCoSoftBoneSolver
{
	private const float Epsilon = 0.000008f;

	/// <summary>
	/// One integration step.  All positions/vectors in renderer-local space.
	/// Returns the solved end position (renderer-local) and the new velocity.
	/// </summary>
	public static Vector3 IntegrateAdvanced(
		Vector3 begin,
		Vector3 endPoint,           // current physics end (may have had Immobile applied before call)
		Vector3 previousEndPoint,   // endPoint before Immobile, for velocity storage
		Vector3 poseVector,         // animated rest direction (renderer-local)
		Vector3 previousVector,     // last frame's (solvedEnd - begin)
		Vector3 previousVelocity,
		float minLength,
		float maxLength,
		float pull,
		float momentum,
		float stiffness,
		Vector3 gravityTargetEnd,   // begin + gravity-blended direction * restLength
		float frameRatio,           // Time.Delta * 60, used for frameLerp
		out Vector3 newVelocity )
	{
		var delta = previousVelocity * momentum;
		delta += (gravityTargetEnd - (endPoint + delta)) * pull;

		if ( stiffness > Epsilon )
		{
			var nextVector = endPoint + delta - begin;
			delta += (previousVector - nextVector) * stiffness;
		}

		endPoint += delta;
		var boundedVector = endPoint - begin;
		var boundedLength = Math.Clamp( boundedVector.Length, minLength, maxLength );
		endPoint = begin + SafeNormal( boundedVector, poseVector ) * boundedLength;

		// Limit per-step commitment so long frames do not inject a velocity spike.
		var frameLerp = Math.Clamp( (1f / Lerp( 1f, frameRatio, pull )) * frameRatio, 0f, 1f );
		endPoint = Lerp( previousEndPoint, endPoint, frameLerp );

		// velocity = where we actually moved to, not the raw delta
		newVelocity = endPoint - previousEndPoint;

		return endPoint;
	}

	/// <summary>
	/// Solve the head bone rotation to point toward the simulated tail.
	/// Works in renderer-local space.
	/// </summary>
	public static Rotation SolveHeadRotation(
		Vector3 poseVector,         // animated rest direction (renderer-local)
		Vector3 solvedVector,       // solvedEnd - begin (renderer-local)
		Rotation animatedRotation )
	{
		var restAxis = SafeNormal( poseVector, Vector3.Forward );
		var solvedAxis = SafeNormal( solvedVector, restAxis );
		return Rotation.FromToRotation( restAxis, solvedAxis ) * animatedRotation;
	}

	// ── Helpers (all renderer-local) ────────────────────────────────────────────

	public static Vector3 SafeNormal( Vector3 value, Vector3 fallback )
	{
		if ( value.Length <= Epsilon )
			return fallback.Length > Epsilon ? fallback.Normal : Vector3.Forward;

		return value.Normal;
	}

	private static Vector3 Lerp( Vector3 a, Vector3 b, float t ) => a + (b - a) * t;
	private static float Lerp( float a, float b, float t ) => a + (b - a) * t;
}