/*
 * path.c - 2D path operations
 *
 * Written 2011 by Werner Almesberger
 * Copyright 2011 by Werner Almesberger
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <assert.h>

#include "ptrude.h"
#include "path.h"


#define	alloc_type(t)	((t *) malloc(sizeof(t)))
#define	stralloc(s)	strdup(s)


static double deg(double rad)
{
	return rad/M_PI*180.0;
}


static struct path *alloc_path(void)
{
	struct path *path;

	path = alloc_type(struct path);
	path->vertices = NULL;
	path->last = &path->vertices;
	return path;
}


static struct vertex *alloc_vertex(void)
{
	struct vertex *v;

	v = alloc_type(struct vertex);
	v->r = 0;
	v->d = 0;
	v->tag = NULL;
	v->next = NULL;
	v->len = 0;
	return v;
}


static void free_vertex(struct vertex *v)
{
	free(v);
}


void free_path(struct path *path)
{
	struct vertex *v, *next;

	for (v = path->vertices; v; v = next) {
		next = v->next;
		free_vertex(v);
	}
	free(path);
}


static struct vertex *clone_vertex(const struct vertex *v)
{
	struct vertex *new;

	new = alloc_type(struct vertex);
	*new = *v;
	new->next = NULL;
	return new;
}


static void append_vertex(struct path *path, struct vertex *v)
{
	*path->last = v;
	path->last = &v->next;
}


static struct vertex *add_vertex(struct path *path, double x, double y,
    double r, double d, const char *tag)
{
	struct vertex *v;

	v = alloc_vertex();
	v->x = x;
	v->y = y;
	v->r = r;
	v->d = d;
	v->tag = tag;
	append_vertex(path, v);
	return v;
}


double path_set_length(struct path *path)
{
	struct vertex *v;
	double sum = 0;

	if (!path->vertices)
		return 0;
	for (v = path->vertices; v->next; v = v->next) {
		v->len = hypot(v->x-v->next->x, v->y-v->next->y);
		sum += v->len;
	}
	v->len = 0;
	return sum;
}


static void adjust_length(struct vertex *from, struct vertex *to, double len)
{
	struct vertex *v;
	double sum, f;

	if (from == to)
		return;
	sum = 0;
	for (v = from->next; v != to; v = v->next) {
		v->len = hypot(v->x-v->next->x, v->y-v->next->y);
		sum += v->len;
	}

	f = len/sum;
	for (v = from->next; v != to; v = v->next)
		v->len *= f;
}


/*
 * "corner" replaces a corner with a ploygon if the corner is too sharp to be
 * within distance "d" of the bend radius. This may change the point from
 * where we resume drawing (originally the corner point, "b"). "corner"
 * therefore returns the new end of the arc.
 */

static struct vertex *corner(struct path *path, struct vertex *a,
    const struct vertex *b, const struct vertex *c, double r, double d)
{
	/* points to vectors */
	double ax = b->x-a->x;
	double ay = b->y-a->y;
	double bx = c->x-b->x;
	double by = c->y-b->y;

	/* vector length */
	double aa = hypot(ax, ay);
	double bb = hypot(bx, by);

	/* dot and cross product */
	double dp = ax*bx+ay*by; /* a * b = a*b*cos 2t */
	double cp = ax*by-ay*bx; /* |a x b| = a*b*sin 2t */
	double dir = copysign(1, cp);

	/* see corner.fig */
	double dd;	/* "d" of the given vectors */
	double tt;	/* tan t */
	double s;	/* distance between start of arc and corner */
	double t2;	/* angle, t*2 */

	/* see arc.fig */
	double p;	/* half-angle of border side of border segment */
	double q;	/* half-angle of connecting segment */
	double u;	/* length of border side of border segment */
	double v;	/* half-length of connecting segment */
	int n;		/* number of connecting segments (0 if none) */

	double f;	/* scale factor; various uses */
	double fa, fb;	/* scale factors for first and last vertex */
	double ang;	/* current angle, for iteration */
	double x, y;	/* current position; for iteration */
	int i;		/* segment; for iteration */

	struct vertex *v0;	/* first vertex of arc */
	struct vertex *v1;	/* last vertex of arc */


	/*
	 * http://en.wikipedia.org/wiki/Dot_product
	 * dp = a*b*cos 2t
	 *
	 * http://en.wikipedia.org/wiki/Cross_product
	 * cp = a*b*sin 2t
	 *
	 * http://en.wikipedia.org/wiki/Tangent_half-angle_formula
	 * tan t = sin 2t/(1+cos 2t)
	 */
	tt = cp/(aa*bb+dp);

	/*
	 * From s = r*tan t
	 */
	s = fabs(r*tt);

	/*
	 * From r^2+s^2 = (r+d)^2
	 */
	dd = hypot(r, s)-r;

	if (debug) {
		fprintf(stderr, "a = (%g, %g)-(%g, %g) = (%g, %g); |a| = %g\n",
		    b->x, b->y, a->x, a->y, ax, ay, aa);
		fprintf(stderr, "b = (%g, %g)-(%g, %g) = (%g, %g); |b| = %g\n",
		    c->x, c->y, b->x, b->y, bx, by, bb);
		fprintf(stderr, "sin 2t = %g, cos 2t = %g, tan t = %g\n",
		    cp/aa/bb, dp/aa/bb, tt);
		fprintf(stderr, "r = %g, d = %g, s = %g, dd = %g\n",
		    r, d, s, dd);
	}

	/*
	 * We only know how to make a rounded corner if two vectors are
	 * involved. They therefore have to be long enough to accommodate the
	 * entire arc, from beginning to end. Furthermore, we split the
	 * available length in half, one for the inbound arc, the other for the
	 * outbound arc.
	 */

	/*
	 * @@@ Our error checking is a bit overzealous and doesn't provide
	 * enough information to debug any problems. Turn errors into warnings
	 * for now.
	 */
	if (aa/2 < s) {
		fprintf(stderr, "first vector is too short (%g/2 < %g)\n",
		    aa, s);
//		exit(1);
	}
	if (bb/2 < s) {
		fprintf(stderr, "second vector is too short (%g/2 < %g)\n",
		    bb, s);
//		exit(1);
	}

	/*
	 * If the corner is already smooth enough, we just keep what we have.
	 */
	if (dd <= d) {
		v1 = clone_vertex(b);
		append_vertex(path, v1);
		return v1;
	}

	/* Step 1: determine the total angle (2*t) */

	t2 = acos(dp/aa/bb);

	/*
	 * Step 2: determine the maximum angle of the first and last segment.
	 *
	 * We use
	 * r*cos p = r-d
	 * cos p = 1-d/r
	 */

	p = acos(1-d/r);

	/*
	 * Step 3: determine the maximum angle of intermediate segments (if
	 * there are any).
	 *
 	 * We use
	 * (r+d)*cos q = r-d
	 * cos q = r-q/(r+d)
	 */

	q = acos((r-d)/(r+d));

	if (debug)
		fprintf(stderr, "t2 = %g, p(max) = %g, q(max) = %g\n",
		    deg(t2), deg(p), deg(q));

	/*
	 * Step 4: emit the starting point of the arc
	 */

	fa = s/aa;
	x = b->x-fa*ax;
	y = b->y-fa*ay;
	v0 = add_vertex(path, x, y, b->r, b->d, b->tag);
	v0->len = a->len*(1-fa);

	/*
	 * Step 5: determine if we need intermediate points. If yes, how many,
	 * and then proceed to add them.
	 */

	if (t2 > 2*p) {
		n = (int) ceil((t2-2*(p+q))/(2*q));

		/*
		 * We could evenly distribute the slack and try to pick a
		 * smaller value for d, but that seems difficult.
		 *
		 * A drawback of reducing p would be that we may make the
		 * corner unnecessarily sharp, possibly even turning against
		 * the general direction of the turn. We'd still respect the
		 * bend radius and the tolerance, but the result may look weird
		 * anyway.
		 *
		 * For now, we just center the polygon.
		 */
		q = (t2/2-p)/(n+1);

		if (n)
			ang = p+q;
		else {
			ang = t2/2;
			/*
			 * @@@ To do: adjust the radius such that we always hug
			 * the r-d circle (see arc.fig) and usually not the
			 * r+d circle. Right now, it's just the opposite.
			 */
		}

		u = tan(p)*(r-d);
		v = tan(q)*(r-d);
		f = (u+v)/aa;
		for (i = 0; i <= n; i++) {
			x += f*ax*cos(ang-q)-dir*f*ay*sin(ang-q);
			y += dir*f*ax*sin(ang-q)+f*ay*cos(ang-q);
			if (debug)
				fprintf(stderr, "  %d/%d: %g %g @ %g\n", i, n,
				    x, y, deg(ang));
			add_vertex(path, x, y, 0, 0, NULL);
			ang += 2*q;
			f = (2*v)/aa;
		}
	}

	/*
	 * Step 6: emit the finishing point of the arc
	 */

	fb = s/bb;
	v1 = add_vertex(path, b->x+fb*bx, b->y+fb*by, 0, 0, NULL);
	v1->len = b->len*(1-fb);

	/*
	 * Step 7: adjust the nominal length of the segments
	 */

	adjust_length(v0, v1, a->len*fa+b->len*fb);


	return v1;
}

	
struct path *round_path(const struct path *path, double r, double d)
{
	struct path *new;
	struct vertex *prev;
	const struct vertex *v;

	new = alloc_path();
	if (!path->vertices)
		return new;

	prev = clone_vertex(path->vertices);
	append_vertex(new, prev);

	if (!path->vertices->next)
		return new;

	if (prev->r)
		r = prev->r;
	if (prev->d)
		d = prev->d;

	for (v = path->vertices->next; v->next; v = v->next) {
		if (v->r)
			r = v->r;
		if (v->d)
			d = v->d;
		prev = corner(new, prev, v, v->next, r, d);
	}
	append_vertex(new, clone_vertex(v));
	return new;
}


static void move_vertex(struct path *path, const struct vertex *v,
    double nx, double ny, double dist, double r)
{
	struct vertex *new;

	new = clone_vertex(v);
	new->x += nx*dist;
	new->y += ny*dist;
	new->r = r;
	append_vertex(path, new);
}


struct path *stretch_path(const struct path *path, double dist, double r)
{
	struct path *new;		/* new path */
	const struct vertex *v;		/* current vertex (for iteration) */
        const struct vertex *a, *b, *c;	/* previous, current, next vertex */
        double nx, ny;			/* 2D normals */
        double f;			/* factor for normalization */
        double tx, ty;			/* temporary 2D normals */

	new = alloc_path();

	a = path->vertices;
	b = a->next;
	nx = b->y-a->y;
	ny = a->x-b->x;
	f = hypot(nx, ny);
	if (a->r)
		r = a->r;
	move_vertex(new, a, nx/f, ny/f, dist, r);

	for (v = path->vertices->next; v->next; v = v->next) {
		double tmp;

		b = v;
		c = v->next;

		tx = b->y-a->y;
		ty = a->x-b->x;
		f = hypot(tx, ty);
		nx = tx/f;
		ny = ty/f;

		tmp = f;

		tx = c->y-b->y;
		ty = b->x-c->x;
		f = hypot(tx, ty);
		nx += tx/f;
		ny += ty/f;
		if (b->r)
			r = b->r;

		f = hypot(nx, ny);
		nx /= f;
		ny /= f;

		/*
		 * We have this far:
		 * nx, ny = normal on corner, normalized
		 * tmp = |a|, length of vector "a" (A -> B)
		 * dist = the distance by which we stretch
		 *
		 * As shown in stretch.fig, we the length we need is
		 * d' = d/cos(90-t)
		 *
		 * With
		 * http://en.wikipedia.org/wiki/Trigonometric_identities#Symmetry
		 * cos(90-t) = sin t = (n x a)/(|n|*|a|)
		 *
		 * Thus
		 * d' = d/sin(t) - d*(|n|*|a|)/(n x a)
		 *    = d/sin(t) - d*|a|/(n x a)
		 */
		tmp = dist*tmp/(nx*(b->y-a->y)-ny*(b->x-a->x));

		move_vertex(new, b, nx, ny, tmp, r+dist);

		a = v;
	}

	nx = v->y-a->y;
	ny = a->x-v->x;
	f = hypot(nx, ny);
	if (v->r)
		r = v->r;
	move_vertex(new, v, nx/f, ny/f, dist, r);

	return new;
}


struct path *load_path(FILE *file)
{
	struct path *path;
	char buf[1100]; /* plenty :) */
	char buf2[sizeof(buf)];
	char *s;
	float x, y, tmp;
	float r = 0, d = 0;
	const char *tag = NULL;

	path = alloc_path();
	while (fgets(buf, sizeof(buf),file)) {
		s = strchr(buf, '\n');
		if (s)
			*s = 0;
		if (sscanf(buf, "#r=%f", &tmp) == 1) {
			r = tmp;
			continue;
		}
		if (sscanf(buf, "#delta=%f", &tmp) == 1) {
			d = tmp;
			continue;
		}
		if (sscanf(buf, "#tag=%s", buf2) == 1) {
			tag = stralloc(buf2);
			continue;
		}
		if (*buf == '#')
			continue;
		if (sscanf(buf, "%f %f", &x, &y) != 2) {
			fprintf(stderr, "can't parse \"%s\"\n", buf);
			exit(1);
		}

		add_vertex(path, x, y, r, d, tag);

		r = 0;
		d = 0;
		tag = NULL;
	}

	path_set_length(path);
	return path;
}


void save_path(FILE *file, const struct path *path)
{
	const struct vertex *v;

	for (v = path->vertices; v; v = v->next) {
		if (v->r)
			fprintf(file, "#r=%f\n", v->r);
		if (v->d)
			fprintf(file, "#delta=%f\n", v->d);
		if (v->tag)
			fprintf(file, "#delta=%f\n", v->d);
		fprintf(file, "%f %f\n", v->x, v->y);
	}
}