/* routesplinesinit:
 * Data initialized once until matching call to routeplineterm
 * Allows recursive calls to dot
 */
void
routesplinesinit()
{
    if (++routeinit > 1) return;
#ifdef UNUSED
    if (!(bs = N_GNEW(BINC, box))) {
	agerr(AGERR, "cannot allocate bs\n");
	abort();
    }
    maxbn = BINC;
#endif
    if (!(ps = N_GNEW(PINC, point))) {
	agerr(AGERR, "cannot allocate ps\n");
	abort();
    }
    maxpn = PINC;
#ifdef DEBUG
    if (Show_boxes) {
	int i;
        for (i = 0; Show_boxes[i]; i++)
	    free (Show_boxes[i]);
	free (Show_boxes);
	Show_boxes = NULL;
	Show_cnt = 0;
    }
#endif
    nedges = 0;
    nboxes = 0;
    if (Verbose)
	start_timer();
}

/* mkTriGraph:
 * Generate graph with triangles as nodes and an edge iff two triangles
 * share an edge.
 */
static tgraph *mkTriGraph(surface_t * sf, int maxv, pointf * pts)
{
    tgraph *g;
    tnode *np;
    int j, i, ne = 0;
    int *edgei;
    int *jp;

    /* ne is twice no. of edges */
    for (i = 0; i < 3 * sf->nfaces; i++)
	if (sf->neigh[i] != -1)
	    ne++;

    g = GNEW(tgraph);

    /* plus 2 for nodes added as endpoints of an edge */
    g->nodes = N_GNEW(sf->nfaces + 2, tnode);

    /* allow 1 possible extra edge per triangle, plus 
     * obstacles can have at most maxv triangles touching 
     */
    edgei = N_GNEW(sf->nfaces + ne + 2 * maxv, int);
    g->edges = N_GNEW(ne/2 + 2 * maxv, tedge);
    g->nedges = 0;

    for (i = 0; i < sf->nfaces; i++) {
	np = g->nodes + i;
	np->ne = 0;
	np->edges = edgei;
	np->ctr = triCenter(pts, sf->faces + 3 * i);
	edgei += degT(sf->neigh + 3 * i) + 1;
    }
    /* initialize variable nodes */
    np = g->nodes + i;
    np->ne = 0;
    np->edges = edgei;
    np++;
    np->ne = 0;
    np->edges = edgei + maxv;

    for (i = 0; i < sf->nfaces; i++) {
	np = g->nodes + i;
	jp = sf->neigh + 3 * i;
        ne = 0;
	while ((ne < 3) && ((j = *jp++) != -1)) {
	    if (i < j) {
		double dist = DIST(np->ctr, (g->nodes + j)->ctr);
		ipair seg =
		    sharedEdge(sf->faces + 3 * i, sf->faces + 3 * j);
		addTriEdge(g, i, j, dist, seg);
	    }
	    ne++;
	}
    }

    return g;
}

boxf*
partition (cell* cells, int ncells, int* nrects, boxf bb)
{
    int nsegs = 4*(ncells+1);
    segment_t* segs = N_GNEW(nsegs+1, segment_t);
    int* permute = N_NEW(nsegs+1, int);
    int hd_size, vd_size;
    int i, j, cnt = 0;
    boxf* rs;
    int ntraps = TRSIZE(nsegs);
    trap_t* trs = N_GNEW(ntraps, trap_t);
    boxf* hor_decomp = N_NEW(ntraps, boxf);
    boxf* vert_decomp = N_NEW(ntraps, boxf);
    int nt;

    /* fprintf (stderr, "cells = %d segs = %d traps = %d\n", ncells, nsegs, ntraps);  */
    genSegments (cells, ncells, bb, segs, 0);
#if 0
fprintf (stderr, "%d\n\n", ncells+1);
for (i = 1; i<= nsegs; i++) {
  if (i%4 == 1) fprintf(stderr, "4\n");
  fprintf (stderr, "%f %f\n", segs[i].v0.x, segs[i].v0.y);
  if (i%4 == 0) fprintf(stderr, "\n");
}
#endif
    srand48(173);
    generateRandomOrdering (nsegs, permute);
    nt = construct_trapezoids(nsegs, segs, permute, ntraps, trs);
    /* fprintf (stderr, "hor traps = %d\n", nt); */
    hd_size = monotonate_trapezoids (nsegs, segs, trs, 0, hor_decomp);

    genSegments (cells, ncells, bb, segs, 1);
    generateRandomOrdering (nsegs, permute);
    nt = construct_trapezoids(nsegs, segs, permute, ntraps, trs);
    /* fprintf (stderr, "ver traps = %d\n", nt); */
    vd_size = monotonate_trapezoids (nsegs, segs, trs, 1, vert_decomp);

    rs = N_NEW (hd_size*vd_size, boxf);
    for (i=0; i<vd_size; i++) 
	for (j=0; j<hd_size; j++)
	    if (rectIntersect(&rs[cnt], &vert_decomp[i], &hor_decomp[j]))
		cnt++;

    rs = RALLOC (cnt, rs, boxf);
    free (segs);
    free (permute);
    free (trs);
    free (hor_decomp);
    free (vert_decomp);
    *nrects = cnt;
    return rs;
}

/* makeMultiSpline:
 * FIX: we don't really use the shortest path provided by ED_path,
 * so avoid in neato spline code.
 * Return 0 on success.
 */
int makeMultiSpline(graph_t* g,  edge_t* e, router_t * rtr, int doPolyline)
{
    Ppolyline_t line = ED_path(e);
    node_t *t = agtail(e);
    node_t *h = aghead(e);
    pointf t_p = line.ps[0];
    pointf h_p = line.ps[line.pn - 1];
    tripoly_t *poly;
    int idx;
    int *sp;
    int t_id = rtr->tn;
    int h_id = rtr->tn + 1;
    int ecnt = rtr->tg->nedges;
    PPQ pq;
    PQTYPE *idxs;
    PQVTYPE *vals;
    int ret;

	/* Add endpoints to triangle graph */
    addEndpoint(rtr, t_p, t, t_id, ED_tail_port(e).side);
    addEndpoint(rtr, h_p, h, h_id, ED_head_port(e).side);

	/* Initialize priority queue */
    PQgen(&pq.pq, rtr->tn + 2, -1);
    idxs = N_GNEW(pq.pq.PQsize + 1, PQTYPE);
    vals = N_GNEW(pq.pq.PQsize + 1, PQVTYPE);
    vals[0] = 0;
    pq.vals = vals + 1;
    pq.idxs = idxs + 1;

	/* Find shortest path of triangles */
    sp = triPath(rtr->tg, rtr->tn+2, h_id, t_id, (PQ *) & pq);

    free(vals);
    free(idxs);
    PQfree(&(pq.pq), 0);

	/* Use path of triangles to generate guiding polygon */
    poly = mkPoly(rtr, sp, h_id, t_id, h_p, t_p, &idx);

    free(sp);

	/* Generate multiple splines using polygon */
    ret = genroute(g, poly, 0, idx, e, doPolyline);
    freeTripoly (poly);

    resetGraph(rtr->tg, rtr->tn, ecnt);
    return ret;
}

static void
make_barriers(Ppoly_t ** poly, int npoly, int pp, int qp,
	      Pedge_t ** barriers, int *n_barriers)
{
    int i, j, k, n, b;
    Pedge_t *bar;

    n = 0;
    for (i = 0; i < npoly; i++) {
	if (i == pp)
	    continue;
	if (i == qp)
	    continue;
	n = n + poly[i]->pn;
    }
    bar = N_GNEW(n, Pedge_t);
    b = 0;
    for (i = 0; i < npoly; i++) {
	if (i == pp)
	    continue;
	if (i == qp)
	    continue;
	for (j = 0; j < poly[i]->pn; j++) {
	    k = j + 1;
	    if (k >= poly[i]->pn)
		k = 0;
	    bar[b].a = poly[i]->ps[j];
	    bar[b].b = poly[i]->ps[k];
	    b++;
	}
    }
    assert(b == n);
    *barriers = bar;
    *n_barriers = n;
}

 /* makeInfo:
  * For each node in the graph, create a Info data structure 
  */
static void makeInfo(Agraph_t * graph)
{
    Agnode_t *node;
    int i;
    Info_t *ip;

    nsites = agnnodes(graph);
    geominit();

    nodeInfo = N_GNEW(nsites, Info_t);

    node = agfstnode(graph);
    ip = nodeInfo;

    pmargin = expFactor (graph);
    for (i = 0; i < nsites; i++) {
	ip->site.coord.x = ND_pos(node)[0];
	ip->site.coord.y = ND_pos(node)[1];

	makePoly(&ip->poly, node, pmargin);

	ip->site.sitenbr = i;
	ip->site.refcnt = 1;
	ip->node = node;
	ip->verts = NULL;
	node = agnxtnode(graph, node);
	ip++;
    }
}

v_data *delaunay_triangulation(double *x, double *y, int n)
{
    v_data *delaunay;
    GtsSurface* s = tri(x, y, n, NULL, 0, 1);
    int i, nedges;
    int* edges;
    estats stats;

    if (!s) return NULL;

    delaunay = N_GNEW(n, v_data);

    for (i = 0; i < n; i++) {
	delaunay[i].ewgts = NULL;
	delaunay[i].nedges = 1;
    }

    stats.n = 0;
    stats.delaunay = delaunay;
    edgeStats (s, &stats);
    nedges = stats.n;
    edges = N_GNEW(2 * nedges + n, int);

    for (i = 0; i < n; i++) {
	delaunay[i].edges = edges;
	edges += delaunay[i].nedges;
	delaunay[i].edges[0] = i;
	delaunay[i].nedges = 1;
    }
    gts_surface_foreach_edge (s, (GtsFunc) add_edge, delaunay);

    gts_object_destroy (GTS_OBJECT (s));

    return delaunay;
}

Operator Operator_diag_precon_new(SparseMatrix A){
  Operator o;
  real *diag;
  int i, j, m = A->m, *ia = A->ia, *ja = A->ja;
  real *a = (real*) A->a;

  assert(A->type == MATRIX_TYPE_REAL);

  assert(a);

  o = N_GNEW(1,struct Operator_struct);
  o->data = N_GNEW((A->m + 1),real);
  diag = (real*) o->data;

  diag[0] = m;
  diag++;
  for (i = 0; i < m; i++){
    diag[i] = 1.;
    for (j = ia[i]; j < ia[i+1]; j++){
      if (i == ja[j] && ABS(a[j]) > 0) diag[i] = 1./a[j];
    }
  }

  o->Operator_apply = Operator_diag_precon_apply;

  return o;
}

static pointf computeScaleXY(pointf * aarr, int m)
{
    pointf *barr;
    double cost, bestcost;
    int k, best = 0;
    pointf scale;

    aarr[0].x = 1;
    aarr[0].y = HUGE_VAL;
    qsort(aarr + 1, m, sizeof(pointf), (sortfn_t) sortf);

    barr = N_GNEW(m + 1, pointf);
    barr[m].x = aarr[m].x;
    barr[m].y = 1;
    for (k = m - 1; k >= 0; k--) {
	barr[k].x = aarr[k].x;
	barr[k].y = MAX(aarr[k + 1].y, barr[k + 1].y);
    }

    bestcost = HUGE_VAL;
    for (k = 0; k <= m; k++) {
	cost = barr[k].x * barr[k].y;
	if (cost < bestcost) {
	    bestcost = cost;
	    best = k;
	}
    }
    assert(bestcost < HUGE_VAL);
    scale.x = barr[best].x;
    scale.y = barr[best].y;

    return scale;
}

/* routesplinesinit:
 * Data initialized once until matching call to routeplineterm
 * Allows recursive calls to dot
 */
int
routesplinesinit()
{
    if (++routeinit > 1) return 0;
    if (!(ps = N_GNEW(PINC, pointf))) {
	agerr(AGERR, "routesplinesinit: cannot allocate ps\n");
	return 1;
    }
    maxpn = PINC;
#ifdef DEBUG
    if (Show_boxes) {
	int i;
        for (i = 0; Show_boxes[i]; i++)
	    free (Show_boxes[i]);
	free (Show_boxes);
	Show_boxes = NULL;
	Show_cnt = 0;
    }
#endif
    nedges = 0;
    nboxes = 0;
    if (Verbose)
	start_timer();
    return 0;
}

/* finishEdge:
 * Finish edge generation, clipping to nodes and adding arrowhead
 * if necessary, and adding edge labels
 */
static void
finishEdge (graph_t* g, edge_t* e, Ppoly_t spl, int flip, pointf p, pointf q)
{
    int j;
    pointf *spline = N_GNEW(spl.pn, pointf);
    pointf p1, q1;

    if (flip) {
	for (j = 0; j < spl.pn; j++) {
	    spline[spl.pn - 1 - j] = spl.ps[j];
	}
	p1 = q;
	q1 = p;
    }
    else {
	for (j = 0; j < spl.pn; j++) {
	    spline[j] = spl.ps[j];
	}
	p1 = p;
	q1 = q;
    }
    if (Verbose > 1)
	fprintf(stderr, "spline %s %s\n", agnameof(agtail(e)), agnameof(aghead(e)));
    clip_and_install(e, aghead(e), spline, spl.pn, &sinfo);
    free(spline);

    addEdgeLabels(g, e, p1, q1);
}

static void initColors(void)
{
    colorlist = N_GNEW(NPENS, Color);
    colorlist[0] = white;
    colorlist[1] = black;
    ColorsUsed = 2;
}

static void initHeap(PairHeap * h, double *place, int *ordering, int n)
{
    int i;
    Pair edge;
    int j;

    h->heapSize = n - 1;
#ifdef REDO
    if (h->heapSize > h->maxSize) {
	h->maxSize = h->heapSize;
	h->data = (Pair *) realloc(h->data, h->maxSize * sizeof(Pair));
    }
#else
    h->maxSize = h->heapSize;
    h->data = N_GNEW(h->maxSize, Pair);
#endif

    for (i = 0; i < n - 1; i++) {
	edge.left = ordering[i];
	edge.right = ordering[i + 1];
	edge.dist = place[ordering[i + 1]] - place[ordering[i]];
	h->data[i] = edge;
    }
    for (j = (n - 1) / 2; j >= 0; j--) {
	heapify(h, j);
    }
}

SparseMatrix call_tri(int n, int dim, real * x)
{
    real one = 1;
    int i, ii, jj;
    SparseMatrix A;
    SparseMatrix B;
    int* edgelist = NULL;
    real* xv = N_GNEW(n, real);
    real* yv = N_GNEW(n, real);
    int numberofedges;

    for (i = 0; i < n; i++) {
	xv[i] = x[i * 2];
	yv[i] = x[i * 2 + 1];
    }

    if (n > 2) {
	edgelist = delaunay_tri (xv, yv, n, &numberofedges);
    } else {
	numberofedges = 0;
    }

    A = SparseMatrix_new(n, n, 1, MATRIX_TYPE_REAL, FORMAT_COORD);
    for (i = 0; i < numberofedges; i++) {
	ii = edgelist[i * 2];
	jj = edgelist[i * 2 + 1];
	SparseMatrix_coordinate_form_add_entries(A, 1, &(ii), &(jj), &one);
    }
    if (n == 2) {		/* if two points, add edge i->j */
	ii = 0;
	jj = 1;
	SparseMatrix_coordinate_form_add_entries(A, 1, &(ii), &(jj), &one);
    }
    for (i = 0; i < n; i++) {
	SparseMatrix_coordinate_form_add_entries(A, 1, &i, &i, &one);
    }
    B = SparseMatrix_from_coordinate_format(A);
    SparseMatrix_delete(A);
    A = SparseMatrix_symmetrize(B, FALSE);
    SparseMatrix_delete(B);
    B = A;

    free (edgelist);
    free (xv);
    free (yv);
    return B;
}

static  void
gd_polygon(point *A, int n, int filled)
{
	pointf		p;
	int		i;
	gdPoint		*points;
	int		style[20];
	int		pen, width;
	gdImagePtr	brush = NULL;

	if (cstk[SP].pen != P_NONE) {
		if (cstk[SP].pen == P_DASHED) {
			for (i = 0; i < 10; i++)
				style[i] = cstk[SP].pencolor;
			for (; i < 20; i++)
				style[i] = gdTransparent;
			gdImageSetStyle(im, style, 20);
			pen = gdStyled;
		} else if (cstk[SP].pen == P_DOTTED) {
			for (i = 0; i < 2; i++)
				style[i] = cstk[SP].pencolor;
			for (; i < 12; i++)
				style[i] = gdTransparent;
			gdImageSetStyle(im, style, 12);
			pen = gdStyled;
		} else {
			pen = cstk[SP].pencolor;
		}
#if 1
		/* use brush instead of Thickness to improve end butts */
		gdImageSetThickness(im, WIDTH_NORMAL);
                if (cstk[SP].penwidth != WIDTH_NORMAL) {
			width=cstk[SP].penwidth * Zoom;
                        brush = gdImageCreate(width,width);
                        gdImagePaletteCopy(brush, im);
                        gdImageFilledRectangle(brush,
                           0,0,width-1, width-1, cstk[SP].pencolor);
                        gdImageSetBrush(im, brush);
			if (pen == gdStyled) pen = gdStyledBrushed;      
			else pen = gdBrushed;      
		}
#else
		width = cstk[SP].penwidth;
		gdImageSetThickness(im, width);
#endif
		points = N_GNEW(n,gdPoint);
		for (i = 0; i < n; i++) {
			p.x = A[i].x; p.y = A[i].y;
			p = gdpt(p);
			points[i].x = ROUND(p.x); points[i].y = ROUND(p.y);
		}
		if (filled) gdImageFilledPolygon(im, points, n, cstk[SP].fillcolor);
		gdImagePolygon(im, points, n, pen);
		free(points);
		if (brush)
			gdImageDestroy(brush);
	}
}

/* inPoly:
 * Return 1 if q is inside polygon vertex[]
 * Assume points are in CCW order
 */
static int inPoly(Point vertex[], int n, Point q)
{
    int i, i1;			/* point index; i1 = i-1 mod n */
    double x;			/* x intersection of e with ray */
    double crossings = 0;	/* number of edge/ray crossings */

    if (tp3 == NULL)
	tp3 = N_GNEW(maxcnt, Point);

    /* Shift so that q is the origin. */
    for (i = 0; i < n; i++) {
	tp3[i].x = vertex[i].x - q.x;
	tp3[i].y = vertex[i].y - q.y;
    }

    /* For each edge e=(i-1,i), see if crosses ray. */
    for (i = 0; i < n; i++) {
	i1 = (i + n - 1) % n;

	/* if edge is horizontal, test to see if the point is on it */
	if ((tp3[i].y == 0) && (tp3[i1].y == 0)) {
	    if ((tp3[i].x * tp3[i1].x) < 0) {
		return 1;
	    } else {
		continue;
	    }
	}

	/* if e straddles the x-axis... */
	if (((tp3[i].y >= 0) && (tp3[i1].y <= 0)) ||
	    ((tp3[i1].y >= 0) && (tp3[i].y <= 0))) {
	    /* e straddles ray, so compute intersection with ray. */
	    x = (tp3[i].x * tp3[i1].y - tp3[i1].x * tp3[i].y)
		/ (double) (tp3[i1].y - tp3[i].y);

	    /* if intersect at origin, we've found intersection */
	    if (x == 0)
		return 1;;

	    /* crosses ray if strictly positive intersection. */
	    if (x > 0) {
		if ((tp3[i].y == 0) || (tp3[i1].y == 0)) {
		    crossings += .5;	/* goes thru vertex */
		} else {
		    crossings += 1.0;
		}
	    }
	}
    }

    /* q inside if an odd number of crossings. */
    if ((((int) crossings) % 2) == 1)
	return 1;
    else
	return 0;
}

PriorityQueue PriorityQueue_new(int n, int ngain){
  PriorityQueue q;
  int i;
  q = N_GNEW(1,struct PriorityQueue_struct);
  q->count = 0;
  q->n = n;
  q->ngain = ngain;
  q->gain_max = -1;/* no entries yet */
  q->buckets = N_GNEW((ngain+1),DoubleLinkedList);
  for (i = 0; i < ngain+1; i++) (q->buckets)[i] = NULL;

  q->where = N_GNEW((n+1),DoubleLinkedList);
  for (i = 0; i < n+1; i++) (q->where)[i] = NULL;

  q->gain = N_GNEW((n+1),int);
  for (i = 0; i < n+1; i++) (q->gain)[i] = -999;
  return q;

}

/* mkCtrlPts:
 * Generate mult points associated with v.
 * The points will lie on the ray bisecting the angle prev--v--nxt.
 * The first point will aways be v.
 * The rest are positioned equally spaced with maximum spacing SEP.
 * In addition, they all lie within the polygon trip->poly.
 * Parameter s gives the index after which a vertex lies on the
 * opposite side. This is necessary to get the "curvature" of the
 * path correct.
 */
static pointf *mkCtrlPts(int s, int mult, pointf prev, pointf v,
			   pointf nxt, tripoly_t * trip)
{
    pointf *ps;
    int idx = ctrlPtIdx(v, &(trip->poly));
    int i;
    double d, sep, theta, sinTheta, cosTheta;
    pointf q, w;

    if (idx < 0)
	return NULL;

    ps = N_GNEW(mult, pointf);
    theta = bisect(prev, v, nxt);
    sinTheta = sin(theta);
    cosTheta = cos(theta);
    w.x = v.x + 100 * cosTheta;
    w.y = v.y + 100 * sinTheta;
    if (idx > s) {
	if (wind(prev, v, w) != 1) {
	    sinTheta *= -1;
	    cosTheta *= -1;
	    w.x = v.x + 100 * cosTheta;
	    w.y = v.y + 100 * sinTheta;
	}
    } else if (wind(prev, v, w) != -1) {
	sinTheta *= -1;
	cosTheta *= -1;
	w.x = v.x + 100 * cosTheta;
	w.y = v.y + 100 * sinTheta;
    }

    if (triPoint(trip, idx, v, w, &q)) {
	return 0;
    }

    d = DIST(q, v);
    if (d >= mult * SEP)
	sep = SEP;
    else
	sep = d / mult;
    if (idx < s) {
	for (i = 0; i < mult; i++) {
	    ps[i].x = v.x + i * sep * cosTheta;
	    ps[i].y = v.y + i * sep * sinTheta;
	}
    } else {
	for (i = 0; i < mult; i++) {
	    ps[mult - i - 1].x = v.x + i * sep * cosTheta;
	    ps[mult - i - 1].y = v.y + i * sep * sinTheta;
	}
    }
    return ps;
}

SparseMatrix call_tri2(int n, int dim, real * xx)
{
    real *x, *y;
    v_data *delaunay;
    int i, j;
    SparseMatrix A;
    SparseMatrix B;
    real one = 1;
    x = N_GNEW(n, real);
    y = N_GNEW(n, real);

    for (i = 0; i < n; i++) {
	x[i] = xx[dim * i];
	y[i] = xx[dim * i + 1];
    }

    delaunay = UG_graph(x, y, n, 0);

    A = SparseMatrix_new(n, n, 1, MATRIX_TYPE_REAL, FORMAT_COORD);

    for (i = 0; i < n; i++) {
	for (j = 1; j < delaunay[i].nedges; j++) {
	    SparseMatrix_coordinate_form_add_entries(A, 1, &i,
						     &(delaunay[i].
						       edges[j]), &one);
	}
    }
    for (i = 0; i < n; i++) {
	SparseMatrix_coordinate_form_add_entries(A, 1, &i, &i, &one);
    }
    B = SparseMatrix_from_coordinate_format(A);
    B = SparseMatrix_symmetrize(B, FALSE);
    SparseMatrix_delete(A);

    free (x);
    free (y);
    freeGraph (delaunay);
    
    return B;

}

void PQinitialize(void)
{
    int i;

    PQcount = 0;
    PQmin = 0;
    PQhashsize = 4 * sqrt_nsites;
    if (PQhash == NULL)
	PQhash = N_GNEW(PQhashsize, Halfedge);
    for (i = 0; i < PQhashsize; i += 1)
	PQhash[i].PQnext = (Halfedge *) NULL;
}