#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 {
	/*
	// way more complex (i think), maybe usefull to calc the collpoints
	const float bsum = r.dir.x + r.dir.y + r.dir.z;
	float p = (2.0f*(r.pos*r.dir)-pos*r.dir) / bsum;
	float q = (r.pos*r.pos-2.0f*r.pos*pos+pos*pos-radius*radius) / bsum;
	return (p*p/4.0f-q >= 0.0f);
	*/
	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;
}

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

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

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

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 && r1 == 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;
}

float Plane::dist(Punkt3D p) const {
		
}

} // namespace segl