/*----------------------------------------------------
 * 
 * NAVIGATION.c - navigation function
 * 
 *--------------------------------------------------*/
#include "INCLUDES.h"

// navigation data
struct NAV_DATA 
	{
	float G400pos[3] , G400rot;
	};
	
// pointer to navigation data type
static NAV_DATA *myNav ;

/*--------------------------------------------------*/

// allocate navigation data from shared memory
void setup_navigation()
{
	myNav = (NAV_DATA*)CAVEMalloc(sizeof(NAV_DATA));
}

/*--------------------------------------------------*/

// reset navigation data (which resides in shared memory) this 
// function is accessible to all processes in app
void reset_navigation_to(float Nx, float Ny, float Nz, float Nrot)
{
    myNav->G400pos[0] = Nx;
	myNav->G400pos[1] = Ny;
	myNav->G400pos[2] = Nz;
    myNav->G400rot = Nrot ;     
}

/*--------------------------------------------------*/

void do_navigation()
{
	float jx=CAVE_JOYSTICK_X,jy=CAVE_JOYSTICK_Y;
	float cw[3], ww[3]; 
	
	CAVEGetVector(CAVE_WAND_FRONT,cw); 
	CAVENavConvertVectorCAVEToWorld(cw,ww);

	if (fabs(jy) > 0.2) 
		{
		myNav->G400pos[0] += ww[0]*jy/30.0;
		myNav->G400pos[1] += ww[1]*jy/30.0;
		myNav->G400pos[2] += ww[2]*jy/30.0;
		}

	if (fabs(jx) > 0.2)myNav->G400rot -= 0.50*jx;
	
	if (*App_in_use == 0)
	{
	if( myNav->G400pos[0] > 15 )  myNav->G400pos[0] = 15;
	if( myNav->G400pos[0] < -15 )  myNav->G400pos[0] = -15;
	if( myNav->G400pos[1] > 10 )  myNav->G400pos[1] = 10;
	if( myNav->G400pos[1] < 0 )  myNav->G400pos[1] = 0;
	if( myNav->G400pos[2] > 60 )  myNav->G400pos[2] = 60;
	if( myNav->G400pos[2] < -50 ) myNav->G400pos[2] = -50;
	}
	if (*App_in_use == 6)
	{
	if( myNav->G400pos[0] > 80 )  myNav->G400pos[0] = 80;
	if( myNav->G400pos[0] < -80 )  myNav->G400pos[0] = -80;
	if( myNav->G400pos[1] > 10 )  myNav->G400pos[1] = 10;
	if( myNav->G400pos[1] < 0 )  myNav->G400pos[1] = 0;
	if( myNav->G400pos[2] > 50 )  myNav->G400pos[2] = 50;
	if( myNav->G400pos[2] < -50 ) myNav->G400pos[2] = -50;
	}
	
	CAVENavLock();
	CAVENavLoadIdentity();
	CAVENavTranslate(myNav->G400pos[0], 0, myNav->G400pos[2]); 
	CAVENavRot(myNav->G400rot,'y');
	CAVENavUnlock();
	
	printf("app = %d    POS X = %f   POS Y = %f   POS Z = %f \n\n\n\n" ,
	       *App_in_use,  myNav->G400pos[0], myNav->G400pos[1], myNav->G400pos[2]); 	
	
 }
 
/*--------------------------------------------------*/