using Godot;

namespace Rokojori
{
  public class Plane3
  {
    public Vector3 normal = Vector3.Up; 
    public float constant = 0;
    

    public Plane3(){}

    public void Set( Vector3 normal, float constant = 0 )
    {
      this.normal = normal;
      this.constant = constant;
    }

    public Vector3 ConstrainToPlane( Vector3 p, float distance = 10000 )
    { 
      var line3 = new Line3();
      line3.Set( p - normal * distance, p + normal * distance );

      var intersection = IntersectLine( line3 );

      if ( intersection == null )
      {
        return p; 
      }

      return (Vector3) intersection;
    }

    public Vector3 ClosestPointTo( Vector3 p )
    {
      return ConstrainToPlane( p );
    }

    public float DistanceTo( Vector3 p, float normalIntersectionDistance = 5000 )
    {
      var line = Line3.Create( p - normal * normalIntersectionDistance, p + normal * normalIntersectionDistance );
      var isc  = IntersectLine( line );

      if ( isc == null )
      {
        return float.PositiveInfinity;
      }   

      var xp = (Vector3)isc;

      return ( p - xp ).Length();
    }

    public Ray3 GetIntersectionRay( Plane3 other )
    {
      var lineDirection = normal.Cross( other.normal );

      var det = lineDirection.LengthSquared();

      if ( det == 0 )
      {
        return null;
      }

      var r = new Ray3();
      r.offset = ( ( lineDirection.Cross( other.normal ) * constant ) +
                 ( normal.Cross( lineDirection ) * other.constant ) ) / det;

      r.direction = lineDirection;

      return r;
    }

    public Vector3? IntersectLine( Line3 line )
    {
      var direction = line.direction;

  		var denominator = Math3D.Dot( normal, direction );

  		if ( denominator == 0 ) 
      {
        return null;
  		}

  		var t = - ( Math3D.Dot( line.start, normal ) + constant ) / denominator;

  		if ( t < 0 || t > 1 ) 
      {
  			return null;
  		}

  		return direction * t + line.start;
    }

    public static Plane3 GetZeroUp()
    {
      var plane = new Plane3();
      plane.Set( Vector3.Up, 0 );
      return plane;
    }

    public void SetFromNormalAndCoplanarPoint( Vector3 normal, Vector3 point ) 
    {
      this.normal = normal;
      this.constant = - Math3D.Dot( point, this.normal );
    }       

    public void SetFromCoplanarPoints( Vector3 a, Vector3 b, Vector3 c ) 
    {
      var normal = Math3D.ComputeNormal( a, b, c );
      SetFromNormalAndCoplanarPoint( normal, a );
    }

    public static Plane3 FromCoplanarPoints( Vector3 a, Vector3 b, Vector3 c ) 
    {
      var p = new Plane3();
      p.SetFromCoplanarPoints( a, b, c );

      return p;
    }
  }
}