/*
 * ***** BEGIN GPL LICENSE BLOCK *****
 *
 * 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.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * ***** END GPL LICENSE BLOCK *****
 */

/** \file blender/bmesh/tools/bmesh_path.c
 *  \ingroup bmesh
 *
 * Find a path between 2 elements.
 *
 */

#include "MEM_guardedalloc.h"

#include "BLI_math.h"
#include "BLI_linklist.h"
#include "BLI_heap.h"

#include "bmesh.h"
#include "bmesh_path.h"  /* own include */


/* -------------------------------------------------------------------- */
/* Generic Helpers */

/**
 * Use skip options when we want to start measuring from a boundary.
 */
static float step_cost_3_v3_ex(
        const float v1[3], const float v2[3], const float v3[3],
        bool skip_12, bool skip_23)
{
	float d1[3], d2[3];

	/* The cost is based on the simple sum of the length of the two edgees... */
	sub_v3_v3v3(d1, v2, v1);
	sub_v3_v3v3(d2, v3, v2);
	const float cost_12 = normalize_v3(d1);
	const float cost_23 = normalize_v3(d2);
	const float cost = ((skip_12 ? 0.0f : cost_12) +
	                    (skip_23 ? 0.0f : cost_23));

	/* but is biased to give higher values to sharp turns, so that it will take
	 * paths with fewer "turns" when selecting between equal-weighted paths between
	 * the two edges */
	return cost * (1.0f + 0.5f * (2.0f - sqrtf(fabsf(dot_v3v3(d1, d2)))));
}

static float step_cost_3_v3(
        const float v1[3], const float v2[3], const float v3[3])
{
	return step_cost_3_v3_ex(v1, v2, v3, false, false);
}


/* -------------------------------------------------------------------- */
/* BM_mesh_calc_path_vert */

static void verttag_add_adjacent(
        Heap *heap, BMVert *v_a, BMVert **verts_prev, float *cost,
        const struct BMCalcPathParams *params)
{
	const int v_a_index = BM_elem_index_get(v_a);

	{
		BMIter eiter;
		BMEdge *e;
		/* loop over faces of face, but do so by first looping over loops */
		BM_ITER_ELEM (e, &eiter, v_a, BM_EDGES_OF_VERT) {
			BMVert *v_b = BM_edge_other_vert(e, v_a);
			if (!BM_elem_flag_test(v_b, BM_ELEM_TAG)) {
				/* we know 'v_b' is not visited, check it out! */
				const int v_b_index = BM_elem_index_get(v_b);
				const float cost_cut = params->use_topology_distance ?
				        1.0f : len_v3v3(v_a->co, v_b->co);
				const float cost_new = cost[v_a_index] + cost_cut;

				if (cost[v_b_index] > cost_new) {
					cost[v_b_index] = cost_new;
					verts_prev[v_b_index] = v_a;
					BLI_heap_insert(heap, cost_new, v_b);
				}
			}
		}
	}

	if (params->use_step_face) {
		BMIter liter;
		BMLoop *l;
		/* loop over faces of face, but do so by first looping over loops */
		BM_ITER_ELEM (l, &liter, v_a, BM_LOOPS_OF_VERT) {
			if (l->f->len > 3) {
				/* skip loops on adjacent edges */
				BMLoop *l_iter = l->next->next;
				do {
					BMVert *v_b = l_iter->v;
					if (!BM_elem_flag_test(v_b, BM_ELEM_TAG)) {
						/* we know 'v_b' is not visited, check it out! */
						const int v_b_index = BM_elem_index_get(v_b);
						const float cost_cut = params->use_topology_distance ?
						        1.0f : len_v3v3(v_a->co, v_b->co);
						const float cost_new = cost[v_a_index] + cost_cut;

						if (cost[v_b_index] > cost_new) {
							cost[v_b_index] = cost_new;
							verts_prev[v_b_index] = v_a;
							BLI_heap_insert(heap, cost_new, v_b);
						}
					}
				} while ((l_iter = l_iter->next) != l->prev);
			}
		}
	}
}

LinkNode *BM_mesh_calc_path_vert(
        BMesh *bm, BMVert *v_src, BMVert *v_dst, const struct BMCalcPathParams *params,
        bool (*filter_fn)(BMVert *, void *user_data), void *user_data)
{
	LinkNode *path = NULL;
	/* BM_ELEM_TAG flag is used to store visited edges */
	BMVert *v;
	BMIter viter;
	Heap *heap;
	float *cost;
	BMVert **verts_prev;
	int i, totvert;

	/* note, would pass BM_EDGE except we are looping over all faces anyway */
	// BM_mesh_elem_index_ensure(bm, BM_VERT /* | BM_EDGE */); // NOT NEEDED FOR FACETAG

	BM_ITER_MESH_INDEX (v, &viter, bm, BM_VERTS_OF_MESH, i) {
		BM_elem_flag_set(v, BM_ELEM_TAG, !filter_fn(v, user_data));
		BM_elem_index_set(v, i); /* set_inline */
	}
	bm->elem_index_dirty &= ~BM_VERT;

	/* alloc */
	totvert = bm->totvert;
	verts_prev = MEM_callocN(sizeof(*verts_prev) * totvert, __func__);
	cost = MEM_mallocN(sizeof(*cost) * totvert, __func__);

	copy_vn_fl(cost, totvert, 1e20f);

	/*
	 * Arrays are now filled as follows:
	 *
	 * As the search continues, verts_prev[n] will be the previous verts on the shortest
	 * path found so far to face n. BM_ELEM_TAG is used to tag elements we have visited,
	 * cost[n] will contain the length of the shortest
	 * path to face n found so far, Finally, heap is a priority heap which is built on the
	 * the same data as the cost array, but inverted: it is a worklist of faces prioritized
	 * by the shortest path found so far to the face.
	 */

	/* regular dijkstra shortest path, but over faces instead of vertices */
	heap = BLI_heap_new();
	BLI_heap_insert(heap, 0.0f, v_src);
	cost[BM_elem_index_get(v_src)] = 0.0f;

	while (!BLI_heap_is_empty(heap)) {
		v = BLI_heap_popmin(heap);

		if (v == v_dst)
			break;

		if (!BM_elem_flag_test(v, BM_ELEM_TAG)) {
			BM_elem_flag_enable(v, BM_ELEM_TAG);
			verttag_add_adjacent(heap, v, verts_prev, cost, params);
		}
	}

	if (v == v_dst) {
		do {
			BLI_linklist_prepend(&path, v);
		} while ((v = verts_prev[BM_elem_index_get(v)]));
	}

	MEM_freeN(verts_prev);
	MEM_freeN(cost);
	BLI_heap_free(heap, NULL);

	return path;
}

/* -------------------------------------------------------------------- */
/* BM_mesh_calc_path_edge */

static float edgetag_cut_cost_vert(BMEdge *e_a, BMEdge *e_b, BMVert *v)
{
	BMVert *v1 = BM_edge_other_vert(e_a, v);
	BMVert *v2 = BM_edge_other_vert(e_b, v);
	return step_cost_3_v3(v1->co, v->co, v2->co);
}

static float edgetag_cut_cost_face(BMEdge *e_a, BMEdge *e_b, BMFace *f)
{
	float e_a_cent[3], e_b_cent[3], f_cent[3];

	mid_v3_v3v3(e_a_cent, e_a->v1->co, e_a->v1->co);
	mid_v3_v3v3(e_b_cent, e_b->v1->co, e_b->v1->co);

	BM_face_calc_center_mean_weighted(f, f_cent);

	return step_cost_3_v3(e_a_cent, e_b_cent, f_cent);
}

static void edgetag_add_adjacent(
        Heap *heap, BMEdge *e_a, BMEdge **edges_prev, float *cost,
        const struct BMCalcPathParams *params)
{
	const int e_a_index = BM_elem_index_get(e_a);

	/* unlike vert/face, stepping faces disables scanning connected edges
	 * and only steps over faces (selecting a ring of edges instead of a loop) */
	if (params->use_step_face == false) {
		BMIter viter;
		BMVert *v;

		BMIter eiter;
		BMEdge *e_b;

		BM_ITER_ELEM (v, &viter, e_a, BM_VERTS_OF_EDGE) {

			/* don't walk over previous vertex */
			if ((edges_prev[e_a_index]) &&
			    (BM_vert_in_edge(edges_prev[e_a_index], v)))
			{
				continue;
			}

			BM_ITER_ELEM (e_b, &eiter, v, BM_EDGES_OF_VERT) {
				if (!BM_elem_flag_test(e_b, BM_ELEM_TAG)) {
					/* we know 'e_b' is not visited, check it out! */
					const int e_b_index = BM_elem_index_get(e_b);
					const float cost_cut = params->use_topology_distance ?
					        1.0f : edgetag_cut_cost_vert(e_a, e_b, v);
					const float cost_new = cost[e_a_index] + cost_cut;

					if (cost[e_b_index] > cost_new) {
						cost[e_b_index] = cost_new;
						edges_prev[e_b_index] = e_a;
						BLI_heap_insert(heap, cost_new, e_b);
					}
				}
			}
		}
	}
	else {
		BMLoop *l_first, *l_iter;

		l_iter = l_first = e_a->l;
		do {
			BMLoop *l_cycle_iter, *l_cycle_end;

			l_cycle_iter = l_iter->next;
			l_cycle_end = l_iter;

			/* good, but we need to allow this otherwise paths may fail to connect at all */
#if 0
			if (l_iter->f->len > 3) {
				l_cycle_iter = l_cycle_iter->next;
				l_cycle_end = l_cycle_end->prev;
			}
#endif

			do {
				BMEdge *e_b = l_cycle_iter->e;
				if (!BM_elem_flag_test(e_b, BM_ELEM_TAG)) {
					/* we know 'e_b' is not visited, check it out! */
					const int e_b_index = BM_elem_index_get(e_b);
					const float cost_cut = params->use_topology_distance ?
					        1.0f : edgetag_cut_cost_face(e_a, e_b, l_iter->f);
					const float cost_new = cost[e_a_index] + cost_cut;

					if (cost[e_b_index] > cost_new) {
						cost[e_b_index] = cost_new;
						edges_prev[e_b_index] = e_a;
						BLI_heap_insert(heap, cost_new, e_b);
					}
				}
			} while ((l_cycle_iter = l_cycle_iter->next) != l_cycle_end);
		} while ((l_iter = l_iter->radial_next) != l_first);
	}
}


LinkNode *BM_mesh_calc_path_edge(
        BMesh *bm, BMEdge *e_src, BMEdge *e_dst, const struct BMCalcPathParams *params,
        bool (*filter_fn)(BMEdge *, void *user_data), void *user_data)
{
	LinkNode *path = NULL;
	/* BM_ELEM_TAG flag is used to store visited edges */
	BMEdge *e;
	BMIter eiter;
	Heap *heap;
	float *cost;
	BMEdge **edges_prev;
	int i, totedge;

	/* note, would pass BM_EDGE except we are looping over all edges anyway */
	BM_mesh_elem_index_ensure(bm, BM_VERT /* | BM_EDGE */);

	BM_ITER_MESH_INDEX (e, &eiter, bm, BM_EDGES_OF_MESH, i) {
		BM_elem_flag_set(e, BM_ELEM_TAG, !filter_fn(e, user_data));
		BM_elem_index_set(e, i); /* set_inline */
	}
	bm->elem_index_dirty &= ~BM_EDGE;

	/* alloc */
	totedge = bm->totedge;
	edges_prev = MEM_callocN(sizeof(*edges_prev) * totedge, "SeamPathPrevious");
	cost = MEM_mallocN(sizeof(*cost) * totedge, "SeamPathCost");

	copy_vn_fl(cost, totedge, 1e20f);

	/*
	 * Arrays are now filled as follows:
	 *
	 * As the search continues, prevedge[n] will be the previous edge on the shortest
	 * path found so far to edge n. BM_ELEM_TAG is used to tag elements we have visited,
	 * cost[n] will contain the length of the shortest
	 * path to edge n found so far, Finally, heap is a priority heap which is built on the
	 * the same data as the cost array, but inverted: it is a worklist of edges prioritized
	 * by the shortest path found so far to the edge.
	 */

	/* regular dijkstra shortest path, but over edges instead of vertices */
	heap = BLI_heap_new();
	BLI_heap_insert(heap, 0.0f, e_src);
	cost[BM_elem_index_get(e_src)] = 0.0f;

	while (!BLI_heap_is_empty(heap)) {
		e = BLI_heap_popmin(heap);

		if (e == e_dst)
			break;

		if (!BM_elem_flag_test(e, BM_ELEM_TAG)) {
			BM_elem_flag_enable(e, BM_ELEM_TAG);
			edgetag_add_adjacent(heap, e, edges_prev, cost, params);
		}
	}

	if (e == e_dst) {
		do {
			BLI_linklist_prepend(&path, e);
		} while ((e = edges_prev[BM_elem_index_get(e)]));
	}

	MEM_freeN(edges_prev);
	MEM_freeN(cost);
	BLI_heap_free(heap, NULL);

	return path;
}


/* -------------------------------------------------------------------- */
/* BM_mesh_calc_path_face */

static float facetag_cut_cost_edge(BMFace *f_a, BMFace *f_b, BMEdge *e, const void * const f_endpoints[2])
{
	float f_a_cent[3];
	float f_b_cent[3];
	float e_cent[3];

	BM_face_calc_center_mean_weighted(f_a, f_a_cent);
	BM_face_calc_center_mean_weighted(f_b, f_b_cent);
#if 0
	mid_v3_v3v3(e_cent, e->v1->co, e->v2->co);
#else
	/* for triangle fans it gives better results to pick a point on the edge */
	{
		float ix_e[3], ix_f[3], f;
		isect_line_line_v3(e->v1->co, e->v2->co, f_a_cent, f_b_cent, ix_e, ix_f);
		f = line_point_factor_v3(ix_e, e->v1->co, e->v2->co);
		if (f < 0.0f) {
			copy_v3_v3(e_cent, e->v1->co);
		}
		else if (f > 1.0f) {
			copy_v3_v3(e_cent, e->v2->co);
		}
		else {
			copy_v3_v3(e_cent, ix_e);
		}
	}
#endif

	return step_cost_3_v3_ex(
	        f_a_cent, e_cent, f_b_cent,
	        (f_a == f_endpoints[0]), (f_b == f_endpoints[1]));
}

static float facetag_cut_cost_vert(BMFace *f_a, BMFace *f_b, BMVert *v, const void * const f_endpoints[2])
{
	float f_a_cent[3];
	float f_b_cent[3];

	BM_face_calc_center_mean_weighted(f_a, f_a_cent);
	BM_face_calc_center_mean_weighted(f_b, f_b_cent);

	return step_cost_3_v3_ex(
	        f_a_cent, v->co, f_b_cent,
	        (f_a == f_endpoints[0]), (f_b == f_endpoints[1]));
}

static void facetag_add_adjacent(
        Heap *heap, BMFace *f_a, BMFace **faces_prev, float *cost,
        const void * const f_endpoints[2], const struct BMCalcPathParams *params)
{
	const int f_a_index = BM_elem_index_get(f_a);

	/* loop over faces of face, but do so by first looping over loops */
	{
		BMIter liter;
		BMLoop *l_a;

		BM_ITER_ELEM (l_a, &liter, f_a, BM_LOOPS_OF_FACE) {
			BMLoop *l_first, *l_iter;

			l_iter = l_first = l_a;
			do {
				BMFace *f_b = l_iter->f;
				if (!BM_elem_flag_test(f_b, BM_ELEM_TAG)) {
					/* we know 'f_b' is not visited, check it out! */
					const int f_b_index = BM_elem_index_get(f_b);
					const float cost_cut = params->use_topology_distance ?
					        1.0f : facetag_cut_cost_edge(f_a, f_b, l_iter->e, f_endpoints);
					const float cost_new = cost[f_a_index] + cost_cut;

					if (cost[f_b_index] > cost_new) {
						cost[f_b_index] = cost_new;
						faces_prev[f_b_index] = f_a;
						BLI_heap_insert(heap, cost_new, f_b);
					}
				}
			} while ((l_iter = l_iter->radial_next) != l_first);
		}
	}

	if (params->use_step_face) {
		BMIter liter;
		BMLoop *l_a;

		BM_ITER_ELEM (l_a, &liter, f_a, BM_LOOPS_OF_FACE) {
			BMIter litersub;
			BMLoop *l_b;
			BM_ITER_ELEM (l_b, &litersub, l_a->v, BM_LOOPS_OF_VERT) {
				if ((l_a != l_b) && !BM_loop_share_edge_check(l_a, l_b)) {
					BMFace *f_b = l_b->f;
					if (!BM_elem_flag_test(f_b, BM_ELEM_TAG)) {
						/* we know 'f_b' is not visited, check it out! */
						const int f_b_index = BM_elem_index_get(f_b);
						const float cost_cut = params->use_topology_distance ?
						        1.0f : facetag_cut_cost_vert(f_a, f_b, l_a->v, f_endpoints);
						const float cost_new = cost[f_a_index] + cost_cut;

						if (cost[f_b_index] > cost_new) {
							cost[f_b_index] = cost_new;
							faces_prev[f_b_index] = f_a;
							BLI_heap_insert(heap, cost_new, f_b);
						}
					}
				}
			}
		}
	}
}

LinkNode *BM_mesh_calc_path_face(
        BMesh *bm, BMFace *f_src, BMFace *f_dst, const struct BMCalcPathParams *params,
        bool (*filter_fn)(BMFace *, void *user_data), void *user_data)
{
	LinkNode *path = NULL;
	/* BM_ELEM_TAG flag is used to store visited edges */
	BMFace *f;
	BMIter fiter;
	Heap *heap;
	float *cost;
	BMFace **faces_prev;
	int i, totface;

	/* Start measuring face path at the face edges, ignoring their centers. */
	const void * const f_endpoints[2] = {f_src, f_dst};

	/* note, would pass BM_EDGE except we are looping over all faces anyway */
	// BM_mesh_elem_index_ensure(bm, BM_VERT /* | BM_EDGE */); // NOT NEEDED FOR FACETAG

	BM_ITER_MESH_INDEX (f, &fiter, bm, BM_FACES_OF_MESH, i) {
		BM_elem_flag_set(f, BM_ELEM_TAG, !filter_fn(f, user_data));
		BM_elem_index_set(f, i); /* set_inline */
	}
	bm->elem_index_dirty &= ~BM_FACE;

	/* alloc */
	totface = bm->totface;
	faces_prev = MEM_callocN(sizeof(*faces_prev) * totface, __func__);
	cost = MEM_mallocN(sizeof(*cost) * totface, __func__);

	copy_vn_fl(cost, totface, 1e20f);

	/*
	 * Arrays are now filled as follows:
	 *
	 * As the search continues, faces_prev[n] will be the previous face on the shortest
	 * path found so far to face n. BM_ELEM_TAG is used to tag elements we have visited,
	 * cost[n] will contain the length of the shortest
	 * path to face n found so far, Finally, heap is a priority heap which is built on the
	 * the same data as the cost array, but inverted: it is a worklist of faces prioritized
	 * by the shortest path found so far to the face.
	 */

	/* regular dijkstra shortest path, but over faces instead of vertices */
	heap = BLI_heap_new();
	BLI_heap_insert(heap, 0.0f, f_src);
	cost[BM_elem_index_get(f_src)] = 0.0f;

	while (!BLI_heap_is_empty(heap)) {
		f = BLI_heap_popmin(heap);

		if (f == f_dst)
			break;

		if (!BM_elem_flag_test(f, BM_ELEM_TAG)) {
			BM_elem_flag_enable(f, BM_ELEM_TAG);
			facetag_add_adjacent(heap, f, faces_prev, cost, f_endpoints, params);
		}
	}

	if (f == f_dst) {
		do {
			BLI_linklist_prepend(&path, f);
		} while ((f = faces_prev[BM_elem_index_get(f)]));
	}

	MEM_freeN(faces_prev);
	MEM_freeN(cost);
	BLI_heap_free(heap, NULL);

	return path;
}