using System;

public enum SoftBoneColliderShapeType
{
	Sphere,
	Capsule,
	Box,
	Flat,
}

public readonly struct SoftBoneColliderShape
{
	public readonly SoftBoneColliderShapeType Type;
	public readonly Vector3 Start;
	public readonly Vector3 End;
	public readonly float Radius;
	public readonly Vector3 Center;
	public readonly Rotation Rotation;
	public readonly Vector3 Size;
	public readonly Vector3 Normal;

	private SoftBoneColliderShape( SoftBoneColliderShapeType type, Vector3 start, Vector3 end, float radius, Vector3 center, Rotation rotation, Vector3 size, Vector3 normal )
	{
		Type = type;
		Start = start;
		End = end;
		Radius = radius;
		Center = center;
		Rotation = rotation;
		Size = size;
		Normal = normal;
	}

	public bool IsSphere => Type == SoftBoneColliderShapeType.Sphere;
	public static SoftBoneColliderShape Sphere( Vector3 center, float radius ) => new( SoftBoneColliderShapeType.Sphere, center, center, radius, center, Rotation.Identity, Vector3.Zero, Vector3.Zero );
	public static SoftBoneColliderShape Capsule( Vector3 start, Vector3 end, float radius ) => new( SoftBoneColliderShapeType.Capsule, start, end, radius, (start + end) * 0.5f, Rotation.Identity, Vector3.Zero, Vector3.Zero );
	public static SoftBoneColliderShape Box( Vector3 center, Rotation rotation, Vector3 size ) => new( SoftBoneColliderShapeType.Box, Vector3.Zero, Vector3.Zero, 0f, center, rotation, size, Vector3.Zero );
	public static SoftBoneColliderShape Flat( Vector3 center, Vector3 normal ) => new( SoftBoneColliderShapeType.Flat, Vector3.Zero, Vector3.Zero, 0f, center, Rotation.Identity, Vector3.Zero, normal.Normal );
}

public static class SoftBoneCollisionSolver
{
	private const float Epsilon = 0.000008f;

	public static bool Resolve( ref Vector3 endpoint, Vector3 constraintBegin, Vector3 contactBegin, float boneRadius, float minLength, float maxLength, in SoftBoneColliderShape collider )
	{
		var link = endpoint - constraintBegin;
		var linkLength = link.Length;
		if ( linkLength <= Epsilon )
			return false;

		var contact = endpoint;
		var contactT = 1f;
		Vector3 closest;
		Vector3 normal;
		float penetration;

		switch ( collider.Type )
		{
			case SoftBoneColliderShapeType.Sphere:
			case SoftBoneColliderShapeType.Capsule:
				ClosestPointsOnSegments( contactBegin, endpoint, collider.Start, collider.End, out contact, out closest, out _ );
				contactT = Math.Clamp( Vector3.Dot( contact - constraintBegin, link ) / (linkLength * linkLength), 0f, 1f );
				if ( !TrySphereContact( contact, closest, boneRadius + collider.Radius, link, collider.End - collider.Start, out normal, out penetration ) )
					return false;
				break;

			case SoftBoneColliderShapeType.Box:
				contact = endpoint;
				closest = ClosestPointOnBox( contact, collider, out var isInside );
				var offset = contact - closest;
				var distance = offset.Length;
				if ( isInside )
				{
					normal = InsideBoxNormal( contact, collider );
					penetration = boneRadius + InsideBoxDepth( contact, collider );
				}
				else if ( distance < boneRadius )
				{
					normal = distance > Epsilon ? offset / distance : FallbackNormal( link, Vector3.Up );
					penetration = boneRadius - distance;
				}
				else return false;
				break;

			case SoftBoneColliderShapeType.Flat:
				contact = endpoint;
				var planeDistance = Vector3.Dot( contact - collider.Center, collider.Normal );
				if ( planeDistance >= boneRadius )
					return false;
				normal = collider.Normal;
				penetration = boneRadius - planeDistance;
				break;

			default:
				return false;
		}

		// The root remains fixed. Scaling the projection by contactT moves only the free tail.
		var influence = Math.Max( contactT, 0.12f );
		endpoint += normal * (penetration / influence);
		var corrected = endpoint - constraintBegin;
		var correctedLength = Math.Clamp( corrected.Length, minLength, maxLength );
		endpoint = constraintBegin + SafeNormal( corrected, link ) * correctedLength;
		return true;
	}

	private static bool TrySphereContact( Vector3 contact, Vector3 closest, float radius, Vector3 link, Vector3 bodyAxis, out Vector3 normal, out float penetration )
	{
		var offset = contact - closest;
		var distance = offset.Length;
		if ( distance >= radius )
		{
			normal = Vector3.Zero;
			penetration = 0f;
			return false;
		}

		normal = distance > Epsilon ? offset / distance : FallbackNormal( link, bodyAxis );
		penetration = radius - distance;
		return true;
	}

	private static Vector3 ClosestPointOnBox( Vector3 point, in SoftBoneColliderShape box, out bool isInside )
	{
		var local = box.Rotation.Inverse * (point - box.Center);
		var half = box.Size * 0.5f;
		isInside = Math.Abs( local.x ) <= half.x && Math.Abs( local.y ) <= half.y && Math.Abs( local.z ) <= half.z;
		local = new Vector3( Math.Clamp( local.x, -half.x, half.x ), Math.Clamp( local.y, -half.y, half.y ), Math.Clamp( local.z, -half.z, half.z ) );
		return box.Center + box.Rotation * local;
	}

	private static Vector3 InsideBoxNormal( Vector3 point, in SoftBoneColliderShape box )
	{
		var local = box.Rotation.Inverse * (point - box.Center);
		var half = box.Size * 0.5f;
		var x = half.x - Math.Abs( local.x );
		var y = half.y - Math.Abs( local.y );
		var z = half.z - Math.Abs( local.z );
		var normal = x <= y && x <= z ? new Vector3( local.x < 0f ? -1f : 1f, 0f, 0f ) : y <= z ? new Vector3( 0f, local.y < 0f ? -1f : 1f, 0f ) : new Vector3( 0f, 0f, local.z < 0f ? -1f : 1f );
		return box.Rotation * normal;
	}

	private static float InsideBoxDepth( Vector3 point, in SoftBoneColliderShape box )
	{
		var local = box.Rotation.Inverse * (point - box.Center);
		var half = box.Size * 0.5f;
		return Math.Min( half.x - Math.Abs( local.x ), Math.Min( half.y - Math.Abs( local.y ), half.z - Math.Abs( local.z ) ) );
	}

	private static Vector3 FallbackNormal( Vector3 boneAxis, Vector3 bodyAxis )
	{
		var normal = Vector3.Cross( boneAxis, bodyAxis );
		if ( normal.Length > Epsilon ) return normal.Normal;
		normal = Vector3.Cross( boneAxis, Vector3.Up );
		return normal.Length > Epsilon ? normal.Normal : Vector3.Right;
	}

	private static Vector3 SafeNormal( Vector3 value, Vector3 fallback ) => value.Length > Epsilon ? value.Normal : fallback.Normal;

	private static void ClosestPointsOnSegments( Vector3 a0, Vector3 a1, Vector3 b0, Vector3 b1, out Vector3 pointA, out Vector3 pointB, out float aT )
	{
		var a = a1 - a0;
		var b = b1 - b0;
		var r = a0 - b0;
		var aa = Vector3.Dot( a, a ); var bb = Vector3.Dot( b, b ); var ab = Vector3.Dot( a, b );
		var ar = Vector3.Dot( a, r ); var br = Vector3.Dot( b, r );
		var denominator = aa * bb - ab * ab;
		var s = 0f; var t = 0f;
		if ( aa <= Epsilon ) t = bb <= Epsilon ? 0f : Math.Clamp( br / bb, 0f, 1f );
		else if ( bb <= Epsilon ) s = Math.Clamp( -ar / aa, 0f, 1f );
		else { s = denominator > Epsilon ? Math.Clamp( (ab * br - bb * ar) / denominator, 0f, 1f ) : 0f; t = Math.Clamp( (ab * s + br) / bb, 0f, 1f ); s = Math.Clamp( (ab * t - ar) / aa, 0f, 1f ); }
		aT = s; pointA = a0 + a * s; pointB = b0 + b * t;
	}
}