#include "ToolsMAth.h"

#include <cmath>
#include <limits>

//------------------------------------------------------//
//       Fonctions Mathématiques diverses (by CJ)       //
//------------------------------------------------------//

// Calcul la distance entre 2 points 
double distance(dpoint pt1, dpoint pt2) 
{
	double dist; 

	dist = std::sqrt((pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y));

	if (dist < 0.000001) 
	{
		dist = 0; 
	}

	return dist;
}

// Renvoie les coefficients p1 et d1 de la droite y = p1.x + b1,  à partir de 2 points
void GetCoeffDroite(dpoint pt1, dpoint pt2, double *p1, double *d1, double *b1) 
{		
	// Leger bug pour des écarts très petits 
	if (fabs(pt1.x - pt2.x) < 0.001) 
	{
		pt1.x = pt2.x; 
	}
	
	if (fabs(pt1.y - pt2.y) < 0.001) 
	{
		pt1.y = pt2.y; 
	}
							
	// Calcul des coefficients de la droite
	*p1 = (double)std::numeric_limits< int >::max();  
	*d1 = pt2.x - pt1.x;
	if (*d1 != 0) 
	{
		*p1 = (pt2.y - pt1.y) / (*d1);
		*b1 = pt1.y - ((*p1) * pt1.x);
	}	
}

// Renvoie le point (sol1 ou sol2) le plus proche de p1
dpoint leplusproche(dpoint vPoint, dpoint sol1, dpoint sol2) 
{
	double dist1, dist2; 
	dpoint res; 

	dist1 = distance(vPoint, sol1);
	dist2 = distance(vPoint, sol2);
	
	if (dist1 < dist2) 
	{
		res.x = sol1.x; 
		res.y = sol1.y;
	} 
	else 
	{
		res.x = sol2.x;
		res.y = sol2.y;
	}

	return res; 
}

// Calcul des 2 points d'intersection entre un cercle et une droite définie par 2 points 
bool InterCercleDroite(dpoint p0, double r, dpoint d1, dpoint d2, dpoint *sr1, dpoint *sr2) 
{
	double a, b, d; 
	double ka, kb, kc, delta; 
	dpoint s1, s2; 
	bool trouve = false; 

	s1.x = -1; 
	s1.y = -1; 
	s2.x = -1; 
	s2.y = -1; 

	// Coefficient de la droite définie par 2 pts successifs
	// Droite y = a.x + b 
	GetCoeffDroite(d1, d2, &a, &d, &b);

	if ((d != 0) && (a != 0)) 
	{ // Cas normal 
		ka = 1 + (a * a); 
		kb = (2 * a * b) - (2 * p0.x) - (2 * a * p0.y);
		kc = (b * b) - (2 * b * p0.y) - ((r * r) - (p0.x * p0.x) - (p0.y * p0.y));

		delta = (kb * kb) - (4 * ka * kc);

		if (delta == 0) 
		{
			s1.x = -kb / (2 * ka);
			s2.x = s1.x;
		} 
		else if (delta > 0) 
		{
			s1.x = (-kb - sqrt(delta)) / (2 * ka);
			s2.x = (-kb + sqrt(delta)) / (2 * ka);
		}

		if (delta >= 0) 
		{
			s1.y = a * s1.x + b;
			s2.y = a * s2.x + b;
			trouve = true; 
		}
	} 
	else if (d == 0) 
	{ // La droite est verticale 
		ka = 1; 
		kb = -2 * p0.y;
		kc = (d1.x * d1.x) - (2 * d1.x * p0.x) - ((r * r) - (p0.x * p0.x) - (p0.y * p0.y));

		delta = (kb * kb) - (4 * ka * kc);

		if (delta == 0) 
		{
			s1.y = -kb / (2 * ka);
			s2.y = s1.y;
		} 
		else if (delta > 0) 
		{
			s1.y = (-kb - sqrt(delta)) / (2 * ka);
			s2.y = (-kb + sqrt(delta)) / (2 * ka);
		}

		if (delta >= 0) 
		{
			s1.x = d1.x; 
			s2.x = d1.x; 
			trouve = true;
		}
	} 
	else if (a == 0) 
	{ // La droite est horizontale 
		ka = 1; 
		kb = -2 * p0.x;
		kc = (d1.y * d1.y) - (2 * d1.y * p0.y) - ((r * r) - (p0.x * p0.x) - (p0.y * p0.y));

		delta = (kb * kb) - (4 * ka * kc);

		if (delta == 0) 
		{
			s1.x = -kb / (2 * ka);
			s2.x = s1.x;
		} 
		else if (delta > 0) 
		{
			s1.x = (-kb - sqrt(delta)) / (2 * ka);
			s2.x = (-kb + sqrt(delta)) / (2 * ka);
		}

		if (delta >= 0) 
		{
			s1.y = d1.y; 
			s2.y = d1.y; 
			trouve = true; 
		}	
	}

	sr1->x = s1.x; 
	sr1->y = s1.y;
	sr2->x = s2.x;
	sr2->y = s2.y;

	return trouve; 
}

// Parabole y = a*x^2 + b*x + c  (by FP)
//
// N : nombre de points (i.e. taille des vecteurs x[] et y[])
// x : vecteurs des abscisses
// y : vecteurs des ordonees
// a, b, c : les 3 coefficients recherches
void moindres_carres_parabole(int N, dpoint *points, double *a, double *b, double *c)
{
	int i;
	double d, d1, d2, d3; 
	double sx = 0.0;
	double sx2 = 0.0;
	double sx3 = 0.0;
	double sx4 = 0.0;
	double sy = 0.0;
	double syx = 0.0;
	double syx2 = 0.0;
  
	double x2;
  
	for ( i = 0; i < N; i++ )
	{
		sx   += points[i].x;
		sx2  += ( x2 = points[i].x * points[i].x );
		sx3  += x2 * points[i].x;
		sx4  += x2 * x2;
		sy   += points[i].y;
		syx  += points[i].y * points[i].x;
		syx2 += points[i].y * x2;
	}

	d  = sx4 * ( (double) N * sx2 - sx * sx ) + sx3 * ( sx * sx2 - (double) N * sx3 ) + sx2 * ( sx * sx3 - sx2 * sx2 );
	d1 = sx4 * ( sx2 * sy - sx * syx ) + sx3 * ( sx * syx2 - sx3 * sy) + sx2 * ( sx3 * syx - sx2 * syx2 );
	d2 = sx4 * ( (double) N * syx - sy * sx ) + sx3 * ( sy * sx2 - (double) N * syx2 ) + sx2 * ( syx2 * sx - syx * sx2 );
	d3 = syx2 * ( (double) N * sx2 - sx * sx ) + syx * ( sx * sx2 - (double) N * sx3 ) + sy * ( sx * sx3 - sx2 * sx2 );

	if (d != 0) 
	{
		*c = d1 / d;	
		*b = d2 / d;
		*a = d3 / d;
	}
	else
	{
		*a = *b = *c = 0;
	}
}

// Regression linéaire 
// Trouve les coefficients de la droite y = bx + a, à partir des points, par regression linéaire
void RegressionLineaire(int n, dpoint *points, double *b, double *a) 
{
	// calculate the averages of arrays x and y
	double xa = 0, ya = 0;

	for (int i = 0; i < n; i++) 
	{
		xa += points[i].x;
		ya += points[i].y;
	}

	if (n != 0)
	{
		xa /= n;
		ya /= n;
	}
	else
	{
		xa = 0; 
		ya = 0; 
	}
 
	// calculate auxiliary sums
	double xx = 0, yy = 0, xy = 0;

	for (int i = 0; i < n; i++) 
	{
		double tmpx = points[i].x - xa, tmpy = points[i].y - ya;
		xx += tmpx * tmpx;
		yy += tmpy * tmpy;
		xy += tmpx * tmpy;
	}
 
	// calculate regression line parameters
 
	// make sure slope is not infinite
	// assert(fabs(xx) != 0);
 
	if (xx != 0)
	{
		*b = xy / xx;
	}
	else
	{
		*b = 0; 
	}

	*a = ya - *b * xa;

//	Coefficient de regression 
//	double coeff; 
//	coeff = (fabs(yy) == 0) ? 1 : xy / sqrt(xx * yy);
}


// Calcul la distance orthogonale entre le point vpt 
// et la droite définie par les 2 points pt1 et pt2
double distanceOrthPtDroite(dpoint vpt, dpoint pt1, dpoint pt2, dpoint *pres)
{
	double p1, d1, b1, b2, dist; 

	GetCoeffDroite(pt1, pt2, &p1, &d1, &b1);

	if ((p1 != 0) && (d1 != 0))
	{
		// Perpendiculaire qui passe par vpt 
		b2 = vpt.y + ((1/p1) * vpt.x);

		// Point de rencontre (qui passe par pb)
		pres->x = (double) (b2 - b1) / (double) (p1 + (1/p1));
		pres->y = (double) p1 * (pres->x) + b1;
	} 
	else if (p1 == 0)
	{ // pt1-pt2 horizontale
		pres->x = vpt.x;
		pres->y = pt1.y;
	} 
	else if (d1 == 0) 
	{ // verticale 
		pres->x = pt1.x;
		pres->y = vpt.y;
	}

	dist = distance(*pres, vpt); 
	return dist; 
}


// Point in Polygon d'après FAQ comp.graphics
bool PtInPolygon(double vx, double vy, dpoint *tpoint, int nbpoint) 
{
	int i, j; 
	bool c = false;  

	for (i = 0, j = nbpoint-1; i < nbpoint; j = i++) 
	{
		if ((((tpoint[i].y <= vy) && (vy < tpoint[j].y))  
		 || ((tpoint[j].y <= vy) && (vy < tpoint[i].y))) && 
		 (vx < (tpoint[j].x - tpoint[i].x) * (vy - tpoint[i].y) / (tpoint[j].y - tpoint[i].y) + tpoint[i].x))
		{
			c = !c;
		}
	}

	return c; 
}

// Est ce que le point x, y est dans l'ellipse a, b 
bool PtInEllipse(double x, double y, double a, double b)
{
	double result = 10.0; 

	if ((a != 0.0) && (b != 0.0)) 
	{
		result = ((x / a) * (x / a)) + ((y / b) * (y / b));
	}

	if (result <= 1) 
	{
		return true; 
	}

	return false;
}