#pragma ident "@(#)voxel.c 1.1 97/10/31 SMI"

/*
 * voxel.c
 *
 * Copyright (C) 1997, by Mike M. Chow (email mchow@graphics.lcs.mit.edu)
 * 
 * Implements a voxel grid. Used for volumetric analysis of surfaces.
 * 
 */

#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <math.h>
#include "types.h"
#include "linalg.h"
#include "zdebug.h"
#include "utils.h"

int CurrVertTag = 0; /* Tag a vertex in a vertlist */

int NumCollisions;

/*
 * vertListNewNode
 *
 *      Adds a new vert to vert list.
 */

VertList *
vertListNewNode ()
{
    VertList *vertList;
    
    vertList = (VertList *) malloc(sizeof(VertList));
    vertList->current = NULL;
    vertList->next = NULL;
    return vertList;
}  /* End of vertListNewNode*/




/*
 * vertListAddVert
 *
 *      Add a new vert to a vertList.
 */

void
vertListAddVert (VertList **vlist, Vertex *vert)
{
    
    VertList *newNode, *nlptr;   /* New nodeList node */
    
    nlptr = *vlist;
    /* Empty list, make nptr the first. */
    if (nlptr->current == NULL)
	{
	    nlptr->current = vert;
	    return;
	}
    /* Normal case, add Node as head */
    
    newNode = vertListNewNode();
    newNode->current = vert;
    newNode->next = nlptr;
    *vlist = newNode;
}  /* End of nodeListAddNode*/




/*
 * vertListRemoveVert
 *
 *      Removes a new vert to a vertList.
 */

void
vertListRemoveVert (VertList **vlist, Vertex *vert)
{
    
    VertList *nlptr;
    VertList *prev;

    if (*vlist == NULL)
      return;

    nlptr = *vlist;
    while (nlptr && (nlptr->current != vert)){
      zdo6 printf("vert found in list.\n");
      prev = nlptr;
      nlptr = nlptr->next;
    }

    if (nlptr != 0) {
      if (nlptr == *vlist)  /* If nlptr is head, */
	if (nlptr->next == NULL)    /* If next is null, 
				       make a new list */
	  *vlist = vertListNewNode();
	else *vlist = nlptr->next;
      else
	prev->next = nlptr->next;   /* Leap-frog current */

      /* Destroy it */
      nlptr->current = 0;
      nlptr->next = 0;
      free(nlptr);
    }
}  /* End of vertListRemoveVert*/



VGrid *
vgridCreate()
{
  VGrid *vg;
 
  vg = (VGrid *) malloc(sizeof(VGrid));
  vg->data1 = NULL;
  vg->data2 = NULL;
  return vg;
}




void
vgridInit(VGrid *vgrid, Point minLoc)
{
  int i, j, k;
  Point delta;
  int c;

  for (c = 0; c < 3; c++)
    delta[c] = vgrid->gridScale;
  
  for (i = 0; i < vgrid->dim[0]; i++)
    for (j = 0; j < vgrid->dim[1]; j++)
      for (k = 0;k < vgrid->dim[2]; k++){
	vgrid->vgridpt[i][j][k].occupied = 0;

	vgrid->vgridpt[i][j][k].vertList = NULL;
	vgrid->vgridpt[i][j][k].numVerts = 0;
	vgrid->vgridpt[i][j][k].flist = NULL;

	vgrid->vgridpt[i][j][k].xsectFacet = 0;

	vgrid->vgridpt[i][j][k].objLoc[0] = minLoc[0] + ((float) i) * delta[0];
	vgrid->vgridpt[i][j][k].objLoc[1] = minLoc[1] + ((float) j) * delta[1];
	vgrid->vgridpt[i][j][k].objLoc[2] = minLoc[2] + ((float) k) * delta[2];
	vgrid->vgridpt[i][j][k].selfx = i;
	vgrid->vgridpt[i][j][k].selfy = j;
	vgrid->vgridpt[i][j][k].selfz = k;
	vgrid->vgridpt[i][j][k].distance = VGRIDPT_INIT_DIST;
	pointCopy(vgrid->vgridpt[i][j][k].projPt, vgrid->vgridpt[i][j][k].objLoc);
	vgrid->vgridpt[i][j][k].data1 = NULL;
	vgrid->vgridpt[i][j][k].data2 = NULL;
      }
}



void
vgridAddVert(VGrid *vgrid, Vertex *vert, int gridLoc[3])
{
  VGridPoint *vgpt;
  FacetList *flist;

  vgpt = &vgrid->vgridpt[gridLoc[0]][gridLoc[1]][gridLoc[2]];
  if (vgpt->occupied)
    NumCollisions++;
  vgpt->occupied = 1;
  vgpt->numVerts++;
  vertListAddVert(&(vgpt->vertList), vert);
  vgpt->selfx = gridLoc[0];
  vgpt->selfy = gridLoc[1];
  vgpt->selfz = gridLoc[2];

  if (vgpt->flist == NULL)
    vgpt->flist = flistNewNode();

  for (flist = vert->facets; flist != NULL; flist = flist->next){
    if (flist->current != NULL) {
      flistAddFacet(&(vgpt->flist), flist->current);
    }
  }
  vert->vgridpt = vgpt;
}


void
vgridRemoveVert(VGrid *vg, Vertex *vert)
{
  VGridPoint *vgpt;

  vgpt = vert->vgridpt;
  zdo6 printf("removing vert from grid..\n");
  vgpt->numVerts--;
  if (vgpt->numVerts == 0) {
    vgpt->occupied = 0;
    zdo6 printf("voxel is now empty.\n");
  }

  vertListRemoveVert(&(vgpt->vertList), vert);
}



void
vgridFillGridPoints(VGrid *vgrid)
{
  int i, j, k;

  vgrid->vgridpt = (VGridPoint ***) malloc(sizeof(VGridPoint **)
					  * (vgrid->dim[0] + 2));
  for (i = 0; i < vgrid->dim[0]; i++){
    vgrid->vgridpt[i] = (VGridPoint **) malloc(sizeof(VGridPoint *)
					      * (vgrid->dim[1] + 2));
    for (j = 0; j < vgrid->dim[1]; j++) {
      vgrid->vgridpt[i][j] = (VGridPoint *) malloc(sizeof(VGridPoint)
						  * (vgrid->dim[2] + 2));
    }
  }
}



float 
objFindAveTriSize(ModelObject *obj)
{
  float sumDist;
  int numSamples;
  int j;
  TriStrip *s;
  float aveLen;

  sumDist = 0.0;
  numSamples = 0;

  for (j = 0; j < obj->partCnt; j++){
    for (s = obj->parts[j]->triStrips->strips; s != NULL; s = s->next) {
      int cnt;
      Vertex *prevVert, *currVert;
      int i;

      for (i = 0, cnt = 0; ((i < s->length) &&
			    (cnt < 4)); i++, cnt++) {
	/* Get vertex position and normal */
	if (cnt == 0) {
	  prevVert = &(obj->varray[s->tri[i]]);
	}
	else {
	  currVert = &(obj->varray[s->tri[i]]);
	  sumDist += sqrtDist(prevVert->p, currVert->p);
	  prevVert = currVert;
	  numSamples++;
	}
      }
    }
  }
  

  aveLen =  sumDist / ((float) numSamples);
  zdo printf("Vert Samples %d Ave leng %2.2f (orig %2.2f, scalef %2.2f)\n", 
	     numSamples, aveLen, sumDist / ((float) numSamples)
	     );

  return aveLen;
}




/* vgridFindGridDim takes the arg, gridScaleFactor, 
 * 
 * The ave triangle size is scaled by this factor to make the actual grid
 * size bigger or smaller than actually represented by the triangle
 * size. This is used to make the voxel grid fit tightly or loosely on
 * the object.
 */

void
vgridFindGridDim(VGrid *vgrid, BBox *bbox, float gridScaleFactor,
		 float aveTriLength)
{
  int i, j, k;
  float width;

  width = gridScaleFactor * aveTriLength;
  vgrid->gridScale = width;

  for (i = 0; i < 3; i++){
    vgrid->dim[i] = (int) (((bbox->max[i] - bbox->min[i]) / width) + 1.0);
    
    if (vgrid->dim[i] == 0)
      vgrid->dim[i] = 1;
    printf("dim %d %d \n", i,     vgrid->dim[i]);
    printf("bboxSize %f \n", (bbox->max[i] - bbox->min[i]));
    
  }
}


void
vgridFindGridLoc(VGrid *vg, Point pt, int gridLoc[3]){
  int i, j, k;
  float value;

  for (j = 0; j < 3; j++){
    value = pt[j] - (vg->bbox->min[j]);
    value /= vg->bboxSize[j];
    gridLoc[j] = (int)((value * (float) (vg->dim[j])));
    
    /* Be sure to snap boundary values to boundary */
    if (gridLoc[j] >= vg->dim[j]){
      /* if dim is 0 this would be -1 */
      gridLoc[j] = vg->dim[j] - 1;
    }
    
    /* This needs to come last */
    if (gridLoc[j] < 0)
      gridLoc[j] = 0;
    
  }
}

void
vgridBinVert(VGrid *vg, Vertex *vert)
{
  int j;
  float value;
  int gridLoc[3];

  vgridFindGridLoc(vg, vert->p, gridLoc);
  zdo6 printf("grid %d %d %d \n", gridLoc[0], gridLoc[1], 
	      gridLoc[2]);
  vgridAddVert(vg, vert, gridLoc);
}




void
vgridReBinVert(Vertex *vert, VGrid *vg)
{
 vgridRemoveVert(vg, vert);
 vgridBinVert(vg, vert);
}


void
vgridBinFacet(VGrid *vg, Facet *fptr)
{
  int i,j,k;
  float value;
  int minGridLoc[3], maxGridLoc[3];
  BBox *fptrBbox;

  facetFindBbox(fptr);
  fptrBbox = fptr->bbox;
  vgridFindGridLoc(vg, fptrBbox->min, minGridLoc);
  vgridFindGridLoc(vg, fptrBbox->max, maxGridLoc);
  zdo6 printf("fptr %x hit %d %d %d total %d voxels \n",
	     fptr,
	     maxGridLoc[0]-minGridLoc[0],
	     maxGridLoc[1]-minGridLoc[1],
	     maxGridLoc[2]-minGridLoc[2],

	     (maxGridLoc[0]-minGridLoc[0]) * 
	     (maxGridLoc[1]-minGridLoc[1]) *
	     (maxGridLoc[2]-minGridLoc[2]));

  for (i = 0; i < 3; i++){
    minGridLoc[i] = minGridLoc[i];
    if (minGridLoc[i] < 0)
      minGridLoc[i] = 0;

    maxGridLoc[i] = maxGridLoc[i];
    if (maxGridLoc[i] >= vg->dim[i])
      maxGridLoc[i] = vg->dim[i] - 1;
    
    if (minGridLoc[i] > maxGridLoc[i])
      printf("Warning!! minGridLoc > max! \n");
    fptr->minGridLoc[i] = minGridLoc[i];
    fptr->maxGridLoc[i] = maxGridLoc[i];
  }


  for (i = minGridLoc[0];i <= maxGridLoc[0]; i++){
    for (j = minGridLoc[1];j <= maxGridLoc[1]; j++){
      for (k = minGridLoc[2];k <= maxGridLoc[2]; k++){
	if (vg->vgridpt[i][j][k].flist == NULL)
	  vg->vgridpt[i][j][k].flist = flistNewNode();

	flistAddFacet(&vg->vgridpt[i][j][k].flist, fptr);
	vg->vgridpt[i][j][k].xsectFacet++;
	}
    }
  }

}


void
vgridInsertObj(ModelObject *obj, float gridScaleFactor)
{
  int i;
  Vertex *vert;
  VGrid *vgrid;
  float aveTriLen;

  vgrid = (VGrid *) malloc(sizeof(VGrid));
  obj->vgrid = vgrid;

  zdo printf("Getting grid dim... \n");
  aveTriLen = objFindAveTriSize(obj);
  vgridFindGridDim(vgrid, obj->bbox, gridScaleFactor, aveTriLen);
  vgridFillGridPoints(vgrid);
  zdo printf("Init grid...\n");

  for(i = 0; i < 3 ;i++) { 
    vgrid->bboxSize[i] = obj->bbox->max[i] - obj->bbox->min[i];
  }

  vgrid->bbox = bboxCreate();
  bboxCopy(vgrid->bbox, obj->bbox);

  vgridInit(vgrid, obj->bbox->min);
  NumCollisions = 0;

  vgrid->obj = obj;

  printf("Num collisions %d. \n", NumCollisions);
}


int
createVertlist(VGrid *vg, VGridPoint *vgpt, void *data)
{
  vgpt->vertList = vertListNewNode();
  return 1;
}

void
vgridInitVertlist(VGrid *vgrid)
{
  vgridForEachGridPt(vgrid, createVertlist, NULL);
}


void
vgridBinVerts(VGrid *vgrid, ModelObject *obj)
{
  int i;
  Vertex *vert;

  vgridInitVertlist(vgrid);

  for (i = 0; i <= obj->vertCnt; i++){
    Vertex *vert;

    vert = &obj->varray[i];
    vgridBinVert(vgrid, vert);
  }

  /* Find all facet bounding boxes */
  for (i = 0; i < obj->partCnt; i++){
    Facet *fptr;
    for (fptr = obj->parts[i]->facets; fptr != NULL; fptr = fptr->next){
      facetFindBbox(fptr);
    }
  }

}


void
vgridForEachNeighbor2(VGrid *vg, Vertex *vert,
		int (*act)(Vertex *vert, void *data),
		void *data)
{
  FacetList *flptr;
  
  for (flptr = vert->facets; flptr != NULL; flptr = flptr->next){
    if (flptr->current != NULL) {
      Facet *fptr;
      int i;
      Vertex *v;
      
      fptr = flptr->current;
      for (i = 0; i < fptr->numVerts; i++){
	v = &(vg->varray[fptr->vindex[i]]);
	if (v != vert)
	  act(v, data);
      }
    }
  }
}


void
vgridForEachNeighbor(VGrid *vg, Vertex *vstart,
		int (*act)(Vertex *vert, VGridPoint *vgpt, void *data),
		void *data)
{
  int basex, basey, basez;
  VGridPoint *vgpt;
  int m;
  int i, j, k;
  VGridPoint *curr;
  float minAxisDist;
  Vertex *bestVert;
  Vertex *vert;
  VertList *vlptr;

  vgpt  = vstart->vgridpt;
  basex = vgpt->selfx;
  basey = vgpt->selfy;
  basez = vgpt->selfz;

  zdo6 printf("Starting at grid %d %d %d. \n", basex, basey, basez);

  /* future: Look at the current gridpt for candidates first */
  zdo6 for (vlptr = vgpt->vertList; vlptr != NULL; vlptr = vlptr->next){
    if (vlptr->current != NULL) {
      Vertex *v;
		
      /* Apply action on vert; punts if returns 0 */
      v = vlptr->current;
      if (v != vstart)
	if (!act(v, vgpt, data))
	  return;
    }
  }

  m = 1;
  for (k = m; k > -1*m; k--)
    if (((k+basez) < vg->dim[2]) && ((k+basez) >= 0) ) {
	for (j = m; j > -1*m; j--)
	  if (((j+basey) < vg->dim[1]) && ((j+basey) >= 0)) {
	    for (i = m; i >= -1*m; i--){
	      if (((i+basex) < vg->dim[0]) && ((i + basex) >= 0)) {
		curr = &vg->vgridpt[i+basex][j+basey][k+basez];
		zdo6 printf("Looking at %d %d %d. \n", 
			   i+basex, j+basey, k+basez);
		if (curr->occupied){
		  zdo6 printf("Found.\n");
		  for (vlptr = curr->vertList; vlptr != NULL; vlptr = vlptr->next){
		    if (vlptr->current != NULL) {
		      Vertex *v;
		      
		      /* Apply action on vert; punts if returns 0 */
		      v = vlptr->current;
		      if (!act(v, curr, data))
			return;
		    }
		  }
		}
		else {
		  zdo6 printf("not occupied\n");
		}
	      }
	    }
	  }
      }
}



Vertex *
vgridFindNearestNeighbor(VGrid *vg, Vertex *vert, float axisline, int dir )
{
  int basex, basey, basez;
  VGridPoint *vgpt;
  int m;
  int i, j, k;
  VGridPoint *curr;
  float minAxisDist;
  Vertex *bestVert;

  vgpt  = vert->vgridpt;
  basex = vgpt->selfx;
  basey = vgpt->selfy;
  basez = vgpt->selfz;

  printf("Starting at grid %d %d %d. \n", basex, basey, basez);

  /* future: Look at the current gridpt for candidates first */
  minAxisDist = MAXFLOAT;
  bestVert = NULL;
  for ( m = 1; m < 3; m++)
    for (k = m; k > -1*m; k--)
      if (((k+basez) < vg->dim[2]) && ((k+basez) >= 0) ) {
	for (j = m; j > -1*m; j--)
	  if (((j+basey) < vg->dim[1]) && ((j+basey) >= 0)) {

	    if (dir > 0)
	    for (i = m; i > 0; i--){
	      if (((i+basex) < vg->dim[0]) && ((i + basex) >= 0)) {
		curr = &vg->vgridpt[i+basex][j+basey][k+basez];
		zdo printf("Looking at %d %d %d. \n", 
			   i+basex, j+basey, k+basez);
		if ((curr->occupied) &&
		    (curr->vertList->current->tag != CurrVertTag)){
		  VertList *vlptr;

		  zdo printf("Found.\n");
		  for (vlptr = curr->vertList; vlptr != NULL; 
		       vlptr = vlptr->next){
		    if (vlptr->current != NULL) {
		      
		      vert =  vlptr->current;
		      if (fabs((double)(vert->p[1] - axisline)) 
			  < minAxisDist){
			bestVert = vert;
			minAxisDist = fabs((double) (vert->p[1] - axisline));
		      }
		    }
		  }
		}
		else {
		  zdo printf("not occupied or have seen.\n");
		}
	      }
	    }
	    
	    else 
	    for (i = -1 * m; i < 0; i++){
	      if (((i+basex) < vg->dim[0]) && ((i + basex) >= 0)) {
		curr = &vg->vgridpt[i+basex][j+basey][k+basez];
		zdo printf("Looking at %d %d %d. \n", 
			   i+basex, j+basey, k+basez);
		if ((curr->occupied) &&
		    (curr->vertList->current->tag != CurrVertTag)){
		  zdo printf("Found.\n");
		  vert =  curr->vertList->current;
		  if (fabs((double)(vert->p[1] - axisline)) < minAxisDist){
		    bestVert = vert;
		    minAxisDist = fabs((double) (vert->p[1] - axisline));
		  }
		}
		else {
		  zdo printf("not occupied or have seen.\n");
		}
	      }
	    }
	  }
      }

  return bestVert;
}
	


void
vgridForEachGridPt(VGrid *vgrid, int (*act)(VGrid *, VGridPoint *, void *),
		   void *data)
{
  int i, j, k;

  for (i = 0; i < vgrid->dim[0]; i++)
    for (j = 0; j < vgrid->dim[1]; j++)
      for (k = 0;k < vgrid->dim[2]; k++){
	if (act(vgrid, &vgrid->vgridpt[i][j][k], data) == 0)
	  break;
      }
}
