/* libsegl - Sebas Extended GL Library
 *           Collection of Opengl/3D-Math helpers
 *
 *      Copyright (c) 2008 by Sebastian Lohff, seba@seba-geek.de
 *      http://www.seba-geek.de
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 * 
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 * 
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
 * Boston, MA  02110-1301, USA.
 */

#include "geotypes.h"

namespace segl {

Sphere::Sphere(Punkt3D _pos, float _radius) {
	pos = _pos;
	radius = _radius;
}

Sphere::Sphere() {
	radius = 1.0f;
}

void Sphere::set(Punkt3D _pos, float _radius) {
	pos = _pos;
	radius = _radius;
}

bool Sphere::collision(const Sphere &s) const {
	return ((pos-s.pos)*(pos-s.pos))<((radius+s.radius)*(radius+s.radius));
}

bool Sphere::collision(const Ray &r) const {
	return (r.dist(pos) <= radius);
}

bool Sphere::collision(const Box & b) const {
	return true;
}

bool Sphere::collision(const Plane &p) const {
	return p.dist(pos) <= radius;
}

bool Sphere::inSphere(Punkt3D p) const {
	return abs(pos-p)<=radius;
}

Punkt3D Sphere::getPos() const {
	return pos;
}

int Sphere::getIntersectionParam(const Ray &ray, float *param1, float *param2) {
	int numerg;
	// ax^2 + bx + c = 0
	float a = ray.dir * ray.dir;
	float b = 2 * ray.dir * (ray.pos - pos);
	float c = ray.pos * ray.pos - 2 * ray.pos * pos + pos * pos - radius * radius;
	
	float p = b/a;
	float q = c/a;
	std::cout << p << " " << q << std::endl;
	if((p*p)/4.0f<q) {
		numerg = 0;
	} else if((p*p)/4.0f==q) {
		numerg = 1;
		if(param1!=0) {
			*param1 = -p/2.0f;
		}
	} else {
		numerg = 2;
		if(param1!=0 || param2!=0) {
			if(param1!=0) {
				*param1 = -p/2.0f + sqrt(((p*p)/4.0f)-q);
			}
			if(param2!=0) {
				*param2 = -p/2.0f - sqrt(((p*p)/4.0f)-q);
			}
		}
	}
	
	return numerg;	
}

int Sphere::getIntersectionPoints(const Ray &ray, segl::Punkt3D *a, segl::Punkt3D *b) {
	float pa, pb;
	int numerg = getIntersectionParam(ray, &pa, &pb);
	if(a)
		*a = ray.get(pa);
	
	if(b)
		*b = ray.get(pb);
	
	return numerg;
}

Ray::Ray() {
	dir.set(0.0f, 1.0f, 0.0f);
}

Ray::Ray(const Punkt3D &_pos, const Punkt3D &_dir) {
	set(_pos, _dir);
}

void Ray::set(const Punkt3D &_pos, const Punkt3D &_dir) {
	pos = _pos;
	dir = _dir;
}

void Ray::setFromPoints(const Punkt3D &a, const Punkt3D &b) {
	pos = a;
	dir = b - a;
}

Punkt3D Ray::get(float x) const {
	return pos + dir*x;
}

// TODO: Heavy Testing
bool Ray::onRay(Punkt3D p, int rnd) const {
	float r1 = 0.0f, r2 = 0.0f, r3 = 0.0f;
	short fcount = 0;
	bool g1=true, g2=true, g3=true;
	Punkt3D f = p-pos;
	
	if(dir.x==0.0f) {
		if(f.x!=0.0f)
			return false;
		g1 = false;
		fcount++;
	} else
		r1 = f.x / dir.x;
		
	if(dir.y==0.0f) {
		if(f.y!=0.0f)
			return false;
		g2 = false;
		fcount++;
	} else
		r2 = f.y / dir.y;
		
	if(dir.z==0.0f) {
		if(f.z!=0.0f)
			return false;
		g2 = false;
		fcount++;
	} else
		r2 = f.z / dir.z;
	
	if(fcount>=2)
		return true;
	
	if(rnd>=0) {
		// TODO:Implement rounding
		// r1 = round(r1, rnd);
		// r2 = round(r2, rnd);
		// r3 = round(r3, rnd);
	}
	
	if(g1)
		return (r2 == r3);
	else if(g2)
		return (r1 == r3);
	else if(g3)
		return (r1 == r2);
	else
		return ((r1 == r2) == r3);
}

float Ray::dist(Punkt3D p) const {
	return abs(p - get( getParam(p) ));
}

float Ray::getParam(Punkt3D p, bool onray) const {
	if(onray) {
		if(!onRay(p))
			return 0.0f;
	}
	return -((pos-p)*dir) / (dir*dir);
}

bool Ray::collision(const Sphere &s) const {
	return s.collision(*this);
}

bool Ray::collision(const Ray &r) const {
	return true;
}

bool Ray::collision(const Box & b) const {
	return true;
}

bool Ray::collision(const Plane &p) const {
	return true;
}

Box::Box() {
	max.set(1.0f, 1.0f, 1.0f);
}

Box::Box(Punkt3D _min, Punkt3D _max) {
	min = _min;
	max = _max;
}

bool Box::collision(const Sphere &s) const {
	return s.collision(*this);
}

bool Box::collision(const Ray &r) const {
	return r.collision(*this);
}

bool Box::collision(const Box & b) const {
	return true;
}

bool Box::collision(const Plane &p) const {
	return true;
}

Plane::Plane() {
	norm.set(0.0f, 1.0f, 0.0f);
}

Plane::Plane(Punkt3D _pos, Punkt3D _norm) {
	pos = _pos;
	norm = _norm;
}

Plane::Plane(float x, float y, float z, float a) {
	// TODO: Implementation (if not too lazy)
	norm.set(x, y, z);
	norm.normalize();
}

bool Plane::collision(const Sphere &s) const {
	return s.collision(*this);
}

bool Plane::collision(const Ray &r) const {
	return r.collision(*this);
}

bool Plane::collision(const Box & b) const {
	return b.collision(*this);
}

bool Plane::collision(const Plane &p) const {
	return true;
}

bool Plane::onPlane(const Punkt3D &p) const {
	return dist(p) == 0.0f;
}

bool Plane::getIntersectionPoint(const Ray &r, Punkt3D *p) const {
	if(r.dir.calcAngle(norm) && !onPlane(r.pos))
		return false;
	
	if(!p)
		return true;
	
	float param = (pos*norm - r.pos*norm) / (r.dir*norm);
	*p = r.get(param);
	return true;
}

float Plane::dist(Punkt3D p) const {
	return 0.0f;
}

} // namespace segl