/*
 * Collision-detection pseudo-tree class
 *
 * Class for performing several collision detection algorithms automatically
 *
 * Copyright (C) 2008  Alejandro Valenzuela Roca, <lanjoe9 at mexinetica NOSPAMPLEASE dot com>
 *
 * This file is part of MotorJ, a free framework for videogame development.
 * <http://wiki.lidsol.net/wiki/index.php?title=MotorJ >
 * <http://motorj.mexinetica.com/ >
 *
 * MotorJ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * MotorJ 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with MotorJ.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include "coltree-class.h"

mjColTree::mjColTree()
{
	 list_init(&clist);
}

short mjColTree::CalcCollision (vector3 & pos, vector3 * correction)
{
	short ret_val = COLR_NOCOLLISION;
	short currentLevel = 0;
	
	
	// lnode:
	// type is taken as "expected return value to continue collision tests"
	// data holds a mjColTreeBranch structure
	// colType defines which collision test to perform and therefore which 
	// argument structure type is stored in void * argumentStructure
	
	// Start by picking the root node of the collision pseudo tree
	lnode = clist.first;
	
	// Now, while the current node (branch) is valid (i.e. "not NULL"), find out..
	while (lnode)
	{
		// ... what sort of collision test it is
		// Then, extract the arguments based on the kind of collision it's 
		// supposed to be and perform the collision test.
		currentLevel--;
		
		cbranch = (mjColTreeBranch *) lnode->data;
		switch(cbranch->colType)
		{
			case COL_DIST:
			{
				vector3 * tmp;
				mjColTreeDistColBranch * dcol = (mjColTreeDistColBranch *) cbranch->argumentStructure;
				if (dcol->pt0_pt1)
					tmp = dcol->pt0_pt1;
				else
					tmp = new vector3;
				memcpy(tmp, &pos, sizeof(vector3));
				vec_sum (-1, *dcol->pt1, tmp);
				
				ret_val = (vec_norm (*tmp) < dcol->dist);
				
				if (!dcol->pt0_pt1)
					delete tmp;
				
			} break;
			case COL_SPH_SPH:
			{
				mjColTreeSphSphColBranch * sscol = (mjColTreeSphSphColBranch *) cbranch->argumentStructure;
				ret_val = col_sph_sph (pos, sscol->r0, *sscol->sphcen1, sscol->r1, sscol->sph0_sph1);
			} break;
			case COL_AABB_PT:
			{
				mjColTreeAABBPtColBranch * apcol = (mjColTreeAABBPtColBranch *) cbranch->argumentStructure;
				ret_val = col_aabb_pt (*apcol->vaabb0, *apcol->vaabb1, pos);
				
			} break;
			case COL_AAB_AAB:
			{
				mjColTreeAABBAABBColBranch * aacol = (mjColTreeAABBAABBColBranch *) cbranch->argumentStructure;
				ret_val = col_aabb_aabb (* aacol->vaabb00, *aacol->vaabb01, 
										 *aacol->vaabb10, *aacol->vaabb11);
			} break;
			case COL_OBB_OBB:
			{
				mjColTreeOBBOBBColBranch * oocol = (mjColTreeOBBOBBColBranch *) cbranch->argumentStructure;
				ret_val = col_obb_obb ( *oocol->obb0, *oocol->obb1);
			} break;
			case COL_RAY_TR:
			{
				mjColTreeRayTrColBranch * rtcol = (mjColTreeRayTrColBranch *) cbranch->argumentStructure;
				ret_val = col_ray_tr ( pos, rtcol->ray_dir, *rtcol->tr, correction);
			} break;
			case COL_RAY_TRSTRIP:
			{
				mjColTreeRayTrStripColBranch * rtcol = (mjColTreeRayTrStripColBranch *) cbranch->argumentStructure;
				ret_val = COLR_NOCOLLISION;
				unsigned trindex = 0;
				while ((rtcol->tr_strip[trindex]) && (ret_val == COLR_NOCOLLISION))
				{
					
					ret_val = col_ray_tr ( pos, rtcol->ray_dir, *rtcol->tr_strip[trindex], correction);
					trindex++;
				}
			} break;
			case COL_PT_AACYL:
			{
				mjColTreePtAACylColBranch * pacol = (mjColTreePtAACylColBranch *) cbranch->argumentStructure;
				ret_val = col_pt_aacyl ( pos, * pacol->cyl_base,  pacol->cyl_height,  pacol->cyl_radius, correction);
			} break;
		}
		
		// If we reach the pseudo tree's last branch, then we return whatever
		// the collision test has returned.
		// Otherwise we must find out if it matches the "expected return value"
		// (stored in lnode->type), if it does, we keep moving on, otherwise
		// we return the maximum level we reached, with its sign inverted.
		
		if (lnode->nxt == NULL)
		{
			return ret_val;
		} else if (ret_val == lnode->type)
		{
			lnode = lnode->nxt;
		} else
		{
			return currentLevel;
		}
	}
	return COLR_NOCOLLISION;
}
mjColTree::~mjColTree ()
{
	list_node * prevnode;
	
	// Destroy the contents of each node
	
	// Start by picking the root node of the collision pseudo tree
	lnode = clist.first;
	
	// Now, while the current node (branch) is valid (i.e. "not NULL")..
	while (lnode)
	{
		// get our current node's data cast as a generic collision branch
		cbranch = (mjColTreeBranch *) lnode->data;
		// delete our current node's data
		delete cbranch;
		// record our current node for destruction too
		prevnode = lnode;
		// set our current node to the "next node"
		lnode = lnode->nxt;
		// and delete the previous node
		delete prevnode;
	}
	// The programmers must delete by themselves, if necessary, all the 
	// particular collision branches (i.e. contained in 
	// cbranch->argumentStructure).
}



void mjColTree::AddColBranch(short expectedResult, 
								MJCOLTREEBRANCHTYPE colType, 
								void * argumentStructure)
{
	mjColTreeBranch * nwBranch = new mjColTreeBranch;
	nwBranch->colType = colType;
	nwBranch->argumentStructure = argumentStructure;
	list_append(expectedResult, (void *) nwBranch, &clist);
}