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

namespace Rokojori
{
  public static class Math3D
  {
    public static Transform3D TRS( Vector3 translation, Quaternion rotation, Vector3 scale )
    {
      var trsf = new Transform3D( new Basis( rotation ), translation );
      trsf.ScaledLocal( scale );

      return trsf;
    }

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

    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 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 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( 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 Quaternion GetGlobalQuaternion( this Node3D node )
    {
      return GetGlobalRotationFrom( node );
    }

    public static Quaternion GetGlobalRotationFrom( Node3D node )
    {
      var quaternion = node.GlobalBasis.GetRotationQuaternion();

      return quaternion;
    } 

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

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

    public static Vector3 SnapRounded( 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 SnapCeiled( 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( 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 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 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 Basis AlignUp( Basis basis, Vector3 upDirection )
    {
      basis.Y = upDirection;
      basis.X = - basis.Z.Cross( upDirection );
      return basis.Orthonormalized();
    }

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

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

      t = t.LookingAt( direction, up );

      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 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 ) { 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 RotateXDegrees( pitch ) * RotateYDegrees( yaw );
    }

    public static void SetGlobalQuaternion( this Node3D node, Quaternion quaternion )
    {
      var localScale = node.Scale;
      node.GlobalBasis = new Basis( quaternion );
      node.Scale = localScale;
    }

    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 void LookTowards( this Node3D node, Vector3 forwardDirection, Vector3 upDirection, Quaternion rotation )
    {
      //forwardDirection = rotation * forwardDirection;
      //upDirection      = rotation * upDirection;

      node.LookTowards( forwardDirection, upDirection );  
      node.SetGlobalQuaternion( node.GetGlobalQuaternion() * rotation );
    }

    public static void LookTowards( this Node3D node, Vector3 forward, Vector3 up )
    {
      node.LookAt( forward + node.GlobalPosition, up );
    }


    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 GlobalForward( this Node3D node )
    {
      return GetGlobalForward( node );
    }

    public static Vector3 GetGlobalForward( Node3D node )
    {
      return -node.GlobalBasis.Z;
    }

    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 GlobalUp( this Node3D node )
    {
      return GetGlobalUp( node );
    }

    public static Vector3 GetGlobalUp( Node3D node )
    {
      return node.GlobalBasis.Y;
    }

     public static Vector3 GlobalRight( this Node3D node )
    {
      return GetGlobalRight( node );
    }

    public static Vector3 GetGlobalRight( Node3D node )
    {
      return node.GlobalBasis.X;
    }

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

      forward.Y = 0;

      return forward.Normalized();
    }

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

      right.Y = 0;

      return right.Normalized();
    }

    public static void SetGlobalX( this Node3D node, float x )
    {
      var gp = node.GlobalPosition;

      gp.X = x;

      node.GlobalPosition = gp;
    }

    public static void SetGlobalY( this Node3D node, float y )
    {
      var gp = node.GlobalPosition;

      gp.Y = y;

      node.GlobalPosition = gp;
    }

    public static void SetGlobalZ( this Node3D node, float z )
    {
      var gp = node.GlobalPosition;

      gp.Z = z;

      node.GlobalPosition = gp;
    }

    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 Aabb? GetWorldBounds( this Node3D node, bool onlyVisible = true )
    {
      return GetWorldBoundsFrom( node, onlyVisible );
    }

    public static Aabb? GetWorldBoundsFrom( Node3D node, bool onlyVisible = true )
    {
      Aabb? worldBounds = null;

      Nodes.ForEach<VisualInstance3D>( node,
        ( vi )=>
        {
          if ( onlyVisible && ! vi.IsVisibleInTree() )
          {
            return;
          }

          var nBounds = vi.GetAabb();

          nBounds.Size *= GetGlobalUniScale( vi );
          nBounds.Position += vi.GlobalPosition;
          nBounds.End += vi.GlobalPosition;

          worldBounds = worldBounds == null ? nBounds : ( ((Aabb)worldBounds).Merge( nBounds ) );
        }
      );

      return worldBounds;
    }
  }

}