#include "grf4.h"
#include "grf4Internal.h"
#include "../h/cdefs.h"

#if HLS

#define EPSILON	(1e-6)

#define BASE_SIZE	10000

static int maxPevents;
static int maxVectors = 0;
static int maxEdges = 0;
static int maxPolygons = 0;

static int numPevents;
static int numVectors;
static int numEdges;
static int numPolygons;
		
static PEVENT *pevents;
static VECTOR *vectors;
static PEDGE *edges;
static SCR_POLYGON *polygons;

static float scrEpsilon = EPSILON;

setScrEpsilon(w,h)
float w, h;
{
	scrEpsilon = EPSILON*(w+h)/2;
}

PEVENT* newPevent(type,r)
int type;
float r;
{
	PEVENT *ev;
	if (numPevents >= maxPevents)
	 { if (maxPevents == 0)
	    { maxPevents = BASE_SIZE;
	      pevents = (PEVENT*)malloc(sizeof(PEVENT)*maxPevents);
	    }
	   else
	    { maxPevents *= 2;
	      pevents = (PEVENT*)realloc((char*)pevents,
					 sizeof(PEVENT)*maxPevents);
	    }
	 }
	ev = &pevents[numPevents++];
	ev->type = type;
	ev->r = r;
	ev->next = 0;
	return (ev);
}

VECTOR* newVector()
{
	VECTOR *v;

	if (numVectors >= maxVectors)
	 { if (maxVectors == 0)
	    { maxVectors = BASE_SIZE;
	      vectors = (VECTOR*)malloc(sizeof(VECTOR)*maxVectors);
	    }
	   else
	    { maxVectors *= 2;
	      vectors = (VECTOR*)realloc((char*)vectors,
					 sizeof(VECTOR)*maxVectors);
	    }
	 }
	v = &vectors[numVectors++];
	v->visible = 1;
	v->events = 0;
	return (v);
}

PEDGE* newPedge(x,y,z)
float x,y,z;
{
	PEDGE *pe;

	if (numEdges >= maxEdges)
	 { if (maxEdges == 0)
	    { maxEdges = BASE_SIZE;
	      edges = (PEDGE*)malloc(sizeof(PEDGE)*maxEdges);
	    }
	   else
	    { maxEdges *= 2;
	      edges = (PEDGE*)realloc((char*)edges,
					 sizeof(PEDGE)*maxEdges);
	    }
	 }
	pe = &edges[numEdges++];
	pe->x = x;
	pe->y = y;
	pe->ez = z;
	pe->nextEvents = 0;
	pe->prevEvents = 0;
	pe->intersect = 0;
	return (pe);
}

SCR_POLYGON* newPolygon()
{
	if (numPolygons >= maxPolygons)
	 { if (maxPolygons == 0)
	    { maxPolygons = BASE_SIZE;
	      polygons = (SCR_POLYGON*)malloc(sizeof(SCR_POLYGON)*maxPolygons);
	    }
	   else
	    { maxPolygons *= 2;
	      polygons = (SCR_POLYGON*)realloc((char*)polygons,
					 sizeof(SCR_POLYGON)*maxPolygons);
	    }
	 }
	return &polygons[numPolygons++];
}

freePolygonMem()
{
	numVectors = numEdges = numPolygons = numPevents = 0;
}

void computeCentroid (xp, yp, pgn)
double *xp, *yp;
SCR_POLYGON *pgn;
{
	/* Compute the centroid of a polygon 'pgn'. */

	PEDGE *edg;
	register double x, y;

	x = y = 0;
	edg = pgn->edges;
	do
	 { x += edg->x;
	   y += edg->y;
	   edg = edg->next;
	 }
	while (edg != pgn->edges);

	*xp = x/pgn->n;
	*yp = y/pgn->n;
}

pointInsidePolygon (x, y, pgn)
register double x, y;
SCR_POLYGON *pgn;
{
	/* Returns true if the point (x,y) is strictly inside the polygon 
	   'pgn'. */

	register PEDGE *eg1, *eg2;

	eg1 = pgn->edges;
	do
	 { eg2 = eg1->next;
	   if (((eg2->x-eg1->x)*(y-eg2->y) - 
		     (x-eg2->x)*(eg2->y-eg1->y)) <= 0.0)
	    { return(0);
	    }
	   eg1 = eg2;
	 }
	while (eg1 != pgn->edges);

	return(1);
}


/* Cross product between two vectors v1 and v2 is v1x*v2y-v2x*v1y */

/* SIDE(pt,e1,e2) computes (e2-e1)X(pt-e1).  

   This also means that:

   SIDE(pt,e1,e2) > 0 if e1,e2,pt form a LEFT turn;
   SIDE(pt,e1,e2) < 0 if e1,e2,pt form a RIGHT turn;
   SIDE(pt,e1,e2) = 0 if e1,e2,pt are colinear,

   and also that

   SIDE(pt,e1,e2) > 0 if pt is LEFT of (e2-e1);
   SIDE(pt,e1,e2) < 0 if pt is RIGHT of (e2-e1);
   SIDE(pt,e1,e2) = 0 if pt is colinear with (e2-e1)

*/   



#define SIDE(pt,e1,e2) \
	( ((e2)->x-(e1)->x)*((pt)->y-(e1)->y) - \
	  ((pt)->x-(e1)->x)*((e2)->y-(e1)->y) )	

#define NO_CROSSING	0
#define CROSSING	1
#define NO_INTERSECTION 2


float computeSide (pt,e1,e2)
PEDGE *pt, *e1, *e2;
{
	register double ax, ay, bx, by;
	register float prod;
	float d;

	ax = e2->x - e1->x;
	ay = e2->y - e1->y;
	bx = pt->x - e1->x;
	by = pt->y - e1->y;
	prod = ax*by - ay*bx;
	d = ABS(ax) + ABS(ay) + scrEpsilon;
	if (ABS(prod)/d < scrEpsilon)
	 { return (0.0);
	 }
	else
	 { return (prod);
	 }
}

int debug;
int intersectionCount;

intersectPolygons(poly1,poly2,draw,fp,fq,cntrXp,cntrYp)
SCR_POLYGON *poly1, *poly2;
int draw;
PEDGE **fp, **fq;
float *cntrXp, *cntrYp;
{
	PEDGE *p1, *q1;
	PEDGE *p2, *q2;
	float x, y, rq, rp, qCrossP;
	int outside = UNDEF;
	int loopCnt;
	int firstIntersection;
	PEDGE *firstP, *firstQ;
	int contact = 0;
	float sideP2, sideP1, sideQ2, sideQ1;
	double cntrX = 0; 
	double cntrY = 0;
	int icnt = 0;
	int rcode;
	int xstat;

	/* check the bounding boxes */

	if (poly1->xmax <= poly2->xmin || poly1->xmin >= poly2->xmax ||
	    poly1->ymax <= poly2->ymin || poly1->ymin >= poly2->ymax)
	 { return (P_DONT_INTERSECT);
	 }

	/* clear the intersection fields of each edge of each polygon */

	p2 = poly1->edges;
	do
	 { p2->intersect = 0;
	   p2 = p2->next;
	 }
	while (p2 != poly1->edges);

	p2 = poly2->edges;
	do
	 { p2->intersect = 0;
	   p2 = p2->next;
	 }
	while (p2 != poly2->edges);

	p2 = poly1->edges;
	q2 = poly2->edges;
	loopCnt = 0;
	firstIntersection = 0;

	intersectionCount = 0;

	do
	 { loopCnt++;

	   /* Check for intersection of both segments */

	   p1 = p2->prev;
	   q1 = q2->prev;

	   sideQ1 = computeSide(q1,p1,p2);
	   sideP1 = computeSide(p1,q1,q2);
	   sideQ2 = computeSide(q2,p1,p2);
	   sideP2 = computeSide(p2,q1,q2);
	   qCrossP = sideP2 - sideP1;

	   xstat = interSeg (&rp, &rq, p2, q2, &contact, &outside);
	
	   if (debug)
	    { printf ("p2 = %g %g, q2 = %g %g, %s contact=%d outside=%s\n",
		       p2->x, p2->y, q2->x, q2->y,
		       (xstat == NO_INTERSECTION ? "NO INTERSECT" :
			xstat == CROSSING ? "CROSS" : "_"),
			contact, 
		       (outside == UNDEF ? "UNDEF" : 
			outside == 'q' ? "q" : "p"));
	    }

	   if (xstat == NO_INTERSECTION)
	    { return (P_DONT_INTERSECT);
	    }
	   else if (xstat == CROSSING)
	    { if (firstIntersection && 
		  ((p2==firstP && q2==firstQ) || (q2==firstP && p2==firstQ)))
	       { if (firstIntersection < loopCnt-1)
		  { if (cntrXp) *cntrXp = cntrX/icnt;
		    if (cntrYp) *cntrYp = cntrY/icnt;
		    if (icnt%2)
		     { printf ("ouch, no. intersections = %d\n", icnt);
		     }
		    intersectionCount = icnt;
		    return(P_INTERSECT);
		  }
	       }
	      else
	       { if (!firstIntersection)
		  { firstIntersection = loopCnt;
		    firstP = p2;
		    firstQ = q2;
		    if (fp) *fp = p2;
		    if (fq) *fq = q2;
		  }
		 if (outside == 'q')
		  { p1->intersect |= INTO;
		    q1->intersect |= OUT;
		    p1->rInto = rp;
		    q1->rOut = rq;
		  }
		 else /* outside == 'p' */
		  { p1->intersect |= OUT;
		    q1->intersect |= INTO;
		    p1->rOut = rp;
		    q1->rInto = rq;
		  }
		 x = q1->x + rq*(q2->x-q1->x);
		 y = q1->y + rq*(q2->y-q1->y);
		 
		 cntrX += x;
		 cntrY += y;
		 icnt++;
#if 0
		 if (draw) drawX (x,y);
		 if (draw) drawBox (x,y);
#endif
	       }
	    }
	   if (qCrossP > 0.0)
	    { if (sideP2 > 0.0)
	       { q2 = q2->next;
		 if (debug) printf ("advance Q\n");
	       }
	      else
	       { p2 = p2->next;
		 if (debug) printf ("advance P\n");
	       }
	    }
	   else if (qCrossP < 0.0)
	    { if (sideQ2 > 0.0)
	       { p2 = p2->next;
		 if (debug) printf ("advance P\n");
	       }
	      else
	       { q2 = q2->next;
		 if (debug) printf ("advance Q\n");
	       }
	    }
	   else /* Q and P are parallel */
	    { if (sideP2 < 0.0)
	       { p2 = p2->next;
		 if (debug) printf ("||, advance P\n");
	       }
	      else if (sideP2 > 0.0)
	       { q2 = q2->next;
		 if (debug) printf ("||, advance Q\n");
	       }
	      else if ( ((p2->x-p1->x)*(p2->x-q2->x) + 
			 (p2->y-p1->y)*(p2->y-q2->y)) < 0.0)
	       { p2 = p2->next;
		 if (debug) printf ("||, advance P\n");
	       }
	      else
	       { q2 = q2->next;
		 if (debug) printf ("||, advance Q\n");
	       }
	    }
	 }
	while (loopCnt <= 2*(poly1->n+poly2->n+1));
	if (debug) printf ("loop done\n");
	
	/* If we got this far, we know that either (1) there is
	   no intersection, (2) poly1 is contained in poly2, or
	   (3) poly2 is contained in poly1. */

	if (outside != UNDEF)
	 { /* Contact occured. If outside is 'q', the poly2 is
	      not contained in poly1, and vice versa. */

	   if (outside == 'p')
	    { computeCentroid (&cntrX, &cntrY, poly2);
	      if (pointInsidePolygon (cntrX, cntrY, poly1))
	       { rcode = P_POLY2_INSIDE;
	       }
	      else
	       { rcode = P_DONT_INTERSECT;
	       }
	    }
	   else
	    { computeCentroid (&cntrX, &cntrY, poly1);
	      if (pointInsidePolygon (cntrX, cntrY, poly2))
	       { rcode = P_POLY1_INSIDE;
	       }
	      else
	       { rcode = P_DONT_INTERSECT;
	       }
	    }
	 }
	else
	 { /* Contact did not occur. This implies that all vertices
	      of poly1 are completely inside poly2, or vice versa,
	      or there is not intersection. */

	   if (pointInsidePolygon (poly1->edges->x, poly1->edges->y, poly2))
	    { /* poly1 is completely inside poly2 */
	      computeCentroid (&cntrX, &cntrY, poly1);
	      rcode = P_POLY1_INSIDE;
	    }
	   else if (pointInsidePolygon (poly2->edges->x, poly2->edges->y, poly1))
	    { /* poly2 is completely inside poly1 */
	      computeCentroid (&cntrX, &cntrY, poly2);
	      rcode = P_POLY2_INSIDE;
	    }
	   else
	    { /* no intersection */
	      rcode = P_DONT_INTERSECT;
	    }
	 }
	if (cntrXp) *cntrXp = cntrX;
	if (cntrYp) *cntrYp = cntrY;
	return (rcode);
}

#define LOOK_AHEAD_P	1
#define LOOK_AHEAD_Q	2

interSeg (rp, rq, p2, q2, contact, outside)
float *rp, *rq;
PEDGE *p2, *q2;
int *contact;
int *outside;
{
	/* Return 1 if polygon edges p2 and q2 intersect, and 0 otherwise.
	   If the edges *do* intersect, then the values rp and rq are each
	   set to the appropriate value from the interval [0,1] to indicate
	   where on each edge the intersection occurs.

	   Intersection only occurs when one edge actually passes over the
	   other. Sometimes, edges will come into contact but one will 
	   not actually cross the other. The variable 'contact' is set
	   when this occurs.

	   The variable 'outside' is set whenever we know which polygon
	   is outside the other. An intersect occurs whenever the value
	   if this variable is known to change.
	 */

	PEDGE *p1, *q1;
	float sideQ2, sideP2, sideQ1, sideP1;
	float r;
	int lookAhead;
	int oldOutside;

	p1 = p2->prev;
	q1 = q2->prev;

	sideQ1 = computeSide(q1,p1,p2);	/* which side of P is q1 on */
	sideP1 = computeSide(p1,q1,q2);	/* which side of Q is p1 on */
	sideQ2 = computeSide(q2,p1,p2);	/* which side of P is q2 on */
	sideP2 = computeSide(p2,q1,q2);	/* which side of Q is p2 on */

	if (*contact)
	 { /* if heads are coincident, then edges are coincident, so return */
	   if (sideP2 == 0.0 || sideQ2 == 0.0)
	    { if (sideP2 == 0.0 && sideQ2 == 0.0)
	       { if ((p2->x-p1->x)*(q2->x-q1->x)+
		     (p2->y-p1->y)*(q2->y-q1->y) < 0.0)
		  { return(NO_INTERSECTION);
		  }
	       }
	      return(NO_CROSSING);
	    }
	   *contact = 0;
	   oldOutside = *outside;
	   *outside = ((sideP2 < 0) ? 'p' : 'q');
	   if (oldOutside != UNDEF)
	    { if (*outside != oldOutside)
	       { *rq = (sideQ1 == 0.0) ? 0.0 : 
			ABS(sideQ1/(sideQ2-sideQ1));
		 *rp = (sideP1 == 0.0) ? 0.0 : 
			ABS(sideP1/(sideP2-sideP1));
		 return(CROSSING);
	       }
	    }
	   return(NO_CROSSING);
	 }
	if (sideQ1 == 0.0 || sideP1 == 0.0)
	 { /* then tails are coincident; */
	   if (sideP2 == 0.0)
	    { /* then edges are coincident. */
	      *contact = 1;
	    }
	   return(NO_CROSSING);
	 }

	/* Contact is also set if: 
	   1. The heads are coincident
	   2. P is incident onto Q
	   3. Q is incident onto P */

	if ((sideP2 == 0.0 && sideQ2 == 0.0) ||
	    (sideP2 == 0.0 && sideQ1*sideQ2 < 0) ||
	    (sideQ2 == 0.0 && sideP1*sideP2 < 0))
	 { *contact = 1;
	   *outside = ((sideP1 < 0) ? 'p' : 'q');
	   return (NO_CROSSING);
	 }
	else if (sideP2*sideP1 < 0 && sideQ1*sideQ2 < 0)
	 { *outside = ((sideP2 < 0) ? 'p' : 'q');
	   *rq = ABS(sideQ1/(sideQ2-sideQ1));
	   *rp = ABS(sideP1/(sideP2-sideP1));
	   return (CROSSING);
	 }
	else
	 { return (NO_CROSSING);
	 }
}

vectorIntersect(poly,vect,xp,yp)
SCR_POLYGON *poly;
VECTOR *vect;
float *xp, *yp;
{
	PEDGE *p1, *p2;
	PEDGE *q1, *q2;
	PEDGE q1e, q2e;
	float r;
	float xmin, xmax, ymin, ymax;
	float sideP1, sideP2, sideQ1, sideQ2;
	float sideP3;
	int intersect = 0;
	int found;

	/* first, the bounding box check */

	/* check the bounding boxes */

	xmin = MIN(vect->x1,vect->x2);
	xmax = MAX(vect->x1,vect->x2);
	ymin = MIN(vect->y1,vect->y2);
	ymax = MAX(vect->y1,vect->y2);

	if (poly->xmax <= xmin || poly->xmin >= xmax ||
	    poly->ymax <= ymin || poly->ymin >= ymax)
	 { return (P_DONT_INTERSECT);
	 }

	q1 = &q1e;
	q2 = &q2e;

	q1->x = vect->x1;
	q1->y = vect->y1;
	q2->x = vect->x2;
	q2->y = vect->y2;

	p2 = poly->edges;
	p1 = p2->prev;
	do
	 { 
	   sideP1 = computeSide(p1,q1,q2);
	   sideP2 = computeSide(p2,q1,q2);
	   sideQ1 = computeSide(q1,p1,p2);
	   sideQ2 = computeSide(q2,p1,p2);
	   found = 0;

	   /* As a first condition, the line of edge p must split q, 
	      and the back of p must be scrEpsilon from q. */

	   if (sideQ1*sideQ2 < 0 && sideP1 != 0.0)
	    { 
	      if (sideP2 == 0.0)
	       { /* head of p just touches q. */
		 sideP3 = computeSide(p2->next,q1,q2);
		 if (sideP3 == 0.0 || sideP1*sideP3 > 0)
		  { return (0);
		  }
		 else
		  { found = 1;
		  }
	       }
	      else if (sideP2*sideP1 < 0)
	       { /* p crosses over q */
		 found = 1;
	       }
	      if (found)
	       { r = sideQ1/(sideQ1-sideQ2);
		 r = MAX(0.0,MIN(r,1.0));
		 if (sideQ2 <= 0)
		  { intersect |= OUT;
		    vect->rOut = r;
		  }
		 else
		  { intersect |= INTO;
		    vect->rInto = r;
		  }
	       }
	    }
	   p1 = p2;
	   p2 = p2->next;
	 }
	while (p2 != poly->edges);

	if (!intersect)
	 { double x, y;
	   x = (q1->x+q2->x)/2;
	   y = (q1->y+q2->y)/2;
	   if (!pointInsidePolygon (x, y, poly))
	    { return (0);
	    }
	   vect->rInto = 0.0;
	   vect->rOut = 1.0;
	 }
	else if (intersect == INTO)
	 { vect->rOut = 1.0;
	 }
	else if (intersect == OUT)
	 { vect->rInto = 0.0;
	 }

	/* compute and return the midpoint of the intersected region */
	 { float r;
	   r = (vect->rInto+vect->rOut)/2;
	   if (xp) *xp = vect->x1 + r*(vect->x2-vect->x1);
	   if (yp) *yp = vect->y1 + r*(vect->y2-vect->y1);
	 }
	return (1);
}

vectorClip(v)
VECTOR *v;
{
	/* Here is the basic paradigm for adding a hidding region delimited 
	   by the interval [h,e]:

	   1. If the region at h is exposed, then add HIDE event at h
	   2. If the region at e is exposed, then add EXPOSE event at e
	   3. Delete all other events in the interval [h,e].

	   h corresponds to the value of rInto, and e corresponds to the value
	   of rOut. 
	 */
	PEVENT *hide;
	PEVENT *expose;
	PEVENT *evBak, *evFwd;
	float e, h;

	if (v->events == NULL && !v->visible)
	 { return;
	 }

	e = v->rOut;
	h = v->rInto;
	
	/* Start by either locating a hide event previous to h, or
	   creating one at h. */

	evBak = NULL;
	for (evFwd=v->events; evFwd && evFwd->r<h; evFwd=evFwd->next)
	 { evBak = evFwd;
	 }
	if (evBak && evBak->type == HIDE_EVENT)
	 { hide = evBak;
	 }
	else
	 { hide = newPevent(HIDE_EVENT,h);
	   if (evBak)
	    { evBak->next = hide;
	    }
	   else
	    { v->events = hide;
	    }
	 }

	/* Now locate an expose event after e, or create one at e. */

	for ( ; evFwd && evFwd->r <= e; evFwd=evFwd->next)
	   ;
	if (evFwd && evFwd->type == EXPOSE_EVENT)
	 { expose = evFwd;
	 }
	else
	 { expose = newPevent(EXPOSE_EVENT,e);
	   expose->next = evFwd;
	 }

	/* Link the hide and expose events together, effectively deleting
	   all events in between. */

	hide->next = expose;

	/* Mark the entire vector invisible if necessary. */

	if (hide->r == 0.0 && expose->r == 1.0)
	 { v->events = NULL;
	   v->visible = 0;
	 }
}

vectorDraw(v)
VECTOR *v;
{
	if (v->events == NULL)
	 { if (v->visible)
	    { grf4ScrMove (v->x1,v->y1);
	      grf4ScrDraw (v->x2,v->y2);
	    }
	 }
	else
	 { float dx, dy;
	   PEVENT *ev, *ep;

	   dx = v->x2 - v->x1;
	   dy = v->y2 - v->y1;
	   ep = NULL;
	   for (ev=v->events; ev; ev=ev->next)
	    { if (ev->type == HIDE_EVENT && ev->r != 0.0)
	       { if (ep)
		  { grf4ScrMove (v->x1 + ep->r*dx, v->y1 + ep->r*dy);
		  }
		 else
		  { grf4ScrMove (v->x1, v->y1);
		  }
		 grf4ScrDraw (v->x1 + ev->r*dx, v->y1 + ev->r*dy);
#if 0
		 if (ep)
		  { drawBox (v->x1 + ep->r*dx, v->y1 + ep->r*dy, 4.0);
		  }
		 drawBox (v->x1 + ev->r*dx, v->y1 + ev->r*dy, 4.0);
#endif
	       }
	      else if (ev->type == EXPOSE_EVENT && ev->next == NULL &&
		       ev->r != 1.0)
	       { 
		 grf4ScrMove (v->x1 + ev->r*dx, v->y1 + ev->r*dy);
		 grf4ScrDraw (v->x2, v->y2);
#if 0
		 drawBox (v->x1 + ev->r*dx, v->y1 + ev->r*dy, 4.0);
#endif
	       }
/*	      printf ("event=%d r=%g\n", ev->type, ev->r); */
	      ep = ev;
	    }
	 }
}

#endif

printPolygon(p)
SCR_POLYGON *p;
{
	PEDGE *e;
	int k = 0;

	printf ("%2d ", p->n);
	e = p->edges;
	do
	 { if ((++k % 2) == 0)
	    { printf ("\n   ");
	    }
	   printf ("%13.8f %13.8f   ", e->x, e->y);
	   e = e->prev;
	 }
	while (e != p->edges);
	printf ("\n");
}
