#include "additionalFunctions.h"

float ofxVec2fDistance(ofxVec2f& first, ofxVec2f& second) {
	float s1 = second.x - first.x;
	float s2 = second.y - first.y;
	return s1 * s1 + s2 * s2;
}

void scale2dVector( ofxVec2f& vec, const float scaleFactor)
{
	vec.x *= scaleFactor;
	vec.y *= scaleFactor;
}

//double  operator * (const ofxVec2f& V) const { return x * V.x + y * V.y; }
//double  operator ^ (const ofxVec2f& V) const { return x * V.y - y * V.x; }
//	Vector operator * (double k)         const { return Vector(x * k, y * k); }
//	Vector operator / (double k)         const { return Vector(x / k, y / k); }
//double	Length() const { return sqrt(x*x + y*y); }
//double	Normalise()    { double l = Length(); x /= l; y /= l; return l; }

//the following is code copied from some Oliii f

bool ParticleSegmentCollide(ofxVec2f& P, const ofxVec2f& A, const ofxVec2f& B, double r)
{
	ofxVec2f AP = P - A;
	ofxVec2f AB = B - A;
	double u = (AP.x * AB.x + AP.y * AB.y) / (AB.x * AB.x + AB.y * AB.y);

	u = (u < 0.0f)? 0.0f : (u > 1.0f)? 1.0f : u;

	ofxVec2f C = A + AB * u;

	ofxVec2f CP = P - C;
	double l2 = CP.x * CP.x + CP.y * CP.y;

	if (l2 < r * r)
		return false;

	double l = sqrt(l2);

	P += CP * (r - l) / l;

	return true;
}

bool ParticleSegmentCollide(const ofxVec2f& Pold, ofxVec2f& Pnew, 
                            const ofxVec2f& A, const ofxVec2f& B)
{
	ofxVec2f V  = Pnew - Pold;
	ofxVec2f AB = B - A;
	ofxVec2f AP = Pold - A;
	
	double VxAB = V.x * AB.y - V.y * AB.x;
	double APxAB = AP.x * AB.y - AP.y * AB.x;
	
	
	if(fabs(VxAB) < 0.00001f)
	{
		ParticleSegmentCollide(Pnew, A, B, 1.0f);
	}
	double t = -(APxAB / VxAB);

	if (t < 0.0f || t > 1.0f)
		return false;

	ofxVec2f Pcoll = Pold + V * t;
	
	AP = Pcoll - A;
	
	double u = (AP.x * AB.x + AP.y * AB.y) / (AB.x * AB.x + AB.y * AB.y);
	
	if(u < 0.0f || u > 1.0f)
		return false;

	Pnew = Pcoll;

	return true;
}

bool ParticleSegmentCollide(const ofxVec2f& Pold, ofxVec2f& Pnew, 
                            const ofxVec2f& Aold, ofxVec2f& Anew, 
                            const ofxVec2f& Bold, ofxVec2f& Bnew)
{
    ofxVec2f AP = Pold - Aold;
    ofxVec2f AB = Bold - Aold;
    ofxVec2f Vp = Pnew - Pold;
    ofxVec2f Va = Anew - Aold;
    ofxVec2f Vb = Bnew - Bold;
    ofxVec2f Vap = Vp - Va;
    ofxVec2f Vab = Vb - Va;

    double a = (Vap.x * Vab.y) - (Vap.y * Vab.x);
    double b = (AP.x*Vab.y + AB.y*Vap.x) - (AP.y*Vab.x + AB.x*Vap.y);
    double c = (AP.x * AB.y) - (AP.y * AB.x);
    double d = (b*b - 4*a*c);
	double t;

	
	if (fabs(a) < 0.00001f)
		return ParticleSegmentCollide(Pold, Pnew, Aold, Bold);

    if(d < 0.0f) 
        return false;

    double t0 = (-b - sqrt(d)) / (2.0f * a);
    double t1 = (-b + sqrt(d)) / (2.0f * a);
    double tmin = (t1 < t0)? t1 : t0;
    double tmax = (t1 > t0)? t1 : t0;
    
    if(tmin > 1.0f || tmax < 0.0f) 
        return false;

    double  tcoll = (tmin < 0.0f)? tmax : tmin;

    if(tcoll > 1.0f || tcoll < 0.0f) 
        return false;

    ofxVec2f Pcoll = Pold + Vp * tcoll;
    ofxVec2f Acoll = Aold + Va * tcoll;
    ofxVec2f Bcoll = Bold + Vb * tcoll;
    
    AP = Pcoll - Acoll;
    AB = Bcoll - Acoll;

    double u = (AP.x * AB.x + AP.y * AB.y) / (AB.x * AB.x + AB.y * AB.y); // '*' = dot product!
   
    if (u < 0.0f || u > 1.0f || !u) 
        return false;
		
	//if(u == nan) return false;
		
	printf("u: %f\n", u);
   
	AP.normalize();
	AB.normalize();
	double delta = (AP.x * AB.y - AP.y * AB.x);

    Pnew = Pcoll;
    Anew = Acoll;
    Bnew = Bcoll;
    t    = tcoll;

    return true;    
}