/*************************************************************************
 * Copyright (c) 2011 AT&T Intellectual Property 
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * https://www.eclipse.org/legal/epl-v10.html
 *
 * Contributors: Details at https://graphviz.org
 *************************************************************************/

#include <assert.h>
#include <pathplan/vis.h>
#include <stdbool.h>
#include <stdlib.h>
#include <util/alloc.h>

/* 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;

    assert(V >= 0);
    array2 arr = gv_calloc(V + extra, sizeof(COORD*));
    COORD *p = gv_calloc((size_t)V * (size_t)V, sizeof(COORD));
    for (i = 0; i < V; i++) {
	arr[i] = p;
	p += V;
    }
    for (i = V; i < V + extra; i++)
	arr[i] = NULL;

    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.
 */
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);
}

/* inBetween:
 * Return true if c is in (a,b), assuming a,b,c are collinear.
 */
static bool 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);
}

/* 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.
 */
static bool 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 true;
    }
    a_abd = wind(a, b, d);
    if (a_abd == 0 && inBetween(a, b, d)) {
	return true;
    }
    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;
}

/* in_cone:
 * Returns true iff point b is in the cone a0,a1,a2
 * NB: the cone is considered a closed set
 */
static bool 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 */
}

/* 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 bool 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 bool clear(Ppoint_t pti, Ppoint_t ptj,
		 int start, int end,
		 int V, Ppoint_t pts[], int nextPt[])
{
    int k;

    for (k = 0; k < start; k++) {
	if (intersect(pti, ptj, pts[k], pts[nextPt[k]]))
	    return false;
    }
    for (k = end; k < V; k++) {
	if (intersect(pti, ptj, pts[k], pts[nextPt[k]]))
	    return false;
    }
    return true;
}

/* 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 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 = 0; 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)) {
		/* 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);
}

/* 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 = (size_t)(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)
{
    const int V = conf->N;
    Ppoint_t *pts = conf->P;
    int *nextPt = conf->next;
    int *prevPt = conf->prev;
    int k;
    int start, end;
    Ppoint_t pk;
    COORD d;

    COORD *vadj = gv_calloc(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)) {
	    /* 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)) {
	    /* 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.
 */
bool 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 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]]))
	    return false;
    }
    for (k = e1; k < s2; k++) {
	if (intersect(p, q, pts[k], pts[nextPt[k]]))
	    return false;
    }
    for (k = e2; k < V; k++) {
	if (intersect(p, q, pts[k], pts[nextPt[k]]))
	    return false;
    }
    return true;
}