using Godot;

public static class Geo
{

	public enum IntersectType
	{
		Before = -1, // Intersection with the infinite line occurs before p1
		In = 0,      // Intersection occurs between p1 and p2
		After = 1,   // Intersection with the infinite line occurs after p2
		Short = 2    // The sphere is already past the segment's endpoint
	};

	/// <summary>
	/// Calculates the intersection of a sphere with a line segment, always returning a point on the segment.
	/// </summary>
	/// <param name="sphereCenter">The world position of the sphere's center.</param>
	/// <param name="sphereRadius">The radius of the sphere.</param>
	/// <param name="segmentP1">The starting point of the line segment.</param>
	/// <param name="segmentP2">The ending point of the line segment.</param>
	/// <param name="intersectionPoint">The resulting intersection point, clamped to the segment.</param>
	/// <param name="intersectionType">The type of intersection that occurred.</param>
	/// <returns>True if an intersection with the infinite line is mathematically possible, false otherwise.</returns>
	public static bool SphereSegment(
			Vector3 sphereCenter,
			float sphereRadius,
			Vector3 segmentP1,
			Vector3 segmentP2,
			out Vector3 intersectionPoint,
			out IntersectType intersectionType )
	{
		Vector3 segmentVector = segmentP2 - segmentP1;
		float segmentLengthSq = segmentVector.LengthSquared();

		if( segmentLengthSq < 0.0001f )
		{
			intersectionPoint = segmentP2;
			intersectionType = IntersectType.Short;
			return false;
		}

		// Project sphereCenter onto the line defined by the segment
		float t = ( sphereCenter - segmentP1 ).Dot( segmentVector ) / segmentLengthSq;
		Vector3 closestPointOnLine = segmentP1 + t * segmentVector;

		float distSq = sphereCenter.DistanceSquaredTo( closestPointOnLine );
		if( distSq > sphereRadius * sphereRadius )
		{
			// No intersection with the infinite line, so return the closest point on the segment.
			intersectionPoint = closestPointOnLine; //.Clamp(segmentP1, segmentP2);
			intersectionType = t < 0 ? IntersectType.Before : IntersectType.After;
			return false;
		}

		// Calculate the intersection point on the infinite line
		float offset = Mathf.Sqrt( sphereRadius * sphereRadius - distSq );
		Vector3 idealIntersection = closestPointOnLine + segmentVector.Normalized() * offset;

		// Determine the intersection type based on the ideal point's position
		float finalT = ( idealIntersection - segmentP1 ).Dot( segmentVector ) / segmentLengthSq;

		if( finalT < 0 )
		{
			intersectionType = IntersectType.Before;
		}
		else if( finalT > 1.0f )
		{
			intersectionType = IntersectType.After;
		}
		else
		{
			intersectionType = IntersectType.In;
		}

		// Always return a point clamped to the segment
		intersectionPoint = idealIntersection; //.Clamp(segmentP1, segmentP2);
		return true;
	}


	/// <summary>
	/// Calculates the intersection of a sphere and a line segment.
	/// </summary>
	/// <param name="sphereCenter">The world position of the sphere's center.</param>
	/// <param name="sphereRadius">The radius of the sphere.</param>
	/// <param name="segmentP1">The starting point of the line segment.</param>
	/// <param name="segmentP2">The ending point of the line segment.</param>
	/// <param name="intersectionPoint">The point where the sphere first intersects the infinite line.</param>
	/// <param name="intersectionType">Indicates where the intersection lies: -1 for before the segment, 0 for on the segment, 1 for after the segment.</param>
	/// <returns>True if an intersection with the infinite line exists, false otherwise.</returns>
	public static bool SphereSegment_old(
			Vector3 sphereCenter,
			float sphereRadius,
			Vector3 segmentP1,
			Vector3 segmentP2,
			out Vector3 intersectionPoint,
			out IntersectType intersectionType )
	{
		// Initialize out parameters
		//intersectionPoint = Vector3.Zero;
		intersectionType = IntersectType.Before;

		Vector3 segmentVector = segmentP2 - segmentP1;
		float segmentLength = segmentVector.Length();

		// Handle zero-length segment case
		if( segmentLength < 0.0001f )
		{
			intersectionType = IntersectType.Short;
			intersectionPoint = segmentP2;
			return false;
		}

		Vector3 lineDir = segmentVector / segmentLength;

		// 1. Find the closest point on the infinite line to the sphere's center
		var toCenter = sphereCenter - segmentP1;
		float t = toCenter.Dot( lineDir );
		var closestPointOnLine = segmentP1 + lineDir * t;

		bool isOnLine = true;

		// 2. Check if an intersection is possible
		float distSq = sphereCenter.DistanceSquaredTo( closestPointOnLine );
		if( distSq > sphereRadius * sphereRadius )
		{
			isOnLine = false;
		}

		// 3. Calculate the intersection point
		float offset = Mathf.Sqrt( sphereRadius * sphereRadius - distSq );
		intersectionPoint = closestPointOnLine - lineDir * offset;

		// 4. Determine where the intersection lies relative to the segment
		float intersectionT = ( intersectionPoint - segmentP1 ).Dot( lineDir );

		if( intersectionT < 0 )
		{
			intersectionType = IntersectType.Before; // Before the segment
		}
		else if( intersectionT > segmentLength )
		{
			intersectionType = IntersectType.After;  // After the segment
		}
		else
		{
			intersectionType = IntersectType.In;  // On the segment
		}

		return isOnLine;
	}
}