/* ply2tri: parse text ply file with rigid format, and produce triangle
mesh with proper normals.  (C) 2005 Sean Brennan. */
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <GL/gl.h>

#define DEBUG 0

#include "ply2tri.h"

void plyerr(char *msg) {
	fprintf(stderr, "Error: %s\n Exiting...\n", msg);
	exit(1);
}

/* 
Sample ply text file:
---------------------
ply
format ascii 1.0
comment zipper output
element vertex 453
property float x
property float y
property float z
property float confidence
property float intensity
element face 948
property list uchar int vertex_indices
end_header
-0.0312216 0.126304 0.00514924 0.850855 0.5
-0.0446774 0.131204 0.00570479 0.900159 0.5
etc....
*/

/* return words in string */
char **getwords(char *buf, int *numwords) {
	int p=0, w=0, lenstr, word_ptr, wordcount;
	char **retstr, c, *tmpstr;

	word_ptr = 0;
	wordcount = 0;
	lenstr = strlen(buf);
	for (p = 0 ; p < lenstr; p++)
		if (isspace(buf[p]))
			wordcount++;
	retstr = (char **) malloc(sizeof(char *) * wordcount);
	p = 0; w = 0;
	for ( p = 0 ; p < lenstr; p++) {
		c = buf[p];
		if (!isgraph(c) || isspace(c)) {
			if ( w) { /* no 0 length words please */
				tmpstr = (char *) malloc(sizeof(char) * (w + 1));
				strncpy(tmpstr, buf + p - w, w);
				tmpstr[w] = (char) NULL;
				retstr[word_ptr] = tmpstr;
				w = 0; /*reset word letter counter */
				word_ptr++;
			}
		} else { w++; }
	}	
	*numwords = word_ptr;
	return(retstr);
}

/* return 1 on end_header */

int processPlyLine(plyInfo *pinfo, char *inbuf) {
	char **words;
	int wcount, i;

	words = getwords(inbuf, &wcount);

	i = 0;	
	if ( ! strcmp(words[0], "format")) {
		i = 10;
		if (wcount < 3) plyerr("format line");
		if (! strcmp(words[1], "ascii")) {
			i = 11;
			/* cool, ascii */
			pinfo->format = (char *) 
				malloc( sizeof(char) * strlen(words[1]) );
			strcpy(pinfo->format, words[1]);
			pinfo->version = (char *) 
				malloc( sizeof(char) * strlen(words[2]) );
			strcpy(pinfo->version, words[2]);
		} else if (! strcmp(words[1], "binary_big_endian")) {
			i = 12;
			/* big butt binary */
			pinfo->format = (char *) 
				malloc( sizeof(char) * strlen(words[1]) );
			strcpy(pinfo->format, words[1]);
			pinfo->version = (char *) 
				malloc( sizeof(char) * strlen(words[2]) );
			strcpy(pinfo->version, words[2]);
			
		} else plyerr("Format not ascii or binary_big_endian");
		
	};
	if ( ! strcmp(words[0], "comment")) {
		i = 20;
	};
	if ( ! strcmp(words[0], "obj_info")) {
		i = 22;
	};
	if ( ! strcmp(words[0], "element")) {
		if (wcount != 3) {
			for (i = 0; i < wcount ; i++)
				fprintf(stderr, "Word %d: <%s>\n", i, words[i]);	
			plyerr("element line");
		}
		if ( ! strcmp(words[1], "vertex")) {
			i = 21;
			pinfo->numVerts = atoi(words[2]);
		}
		if ( ! strcmp(words[1], "face")) {
			i = 22;
			pinfo->numFaces = atoi(words[2]);
		}
	}
	if ( ! strcmp(words[0], "property")) {
		i = 30;
	};
	if ( ! strcmp(words[0], "end_header")) {
		i = 1;
	};

	return (i);

}

int processVertexline(vertlist *v, char *inbuf) {
	char **words;
	int wcount;
	double x, y, z;

	words = getwords(inbuf, &wcount);
	x = atof(words[0]);
	y = atof(words[1]);
	z = atof(words[2]);
	v->x = x;
	v->y = y;
	v->z = z;
}

int processFaceline(facelist *f, char *inbuf) {
	char **words;
	int wcount, numv, i;
	double x, y, z;

	words = getwords(inbuf, &wcount);

	numv = atof(words[0]);
	f->numverts = numv;
	f->v = (int *) malloc(sizeof(int) * numv);
	if (numv > wcount) plyerr("Not enough vert idx on line");
	for ( i = 0 ; i < numv; i++) 
		f->v[i] =  atoi(words[i + 1]);
		
}
	
void printPly(plyInfo *pinfo) {
	char *version;
	int numVerts;
	int numFaces;
	int i, ii, nv, minv, maxv, tmpv;
	double minx, miny, minz, maxx, maxy, maxz, tx, ty, tz;

	vertlist *vl;
	facelist *fl;

	minx = miny = minz = 1000000.0;
	maxx = maxy = maxz = -1000000.0;

	numVerts =  pinfo->numVerts;
	numFaces =  pinfo->numFaces;
	printf("PINFO PRINT\n");
	printf("NUM VERTS: %d\n", numVerts);
	printf("NUM FACES: %d\n", numFaces);
	printf("Verts:\n");
	for ( i = 0 ; i < numVerts; i++) {
		tx = pinfo->vl[i].x;
	 	ty = pinfo->vl[i].y; 
		tz = pinfo->vl[i].z;
		printf("[<%8.4d> %5.2f %5.2f %5.2f] ", i, tx, ty, tz);
		if ( tx > maxx) maxx = tx;
		if ( ty > maxy) maxy = ty;
		if ( tz > maxz) maxz = tz;
		if ( tx < minx) minx = tx;
		if ( ty < miny) miny = ty;
		if ( tz < minz) minz = tz;
		
		if ( !( (i + 1) % 5)) printf("\n");
	}
	printf("\n");
	printf("Faces:\n");
	maxv = -100;
	minv = 1000000000;
	for ( i = 0 ; i < numFaces; i++) {
		nv = pinfo->fl[i].numverts;	
		printf("[<%8.4d>(%d)", i, nv);
		for(ii = 0 ; ii < nv ; ii++) {
			tmpv = pinfo->fl[i].v[ii];
			printf(" %5d", tmpv);
			if ( tmpv > maxv ) maxv = tmpv;
			if ( tmpv < minv ) minv = tmpv;
		}
		printf("] ");
		if ( !( (i + 1) % 5)) printf("\n");
	}
	printf("\n");
	printf("SMALLEST VERT INDEX: %d / LARGEST INDEX: %d / TOTAL INDEX: %d\n",
		minv, maxv, numVerts);
	printf("BOUNDING BOX[ %3.2f %3.2f %3.2f / %3.2f %3.2f %3.2f ]\n",
		minx, miny, minz, maxx, maxy, maxz);
		
	printf("END OF PINFO\n");
}

	

int readPly(plyInfo *pinfo, FILE *infile) {
	int lineno, linetype, i, loop, tmpi;
	char inbuf[90], errbuf[120];
	unsigned char tmpc;
	float tmpf;

	lineno=0;

	if (!	( fgets(inbuf, 90, infile)))
	 	plyerr("header1");
	/* translate newline to null*/
	inbuf[strlen(inbuf) -1] = (char) NULL;
	if (strcmp(inbuf, "ply"))
		plyerr("not ply file");
	
	/* read header*/
	lineno = 0;
	while (fgets(inbuf, 90, infile)) {
		linetype = processPlyLine(pinfo, inbuf);
		if (linetype == 1)   break; /* end_header */
		if (linetype == 0) {
			sprintf(errbuf, "Could non parse: %s\n", inbuf);
			plyerr(errbuf);
		}
		lineno++;
	}
	if (linetype == 1) { /* end-header */
	}
	/* read data */
	pinfo->vl = (vertlist *) malloc(  sizeof(vertlist) * pinfo->numVerts);
	pinfo->fl = (facelist *) malloc(  sizeof(facelist) * pinfo->numFaces);
	pinfo->vertnorms = (vertlist *) malloc(  sizeof(vertlist) * pinfo->numVerts);
	pinfo->facenorms = (vertlist *) malloc(  sizeof(vertlist) * pinfo->numFaces);
	if ( !strcmp(pinfo->format, "ascii")) {
		for ( i = 0 ; i < pinfo->numVerts ; i++) {
			if ( !fgets(inbuf, 90, infile)) plyerr("Vertex data underrun");
			processVertexline(&pinfo->vl[i], inbuf) ;
			if (DEBUG) printf("V verify: %3.2f %s\n", pinfo->vl[i].x, inbuf);
		}
		for ( i = 0 ; i < pinfo->numFaces ; i++) {
			if ( !fgets(inbuf, 90, infile)) plyerr("Face data underrun");
			processFaceline(&pinfo->fl[i], inbuf) ;
			if (DEBUG) printf("F verify: [%d] %d %d %d %s\n", pinfo->fl[i].numverts, pinfo->fl[i].v[0], pinfo->fl[i].v[1],pinfo->fl[i].v[2],inbuf);
		}
	} else if (! strcmp(pinfo->format, "binary_big_endian")) {
		for ( i = 0 ; i < pinfo->numVerts ; i++) {
			vertlist *v;
		
			v = &pinfo->vl[i];
			if ( !fread(&tmpf, sizeof(float), 1,   infile))
				plyerr("Vertex data underrun");
			v->x = (double) tmpf;
			if ( !fread(&tmpf, sizeof(float), 1,   infile))
				plyerr("Vertex data underrun");
			v->y = (double) tmpf;
			if ( !fread(&tmpf, sizeof(float), 1,   infile))
				plyerr("Vertex data underrun");
			v->z = (double) tmpf;
			if ( !fread(&tmpf, sizeof(float), 1,   infile))
				plyerr("Vertex data underrun");
			/* remove/eat confidence number */
			if ( (i % 1000) == 0) 
				printf("vert %d of %d : %f %f %f\n", 
					i, pinfo->numVerts, v->x, v->y, v->z);
		}
		for ( i = 0 ; i < pinfo->numFaces ; i++) {
			facelist *f;

			f = &pinfo->fl[i];

			if ( !fread(&tmpc, sizeof(unsigned char), 1, infile))
				plyerr("face data underrun");
			printf("Face %d:This many sides: %d\n", i, tmpc);
			if (tmpc != 3) {
				printf("Face %d:This many sides: %d\n", i, tmpc);
				 plyerr("Face not of 3 sides");
			}
			if ( !fread(&tmpi, sizeof(int), 1, infile))
				plyerr("face data underrun");

			f->numverts = tmpc;
			f->v = (int *) malloc(sizeof(int) * f->numverts);
			for ( loop = 0 ; loop < f->numverts; loop++)  {
				if ( !fread(&tmpi, sizeof(int), 1, infile))
					plyerr("face data underrun");
				f->v[i] =  tmpi;
				printf("Face has vertex %d\n");
			}
		}
	}
	if (DEBUG) printPly(pinfo);
}

int getPly(FILE *inf, plyInfo *pinfo)
{
	pinfo->numVerts = 0;
	pinfo->numFaces = 0;
	pinfo->vl = NULL;
	pinfo->fl = NULL;

	readPly(pinfo, inf);
}

void vdiff(vertlist *v1, vertlist *v2, vertlist *res)
{
	res->x = v1->x - v2->x;
	res->y = v1->y - v2->y;
	res->z = v1->z - v2->z;
}
	
void vcross(vertlist *v1, vertlist *v2, vertlist *res)
{
        res->x = (v1->y * v2->z - v1->z * v2->y);
        res->y = (v1->z * v2->x - v1->x * v2->z);
        res->z = (v1->x * v2->y - v1->y * v2->x);
	/* printf("x: c %f -- %f * %f - %f * %f\n", 
		res->x, v1->y, v2->z, v1->z, v2->y); */
}

void vunit(vertlist *v)
{
	double len, olen;

	len =   sqrt(  v->x * v->x + v->y * v->y + v->z * v->z);
	olen = (double) 1.0 / len;
	if (! finite(olen)) {
		fprintf(stderr, "Short length for unit: %g from %g, %g, %g\nn", len, v->x, v->y, v->z);
		v->x = 0.0;
		v->y = 0.0;
		v->z = 1.0;
	} else {
		v->x = v->x * olen;
		v->y = v->y * olen;
		v->z = v->z * olen;
	}
}

void makenorm(vertlist *v1, vertlist *v2, vertlist *v3, vertlist *norm)
{
	vertlist tmpv1, tmpv2;

	vdiff(v3, v2, &tmpv1);
	vdiff(v1, v2, &tmpv2);
	vcross(&tmpv1, &tmpv2, norm);
}



void crybaby(plyInfo *pinfo, int idx)
{
	int i, numf;
	vertlist *v, *n;

	v = &pinfo->vl[idx];
	numf = v->num_faces;
	fprintf(stderr, "NAN num faces: %d\n", numf);
	for ( i = 0 ; i < numf ; i++) {
		n = &pinfo->facenorms[v->face_idx[i]];
		fprintf(stderr, "NAN FACE %d: %d.  Norm: %3.2f %3.2f %3.2f\n",
			i, v->face_idx[i], n->x, n->y, n->z);
		fprintf(stderr, "NAN FACE VERTS: %d, %d, %d\n", 
			pinfo->fl[v->face_idx[i]].v[0],
			pinfo->fl[v->face_idx[i]].v[1],
			pinfo->fl[v->face_idx[i]].v[2]);
	}
}

	

int printPly2tri(plyInfo *pinfo)
{
	int idf, idv;
	vertlist *v, *vn, norm;
	
	printf("#include \"bunny.h\"\n");
	printf("void bunnytris() {\n");
	printf("glBegin(GL_TRIANGLES);\n");
	for ( idf = 0 ; idf < pinfo->numFaces ; idf++) {
		/* norm.x = pinfo->facenorms[idf].x;
		norm.y = pinfo->facenorms[idf].y;
		norm.z = pinfo->facenorms[idf].z; */
		for ( idv = 0 ; idv < pinfo->fl[idf].numverts; idv++) {
			v = &pinfo->vl[pinfo->fl[idf].v[idv]];
			vn = &pinfo->vertnorms[pinfo->fl[idf].v[idv]];
			printf("glNormal3f(%3.7f, %3.7f, %3.7f);\n",
				vn->x, vn->y, vn->z);
			if (isnan(vn->x) || isnan(vn->y) || isnan(vn->z)) {
				printf("Nan IN!\n");
				printf("Nan Vert: %d\n", pinfo->fl[idf].v[idv]);
				crybaby(pinfo, pinfo->fl[idf].v[idv]);
				printf("Nan OUT!\n");
			}
				/* norm.x, norm.y, norm.z); */
			printf("glVertex3f(%3.7f, %3.7f, %3.7f);\n",
				v->x, v->y, v->z);
		}
			
	}
	printf("glEnd();\n");
	printf("}\n");
		
}

int callPly2tri(plyInfo *pinfo)
{
	int idf, idv, percnt;
	vertlist *v, *vn, norm;

	percnt = pinfo->numFaces / 100;
	
	glBegin(GL_TRIANGLES);

	for ( idf = 0 ; idf < pinfo->numFaces ; idf++) {
		/* norm.x = pinfo->facenorms[idf].x;
		norm.y = pinfo->facenorms[idf].y;
		norm.z = pinfo->facenorms[idf].z; */
		for ( idv = 0 ; idv < pinfo->fl[idf].numverts; idv++) {
			v = &pinfo->vl[pinfo->fl[idf].v[idv]];
			vn = &pinfo->vertnorms[pinfo->fl[idf].v[idv]];
			glNormal3f( vn->x, vn->y, vn->z);
			if (isnan(vn->x) || isnan(vn->y) || isnan(vn->z)) {
				printf("Nan IN!\n");
				printf("Nan Vert: %d\n", pinfo->fl[idf].v[idv]);
				crybaby(pinfo, pinfo->fl[idf].v[idv]);
				printf("Nan OUT!\n");
			}
				/* norm.x, norm.y, norm.z); */
			glVertex3f( v->x, v->y, v->z);
		}
		if (idf % percnt == 0) printf("%d %% done\n", idf / percnt);
			
	}
	glEnd();
}


int makeFaceNorms(plyInfo *pinfo)
{
	int idf;
	vertlist *v, norm;
	
	for ( idf = 0 ; idf < pinfo->numFaces ; idf++) {
		makenorm( &pinfo->vl[pinfo->fl[idf].v[0]],
		 	  &pinfo->vl[pinfo->fl[idf].v[1]],
		 	  &pinfo->vl[pinfo->fl[idf].v[2]],
		 	  &norm);
		 v = &pinfo->facenorms[idf];
		 v->x =	norm.x;
		 v->y = norm.y;
		 v->z = norm.z;
		}
}

int makeVertNorms(plyInfo *pinfo)
{
	int idv, idf, idebug, *tmpil, *t2;
	vertlist *v, *fn, norm;
	facelist *f;

	/* tag each vertex with list of triangles it belongs to */	
	for ( idf = 0 ; idf < pinfo->numFaces ; idf++) {
		f = &pinfo->fl[idf];	
		for (idv = 0; idv < f->numverts ; idv++) {
			v = &pinfo->vl[f->v[idv]];
			if ( v->num_faces == 0) {
				v->face_idx = (int *) malloc(sizeof(int) * 10);
			}
			v->face_idx[v->num_faces] = idf;
			v->num_faces++;
			if ( v->num_faces % 10 == 0) {
				tmpil = (int *) malloc(sizeof(int) * (v->num_faces + 10));
				memcpy(tmpil, v->face_idx, sizeof(int) * v->num_faces);
				t2 = v->face_idx;
				v->face_idx = tmpil;
				free(t2);
			}
			if (DEBUG)
			if (v->num_faces > 12) { 
				printf("Vert %d on %d faces\n",
				f->v[idv], v->num_faces);
				printf("They are:");
				for (idebug = 0 ; idebug < v->num_faces; idebug++)
					printf(" %d", v->face_idx[idebug]);
				printf("\n");
			}
		}
	}
	/* average each triangle's normal to make vertex normal */
	for (idv = 0; idv < pinfo->numVerts ; idv++) {
		v = &pinfo->vl[idv];
		norm.x = 0.0;	
		norm.y = 0.0;	
		norm.z = 0.0;	
		for (idf = 0 ; idf < v->num_faces ; idf++) {
			fn = &pinfo->facenorms[v->face_idx[idf]];
			norm.x += fn->x;
			norm.y += fn->y;
			norm.z += fn->z;
		}
		if (isnan(norm.x) || isnan(norm.x) || isnan(norm.x)) {
			printf("Nan!\n");
		}
		vunit(&norm);
		pinfo->vertnorms[idv].x = norm.x;
		pinfo->vertnorms[idv].y = norm.y;
		pinfo->vertnorms[idv].z = norm.z;
	}
}
	


int mainn(int argc, char **argv)
{
	FILE *inf;
	plyInfo pinfo;

	if (( inf = fopen(argv[1], "r")) == NULL)
		plyerr("Could not open file for reading");

	getPly(inf, &pinfo);
	makeFaceNorms(&pinfo);
	makeVertNorms(&pinfo);
	 printPly2tri(&pinfo); 
}

int genPlyTris(char *filename)
{
	FILE *inf;
	plyInfo pinfo;

	if (( inf = fopen(filename, "r")) == NULL)
		plyerr("Could not open file for reading");

	getPly(inf, &pinfo);
	makeFaceNorms(&pinfo);
	makeVertNorms(&pinfo);
	callPly2tri(&pinfo); 
}