using Godot;
using System.Collections;
using System.Collections.Generic;

namespace Rokojori
{
	public class Distance
	{
    
    /*
    
    P = Point 
    S = Sphere
    L = Line
    C = Capsule

    
      P S L C   
    P y y y y   
    S   y y y      
    L     y y      
    C       y  

    */

    public static float Of( Vector3 a, Vector3 b )
    {
      return ( a - b ).Length();
    }    

    public static float Of( Vector3 a, Sphere b )
    {
      return b.DistanceToPoint( a );
    }

    public static float Of( Sphere a, Vector3 b )
    {
      return Of( b, a );
    }

    public static float Of( Vector3 a, Line3 b )
    {
      return b.DistanceToPoint( a );
    }

    public static float Of( Line3 b, Vector3 a )
    {
      return Of( a, b );
    }

    public static float Of( Vector3 a, Capsule3 b )
    {
      return b.DistanceToPoint( a );
    }
    
    public static float Of( Capsule3 a, Vector3 b )
    {
      return Of( b, a );
    }

    public static float Of( Sphere a, Sphere b )
    {
      return a.DistanceToSphere( b );
    }

    public static float Of( Sphere a, Line3 b )
    {
      var centerDistance = Of( a.center, b );
      return Mathf.Max( centerDistance - a.radius, 0 ); 
    } 

    public static float Of( Line3 a, Sphere b )
    {
      return Of( b, a );
    }

    public static float Of( Sphere a, Capsule3 b )
    {
      var centerDistance = Of( a.center, b );
      return Mathf.Max( centerDistance - a.radius, 0 ); 
    }

    public static float Of( Capsule3 a, Sphere b )
    {
      return Of( b, a );
    }

    public static float Of( Line3 a, Line3 b )
    {
      return a.DistanceToLine( b );
    }

    public static float Of( Line3 a, Capsule3 b )
    {
      var lineDistance = Of( a, b.centerLine );
      return Mathf.Max( 0, lineDistance - b.radius );
    }

    public static float Of( Capsule3 a, Line3 b )
    {
      return Of( b, a );
    }

    public static float Of( Capsule3 a, Capsule3 b )
    {
      var lineDistance = Of( a.centerLine, b.centerLine );
      return Mathf.Max( 0, lineDistance - ( a.radius + b.radius ) );
    }
    
    

    


  }
}