///////////////////////////////////////////////////////////////////////////////
//
// refine_tin.c - This code contains allows you to tile, and refine a tin
//
// Author: Jon Todd
//
// real-time rednering by rajiv wickremesinghe
//
///////////////////////////////////////////////////////////////////////////////


#include "refine_tin.h"

#include <pthread.h>

typedef struct {
  TINNODE *head;
  GRID *bigGrid;
  int TL;
  double e;
} render_t;


pthread_t thd;
pthread_mutex_t mutex;
void refine_start(render_t *);
void refine(GRID* g, double e, short delaunay, TIN **);

#define DEBUG if(0)

//enables printing points that are refined
//#define REFINE_DEBUG 

// This is a special point which will be used to mark that the max
// error for a given triangle is less than e and thus it is 'done'
extern COORD DONE[3];


//
// initTin - initialize TIN structure, returns a pointer to lower left tri
//
TIN* initTin(GRID* g, PQueue* pq, double e) {

  assert(g && g->data && pq);

  // Create a pointer to the lower left tri in the tin
  TRIANGLE *first, *second;
  TIN *s = (TIN*)malloc(sizeof(TIN));
  assert(s);
  s->nrows = g->nrows;
  s->ncols = g->ncols;

  // Store nodata value
  s->nodata = g->nodata;
  
  // Add 2 triangles into TIN from the 4 corners of the grid
  COORD *nw = (COORD*)malloc(3 * sizeof(COORD));
  COORD *ne = (COORD*)malloc(3 * sizeof(COORD));
  COORD *sw = (COORD*)malloc(3 * sizeof(COORD));
  COORD *se = (COORD*)malloc(3 * sizeof(COORD));
  
  nw[X]=0;
  nw[Y]=0;
  nw[Z]=g->data[0][0];

  ne[X]=0;
  ne[Y]=g->ncols-1;
  ne[Z]=g->data[0][g->ncols-1];

  se[X]=g->nrows-1;
  se[Y]=g->ncols-1;
  se[Z]=g->data[g->nrows-1][g->ncols-1];

  sw[X]=g->nrows-1;
  sw[Y]=0;
  sw[Z]=g->data[g->nrows-1][0];
  
  // Create the first two triangles
  s->t = first = addTri(nw,sw,se,NULL,NULL,NULL);
  second = addTri(nw,ne,se,NULL,first,NULL);
  assert(s->t);
  assert(first);
  assert(second);

  // Point the tins lower left corner to sw
  s->v = sw;
  
  // Lower left edge nw-sw gets stored in tin
  s->e.t1 = NULL;
  s->e.t2 = NULL;
  s->e.p1 = nw;
  s->e.p2 = sw;
  s->e.type = IN;

  // Now build list of points in the triangle
  // Create a dummy tail for both point lists
  first->points = Q_init();
  second->points = Q_init();
  
  // Build the two point lists
  register int row, col;
  long maxE_first=0;
  long maxE_second=0;
  long tempE = 0;
  
  POINT temp;
  
  // iterate through all points and distribute them to the 
  // two triangles
  for(row=0;row<g->nrows;row++) {
    temp[X]=row;
    for(col=0;col<g->ncols;col++) {
      temp[Y]=col;
      temp[Z] = g->data[row][col];
      //Ignore the corner points
      if((row==0 && col==0) || 
	 (row==0 && col==g->ncols-1) || 
	 (row==g->nrows-1 && col==g->ncols-1) || 
	 (row==g->nrows-1 && col==0)) 
	continue;
      
      // Add to the first triangle's list
      if(inTri2D(first->p1, first->p2, first->p3, (COORD*)temp)) {
	
	Q_insert_elem_head(first->points, temp);
	
	// Ignore NODATA points
	if (g->data[row][col] == g->nodata){
	 continue;
	}

	//Update max error
	tempE = findError(row,col,g->data[row][col],first);
	if (tempE > maxE_first) {
	  maxE_first = tempE;
	  assert(Q_first(first->points));
	  // store pointer to triangle w/ max err
	  first->maxE = Q_first(first->points)->e;
	  first->maxErrorValue = tempE;
	}
      }
      // Add to the second triangle's list
      else {

	Q_insert_elem_head(second->points, temp);
	
	// Ignore NODATA points
	if (g->data[row][col] == g->nodata){
	 continue;
	}

	//Update max error
	tempE = findError(row,col,g->data[row][col],second);
	if (tempE > maxE_second) {
	  maxE_second = tempE;
	  assert(Q_first(second->points));
	  // store pointer to triangle w/ max err
	  second->maxE = Q_first(second->points)->e; 
	  second->maxErrorValue = tempE;
	}

      }

    }//for col
  }//for row
  //end distribute points among initial triangles

  DEBUG {checkPointList(first); checkPointList(second);}

  // Free grid data
  assert(g && g->data);
  for(row=0;row<g->nrows;row++){
    assert(g->data[row]); 
    free(g->data[row]);
  }
  free(g->data);

  // First triangle has no points with error > e, mark as done
  if (maxE_first <= e){
    first->maxE = (COORD*)DONE;
  }
  // Insert max error point into the PQ
  else
    PQ_insert(pq,first);

  // Second triangle has no points with error > e, mark as done
  if (maxE_second <= e){
    second->maxE = (COORD*)DONE;
  }
  // Insert max error point into the PQ
  else
    PQ_insert(pq,second);
  
  return s;
}


//
// Take in the big grid, and splits into tiles, refines them, and puts
// them in a TINLIST
//
TINLIST doTiles(GRID *bigGrid, double e, double mem){
  // create a dummytail for the TINLIST
  TINNODE *head = (TINNODE*)malloc(sizeof(TINNODE));
  assert(head);
  TINNODE *dummytail = (TINNODE*)malloc(sizeof(TINNODE));
  assert(dummytail);
  head->next = dummytail;
  dummytail->next = NULL;
  
  int TL = getTileLength(mem); // compute length of tile for mem

  int jTiles, iTiles;
  
  // Find number of tiles needed in i and j direction
  jTiles = ceil( ((double) bigGrid->ncols)/ ((double) TL));
  iTiles = ceil( ((double) bigGrid->nrows)/ ((double) TL));

  int i,j;
  for (i = 0; i < iTiles; i++){
    for (j = 0; j < jTiles; j++){
      TINNODE * tNode = (TINNODE*)malloc(sizeof(TINNODE));
      
      tNode->iOffset = TL*i;
      tNode->jOffset = TL*j;
      tNode->t = NULL;
      tNode->next = head->next;
      head->next = tNode;
    }
  }

  static render_t c;
  c.head = head;
  c.bigGrid = bigGrid;
  c.TL = TL;
  c.e = e;
  int err = pthread_create(&thd, NULL, refine_start, &c);

  if(err) {
    fprintf(stderr, "thread error\n");
    exit(1);
  }

  return head;
}

void refine_start(render_t *c) {
  GRID *tile = NULL;
  TINNODE * tNode;

  tNode = c->head->next;  //skip dummy head
  while(tNode->next != NULL){
    tile = getTile(c->bigGrid, tNode->iOffset, tNode->jOffset, c->TL, c->bigGrid->nodata);
    tile->min = c->bigGrid->min;
    tile->max = c->bigGrid->max;

    fprintf(stderr, "refine thread running...\n");
    pthread_mutex_lock(&mutex);
    refine(tile, c->e, 1, &tNode->t);
    fprintf(stderr, "refine thread done.\n");
    pthread_mutex_unlock(&mutex);
    free(tile);

    tNode = tNode->next;
  }

  return;
}

//
// get tile, this will get a tile of size a^2 out of the grid to work
// on
//
GRID *getTile(GRID *bigGrid, int startI, int startJ, int a, int noData){
  //printf("entering get tile\n"); fflush(stdout);
  int i, j, aRows, aCols;
  GRID *tile = (GRID *) malloc(sizeof(GRID));
  assert(tile);
  
  // make sure we don't go off the big grid's edges
  if ((startI + a) > bigGrid->nrows){
    aRows = bigGrid->nrows - startI;
    tile->nrows = bigGrid->nrows - startI;
  }
  else{
    aRows = a;
    tile->nrows = a;
  }

  if((startJ + a) > bigGrid->ncols){
    aCols = bigGrid->ncols - startJ;
    tile->ncols = bigGrid->ncols - startJ;
  }
  else {
    aCols = a;
    tile->ncols = a; 
  }    
  tile->min = bigGrid->min;
  tile->max = bigGrid->max;
  tile->nodata = noData;

 
  // Dynamically allocate array for tile
  if((tile->data = (short**) malloc((aRows)*sizeof(short*)))==NULL){
     printf("tin: insufficient memory");
     exit(1);
  }
  for(i=0;i<aRows;i++){
    if((tile->data[i] = (short*) malloc((aCols)*sizeof(short)))==NULL){
      printf("tin: insufficient memory2");
      exit(1);
    }
  }

  for (i = 0; i < aRows; i++){

    for(j = 0; j < aCols; j++){
      tile->data[i][j] = bigGrid->data[startI + i][startJ + j];
    }
  }

  return tile;
}




//
// Determine if p is inside the circum circle of pa,pb,pc
//
short inCircumCircle(COORD *pa, COORD *pb, COORD *pc, COORD *p){
  
/*   // A point is in the circum circle of the triangle pa,pb,pc if the */
/*   // 4x4 determinant of the points and their parabola x^2+y^2 are */
/*   // greater than 0. For further information on this refer to: */
/*   // H. Edelsbrunner, "Triangulations and meshes in computational */
/*   // geometry" */

/*   double det; */
/*   int area; */
/*   if(area){ */
/*     determinant4x4(det, */
/* 		   1,pc[X],pc[Y],((pc[X]*pc[X])+(pc[Y]*pc[Y])), */
/* 		   1,pb[X],pb[Y],((pb[X]*pb[X])+(pb[Y]*pb[Y])), */
/* 		   1,pa[X],pa[Y],((pa[X]*pa[X])+(pa[Y]*pa[Y])), */
/* 		   1, p[X], p[Y],(( p[X]* p[X])+( p[Y]* p[Y]))); */
/*     det = det * area; */
  
/*     if(det > 0){ */
/*       DEBUG{printf("inCircumCircle: Tri:(%d,%d)(%d,%d)(%d,%d) Pt: (%d,%d) No\n", */
/* 		   pa[X],pa[Y],pb[X],pb[Y],pc[X],pc[Y], p[X], p[Y]);} */
/*       return 0; */
/*     } */
/*     else{ */
/*       DEBUG{printf("inCircumCircle: Tri:(%d,%d)(%d,%d)(%d,%d) Pt: (%d,%d) Yes\n", */
/* 		   pa[X],pa[Y],pb[X],pb[Y],pc[X],pc[Y], p[X], p[Y]);} */
/*       return 1; */
/*     } */
/*   } */
  
/*   // Should not be collinear */
/*   assert(0); */
/*   return 0; */
  

  int det2;
  double xc,yc,r;
  det2 = CircumCircle(p[X],p[Y],pa[X],pa[Y],pb[X],pb[Y],pc[X],pc[Y],&xc,&yc,&r);
  
  if(det2 == FALSE){
    DEBUG{printf("inCircumCircle: Tri:(%d,%d)(%d,%d)(%d,%d) Pt: (%d,%d) No\n",
		 pa[X],pa[Y],pb[X],pb[Y],pc[X],pc[Y], p[X], p[Y]);}
    return 0;
  }
  else{
    DEBUG{printf("inCircumCircle: Tri:(%d,%d)(%d,%d)(%d,%d) Pt: (%d,%d) Yes\n",
		 pa[X],pa[Y],pb[X],pb[Y],pc[X],pc[Y], p[X], p[Y]);}
    return 1;
  }







 /*  if( (det > 0 && det2 != FALSE) || (det <= 0 && det2 != TRUE) ){ */
/*     DEBUG{printf("inCircumCircle: Disagreement! det: %f det2: %d \n \t Tri:(%d,%d)(%d,%d)(%d,%d) Pt: (%d,%d) \n",det,det2,pa[X],pa[Y],pb[X],pb[Y],pc[X],pc[Y], p[X], p[Y]);} */
/*     //assert(0); */
/*   } */
  // This should never happen
  //else 
  //  assert(0);
  
  
}

/*
   Return TRUE if a point (xp,yp) is inside the circumcircle made up
   of the points (x1,y1), (x2,y2), (x3,y3)
   The circumcircle centre is returned in (xc,yc) and the radius r
   NOTE: A point on the edge is inside the circumcircle
*/
int CircumCircle(double xp,double yp,
   double x1,double y1,double x2,double y2,double x3,double y3,
   double *xc,double *yc,double *r)
{
   double m1,m2,mx1,mx2,my1,my2;
   double dx,dy,rsqr,drsqr;

   /* Check for coincident points */
   if (ABS(y1-y2) < EPSILON && ABS(y2-y3) < EPSILON)
       return(FALSE);

   if (ABS(y2-y1) < EPSILON) {
      m2 = - (x3-x2) / (y3-y2);
      mx2 = (x2 + x3) / 2.0;
      my2 = (y2 + y3) / 2.0;
      *xc = (x2 + x1) / 2.0;
      *yc = m2 * (*xc - mx2) + my2;
   } else if (ABS(y3-y2) < EPSILON) {
      m1 = - (x2-x1) / (y2-y1);
      mx1 = (x1 + x2) / 2.0;
      my1 = (y1 + y2) / 2.0;
      *xc = (x3 + x2) / 2.0;
      *yc = m1 * (*xc - mx1) + my1;
   } else {
      m1 = - (x2-x1) / (y2-y1);
      m2 = - (x3-x2) / (y3-y2);
      mx1 = (x1 + x2) / 2.0;
      mx2 = (x2 + x3) / 2.0;
      my1 = (y1 + y2) / 2.0;
      my2 = (y2 + y3) / 2.0;
      *xc = (m1 * mx1 - m2 * mx2 + my2 - my1) / (m1 - m2);
      *yc = m1 * (*xc - mx1) + my1;
   }

   dx = x2 - *xc;
   dy = y2 - *yc;
   rsqr = dx*dx + dy*dy;
   *r = sqrt(rsqr);

   dx = xp - *xc;
   dy = yp - *yc;
   drsqr = dx*dx + dy*dy;

   return((drsqr <= rsqr) ? TRUE : FALSE);
	// Suggested
	// return((drsqr <= rsqr + EPSILON) ? TRUE : FALSE);

}




//
// Swap the common edge between two triangles t1 and t2. The common
// edge should always be edge ac, abc are part of t1 and acd are part
// of t2.
//
void edgeSwap(TRIANGLE *t1, TRIANGLE *t2, 
	      COORD *a, COORD *b, COORD *c, COORD *d,
	      double e, PQueue* pq, TIN *tin){

  // Common edge must be ac
  assert(isEndPoint(t1,a) && isEndPoint(t1,b) && isEndPoint(t1,c) && 
	 isEndPoint(t2,a) && isEndPoint(t2,c) && isEndPoint(t2,d));

  assert(a != b && a != c && a != d && b != c && b != d && c != d);

  assert(t1 != t2);
  
  // Add the two new triangles with the swapped edge 
  TRIANGLE *tn1, *tn2;
  tn1 = addTri(a, b, d,whichTri(t1,a,b),whichTri(t2,a,d),NULL);
  tn2 = addTri(c, b, d,whichTri(t1,c,b),whichTri(t2,c,d),tn1);
  
  assert(isEndPoint(tn1,a) && isEndPoint(tn1,b) && isEndPoint(tn1,d) && 
	 isEndPoint(tn2,b) && isEndPoint(tn2,c) && isEndPoint(tn2,d));

  assert(tn1->p2p3 == tn2 && tn2->p2p3 == tn1);

 
  if(tn1->p1p2 != NULL)
    assert(whichTri(tn1->p1p2,a,b) == tn1);
  if(tn1->p1p3 != NULL)
    assert(whichTri(tn1->p1p3,a,d) == tn1);
  if(tn2->p1p2 != NULL)
    assert(whichTri(tn2->p1p2,c,b) == tn2);
  if(tn2->p1p3 != NULL)
    assert(whichTri(tn2->p1p3,c,d) == tn2);

  // Debug
  DEBUG{
  printf("EdgeSwap: \n");
  printTriangle(t1);
  printTriangle(t2);
  printTriangle(tn1);
  printTriangle(tn2);
  }

  // mark triangles for deletion from the PQ
  t1->p1p2 = t1->p1p3 = t1->p2p3 = NULL;
  t2->p1p2 = t2->p1p3 = t2->p2p3 = NULL;
  assert(t1->maxE == (COORD*)DONE || 
	 t1->maxErrorValue == findError(t1->maxE[X], t1->maxE[Y], t1->maxE[Z], t1));
  assert(t2->maxE == (COORD*)DONE || 
	 t2->maxErrorValue == findError(t2->maxE[X], t2->maxE[Y], t2->maxE[Z], t2));

  // Distribute point list from t1 and t2 to tn1 and tn2. Distribute
  // points requires that the fourth argument (s) be a valid triangle
  // with a point list so we must check that t1 and t2 are not already
  // done and thus do not have a point list. If at least one does then
  // call distribute points with s = the trinagle with the valid point
  // list. If both are done then the newly created triangles are done
  // tooand need to be marked accordingly
  if(t1->maxE != (COORD*)DONE && t2->maxE != (COORD*)DONE)
    distrPoints(tn1,tn2,NULL,t1,t2,e,pq,tin);
  else if(t1->maxE != (COORD*)DONE){
    distrPoints(tn1,tn2,NULL,t1,NULL,e,pq,tin);
  }
  else if(t2->maxE != (COORD*)DONE){
    distrPoints(tn1,tn2,NULL,t2,NULL,e,pq,tin);
  }
  else{
    tn1->maxE = tn2->maxE = (COORD*)DONE;
    tn1->points = tn2->points= NULL;
  }

  // Update the corner if the corner is being swapped. Distrpoints
  // will not always catch this so it must be done here
  if(t1 == tin->t || t2 == tin->t){
    updateTinCorner(tin,tn1,tn2,NULL);    
  }

  DEBUG{checkPointList(tn1); checkPointList(tn2);}

  // We have created two different triangles, we need to check
  // delaunay on their 2 edges
  enforceDelaunay(tn1,a,d,b,e,pq,tin);
  enforceDelaunay(tn2,c,d,b,e,pq,tin);

  

}

//
// Enforce delaunay on triangle t. Assume that p1 & p2 are the
// endpoints to the edge that is being checked for delaunay
//
void enforceDelaunay(TRIANGLE *t, COORD *p1, COORD *p2, COORD *p3,
		     double e, PQueue* pq, TIN *tin){
  
  // Find the triangle on the other side of edge p1p2
  TRIANGLE *tn = whichTri(t,p1,p2);  

  // If tn is NULL then we are done, otherwise we need to check delaunay
  if(tn != NULL){

    // Find point across from edge p1p2 in tn
    COORD *d = findThirdPoint(tn->p1,tn->p2,tn->p3,p1,p2);

    if(inCircumCircle(p1,p3,p2,d)){
      edgeSwap(t,tn,p1,p3,p2,d,e,pq,tin);
    }
  }
}




//
// Refine a grid into a TIN with error < e
//
void refine(GRID* g, double e, short delaunay, TIN **tinp) {  
  BOOL complete;    // is maxE < e
  complete = 0;
  TRIANGLE *t1, *t2, *t3, *s, *snext;
  PQueue* pq;
  int refineCount = 0;

  // Create a PQ of the error of the triangles
  pq = PQ_initialize();

  // Initialize TIN with two triangles
  (*tinp) = initTin(g, pq, e);
  
  // While there still is a triangle with max error > e
  while(PQ_extractMin(pq, &s)){

    // Skip triangle that were marked for deletion
    if(s->p1p2 == NULL && s->p1p3 == NULL && s->p2p3 == NULL){
#ifdef REFINE_DEBUG 
      printf("skipping deleted triangle: err=%li",s->maxErrorValue);
      printTriangleCoords(s);
      fflush(stdout);
#endif
      removeTri(s);
      goto next;
    }	

    assert(s); 
    refineCount++;

    // malloc the point with max error as it will become a corner
    COORD* maxError = (COORD*)malloc(3*sizeof(COORD));
    assert(maxError);	
    maxError[X] = s->maxE[X];
    maxError[Y] = s->maxE[Y];
    maxError[Z] = s->maxE[Z];

    // Debug - print the point being added
#ifdef REFINE_DEBUG 
    {
      long err = findError(s->maxE[X], s->maxE[Y], s->maxE[Z], s); 
      printf("Point (%6d,%6d,%6d) error=%10ld \t", 
	     s->maxE[X], s->maxE[Y], s->maxE[Z], err );
      printTriangleCoords(s);
      fflush(stdout);

      if(err != s->maxErrorValue){
	printf("Died err= %ld maxE= %ld \n",err,s->maxErrorValue);
	exit(1);
      }

      PQ_min(pq, &snext); 
      assert(s->maxErrorValue >= snext->maxErrorValue); 
    }
#endif
    // Check for collinear points. We make the valid assumption that
    // MaxE cannot be collinear with > 1 tri
    int area12,area13,area23;

    area12 = areaSign(s->p1, s->p2, maxError);
    area13 = areaSign(s->p1, maxError, s->p3);
    area23 = areaSign(maxError, s->p2, s->p3);

    DEBUG{ printf("\nRefine: (%d,%d) (%d,%d) (%d,%d) (%d,%d,%d) %p %p %p",
		  s->p1[X],s->p1[Y],s->p2[X],s->p2[Y],s->p3[X],s->p3[Y],
		  maxError[0],maxError[1],maxError[2],s->p1p2,s->p1p3,
		  s->p2p3); fflush(stdout); }

    // If p1 p2 is collinear with MaxE
    if (!area12){
      fixCollinear(s->p1,s->p2,s->p3,s,e,pq, maxError,(*tinp),delaunay);
    }
    else if (!area13){
      fixCollinear(s->p1,s->p3,s->p2,s,e,pq, maxError,(*tinp),delaunay);
    }
    else if (!area23){
      fixCollinear(s->p2,s->p3,s->p1,s,e,pq, maxError,(*tinp),delaunay);
    }
    else {
      // add three new triangles
      t1 = addTri(s->p1, s->p2, maxError,s->p1p2,NULL,NULL);
      t2 = addTri(s->p1, maxError, s->p3,t1,s->p1p3,NULL);
      t3 = addTri(maxError, s->p2, s->p3,t1,t2,s->p2p3);
      DEBUG{triangleCheck(s,t1,t2,t3);}

      // create poinlists from the original tri (this will yeild the max error)
      distrPoints(t1,t2,t3,s,NULL,e,pq,(*tinp));
      DEBUG{checkPointList(t1);checkPointList(t2);checkPointList(t3);}  

      // Enforce delaunay on three new edges of the new triangles if
      // specified
      if(delaunay){
	// we enforce on the edge that does not have maxE as an
	// endpoint, so the 4th argument to enforceDelaunay should
	// always be the maxE point to s for that particular tri
	enforceDelaunay(t1,t1->p1,t1->p2,t1->p3,e,pq,(*tinp));
	enforceDelaunay(t2,t2->p1,t2->p3,t2->p2,e,pq,(*tinp));
	enforceDelaunay(t3,t3->p2,t3->p3,t3->p1,e,pq,(*tinp));
      }
    }
    
    // remove original tri
    removeTri(s);
    //DEBUG{printTin(tin);}

    extern int displayValid;
    displayValid = 0;

  next:
    pthread_mutex_unlock(&mutex);
    pthread_mutex_lock(&mutex);

  } 
  s = (*tinp)->t;
  printf("\n\n REFINE DONE: (%d,%d) (%d,%d) (%d,%d) rows & cols:%d & %d\n Count:%d \n Left Edge: (%d,%d) (%d,%d)\n",
	 s->p1[X],s->p1[Y],s->p2[X],s->p2[Y],s->p3[X],s->p3[Y],(*tinp)->nrows,(*tinp)->ncols,
	 refineCount,(*tinp)->e.p1[X],(*tinp)->e.p1[Y],(*tinp)->e.p2[X],(*tinp)->e.p2[Y]);

  return;
}

//
// Add triangles and distribute points when there are two collinear
// triangles. Assumes that maxE is on line pa pb
//
void fixCollinear(POINT pa, POINT pb, POINT pc, TRIANGLE* s, double e, 
		  PQueue* pq, COORD* maxError, TIN *tin, short delaunay)
{

  assert(s && pq && maxError);
  TRIANGLE *sp, *t1, *t2, *t3, *t4;
  
  // add 2 tris in s
  t1 = addTri(pa, maxError, pc,NULL,whichTri(s,pa,pc),NULL);
  assert(t1);
  t2 = addTri(maxError, pc, pb,t1,NULL,whichTri(s,pb,pc));
  assert(t2);
  DEBUG{triangleCheck(s,t1,t2,NULL);}
  
  // Distribute points in the 2 triangles
  distrPoints(t1,t2,NULL,s,NULL,e,pq,tin);

  DEBUG{checkPointList(t1); checkPointList(t2);}  

  // Find other triangle Note: there may not be another triangle if s
  // is on the edge
  sp = whichTri(s,pa,pb);
  
  // If there is a triangle on the other side of the collinear edge
  // then we need to split that triangle into two
  if(sp != NULL){
    
    // Find the unknown point of the triangle on the other side of the line
    COORD* pd;
    pd = findThirdPoint(sp->p1,sp->p2,sp->p3,pa,pb);
    assert(pd);
    
    // add 2 tris adjacent to s on pa pb
    t3 = addTri(pa,maxError,pd,t1,whichTri(sp,pd,pa),NULL);
    assert(t3);
    t4 = addTri(pb,maxError,pd,t2,whichTri(sp,pd,pb),t3);
    assert(t4);
    DEBUG{triangleCheck(sp,t3,t4,NULL);}
    
    //Mark triangle sp for deletion from pq before distrpoints so we
    //can include its maxE in the newly created triangle
    sp->p1p2 = sp->p1p3 = sp->p2p3 = NULL;
    assert(sp->maxE == (COORD*)DONE || 
	   sp->maxErrorValue == findError(sp->maxE[X], sp->maxE[Y], sp->maxE[Z], sp));
    
    //If this triangle was already "done" meaning it has no more
    //points in it's point list > MaxE then we don't distribute points
    //because sp has no point list
    if(sp->maxE != (COORD*)DONE)
      distrPoints(t3,t4,NULL,sp,NULL,e,pq,tin);
    else{
      // Since distrpoints normally fixes corner we need to do it here
      if(sp == tin->t){
	updateTinCorner(tin,t3,t4,NULL);    
      }
      t3->maxE = t4->maxE = (COORD*)DONE;
      //t3->points = t4->points = NULL;
    }
    DEBUG{checkPointList(t3); checkPointList(t4);}

    // Enforce delaunay on two new edges of the new triangles if
    // specified
    if(delaunay){
      // we enforce on the edge that does not have maxE as an
      // endpoint, so the 4th argument to enforceDelaunay should
      // always be the maxE point to s for that particular tri
      enforceDelaunay(t1,t1->p1,t1->p3,t1->p2,e,pq,tin);
      enforceDelaunay(t2,t2->p2,t2->p3,t2->p1,e,pq,tin);
      enforceDelaunay(t3,t3->p1,t3->p3,t3->p2,e,pq,tin);
      enforceDelaunay(t4,t4->p1,t4->p3,t4->p2,e,pq,tin);
    }
  }
  else{
    // Enforce delaunay on two new edges of the new triangles if
    // specified
    if(delaunay){
      // we enforce on the edge that does not have maxE as an
      // endpoint, so the 4th argument to enforceDelaunay sho uld
      // always be the maxE point to s for that particular tri
      enforceDelaunay(t1,t1->p1,t1->p3,t1->p2,e,pq,tin);
      enforceDelaunay(t2,t2->p2,t2->p3,t2->p1,e,pq,tin);
    }
  }
}


//
// Divide a pointlist of s and sp into the three pointlists of the new
// tris. This assumes that s exists and has a point list. Most of the
// time sp will be NULL because we are distributing points from a
// parent triangle. sp may not be NULL when edge swapping for
// delaunay. Assume that if sp is not null then it has a valid point list
//
 void distrPoints(TRIANGLE* t1, TRIANGLE* t2, TRIANGLE* t3, TRIANGLE* s, 
		  TRIANGLE* sp, double e, PQueue* pq, TIN *tin) {

  //at most one can be null
  assert((t1 && t2) || (t1 && t3) || (t2 && t3));
  // Distribute points should never be called on a triangle that does
  // not have a list of points
  assert(s->points);

  // Assume that if sp is not null then it has a valid point list
  if(sp != NULL)
    assert(sp->points);

  long max1=e;
  long max2=e;
  long max3=e;

  long tempE=0;
  register QUEUE h;
  register QNODE* cur;
  h = s->points;



  // Add a queue of points for each triangle that is not NULL and that
  // does not already have a point list
  if(t1 != NULL){
    t1->points = Q_init();
    t1->maxE = (COORD*)DONE; // this will change if not actually done
  }
  if(t2 != NULL){
    t2->points = Q_init();
    t2->maxE = (COORD*)DONE; // this will change if not actually done
  }
  if(t3 != NULL){
    t3->points = Q_init();
    t3->maxE = (COORD*)DONE; // this will change if not actually done
  }
    
  
  
  int doneDistr = 0;
  
  // Have we seen only nodata points?
  short hasNoData1 = 0;
  short hasNoData2 = 0;
  short hasNoData3 = 0;

  short pointAdded = 0;    

  // Build point list from s and sp also update lower left corner if nessesary
  while(doneDistr == 0){

    DEBUG{checkPointList(s);} 

    // if s is the lower leftmost tri then update the lower left tri
    if(s == tin->t){
      updateTinCorner(tin,t1,t2,t3);    
    }

    //Loop and distribute points until the dummy head->next == NULL
    while((cur = Q_remove_first(h)) != NULL) {
      assert(inTri2D(s->p1, s->p2, s->p3, cur->e));
      
      // We have not yet added a point this loop
      pointAdded = 0;

      // skip the point with the maxE if this triangle is not marked for
      // deletion. If it is marked for deletion then it needs to be
      // added to one of the triangles being created
      if(cur->e == s->maxE && 
	 !(s->p1p2 == NULL && s->p1p3 == NULL && s->p2p3 == NULL)){
	//free(cur);
	continue;
      } 

      // Triangle 1
      if(t1 != NULL){
	if(inTri2D(t1->p1, t1->p2, t1->p3, cur->e)) {
	  // Add point to point list
	  Q_insert_qnode_head(t1->points,cur);
	  pointAdded = 1;
	  // Don't calc maxE for nodata points
	  //if(cur->e[Z] != tin->nodata){
	    tempE = findError(cur->e[X], cur->e[Y], cur->e[Z], t1);
	    // Update max error
	    if (tempE>=max1) {
	      max1 = tempE;
	      t1->maxE = cur->e;
	      t1->maxErrorValue = tempE;
	    }
	    continue;
	    //}
	  /* else */
/* 	    hasNoData1 = 1; */
	}
      }

      // Triangle 2
      if(t2 != NULL){
	if(inTri2D(t2->p1, t2->p2, t2->p3, cur->e)) {
	  // Don't calc maxE for nodata points
	  //if(cur->e[Z] != tin->nodata){ 
	    Q_insert_qnode_head(t2->points,cur);
	    tempE = findError(cur->e[X], cur->e[Y], cur->e[Z], t2);
	    //Update max error
	    if (tempE>=max2) {
	      max2 = tempE;
	      t2->maxE = cur->e;
	      t2->maxErrorValue = tempE;
	    }
	    continue;
	    //}
	  // Add a nodata point to the triangle.
	  /* else{ */
/* 	    // If point is already added to another triangle (on edge) */
/* 	    // then we need to use insert_elem_head */
/* 	    if(pointAdded){ */
/* 	      Q_insert_elem_head(t2->points,cur->e); */
/* 	      //printf("Double added %d,%d,%d\n",cur->e[X],cur->e[Y],cur->e[Z]); */
/* 	    } */
/* 	    else{ */
/* 	      Q_insert_qnode_head(t2->points,cur); */
/* 	      pointAdded = 1; */
/* 	    } */
/* 	    hasNoData2 = 1; */
/* 	  } */
	  
	}
      }

      // Triangle 3
      if(t3 != NULL){
	if(inTri2D(t3->p1, t3->p2, t3->p3, cur->e)){ 
	  // Don't calc maxE for nodata points
	  //if(cur->e[Z] != tin->nodata){
	    Q_insert_qnode_head(t3->points,cur);
	    tempE = findError(cur->e[X], cur->e[Y], cur->e[Z], t3);
	    // Update max error
	    if (tempE>=max3) {
	      max3 = tempE;
	      t3->maxE = cur->e;
	      t3->maxErrorValue = tempE;
	    }  
	    continue;
	    //}
	  // Add a nodata point to the triangle.
	  /* else{ */
/* 	    // If point is already added to another triangle (on edge) */
/* 	    // then we need to use insert_elem_head */
/* 	    if(pointAdded){ */
/* 	      Q_insert_elem_head(t3->points,cur->e); */
/* 	      //printf("Double added %d,%d,%d\n",cur->e[X],cur->e[Y],cur->e[Z]); */
/* 	    } */
/* 	    else{ */
/* 	      Q_insert_qnode_head(t3->points,cur); */
/* 	    } */
/* 	    hasNoData3 = 1; */
/* 	  } */
/* 	  continue;  */
	}
      }

      //should never get here if point is not nodata
      if(cur->e[Z] != tin->nodata){
	assert(0);
	exit(1);
      }

    }//while

    // If we have an sp, go through and distribute points for sp
    if(sp != NULL){
      s = sp;
      h = s->points;
      sp = NULL;
    }
    // No sp? then we are done distributing the points
    else
      doneDistr = 1;

  }//while

  // Free any unused point lists
  if(t1 != NULL && t1->maxE == (COORD*)DONE){
    // If all the points in t1 are nodata and there are points in t1
    // then this triangle should be marked to not be drawn
    if(hasNoData1){
      //allNoData1 == 1 && Q_first(t1->points) != NULL
      //Q_free_queue(t1->points);
      //t1->points = NULL;
    }
    else{
      //Q_free_queue(t1->points);
      //t1->points = NULL;
    }
  }
  else if(t1 != NULL){
    PQ_insert(pq,t1);
  }

  if(t2 != NULL && t2->maxE == (COORD*)DONE){
    // If all the points in t1 are nodata and there are points in t1
    // then this triangle should be marked to not be drawn
    if(hasNoData2){
      //Q_free_queue(t2->points);
      //t2->points = NULL;
    }
    else{
      //Q_free_queue(t2->points);
      //t2->points = NULL;
    }
  }
  else if(t2 != NULL){
    PQ_insert(pq,t2);
  }

  if(t3 != NULL && t3->maxE == (COORD*)DONE){ 
    // If all the points in t1 are nodata and there are points in t1
    // then this triangle should be marked to not be drawn
    if(hasNoData3){
      //Q_free_queue(t3->points);
      //t3->points = NULL;
    }
    else{
      //Q_free_queue(t3->points);
      //t3->points = NULL;
    }
  }
  else if(t3 != NULL){    
    PQ_insert(pq,t3);
  }
}


//
// This function is called when we are refining the lower left most
// triangle since we need to choose the new lower left triangle
//
void updateTinCorner(TIN *tin, TRIANGLE* t1, TRIANGLE* t2, TRIANGLE* t3){
  if(t1 != NULL){
    if(t1->p1[Y]==0 && t1->p2[Y]==0 && t1->p1[X]==tin->nrows-1){
      tin->t = t1;
      tin->v = t1->p1;
      tin->e.p1 = t1->p1;
      tin->e.p2 = t1->p2;
    }
    else if(t1->p1[Y]==0 && t1->p2[Y]==0 && t1->p2[X]==tin->nrows-1){
      tin->t = t1;
      tin->v = t1->p2;
      tin->e.p1 = t1->p1;
      tin->e.p2 = t1->p2;
    }
    else if(t1->p1[Y]==0 && t1->p3[Y]==0 && t1->p1[X]==tin->nrows-1){
      tin->t = t1;
      tin->v = t1->p1;
      tin->e.p1 = t1->p1;
      tin->e.p2 = t1->p3;
    }
    else if(t1->p1[Y]==0 && t1->p3[Y]==0 && t1->p3[X]==tin->nrows-1){
      tin->t = t1;
      tin->v = t1->p3;
      tin->e.p1 = t1->p1;
      tin->e.p2 = t1->p3;
    }
    else if(t1->p2[Y]==0 && t1->p3[Y]==0 && t1->p2[X]==tin->nrows-1){
      tin->t = t1;
      tin->v = t1->p2;
      tin->e.p1 = t1->p2;
      tin->e.p2 = t1->p3;
    }
    else if(t1->p2[Y]==0 && t1->p3[Y]==0 && t1->p3[X]==tin->nrows-1){
      tin->t = t1;
      tin->v = t1->p3;
      tin->e.p1 = t1->p2;
      tin->e.p2 = t1->p3;
    }
  }
  if(t2 != NULL){
    if(t2->p1[Y]==0 && t2->p2[Y]==0 && t2->p1[X]==tin->nrows-1){
      tin->t = t2;
      tin->v = t2->p1;
      tin->e.p1 = t2->p1;
      tin->e.p2 = t2->p2;
    }
    else if(t2->p1[Y]==0 && t2->p2[Y]==0 && t2->p2[X]==tin->nrows-1){
      tin->t = t2;
      tin->v = t2->p2;
      tin->e.p1 = t2->p1;
      tin->e.p2 = t2->p2;
    }
    else if(t2->p1[Y]==0 && t2->p3[Y]==0 && t2->p1[X]==tin->nrows-1){
      tin->t = t2;
      tin->v = t2->p1;
      tin->e.p1 = t2->p1;
      tin->e.p2 = t2->p3;
    }
    else if(t2->p1[Y]==0 && t2->p3[Y]==0 && t2->p3[X]==tin->nrows-1){
      tin->t = t2;
      tin->v = t2->p3;
      tin->e.p1 = t2->p1;
      tin->e.p2 = t2->p3;
    }
    else if(t2->p2[Y]==0 && t2->p3[Y]==0 && t2->p2[X]==tin->nrows-1){
      tin->t = t2;
      tin->v = t2->p2;
      tin->e.p1 = t2->p2;
      tin->e.p2 = t2->p3;
    }
    else if(t2->p2[Y]==0 && t2->p3[Y]==0 && t2->p3[X]==tin->nrows-1){
      tin->t = t2;
      tin->v = t2->p3;
      tin->e.p1 = t2->p2;
      tin->e.p2 = t2->p3;
    }
  }
  if(t3 != NULL){
    if(t3->p1[Y]==0 && t3->p2[Y]==0 && t3->p1[X]==tin->nrows-1){
      tin->t = t3;
      tin->v = t3->p1;
      tin->e.p1 = t3->p1;
      tin->e.p2 = t3->p2;
    }
    else if(t3->p1[Y]==0 && t3->p2[Y]==0 && t3->p2[X]==tin->nrows-1){
      tin->t = t3;
      tin->v = t3->p2;
      tin->e.p1 = t3->p1;
      tin->e.p2 = t3->p2;
    }
    else if(t3->p1[Y]==0 && t3->p3[Y]==0 && t3->p1[X]==tin->nrows-1){
      tin->t = t3;
      tin->v = t3->p1;
      tin->e.p1 = t3->p1;
      tin->e.p2 = t3->p3;
    }
    else if(t3->p1[Y]==0 && t3->p3[Y]==0 && t3->p3[X]==tin->nrows-1){
      tin->t = t3;
      tin->v = t3->p3;
      tin->e.p1 = t3->p1;
      tin->e.p2 = t3->p3;
    }
    else if(t3->p2[Y]==0 && t3->p3[Y]==0 && t3->p2[X]==tin->nrows-1){
      tin->t = t3;
      tin->v = t3->p2;
      tin->e.p1 = t3->p2;
      tin->e.p2 = t3->p3;
    }
    else if(t3->p2[Y]==0 && t3->p3[Y]==0 && t3->p3[X]==tin->nrows-1){
      tin->t = t3;
      tin->v = t3->p3;
      tin->e.p1 = t3->p2;
      tin->e.p2 = t3->p3;
    }
  }
  DEBUG{printf("Change LLC: (%d,%d)(%d,%d)(%d,%d) \n",
	       tin->t->p1[X],tin->t->p1[Y],tin->t->p2[X],tin->t->p2[Y],
	       tin->t->p3[X],tin->t->p3[Y]);}
}