/*
    This software may only be used by you under license from AT&T Corp.
    ("AT&T").  A copy of AT&T's Source Code Agreement is available at
    AT&T's Internet website having the URL:
    <http://www.research.att.com/sw/tools/graphviz/license/source.html>
    If you received this software without first entering into a license
    with AT&T, you have an infringing copy of this software and cannot use
    it without violating AT&T's intellectual property rights.
*/

#include <vis.h>

#ifdef DMALLOC
#include "dmalloc.h"
#endif

	/* TRANSPARENT means router sees past colinear obstacles */
#ifdef TRANSPARENT
#define INTERSECT(a,b,c,d,e) intersect1((a),(b),(c),(d),(e))
#else
#define INTERSECT(a,b,c,d,e) intersect((a),(b),(c),(d))
#endif

/* allocArray:
 * Allocate a VxV array of COORD values.
 * (array2 is a pointer to an array of pointers; the array is
 * accessed in row-major order.)
 * The values in the array are initialized to 0.
 * Add extra rows.
 */
static array2 allocArray (int V, int extra)
{
    int    i, k;
    array2 arr;
    COORD* p;

    arr = (COORD**)malloc((V + extra)*sizeof(COORD *));
    for (i=0; i < V; i++) {
      p = (COORD*)malloc(V*sizeof(COORD));
      arr[i] = p;
      for (k=0; k < V; k++) {
        *p++ = 0;
      }
    }
    for (i=V; i < V+extra; i++)
      arr[i] = (COORD*)0;

    return arr;
}

/* area2:
 * Returns twice the area of triangle abc.
 */
COORD area2(Ppoint_t a, Ppoint_t b, Ppoint_t c)
{
  return ((a.y - b.y)*(c.x - b.x) - (c.y - b.y)*(a.x - b.x));
}

/* wind:
 * Returns 1, 0, -1 if the points abc are counterclockwise,
 * collinear, or clockwise.
 */
static int wind(Ppoint_t a, Ppoint_t b, Ppoint_t c)
{
  COORD w;

  w = ((a.y - b.y)*(c.x - b.x) - (c.y - b.y)*(a.x - b.x));
  /* need to allow for small math errors.  seen with "gcc -O2 -mcpu=i686 -ffast-math" */
  return (w > .0001) ? 1 : ((w < -.0001) ? -1 : 0);
}

#if 0 /* NOT USED */
/* open_intersect:
 * Returns true iff segment ab intersects segment cd.
 * NB: segments are considered open sets
 */
static int open_intersect(Ppoint_t a,Ppoint_t b,Ppoint_t c,Ppoint_t d)
{
  return (((area2(a,b,c) > 0 && area2(a,b,d) < 0) ||
       (area2(a,b,c) < 0 && area2(a,b,d) > 0))
      && 
      ((area2(c,d,a) > 0 && area2(c,d,b) < 0 ) ||
       (area2(c,d,a) < 0 && area2(c,d,b) > 0 )));
}
#endif

/* inBetween:
 * Return true if c is in (a,b), assuming a,b,c are collinear.
 */
int inBetween (Ppoint_t a,Ppoint_t b,Ppoint_t c)
{
  if (a.x != b.x)   /* not vertical */
    return (((a.x < c.x) && (c.x < b.x)) || ((b.x < c.x) && (c.x < a.x)));
  else
    return (((a.y < c.y) && (c.y < b.y)) || ((b.y < c.y) && (c.y < a.y)));
}

	/* TRANSPARENT means router sees past colinear obstacles */
#ifdef TRANSPARENT
/* intersect1:
 * Returns true if the polygon segment [q,n) blocks a and b from seeing
 * each other.
 * More specifically, returns true iff the two segments intersect as open
 * sets, or if q lies on (a,b) and either n and p lie on
 * different sides of (a,b), i.e., wind(a,b,n)*wind(a,b,p) < 0, or the polygon
 * makes a left turn at q, i.e., wind(p,q,n) > 0.
 *
 * We are assuming the p,q,n are three consecutive vertices of a barrier
 * polygon with the polygon interior to the right of p-q-n.
 *
 * Note that given the constraints of our problem, we could probably
 * simplify this code even more. For example, if abq are collinear, but
 * q is not in (a,b), we could return false since n will not be in (a,b)
 * nor will the (a,b) intersect (q,n).
 *
 * Also note that we are computing w_abq twice in a tour of a polygon,
 * once for each edge of which it is a vertex.
 */
static int intersect1(Ppoint_t a,Ppoint_t b,Ppoint_t q,Ppoint_t n, Ppoint_t p)
{
  int w_abq;
  int w_abn;
  int w_qna;
  int w_qnb;

  w_abq = wind(a,b,q);
  w_abn = wind(a,b,n);

    /* If q lies on (a,b),... */
  if ((w_abq == 0) && inBetween (a,b,q)) {
    return ((w_abn * wind(a,b,p) < 0) || (wind(p,q,n) > 0));
  }
  else {
    w_qna = wind(q,n,a);
    w_qnb = wind(q,n,b);
    /* True if q and n are on opposite sides of ab,
     * and a and b are on opposite sides of qn.
     */
    return (((w_abq * w_abn) < 0) && ((w_qna * w_qnb) < 0));
  }
}
#else

/* intersect:
 * Returns true if the segment [c,d] blocks a and b from seeing each other.
 * More specifically, returns true iff c or d lies on (a,b) or the two
 * segments intersect as open sets.
 */
int intersect(Ppoint_t a,Ppoint_t b,Ppoint_t c,Ppoint_t d)
{
  int a_abc;
  int a_abd;
  int a_cda;
  int a_cdb;

  a_abc = wind(a,b,c);
  if ((a_abc == 0) && inBetween (a,b,c)) {
    return 1;
  }
  a_abd = wind(a,b,d);
  if ((a_abd == 0) && inBetween (a,b,d)) {
    return 1;
  }
  a_cda = wind(c,d,a);
  a_cdb = wind(c,d,b);

    /* True if c and d are on opposite sides of ab,
     * and a and b are on opposite sides of cd.
     */
  return (((a_abc * a_abd) < 0) && ((a_cda * a_cdb) < 0));
}
#endif

/* in_cone:
 * Returns true iff point b is in the cone a0,a1,a2
 * NB: the cone is considered a closed set
 */
static int in_cone (Ppoint_t a0,Ppoint_t a1,Ppoint_t a2,Ppoint_t b)
{
    int m = wind(b,a0,a1);
    int p = wind(b,a1,a2);

    if (wind(a0,a1,a2) > 0)
      return ( m >= 0 && p >= 0 ); /* convex at a */
    else
      return ( m >= 0 || p >= 0 ); /* reflex at a */
}

#if 0 /* NOT USED */
/* in_open_cone:
 * Returns true iff point b is in the cone a0,a1,a2
 * NB: the cone is considered an open set
 */
static int in_open_cone (Ppoint_t a0,Ppoint_t a1,Ppoint_t a2,Ppoint_t b)
{
    int m = wind(b,a0,a1);
    int p = wind(b,a1,a2);

    if (wind(a0,a1,a2) >= 0)
      return ( m > 0 && p > 0 ); /* convex at a */
    else
      return ( m > 0 || p > 0 ); /* reflex at a */
}
#endif

/* dist2:
 * Returns the square of the distance between points a and b.
 */
COORD dist2 (Ppoint_t a, Ppoint_t b)
{
    COORD delx = a.x - b.x;
    COORD dely = a.y - b.y;

    return (delx*delx + dely*dely);
}

/* dist:
 * Returns the distance between points a and b.
 */
static COORD dist (Ppoint_t a, Ppoint_t b)
{
    return sqrt(dist2(a,b));
}

static int inCone (int i, int j, Ppoint_t pts[], int nextPt[], int prevPt[])
{
  return in_cone (pts[prevPt[i]],pts[i],pts[nextPt[i]],pts[j]);
}

/* clear:
 * Return true if no polygon line segment non-trivially intersects
 * the segment [pti,ptj], ignoring segments in [start,end).
 */
static int clear (Ppoint_t pti, Ppoint_t ptj, 
                  int start, int end,
                  int V, Ppoint_t pts[], int nextPt[], int prevPt[])
{
    int   k;

    for (k=0; k < start; k++) {
      if (INTERSECT(pti,ptj,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
        return 0;
    }
    for (k=end; k < V; k++) {
      if (INTERSECT(pti,ptj,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
        return 0;
    }
    return 1;
}

/* compVis:
 * Compute visibility graph of vertices of polygons.
 * Only do polygons from index startp to end.
 * If two nodes cannot see each other, the matrix entry is 0.
 * If two nodes can see each other, the matrix entry is the distance
 * between them.
 */
static void compVis (vconfig_t *conf, int start)
{
    int			V = conf->N;
    Ppoint_t*	pts = conf->P;
    int*   		nextPt = conf->next;
    int*   		prevPt = conf->prev;
    array2 		wadj = conf->vis;
    int    		j, i, previ;
    COORD  		d;

    for (i = start; i < V; i++) {
        /* add edge between i and previ.
         * Note that this works for the cases of polygons of 1 and 2
         * vertices, though needless work is done.
         */
      previ = prevPt[i];
      d = dist (pts [i], pts [previ]);
      wadj[i][previ] = d;
      wadj[previ][i] = d;

        /* Check remaining, earlier vertices */
      if (previ == i-1) j = i-2;
      else j = i-1;
      for (; j >= 0; j--) {
        if (inCone(i,j,pts,nextPt,prevPt) &&
            inCone(j,i,pts,nextPt,prevPt) &&
            clear(pts[i],pts[j],V,V,V,pts,nextPt,prevPt)) {
            /* if i and j see each other, add edge */
          d = dist (pts [i], pts [j]);
          wadj[i][j] = d;
          wadj[j][i] = d;
        }
      }
    }
}

/* visibility:
 * Given a vconfig_t conf, representing polygonal barriers,
 * compute the visibility graph of the vertices of conf. 
 * The graph is stored in conf->vis. 
 */
void visibility (vconfig_t *conf)
{
    conf->vis = allocArray (conf->N, 2);
    compVis(conf, 0);
}

/* polyhit:
 * Given a vconfig_t conf, as above, and a point,
 * return the index of the polygon that contains
 * the point, or else POLYID_NONE.
 */
static int polyhit(vconfig_t *conf, Ppoint_t p)
{
	int		i;
	Ppoly_t	poly;

	for (i = 0; i < conf->Npoly; i++) {
		poly.ps = &(conf->P[conf->start[i]]);
		poly.pn = conf->start[i+1] - conf->start[i];
		if (in_poly(poly, p))
			return i;
	}
	return POLYID_NONE;
}

/* ptVis:
 * Given a vconfig_t conf, representing polygonal barriers,
 * and a point within one of the polygons, compute the point's
 * visibility vector relative to the vertices of the remaining
 * polygons, i.e., pretend the argument polygon is invisible.
 *
 * If pp is NIL, ptVis computes the visibility vector for p
 * relative to all barrier vertices.
 */
COORD* ptVis (vconfig_t *conf, int pp, Ppoint_t p)
{
    int    V = conf->N;
    Ppoint_t* pts = conf->P;
    int*   nextPt = conf->next;
    int*   prevPt = conf->prev;
    int    k;
    int    start, end;
    COORD* vadj;
    Ppoint_t  pk;
    COORD  d;

    vadj = (COORD*)malloc((V+2)*sizeof(COORD));
    

	if (pp == POLYID_UNKNOWN) pp = polyhit(conf,p);
    if (pp >= 0) {
		start = conf->start[pp];
		end = conf->start[pp+1];
    }
    else {
		start = V;
		end = V;
	}

    for (k = 0; k < start; k++) {
      pk = pts[k];
      if (in_cone (pts[prevPt[k]],pk,pts[nextPt[k]],p) &&
          clear(p,pk,start,end,V,pts,nextPt,prevPt)) {
          /* if p and pk see each other, add edge */
        d = dist (p, pk);
        vadj[k] = d;
      }
      else vadj[k] = 0;
    }

    for (k = start; k < end; k++)
      vadj[k] = 0;

    for (k = end; k < V; k++) {
      pk = pts[k];
      if (in_cone (pts[prevPt[k]],pk,pts[nextPt[k]],p) &&
          clear(p,pk,start,end,V,pts,nextPt,prevPt)) {
          /* if p and pk see each other, add edge */
        d = dist (p, pk);
        vadj[k] = d;
      }
      else vadj[k] = 0;
    }
    vadj[V] = 0;
    vadj[V+1] = 0;

    return vadj;
    
}

/* directVis:
 * Given two points, return true if the points can directly see each other.
 * If a point is associated with a polygon, the edges of the polygon
 * are ignored when checking visibility.
 */
int directVis (Ppoint_t p, int pp, Ppoint_t q, int qp, vconfig_t *conf)
{
    int    V = conf->N;
    Ppoint_t* pts = conf->P;
    int*   nextPt = conf->next;
    /* int*   prevPt = conf->prev; */
    int   k;
    int   s1, e1;
    int   s2, e2;

    if (pp < 0) {
      s1 = 0;
      e1 = 0;
      if (qp < 0) {
        s2 = 0;
        e2 = 0;
      }
      else {
        s2 = conf->start[qp];
        e2 = conf->start[qp+1];
      }
    }
    else if (qp < 0) {
      s1 = 0;
      e1 = 0;
      s2 = conf->start[pp];
      e2 = conf->start[pp+1];
    }
    else if (pp <= qp) {
      s1 = conf->start[pp];
      e1 = conf->start[pp+1];
      s2 = conf->start[qp];
      e2 = conf->start[qp+1];
    }
    else {
      s1 = conf->start[qp];
      e1 = conf->start[qp+1];
      s2 = conf->start[pp];
      e2 = conf->start[pp+1];
    }

    for (k=0; k < s1; k++) {
      if (INTERSECT(p,q,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
        return 0;
    }
    for (k=e1; k < s2; k++) {
      if (INTERSECT(p,q,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
        return 0;
    }
    for (k=e2; k < V; k++) {
      if (INTERSECT(p,q,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
        return 0;
    }
    return 1;
}