#ifndef ENTITY_TRANSFORM
#define ENTITY_TRANSFORM

#include <iostream>
#include <cassert>

#include "entity.h"
#include "transform.h"

struct Entity_Transform : public Entity
{
	virtual ~Entity_Transform(){}

	void SetParentToChild(const Transform& t)
		{
			p2c = t;
			c2p = t;
			c2p.MakeInverse();
		}
	void SetChildToParent(const Transform& t)
		{
			c2p = t;
			p2c = t;
			p2c.MakeInverse();
		}
	virtual RayIntersect CalcRayIntersect(Ray ray, int flags, int special=-1)
		{
			ray.o = p2c * ray.o;
			ray.d = p2c.rot * ray.d;
			double d_mag = 1.0/ray.d.mag();
			ray.d *= d_mag;
			RayIntersect ri = E->CalcRayIntersect(ray, flags, special);
			ri.param_t *= d_mag;
			return ri;
		}
	virtual vector3d CalcNormal(Ray ray, RayIntersect&ri)
		{
			ray.o = p2c * ray.o;
			ray.d = p2c.rot * ray.d;
			double d_mag = 1.0/ray.d.mag();
			ray.d *= d_mag;
			RayIntersect ri2 = ri;
			ri2.param_t /= d_mag;
			vector3d V = E->CalcNormal(ray, ri2);
			return c2p * V;
		}
	virtual void PathCompress(const Transform&T,
							  queue<pair<Entity*,Transform> >&Q,
							  vector<Entity*>&V)
		{
			
			Q.push(pair<Entity*,Transform>(E, T * c2p));
		}
	virtual int IntersectionTest(Ray ray, int flags, int special=-1)
		{
			ray.o = p2c * ray.o;
			ray.d = (p2c.rot * ray.d).make_unit();
			return E->IntersectionTest(ray, flags, special);
		}
	virtual void GridCollect(vector<EntityWrapBound>& vec)
		{
			assert("transformations & binning not supported" && 0);
		}


	Transform p2c, c2p;
	Entity * E;
};

struct Entity_Translate : public Entity
{
	virtual ~Entity_Translate(){}
	void SetParentToChild(const vector3d& t)
		{
			p2c = t;
		}
	void SetChildToParent(const vector3d& t)
		{
			p2c = -t;
		}
	virtual RayIntersect CalcRayIntersect(Ray ray, int flags, int special=-1)
		{
			ray.o += p2c;
			return E->CalcRayIntersect(ray, flags, special);
		}
	virtual vector3d CalcNormal(Ray ray, RayIntersect& ri)
		{
			ray.o += p2c;
			return E->CalcNormal(ray, ri);
		}
	virtual void PathCompress(const Transform&T,
							  queue<pair<Entity*,Transform> >&Q,
							  vector<Entity*>&V)
		{
			Transform TT = T;
			TT.trans += T.rot * -p2c;
			Q.push(pair<Entity*,Transform>(E, TT));
		}
	virtual int IntersectionTest(Ray ray, int flags, int special=-1)
		{
			ray.o += p2c;
			return E->IntersectionTest(ray, flags, special);
		}
	virtual void GridCollect(vector<EntityWrapBound>& vec)
		{
			assert("transformations & binning not supported" && 0);
		}


	vector3d p2c;
	Entity * E;
};

struct Entity_RotateScale : public Entity
{
	virtual ~Entity_RotateScale(){}
	void SetParentToChild(const matrix3d& t)
		{
			p2c = t;
			c2p = t.Inverse();
		}
	void SetChildToParent(const matrix3d& t)
		{
			c2p = t;
			p2c = t.Inverse();
		}
	virtual RayIntersect CalcRayIntersect(Ray ray, int flags, int special=-1)
		{
			ray.o = p2c * ray.o;
			ray.d = p2c * ray.d;
			double d_mag = 1.0/ray.d.mag();
			ray.d *= d_mag;
			RayIntersect ri = E->CalcRayIntersect(ray, flags, special);
			ri.param_t *= d_mag;
			return ri;
		}
	virtual vector3d CalcNormal(Ray ray, RayIntersect&ri)
		{
			ray.o = p2c * ray.o;
			ray.d = p2c * ray.d;
			double d_mag = 1.0/ray.d.mag();
			ray.d *= d_mag;
			RayIntersect ri2 = ri;
			ri2.param_t /= d_mag;
			vector3d V = E->CalcNormal(ray, ri2);
			return c2p * V;
		}
	virtual void PathCompress(const Transform&T,
							  queue<pair<Entity*,Transform> >&Q,
							  vector<Entity*>&V)
		{
			Transform TT = T;
			TT.rot = T.rot * c2p;
			Q.push(pair<Entity*,Transform>(E, TT));
		}
	virtual int IntersectionTest(Ray ray, int flags, int special=-1)
		{
			ray.o = p2c * ray.o;
			ray.d = (p2c * ray.d).make_unit();
			return E->IntersectionTest(ray, flags, special);
		}
	virtual void GridCollect(vector<EntityWrapBound>& vec)
		{
			assert("transformations & binning not supported" && 0);
		}

	matrix3d p2c, c2p;
	Entity * E;
};

#endif