/*  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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA

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

/*#define GRBS_ROUTE_COMMON_TRACE*/

#undef tprintf
#ifdef GRBS_ROUTE_COMMON_TRACE
#include <stdio.h>
#define tprintf printf
#else
#define tprintf grbs_nullprintf
#endif

void grbs_del_arc(grbs_t *grbs, grbs_arc_t *arc)
{
	if (arc->link_2net.parent != NULL)
		gdl_remove(arc->link_2net.parent, arc, link_2net);
	if (arc->link_point.parent != NULL)
		gdl_remove(arc->link_point.parent, arc, link_point);
	if (arc->sline != NULL)
		grbs_line_del(grbs, arc->sline);
	if (arc->eline != NULL)
		grbs_line_del(grbs, arc->eline);
	if (arc->registered)
		grbs_arc_unreg(grbs, arc);
/*	arc->in_use = 0;
	arc->new_in_use = 0;*/
	grbs_arc_free(grbs, arc);
	arc->uid = -1;
}

/* Calculate the radius of a new orbit (arc->new_r) around addr for tn */
static double grbs_get_new_radius_(grbs_t *grbs, grbs_2net_t *tn, grbs_arc_t *arc)
{
	return arc->r + arc->copper + tn->copper + GRBS_MAX(arc->clearance, tn->clearance);
}

static double grbs_get_new_radius(grbs_t *grbs, grbs_2net_t *tn, grbs_addr_t *addr)
{
	assert(addr->type & ADDR_ARC_CONVEX);
	assert((addr->type & 0xF0) != 0);

	return grbs_get_new_radius_(grbs, tn, addr->obj.arc);
}

static void grbs_get_arc_end_(grbs_t *grbs, grbs_2net_t *tn, grbs_arc_t *arc, int which, double *ex, double *ey)
{
	double ang, r;

	assert((which == 0) || (which == 1));

	ang = arc->new_sa;
	if (which == 1)
		ang += arc->new_da;

	r = grbs_get_new_radius_(grbs, tn, arc);

	*ex = arc->parent_pt->x + cos(ang) * r;
	*ey = arc->parent_pt->y + sin(ang) * r;
}

static void grbs_get_new_arc_end(grbs_t *grbs, grbs_2net_t *tn, grbs_addr_t *addr, int which, double *ex, double *ey)
{
	assert(addr->type & ADDR_ARC_CONVEX);
	assert((addr->type & 0xF0) != 0);

	grbs_get_arc_end_(grbs, tn, addr->obj.arc, which, ex, ey);
}

/* Return if ang is a valid entry/exit point between two existing arcs
   (addr being the arc below) */
static int grbs_angle_visible_between_arcs(grbs_t *grbs, grbs_2net_t *tn, grbs_addr_t *addr, double ang)
{
	grbs_arc_t *under = addr->obj.arc;
	grbs_arc_t *above = under->link_point.next;

	if ((above == NULL) && (under->da == 0)) /* new segment with yet zero length - always accept */
		return 1;

	/* has to be within the arc under, else it will cross the under-arc or its line */
	if (under->in_use && !grbs_angle_in_arc(under->sa, under->da, ang, 1)) {
		coll_report_arc(grbs, tn, under);
		return 0;
	}

	/* has to be outside the arc above, else it will cross the above-arc or its line */
	if ((above != NULL) && (grbs_angle_in_arc(above->sa, above->da, ang, 0))) {
		coll_report_arc(grbs, tn, above);
		return 0;
	}

	return 1;
}

/* Check if a given angle range (for an arc) crosses any incident line of that pt */
static int arc_crosses_any_incident(grbs_t *grbs, grbs_2net_t *tn, grbs_point_t *pt, double sa, double da)
{
	grbs_arc_t *i;

	for(i = gdl_first(&pt->incs); i != NULL; i = gdl_next(&pt->incs, i)) {
		if (i->in_use && (grbs_angle_in_arc(sa, da, i->sa, 0))) {
			coll_report_arc(grbs, tn, i);
			return 1;
		}
	}

	return 0;
}

/* find next arc in dir: the one that starts or ends closest to our
   startpoint in the right direction */
static grbs_arc_t *grbs_next_seg(grbs_point_t *from_pt, int from_segi, double from_arc_sa, int adir)
{
	grbs_arc_t *arc, *best_arc = NULL;
	double da, best_da = 4*GRBS_PI;
	int n;

	for(n = 0; n < GRBS_MAX_SEG; n++) {
		if (n == from_segi) /* self collision */
			continue;
		arc = gdl_first(&from_pt->arcs[n]);
		if (arc == NULL)
			continue;
		da = grbs_arc_get_delta(from_arc_sa, arc->sa, adir);
		if (da < best_da) {
			best_da = da;
			best_arc = arc;
		}
		da = grbs_arc_get_delta(from_arc_sa, arc->sa + arc->da, adir);
		if (da < best_da) {
			best_da = da;
			best_arc = arc;
		}
	}

	return best_arc;
}

/* "from" is an existing arc with a start angle where the current route
   entered the orbit in its sector; "end_a" is where it should exit (the
   direction is known from "from"). This function checks wether this enter-exit
   is valid and potentially merges orbit segments */
int grbs_arc_tune4exit(grbs_t *grbs, grbs_2net_t *tn, grbs_addr_t *from, double end_a)
{
	grbs_arc_t *best_arc = NULL, *sentinel;
	double nsa, nda;
	int adir = from->obj.arc->new_adir, is_new_seg, collides_next;
	grbs_point_t *from_pt = addr_point(from);

	assert(from->type & ADDR_ARC_END);

	nsa = from->obj.arc->new_sa;
	nda = grbs_arc_get_delta(nsa, end_a, adir);

	if (arc_crosses_any_incident(grbs, tn, from->obj.arc->parent_pt, nsa, nda)) {
		tprintf("tune4exit: incident cross\n");
		return -1;
	}

	from->obj.arc->new_da = nda;

	is_new_seg = ((from->obj.arc->link_point.next == NULL) && (from->obj.arc->link_point.prev == NULL) && !from->obj.arc->in_use);

	best_arc = grbs_next_seg(from_pt, from->obj.arc->segi, nsa, adir);
/*	tprintf("@@@ best arc: %p\n", best_arc);*/

	/* first arc around this point */
	if (best_arc == NULL) {
			grbs_arc_t *below_me = from->obj.arc, *above_me = grbs_next_arc_in_use(from->obj.arc);

			/* check for local collision: new arc can not cut into the arc above */
			if (above_me != NULL) {
				if (grbs_angle_in_arc(above_me->sa, above_me->da, nsa, 0) || grbs_angle_in_arc(above_me->sa, above_me->da, nsa+nda, 0)) {
					tprintf("tune4exit: extend original seg: crossing above_me\n");
					from->obj.arc->new_da = 0;
					return -1;
				}
			}

			/* check for local collision: the arc below can not cut into new arc */
			if (below_me->in_use) {
				if (grbs_angle_in_arc(nsa, nda, below_me->sa, 0) || grbs_angle_in_arc(nsa, nda, below_me->sa+below_me->da, 0)) {
					tprintf("tune4exit: extend original seg: crossing below_me\n");
					from->obj.arc->new_da = 0;
					return -1;
				}
			}

		/* check if it would collide globally */
		if (coll_check_arc(grbs, tn, from->obj.arc, 1)) {
			from->obj.arc->new_da = 0;
			return -1;
		}
/*		tprintf(" -> first arc\n");*/
		goto fin;
	}

 /* whether the next existing arc is within our new arc */
	collides_next = grbs_angle_in_arc(nsa, nda, best_arc->sa, 1) || grbs_angle_in_arc(nsa, nda, best_arc->sa + best_arc->da, 1);


	/* 'from' can extend anything only if it's a new segment - we never merge
	   existing segments as that would always make crossings */
	if (!is_new_seg) {
		if (collides_next) {
			grbs_arc_t *first = best_arc->link_point.next; /* collision detection happened at a sentinel; look at the first real arc to have a 2net */
			if (first != NULL)
				coll_report_arc(grbs, tn, first);
			goto fail; /* new arc within an existing segment would cross a different segment */
		}
		goto fin;
	}

	/* from is a new segment, check if it needs to be merged with the next */
	if (collides_next) {
		int to_sid = best_arc->segi;
/*		tprintf(" --> extend!\n");*/

		/* The only possible way the new, wider arc would be able to extend
		   is if it becomes the first arc. By now we know it starts before the
		   lowest orbit, because that's how it got a new segment originally. Check
		   if end is beyond the lowest orbit of the target segment (best_arc) */
		if (grbs_angle_in_arc(best_arc->sa, best_arc->da, nsa+nda, 0)) {
			grbs_arc_t *first;

			collided:;
			first = best_arc->link_point.next; /* collision detection happened at a sentinel; look at the first real arc to have a 2net */
			if (first != NULL)
				coll_report_arc(grbs, tn, first);
			goto fail; /* existing arc would cross our new arc */
		}

		/* corner case: arcs touch on nsa but then nsa+nda is within best so it's
		   a crossing; see regression/inv_loop.grbs */
		if (grbs_angle_in_arc(best_arc->sa, best_arc->da, nsa, 0))
			goto collided; /* existing arc would cross our new arc */

		/* the arc we are about to extend needs to be smaller so it doesn't cross
		   our ends */
		if (!grbs_angle_in_arc(nsa, nda, best_arc->sa, 1) || !grbs_angle_in_arc(nsa, nda, best_arc->sa + best_arc->da, 1))
			goto collided; /* existing arc would cross our new arc */

		/* spiral: we are inserting a widest arc in the convex stack so it goes
		   to the bottom; if the bottom already has a ->new, that means we collided
		   with ourselves while routing. This typically means we are doing an
		   unnecessary spiral and we should deny that. */
		if (best_arc->new_in_use)
			goto fail;

		/* remove 'from' from its new segment and add it as lowest orbit under best_arc */
		gdl_remove(&from_pt->arcs[from->obj.arc->segi], from->obj.arc, link_point);

		sentinel = gdl_first(&from_pt->arcs[to_sid]);
		if (!sentinel->new_in_use) {
			gdl_remove(&from_pt->arcs[to_sid], sentinel, link_point);
			gdl_insert(&from_pt->arcs[to_sid], from->obj.arc, link_point);
			from->obj.arc->sa = sentinel->sa;
			from->obj.arc->da = sentinel->da;
			grbs_del_arc(grbs, sentinel);
		}
		else
			gdl_insert_after(&from_pt->arcs[to_sid], sentinel, from->obj.arc, link_point);

		from->obj.arc->segi = to_sid;

		if (grbs_path_dry_realize(grbs, tn, from, 0) != 0) {
			tprintf("tun4exit dry realize fail\n");
		}

	}

	fin:;
	if (coll_check_arc(grbs, tn, from->obj.arc, 1)) {
		tprintf("tune4exit collision failed\n");
		goto fail;
	}

	return 0;

	fail:;
	from->obj.arc->new_da = 0; /* keep the to-be-new-seg */
	return -1;
}

/* Decide if the exit side of an arc continues the right direction; returns
   non-zero for invalid curving. Invalid curving is a concave go-around that
   has started as convex: the incoming line starts a tangential arc but the
   next exit of that arc is not continuing forward but goes backward */
static int grbs_invalid_convex_(grbs_t *grbs, grbs_point_t *pt, double r, double arc_ang, int adir, double other_x, double other_y)
{
	double cs = cos(arc_ang), sn = sin(arc_ang);
	double rx = cs * r, ry = sn * r; /* radius vector */
	double tx = -ry, ty = rx; /* tangent vector */
	double ax = pt->x + rx, ay = pt->y + ry; /* arc endpoint */
	double vx = other_x - ax, vy = other_y - ay; /* outgoing line vector */
	double diff;
	int res;

	if (adir < 0) {
		tx = -tx;
		ty = -ty;
	}

/*	tang = atan2(ty, tx); vang = atan2(vy, vx); res = fabs(tang-vang) > 0.001; */

	diff = atan2(ty*vx - vy*tx, tx*vx + ty*vy); /* same as atan(t) - atan(v); see https://en.wikipedia.org/wiki/Atan2 */
	res = (diff < -0.001) || (diff > 0.001);

	tprintf(" Invalid convex_: v=(%f;%f) t=(%f;%f) arcdir=%d -> %d\n", vx, vy, tx, ty, adir, res);
	return res;
}

/* choose the angle-side that continues the trace: check delta angle
   for both possible angle-sides in our predefined trace direction and
   pick the one that is shorter and is convex */
static int from_arc_ang_side(grbs_t *grbs, grbs_addr_t *from, double to_x, double to_y, double a[2])
{
	int as, from_ang_side = -1;
	double best_da;

	assert(from->obj.arc->new_adir != 0);

	best_da = 2*GRBS_PI+1;
	for(as = 0; as < 2; as++) {
		double da = fabs(grbs_arc_get_delta(from->obj.arc->new_sa, a[as], from->obj.arc->new_adir));
		if (da < best_da) {
			if (!grbs_invalid_convex_(grbs, from->obj.arc->parent_pt, from->obj.arc->new_r, a[as], from->obj.arc->new_adir, to_x, to_y)) {
				from_ang_side = as;
				best_da = da;
			}
		}
	}

	return from_ang_side;
}

grbs_arc_t *grbs_new_sentinel(grbs_t *grbs, grbs_point_t *pt, double sa, double da, int *seg_out)
{
	int n;

	for(n = 0; n < GRBS_MAX_SEG; n++) {
		grbs_arc_t *seg = gdl_first(&pt->arcs[n]);
		if (seg == NULL) {
			seg = grbs_arc_new(grbs, pt, n, GRBS_MAX(pt->copper, 0.0001), sa, da);
			seg->copper = 0;
			seg->clearance = pt->clearance;
			if (seg_out != NULL) *seg_out = n;
			return seg;
		}
	}

	if (seg_out != NULL) *seg_out = -1;
	return NULL;
}

int grbs_get_seg_idx(grbs_t *grbs, grbs_point_t *pt, double ang, int alloc)
{
	int n;

	/* simple case: angle is within the range of an existing segment */
	for(n = 0; n < GRBS_MAX_SEG; n++) {
		grbs_arc_t *seg = gdl_first(&pt->arcs[n]);
		if (seg != NULL) {
			if ((seg->new_in_use) && (grbs_angle_in_arc(seg->new_sa, seg->new_da, ang, 1)))
				return -1; /* Spiral: we are about to enter above a new orbit we created */

			/* do not chekc for seg->in_use: first arc could be a sentinel representing the angle span */
			if (grbs_angle_in_arc(seg->sa, seg->da, ang, 1)) {
				if (seg->new_in_use)
					return -1; /* Spiral: we are about to enter under a new orbit we created */
				return n;
			}
		}
	}

	/* ang is in an empty zone, allocate a new sentinel, zero length */
	if (alloc && (grbs_new_sentinel(grbs, pt, ang, 0, &n) != NULL))
		return n;

	return -1; /* probably ran out of available sentinel slots */
}

static int has_anything_above(grbs_arc_t *seg)
{
	for(seg = seg->link_point.next; seg != NULL; seg = seg->link_point.next)
		if (seg->in_use || seg->new_in_use)
			return 1;
	return 0;
}

/* returns 1 if sentinel got removed; if after_del is true, also tune
   the sentinel to match first arc's fields; this is useful
   after the first arc has been removed and the sentinel needs to be
   "set back to it's original state". */
int grbs_clean_unused_sentinel_seg(grbs_t *grbs, grbs_point_t *pt, int segi, int after_del)
{
	grbs_arc_t *seg = gdl_first(&pt->arcs[segi]), *f;

	if ((seg != NULL) && !seg->new_in_use && !has_anything_above(seg)) {
		grbs_del_arc(grbs, seg);
		return 1;
	}

	if ((seg != NULL) && after_del) {
		for(f = gdl_next(&pt->arcs[segi], seg); f != NULL; f = gdl_next(&pt->arcs[segi], f)) {
			if (f->in_use) {
				seg->sa = f->sa;
				seg->da = f->da;
				goto normalize;
			}
			if (f->new_in_use) {
				seg->sa = f->new_sa;
				seg->da = f->new_da;
				goto normalize;
			}
		}
	}

	return 0;

	normalize:;
	if (seg->da < 0) { /* swap endpoints so da is always positive */
		seg->sa = seg->sa + seg->da;
		seg->da = -seg->da;
	}
	if (seg->sa < 0)
		seg->sa += 2.0*GRBS_PI;
	else if (seg->sa > 2.0*GRBS_PI)
		seg->sa -= 2.0*GRBS_PI;

	return 0;
}

/* remove any orphaned sentinel so that it won't limit new segments or
   segment expansion */
void grbs_clean_unused_sentinel(grbs_t *grbs, grbs_point_t *pt)
{
	int segi, cnc;

	for(cnc = 0; cnc < 2; cnc++)
		for(segi = 0; segi < GRBS_MAX_SEG; segi++)
			grbs_clean_unused_sentinel_seg(grbs, pt, segi, 0);
}

static double grbs_self_isect_convex_r2_cline(grbs_t *grbs, grbs_point_t *pt, int da_positive, g2d_cline_t *cline)
{
	double dx, dy;
	g2d_offs_t o;
	g2d_vect_t vpt, vcr;

	vpt.x = pt->x; vpt.y = pt->y;

	/* if arc is going the long way around, our direct line-of-sight is on
	   the other side of point center, we are safe with any small r */
	if (g2d_side_cline_pt(cline, vpt) < 0) {
		if (!da_positive)
			return 0;
	}
	else {
		if (da_positive)
			return 0;
	}

	o = g2d_project_pt_cline(vpt, cline);
	if ((o < 0.0) || (o > 1.0)) {
		assert("!grbs_self_isect_convex_r_: line out of range");
		return 0;
	}
	vcr = g2d_cline_offs(cline, o);



	dx = vcr.x - vpt.x;
	dy = vcr.y - vpt.y;
	return dx*dx + dy*dy;
}

/* Check realized arc and return the minimum a->r value where it is still
   convex. */
double grbs_self_isect_convex_r2(grbs_t *grbs, grbs_arc_t *arc)
{
	grbs_arc_t *prev = arc->link_2net.prev, *next = arc->link_2net.next;
	g2d_cline_t cline;

	if ((prev == NULL) || (next == NULL))
		return 0;

	assert(arc->sline != NULL);
	assert(arc->eline != NULL);
	cline.p1.x = arc->sline->x1; cline.p1.y = arc->sline->y1;
	cline.p2.x = arc->eline->x2; cline.p2.y = arc->eline->y2;

	return grbs_self_isect_convex_r2_cline(grbs, arc->parent_pt, arc->da > 0, &cline);
}

