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

namespace Rokojori
{
  public static class Math3D
  {
    public enum Axis 
    {
      X, Y, Z
    }
    
    public static float LookingAtEachOtherAngle( Vector3 lookDirectionA, Vector3 lookDirectionB )
    {
      return Dot( lookDirectionA, lookDirectionB );
    }

    public static bool LookingAtEachOther( Vector3 lookDirectionA, Vector3 lookDirectionB )
    {
      return LookingAtEachOtherAngle( lookDirectionA, lookDirectionB ) > 0;
    }

    public static bool FacingSameDirection( Vector3 lookDirectionA, Vector3 lookDirectionB )
    {
      return ! LookingAtEachOther( lookDirectionA, lookDirectionB );
    }

    public static bool LookingTowards( Vector3 from, Vector3 fromDirection, Vector3 to )
    {
      return LookingAtEachOther( fromDirection, to - from );
    }


    public static Vector4 Lerp( this Vector3 w, Vector4 x, Vector4 y, Vector4 z )
    {
      return w.X * x + w.Y * y + w.Z * z;
    }

    public static Vector3 Lerp( this Vector3 w, Vector3 x, Vector3 y, Vector3 z )
    {
      return w.X * x + w.Y * y + w.Z * z;
    }
    

    public static Vector2 Lerp( this Vector3 w, Vector2 x, Vector2 y, Vector2 z )
    {
      return w.X * x + w.Y * y + w.Z * z;
    }

    public static float Lerp( this Vector3 w, float x, float y, float z )
    {
      return w.X * x + w.Y * y + w.Z * z;
    }

    public static Vector3 Clamp( Vector3 x, Vector3 min, Vector3 max )
    {
      return x.Clamp( min, max );
    }
    

    public static Vector3 ComputeAverage( List<Vector3> points )
    {
      if ( points == null || points.Count == 0 )
      {
        return Vector3.Zero;
      }

      var mean = Vector3.Zero;

      for ( int i = 0; i < points.Count; i++ )
      {
          mean += ( points[ i ] - mean ) / ( i + 1 );
      }

      return mean;

    }

    
    public static Vector3 ComputeAverage<T>( List<T> containers, Func<T,Vector3> getPoint )
    {
      if ( containers == null || containers.Count == 0 )
      {
        return Vector3.Zero;
      }

      var mean = Vector3.Zero;

      for ( int i = 0; i < containers.Count; i++ )
      {
        mean += ( getPoint( containers[ i ] ) - mean ) / ( i + 1 );
      }

      return mean;
    }   

    public static Vector4 ComputeAverage( List<Vector4> points )
    {
      if ( points == null || points.Count == 0 )
      {
        return Vector4.Zero;
      }

      var mean = Vector4.Zero;

      for ( int i = 0; i < points.Count; i++ )
      {
          mean += ( points[ i ] - mean ) / ( i + 1 );
      }

      return mean;

    }

    public static Vector4 ComputeAverage<T>( List<T> containers, Func<T,Vector4> getPoint )
    {
      if ( containers == null || containers.Count == 0 )
      {
        return Vector4.Zero;
      }

      var mean = Vector4.Zero;

      for ( int i = 0; i < containers.Count; i++ )
      {
        mean += ( getPoint( containers[ i ] ) - mean ) / ( i + 1 );
      }

      return mean;
    }

    public static Transform3D TRS( Vector3 translation, Quaternion rotation, Vector3 scale )
    {
      var trsf = new Transform3D( new Basis( rotation ), translation );
      return trsf.ScaledLocal( scale );
    }

    public static Transform3D TRS( Vector3 translation, Vector3 eulerRotation, Vector3 scale )
    {
      var rotation = Quaternion.FromEuler( eulerRotation );
      return TRS( translation, rotation, scale );
    }

    public static Transform3D TRS( Vector3 translation, Vector4 rotation, Vector3 scale )
    {
      var quaternion = new Quaternion( rotation.X, rotation.Y, rotation.Z, rotation.W );

      return TRS( translation, quaternion.Normalized(), scale );
    }


    public static Transform3D TRS( Pose pose, Vector3 scale )
    {
      return TRS( pose.position, pose.rotation, scale );
    }

    public static Vector4 QuaternionToVector4( Quaternion rotation )
    {
      return new Vector4( rotation.X, rotation.Y, rotation.Z, rotation.W );
    }

    public static Vector3 Clamp01( Vector3 v )
    {
      return new Vector3(
        MathX.Clamp01( v.X ),
        MathX.Clamp01( v.Y ),
        MathX.Clamp01( v.Z )
      );
    }

    public static Vector3 OnCircleXZ( float radians, float size = 1 )
    {
      var x = Mathf.Cos( radians ) * size;
      var z = Mathf.Sin( radians ) * size;

      return new Vector3( x, 0, z );
    }

    public static Vector3 OnCircleXY( float radians, float size = 1 )
    {
      var x = Mathf.Cos( radians ) * size;
      var y = Mathf.Sin( radians ) * size;

      return new Vector3( x, y, 0 );
    }

    public static Vector3 XYasXZ( Vector2 v )
    {
      return new Vector3( v.X, 0, v.Y );
    }

    public static Vector3 XY( Vector2 v )
    {
      return new Vector3( v.X, v.Y, 0.0f );
    }

    public static float Dot( Vector3 a, Vector3 b )
    {
      return a.Dot( b );
    }

    public static Vector3 Cross( Vector3 a, Vector3 b )
    {
      return a.Cross( b );
    }

    public static Vector3 Fract( Vector3 a )
    {
      return new Vector3( MathX.Fract( a.X ), MathX.Fract( a.Y ), MathX.Fract( a.Z ) );
    }

    public static bool IsExactlyZero( Vector3 v )
    {
      return v.X == 0 && v.Y == 0 && v.Z == 0;
    } 

    
    public static Vector3 ConstrainLength( this Vector3 v, float maxLength )
    {
      var length = v.Length();

      if ( length <= maxLength )
      {
        return v;
      }

      return v.Normalized() * maxLength;
    }

    public static Vector3 ComputeNormalFast( Vector3 a, Vector3 b, Vector3 c )
    {
      return Math3D.Cross( c - b , a - b  ).Normalized();
    }



    public static Vector3 ComputeNormal( Vector3 a, Vector3 b, Vector3 c )
    {
      var cross = Math3D.Cross( c - b , a - b  );

      if ( Math3D.IsExactlyZero( cross ) )
      {
        cross = Math3D.Cross( b - c , a - c  );
      }

      if ( Math3D.IsExactlyZero( cross ) )
      {
        cross = Math3D.Cross( c - a , b - a  );
      }

      return cross.Normalized();
    }


    public static Vector3 SmoothStep( Vector3 a, Vector3 b, Vector3 t )
    {
      var x = MathX.Smoothstep( a.X, b.X, t.X );
      var y = MathX.Smoothstep( a.Y, b.Y, t.Y );
      var z = MathX.Smoothstep( a.Z, b.Z, t.Z ); 

      return new Vector3( x, y, z );
    }

    public static Vector3 Center( params Vector3[] points )
    {
      var center = Vector3.Zero;

      if ( points.Length == 0 )
      {
        return center;
      }

      for ( int i = 0; i < points.Length; i++ )
      {
        center += points[ i ];
      }

      return center / points.Length;
    }

    
    public static Vector3 Center<N>( List<N> points ) where N:Node3D
    {
      var center = Vector3.Zero;

      if ( points.Count == 0 )
      {
        return center;
      }

      for ( int i = 0; i < points.Count; i++ )
      {
        center += points[ i ].GlobalPosition;
      }

      return center / points.Count;
    }

    public static Vector3 Center( List<Vector3> points )
    {
      var center = Vector3.Zero;

      if ( points.Count == 0 )
      {
        return center;
      }

      for ( int i = 0; i < points.Count; i++ )
      {
        center += points[ i ];
      }

      return center / points.Count;
    }

    public static Vector3 GetClosest( Vector3 from, params Vector3[] points )
    {
      var distance = float.MaxValue;
      var index = -1;

      for ( int i = 0; i < points.Length; i++ )
      {
        var d = from.DistanceSquaredTo( points[ i ] );

        if ( d < distance )
        {
          index = i;
          distance = d;
        }
      }

      return points[ index ];
    }

    public static Vector3 GetClosest( Vector3 from, List<Vector3> points )
    {
      var distance = float.MaxValue;
      var index = -1;

      for ( int i = 0; i < points.Count; i++ )
      {
        var d = from.DistanceSquaredTo( points[ i ] );

        if ( d < distance )
        {
          index = i;
          distance = d;
        }
      }

      return points[ index ];
    }


    public static Vector3 LerpUnclamped( Vector3 a, Vector3 b, float amount )
    {
      return a + amount * ( b - a );
    }

    public static Vector3 LerpClamped( Vector3 a, Vector3 b, float amount )
    {
      return LerpUnclamped( a, b, MathX.Clamp01( amount ) );
    }

    

    public static Vector3 GetMin<T>( List<T> data, Func<T,Vector3> getPosition )
    {
      var min = new Vector3( float.MaxValue, float.MaxValue, float.MaxValue );

      for ( int i = 0; i < data.Count; i++ )
      {
        min = min.Min( getPosition( data[ i ] ) );
      }

      return min;
    }

    public static Vector3 GetMax<T>( List<T> data, Func<T,Vector3> getPosition )
    {
      var max = new Vector3( -float.MaxValue, -float.MaxValue, -float.MaxValue );

      for ( int i = 0; i < data.Count; i++ )
      {
        max = max.Max( getPosition( data[ i ] ) );
      }

      return max;
    }

    public static float MaxDimension( this Vector3 v )
    {
      return MathX.Max( v.X, v.Y, v.Z );
    }

     public static float MinDimension( this Vector3 v )
    {
      return MathX.Min( v.X, v.Y, v.Z );
    }

    public static Vector3 MinGlobalPosition( Node3D a, Node3D b )
    {
      return a.GlobalPosition.Min( b.GlobalPosition );
    }

    public static Vector3 MaxGlobalPosition( Node3D a, Node3D b )
    {
      return a.GlobalPosition.Max( b.GlobalPosition );
    }

    public static Vector3 MinLocalPosition( Node3D a, Node3D b )
    {
      return a.Position.Min( b.Position );
    }

    public static Vector3 MaxLocalPosition( Node3D a, Node3D b )
    {
      return a.Position.Max( b.Position );
    }

    public static Vector3 SnapRounded( this Vector3 v, Vector3 snapping )
    {
      v.X = MathX.SnapRounded( v.X, snapping.X );
      v.Y = MathX.SnapRounded( v.Y, snapping.Y );
      v.Z = MathX.SnapRounded( v.Z, snapping.Z );

      return v;
    }

     public static Vector3 SnapRoundedXZ( this Vector3 v, float snapX, float snapZ )
    {
      v.X = MathX.SnapRounded( v.X, snapX );
      v.Z = MathX.SnapRounded( v.Z, snapZ );

      return v;
    }

    public static Vector3 SnapCeiled( this Vector3 v, Vector3 snapping )
    {
      v.X = MathX.SnapCeiled( v.X, snapping.X );
      v.Y = MathX.SnapCeiled( v.Y, snapping.Y );
      v.Z = MathX.SnapCeiled( v.Z, snapping.Z );

      return v;
    }

    public static Vector3 SnapFloored( this Vector3 v, Vector3 snapping )
    {
      v.X = MathX.SnapFloored( v.X, snapping.X );
      v.Y = MathX.SnapFloored( v.Y, snapping.Y );
      v.Z = MathX.SnapFloored( v.Z, snapping.Z );

      return v;
    }

   

    public static Vector3I RoundToInt( this Vector3 v )
    {
      return new Vector3I( Mathf.RoundToInt( v.X ) , Mathf.RoundToInt( v.Y ), Mathf.RoundToInt( v.Z ) );
    }

    public static Vector3I FloorToInt( this Vector3 v )
    {
      return new Vector3I( Mathf.FloorToInt( v.X ) , Mathf.FloorToInt( v.Y ), Mathf.FloorToInt( v.Z ) );
    }

    public static Vector3I CeilToInt( this Vector3 v )
    {
      return new Vector3I( Mathf.CeilToInt( v.X ) , Mathf.CeilToInt( v.Y ), Mathf.CeilToInt( v.Z ) );
    }

    public static Vector2 XY( this Vector3 v )
    {
      return new Vector2( v.X, v.Y );
    }

    public static Vector2 YX( this Vector3 v )
    {
      return new Vector2( v.Y, v.X );
    }

    public static Vector2 XZ( this Vector3 v )
    {
      return new Vector2( v.X, v.Z );
    }    

    public static Vector2 ZX( this Vector3 v )
    {
      return new Vector2( v.Z, v.X );
    }

    public static Vector2 YZ( this Vector3 v )
    {
      return new Vector2( v.Y, v.Z );
    }

    public static Vector2 ZY( this Vector3 v )
    {
      return new Vector2( v.Y, v.Z );
    }

    public static Vector3 ZeroY( this Vector3 v )
    {
      return new Vector3( v.X, 0, v.Z );
    }

    public static Vector3 SetY( this Vector3 v, float y )
    {
      return new Vector3( v.X, y, v.Z );
    }

    public static Basis AlignUp( Basis basis, Vector3 upDirection )
    {
      basis.Y = upDirection;
      basis.X = - basis.Z.Cross( upDirection );
      return basis.Orthonormalized();
    }
    

    public static Quaternion AlignUp( Quaternion rotation, Vector3 upDirection )
    {
      var basis = new Basis( rotation );
      var aligned = AlignUp( basis, upDirection );
      return aligned.GetRotationQuaternion();
    }


    public static Quaternion AlignUp( Vector3 upDirection, Quaternion? q = null )
    {
      var quaternion = q == null ? Quaternion.Identity : (Quaternion) q;
      return AlignUp( quaternion, upDirection );
    }

    public static Quaternion AlignUpFromDirections( Vector3 upFrom, Vector3 upTo )
    {
      var fromRotation = AlignUp( upFrom );

      return AlignUp( fromRotation, upTo );
    }

    public static float AngleXY( Vector3 direction )
    {
      return Mathf.Atan2( direction.Y, direction.X );
    }

    public static float AngleXZ( Vector3 direction )
    {
      return Mathf.Atan2( direction.Z, direction.X );
    }

    public static Vector3 NormalAngle( float angle )
    {
      var y = Mathf.Sin( angle );
      var z = Mathf.Cos( angle );

      return new Vector3( 0, y, z );
    }

   


    public static Vector3 Lerp( Vector3 a, Vector3 b, float lerp )
    {
      return a.Lerp( b, lerp );
    }

    // public static Vector4 Lerp( Vector4 a, Vector4 b, float lerp )
    // {
    //   return a.Lerp( b, lerp );
    // }


    public static Vector3 LerpGlobalPosition( Node3D a, Node3D b, float lerp )
    {
      return a.GlobalPosition.Lerp( b.GlobalPosition, lerp );
    }

    public static Quaternion LookRotation( Vector3 direction, bool useModelFront = false )
    { 
      if ( direction.Normalized() == Vector3.Up )
      {
        return LookRotation( direction, Vector3.Back, useModelFront );
      }

      return LookRotation( direction, Vector3.Up, useModelFront );
    }

    public static Quaternion LookRotation( Vector3 direction, Vector3 up, bool useModelFront = false )
    {
      if ( direction.Length() == 0 )
      {
        return Quaternion.Identity;
      }
      
      var t = new Transform3D();
      t.Basis = Basis.Identity;
      t.Origin = Vector3.Zero;    

      t = t.LookingAt( direction, up, useModelFront );

      return t.Basis.GetRotationQuaternion();
    }

    public static Vector3 LerpComponents( Vector3 a, Vector3 b, Vector3 t )
    {
      return new Vector3( Mathf.Lerp( a.X, b.X, t.X ), Mathf.Lerp( a.Y, b.Y, t.Y ), Mathf.Lerp( a.Z, b.Z, t.Z ) );
    }

    public static Vector4 AsVector4( this Quaternion q)
    {
      return new Vector4( q.X, q.Y, q.Z, q.W );
    }

    public static Quaternion FromEulerDegrees( Vector3 eulerYXZ )
    {
      return Quaternion.FromEuler( eulerYXZ * MathX.DegreesToRadians );
    }

    public static Quaternion RotateX( float radians )
    {
      if ( radians == 0 ) { return Quaternion.Identity; }

      return Quaternion.FromEuler( new Vector3( radians, 0, 0 ) );
    }

    public static Quaternion RotateXDegrees( float degrees )
    {
      return RotateX( Mathf.DegToRad( degrees ) );
    }

    public static Quaternion RotateY( float radians )
    {
      if ( radians == 0 ) { return Quaternion.Identity; }

      return Quaternion.FromEuler( new Vector3( 0, radians, 0 ) );
    }

    public static Quaternion RotateYDegrees( float degrees )
    {
      return RotateY( Mathf.DegToRad( degrees ) );
    }

    public static Quaternion RotateZ( float radians )
    {
      if ( radians == 0 || float.IsNaN( radians ) ) { return Quaternion.Identity; }
      
      return Quaternion.FromEuler( new Vector3( 0, 0, radians ) );
    }

    public static Quaternion RotateZDegrees( float degrees )
    {
      return RotateZ( Mathf.DegToRad( degrees ) );
    }

    public static Quaternion YawPitchRotation( float yaw, float pitch )
    {
      return RotateYDegrees( yaw ) * RotateXDegrees( pitch );
    }

    public static float GlobalYaw( Vector3 direction )
    {
      return Mathf.Atan2( direction.X, direction.Z );
      // return ( Mathf.Pi * 2.0f - Mathf.Atan2( direction.Z, direction.X ) ) - Mathf.Pi / 2.0f;
    }

    public static float GlobalYawDegrees( Vector3 direction )
    {
      return Mathf.RadToDeg( GlobalYaw( direction ) );
    }

    public static float GlobalPitch( Vector3 direction )
    {
      var xz = new Vector2( direction.X, direction.Z ).Length();
      var y  = direction.Y; 

      return Mathf.Atan2( y, xz );
    }

    public static float GlobalPitchDegrees( Vector3 direction )
    {
      return Mathf.RadToDeg( GlobalPitch( direction ) );
    }

   
  
    
    public static void SetGlobalRotationTo( Node3D node, Quaternion quaternion )
    {
      var forward = quaternion * Vector3.Forward;
      var up      = quaternion * Vector3.Up;

      node.LookAt( node.GlobalPosition + forward, up );
    }

    

    public static bool IsZero( this Quaternion q ) 
    {
      return q.X == 0 && q.Y == 0 && q.Z == 0 && q.W == 0;
    }

    public static bool IsZero( this Vector3 v ) 
    {
      return v.X == 0 && v.Y == 0 && v.Z == 0;
    }

    public static bool IsValid( this Quaternion q ) 
    {
      return ! ( q.IsFinite() || q.IsZero() );
    }

    public static bool IsValid( this Vector3 v ) 
    {
      if ( float.IsNaN( v.X ) || float.IsNaN( v.Y ) || float.IsNaN( v.Z ) )
      {
        return false;
      }

      if ( float.IsInfinity( v.X ) || float.IsInfinity( v.Y ) || float.IsInfinity( v.Z ) )
      {
        return false;      
      }

      return true;
    }

    public static Quaternion GetNormalized( this Quaternion q ) 
    {
      if ( q.IsValid() )
      {
        return Quaternion.Identity;
      }

      return q.Normalized();
    }

    public static Quaternion GetDifference( this Quaternion q, Quaternion other )
    {
      return GetQuaternionDifference( q, other );
    }

    public static Quaternion GetQuaternionDifference( Quaternion a, Quaternion b )
    {
      return b.Inverse() * a;
    } 

    public static Quaternion GetQuaternionFraction( Quaternion q, float fraction )
    {
      return Quaternion.Identity.Slerp( q, fraction );
    }

    
    public static Vector3 GetGlobalScale( Node3D node )
    {
      return node.GlobalTransform.Basis.Scale;
    }

    

    public static float GetGlobalUniScale( Node3D node )
    {
      var scale3 = GetGlobalScale( node );
      return MathX.Max( scale3.X, scale3.Y, scale3.Z );
    }

    public static Vector3 GetYPlaneForward( Node3D node )
    {
      var forward = node.GlobalForward();

      forward.Y = 0;

      return forward.Normalized();
    }

    public static Vector3 GetYPlaneRight( Node3D node )
    {
      var right = node.GlobalRight();

      right.Y = 0;

      return right.Normalized();
    }

    


    public static Vector3 Average( List<Vector3> vectors )
    {
      var average = Vector3.Zero;

      vectors.ForEach( v => average += v );

      if ( average == Vector3.Zero )
      {
        return vectors[ 0 ];
      }

      return ( average / vectors.Count ).Normalized();
    }

    public static Vector3 BlendNormals( Vector3 a, Vector3 b, float amount )
    {
      if ( amount <= 0 )
      {
        return a;
      }

      if ( amount >= 1 )
      {
        return b;
      }

      var n = a.Lerp( b, amount );
      var length = n.Length();

      if ( length > 0 )
      {
        return n / length;
      }

      return amount <= 0.5 ? a : b;
    }

    public static Box3 ToBox3( this Aabb aabb )
    {
      return (Box3) aabb;
    }

    
  }

}