#include "glut.h"
#include "gl/glu.h"
#include "gl/gl.h"
#include <iostream>
#include "mathlib.h"
#include "langlib.h"
#include "ledslib.h"
#include "stllib.h"
#include "parsestl.h"


octreeNode* CollideNode;
Point* CollidePoint;
Point* Testpoint;
int IN = 1;
int ON = 0;
int OUT = -1;


// Returns TRUE if PHANToM is currently in contact with this object.
// If so, the collision is added to the PHANToM's list through
// gstPHANToM::getCollisionInfo() and gstPHANToM::collisionAdded().
int gstHybridHashGridTriMeshes::collisionDetect(gstPHANToM *PHANToM){
    gstPoint lastSCP,phantomPos;
    
    int inContact=getStateForPHANToM(PHANToM);
    
    // Refer to the release notes??? to explain this
    if(!_touchableByPHANToM || _resetPHANToMContacts){
        _resetPHANToMContacts = FALSE;
        inContact=FALSE;
        updateStateForPHANToM(PHANToM,inContact);
		return inContact;
    }
    else {
        
        // get previous SCP and current Phantom position
        PHANToM->getLastSCP_WC(lastSCP);
        PHANToM->getPosition_WC(phantomPos);
        
        // make sure the probe has moved
        if(lastSCP==phantomPos){
            return inContact;
        }
        
		// Put current phantom position into global TestPoint and determine which node its in
		CollidePoint->Set(phantomPos[0],phantomPos[1],phantomPos[2]);
		CollideNode=findNode(ADF,CollidePoint);
		//If the phantom is OUT of the solid then no collision, otherwise...
		if(CollideNode->inOnOut==OUT){
			inContact=FALSE;
			updateStateForPHANToM(PHANToM,inContact);
			return inContact;
		}
		else{ //The phantom is either IN or ON the solid
			float dist = findInterpDist(CollideNode, CollidePoint);
			//If distance is positive, then phantom is imbedded in the solid 
			//otherwise it didn't collide
			if(dist<0){
				inContact=FALSE;
				updateStateForPHANToM(PHANToM,inContact);
				return inContact;
			}
			else{
				Vector Vect2Collide;
				Vect2Collide=GetNodeGradient(CollideNode, CollidePoint, dist);
				Point Nearest;
				Nearest=(*CollidePoint)+Vect2Collide; // The SCP but in Point form
				//Set the SCP
				gstPoint SCP = gstPoint(Nearest[0],Nearest[1],Nearest[2]); //SCP
				//Set the SCP normal to be the vector used to translate to the SCP
				gstVector SCPnormal = gstVector(Vect2Collide[0],Vect2Collide[1],Vect2Collide[2]);
				//Add the SCP and the SCPnormal to the collision list of the PHANToM
				inContact = TRUE;
				updateStateForPHANToM(PHANToM,inContact);
				addCollision(PHANToM,SCP,SCPnormal);
				return inContact;
			}
		}
	}
}