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

namespace Rokojori
{
  public class Pose
	{
    bool _needsUpdate = true;
    Quaternion _rotation = Quaternion.Identity;
    public Quaternion rotation 
    {
      get => _rotation;
      set
      {
        _rotation = value;
        _needsUpdate = true;
      }
    }

    Vector3 _position = Vector3.Zero;
    public Vector3 position
    {
      get => _position;
      set
      {
        _position = value;
        _needsUpdate = true;
      }    
      
    }

    Basis _basis;

    void Update()
    {
      if ( ! _needsUpdate )
      {
        return;
      }
      
      _needsUpdate = false;

      _basis = new Basis( _rotation );
    }

    public Vector3 forward
    {
      get
      {
        Update();

        return -_basis.Z; 
      }
    }

    public Vector3 right
    {
      get
      {
        Update();

        return _basis.X; 
      }
    }

    public Vector3 up
    {
      get
      {
        Update();

        return _basis.Y; 
      }
    }

    public Pose()
    {

    }

    public static Pose Create( Vector3 position, Quaternion rotation )
    {
      var p = new Pose();
      p.position = position;
      p.rotation = rotation;
      return p;
    }

    public static Pose From( Pose a )
    {
       var p = new Pose();
      p.position = a.position;
      p.rotation = a.rotation;
      return p;

    }

    public static Pose Lerp( Pose a, Pose b, float weight = 0.5f )
    {
      var p = From( a );
      p.position = p.position.Lerp( b.position, weight );
      p.rotation = p.rotation.Slerp( b.rotation, weight );
      return p;

    }

    public static Pose From( Node3D node3D )
    {
      var p = new Pose();
      p.position = node3D.GlobalPosition;
      p.rotation = node3D.GetGlobalQuaternion();
      return p;
    }

    public static Pose InverseFrom( Node3D node3D )
    {
      var p = new Pose();
      p.position = - node3D.GlobalPosition;
      p.rotation = node3D.GetGlobalQuaternion().Inverse();
      return p;
    }

    public Pose ToGlobal( Node3D n )
    {
      var p = new Pose();
      p.rotation = rotation * n.GetGlobalQuaternion();
      p.position = n.ToGlobal( position );
      return p;
    }

    public Vector3 Apply( Vector3 p )
    {
      p = rotation * p;
      p += position;

      return p;
    }

    public Vector3 ApplyInverse( Vector3 p )
    {
      p -= position;
      p = rotation.Inverse() * p;     

      return p;
    }

    public Vector3 OnRightUpCircle( float radians, float size = 1 )
    {
      var c = Mathf.Cos( radians ) * size;
      var s = Mathf.Sin( radians ) * size;

      return s * right + c * up + position;
    }

    public Vector3 ComputeCircleValues( float radians, Vector3 cos, Vector3 sin, float size )
    {
      var c = Mathf.Cos( radians ) * size;
      var s = Mathf.Sin( radians ) * size;

      var cValue = c * cos.X * right +
                   c * cos.Y * up +
                   c * cos.Z * forward;

      var sValue = s * sin.X * right +
                   s * sin.Y * up +
                   s * sin.Z * forward;

      return cValue + sValue + position;
    }


  }
}