#define OPENGL
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <getopt.h>
#include <Performer/pf.h>
#include <Performer/pf/pfChannel.h>
#include <Performer/pf/pfEarthSky.h>
#include <Performer/pf/pfLightSource.h>
#include <Performer/pf/pfDCS.h>
#include <Performer/pf/pfSwitch.h>
#include <Performer/pf/pfPartition.h>
#include <Performer/pfdu.h>
#include <Performer/pfutil.h>
#include <pfcave.h>
#ifdef SOUND
#define Boolean long
#include <vssClient.h>
#include "AudioEnv.h"
#endif

struct _keyword {
		char *str;
		int id;
		};


struct _ppoint { float x,y,z; };

typedef struct
	{
	float time;
	int npoints;
	struct _ppoint *point;
	float *heading;
	} path_t;

pfNode * load_objects(char *filename/*,pfGroup* */);
void do_navigation(void);
pfNode * loadObject(char *line);
static int getStringID(char *str,struct _keyword *table);
static char * nextToken(char **str);
void compute_nav_height(pfScene *floor);
pfScene * get_floor(char *filename);
path_t *load_path(char *fname);
int Navigate_path(path_t *path);
void restart_path(void);
void create_scene(pfScene **,pfDCS **,pfSwitch **);
void initGraphics(void);
void init_textures(void);

static pfList **texlist;

main(int argc,char **argv)
{
 int sceneNum=0,numscenes,flying=1,opt,followPath=0;
 char *sounddata=NULL,*floorName=NULL;
 path_t *path = NULL;
#ifdef SOUND
 AudioEnv *audio_env = NULL;
#endif
 pfScene *scene,*floor=NULL;
 pfDCS *nav_dcs;
 pfSwitch *sceneSwitch;
 pfChannel *masterChan;
 pfInit();
 pfCAVEConfig(&argc,argv,NULL);
 CAVEFar = 250;
 while ((opt = getopt(argc,argv,"f:p:s:")) != EOF)
	switch (opt)
		{
		case 'f': floorName = strdup(optarg); break;
		case 'p': path = load_path(optarg); break;
		case 's': sounddata = strdup(optarg); break;
		}
 if (optind >= argc)
	{
	fprintf(stderr,"\nUsage: %s [-f floor.obj] [-p path] [-s sound] objlist1 [objlist2 ...]\n",argv[0]);
	exit(1);
	}
 texlist = (pfList **)pfMalloc(sizeof(pfList *),pfGetSharedArena());
 pfConfig();
 create_scene(&scene,&nav_dcs,&sceneSwitch);
 if (floorName)
	floor = get_floor(floorName);
 if (path)
	followPath = 1;
 numscenes = argc - optind;
 for (sceneNum=0; sceneNum < numscenes; sceneNum++)
	sceneSwitch->addChild(load_objects(argv[sceneNum+optind]));
{ pfSphere bsphere;
 scene->getBound(&bsphere);
 CAVENavTranslate(bsphere.center.vec[0],bsphere.center.vec[1],bsphere.center.vec[2]);
}
 sceneNum=0;
 sceneSwitch->setVal(sceneNum);
 scene->setTravMask(PFTRAV_ISECT, 0xffffffff,
			PFTRAV_SELF|PFTRAV_DESCEND|PFTRAV_IS_CACHE, PF_OR);
 pfCAVEInitChannels();
 masterChan = pfCAVEMasterChan();
 masterChan->setScene(scene);
 if (CAVEConfig->Simulator)
	masterChan->getFStats()->setClass(PFSTATS_ALL^PFSTATSHW_ENGFXPIPE_FILL, PFSTATS_ON);
 else
	masterChan->getFStats()->setClass(PFSTATS_ALL, PFSTATS_OFF);
 pfCAVEPreDrawFunc((pfChanFuncType)initGraphics);
 *texlist = pfuMakeSceneTexList(scene);
#ifdef SOUND
 if ((sounddata) && (CAVEDistribMaster()))
	{
	if (!FBgnSoundServer())
	        printf("UDP connection to sound server failed\n");
	else
		audio_env = new AudioEnv(sounddata);
	}
#endif
 while (!CAVEgetbutton(CAVE_ESCKEY))
	{
	pfSync();
	pfCAVEPreFrame();
	pfFrame();
	pfCAVEPostFrame();
	if (CAVEDistribMaster())
		{
		if (CAVEButtonChange(3) == -1)
			{
			sceneNum = (sceneNum+1) % numscenes;
			sceneSwitch->setVal(sceneNum);
			}
		if ((path) && (CAVEgetbutton(CAVE_PKEY)))
			{
			restart_path();
			followPath = 1;
			}
		if (followPath)
			followPath = Navigate_path(path);
		else
			{
			if (CAVEButtonChange(2) == -1)
				flying = !flying;
			do_navigation();
			if ((!flying) && (floor))
				compute_nav_height(floor);
			}
		pfCAVEDCSNavTransform(nav_dcs);
#ifdef SOUND
		if (audio_env)
			audio_env->Update(1);
#endif
		}
	}
#ifdef SOUND
 if (audio_env)
	{
	delete audio_env;
	EndSoundServer();
	}
#endif
 pfCAVEHalt();
 pfExit();
 return 0;
}

void initGraphics(void)
{
 pfCullFace(PFCF_OFF);
 pfOverride(PFSTATE_CULLFACE, PF_ON);
 pfuDownloadTexList(*texlist, PFUTEX_SHOW);
 pfCAVEPreDrawFunc(NULL);
}

path_t *load_path(char *fname)
{
 FILE *fp;
 char line[512];
 path_t *p;
 int i;
 if ((fp = fopen(fname,"r")) == NULL)
	{
	perror(fname);
	return NULL;
	}
 p = (path_t *) malloc(sizeof(path_t));
 p->npoints = 0;
 fgets(line,512,fp);
 while (fgets(line,512,fp))
	p->npoints++;
 fclose(fp);
 p->point = (struct _ppoint *) malloc(p->npoints*sizeof(struct _ppoint));
 p->heading = (float *) malloc(p->npoints * sizeof(float));
 fp = fopen(fname,"r");
 fgets(line,512,fp);
 sscanf(line,"%f",&p->time);
 for (i=0; i<p->npoints; i++)
	{
	fgets(line,512,fp);
	sscanf(line,"%f%f%f",&p->point[i].x,&p->point[i].y,&p->point[i].z);
	if (i)
		{
		p->heading[i] = -90 - atan2(p->point[i].z - p->point[i-1].z,
				p->point[i].x - p->point[i-1].x) * 180.0f/M_PI;
		if (p->heading[i] - p->heading[i-1] > 180)
			p->heading[i] -= 360;
		if (p->heading[i] - p->heading[i-1] < -180)
			p->heading[i] += 360;
		}
	else
		p->heading[i] = 0;
	}
 fclose(fp);
 return p;
}

float path_startTime=-1;

void restart_path(void)
{
 path_startTime = pfGetTime();
}

int Navigate_path(path_t *path)
{
 static float caveOrient=0;
 float t,orig[3]={0,6,0},worldPos[3],worldVec[3],vec[3],pt,alpha,px,py,pz,
	angle;
 int ipt;
 if (path_startTime < 0)
	path_startTime = pfGetTime();
 t = pfGetTime() - path_startTime;
 pt = (path->npoints * t) / path->time;
 ipt = pt;
 alpha = pt - ipt;
 if (ipt >= path->npoints-1)
	return 0;
 CAVENavConvertCAVEToWorld(orig,worldPos);
 px = path->point[ipt].x + (path->point[ipt+1].x - path->point[ipt].x) * alpha;
 py = path->point[ipt].y + (path->point[ipt+1].y - path->point[ipt].y) * alpha;
 pz = path->point[ipt].z + (path->point[ipt+1].z - path->point[ipt].z) * alpha;
 worldVec[0] = px - worldPos[0];
 worldVec[1] = py - worldPos[1];
 worldVec[2] = pz - worldPos[2];
 CAVENavConvertVectorWorldToCAVE(worldVec,vec);
 CAVENavTranslate(vec[0],vec[1],vec[2]);
 angle = path->heading[ipt] + (path->heading[ipt+1] - path->heading[ipt]) * alpha;
 CAVENavRot(angle - caveOrient,'z');
 caveOrient = angle;
 return 1;
}

void compute_nav_height(pfScene *floor)
{
 pfSegSet segset;
 float head[3],worldHead[3];
 pfHit **hits[32];
 int isect;
 segset.activeMask = 1;
 segset.isectMask = 0xFFFF;
 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;
 pfCAVEGetPosition(CAVE_HEAD,head);
 CAVENavConvertCAVEToWorld(head,worldHead);
 segset.segs[0].pos.set(worldHead[0],worldHead[1],worldHead[2]);
 isect = floor->isect(&segset, hits);
 if (isect)
	{
	pfVec3 pnt, xpnt;
	pfMatrix xmat;
	float foot[3],worldFoot[3];
	foot[0] = head[0];
	foot[1] = head[1];
	foot[2] = 0;
	CAVENavConvertCAVEToWorld(foot,worldFoot);
	(*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]-worldFoot[2]);
	}
}

pfScene * get_floor(char *objFile)
{
 pfScene *scene=NULL;
 pfNode *obj;
 if (obj = pfdLoadFile(objFile))
	{
	scene = new pfScene;
	scene->addChild(obj);
	}
 return scene;
}


void do_navigation(void)
{
 float jx=CAVE_JOYSTICK_X,jy=CAVE_JOYSTICK_Y;
 if (fabs(jx) > 0.125) CAVENavRot(-2.5f*jx,'z');
 if (fabs(jy) > 0.125)
	{
	float w[3];
	pfCAVEGetVector(CAVE_WAND_FRONT,w);
	CAVENavTranslate(w[0]*jy/2.5,w[1]*jy/2.5,w[2]*jy/2.5);
	}
}

pfNode * load_objects(char *filename)
{
 FILE *fp;
 char line[512];
 pfGroup *parent = new pfGroup;
/* pfPartition *parent = new pfPartition; */
 if ((fp = fopen(filename,"r")) == NULL)
	{
	perror(filename);
	return NULL;
	}
 while (fgets(line,512,fp))
	{
	pfNode *obj = loadObject(line);
	if (obj)
		parent->addChild(obj);
	}
 fclose(fp);
 parent->flatten(0);
/* parent = pfdCleanTree(parent,NULL); */
/* parent->build(PFPART_DEFAULT); */
 return (pfNode *)parent;
}

pfNode *getObject(char *filename)
{
#define CACHEMAX 1024
 static int cacheSize=0;
 static struct { char *file; pfNode *obj; } cache[CACHEMAX];
 int i;
 pfNode *obj;
 for (i=0; i<cacheSize; i++)
	{
	if (!strcmp(filename,cache[i].file))
		return cache[i].obj;
	}
 obj = pfdLoadFile(filename);
 if (cacheSize < CACHEMAX)
	{
	cache[cacheSize].file = strdup(filename);
	cache[cacheSize].obj = obj;
	cacheSize++;
	}
 return obj;
}

pfNode * loadObject(char *line)
{
 pfNode *node;
 char *fname,*s,*opt,*eq,*val;
 float tx,ty,tz,rx,ry,rz,sc;
 enum _opt_type { OPT_POS, OPT_ORIENT, OPT_SCALE, OPT_BLEND } opttype;
 int bl;
 struct _keyword opt_table[] = {
		{ "position", OPT_POS },
		{ "pos", OPT_POS },
		{ "translation", OPT_POS },
		{ "trans", OPT_POS },
		{ "orientation", OPT_ORIENT },
		{ "orient", OPT_ORIENT },
		{ "angle", OPT_ORIENT },
		{ "rotation", OPT_ORIENT },
		{ "rot", OPT_ORIENT },
		{ "scale", OPT_SCALE },
		{ "size", OPT_SCALE },
		{ "blend", OPT_BLEND },
		{ NULL, -1 }
		};
 fname = nextToken(&line);
 if ((!fname) || (!*fname) || (*fname == '#'))
	return NULL;
 if (!(node = getObject(fname)))
	return NULL;
 tx = ty = tz = 0;
 rx = ry = rz = 0;
 sc = 1;
 while (s = nextToken(&line))
	{
	opt = strdup(s);
	eq = strchr(opt,'=');
	if (!eq) continue;
	*eq = '\0';
	val = eq+1;
	opttype = (enum _opt_type) getStringID(opt,opt_table);
	switch (opttype)
		{
		case OPT_POS: sscanf(val,"%f,%f,%f",&tx,&ty,&tz); break;
		case OPT_ORIENT: sscanf(val,"%f,%f,%f",&rx,&ry,&rz); break;
		case OPT_SCALE: sc = atof(val); break;
		case OPT_BLEND: bl = atoi(val); break;
		default: fprintf(stderr,"Unsupported object option: \"%s\"\n",s);
		}
	free(opt);
	}
 {
  pfSCS *scs;
  pfMatrix scsMat;
  scsMat.makeTrans(tx,ty,tz);
  scsMat.preRot(rz,0.0,0.0,1.0,scsMat);
  scsMat.preRot(rx,1.0,0.0,0.0,scsMat);
  scsMat.preRot(ry,0.0,1.0,0.0,scsMat);
  scsMat.preScale(sc,sc,sc,scsMat);
  scs = new pfSCS(scsMat);
  scs->addChild(node);
  return scs;
 }
}

static int getStringID(char *str,struct _keyword *table)
{
 int n;
 if (str)
    {
    for (n=0; table[n].str; n++)
	if (!strcasecmp(str,table[n].str)) return table[n].id;
    }
 return -1;
}

static char * nextToken(char **str)
{
 char *end,*token;
 while ((**str) && ((**str==' ') || (**str=='\t') || (**str=='\n')))
        (*str)++;
 if (!(**str))
        return NULL;
 end = strpbrk(*str," \t\n\0");
 token = *str;
 if (*end)
        *str = end+1;
 *end = '\0';
 return token;
}



void create_scene(pfScene **scene,pfDCS **nav_dcs,pfSwitch **sw)
{
 pfLightSource *light;
 *scene = new pfScene;
 *nav_dcs = new pfDCS;
 (*scene)->addChild(*nav_dcs);
 pfGeoState *gstate = new pfGeoState;
 gstate->setMode(PFSTATE_ENLIGHTING, PF_ON);
 (*scene)->setGState(gstate);
 light = new pfLightSource;
 light->setPos(1.0f,1.0f,1.0f,0.0f);
 (*scene)->addChild(light);
 light = new pfLightSource;
 light->setPos(-1.0f,-0.2f,-0.2f,0.0f);
 (*scene)->addChild(light);
 *sw = new pfSwitch;
 (*nav_dcs)->addChild(*sw);
}