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

#include "camera.h"

#ifdef DMALLOC
#include<dmalloc.h>
#endif

int keypointCount,duration,pathToTimeLeft;
float *keypoint;
extern float from[3];
extern float to[3];

void initCamera()
{
	keypoint=0;
	from[0]=0;
	from[1]=0;
	from[2]=0;
	to[0]=0;
	to[1]=0;
	to[2]=0;
	keypointCount=0;
	duration=0;
	pathToTimeLeft=0;
}

void freeCamera()
{
	if(keypoint) free(keypoint);
	keypoint=0;
}

static void orientxyz(float *x,float *y,float *z)
{
	float t1,t2,t3;
	t1=(*z);
	t2=(*x);
	t3=-(*y);
	*y=t1;
	*z=t3;
	*x=t2;
}

void loadCameraPath(const char *filename)
{
	int orient=1;

	if(keypoint) free(keypoint);
	keypoint=0;
	from[0]=0;
	from[1]=0;
	from[2]=0;
	to[0]=0;
	to[1]=0;
	to[2]=0;
	keypointCount=0;
	duration=0;
	pathToTimeLeft=0;
	FILE *file=fopen(filename,"r");
	if(!file) {
		printf("Couldn't read '%s'\n",filename);
		return;
	}
	printf("Camera object loading from '%s'\n",filename);
	char line[257];
	float origin[3]={0,0,0};
	enum AseMode { ASE_START, ASE_CAMERAFROM, ASE_CAMERATO, ASE_LINE } mode=ASE_START;
	char name[256];
	int framespeed=1;
	int frames=0;
	while(fgets(line,255,file)) {
		if(strstr(line,"*NODE_NAME")) {
			char *s=strstr(line, "\"");
			if(s) {
				s++;
				char *e=strstr(s,"\"");
				if(e) e[0]=0;
				strcpy(name,s);
				if(strcmp(name,"Camera01")==0) {
					//printf("Camera from\n");
					mode=ASE_CAMERAFROM;
				} else if(strcmp(name,"Camera01.Target")==0) {
					//printf("Camera to\n");
					mode=ASE_CAMERATO;
				} else if(strcmp(name,"Line01")==0) {
					//printf("Line\n");
					mode=ASE_LINE;
				} else {
					//printf("Skipping Camera '%s'\n",name.c_str());
					mode=ASE_START;
				}
			}
		} else if(strstr(line,"*TM_POS")) {
			char *s=strstr(line,"*TM_POS ");
			if(s) {
				int res=0;
				s+=8;
				//printf("pos says '%s'\n",s);
				if(mode==ASE_CAMERAFROM) {
					res=sscanf(s,"%f%f%f",&from[0],&from[1],&from[2]);
					if(orient) {
						// rotatexyz(-m_pi/2,m_pi);
						orientxyz(&from[0],&from[1],&from[2]);
					}
					//printf("from[%d]: %.2f,%.2f,%.2f\n",res,from[0],from[1],from[2]);
				} else if(mode==ASE_CAMERATO) {
					res=sscanf(s,"%f%f%f",&to[0],&to[1],&to[2]);
					if(orient) {
						// rotatexyz(-m_pi/2,m_pi);
						orientxyz(&to[0],&to[1],&to[2]);
					}
					//printf("to[%d]: %.2f,%.2f,%.2f\n",res,to[0],to[1],to[2]);
				} else if(mode==ASE_LINE) {
					res=sscanf(s,"%f%f%f",&origin[0],&origin[1],&origin[2]);
					printf("line[%d]: %.2f,%.2f,%.2f\n",res,origin[0],origin[1],origin[2]);
				}
			}
		} else if(strstr(line,"*SCENE_LASTFRAME")) {
			char *s=strstr(line,"*SCENE_LASTFRAME");
			if(s) {
				// skip whitespace and command name.
				while ((*s>'9' || *s<'0') && *s!='.' && *s!=0) s++;
				sscanf(s,"%d",&frames);
				//printf("last frame: %d\n",frames);
			}
		} else if(strstr(line,"*SCENE_FRAMESPEED")) {
			char *s=strstr(line,"*SCENE_FRAMESPEED");
			if(s) {
				// skip whitespace and command name.
				while ((*s>'9' || *s<'0') && *s!='.' && *s!=0) s++;
				sscanf(s,"%d",&framespeed);
				//printf("frame speed: %d\n",framespeed);
			}
		} else if(strstr(line,"*SHAPE_VERTEXCOUNT")) {
			char *s=strstr(line,"*SHAPE_VERTEXCOUNT");
			if(s) {
				s+=19;
				int count;
				sscanf(s,"%d",&count);
				keypointCount=count;
				keypoint=(float *)malloc(sizeof(float)*count*3);
				int i;
				for(i=0;i<count*3;i++) {
					keypoint[i]=0;
				}
				//printf("keyframe count: %d\n",count);
			}
		} else if(strstr(line,"*SHAPE_VERTEX_")) {
			char *s=strstr(line,"*SHAPE_VERTEX_");
			if(s) {
				// skip whitespace and command name.
				while ((*s>'9' || *s<'0') && *s!='.' && *s!=0) s++;
				int num=0;
				float x=0,y=0,z=0;
				int res=sscanf(s,"%d%f%f%f",&num,&x,&y,&z);
				if(num<keypointCount && num>=0 && res==4) {
					printf("Got [%d]: %.2f,%.2f,%.2f\n",num,x,y,z);
					//x+=origin[0];
					//y+=origin[1];
					//z+=origin[2];
					if(orient) {
						// rotatexyz(-m_pi/2,m_pi);
						orientxyz(&x,&y,&z);
					}
					printf("Final [%d]: %.2f,%.2f,%.2f\n",num,x,y,z);
					keypoint[num*3+0]=x;
					keypoint[num*3+1]=y;
					keypoint[num*3+2]=z;
				} else {
					printf("Bad data [%d]: %.2f,%.2f,%.2f (from '%s')\n",num,x,y,z,s);
				}
			}
		}
		
	}
	fclose(file);
	duration=1000*frames/framespeed;
	pathToTimeLeft=duration;
	printf("Camera object loaded ('%s')\n",filename);
}

void cameraUpdate(unsigned long elapsed)
{
	if(elapsed<=0) return;
	if((long)elapsed>pathToTimeLeft) {
		pathToTimeLeft=0;
	} else {
		pathToTimeLeft-=elapsed;
	}
	if(keypointCount<1) return;
	if(duration<1) return;
	long segment=keypointCount*(duration-pathToTimeLeft)/duration;
	if(segment>=keypointCount-1) return;
	if(segment<0) segment=0;
	
	// Now look up the relevant points.
	float past=duration-pathToTimeLeft;
	float range=(float)duration/keypointCount;
	float seg=floor(past/range);
	float t=past/range-seg;
	float *ptstart=keypoint+segment*3;
	float *ptend=keypoint+segment*3+3;
	//float p[3];=ptstart+(ptend-ptstart)*t;

	from[0]=ptstart[0]+(ptend[0]-ptstart[0])*t;
	from[1]=ptstart[1]+(ptend[1]-ptstart[1])*t;
	from[2]=ptstart[2]+(ptend[2]-ptstart[2])*t;
	printf("From seg=%d (t=%.2f)-> %.2f,%.2f,%.2f\n",(int)segment,t,from[0],from[1],from[2]);

	// To is looking ahead:
		to[0]=10;
		to[0]=50;
		to[0]=0;
/*		
	t+=0.1f;
	to[0]=to[0]*0.9f+(ptstart[0]+(ptend[0]-ptstart[0])*t)*0.1f;
	to[1]=to[1]*0.9f+(ptstart[1]+(ptend[1]-ptstart[1])*t)*0.1f;
	to[2]=to[2]*0.9f+(ptstart[2]+(ptend[2]-ptstart[2])*t)*0.1f;
	printf("To seg=%d (t=%.2f)-> %.2f,%.2f,%.2f\n",(int)segment,t,to[0],to[1],to[2]);
*/	
}

void cameraDoPath()
{
	pathToTimeLeft=duration;
	cameraUpdate(1);
	to[0]=from[0]+1;
	to[1]=from[1]+1;
	to[2]=from[2]+1;
}

/*
 * Checks to see if the camera path is still active, or if it is at the end of the path.
 * 
 * \returns true if the camera is still active.
 */
int cameraActive()
{
	printf("pathToTimeLeft=%d\n",pathToTimeLeft);

	return (int)(pathToTimeLeft>0);
}
