/*
 * path.c - Toolpath operations
 *
 * Written 2010 by Werner Almesberger
 * Copyright 2010 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"


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(path);
}


struct path *path_new(double r_tool)
{
	struct path *path;

	path = alloc_type(struct path);
	path->r_tool = r_tool;
	path->outside = 0;
	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);
	new->outside = old->outside;
	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 path_is_closed(const struct path *path)
{
	if (path->first == path->last)
		return 1;
	return path->first->x == path->last->x &&
	    path->first->y == path->last->y && path->first->z == path->last->z;
}


static void path_add_point(struct path *path, struct point *p)
{
	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);
}


void path_replace(struct path *old, struct path *new)
{
	struct path *next = old->next;

	free_points(old->first);
	*old = *new;
	old->next = next;
	free(new);
}


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 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;
}


struct path *path_find_leftmost(struct path *path)
{
	const struct point *p;
	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;
}