/*
 * path.c - Toolpath operations
 *
 * Written 2010-2011 by Werner Almesberger
 * Copyright 2010-2011 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 <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>

#include "util.h"
#include "path.h"


/*
 * We allow for a bit of tolerance, to absorb the rounding errors KiCad
 * produces with designs using a metric grid.
 */

#define	EPSILON_MM	0.006	/* 6 um */


static void free_points(struct point *points)
{
	struct point *next;

	while (points) {
		next = points->next;
		free(points);
		points = next;
	}
}


void path_free(struct path *path)
{
	free_points(path->first);
	free((void *) path->id);
	free(path);
}


struct path *path_new(double r_tool, const char *id)
{
	struct path *path;

	path = alloc_type(struct path);
	path->r_tool = r_tool;
	path->outside = 0;
	path->notch = 0;
	path->id = id ? stralloc(id) : NULL;
	path->first = path->last = NULL;
	path->next = NULL;
	return path;
}


static struct path *path_from(const struct path *old)
{
	struct path *new;

	new = path_new(old->r_tool, old->id);
	new->outside = old->outside;
	new->notch = old->notch;
	return new;
}


static struct point *clone_point(struct point *p)
{
	struct point *n;

	n = alloc_type(struct point);
	n->x = p->x;
	n->y = p->y;
	n->z = p->z;
	n->next = NULL;
	return n;
}


static int points_eq(const struct point *a, const struct point *b)
{
	if (hypot(a->x-b->x, a->y-b->y) > EPSILON_MM)
		return 0;
	if (fabs(a->z-b->z) > EPSILON_MM)
		return 0;
	return 1;
}


static int path_is_closed(const struct path *path)
{
	if (path->first == path->last)
		return 1;
	return points_eq(path->first, path->last);
}


static void path_add_point(struct path *path, struct point *p)
{
	if (path->last &&
	    path->last->x == p->x && path->last->y == p->y &&
	    path->last->z == p->z) {
		free(p);
		return;
	}
	p->next = NULL;
	if (path->last)
		path->last->next = p;
	else
		path->first = p;
	path->last = p;
}


void path_add(struct path *path, double x, double y, double z)
{
	struct point *p;

	p = alloc_type(struct point);
	p->x = x;
	p->y = y;
	p->z = z;
	return path_add_point(path, p);
}


struct path *path_reverse(const struct path *path)
{
	struct path *new;
	const struct point *p;
	struct point *n;

	new = path_from(path);
	for (p = path->first; p; p = p->next) {
		n = alloc_type(struct point);
		n->x = p->x;
		n->y = p->y;
		n->z = p->z;
		n->next = new->first;
		if (!new->last)
			new->last = n;
		new->first = n;
	}
	return new;
}


static void path_reverse_inplace(struct path *path)
{
	struct point *points = NULL, *p, *next;

	path->last = path->first;
	for (p = path->first; p; p = next) {
		next = p->next;
		p->next = points;
		points = p;
	}
	path->first = points;
}


static struct point *offset_point(const struct point *a, const struct point *b,
    const struct point *c, double off, int left)
{
	double ax, ay, bx, by;
	double aa, bb;
	double nx, ny;
	double angle, f;
	struct point *p;

	ax = b->x-a->x;
	ay = b->y-a->y;
	bx = c->x-b->x;
	by = c->y-b->y;

	aa = hypot(ax, ay);
	bb = hypot(bx, by);

	if (left) {
		nx = -(ay/aa+by/bb);
		ny = ax/aa+bx/bb;
	} else {
		nx = ay/aa+by/bb;
		ny = -(ax/aa+bx/bb);
	}

	/* angle between AB and BC */
	angle = acos(-(ax*bx+ay*by)/aa/bb);

	/* multiplier for combination of normal vectors */
	f = off/sin(angle/2);
	f /= hypot(nx, ny);

	nx *= f;
	ny *= f;

	p = alloc_type(struct point);
	p->x = b->x+nx;
	p->y = b->y+ny;
	p->z = b->z;
	p->next = NULL;

	return p;
}


static int left_turn(const struct point *a, const struct point *b,
    const struct point *c)
{
	double ax, ay, bx, by;

	ax = b->x-a->x;
	ay = b->y-a->y;
	bx = c->x-b->x;
	by = c->y-b->y;

	return (ax*by-ay*bx) >= 0;
}


/*
 * Angle in counter-clockwise direction to turn at point B when coming from A
 * in order to face towards C.
 */

static double angle_3(const struct point *a, const struct point *b,
    const struct point *c)
{
	double ax, ay, bx, by;
	double aa, bb;
	double angle;

	ax = b->x-a->x;
	ay = b->y-a->y;
	bx = c->x-b->x;
	by = c->y-b->y;

	aa = hypot(ax, ay);
	bb = hypot(bx, by);

	angle = acos((ax*bx+ay*by)/aa/bb)/M_PI*180.0;

	return (ax*by-ay*bx) >= 0 ? angle : -angle;
}


/*
 * If we predominantly turn to the right, then the tool must be on the
 * left-hand side. Otherwise, it's on the right.
 */

int path_tool_is_left(const struct path *path)
{
	const struct point *prev, *p, *next;
	double a = 0;

	assert(path_is_closed(path));
	prev = path->first;
	for (p = path->first->next; p; p = p->next) {
		next = p->next ? p->next : path->first->next;
		a += angle_3(prev, p, next);
		prev = p;
	}
	return a < 0;
}


/*
 * http://www.makecnc.com/bones.jpg
 */


static struct point *dog_point(const struct point *edge,
    const struct point *tool, double off)
{
	double vx, vy, v;
	struct point *p;

	vx = edge->x-tool->x;
	vy = edge->y-tool->y;
	v = hypot(vx, vy);

	vx *= 1-off/v;
	vy *= 1-off/v;

	p = alloc_type(struct point);
	p->x = tool->x+vx;
	p->y = tool->y+vy;
	p->z = tool->z;
	p->next = NULL;

	return p;
}


/*
 * The tool is on the "left" side of the path. E.g., if the path goes from
 * 6 o'clock to 12 o'clock, the tool would be at 9 o'clock.
 */

struct path *path_offset(const struct path *path, int left, int notch)
{
	struct path *new;
	const struct point *prev, *p, *next;
	struct point *n, *n2;
	int dog;

	assert(path_is_closed(path));
	new = path_from(path);
	prev = path->first;
	for (p = path->first->next; p; p = p->next) {
		next = p->next ? p->next : path->first->next;
		n = offset_point(prev, p, next, path->r_tool, left);
		dog = notch && left_turn(prev, p, next) == left;
		if (dog)
			n2 = clone_point(n);
		path_add_point(new, n);
		if (dog) {
			path_add_point(new, dog_point(p, n2, path->r_tool));
			path_add_point(new, n2);
		}
		prev = p;
	}
	path_add_point(new, clone_point(new->first));
	return new;
}


const struct path *path_find_leftmost(const struct path *path)
{
	const struct point *p;
	const struct path *best = NULL;
	double best_x = HUGE_VAL;

	while (path) {
		for (p = path->first; p; p = p->next)
			if (p->x < best_x) {
				best = path;
				best_x = p->x;
				break;
			}
		path = path->next;
	}
	return best;
}


static int attr_eq(const struct path *a, const struct path *b)
{
	return a->r_tool == b->r_tool && a->outside == b->outside &&
	    a->notch == b->notch;
}


struct path *path_connect(struct path *path)
{
	struct path **a, **b;
	struct path *tmp;

again:
	for (a = &path; *a; a = &(*a)->next)
		for (b = &(*a)->next; *b; b = &(*b)->next) {
			if (!attr_eq(*a, *b))
				continue;
			if (points_eq((*a)->last, (*b)->last))
				path_reverse_inplace(*b);
			if (points_eq((*a)->last, (*b)->first)) {
				(*a)->last->next = (*b)->first->next;
				(*a)->last = (*b)->last;
				free((*b)->first);
				tmp = *b;
				*b = tmp->next;
				free(tmp);
				goto again;
			}
			if (points_eq((*a)->first, (*b)->first))
				path_reverse_inplace(*b);
			if (points_eq((*a)->first, (*b)->last)) {
				(*b)->last->next = (*a)->first->next;
				(*b)->last = (*a)->last;
				free((*a)->first);
				tmp = *a;
				*a = tmp->next;
				free(tmp);
				goto again;
			}
		}
	return path;
}


void path_stats(const struct path *path)
{
	int paths = 0, segs = 0;
	double len = 0;
	const struct point *p;

	while (path) {
		paths++;
		for (p = path->first; p; p = p->next) {
			if (!p->next)
				continue;
			segs++;
			len += hypot(hypot(p->x-p->next->x, p->y-p->next->y),
			    p->z-p->next->z);
		}
		path = path->next;
	}
	fprintf(stderr, "%d path%s, %d segment%s, %f mm\n",
	    paths, paths == 1 ? "" : "s", segs, segs == 1 ? "" : "s", len);
}