/**
 * \file Line.cpp
 * \date 5.04.2004
 * \author Pavol Misik
 * Implementation of the Line class.
 */

#include "Common/World/Line.h"


/// \brief Vypocita druhu mocninu cisla x
/// \param x Umocnovane cislo
/// \return Druha mocnina cisla x (x^2 resp. x*x)
inline float sqr(float x)
{
	return x*x;
}

/// \brief Riesenie kvadratickej rovnice ax^2+bx+c=0
/// \param a Koeficient a 
/// \param b Koeficient b
/// \param c Koeficient c
/// \param v1 Vysledok 1
/// \param v2 Vysledok 2
/// \return Pocet rieseni kvadratickej rovnice
int QuadraticFormula(float a, float b, float c, float& v1, float& v2)
{
	float d = sqr(b) - 4*a*c;
	if (fabsf(d) < FLOAT_EPS) {
		v1 = -b / (2 * a);
		return 1;
	}
	else if (d < 0) {
	    return 0;
	}
	else{
		d = float(sqrt(d));
		v1 = (-b + d ) / (2 * a);
		v2 = (-b - d ) / (2 * a);
		return 2;
	}
}

/// \brief Vytvori priamku z parametrov implicitnej rovnice priamky (ax+by+c=0)
/// \param a Koeficient a
/// \param b Koeficient b
/// \param c Koeficient c
Line::Line(float a, float b, float c)
{
	fA = a;
	fB = b;
	fC = c;
}

/// \brief Konstruktor priamky z dvoch bodov
/// \param p1 Bod1
/// \param p2 Bod2
Line::Line(const Point& p1, const Point& p2)
{
	FromPoints(p1, p2);
}

/// \brief Destruktor
Line::~Line()
{
}

/// \brief Vytvori priamku z dvoch bodob
/// \param p1 Prvy bod priamky
/// \param p2 Druhy bod priamky
void Line::FromPoints(const Point& p1, const Point& p2)
{ 
	float temp = p2.GetX() - p1.GetX();
	if (fabs(temp) < FLOAT_EPS) {
		fA = 1.0f;
		fB = 0.0f;
	}
	else {
		float m = (p2.GetY() - p1.GetY()) / temp;
		fA = -m;
		fB = 1.0f;
	}  
	fC = -(fA * p2.GetX() + fB * p2.GetY());
}

/// \brief Urci priesecnik dvoch priamok
/// \param l, Druha priamka
/// \param pos Priesecnik
/// \return \li true, ak priesecnik existuje
///         \li false, ak priesecnik neexistuje, (priamky su rovnobezne)
bool Line::Intercept(const Line& l, Point& pos)const
{
	float x, y;
	
	if (fB == 0 && l.fB == 0 || fA/fB == l.fA/l.fB)
	{	// su rovnobezne
		return false;
	}

	if (fB == 0)
	{
		x = -fC/fA;
		y = l.GetY(x);
	}
	else if (l.fB == 0)
	{
		x = -l.fC/l.fA;
		y = GetY(x);
	}
	else
	{
		x = (fC*l.fB - fB*l.fC)/(l.fA*fB - fA*l.fB);
		y = GetY(x);
	}
	pos.SetX(x);
	pos.SetY(y);
	return true;
}

/// \brief Urci priesecnik priamky a kriznice
/// \param c kruznica, s ktoru sa ma pocitat priesecnik
/// \param pos1 Priesecnik1
/// \param pos2 Priesecnik1
/// \return Pocet priesecnikov (0, 1, 2)
int Line::Intercept(const Circle& c, Point& pos1, Point& pos2)const
{
	int numSol;
	Point center = c.GetCenter();
	float radius = c.GetRadius();
	float x1, y1, x2, y2;

	if (fabs(fA) > FLOAT_EPS) {
		float a,b,c;
		a = 1 + sqr(fB/fA);
		b = 2*(-center.GetY() + (fC/fA + center.GetX()) * (fB/fA));
		c = -sqr(radius) + sqr(fC/fA + center.GetX()) + sqr(center.GetY());
		numSol = QuadraticFormula(a, b, c, y1, y2);
		x1 = - ( (fB * y1 + fC) / fA );
		x2 = - ( (fB * y2 + fC) / fA );
		pos1.Set(x1, y1);
		pos2.Set(x2, y2);

		return numSol;
	}
	else{
		numSol = QuadraticFormula(1, -2*center.GetX(), sqr(center.GetX()) + sqr(fC/fB + center.GetY()) - sqr(radius), x1, x2);
		y1 = y2 = - fC / fB;
		pos1.Set(x1, y1);
		pos2.Set(x2, y2);
		return numSol;
	}
}

/// \brief Pomocny vypocet pre urcenie y-ovej suradnice bodu na priamke
/// \return y-onova suradnica bodu s x-ovou suradnicou na priamke
float Line::GetY(float x) const{
	if ( fB != 0 ) 
		return (-fA*x - fC)/fB;
	return 0;
}

/// \brief Vytvori kolmu priamku na tuto priamku, a priamka bude prechadzat bodom pos
/// \param pos Bod, ktorym ma kolma priamka prechadzat
/// \return Kolma priamka na tuto priamku, ktora prechadza bodom pos
Line Line::Perpendicular(const Point& pos)const
{
	// kolmica bymenime fA a fB, a uz ju len posunieme na spravne miesto
/*	if (fA==0 && fB!=0)
		return Line(-fB, fA, fA*pos.GetY() - fB*pos.GetX());
	if (fB==0 && fA!=0)
		return Line(fB,-fA, fA*pos.GetY() - fB*pos.GetX());
		*/
	return Line(fB,-fA, fA*pos.GetY() - fB*pos.GetX());
}

/// \brief Urci najkratsiu vzdialenost medzi priamkou a bodom pos
/// \param pos Bod voci ktoremu sa urcuje vzdialenost od priamky
/// \return Minimalna vzdialenost bodu a priamky
float Line::DistanceToPoint(const Point& pos)const
{
	Point priesecnik;
	Line kolmica(fB,-fA, fA*pos.GetY() - fB*pos.GetX());
	Intercept(kolmica, priesecnik);
	return (pos - priesecnik).GetLength();
}

/// \brief Vytvori rovnobeznu priamku s tuto priamkou, a priamka bude prechadzat bodom p
/// \param p Bod, ktorym ma rovnobezna priamka prechadzat
/// \return Rovnobezna priamka s tuto priamku, ktora prechadza bodom p
/// Ax + By + C = 0  => C = -Ax-By
/// (A, B) ||  (A, B)
Line Line::Parallel(const Point &p)const
{
	return Line(fA,fB, -fA*p.GetX() -fB*p.GetY());
}

/// Testovanie funkcnosti a spravnosti
/*
#include <stdio.h>

void main()
{
	Point p1(-52.5,0.0);
	Point p2(+52.5,0.0);
	Point p3(0.0,-34.0);
	Point p4(0.0,+34.0);
	Line l1(p1, p2);
	Line l2(p3, p4);
	Point inPos, inPos2;
	if (l1.Intercept(l2, inPos))
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());

	Point cc(0.0, 0.0);
	Point lt(-52.5,-34.0);
	Point lb(-52.5,+34.0);
	Point rt(+52.5,-34.0);
	Point rb(+52.5,+34.0);
	Point ct(0.0, -34.0);
	Point cb(0.0, +34.0);


	Line left(lb, lt);
	Line up(lt, rt);
	Line right(rt, rb);
	Line down(lb, rb);
	Line yCenter(ct, cb);
	
	float ofsX = 10.0;
	Line offs(Point(ofsX, -34.0), Point(ofsX, 34.0));

	if (left.Intercept(up, inPos))
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());

	if (down.Intercept(right, inPos))
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());

	if (down.Intercept(left, inPos))
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());

	if (up.Intercept(right, inPos))
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());

	if (offs.Intercept(up, inPos))
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());

	if (offs.Intercept(down, inPos))
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());

	Circle c(cc, 10.0);
	if (yCenter.Intercept(c, inPos, inPos2)>0)
	{
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());
		printf("\n%3.2f, %3.2f", inPos2.GetX(), inPos2.GetY());
	}

	Circle c2(rt, 10.0);
	if (up.Intercept(c2, inPos, inPos2)>0)
	{
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());
		printf("\n%3.2f, %3.2f", inPos2.GetX(), inPos2.GetY());
	}
	Line cross(rt, lb);
	if (cross.Intercept(c, inPos, inPos2)>0)
	{
		printf("\n%3.2f, %3.2f", inPos.GetX(), inPos.GetY());
		printf("\n%3.2f, %3.2f", inPos2.GetX(), inPos2.GetY());
	}

	
	printf("\n%3.2f", left.DistanceToPoint(cc));
	printf("\n%3.2f", offs.DistanceToPoint(cc));
	
	printf("\n%3.2f", up.DistanceToPoint(cc));
	printf("\n%3.2f", down.DistanceToPoint(cc));
	printf("\n%3.2f", cross.DistanceToPoint(cc));
	Line at(Point(-10.0, -10.0), Point(52.5, 0.0));
	printf("\n%3.2f", at.DistanceToPoint(cc));
}

*/
