#include <stdio.h>
#include "pathplan.h"

static Ppoint_t polypoints[] = {
    { 0.0, 0.0 },
    { 2.0, 0.0 },
    { 2.0, 2.0 },
    { 3.0, 2.0 },
    { 3.0, 0.0 },
    { 5.0, 0.0 },
    { 5.0, 1.0 },
    { 4.0, 1.0 },
    { 4.0, 3.0 },
    { 1.0, 3.0 },
    { 1.0, 1.0 },
    { 0.0, 1.0 }
};

static Ppoint_t ps[] = {
    { 4.9, 0.9 },
    { 0.05, 0.9 }
};

static Pvector_t evs[] = {
    { 0.0, 0.0 },
    { 0.0, 0.0 }
};

static Pedge_t edges[100];
static int edgen;

int main (int argc, char **argv) {
    Ppoly_t poly;
    Ppoint_t *outpoints;
    int outpointn, pi, ei;

    poly.ps = &polypoints[0];
    poly.pn = sizeof (polypoints) / sizeof (Ppoint_t);

    Pshortestpath (&poly, ps, &outpoints, &outpointn);
    for (pi = 0; pi < outpointn; pi++)
        printf ("%d %f %f\n", pi, outpoints[pi].x, outpoints[pi].y);

    edgen = poly.pn;
    for (ei = 0; ei < edgen; ei++) {
        edges[ei].a = polypoints[ei];
        edges[ei].b = polypoints[(ei + 1) % poly.pn];
    }
    edgen += 4;
    edges[ei + 0].a.x = 2.2, edges[ei + 0].a.y = 2.02;
    edges[ei + 0].b.x = 2.2, edges[ei + 0].b.y = 2.7;
    edges[ei + 1].a.x = 2.2, edges[ei + 1].a.y = 2.7;
    edges[ei + 1].b.x = 2.7, edges[ei + 1].b.y = 2.7;
    edges[ei + 2].a.x = 2.7, edges[ei + 2].a.y = 2.7;
    edges[ei + 2].b.x = 2.7, edges[ei + 2].b.y = 2.02;
    edges[ei + 3].a.x = 2.7, edges[ei + 3].a.y = 2.02;
    edges[ei + 3].b.x = 2.2, edges[ei + 3].b.y = 2.02;
    Proutespline (edges, edgen, outpoints, outpointn, evs,
            &outpoints, &outpointn);
    for (pi = 0; pi < outpointn; pi++)
        printf ("%d %f %f\n", pi, outpoints[pi].x, outpoints[pi].y);
}
