/*
    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.
*/
#pragma prototyped

#include <stdlib.h>
#include <stdio.h>
#ifdef HAVE_MALLOC_H
#include <malloc.h>
#endif
#include <math.h>
#include "pathplan.h"
#include "solvers.h"

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

#define EPSILON1 1E-3
#define EPSILON2 1E-6

#define ABS(a) ((a) >= 0 ? (a) : -(a))

typedef struct tna_t {
    double t;
    Ppoint_t a[2];
} tna_t;

#define prerror(msg) \
        fprintf (stderr, "libpath/%s:%d: %s\n", __FILE__, __LINE__, (msg))

#define DISTSQ(a, b) ( \
    (((a).x - (b).x) * ((a).x - (b).x)) + (((a).y - (b).y) * ((a).y - (b).y)) \
)

#define POINTSIZE sizeof (Ppoint_t)

#define LT(pa, pbp) ((pa.y > pbp->y) || ((pa.y == pbp->y) && (pa.x < pbp->x)))
#define GT(pa, pbp) ((pa.y < pbp->y) || ((pa.y == pbp->y) && (pa.x > pbp->x)))

typedef struct p2e_t {
    Ppoint_t *pp;
    Pedge_t *ep;
} p2e_t;

typedef struct elist_t {
    Pedge_t *ep;
    struct elist_t *next, *prev;
} elist_t;

#if 0
static p2e_t *p2es;
static int p2en;
#endif

#if 0
static elist_t *elist;
#endif

static Ppoint_t *ops;
static int opn, opl;

static int reallyroutespline (Pedge_t *, int,
        Ppoint_t *, int, Ppoint_t, Ppoint_t);
static int mkspline (Ppoint_t *, int, tna_t *, Ppoint_t, Ppoint_t,
        Ppoint_t *, Ppoint_t *, Ppoint_t *, Ppoint_t *);
static int splinefits (Pedge_t *, int, Ppoint_t, Pvector_t, Ppoint_t, Pvector_t, Ppoint_t *, int);
static int splineisinside (Pedge_t *, int, Ppoint_t *);
static int splineintersectsline (Ppoint_t *, Ppoint_t *, double *);
static void points2coeff (double, double, double, double, double *);
static void addroot (double, double *, int *);

static Pvector_t normv (Pvector_t);

static void growops (int);

static Ppoint_t add (Ppoint_t, Ppoint_t);
static Ppoint_t sub (Ppoint_t, Ppoint_t);
static double dist (Ppoint_t, Ppoint_t);
static Ppoint_t scale (Ppoint_t, double);
static double dot (Ppoint_t, Ppoint_t);
static double B0 (double t);
static double B1 (double t);
static double B2 (double t);
static double B3 (double t);
static double B01 (double t);
static double B23 (double t);
#if 0
static int cmpp2efunc (const void *, const void *);

static void listdelete (Pedge_t *);
static void listreplace (Pedge_t *, Pedge_t *);
static void listinsert (Pedge_t *, Ppoint_t);
#endif

int Proutespline (Pedge_t *edges, int edgen, Ppolyline_t input,
        Ppoint_t *evs, Ppolyline_t *output) {
#if 0
    Ppoint_t p0, p1, p2, p3;
    Ppoint_t *pp;
    Pvector_t v1, v2, v12, v23;
    int ipi, opi;
    int ei, p2ei;
    Pedge_t *e0p, *e1p;
#endif
	Ppoint_t	*inps;
	int			inpn;

	/* unpack into previous format rather than modify legacy code */
	inps = input.ps;
	inpn = input.pn;

#if 0
    if (!(p2es = (p2e_t *) malloc (sizeof (p2e_t) * (p2en = edgen * 2)))) {
        prerror ("cannot malloc p2es");
        abort ();
    }
    for (ei = 0, p2ei = 0; ei < edgen; ei++) {
        if (edges[ei].a.x == edges[ei].b.x && edges[ei].a.y == edges[ei].b.y)
            continue;
        p2es[p2ei].pp = &edges[ei].a;
        p2es[p2ei++].ep = &edges[ei];
        p2es[p2ei].pp = &edges[ei].b;
        p2es[p2ei++].ep = &edges[ei];
    }
    p2en = p2ei;
    qsort (p2es, p2en, sizeof (p2e_t), cmpp2efunc);
    elist = NULL;
    for (p2ei = 0; p2ei < p2en; p2ei += 2) {
        pp = p2es[p2ei].pp;
#if DEBUG >= 1
fprintf (stderr, "point: %d %lf %lf\n", p2ei, pp->x, pp->y);
#endif
        e0p = p2es[p2ei].ep;
        e1p = p2es[p2ei + 1].ep;
        p0 = (&e0p->a == p2es[p2ei].pp) ? e0p->b : e0p->a;
        p1 = (&e0p->a == p2es[p2ei + 1].pp) ? e1p->b : e1p->a;
        if (LT (p0, pp) && LT (p1, pp)) {
            listdelete (e0p), listdelete (e1p);
        } else if (GT (p0, pp) && GT (p1, pp)) {
            listinsert (e0p, *pp), listinsert (e1p, *pp);
        } else {
            if (LT (p0, pp))
                listreplace (e0p, e1p);
            else
                listreplace (e1p, e0p);
        }
    }
#endif
    /* generate the splines */
    evs[0] = normv (evs[0]);
    evs[1] = normv (evs[1]);
    opl = 0;
    growops (4);
    ops[opl++] = inps[0];
    if (reallyroutespline (edges, edgen, inps, inpn, evs[0], evs[1]) == -1)
        return -1;
	output->pn = opl;
	output->ps = ops;

#if 0
    fprintf (stderr, "edge\na\nb\n");
    fprintf (stderr, "points\n%d\n", inpn);
    for (ipi = 0; ipi < inpn; ipi++)
        fprintf (stderr, "%f %f\n", inps[ipi].x, inps[ipi].y);
    fprintf (stderr, "splpoints\n%d\n", opl);
    for (opi = 0; opi < opl; opi++)
        fprintf (stderr, "%f %f\n", ops[opi].x, ops[opi].y);
#endif

    return 0;
}

static int reallyroutespline (Pedge_t *edges, int edgen,
        Ppoint_t *inps, int inpn, Ppoint_t ev0, Ppoint_t ev1) {
    Ppoint_t p1, p2, cp1, cp2, p;
    Pvector_t v1, v2, splitv, splitv1, splitv2;
    double maxd, d, t;
    int maxi, i, spliti;

    static tna_t *tnas;
    static int tnan;

    if (tnan < inpn) {
        if (!tnas) {
            if (!(tnas = malloc (sizeof (tna_t) * inpn)))
                return -1;
        } else {
            if (!(tnas = realloc (tnas, sizeof (tna_t) * inpn)))
                return -1;
        }
        tnan = inpn;
    }
    tnas[0].t = 0;
    for (i = 1; i < inpn; i++)
        tnas[i].t = tnas[i - 1].t + dist (inps[i], inps[i - 1]);
    for (i = 1; i < inpn; i++)
        tnas[i].t /= tnas[inpn - 1].t;
    for (i = 0; i < inpn; i++) {
        tnas[i].a[0] = scale (ev0, B1 (tnas[i].t));
        tnas[i].a[1] = scale (ev1, B2 (tnas[i].t));
    }
    if (mkspline (inps, inpn, tnas, ev0, ev1, &p1, &v1, &p2, &v2) == -1)
        return -1;
    if (splinefits (edges, edgen, p1, v1, p2, v2, inps, inpn))
        return 0;
    cp1 = add (p1, scale (v1, 1 / 3.0));
    cp2 = sub (p2, scale (v2, 1 / 3.0));
    for (maxd = -1, maxi = -1, i = 1; i < inpn - 1; i++) {
        t = tnas[i].t;
        p.x = B0 (t) * p1.x + B1 (t) * cp1.x +
                B2 (t) * cp2.x + B3 (t) * p2.x;
        p.y = B0 (t) * p1.y + B1 (t) * cp1.y +
                B2 (t) * cp2.y + B3 (t) * p2.y;
        if ((d = dist (p, inps[i])) > maxd)
            maxd = d, maxi = i;
    }
    spliti = maxi;
    splitv1 = normv (sub (inps[spliti], inps[spliti - 1]));
    splitv2 = normv (sub (inps[spliti + 1], inps[spliti]));
    splitv = normv (add (splitv1, splitv2));
    reallyroutespline (edges, edgen, inps, spliti + 1, ev0, splitv);
    reallyroutespline (edges, edgen, &inps[spliti], inpn - spliti, splitv, ev1);
    return 0;
}

static int mkspline (Ppoint_t *inps, int inpn, tna_t *tnas, Ppoint_t ev0, Ppoint_t ev1,
        Ppoint_t *sp0, Ppoint_t *sv0, Ppoint_t *sp1, Ppoint_t *sv1) {
    Ppoint_t tmp;
    double c[2][2], x[2], det01, det0X, detX1;
    double d01, scale0, scale3;
    int i;

    scale0 = scale3 = 0.0;
    c[0][0] = c[0][1] = c[1][0] = c[1][1] = 0.0;
    x[0] = x[1] = 0.0;
    for (i = 0; i < inpn; i++) {
        c[0][0] += dot (tnas[i].a[0], tnas[i].a[0]);
        c[0][1] += dot (tnas[i].a[0], tnas[i].a[1]);
        c[1][0] = c[0][1];
        c[1][1] += dot (tnas[i].a[1], tnas[i].a[1]);
        tmp = sub (inps[i], add (scale (inps[0], B01 (tnas[i].t)),
                scale (inps[inpn - 1], B23 (tnas[i].t))));
        x[0] += dot (tnas[i].a[0], tmp);
        x[1] += dot (tnas[i].a[1], tmp);
    }
    det01 = c[0][0] * c[1][1] - c[1][0] * c[0][1];
    det0X = c[0][0] * x[1] - c[0][1] * x[0];
    detX1 = x[0] * c[1][1] - x[1] * c[0][1];
    if (ABS(det01) >= 1e-6) {
        scale0 = detX1 / det01;
        scale3 = det0X / det01;
    }
    if (ABS (det01) < 1e-6 || scale0 <= 0.0 || scale3 <= 0.0) {
        d01 = dist (inps[0], inps[inpn - 1]) / 3.0;
        scale0 = d01;
        scale3 = d01;
    }
    *sp0 = inps[0];
    *sv0 = scale (ev0, scale0);
    *sp1 = inps[inpn - 1];
    *sv1 = scale (ev1, scale3);
    return 0;
}

static double dist_n(Ppoint_t *p, int n)
{
	int		i;
	double	rv;

	rv = 0.0;
	for (i = 1; i < n; i++) {
		rv += sqrt((p[i].x - p[i-1].x)*(p[i].x - p[i-1].x) + (p[i].y - p[i-1].y)*(p[i].y - p[i-1].y));
	}
	return rv;
}

static int splinefits (Pedge_t *edges, int edgen, Ppoint_t pa, Pvector_t va,
        Ppoint_t pb, Pvector_t vb, Ppoint_t *inps, int inpn) {
    Ppoint_t sps[4];
    double a, b;
#if 0
    double d;
#endif
    int pi;
	int	forceflag;
	int first = 1;

	forceflag = (inpn == 2 ? 1 : 0);

#if 0
    d = sqrt ((pb.x - pa.x) * (pb.x - pa.x) + (pb.y - pa.y) * (pb.y - pa.y));
    a = d, b = d;
#else
    a = b = 4;
#endif
    for ( ;; ) {
        sps[0].x = pa.x;
        sps[0].y = pa.y;
        sps[1].x = pa.x + a * va.x / 3.0;
        sps[1].y = pa.y + a * va.y / 3.0;
        sps[2].x = pb.x - b * vb.x / 3.0;
        sps[2].y = pb.y - b * vb.y / 3.0;
        sps[3].x = pb.x;
        sps[3].y = pb.y;

		/* shortcuts (paths shorter than the shortest path) not allowed -
		 * they must be outside the constraint polygon.  this can happen
		 * if the candidate spline intersects the constraint polygon exactly
		 * on sides or vertices.  maybe this could be more elegant, but
		 * it solves the immediate problem. we could also try jittering the
		 * constraint polygon, or computing the candidate spline more carefully,
		 * for example using the path. SCN */

		if (first && (dist_n(sps,4) < (dist_n(inps,inpn) - EPSILON1))) return 0;
		first = 0;

        if (splineisinside (edges, edgen, &sps[0])) {
            growops (opl + 4);
            for (pi = 1; pi < 4; pi++)
                ops[opl].x = sps[pi].x, ops[opl++].y = sps[pi].y;
#if DEBUG >= 1
fprintf (stderr, "success: %f %f\n", a, b);
#endif
            return 1;
        }
        if (a == 0 && b == 0) {
            if (forceflag) {
                growops (opl + 4);
                for (pi = 1; pi < 4; pi++)
                    ops[opl].x = sps[pi].x, ops[opl++].y = sps[pi].y;
#if DEBUG >= 1
fprintf (stderr, "forced straight line: %f %f\n", a, b);
#endif
                return 1;
            }
            break;
        }
        if (a > .01)
            a /= 2, b /= 2;
        else
            a = b = 0;
    }
#if DEBUG >= 1
fprintf (stderr, "failure\n");
#endif
    return 0;
}

static int splineisinside (Pedge_t *edges, int edgen, Ppoint_t *sps) {
    double roots[4];
    int rooti, rootn;
    int ei;
    Ppoint_t lps[2], ip;
    double t, ta, tb, tc, td;

    for (ei = 0; ei < edgen; ei++) {
        lps[0] = edges[ei].a, lps[1] = edges[ei].b;
        /* if ((rootn = splineintersectsline (sps, lps, roots)) == 4)
            return 1; */
        if ((rootn = splineintersectsline (sps, lps, roots)) == 4)
            continue;
        for (rooti = 0; rooti < rootn; rooti++) {
            if (roots[rooti] < EPSILON2 || roots[rooti] > 1 - EPSILON2)
                continue;
            t = roots[rooti];
            td = t * t * t;
            tc = 3 * t * t * (1 - t);
            tb = 3 * t * (1 - t) * (1 - t);
            ta = (1 - t) * (1 - t) * (1 - t);
            ip.x = ta * sps[0].x + tb * sps[1].x +
                    tc * sps[2].x + td * sps[3].x;
            ip.y = ta * sps[0].y + tb * sps[1].y +
                    tc * sps[2].y + td * sps[3].y;
            if (DISTSQ (ip, lps[0]) < EPSILON1 ||
                    DISTSQ (ip, lps[1]) < EPSILON1)
                continue;
            return 0;
        }
    }
    return 1;
}

static int splineintersectsline (Ppoint_t *sps, Ppoint_t *lps,
        double *roots) {
    double scoeff[4], xcoeff[2], ycoeff[2];
    double xroots[3], yroots[3], tv, sv, rat;
    int rootn, xrootn, yrootn, i, j;

    xcoeff[0] = lps[0].x;
    xcoeff[1] = lps[1].x - lps[0].x;
    ycoeff[0] = lps[0].y;
    ycoeff[1] = lps[1].y - lps[0].y;
    rootn = 0;
    if (xcoeff[1] == 0) {
        if (ycoeff[1] == 0) {
            points2coeff (sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
            scoeff[0] -= xcoeff[0];
            xrootn = solve3 (scoeff, xroots);
            points2coeff (sps[0].y, sps[1].y, sps[2].y, sps[3].y, scoeff);
            scoeff[0] -= ycoeff[0];
            yrootn = solve3 (scoeff, yroots);
            if (xrootn == 4)
                if (yrootn == 4)
                    return 4;
                else
                    for (j = 0; j < yrootn; j++)
                        addroot (yroots[j], roots, &rootn);
            else if (yrootn == 4)
                for (i = 0; i < xrootn; i++)
                    addroot (xroots[i], roots, &rootn);
            else
                for (i = 0; i < xrootn; i++)
                    for (j = 0; j < yrootn; j++)
                        if (xroots[i] == yroots[j])
                            addroot (xroots[i], roots, &rootn);
            return rootn;
        } else {
            points2coeff (sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
            scoeff[0] -= xcoeff[0];
            xrootn = solve3 (scoeff, xroots);
            if (xrootn == 4)
                return 4;
            for (i = 0; i < xrootn; i++) {
                tv = xroots[i];
                if (tv >= 0 && tv <= 1) {
                    points2coeff (sps[0].y, sps[1].y, sps[2].y, sps[3].y,
                            scoeff);
                    sv = scoeff[0] + tv * (scoeff[1] + tv *
                        (scoeff[2] + tv * scoeff[3]));
                    sv = (sv - ycoeff[0]) / ycoeff[1];
                    if ((0 <= sv) && (sv <= 1))
                        addroot (tv, roots, &rootn);
                }
            }
            return rootn;
        }
    } else {
        rat = ycoeff[1] / xcoeff[1];
        points2coeff (sps[0].y - rat * sps[0].x, sps[1].y - rat * sps[1].x,
                sps[2].y - rat * sps[2].x, sps[3].y - rat * sps[3].x, scoeff);
        scoeff[0] += rat * xcoeff[0] - ycoeff[0];
        xrootn = solve3 (scoeff, xroots);
        if (xrootn == 4)
            return 4;
        for (i = 0; i < xrootn; i++) {
            tv = xroots[i];
            if (tv >= 0 && tv <= 1) {
                points2coeff (sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
                sv = scoeff[0] + tv * (scoeff[1] + tv *
                    (scoeff[2] + tv * scoeff[3]));
                sv = (sv - xcoeff[0]) / xcoeff[1];
                if ((0 <= sv) && (sv <= 1))
                    addroot (tv, roots, &rootn);
            }
        }
        return rootn;
    }
}

static void points2coeff (double v0, double v1, double v2, double v3,
        double *coeff) {
    coeff[3] = v3 + 3 * v1 - (v0 + 3 * v2);
    coeff[2] = 3 * v0 + 3 * v2 - 6 * v1;
    coeff[1] = 3 * (v1 - v0);
    coeff[0] = v0;
}

static void addroot (double root, double *roots, int *rootnp) {
    if (root >= 0 && root <= 1)
        roots[*rootnp] = root, (*rootnp)++;
}

static Pvector_t normv (Pvector_t v) {
    double d;

    d = sqrt (v.x * v.x + v.y * v.y);
    if (d != 0)
        v.x /= d, v.y /= d;
    return v;
}

static void growops (int newopn) {
    if (newopn <= opn)
        return;
    if (!ops) {
        if (!(ops = (Ppoint_t *) malloc (POINTSIZE * newopn))) {
            prerror ("cannot malloc ops");
            abort ();
        }
    } else {
        if (!(ops = (Ppoint_t *) realloc ((void *) ops,
                POINTSIZE * newopn))) {
            prerror ("cannot realloc ops");
            abort ();
        }
    }
    opn = newopn;
}

static Ppoint_t add (Ppoint_t p1, Ppoint_t p2) {
    p1.x += p2.x, p1.y += p2.y;
    return p1;
}

static Ppoint_t sub (Ppoint_t p1, Ppoint_t p2) {
    p1.x -= p2.x, p1.y -= p2.y;
    return p1;
}

static double dist (Ppoint_t p1, Ppoint_t p2) {
    double dx, dy;

    dx = p2.x - p1.x, dy = p2.y - p1.y;
    return sqrt (dx * dx + dy * dy);
}

static Ppoint_t scale (Ppoint_t p, double c) {
    p.x *= c, p.y *= c;
    return p;
}

static double dot (Ppoint_t p1, Ppoint_t p2) {
    return p1.x * p2.x + p1.y * p2.y;
}

static double B0 (double t)
{
    double tmp = 1.0 - t;
    return tmp * tmp * tmp;
}

static double B1 (double t)
{
    double tmp = 1.0 - t;
    return 3 * t * tmp * tmp;
}

static double B2 (double t)
{
    double tmp = 1.0 - t;
    return 3 * t * t * tmp;
}

static double B3 (double t)
{
    return t * t * t;
}

static double B01 (double t)
{
    double tmp = 1.0 - t;
    return tmp * tmp * (tmp + 3 * t);
}

static double B23 (double t)
{
    double tmp = 1.0 - t;
    return t * t * (3 * tmp + t);
}

#if 0
static int cmpp2efunc (const void *v0p, const void *v1p) {
    p2e_t *p2e0p, *p2e1p;
    double x0, x1;

    p2e0p = (p2e_t *) v0p, p2e1p = (p2e_t *) v1p;
    if (p2e0p->pp->y > p2e1p->pp->y)
        return -1;
    else if (p2e0p->pp->y < p2e1p->pp->y)
        return 1;
    if (p2e0p->pp->x < p2e1p->pp->x)
        return -1;
    else if (p2e0p->pp->x > p2e1p->pp->x)
        return 1;
    x0 = (p2e0p->pp == &p2e0p->ep->a) ? p2e0p->ep->b.x : p2e0p->ep->a.x;
    x1 = (p2e1p->pp == &p2e1p->ep->a) ? p2e1p->ep->b.x : p2e1p->ep->a.x;
    if (x0 < x1)
        return -1;
    else if (x0 > x1)
        return 1;
    return 0;
}

static void listdelete (Pedge_t *ep) {
    elist_t *lp;

    for (lp = elist; lp; lp = lp->next) {
        if (lp->ep != ep)
            continue;
        if (lp->prev)
            lp->prev->next = lp->next;
        if (lp->next)
            lp->next->prev = lp->prev;
        if (elist == lp)
            elist = lp->next;
        free (lp);
        return;
    }
    if (!lp) {
        prerror ("cannot find list element to delete");
        abort ();
    }
}

static void listreplace (Pedge_t *oldep, Pedge_t *newep) {
    elist_t *lp;

    for (lp = elist; lp; lp = lp->next) {
        if (lp->ep != oldep)
            continue;
        lp->ep = newep;
        return;
    }
    if (!lp) {
        prerror ("cannot find list element to replace");
        abort ();
    }
}

static void listinsert (Pedge_t *ep, Ppoint_t p) {
    elist_t *lp, *newlp, *lastlp;
    double lx;

    if (!(newlp = (elist_t *) malloc (sizeof (elist_t)))) {
        prerror ("cannot malloc newlp");
        abort ();
    }
    newlp->ep = ep;
    newlp->next = newlp->prev = NULL;
    if (!elist) {
        elist = newlp;
        return;
    }
    for (lp = elist; lp; lp = lp->next) {
        lastlp = lp;
        lx = lp->ep->a.x + (lp->ep->b.x - lp->ep->a.x) * (p.y - lp->ep->a.y) /
                (lp->ep->b.y - lp->ep->a.y);
        if (lx <= p.x)
            continue;
        if (lp->prev)
            lp->prev->next = newlp;
        newlp->prev = lp->prev;
        newlp->next = lp;
        lp->prev = newlp;
        if (elist == lp)
            elist = newlp;
        return;
    }
    lastlp->next = newlp;
    newlp->prev = lastlp;
    if (!elist)
        elist = newlp;
}
#endif