#include "vector2D.h"
#include <num/num.h>



libnum::num32 sqrt_num32(libnum::num32 v)
{
    uint32_t t, q, b, r;
    r = v.v;
    b = 0x40000000;
    q = 0;
    while( b > 0x40 )
    {
        t = q + b;
        if( r >= t )
        {
            r -= t;
            q = t + b;
        }
        r <<= 1;
        b >>= 1;
    }
    q >>= 8;
    libnum::num32 ret;
    ret.v = q;
    return ret;
}


Vector2D::Vector2D()
{
    this->x = 0.0;
    this->y = 0.0;
}

Vector2D::Vector2D( float x, float y )
{
    this->x = libnum::num32(x);
    this->y = libnum::num32(y);
}

Vector2D::Vector2D( libnum::num32 x, libnum::num32 y )
{
    this->x = x;
    this->y = y;
}

Vector2D::~Vector2D()
{
    this->x = 0.0;
    this->y = 0.0;
}

Vector2D Vector2D::Clone( void )
{
    Vector2D NewVector( this->x, this->y );
    return NewVector;
}

void Vector2D::AddVectors( Vector2D a, Vector2D b )
{
    this->x = a.x + b.x;
    this->y = a.y + b.y;
}

void Vector2D::Add( Vector2D v, libnum::num32 scale )
{
	this->x += v.x * scale;
	this->y += v.y * scale;
}

void Vector2D::SubtractVectors( Vector2D a, Vector2D b )
{
    this->x = a.x - b.x;
    this->y = a.y - b.y;
}

void Vector2D::Subtract( Vector2D v, libnum::num32 scale )
{
	this->x -= v.x * scale;
	this->y -= v.y * scale;
}

libnum::num32 Vector2D::Length( void )
{
    return sqrt_num32( this->x * this->x + this->y * this->y );
}

void Vector2D::Scale( libnum::num32 scale )
{
   	this->x *= scale;
	this->y *= scale; 
}

libnum::num32 Vector2D::Dot( Vector2D v )
{
    return ( this->x * v.x + this->y * v.y );
}

Vector2D Vector2D::Perp( void )
{
    Vector2D temp( -this->y, this->x );
    return temp;
}


bool CollisionDroite(Point2D A,Point2D B, Circle C)
{
   Vector2D u;
   u.x = B.x - A.x;
   u.y = B.y - A.y;
   Vector2D AC;
   AC.x = C.x - A.x;
   AC.y = C.y - A.y;
   libnum::num numerateur = u.x*AC.y - u.y*AC.x;   // norme du vecteur v
   if (numerateur <0)
      numerateur = -numerateur ;   // valeur absolue ; si c'est négatif, on prend l'opposé. 
   libnum::num denominateur = sqrt_num32(u.x*u.x + u.y*u.y);  // norme de u
   libnum::num CI = numerateur / denominateur;
   
   if (CI<C.r)
      return true;
   else
      return false;
}


bool CollisionSegment(Point2D A,Point2D B, Circle C)
{
   if (CollisionDroite(A,B,C) == false)
     return false;  // si on ne touche pas la droite, on ne touchera jamais le segment 
   
   Vector2D AB,AC,BC;
   AB.x = B.x - A.x;
   AB.y = B.y - A.y;
   AC.x = C.x - A.x;
   AC.y = C.y - A.y; 
   BC.x = C.x - B.x; 
   BC.y = C.y - B.y; 
   
   libnum::num pscal1 = AB.x*AC.x + AB.y*AC.y;  // produit scalaire
   libnum::num pscal2 = (-AB.x)*BC.x + (-AB.y)*BC.y;  // produit scalaire 
   
   if (pscal1>=0 && pscal2>=0)
      return true;   // I entre A et B, ok.
   
   /*
   // dernière possibilité, A ou B dans le cercle
   if (CollisionPoint2DCercle(A,C))
     return true;
   if (CollisionPoint2DCercle(B,C))
     return true;
   */

   return false;
}


bool CollisionBorder(Border B, Circle C)
{
   return CollisionSegment( B.A, B.B, C );
}


Vector2D GetNormale(Point2D A,Point2D B,Point2D C)
{
  Vector2D AC,u,N; 
  u.x = B.x - A.x; 
  u.y = B.y - A.y; 
  AC.x = C.x - A.x; 
  AC.y = C.y - A.y;
  libnum::num parenthesis = u.x*AC.y-u.y*AC.x;  // calcul une fois pour les deux 
  
  N.x = -u.y*(parenthesis);
  N.y = u.x*(parenthesis);
  
  // normalisons
  libnum::num norme = sqrt_num32(N.x*N.x + N.y*N.y);
  N.x/=norme;
  N.y/=norme;
  
  return N;
}

Point2D ProjectionI(Point2D A,Point2D B,Point2D C)
{
  Vector2D u,AC;
  u.x = B.x - A.x;
  u.y = B.y - A.y;
  AC.x = C.x - A.x;
  AC.y = C.y - A.y;
  libnum::num ti = (u.x*AC.x + u.y*AC.y)/(u.x*u.x + u.y*u.y); 
  Point2D I;
  I.x = A.x + ti*u.x;
  I.y = A.y + ti*u.y;
  return I;
}

Vector2D ComputeVectorRebound(Vector2D v,Vector2D N)
{
  Vector2D v2;
  libnum::num pscal = (v.x*N.x +  v.y*N.y); 
  v2.x = v.x -2*pscal*N.x;
  v2.y = v.y -2*pscal*N.y;
  return v2;
}

Border::Border()
{

}

Border::~Border()
{
    
}