#include <stdio.h>
#include <stdlib.h>
#include <assert.h>

#include "tools.h"

/* return a pointer into the grid at (approx) x,y */
GLubyte *
grid_rgb_value(GLubyte *gridData, int gridWidth, int gridHeight, double xd, double yd) {
  GLubyte *ptr;
  int x, y;
  static GLubyte zeroptr[4] = {0,0,0,0};

  x = (int)xd;
  y = (int)yd;

  if(x < 0 || x >= gridWidth ||
	 y < 0 || y >= gridHeight) {
	ptr = zeroptr;
  } else {
	ptr = &gridData[(4 * (gridWidth * gridHeight - y * gridWidth + x) )];
  }

  return ptr;
}

/* return a single value representing the grid at x,y, scaled 0...1 */
GLfloat
grid_value(GLubyte *data, int width, int height, double x, double y) {
  GLubyte *ptr;
  double v = 0;

  ptr = grid_rgb_value(data, width, height, x, y);
  v = ptr[0] + ptr[1] + ptr[2];
  v /= (255 * 3.0);
  
  return v;
}


/*
 * create a grid (nx x ny) from a rgba matrix (width x height)
 */
grid_t *
sample_grid(GLubyte *data, int width, int height, int nx, int ny) {
  double gx, gy;				/* index into grid */
  double gdx, gdy;
  int x, y;
  GLfloat *gp;
  grid_t *grid;

  grid = (grid_t *)(malloc(sizeof(grid_t) + sizeof(GLfloat) * nx * ny));
  if(!grid) {
	return NULL;
  }
  grid->w = nx;
  grid->h = ny;

  gdx = (double)width / nx;
  gdy = (double)height / ny;
  gp = grid->data;
  for(y=0, gy=0; y < ny; y++, gy+=gdy) {
	for(x=0, gx=0; x < nx; x++, gx+=gdx) {
	  /* should be average XXX */
	  *gp++ = grid_value(data, width, height, gx, gy);
	}
  }

  return grid;
}

void
grid_bounds_check(grid_t *grid, int x, int y) {
  assert(grid);
  assert(x >= 0);
  assert(y >= 0);
  assert(x < grid->w);
  assert(y < grid->h);
}

/* ********************************************************************** */

#include "matrix.h"

#define NEQUAL(a,b)								\
if(EQUAL(a,b)) {								\
  printf("equal vectors (%s=%s): ", #a, #b);	\
  PRINT(a); printf(" "); PRINT(b);				\
  printf("\n");									\
  return NULL; \
}


/* compute normal of triangle abc (not reentrant) */
GLfloat *
normal(GLfloat *a, GLfloat *b, GLfloat *c) {
  static GLfloat n[4];
  GLfloat t1[4], t2[4];

  NEQUAL(a,b);
  NEQUAL(b,c);
  NEQUAL(a,c);

  (void)SUB2(t1,b,a);			/* AB */
  assert(LENGTH(t1));
  (void)SUB2(t2,c,b);			/* BC */
  assert(LENGTH(t2));
  CROSS2(n,t1,t2);				/* AB x BC */
  NEQUAL(t1,t2);
  if(!LENGTH(n)) {
	printf("zero length: "); PRINT(n); printf("\n");
	PRINT(a); PRINT(b); PRINT(c);
	PRINT(t1); PRINT(t2);
	printf("\n");
	assert(0);
  }
  NORMALIZE(n);
  n[3] = 1;

/*   printf("normal: "); */
/*   PRINT(a); PRINT(b); PRINT(c); PRINT(n);  */
/*   printf("\n"); */

  return n;
}

/* ********************************************************************** */

/* we use an array of these to keep track of the vertices and faces -
 * vertex(x,y) and two faces in cell w/ smallest corner at (x,y) */
struct grid_face_t {
  vertex_t *vertex;
  face_t *faces[2];
};


/* make a face out the vertices at p1..3 (offset from gft) */
face_t *
make_face(struct grid_face_t *gft, int p1, int p2, int p3) {
  face_t *face;
  int k;

  face = (face_t *)malloc(sizeof(face_t));
  assert(face);
  face->vertices[0] = (gft + p1)->vertex;
  face->vertices[1] = (gft + p2)->vertex;
  face->vertices[2] = (gft + p3)->vertex;
  for(k=0; k<3; k++) {
	TAILQ_INSERT_TAIL(&face->vertices[k]->vfl, face, vfl_link);
  }
  return face;
}


/* make a triangulation out of a grid (two triangles per cell) */
triangulation_t *
grid2triangulation(grid_t *grid) {
  triangulation_t *tr;
  int i, j;
  vertex_t *vertex;
  face_t *face;
  struct grid_face_t *lkup, *gft;

  printf("triangulating grid...\n");

  tr = (triangulation_t *)malloc(sizeof(triangulation_t));
  assert(tr);
  TAILQ_INIT(&tr->flist);
  TAILQ_INIT(&tr->vlist);

  lkup = (struct grid_face_t *)malloc(sizeof(struct grid_face_t) *
									  grid->w * grid->h);
  assert(lkup);
  
  /* make vertices */
  for(j=0; j<grid->h; j++) {
	for(i=0; i<grid->w; i++) {
	  vertex = (vertex_t *)malloc(sizeof(vertex_t));
	  assert(vertex);
	  vertex->pt[0] = i;
	  vertex->pt[1] = j;
	  vertex->pt[2] = GRID_VALUE(grid, i, j);
	  TAILQ_INIT(&vertex->vfl);
	  TAILQ_INSERT_TAIL(&tr->vlist, vertex, vlist_link);

	  gft = &lkup[j*grid->w + i];
	  gft->vertex = vertex;
	}
  }

  /* make faces */
  for(j=0; j < grid->h - 1; j++) {
	for(i=0; i < grid->w - 1; i++) {
	  gft = &lkup[j*grid->w + i];

	  face = make_face(gft, 0, 1, grid->w + 1);
	  gft->faces[0] = face;
	  TAILQ_INSERT_TAIL(&tr->flist, face, flist_link);

	  face = make_face(gft, grid->w + 1, grid->w, 0);
	  gft->faces[1] = face;
	  TAILQ_INSERT_TAIL(&tr->flist, face, flist_link);
  
	}
  }

  /* do normals */
  TAILQ_FOREACH(face, &tr->flist, flist_link) {
	GLfloat *n;
	n = normal(face->vertices[0]->pt,
			   face->vertices[1]->pt,
			   face->vertices[2]->pt);
	COPY(face->normal, n);
  }
  TAILQ_FOREACH(vertex, &tr->vlist, vlist_link) {
	GLfloat n[3] = {0,0,0};
	TAILQ_FOREACH(face, &vertex->vfl, vfl_link) {
	  (void)ADD2(n, face->normal, n);
	}
	NORMALIZE(n);
	COPY(vertex->normal, n);
	//PRINT(n); printf("\n");
  }

  free(lkup);
  printf("done.\n");
  return tr;
}


/* cleanup */
void
trFree(triangulation_t *tr) {
  vertex_t *vertex;
  face_t *face;

  TAILQ_FOREACH(face, &tr->flist, flist_link) {
	free(face);
  }
  TAILQ_FOREACH(vertex, &tr->vlist, vlist_link) {
	free(vertex);
  }
  free(tr);
}