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

namespace Rokojori
{  
  public static class Node3DExtensions
  {
    public static Vector3 ToLocalFromLocal( this Node3D self, Node3D from, Vector3 fromLocalPosition )
    {
      var world = from.ToGlobal( fromLocalPosition );
      return self.ToLocal( world );
    }

    public static Vector3 ToLocalFromLocalDirection( this Node3D self, Node3D from, Vector3 fromLocalDirection )
    {
      var world = from.ToGlobalDirection( fromLocalDirection );
      return self.ToLocalDirection( world );
    }

    public static Vector3 ToLocalDirection( this Node3D self, Vector3 globalDirection )
    {
      return self.GlobalTransform.Basis.Inverse() * globalDirection;
    }

    public static Vector3 ToGlobalDirection( this Node3D self, Vector3 localDirection )
    {
      return self.GlobalTransform.Basis * localDirection;
    }

    public static void LookTowards( this Node3D self, Vector3 direction )
    {
      self.LookAt( direction + self.GlobalPosition );
    } 

    public static float DistanceTo( this Node3D self, Node3D other )
    {
      return self.DirectionTowards( other ).Length();
    }

    public static Vector3 DirectionTowards( this Node3D self, Node3D other )
    {
      return ( other.GlobalPosition - self.GlobalPosition );
    } 

    public static Vector3 DirectionTowards( this Node3D self, Vector3 other )
    {
      return ( other - self.GlobalPosition );
    } 

    public static bool IsInRange( this Node3D a, Node3D other, float distance )
    {
      return a.DistanceTo( other ) <= distance;
    }

    public static Vector3 GetLocalOrGlobalPosition( this Node3D node, bool global )
    {
      return global ? node.GlobalPosition : node.Position;
    }

    public static void SetLocalOrGlobalPosition( this Node3D node, Vector3 position, bool global )
    {
      if ( global )
      { 
        node.GlobalPosition = position; 
      }
      else
      {
        node.Position = position;
      }
    }

    public static void SetLocalOrGlobalRotation( this Node3D node, Vector3 rotation, bool global )
    {
      if ( global )
      { 
        node.GlobalRotation = rotation; 
      }
      else
      {
        node.Rotation = rotation;
      }
    }

    public static void SetGlobalPose( this Node3D node, Vector3 position, Quaternion rotation )
    {
      node.SetGlobalQuaternion( rotation );
      node.GlobalPosition = position;
    }

    public static void SetGlobalPose( this Node3D node, Pose pose )
    {
      node.SetGlobalPose( pose.position, pose.rotation );
    }

    public static Pose GetGlobalPose( this Node3D node )
    {
      return Pose.From( node ); 
    }

    public static void CopyGlobalPoseFrom( this Node3D node, Node3D other )
    {
      node.SetGlobalPose( other.GetGlobalPose() );
    }

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

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

    public static void LookTowards( this Node3D node, Vector3 forwardDirection, Vector3 upDirection, Quaternion rotation )
    {
      node.LookTowards( forwardDirection, upDirection );  
      node.SetGlobalQuaternion( node.GlobalQuaternion() * rotation );
    }

    public static void LookTowards( this Node3D node, Vector3 forward, Vector3 up, Vector3? up2 = null )
    {
      if ( forward == up )
      {
        up = up2 == null ? Vector3.Back : (Vector3)up2;
      }

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


    public static Vector3 GetGlobalOffset( this Node3D node, Vector3 direction )
    {
      return direction.X * node.GlobalRight() + 
             direction.Y * node.GlobalUp() +
             direction.Z * node.GlobalForward() ;
    }

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

    public static Vector3 GetPosition( this Node3D node, bool global = false )
    {
      return global ? node.GlobalPosition : node.Position;
    }

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


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

    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 void SetLocalX( this Node3D node, float x )
    {
      var gp = node.Position;

      gp.X = x;

      node.Position = gp;
    }

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

      gp.Y = y;

      node.Position = gp;
    }

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

      gp.Z = z;

      node.Position = gp;
    }

    public static Vector3 GetOrientationBasedGlobalOffset( this Node3D node, Vector3 offset )
    {
      return offset.X * node.GlobalRight() + offset.Y * node.GlobalUp() + offset.Z * node.GlobalForward();
    }


    public static Box3 GetWorldBox( this Node3D node, bool onlyVisible = true )
    {
      var aabb = GetWorldBounds( node, onlyVisible );

      return aabb == null ? null : ( (Aabb)aabb).ToBox3();
    }

    public static Aabb? GetWorldBounds( this Node3D node, bool onlyVisible = true )
    {
      return node.GetWorldBoundsFrom( onlyVisible );
    }

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

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

          var nBounds = vi.GetAabb();

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

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

      return worldBounds;
    }

    
    public static Quaternion GlobalQuaternion( this Node3D self )
    {
      return self.GlobalBasis.GetRotationQuaternion();
    }

    public static Quaternion GlobalYawQuaterntion( this Node3D self )
    {
      return Math3D.RotateY( self.GlobalYawRadians() + Mathf.Pi );
    } 

    public static float GlobalYawDegrees( this Node3D node3D )
    {
      return Math3D.GlobalYawDegrees( node3D.GlobalForward() );
    } 

    public static float GlobalYawRadians( this Node3D node3D )
    {
      return Math3D.GlobalYaw( node3D.GlobalForward() );
    } 

    public static Vector2 GlobalXZ( this Node3D n )
    {
      var v = n.GlobalPosition;
      return new Vector2( v.X, v.Z );
    }

    public static void SetGlobalXZ( this Node3D n, Vector2 xz )
    {
      var p = n.GlobalPosition;
      p.X = xz.X;
      p.Z = xz.Y;

      n.GlobalPosition = p;
    }

    public static void SetGlobalYaw( this Node3D n, float yawRadians )
    {
      n.SetGlobalQuaternion( Math3D.RotateY( yawRadians ) );
    }

    public static void SetGlobalYawDegrees( this Node3D n, float yawDegrees )
    {
      n.SetGlobalQuaternion( Math3D.RotateY( yawDegrees * MathX.DegreesToRadians ) );
    }

  }
}