/* Brian O'Connor
 *
 * cell.c: Read in the environment and decompose it into a set of
 * convex polygonal cells.
 */

#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include "pursuer.h"
#include "input.h"
#include "cell.h"
#include "line.h"
#include "rebuild.h"

#define INIT_DIST 0.001 /* not zero, but smaller than any real distance */

void ExtendRays(WorldInfo *wInfo, EdgeInfo *edges, EdgeInfo *rays);
void MakeEdges(WorldInfo *wInfo, EdgeInfo *eInfo);
void MakeCellEdges(WorldInfo *wInfo, EdgeInfo *rays, CellInfo *cInfo);
void ExtendOneRay(Point *p1, Point *p2, EdgeInfo *e, EdgeInfo *rays,
		Obstacle *o1, Obstacle *o2, Obstacle *border);
void AddRay(Point *p1, Point *p2, EdgeInfo *rays);
void DumpRayInfo(EdgeInfo *rays);
void DumpCellInfo(CellInfo *cells);
//int NotEdgeTest(Obstacle *ob, Point *p1, Point *p2);
void InsertPoint(Point *p, double dist, Point **ptArr, int *nPts);	
void RemoveDuplicates(Point **ptArr, int *nPts);
int NextClockwiseEdge(CellInfo *cellInfo, Point *p, int i);
void SortEdges(CellEdge **edges, int nEdges);
void MakeCells(WorldInfo *wInfo, CellInfo *cells);
int IsCellObstacle(WorldInfo *wInfo, Cell *cell, int *bBorder);
Point *CellCenter(Cell *cell);
void CollapseRays(EdgeInfo *rays);
void RemoveRay(EdgeInfo *rays, int i);
int NotEdgeTest(Point *p1, Point *p2, Obstacle *ob, Line *l1a, Line *l1b, 
		Line *l2a, Line *l2b, int bBorder);
Line *FindEdgeB(EdgeInfo *edges, Point *p);



void BuildDecomposition(char *file, PInfo *pInfo) 
{
  EdgeInfo *rays;
  int i;
  
  pInfo->wInfo = new WorldInfo;
  pInfo->edgeInfo = new EdgeInfo;
  rays = new EdgeInfo;
  
  /*		DotProductTest(); */ 
  ReadInputFile(file, pInfo->wInfo);
  MakeEdges(pInfo->wInfo, pInfo->edgeInfo);
  ExtendRays(pInfo->wInfo, pInfo->edgeInfo, rays);
  //DumpRayInfo(rays);
  CollapseRays(rays);
  //DumpRayInfo(rays);
  MakeCellEdges(pInfo->wInfo, rays, pInfo->cellInfo);
  //DumpCellInfo(pInfo->cellInfo); 
  SortEdges(pInfo->cellInfo->edges, pInfo->cellInfo->nEdges);
  //DumpCellInfo(pInfo->cellInfo); 
  MakeCells(pInfo->wInfo, pInfo->cellInfo); 
  /*for(i = 0; i < pInfo->cellInfo->nCells; i++) {
    printf("Num edges: %d\n", pInfo->cellInfo->cells[i]->nEdges);
  }*/

  delete rays;
}

/* for multiple pursuers: build a new decomposition given the old one
 * which will also tell us what cell to start the new decomposition
 * at. */
void RebuildDecomposition(PInfo *pInfo1, PInfo *pInfo)
{
  EdgeInfo *rays;
  int i;
  
  pInfo->wInfo = new WorldInfo;
  pInfo->edgeInfo = new EdgeInfo;
  rays = new EdgeInfo;

  RebuildInput(pInfo1, pInfo->wInfo);
  MakeEdges(pInfo->wInfo, pInfo->edgeInfo);
  ExtendRays(pInfo->wInfo, pInfo->edgeInfo, rays);
  //DumpRayInfo(rays);
  CollapseRays(rays);
  //DumpRayInfo(rays);
  MakeCellEdges(pInfo->wInfo, rays, pInfo->cellInfo);
  //DumpCellInfo(pInfo->cellInfo); 
  SortEdges(pInfo->cellInfo->edges, pInfo->cellInfo->nEdges);
  //DumpCellInfo(pInfo->cellInfo); 
  MakeCells(pInfo->wInfo, pInfo->cellInfo); 
  /*for(i = 0; i < pInfo->cellInfo->nCells; i++) {
    printf("Num edges: %d\n", pInfo->cellInfo->cells[i]->nEdges);
  }*/

  delete rays;

}

void MakeEdges(WorldInfo *wInfo, EdgeInfo *eInfo)
{
  int nEdges, currEdge;
  int i,j;
  
  /* count the number of edges we will need */
  nEdges = wInfo->border->nPts;
  for( i = 0; i < wInfo->nObs; i++ ) {
    nEdges += wInfo->obs[i]->nPts;
  }
  eInfo->edges = new Line *[nEdges];
  eInfo->nEdges = nEdges;
  
  /* now read in the edges */
  currEdge = 0;
  for( i = 0; i < wInfo->border->nPts; i++ ) {
    eInfo->edges[currEdge] = PointsToLine(&wInfo->border->pts[i],
		  (&wInfo->border->pts[(i + 1)% wInfo->border->nPts]) );
    eInfo->edges[currEdge]->ob = wInfo->border;
    currEdge++;
  }
  eInfo->firstEdge = 0;
  
  for( i = 0; i < wInfo->nObs; i++ ) {
    for(j = 0; j < wInfo->obs[i]->nPts; j++ ) {
      eInfo->edges[currEdge] = PointsToLine(&wInfo->obs[i]->pts[j],
		    &wInfo->obs[i]->pts[(j + 1) % wInfo->obs[i]->nPts] );
      eInfo->edges[currEdge]->ob = wInfo->obs[i];
      currEdge++;
    }
  }
  assert( currEdge <= eInfo->nEdges );
}

/* Given a list of edges, extend them using the algorithm on the web
 * into the segments that will partition the cells of our decomposition.
 */
void ExtendRays(WorldInfo *wInfo, EdgeInfo *edges, EdgeInfo *rays)
{
  
  int i,j, bBorder;
  int sameOb;
  Obstacle *ob;
  
  i = j = 0;
  /* allocate for maximum O(N^2) edges */
  rays->nEdges = 0;
  rays->firstEdge = 0;
  rays->edges = new(Line *)[(edges->nEdges - i) * (edges->nEdges - i)];
  
  /* loop over all vertex pairs */
  for( i = edges->firstEdge; i < edges->nEdges; i++ ) {
    for( j = i + 1; j < edges->nEdges; j++ ) {
      sameOb = (edges->edges[i]->ob == edges->edges[j]->ob);
      ob = edges->edges[i]->ob;
      if( ob == wInfo->border ) bBorder = 1; // flag for NotEdgeTest
      else bBorder = 0;
      /*if(!sameOb || NotEdgeTest(edges->edges[j]->ob, 
				edges->edges[i]->A, edges->edges[j]->A) )
				{*/
        if( !sameOb || NotEdgeTest(edges->edges[i]->A,
				   edges->edges[j]->A,
                           ob,
			   edges->edges[i],
			   FindEdgeB(edges, edges->edges[i]->A),
			   edges->edges[j],
			   FindEdgeB(edges, edges->edges[j]->A), bBorder )
	  ) {

	ExtendOneRay(edges->edges[i]->A, edges->edges[j]->A, edges, rays,
		     edges->edges[i]->ob, edges->edges[j]->ob, wInfo->border);
	if( rays->nEdges > 25 ) {
	  //printf("In testcase\n");
	}
      }
    }
  }
}

/* add one or two new rays to the list, with new endpoints 
 * equal to the furthest extent of the ray.
 * sameOb == TRUE if p1 and p2 are part of the same obstacle
 */
void ExtendOneRay(Point *p1, Point *p2, EdgeInfo *e, EdgeInfo *rays,
	Obstacle *o1, Obstacle *o2, Obstacle *border)
{
  Line *l, *l2;
  int i, sameOb;
  int p1vis, p2vis;
  int visible = TRUE;
  double minDist, maxDist, currDist;
  /* because the points we create are saved in the rays structure,
   * we need to dynamically allocate memory for them each time
   * we create a new ray. */
  Point* minPt, *maxPt, *currPt;
  
  /* initialize fields */
  sameOb = ( (o1 == o2) && IsPointAdjacent(o1, p1, p2));
  minDist = -INFINITY;
  maxDist = INFINITY;
  p1vis = p2vis = TRUE;
  
  l = PointsToLine(p1, p2);
  l->ob = NULL;

  if( p1->x == 175 && p1->y == 100 && p2->x == 275 && p2->y == 200 ) {
    //printf("In testcase\n");
  }
  
  /* new: perform OpenEdgeTest (could move up 1 function too) */
  if(!sameOb && 
     (!OpenEdgeTest(o2, p1, p2) || !OpenEdgeTest(o1, p2, p1)) ) {
       
    p1vis = FALSE; 
    p2vis = FALSE;
    return;
  }

  minPt = NULL; maxPt = NULL;
  currPt = NewPoint();
  
  /*printf(" (%f, %f) to (%f, %f)\n", p1->x, p1->y, p2->x, p2->y); */
  for( i = 0; i < e->nEdges; i++ ) {
    l2 = e->edges[i];
    if( (l2->A == p1 && l2->B == p2) ||
	(l2->B == p1 && l2->A == p2) ) continue;
    
    LineIntersectionDistance(l, l2, currPt, &currDist);
    if( !PointOnSegment(l2, currPt) ) currDist = INFINITY;
    if( currDist == INFINITY ) continue;
    if( FloatCompare(currDist, 0.0) > 0 &&
	FloatCompare(currDist, l->length) < 0 ) {
      visible = FALSE;
      return;
    } else if( FloatCompare(currDist, 0.0) < 0 &&
	       FloatCompare(currDist, minDist) > 0 ) {
      /* if minPt/maxPt is on same ob as p1, p2 then make sure
       * we didn't extend the ray through an obstacle. */
      if(/*!sameOb ||*/ /*l2->ob == border || */
	 l2->ob != o1 ||
	 !InsideTest(l2, p1, currPt)) {
	minDist = currDist;
	if( minPt != NULL ) delete minPt;
	minPt = currPt;
	currPt = NewPoint();
      } else {
	if( minPt != NULL ) delete minPt;
	minPt = p1;
	minDist = currDist;
	//minDist = INFINITY; 
	/* possible bug: to fix eagle test, needed to remove suppression
	   code (setting dists to INFINITY).  This may cause bugs in
	   other examples however. Same change made below in max code.*/
      }
    } else if( FloatCompare(currDist, l->length) > 0 &&
	       FloatCompare(currDist, maxDist) < 0 ) {
      if(/*!sameOb ||*/ /* l2->ob == border || */
	 l2->ob != o2 ||
	 !InsideTest(l2, currPt, p2)) {
	maxDist = currDist;
	if( maxPt != NULL ) delete maxPt;
	maxPt = currPt;
	currPt = NewPoint();
      } else {
	if( maxPt != NULL ) delete maxPt;
	maxPt = p2;
	maxDist = currDist;
	//	maxDist = -INFINITY;
      }
    }
  }
  
  /* make rays out of the data we've found */
  if( minPt == NULL ) minPt = p1;
  if( maxPt == NULL ) maxPt = p2;
  
  /* this assert isn't always true, but it is for convex obstacles */
  /* ray rules:
   * 1) same obstacle: final = (minPt, maxPt)
   * 2) diff. obstacle: final = (minPt, p1) and (p2, maxPt) */
  if( sameOb && visible ) {
    AddRay(minPt, maxPt, rays);
  } else if ( visible ) {
    /* right now, p1vis & p2vis will have the same value, but this
       may need to change later. */
    if( p1vis && !PointEqual(minPt, p1) ) {
      AddRay(minPt, p1, rays);
    }
    if( p2vis && !PointEqual(maxPt, p2) ) {
      AddRay(p2, maxPt, rays);
    }
  }
  
  /* cleanup */
  if( currPt != minPt && currPt != maxPt ) delete currPt;
}

void AddRay(Point *p1, Point *p2, EdgeInfo *rays)
{
  /*printf("Adding edge %d\n", rays->nEdges); */
  rays->edges[rays->nEdges++] = PointsToLine(p1, p2);
}

void DumpRayInfo(EdgeInfo *rays)
{
  int i;
  for( i = 0; i < rays->nEdges; i++ ) {
    printf("Ray %d: (%f, %f) to (%f, %f)\n", i,
	   rays->edges[i]->A->x, rays->edges[i]->A->y,
	   rays->edges[i]->B->x, rays->edges[i]->B->y);
  }
}

void DumpCellInfo(CellInfo *cells)
{
  int i;
  for( i = 0; i < cells->nEdges; i++ ) {
    printf("Cell Edge %d: (%f, %f) to (%f, %f)\n", i,
	   cells->edges[i]->A->x, cells->edges[i]->A->y,
	   cells->edges[i]->B->x, cells->edges[i]->B->y);
  }
}

#define DISPLACEMENT 1.1

int OpenEdgeTest(Obstacle *ob, Point *p1, Point *p2)
{
  /* if (p1, p2) is extended by delta in p2's direction, is this point
   * inside obstacle ob (the obstacle p2 is part of) */
  
  float delta = DISPLACEMENT;
  double dot1, dot2;
  int i;
  Point newPt;
  Line *l;
  newPt.x = p1->x + (p2->x - p1->x) * delta;
  newPt.y = p1->y + (p2->y - p1->y) * delta;
  newPt.w = 1;
  
  /* loop over edges in the obstacle, ignoring edges p2 is not part of.
     if the dot product sign differs for the two edges p2 is on, then
     the line connecting p1 to p2 is open in that direction.  Otherwise,
     the line connecting these points is closed.
     
     Right now I don't make a distinction between being on the 
     positive side or the negative side of an obstacle because I want
     to avoid dealing with directed edges.  This may have to change.
     */
  
  for( i = 0; i < ob->nPts; i++ ) {
    if( &ob->pts[i] == p2 ) break;
  }
  assert( i < ob->nPts );
  l = PointsToLine(&ob->pts[i], &ob->pts[( (i + 1) % ob->nPts)]);
  dot1 = l->a * newPt.w + l->b * newPt.x + l->c * newPt.y;
  delete l;
  
  l = PointsToLine(&ob->pts[( (i - 1 + ob->nPts) % ob->nPts)],
		   &ob->pts[i]);
  dot2 = l->a * newPt.w + l->b * newPt.x + l->c * newPt.y;
  delete l;
	
  /* what to do if a dot product = 0 (ie colinear?) */
  if( (FloatCompare(dot1, 0.0) > 0 && FloatCompare(dot2, 0.0) > 0) ||
      (FloatCompare(dot1, 0.0) < 0 && FloatCompare(dot2, 0.0) < 0) ) {
    return(FALSE); /* ray is blocked */
  } else return(TRUE); /* ray is clear */
}

/* like NotEdgeTest but works for concave obstacles */
int InsideTest(Line *l, Point *p1, Point *p2)
{
  Point newPt;
  newPt.x = (p1->x + p2->x) / 2;
  newPt.y = (p1->y + p2->y) / 2;
  newPt.w = 1;

  if(FloatCompare(l->a * newPt.w + l->b * newPt.x + l->c * newPt.y,
		  0.0) > 0 ) {
    return(FALSE);
  } else {
    return(TRUE);
  }
}

/* Majorly overhauled this function: 
 * now it checks the corner near each point and sees if the other point is
 * 'inside' in relation to this corner, meaning the the dot product is
 * positive at this corner for both points.
 */

int NotEdgeTest(Point *p1, Point *p2, Obstacle *ob, Line *l1a, Line *l1b, 
		Line *l2a, Line *l2b, int bBorder)
{
  double lastSide, currSide;
  int t1, t2, t3, t4;
  double d1, d2, d3, d4;
  int i;
  Line *l;
  Point pMid;

  if( bBorder ) { // border, use inside testing 
    d1 = l1a->a * p2->w + l1a->b * p2->x + l1a->c * p2->y;  
    d2 = l1b->a * p2->w + l1b->b * p2->x + l1b->c * p2->y;  
    d3 = l2a->a * p1->w + l2a->b * p1->x + l2a->c * p1->y;  
    d4 = l2b->a * p1->w + l2b->b * p1->x + l2b->c * p1->y;  

    t1 = FloatCompare(d1,0); 
    t2 = FloatCompare(d2,0);  
    t3 = FloatCompare(d3,0); 
    t4 = FloatCompare(d4,0); 

    if( ((t1 >= 0) && (t2 >= 0)) && ((t3 >= 0) && (t4 >= 0)) ) {
      return(TRUE);
    }
  }

  assert(ob != NULL);

  pMid.x = (p1->x + p2->x) / 2;
  pMid.y = (p1->y + p2->y) / 2;
  pMid.w = 1;
  assert( FloatEqual(p1->w,1) && FloatEqual(p2->w,1) );
  
  lastSide = 0;
  for( i = 0; i < ob->nPts; i++ ) {
    l = PointsToLine(&ob->pts[i], &ob->pts[(i + 1) % ob->nPts] );
    currSide = l->a * pMid.w + l->b * pMid.x + l->c * pMid.y;
    if( (PointEqual(l->A, p1) && PointEqual(l->B, p2) ) ||
	(PointEqual(l->B, p1) && PointEqual(l->A, p2) ) ) {
      delete l;
      return(TRUE);
    }
    delete l;
    if( FloatEqual(currSide, 0.0) ) {
      /* close enough to be on an edge */
      return(TRUE);
    } else if( FloatCompare(lastSide, 0.0) <= 0 &&
	       FloatCompare(currSide, 0.0) < 0 ) {
      lastSide = -1;
    } else if( FloatCompare(lastSide, 0.0) >= 0 &&
	       FloatCompare(currSide, 0.0) > 0 ) {
      lastSide = 1;
      /*} else if( currSide != 0 ) {
	return(TRUE); */	
    } else {
      return(TRUE); /* even if currSide = 0 */
    }
  }

  /* if we get this far, we are inside the obstacle. */
  return(FALSE);
}

int IsPointAdjacent(Obstacle *ob, Point *p1, Point *p2)
{
  int i;
  for(i = 0; i < ob->nPts; i++ ) {
    if( &ob->pts[i] == p1 )
      if( &(ob->pts[(i + 1) % ob->nPts]) == p2 )
	return(TRUE);
    if( &ob->pts[i] == p2 )
      if( &(ob->pts[(i + 1) % ob->nPts]) == p1 )
	return(TRUE);
  }
  return(FALSE);
}


/* convert rays into cell Edges by finding every possible intersection 
   and separating each edge at each intersection.  Also we introduce
   two edges for each original edge (one in each direction) because each
   edge will be used as a border for 2 cells (1 clockwise, 1 
   counterclockwise.) */
void MakeCellEdges(WorldInfo *wInfo, EdgeInfo *rays, CellInfo *cInfo)
{
  int i,j;
  
  double dist;
  Point *currPt;
  CellEdge *currEdge;
  CellEdge *currEdgeDual;
  int nPts; Point **ptArr;

  nPts = 0;
  ptArr = new (Point *)[MAX_INTERSECTIONS];

  cInfo->nEdges = 0;
  cInfo->edges = new (CellEdge *)[4 * MAX_INTERSECTIONS * MAX_OBS];

  currPt = NewPoint();
	
  for( i = 0; i < rays->nEdges; i++ ) {
    nPts = 0;
    for( j = 0; j < rays->nEdges; j++ ) {
      if( i == j ) continue;
      LineIntersectionDistance(rays->edges[i], 
			       rays->edges[j], currPt, &dist);
      if( !PointOnSegment(rays->edges[i], currPt))
	continue;
      InsertPoint(currPt, dist, ptArr, &nPts);	
      currPt = NewPoint();
    }
    /* now make edges out of point list */
    
    RemoveDuplicates(ptArr, &nPts);
    for( j = 0; j < nPts - 1; j++ ) {
      currEdge = new CellEdge(ptArr[j], ptArr[j+1]);	
      currEdgeDual = new CellEdge(ptArr[j+1],
				  ptArr[j]);
      currEdge->dual = currEdgeDual;
      currEdgeDual->dual = currEdge;
      currEdge->ob = currEdgeDual->ob = NULL;
      currEdge->cell = currEdge->dualCell = NULL;
      currEdgeDual->cell = currEdgeDual->dualCell = NULL;
      cInfo->edges[cInfo->nEdges++] = currEdgeDual;
      
      cInfo->edges[cInfo->nEdges++] = currEdge;
    }
  }
  delete ptArr;
}

void InsertPoint(Point *p, double dist, Point **ptArr, int *nPts)	
{
  int i;

  p->w = dist; /* save storage space */
  for(i = *nPts; i > 0; i-- ) {
    if( FloatCompare(p->w, ptArr[i - 1]->w) > 0 ) {
      ptArr[i] = p;
      (*nPts)++;
      return;
    }
    ptArr[i] = ptArr[i - 1];
  }
  ptArr[0] = p;
  (*nPts)++;
}

void RemoveDuplicates(Point **ptArr, int *nPts)
{
  int i, j;
  for(i = 0; i < (*nPts); i++) {
    while( i + 1 < (*nPts) && FloatEqual(ptArr[i]->w, 
					 ptArr[i + 1]->w) ) {
      for( j = i + 1; j < (*nPts) - 1; j++ ) {
	ptArr[j] = ptArr[j + 1];
      }
      (*nPts)--;
    }
  }
  for( i = 0; i < (*nPts); i++ ) {
    ptArr[i]->w = 1;
  }
}

void SortEdges(CellEdge **edges, int nEdges)
{
  int minElem;
  CellEdge *temp;
  int i, j;
  for(i = 0; i < nEdges; i++) {
    VerifyLine(edges[i]);
    minElem = i;
    for( j = i; j < nEdges; j++) {
      if( FloatCompare(edges[j]->A->x,
		       edges[minElem]->A->x) > 0 ) continue;
      if( FloatEqual(edges[j]->A->x,
		     edges[minElem]->A->x) &&
	  FloatCompare(edges[j]->A->y,
		       edges[minElem]->A->y) > 0 ) continue;
      minElem = j;
    }	
    temp = edges[i];
    edges[i] = edges[minElem];
    edges[minElem] = temp;
  }
}

void RemoveEdge(CellInfo *cells, int i)
{
  int j;
  assert( i < cells->nEdges );
  for(j = i; j < cells->nEdges - 1; j++) {
    cells->edges[j] = cells->edges[j + 1]; 
  }
  cells->nEdges--;
}

void MakeCells(WorldInfo *wInfo, CellInfo *cellInfo)
{
  int i, j;
  int iList[CELL_INTS];
  Cell *c;
  int bRemoved,bBorder;
  
  cellInfo->cells = new (Cell *)[CELL_INTS];
  cellInfo->nCells = 0;
  bRemoved = FALSE;

  while(cellInfo->nEdges > 0) {
    i = 0;
    cellInfo->cells[cellInfo->nCells++] = new Cell;
    c = cellInfo->cells[cellInfo->nCells - 1];
    
    c->edges = new (CellEdge *)[CELL_INTS];
    c->nEdges = 1;
    c->edges[0] = cellInfo->edges[0];
    iList[0] = i /* = 0 */;
    
    while(TRUE) {
      assert(c->nEdges < CELL_INTS);
      /*printf("Edge: (%f, %f), (%f, %f)\n",
	cellInfo->edges[i]->A->x,
	cellInfo->edges[i]->A->y,
	cellInfo->edges[i]->B->x,
	cellInfo->edges[i]->B->y);*/ 
      i = NextClockwiseEdge(cellInfo,
			    cellInfo->edges[i]->B, i);
      if( i == 0 ) break;
      iList[c->nEdges++] = i;
    }
    for( i = 0; i < c->nEdges; i++ ) {
      assert(iList[i] < cellInfo->nEdges);
      c->edges[i] = cellInfo->edges[iList[i]];
      c->edges[i]->cell = c;
      c->edges[i]->dual->dualCell = c;
      RemoveEdge(cellInfo, iList[i]);
      for( j = i; j < c->nEdges; j++ ) {
	if( iList[j] > iList[i] ) {
	  iList[j]--;
	}
      }
    }
    /* if the cell we just made is actually the border, omit it. */
    /* this will cause problems if we ever get a world that can
       be solved with only 1 cell, but that isn't a very useful
       case so ignoring it is OK. */
    if(IsCellObstacle(wInfo, c, &bBorder) ) {
      if( bBorder && !bRemoved ) {
        bRemoved = TRUE;
      } else if ( bBorder && bRemoved ) {
        c->pt = CellCenter(c);
        continue;
      }
      cellInfo->nCells--; /* delete this cell */
      for( i = 0; i < c->nEdges; i++ ) {
	c->edges[i]->dual->dualCell = NULL;
	c->edges[i] = NULL;
      }
      delete c->edges;
      delete c;
    } else {
      c->pt = CellCenter(c);
    }
  }
}

int NextClockwiseEdge(CellInfo *cellInfo, Point *p, int i)
{
  int j, k;
  CellEdge *e;
  int bestEdge;
  float angle, bestAngle = -INFINITY;
  
  for( j = 0; j < cellInfo->nEdges; j++ ) {
    e = cellInfo->edges[j];
    if( PointEqual(e->A, p) ) break;
  }
	
  if( j >= cellInfo->nEdges ) {
    printf("NextClockwiseEdge: no compatible edge found.\n");
    assert(0);
  }

  /* now iterate over all edges with this endpoint & find one
     w. lowest dot product. */
  for( k = j; k < cellInfo->nEdges && 
	 PointEqual(e->A, p);k++ ) {
    if( i == k ) continue;
    e = cellInfo->edges[k];
    if( !PointEqual(e->A, p) ) break;
    angle = LineAngle(cellInfo->edges[i], e);
    if( angle > bestAngle && angle < 180) {
      bestAngle = angle;
      bestEdge = k;
    }
  }

  if( bestAngle == -INFINITY || bestAngle >= 180 ||
      bestAngle <= -180 ) {
    printf("NextClockwiseEdge: no compatible edge found. \n");
    assert(0);
  }
  return( bestEdge );
}

/* returns TRUE if every edge in this cell corresponds to an edge in
   either the border or an obstacle. */
int IsCellObstacle(WorldInfo *wInfo, Cell *cell, int *bBorder)
{
  int i,j,k, bOK, bSame;
  Point *p1, *p2;
  Obstacle *border = wInfo->border;
  *bBorder = FALSE;

  /* a hack to catch implausible obstacles */
  if( cell->nEdges > 20 ) {
    /*printf("Deleting illegal cell\n"); */
    return(TRUE); 
  }

  for( i = 0; i < border->nPts; i++) {
    bOK = FALSE;
    for( j = 0; !bOK && j < cell->nEdges; j++ ) {
      if( PointEqual(cell->edges[j]->A,
		     &border->pts[i]) ) {
        *bBorder = TRUE;
	bOK = TRUE;
      }
    }
    if( !bOK ) break;
  }	
  if(bOK) return TRUE;

  /* now check obstacles */
  for( k = 0; k < wInfo->nObs; k++ ) {
    border = wInfo->obs[k];
    for( i = 0; i < border->nPts; i++ ) {
      bOK = FALSE;
      for( j = 0; !bOK && j < cell->nEdges; j++ ) {
	if( PointEqual(cell->edges[j]->A,
		       &border->pts[i]) ) {
	  bOK = TRUE;
	}
      }
      if( !bOK ) break;
    }
    if( bOK ) return TRUE;
  }

  return(FALSE);
}

/* use vertex removal to reduce cell to a line or triangle; then interpolate 
 * to find center. */
Point *CellCenter(Cell *cell)
{
  int i,j;
  Point *p;
  double x, y;
  
  x = y = 0;
  for( i = 0; i < cell->nEdges; i++ ) {
    x += cell->edges[i]->A->x;
    y += cell->edges[i]->A->y;
  }
  p = NewPoint();
  p->x = x / cell->nEdges;
  p->y = y / cell->nEdges;
  return(p);
}

/* function that removes useless rays that will appear whenever we have
 * singularities in our environment. Does a N^2 pass through the rays
 * and collapses any two rays that are colinear and overlapping into
 * a single ray.  The new ray replaces the first of the two in the array
 * and the second is deleted. 
 */

void CollapseRays(EdgeInfo *rays)
{
  int i,j;
  Point *pmin, *pmax;
  for( i = 0; i < rays->nEdges; i++ ) {
    for( j = 0; j < rays->nEdges; j++ ) {
      if( i == j ) continue;
      if( LinesOverlap(rays->edges[i], rays->edges[j], &pmin, &pmax) ) {
	/*1) make new ray from pmin to pmax
	  2) delete old rays i & j
	  3) insert new ray from pmin to pmax. */
	if( i < j ) {
	  delete rays->edges[j];
	  rays->edges[j] = new Line(pmin, pmax);
	  RemoveRay(rays, i);
	} else {
	  delete rays->edges[i];
	  rays->edges[i] = new Line(pmin, pmax);
	  RemoveRay(rays, j);
	}
      }
    }
  }
}

void RemoveRay(EdgeInfo *rays, int i)
{
  int j;
  assert( i < rays->nEdges );
  delete rays->edges[i];
  for(j = i; j < rays->nEdges - 1; j++) {
    rays->edges[j] = rays->edges[j + 1]; 
  }
  rays->nEdges--;
}

void RemoveCell(CellInfo *cells, int i)
{
  int j, k;
  assert( i < cells->nCells );
  for( k = 0; k < cells->cells[i]->nEdges; k++ ) {
    // invalidate dual information.
    cells->cells[i]->edges[k]->dual->dual = NULL;
    cells->cells[i]->edges[k]->dual->dualCell = NULL;
  }
  for(j = i; j < cells->nCells - 1; j++) {
    cells->cells[j] = cells->cells[j + 1]; 
  }
  cells->nCells--;
}

EdgeInfo::~EdgeInfo() {
  int i;
  for(i = 0; i < nEdges; i++) {
    delete edges[i];
  }
  delete edges;
}

/* returns the line which has p as it's endpoint B. */
Line *FindEdgeB(EdgeInfo *edges, Point *p)
{
  int i;
  for( i = 0; i < edges->nEdges; i++ ) {
    if(PointEqual(edges->edges[i]->B, p) ) {
      return(edges->edges[i]);
    }
  }
  assert(0); // should always find something
  return(NULL);
}