#include <stdio.h>
#include <stdlib.h>
#include <cave_ogl.h>
#include <math.h>

static GLuint fontbase=0;

static void LoadTextFont(Display *display,char *fontname)
{
 XFontStruct *fontinfo=NULL;
 fontinfo = XLoadQueryFont(display,fontname);
 if ((fontinfo) && (!fontinfo->min_byte1) && (!fontinfo->max_byte1))
	{
	fontbase = glGenLists(fontinfo->max_char_or_byte2);
	glXUseXFont(fontinfo->fid,0,fontinfo->max_char_or_byte2,fontbase);
	}
 else
	fprintf(stderr,"ERROR: failed to load font \"%s\"\n",fontname);
}

void DrawText(char *string)
{
 if (fontbase)
	{
	glPushAttrib(GL_LIST_BIT);
	glListBase(fontbase);
	glCallLists(strlen(string),GL_UNSIGNED_BYTE,(GLubyte *)string);
	glPopAttrib();
	}
}


void init_font(void)
{
 LoadTextFont(CAVEXDisplay(),"fixed");
}

void draw_text(void)
{
 char str[256];
 int change,i;
 float pos[3],ori[3],curYpos;

 glMatrixMode(GL_PROJECTION);
 glLoadIdentity();
 glOrtho(0.0,10.0,0.0,10.0,-100.0,100.0);
 glMatrixMode(GL_MODELVIEW);
 glLoadIdentity();
 glColor3f(0.0,0.0,0.0);

 glRasterPos2f(1.0, 8.0);
 DrawText(CAVEVersion);

 sprintf(str,"Wall #%d  %s     Pipe %d of %d",CAVEWall,CAVEWallName(CAVEWall),
	CAVEPipeNumber(),CAVENumPipes());
 glRasterPos2f(1.0,7.6);
 DrawText(str);

 sprintf(str, "Time %.1f   (%.1f)    ", *CAVETime,CAVEGetTime());
 glRasterPos2f(1.0, 7.3);
 DrawText(str);

 sprintf(str, "Frames Per Second %.0f", *CAVEFramesPerSecond);
 glRasterPos2f(1.0, 7.0);
 DrawText(str);

 change = CAVEButtonChange(1);
 if (CAVEBUTTON1)
	sprintf(str, "Button1 Pressed     (%d)",change);
 else
	sprintf(str, "Button1 Not Pressed (%d)",change);
 glRasterPos2f(1.0, 6.7);
 DrawText(str);

 change = CAVEButtonChange(2);
 if (CAVEBUTTON2)
        sprintf(str, "Button2 Pressed     (%d)",change);
 else
        sprintf(str, "Button2 Not Pressed (%d)",change);
 glRasterPos2f(1.0, 6.4);
 DrawText(str);

 change = CAVEButtonChange(3);
 if (CAVEBUTTON3)
        sprintf(str, "Button3 Pressed     (%d)",change);
 else
        sprintf(str, "Button3 Not Pressed (%d)",change);
 glRasterPos2f(1.0, 6.1);
 DrawText(str);

 glRasterPos2f(1.0, 5.8);
/* sprintf(str, "Joystick X: %.2f,  Joystick Y: %.2f", CAVE_JOYSTICK_X, CAVE_JOYSTICK_Y);
 DrawText(str);
*/
 { int i;
   for (i=0; i<CAVEController->num_valuators; i++)
	{
	sprintf(str,"Val %d: %.2f  ",i,CAVEController->valuator[i]);
	DrawText(str);
	}
 }

 CAVEGetPosition(CAVE_HEAD,pos);
 CAVEGetOrientation(CAVE_HEAD,ori);
 sprintf(str, "Head Position: (%.2f,%.2f,%.2f)"
		"   Orientation:(%.2f,%.2f,%.2f)",
		pos[0],pos[1],pos[2],ori[0],ori[1],ori[2]);
 glRasterPos2f(1.0, 5.4);
 DrawText(str);

 CAVEGetPosition(CAVE_WAND,pos);
 CAVEGetOrientation(CAVE_WAND,ori);
 sprintf(str, "Wand Position: (%.2f,%.2f,%.2f)"
		"   Orientation:(%.2f,%.2f,%.2f)",
		pos[0],pos[1],pos[2],ori[0],ori[1],ori[2]);
 glRasterPos2f(1.0, 5.1);
 DrawText(str);

 curYpos = 4.8;
 for (i=2; i<CAVEptr->num_sensors; i++)
	{
	CAVEGetSensorPosition(CAVESENSOR(i),CAVE_TRACKER_FRAME,pos);
	CAVEGetSensorOrientation(CAVESENSOR(i),CAVE_TRACKER_FRAME,ori);
	sprintf(str, "Sensor %d Position: (%.2f,%.2f,%.2f)"
		"   Orientation:(%.2f,%.2f,%.2f)",
		i,pos[0],pos[1],pos[2],ori[0],ori[1],ori[2]);
	glRasterPos2f(1.0, curYpos);	curYpos -= 0.3;
	DrawText(str);
	}

 CAVEGetPosition(CAVE_LEFT_EYE,pos);
 sprintf(str, "Left eye position: (%.2f,%.2f,%.2f)",pos[0],pos[1],pos[2]);
 glRasterPos2f(1.0, curYpos);	curYpos -= 0.3;
 DrawText(str);

 CAVEGetPosition(CAVE_RIGHT_EYE,pos);
 sprintf(str, "Right eye position: (%.2f,%.2f,%.2f)",pos[0],pos[1],pos[2]);
 glRasterPos2f(1.0, curYpos);	curYpos -= 0.3;
 DrawText(str);

 if (CAVEEye == CAVE_LEFT_EYE)
	{
	glRasterPos2f(1.0, curYpos);	curYpos -= 0.3;
	DrawText("CAVEEye Left Eye");
	}
 else if (CAVEEye == CAVE_RIGHT_EYE)
	{
	glRasterPos2f(5.0, curYpos);	curYpos -= 0.3;
	DrawText("CAVEEye Right Eye");
	}

 glRasterPos2f(1.0, curYpos);	curYpos -= 0.3;
 if (CAVEMasterDisplay())
	DrawText("CAVEMasterDisplay True");
 else
	DrawText("CAVEMasterDisplay False");
}
    	

void draw_axes()
{
    float wand[3],vector[3],org[3] = {0,0,0};
    glLineWidth(2.0);
    glPushMatrix();
    CAVEGetPosition(CAVE_WAND,wand);
    glTranslatef(wand[0],wand[1],wand[2]);
    CAVEGetVector(CAVE_WAND_UP,vector);
    glColor3f(0.0,0.0,0.0);
    glBegin(GL_LINE_STRIP);
     	glVertex3fv(org);
	glVertex3fv(vector);
    glEnd();
    CAVEGetVector(CAVE_WAND_FRONT,vector);
    glBegin(GL_LINE_STRIP);
     	glVertex3fv(org);
	glVertex3fv(vector);
    glEnd();
    CAVEGetVector(CAVE_WAND_LEFT,vector);
    glBegin(GL_LINE_STRIP);
     	glVertex3fv(org);
	glVertex3fv(vector);
    glEnd();
    glColor3f(1.0,0.0,0.0);
    CAVEGetVector(CAVE_WAND_DOWN,vector);
    glBegin(GL_LINE_STRIP);
     	glVertex3fv(org);
	glVertex3fv(vector);
    glEnd();
    CAVEGetVector(CAVE_WAND_BACK,vector);
    glBegin(GL_LINE_STRIP);
     	glVertex3fv(org);
	glVertex3fv(vector);
    glEnd();
    CAVEGetVector(CAVE_WAND_RIGHT,vector);
    glBegin(GL_LINE_STRIP);
     	glVertex3fv(org);
	glVertex3fv(vector);
    glEnd();
    glPopMatrix();    
    glLineWidth(1.0);
}	

void draw_all(void)
{
 glClearColor(1.0,1.0,1.0,1.0);
 glClear(GL_COLOR_BUFFER_BIT);
 glClear(GL_DEPTH_BUFFER_BIT);
 draw_axes();
 draw_text();
}

main(int argc, char* argv[])
{
 CAVEConfigure(&argc, argv, NULL);
 CAVEInit();
 CAVEInitApplication(init_font, 0);
 CAVEDisplay(draw_all, 0);
 while ((!CAVEgetbutton(CAVE_ESCKEY)) && (!CAVESync->Quit))
	{
	if (CAVEgetbutton(CAVE_F12KEY))
		{
		CAVEResetTracker();
		while (CAVEgetbutton(CAVE_F12KEY)) ;
		}
	sginap(1);
	}
 CAVEExit();
}