#include <pfcave.h>
#include <Performer/pf/pfTraverser.h>
#include "walkNavigator.h"

pfType *walkNavigator::classType_ = NULL; 

void walkNavigator::init(void)
{
 if (classType_ == NULL)
	{
        pfDCS::init();
        classType_ =  new pfType(pfDCS::getClassType(), "walkNavigator");
	}
}

walkNavigator::walkNavigator(void)
{
 setType(classType_);  /* set the Performer type of this instance */
 transSpeed_ = 1.0f;
 rotSpeed_ = 1.0f;
 ground_ = NULL;
 isectMask_ = 0xffff;
}

void walkNavigator::setTransSpeed(float s)
{
 transSpeed_ = s;
}

void walkNavigator::setRotSpeed(float s)
{
 rotSpeed_ = s;
}

void walkNavigator::setGround(pfNode *g)
{
 ground_ = g;
}

void walkNavigator::setIsectMask(int m)
{
 isectMask_ = m;
}

int walkNavigator::app(pfTraverser *trav)
{
 float jx=CAVE_JOYSTICK_X,jy=CAVE_JOYSTICK_Y;
 if (fabs(jx) > 0.1)
	CAVENavRot(-jx * rotSpeed_,'z');
 if (fabs(jy) > 0.1)
	{
	pfVec3 dir;
	CAVEGetVector(CAVE_WAND_FRONT,dir.vec);
	dir[2] = 0;
	dir.normalize();
	dir *= jy * transSpeed_;
	CAVENavTranslate(dir[0],dir[1],dir[2]);
	}
 followGround();
 if (CAVEBUTTON1 && CAVEBUTTON3)
	CAVENavLoadIdentity();
 pfCAVEDCSNavTransform(this);
 return pfDCS::app(trav);
}

int walkNavigator::needsApp(void)
{
 return TRUE;
}

void walkNavigator::followGround(void)
{
 pfSegSet segset;
 pfHit **hits[32];
 if (!ground_)
	return;
 segset.activeMask = 1;
 segset.isectMask = isectMask_;
 segset.discFunc = NULL;
 segset.bound = NULL;
 segset.mode = PFTRAV_IS_PRIM;
 segset.segs[0].dir.set(0.0f, 0.0f, -1.0f);
 segset.segs[0].length = 1000.0f;
 CAVEGetPosition(CAVE_HEAD_NAV,segset.segs[0].pos.vec);
 if (ground_->isect(&segset, hits))
	{
	pfVec3 pnt, xpnt, foot;
	pfMatrix xmat;
	getFootPosition(foot);
	(*hits[0])->query(PFQHIT_POINT, pnt.vec);
	(*hits[0])->query(PFQHIT_XFORM, (float*)xmat.mat);
	xpnt.xformPt(pnt, xmat);
	CAVENavTranslate(0,0,xpnt[PF_Z]-foot[PF_Z]);
	}
}

void walkNavigator::getFootPosition(pfVec3 & foot)
{
 pfVec3 head;
 CAVEGetPosition(CAVE_HEAD,head.vec);
 head[PF_Z] = 0;
 CAVENavConvertCAVEToWorld(head.vec,foot.vec);
}