using System;

public enum SoftBoneLimitType 
{ 
	/// <summary>
	/// No Limits, The chain can move freely without angular limits.
	/// </summary>
	None, 

	/// <summary>
	/// Limits the movement within a cone-shaped angle from the rest direction.
	/// </summary>
	Cone, 

	/// <summary>
	/// Limits the movement to one side of a plane, preventing the chain from crossing it.
	/// </summary>
	Plane, 

	/// <summary>
	/// Limits the movement within an elliptical cone, allowing different horizontal and vertical angle limits.
	/// </summary>
	Ellipse 
}

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

	public static Vector3 Apply( Vector3 poseVector, Vector3 solvedDirection, SoftBoneLimitType type, float maxAngle, float maxYaw, float maxPitch, Angles limitRotation )
	{
		var direction = SafeNormal( solvedDirection, poseVector );
		if ( type == SoftBoneLimitType.None ) return direction;
		GetAxes( poseVector, limitRotation, out var axisX, out var axisY, out var axisZ );
		return type switch
		{
			SoftBoneLimitType.Cone => ClampCone( direction, axisX, axisY, maxAngle ),
			SoftBoneLimitType.Plane => ClampPlane( direction, axisX, axisY, axisZ, maxAngle ),
			SoftBoneLimitType.Ellipse => ClampEllipse( direction, axisX, axisY, axisZ, maxYaw, maxPitch ),
			_ => direction
		};
	}

	public static void GetAxes( Vector3 poseVector, Angles limitRotation, out Vector3 axisX, out Vector3 axisY, out Vector3 axisZ )
	{
		var frame = Rotation.FromToRotation( Vector3.Up, SafeNormal( poseVector, Vector3.Up ) ) * limitRotation.ToRotation();
		axisX = SafeNormal( frame * Vector3.Right, Vector3.Right);
		axisY = SafeNormal( frame * Vector3.Up, Vector3.Up );
		axisZ = SafeNormal( Vector3.Cross( axisX, axisY ), Vector3.Forward );
	}

	private static Vector3 ClampCone( Vector3 direction, Vector3 axisX, Vector3 axisY, float maxAngle )
	{
		if ( maxAngle >= 180f ) return direction;
		var angle = MathF.Acos( Math.Clamp( Vector3.Dot( axisY, direction ), -1f, 1f ) ) * 180f / MathF.PI;
		if ( angle <= maxAngle ) return direction;
		var rotationAxis = Vector3.Cross( axisY, direction );
		if ( rotationAxis.Length <= Epsilon ) rotationAxis = axisX;
		return Rotation.FromAxis( SafeNormal( rotationAxis, axisX ), maxAngle ) * axisY;
	}

	private static Vector3 ClampPlane( Vector3 direction, Vector3 axisX, Vector3 axisY, Vector3 axisZ, float maxAngle )
	{
		var planar = direction - axisX * Vector3.Dot( direction, axisX );
		planar = SafeNormal( planar, axisY );
		return ClampCone( planar, axisZ, axisY, maxAngle );
	}

	private static Vector3 ClampEllipse( Vector3 direction, Vector3 axisX, Vector3 axisY, Vector3 axisZ, float maxYaw, float maxPitch )
	{
		var x = Vector3.Dot( direction, axisZ );
		var y = Vector3.Dot( direction, axisX );
		var z = Vector3.Dot( direction, axisY );
		var yaw = Math.Clamp( MathF.Atan2( y, z ), -maxYaw * MathF.PI / 180f, maxYaw * MathF.PI / 180f );
		var pitch = Math.Clamp( MathF.Asin( Math.Clamp( x, -1f, 1f ) ), -maxPitch * MathF.PI / 180f, maxPitch * MathF.PI / 180f );
		var horizontal = MathF.Cos( pitch );
		return SafeNormal( axisZ * MathF.Sin( pitch ) + axisX * (MathF.Sin( yaw ) * horizontal) + axisY * (MathF.Cos( yaw ) * horizontal), axisY );
	}

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