/*  libgrbs - geometric rubber band sketch model
    Copyright (C) 2021  Tibor 'Igor2' Palinkas
    (Supported by NLnet NGI0 PET Fund in 2021)

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 31 Milk Street, # 960789 Boston, MA 02196 USA

    Contact:
      Project page: http://repo.hu/projects/libgrbs
      lead developer: http://repo.hu/projects/pcb-rnd/contact.html
*/

#include <math.h>

#include "geo.h"

int grbs_bicycle_angles(double cx1, double cy1, double r1, double cx2, double cy2, double r2, double a[4], int cross)
{
	double dx = cx2 - cx1, dy = cy2 - cy1;
	double dlen, alpha, beta;

	dlen = sqrt(dx*dx + dy*dy);

	if (fabs(r2-r1) >= dlen)
		return -1; /* degenerate case: bigger circle contains the smaller one; also catchec concentric */

	if (cross) {
		double tmp = (r1 + r2) / dlen;
		if (tmp > 1.0)
			return -1;
		alpha = acos(tmp);
	}
	else {
		double tmp = (r1 - r2) / dlen;
		if (tmp < -1.0)
			return -1;
		alpha = acos(tmp);
	}
	beta = atan2(dy, dx);

	a[0] = beta + alpha;
	a[1] = beta - alpha;
	if (cross) {
		a[2] = beta + (GRBS_PI - alpha);
		a[3] = beta - (GRBS_PI - alpha);
	}
	else {
		a[2] = a[0];
		a[3] = a[1];
	}

	if (a[0] < 0) a[0] += 2.0*GRBS_PI;
	if (a[1] < 0) a[1] += 2.0*GRBS_PI;
	if (a[2] < 0) a[2] += 2.0*GRBS_PI;
	if (a[3] < 0) a[3] += 2.0*GRBS_PI;

	return 0;
}

void grbs_gen_bicycle_idx(grbs_arc_t *arc, grbs_arc_t *next, int crossbelt, int arc_at_end, int *ai, int *ni)
{
	double dx = next->parent_pt->x - arc->parent_pt->x;
	double dy = next->parent_pt->y - arc->parent_pt->y;
	double ex, ey, ang, cp;

/*
fprintf(stderr, "GEN: (%f;%f -> %f;%f)\n", arc->parent_pt->x, arc->parent_pt->y, next->parent_pt->x, next->parent_pt->y);
fprintf(stderr, "   d: %f %f\n", dx, dy);
*/

	ang = arc_at_end ? arc->sa + arc->da : arc->sa;
	ex = cos(ang);
	ey = sin(ang);
	cp = dx*ey - dy*ex;
	*ai = (cp < 0);
/*fprintf(stderr, "   e: %f %f -> ai %f %d\n", ex, ey, cp, *ai);*/


	ang = arc_at_end ? next->sa : next->sa + next->da;
	ex = cos(ang);
	ey = sin(ang);
	cp = dx*ey - dy*ex;
	*ni = (cp < 0) + 2;
/*fprintf(stderr, "   e: %f %f -> ni %f %d\n", ex, ey, cp, *ni);*/
}


int grbs_angle_in_arc(double arc_sa, double arc_da, double ang, int inclusive)
{
	double arc_ea;
	static const double tolerance = 0.00000000001;

	/* make sure angle is always positive */
	if (ang < 0)
		ang += 2.0 * GRBS_PI;
	else if (ang > 2.0 * GRBS_PI)
		ang -= 2.0 * GRBS_PI;

	if (arc_da < 0) { /* swap endpoints so da is always positive */
		arc_sa = arc_sa + arc_da;
		arc_da = -arc_da;
	}
	if (arc_sa < 0)
		arc_sa += 2.0*GRBS_PI;

	arc_ea = arc_sa + arc_da;

	/* if arc spans from some high value through zero, the end angle has
	   to be larger than 2*pi; if ang is under both start and end, that may
	   be the case so add a full circle to ang, last chance to get it in
	   range */
	if ((arc_sa > ang) && (arc_ea > ang))
		ang += 2.0*GRBS_PI;

	if (inclusive) {
		if ((ang >= arc_sa) && (ang <= arc_ea))
			return 1;
	}
	else {
		if (((ang-tolerance) > arc_sa) && ((ang+tolerance) < arc_ea))
			return 1;
	}

	if (arc_ea > 2.0*GRBS_PI) {
		ang += 2.0*GRBS_PI;
		if (inclusive) {
			if ((ang >= arc_sa) && (ang <= arc_ea))
				return 1;
		}
		else {
			if ((ang > arc_sa) && (ang < arc_ea))
				return 1;
		}
	}

	return 0;
}

double grbs_arc_get_delta(double sa, double ea, int dir)
{
	double da;

	if (dir > 0) {
		da = ea - sa;
		if (da < 0)
			da = 2*GRBS_PI + da;
		else if (da > 2*GRBS_PI)
			da -= 2*GRBS_PI;
		return da;
	}
	else {
		da = sa - ea;
		if (da < 0)
			da = 2*GRBS_PI + da;
		else if (da > 2*GRBS_PI)
			da -= 2*GRBS_PI;
		return -da;
	}
}

int grbs_get_adir(double from_x, double from_y, double to_cx, double to_cy, double to_r, double to_a)
{
	double vx = to_cx - from_x, vy = to_cy - from_y, asplit;
	int top_side;

	/* cut the circle in half by incoming centerline the see which half
	   the actual line hits */
	asplit = atan2(vy, vx);
	top_side = grbs_angle_in_arc(asplit, GRBS_PI, to_a, 1);

	return top_side ? -1 : +1;
}

grbs_arc_t *grbs_next_arc_in_use(grbs_arc_t *arc)
{
	for(arc = arc->link_point.next; arc != NULL; arc = arc->link_point.next)
		if (arc->in_use)
			return arc;
	return NULL;
}

grbs_arc_t *grbs_prev_arc_in_use(grbs_arc_t *arc)
{
	for(arc = arc->link_point.prev; arc != NULL; arc = arc->link_point.prev)
		if (arc->in_use)
			return arc;
	return NULL;
}

