// Description: creates a navigator with CAVE navigation data for the user
//
//<b>notes:</b>
//<ul>
//<li> derived nodes inherit all messages from their base classes
//<li> see reset method for default settings
//<li> this node can also be created with the alias: CAVENavigator
//<li> this node should be a child of a <a href="ygUser.html">ygUser</a> node
//<li> sets the navigator of the user above it for use in navigation
//<li> this node is required for navigating the CAVE within the virtual space
//<li> reads straight from the joystick port on the host machine.  
//<li> useful if you want driving without trackd.
//</ul>
//
// 
// Category: User
// Author: Ben Chang
// Revision: 2/7/04
//
#include <Performer/pr/pfLinMath.h>
#include <Performer/pf/pfScene.h>
#include <pfcave.h>
#include "ygWorld.h"
#include "ygUtil.h"
#include "ygSphereIsect.h"
#include "JoystickNavigator.h"
#include "ygWand.h"
#include "DummyWand.h"
#include "ygUser.h"

using namespace std;

extern "C" ygNode* construct_JoystickNavigator(const char* name,bool master) { return new JoystickNavigator(name,master); }


JoystickNavigator::JoystickNavigator(const char* name,bool master) : ygNavigator(name,master)
	{
	rotMaxSpeed_ = 45;
	rotSpeed_ = 0;
	transSpeed_ = 0;
	maxSpeed_ = 10;
	acceleration_ = 1;
	rotAcceleration_ = 10;
	friction_ = .5;
	collide_ = true;
	collideRadius_ = 1;
	fly_ = true;
	
	joystickX_ = 0;
	joystickY_ = 0;
	
	char axis = 2;
	char buttons = 4;
	int version = 0x000800;
	char jname[NAME_LENGTH] = "Unknown";
	char *devname = "/dev/js0";

	if ((fd_ = open(devname, O_RDONLY)) < 0) {
		perror("couldn't open joystick port");
	}

	ioctl(fd_, JSIOCGVERSION, &version);
	ioctl(fd_, JSIOCGAXES, &axis);
	ioctl(fd_, JSIOCGBUTTONS, &buttons);
	ioctl(fd_, JSIOCGNAME(NAME_LENGTH), jname);
	
	for (int i=0;i<16;i++) {
		buttonMapping[i] = i;
		flags[i]=0;
	}
		
	mode = 1;

	}

void JoystickNavigator::reset(void)
	{
	//set rotation speed to 45
	rotMaxSpeed_ = 45;
	rotSpeed_ = 0;
	rotAcceleration_ = 10;
	//set translation speed to 0
	transSpeed_ = 0;
	maxSpeed_ = 10;
	acceleration_ = 1;
	friction_ = .5;
	
	//set collide to true
	collide_ = true;
	//set collide radius to 1
	collideRadius_ = 1;
	//set fly mode to true
	fly_ = true;
	ygNavigator::reset();
	}

void JoystickNavigator::message(const ygMessage& msg)
	{
	//move the user to the absolute position given
	if (msg == "teleport")
		{
		pfVec3 pos;
		if (msg.getVec3Args(pos))
			teleportTo(pos);
		}
	//orient the user to the absolute rotation given
	else if (msg == "heading")
		{
		if (msg.args.size() > 0)
			setHeading(msg.floatArg(0));
		}
	//set the forward and backward navigation speed
	else if (msg == "speed")
		{
		if (msg.args.size() > 0)
			setSpeed(msg.floatArg(0));
		}
	//set the turning rotational speed
	else if (msg == "rotspeed")
		{
		if (msg.args.size() > 0)
			setRotSpeed(msg.floatArg(0));
		}
	// set the acceleration
	else if (msg == "acceleration")
		{
		if (msg.args.size() > 0)
			setAcceleration(msg.floatArg(0));
		}
	else if (msg == "rotacceleration")
		{
		if (msg.args.size() > 0)
			setRotAcceleration(msg.floatArg(0));
		}
	else if (msg == "friction")
		{
		if (msg.args.size() > 0)
			setFriction(msg.floatArg(0));
		}
	// button remapping 
	else if (msg == "remapButton")
		{
		if (msg.args.size()>1)
			setButtonMapping (msg.intArg(0),msg.intArg(1));
		}	
	//turn on collision detection with objects
	else if (msg == "collide")
		{
		if (msg.args.size() > 0)
			setCollide(msg.boolArg(0));
		else
			setCollide();
		}
	//set the collision radius around the head
	else if (msg == "collideRadius")
		collideRadius_ = msg.floatArg(0);
	//toggle the collision detection mode
	else if (msg == "togglecollide")
		setCollide(!collide_);
	//set the fly mode between walking and flying
	else if (msg == "fly")
		{
		if (msg.args.size() > 0)
			setFly(msg.boolArg(0));
		else
			setFly();
		}
	//toggle the fly mode
	else if (msg == "togglefly")
		setFly(!fly_);
	//print out the current navigator position
	else if (msg == "printnav")
		{
		cout << "Navigator position = " << data().position()
			<< "  orientation = " << data().orientation() << endl;
		}
	else
		ygNavigator::message(msg);
	}


void JoystickNavigator::teleportTo(const pfVec3& pos)
	{
	data().setPosition(pos);
	resetCollision();
	}
	
void JoystickNavigator::setHeading(float deg)
	{
	data().setHeading(deg);
	resetCollision();
	}

void JoystickNavigator::setSpeed(float s)
	{
	maxSpeed_ = s;
	}


void JoystickNavigator::setRotSpeed(float s)
	{
	rotMaxSpeed_ = s;
	}

void JoystickNavigator::setAcceleration(float a)
	{
	acceleration_ = a;
	}
void JoystickNavigator::setRotAcceleration(float a)
	{
	rotAcceleration_ = a;
	}
void JoystickNavigator::setFriction (float f)
	{
	friction_ = f;
	}
	
void JoystickNavigator::setCollide(bool val)
	{
	if ((val) && (!collide_))
		resetCollision();
	collide_ = val;
	}


void JoystickNavigator::setFly(bool val)
	{
	fly_ = val;
	}

void JoystickNavigator::setButtonMapping(int real,int reported)
	{
		buttonMapping[real]=reported;
	}

void JoystickNavigator::app(void)
	{
	// 1. read joystick data
	// 2. check events, update joystick values, check and move and stuff.
	
	//if the joystick X direction is above threshold
	
	//fprintf (stderr,"x: %f y:%f\n",CAVE_JOYSTICK_X,CAVE_JOYSTICK_Y);
	
	
	float fJS_X,fJS_Y;
	struct js_event js;
	struct timeval tv;
	int readrc;
	fd_set set;
	ygUser * myUser ;
	ygWand * wand ;
	tv.tv_sec = 0;
	tv.tv_usec = JS_SELECT_UDELAY;

	FD_ZERO (&set);
	FD_SET (fd_,&set);
	if (select(fd_+1,&set,NULL,NULL, &tv))
	{

		readrc = read(fd_,&js,sizeof(struct js_event));
		if (readrc == sizeof(struct js_event)) {
			//printf ("%d\n",js.type);
			switch ((js.type&3)) {
			case JS_EVENT_BUTTON:
				//printf ("[button %d:%d] \n ",js.number, js.value);
				myUser = user ();
				wand = (ygWand*) (myUser->wand(0));
				if (wand->isOfClass("DummyWand"))
				{
					
					DummyWand * dwand = (DummyWand*) wand;
					dwand->setButton(buttonMapping[js.number],js.value);
					flags[buttonMapping[js.number]] = js.value;
				}
				break;
			case JS_EVENT_AXIS:
				//printf ("[axis %d:%d]\n",js.number, js.value); 
				if (js.number == 0)
					joystickX_ = js.value;
				if (js.number == 1)
					joystickY_ = js.value;
				break;
			}
		} else if (errno != EAGAIN) {
			perror ("\njoystick: error reading");

		}
	}
	if (joystickX_ > 15000) joystickX_ = 15000;
	if (joystickX_ < -15000) joystickX_ = -15000;
	if (joystickY_ > 15000) joystickY_ = 15000;
	if (joystickY_ < -15000) joystickY_ = -15000;
	
	fJS_X = (float) joystickX_ / 15000.0;
	fJS_Y = -(float) joystickY_ / 15000.0;
		
	if (fabs(fJS_X) > 0.01f)
		{
		
		//rotate the navigation heading relative to the joystick value
		/*h -= fJS_X * (fabs(fJS_X)-0.2f) *
			rotSpeed_ * ygWorld::FrameDeltaTime;*/
		
		rotSpeed_ += (rotAcceleration_ * fJS_X * ygWorld::FrameDeltaTime);
		if (rotSpeed_>rotMaxSpeed_) rotSpeed_=rotMaxSpeed_;
		if (rotSpeed_<-rotMaxSpeed_) rotSpeed_=-rotMaxSpeed_;
		}
	rotSpeed_ *= (1-(friction_*ygWorld::FrameDeltaTime));	
	float h = data().heading();
	h-= (rotSpeed_ * ygWorld::FrameDeltaTime);
		
	data().setHeading(h);	
	//if the joystick Y direction is above threshold
	if (fabs(fJS_Y) > 0.01f)
		{

		//translate the navigation position relative to the joystick value
		transSpeed_ += (acceleration_ * fJS_Y * ygWorld::FrameDeltaTime);
		
		if (transSpeed_ > maxSpeed_) transSpeed_ = maxSpeed_;
		if (transSpeed_ < -maxSpeed_) transSpeed_ = -maxSpeed_;	
		}
		
	pfVec3 dir, pos = data().position();
	//get the CAVE wand direction
	CAVEGetVector(CAVE_WAND_FRONT_NAV, dir.vec);
	//if fly mode is false then zero the Z component
	if (!fly_)
		dir[PF_Z] = 0;
	
	transSpeed_ *= (1-(friction_*ygWorld::FrameDeltaTime));	
	pos += dir * transSpeed_ * ygWorld::FrameDeltaTime;
		/*
		pos += dir * fJS_Y * transSpeed_
			* ygWorld::FrameDeltaTime;*/
			
		data().setPosition(pos);
		
	//if fly mode is false then adjust height to any ground collision
	if (!fly_)
		checkGround();
	//if collision detection is on then test for object collisions
	if (collide_)
		checkCollision();
	ygNavigator::app();
	}


void JoystickNavigator::checkGround(void)
	{
	pfSegSet segset;
	pfHit **hits[32];
	segset.activeMask = 1;
	segset.isectMask = YG_ISECT_FLOORMASK;
	segset.discFunc = NULL;
	segset.bound = NULL;
	segset.mode = PFTRAV_IS_PRIM;
	//generate intersection segment down from user head
	segset.segs[0].dir.set(0.0f, 0.0f, -1.0f);
	segset.segs[0].length = 5000.0f;
	CAVEGetPosition(CAVE_HEAD_NAV, segset.segs[0].pos.vec);
	//if intersection detected then adjust user height to collision position
	if (ygWorld::World->pfscene()->isect(&segset, hits))
		{
		pfVec3 pnt, xpnt, pos = data().position();
		pfMatrix xmat;
		(*hits[0])->query(PFQHIT_POINT, pnt.vec);
		(*hits[0])->query(PFQHIT_XFORM, (float*)xmat.mat);
		xpnt.xformPt(pnt, xmat);
		if (pos[PF_Z] > xpnt[PF_Z] + 2.0f)
			pos[PF_Z] -= 2.0f;
		else
			pos[PF_Z] = xpnt[PF_Z];
		data().setPosition(pos);
		}
	}


void JoystickNavigator::checkCollision(void)
	{
	pfVec3 headTrackerPos, rotHeadTrackerPos, curPos;
	pfMatrix rotXform;
	bool OK;
	CAVEGetPosition(CAVE_HEAD, headTrackerPos.vec);
	rotXform.makeRot(data().heading(),0,0,1);
	rotHeadTrackerPos.xformPt(headTrackerPos, rotXform);
	curPos = rotHeadTrackerPos + data().position();
	if (curPos.almostEqual(prevCollidePos_,.001))
		return;
	OK = positionOK(curPos);
	if (OK)
		prevCollidePos_ = curPos;
	else
		data().setPosition(prevCollidePos_ - rotHeadTrackerPos);
	}


bool JoystickNavigator::positionOK(pfVec3 pos)
	{
	pfSphere sphere;
	sphere.center = pos;
	sphere.radius = collideRadius_;
	return !ygSphereIsect(ygWorld::World->pfscene(),&sphere,YG_ISECT_WALLMASK);
	}


void JoystickNavigator::resetCollision(void)
	{
	pfVec3 headTrackerPos, rotHeadTrackerPos;
	pfMatrix rotXform;
	CAVEGetPosition(CAVE_HEAD, headTrackerPos.vec);
	rotXform.makeRot(data().heading(),0,0,1);
	rotHeadTrackerPos.xformPt(headTrackerPos, rotXform);
	prevCollidePos_ = rotHeadTrackerPos + data().position();
	}