using Godot;

namespace Rokojori
{ 

  public class SVGArc
  {
    public static Vector2 GetPointOnCenterArc( Vector2 r, float theta, float delta )
    {
      return new Vector2(
       r.X * Mathf.Cos( theta ) * Mathf.Cos( delta ) - r.Y * Mathf.Sin( theta ) * Mathf.Sin( delta ),
       r.X * Mathf.Sin( theta ) * Mathf.Cos( delta ) + r.Y * Mathf.Cos( theta ) * Mathf.Sin( delta )
      );


    }

    public static void EndpointToCenterArc( 
      Vector2 start, Vector2 end, Vector2 radius, float xAngle, bool flagA, bool flagS, 
      out Vector2 centerPosition, 
      out Vector2 centerRadius, 
      out Vector2 centerAngles 
      )
    {
      var rX = Mathf.Abs( radius.X );
      var rY = Mathf.Abs( radius.Y );

      var dx2 = ( start.X - end.X ) / 2f;
      var dy2 = ( start.Y - end.Y ) / 2f;

      var x1p =  Mathf.Cos( xAngle ) * dx2 + Mathf.Sin( xAngle ) * dy2;
      var y1p = -Mathf.Sin( xAngle ) * dx2 + Mathf.Cos( xAngle ) * dy2;

      var rxs = rX * rX;
      var rys = rY * rY;
      var x1ps = x1p * x1p;
      var y1ps = y1p * y1p;

      var cr = x1ps / rxs + y1ps / rys;

      if ( cr > 1 ) 
      {
        var s = Mathf.Sqrt( cr );

        rX = s * rX;
        rY = s * rY;

        rxs = rX * rX;
        rys = rY * rY;
      }

      var dq = ( rxs * y1ps + rys * x1ps );
      var pq = ( rxs * rys - dq ) / dq;
      var q = Mathf.Sqrt( Mathf.Max( 0, pq ) );

      if ( flagA == flagS )
      {
        q = -q;
      }

      var cxp = q * rX * y1p / rY;
      var cyp = - q * rY * x1p / rX;

      var cx = Mathf.Cos( xAngle ) * cxp - Mathf.Sin( xAngle ) * cyp + ( start.X + end.X ) / 2;
      var cy = Mathf.Sin( xAngle ) * cxp + Mathf.Cos( xAngle ) * cyp + ( start.Y + end.Y ) / 2;

      var theta = Angle( 1f, 0f, ( x1p-cxp ) / rX, ( y1p - cyp ) / rY );
      
      var delta = Angle(
        ( x1p - cxp ) / rX, 
        ( y1p - cyp ) / rY,
        ( -x1p - cxp ) / rX, 
        ( -y1p-cyp ) / rY );

      delta = delta % Mathf.Pi * 2;

      if ( ! flagS )
      {
        delta -= 2 * Mathf.Pi;
      }

      centerRadius = new Vector2( rX, rY );
      centerPosition =  new Vector2(cx, cy );

      centerAngles =  new Vector2( theta, delta );
    }

    static float Angle( float ux, float uy, float vx, float vy )
    {
      var u =  new Vector2( ux, uy );
      var v =  new Vector2(  vx, vy );
      
      var dot = Math2D.Dot( u, v );
      var len = u.Length( ) * v.Length( );

      var ang = Mathf.Acos( Mathf.Clamp( dot / len,-1,1 ) );
    
      if ( ( u.X * v.Y - u.Y * v.X ) < 0 )
      {
        ang = -ang; 
      }

      return ang;
    }
  }
}