#include "dijkstragraph.h"
#include <stdlib.h>
#include <glib/glist.h>
#include <math.h>
#include "logging.h"

#define FP_THRESHOLD 0.01

int dijkstra_node_same (struct dijkstra_node *node_a,
			struct dijkstra_node *node_b)
{
	int same = 1;

	// Not the proper way to do it, but it works for now.
	same &= (fabs(node_a->position.x - node_b->position.x) < FP_THRESHOLD);
	same &= (fabs(node_a->position.y - node_b->position.y) < FP_THRESHOLD);
	same &= (node_a->position.z == node_b->position.z);

	return same;
}

/*!
 * \brief dijkstra_node_new creates a new dijkstra_node
 * \param x x position
 * \param y y position
 * \return a new dijkstra node or NULL. Needs to be freed manually.
 */
struct dijkstra_node *dijkstra_node_new(float x,
					float y,
					int z,
					int uid)
{
	struct dijkstra_node *node;

	node = malloc(sizeof(*node));
	if (node) {
		node->position.x = x;
		node->position.y = y;
		node->position.z = z;
		node->uid = uid;
		node->paths = NULL;
	}

	return node;
}

struct dijkstra_node *dijkstra_node_exists(GList *list,
					   struct dijkstra_node *node)
{
	GList *l;
	struct dijkstra_node *node_existing;

	for (l = list; l != NULL; l = l->next) {
		node_existing = (struct dijkstra_node*) l->data;
		if (dijkstra_node_same(node, node_existing)) {
			return node_existing;
		}
	}

	return NULL;
}

/*!
 * \brief dijkstra_node_new_to_list creates a new node entry if it does not already exist in the list
 * \param list list of dijkstra_node
 * \param x
 * \param y
 * \param is_new When != NULL: is_new will be set to true if a new node had to be created
 * \return a dijkstra_node with the specified position (either newly created or from the list). Or NULL on error.
 */
struct dijkstra_node *dijkstra_node_new_to_list(GList **list,
						float x,
						float y,
						int z)
{
	struct dijkstra_node *node;
	struct dijkstra_node *found;
	int uid;

	uid = g_list_length(*list);

	node = dijkstra_node_new(x, y, z, uid);
	if (!node)
		return NULL;

	if ((found = dijkstra_node_exists(*list, node))) {
		free(node);
		node = found;
	} else {
		*list = g_list_append(*list, node);
	}

	return node;
}

int dijkstra_connect_destination_node_to_path(struct dijkstra_node *destination,
					      struct dijkstra_path *path)
{
	if (!path || !destination)
		return -1;

	if (path->destination) {
		report(LL_WARNING, "Connecting an already connected path (destination).");
	}
	destination->paths = g_list_append(destination->paths, (gpointer) path);
	path->destination = destination;

	return 0;
}

int dijkstra_connect_source_node_to_path(struct dijkstra_node *source,
					 struct dijkstra_path *path)
{
	if (!path || !source)
		return -1;

	if (path->source) {
		report(LL_WARNING, "Connecting an already connected path (source).");
	}

	source->paths = g_list_append(source->paths, (gpointer) path);
	path->source = source;

	return 0;
}

/*!
 * \brief dijkstra_connect_nodes_to_path connect the two nodes using the path
 * \param source
 * \param destination
 * \param path
 * \return
 */
int dijkstra_connect_nodes_to_path(struct dijkstra_node *source,
				   struct dijkstra_node *destination,
				   struct dijkstra_path *path)
{
	int ret = 0;

	ret |= dijkstra_connect_source_node_to_path(source, path);
	ret |= dijkstra_connect_destination_node_to_path(destination, path);

	return ret;
}

// Creates an unlinked (!) copy of the path
struct dijkstra_path *dijkstra_path_dup_shallow(struct dijkstra_path *path)
{
	struct dijkstra_path *dup;

	if (!path)
		return NULL;

	dup = malloc(sizeof(*path));
	if (dup) {
		memcpy(dup, path, sizeof(*path));
		dup->source = NULL;
		dup->destination = NULL;
	}

	return dup;
}

float dijkstra_position_get_distance(struct position *position_a,
				     struct position *position_b) {
	return sqrt(pow(position_a->x - position_b->x, 2) +
		    pow(position_a->y - position_b->y, 2));
}

dijkstra_cost dijkstra_get_weight_from_distance(struct dijkstra_node *node_a,
						struct dijkstra_node *node_b,
						enum dijkstra_net_type net_type)
{
	float dist;
	float scale;

	switch (net_type) {
	case TRAINLINE:
		scale = 5.0;
		break;
	case ACCESS:
		scale = 0.5;
		break;
	default:
		scale = 1.0;
		break;
	}

	dist = dijkstra_position_get_distance(&node_a->position, &node_b->position);
	dist /= scale;

	if (dist < DIJKSTRA_COST_MIN)
		dist = DIJKSTRA_COST_MIN;

	return (dijkstra_cost) (dist * 100);
}

/*!
 * \brief dijkstra_path_new creates a new dijkstra_path between two nodes.
 * \param name the name of the path (pointer is not copied and must stay valid!)
 * \return a new dijkstra path or NULL. Needs to be freed manually.
 */
struct dijkstra_path *dijkstra_path_new(char *name,
					struct dijkstra_node *source,
					struct dijkstra_node *destination,
					dijkstra_cost weight,
					struct dijkstra_net_meta *net_meta)
{
	struct dijkstra_path *path;

	path = malloc(sizeof(*path));
	if (path) {
		path->name = name;
		if (weight == DIJKSTRA_WEIGHT_AUTO)
			path->weight = dijkstra_get_weight_from_distance(source,
									 destination,
									 net_meta->type);
		else
			path->weight = weight;

		path->source = NULL;
		path->destination = NULL;

		path->net_meta = net_meta;

		if (dijkstra_connect_nodes_to_path(source, destination, path) != 0) {
			report(LL_CRITICAL, "Connect nodes failed: %p with %p as %p (%s)",
			       source, destination, path, path->name);
		}
	}

	return path;
}

struct dijkstra_node *dijkstra_disconnect_node_from_path(struct dijkstra_path *path,
							 struct dijkstra_node *node)
{
	if (!g_list_find(node->paths, path)) {
		report(LL_CRITICAL, "Tried disconnecting node %p from path %p (%s) that wasn't connected in the first place.",
		       node, path, path->name);
	}
	node->paths = g_list_remove(node->paths, path);

	if (path->source == path->destination) {
		report(LL_CRITICAL, "Zero-length path. Destination node = source node for path %p", path);
	}

	if (path->source == node) {
		path->source = NULL;
	}
	if (path->destination == node) {
		path->destination = NULL;
	}

	return node;

}

void checkycheckcheck(struct  dijkstra_solver *solver)
{
	GList *l;
	struct dijkstra_node *node;
	struct dijkstra_path *path;

	report(LL_INFO, "");
	for (l = solver->nodes; l != NULL; l = l->next) {
		node = (struct dijkstra_node*) l->data;
		report(LL_INFO, "Node %5d @%p: %f,\t%f [%d paths]", node->uid, node, node->position.x, node->position.y, g_list_length(node->paths));

		for (GList *m = node->paths; m != NULL; m = m->next) {
			path = (struct dijkstra_path*) m->data;
			struct dijkstra_node *foo = dijkstra_node_get_connection(node, path);
		}
	}
}

int dijkstra_path_intersect(struct dijkstra_solver *solver,
			    struct dijkstra_path *path_a,
			    struct dijkstra_path *path_b)
{
	// Yes, I was too lazy to code this myself:
	//   https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect

	struct dijkstra_path *path_new;
	struct dijkstra_node *node_new;
	struct dijkstra_node *disconnected_node;
	struct dijkstra_path *access_path = NULL;
	struct dijkstra_path *target_path = NULL;

	float p0_x, p0_y, p1_x, p1_y;
	float p2_x, p2_y, p3_x, p3_y;
	int p0_z, p1_z, p2_z, p3_z;
	float i_x, i_y;
	int i_z;

	float s1_x, s1_y, s2_x, s2_y;
	float s, t;

	if (!path_a || !path_b)
		return -1;

	if (path_a->net_meta->type == ACCESS) {
		access_path = path_a;
		target_path = path_b;
	} else if (path_b->net_meta->type == ACCESS) {
		access_path = path_b;
		target_path = path_a;
	}

	if (access_path) {
		if (target_path->net_meta->type != STREET &&
				!strstr(access_path->net_meta->access, target_path->name)) {
			return 0; // Don't intersect if access != name of target
		}
	} else {
		if (path_a->net_meta->type != path_b->net_meta->type)
			return 0; // Never intersect two layers of different types, unless it's through access layers
		// path_a and path_b types are now always guaranteed to be the same
		if (path_a->net_meta->type == TRAINLINE)
			return 0; // Never intersect trainlines (apart from with access types)
	}

	// Check height
	p0_z = path_a->source->position.z;
	p1_z = path_a->destination->position.z;
	p2_z = path_b->source->position.z;
	p3_z = path_b->destination->position.z;

	if (!access_path && // For access paths, z is invalid, so only check this if we are intersecting streets (trainlines are filtered out above)
			!(p0_z == p1_z && p1_z == p2_z && p2_z == p3_z)) // We don't want to intersect on height, they must match exactly for all four points.
		return 0; // No collision

	p0_x = path_a->source->position.x;
	p0_y = path_a->source->position.y;
	p1_x = path_a->destination->position.x;
	p1_y = path_a->destination->position.y;

	p2_x = path_b->source->position.x;
	p2_y = path_b->source->position.y;
	p3_x = path_b->destination->position.x;
	p3_y = path_b->destination->position.y;

	s1_x = p1_x - p0_x;
	s1_y = p1_y - p0_y;

	s2_x = p3_x - p2_x;
	s2_y = p3_y - p2_y;

	s = (-s1_y * (p0_x - p2_x) + s1_x * (p0_y - p2_y)) / (-s2_x * s1_y + s1_x * s2_y);
	t = ( s2_x * (p0_y - p2_y) - s2_y * (p0_x - p2_x)) / (-s2_x * s1_y + s1_x * s2_y);

	if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
	{
		// Collision detected
		i_x = p0_x + (t * s1_x);
		i_y = p0_y + (t * s1_y);
		i_z = p0_z; // They must all on the same height when intersecting, so we can pick whichever.

		node_new = dijkstra_node_new_to_list(&solver->nodes, i_x, i_y, i_z);
		if (node_new != path_a->source && node_new != path_a->destination) {
			path_new = dijkstra_path_dup_shallow(path_a);
			solver->paths = g_list_append(solver->paths, (gpointer) path_new);

			disconnected_node = dijkstra_disconnect_node_from_path(path_a, path_a->destination);
			dijkstra_connect_destination_node_to_path(node_new, path_a);
			path_a->weight = dijkstra_get_weight_from_distance(
						path_a->source,
						path_a->destination,
						path_new->net_meta->type);

			dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
			path_new->weight = dijkstra_get_weight_from_distance(
						path_new->source,
						path_new->destination,
						path_new->net_meta->type);

			// TODO: I don't have to iterate over the *entire* array here, it should be enough to only iterate over the entries after path_b.
			dijkstra_path_intersect_all(solver, path_new, path_new); // Since straight paths can only cross once, we don't need to check any newly created entries.
		}

		// Now do the same thing for path b
		if (node_new != path_b->source && node_new != path_b->destination) {
			path_new = dijkstra_path_dup_shallow(path_b);
			solver->paths = g_list_append(solver->paths, (gpointer) path_new);

			disconnected_node = dijkstra_disconnect_node_from_path(path_b, path_b->destination);
			dijkstra_connect_destination_node_to_path(node_new, path_b);
			path_b->weight = dijkstra_get_weight_from_distance(
						path_b->source,
						path_b->destination,
						path_new->net_meta->type);

			dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new);
			path_new->weight = dijkstra_get_weight_from_distance(path_new->source,
									     path_new->destination,
									     path_new->net_meta->type);
		}

		// In case neither if matches that means we are on the corner where two paths intersect at their ends.
		// No need to do anything then, dijkstra_node_new_to_list will have taken care of everything.

		return 1;
	}

	return 0; // No collision
}

void dijkstra_path_intersect_all(struct dijkstra_solver *solver,
				 struct dijkstra_path *path,
				 struct dijkstra_path *path_last)
{
	GList *l;
	struct dijkstra_path *existing_path;

	for (l = solver->paths; l != NULL; l = l->next) {
		existing_path = (struct dijkstra_path*) l->data;
		dijkstra_path_intersect(solver, path, existing_path);
		if (l->data == path_last) /* Avoid testing newly added paths */
			break;
	}
}

struct dijkstra_path *dijkstra_path_new_to_list(struct dijkstra_solver *solver,
						float x1,
						float y1,
						float x2,
						float y2,
						int z1,
						int z2,
						char *name,
						struct dijkstra_net_meta *net_meta,
						dijkstra_cost weight)
{
	struct dijkstra_node *n1, *n2;
	struct dijkstra_path *path;
	GList *l_last;
	struct dijkstra_path *path_last = NULL;

	n1 = dijkstra_node_new_to_list(&solver->nodes, x1, y1, z1);
	n2 = dijkstra_node_new_to_list(&solver->nodes, x2, y2, z2);
	path = dijkstra_path_new(name, n1, n2, weight, net_meta);

	l_last = g_list_last(solver->paths);
	if (l_last)
		path_last = l_last->data;
	solver->paths = g_list_append(solver->paths, (gpointer) path);

	dijkstra_path_intersect_all(solver, path, path_last);

	return path;
}

struct dijkstra_solver *dijkstra_solver_new()
{
	struct dijkstra_solver *solver;

	solver = malloc(sizeof(*solver));
	if (solver) {
		solver->nodes = NULL;
		solver->paths = NULL;
		solver->net_metas = NULL;
	}

	return solver;
}

void dijkstra_solver_free(struct dijkstra_solver *solver)
{
	GList *l;
	struct dijkstra_path *path;
	struct dijkstra_node *node;
	struct dijkstra_net_meta *net_meta;

	// Free memory
	for (l = solver->paths; l != NULL; l = l->next) {
		path = (struct dijkstra_path*) l->data;
		// TODO: free(path->name);
		//   Currently, we reuse the existing memory block when for the
		//   name when cloning streets. As such, clearing it here will
		//   cause a double free if the street has been split.
		free(path);
	}
	g_list_free(solver->paths);

	for (l = solver->nodes; l != NULL; l = l->next) {
		node = (struct dijkstra_node*) l->data;
		free(node);
	}
	g_list_free(solver->nodes);

	for (l = solver->net_metas; l != NULL; l = l->next) {
		net_meta = (struct dijkstra_net_meta*) l->data;
		free(net_meta->name);
		if (net_meta->access)
			free(net_meta->access);
		free(net_meta);
	}
}

inline struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node,
							  struct dijkstra_path *path)
{
	if (!node || !path) {
		report(LL_CRITICAL, "Called get_connection with NULL node (%p) or path (%p).",
		       node, path);
	}
	if (node != path->destination && node != path->source) {
		report(LL_WARNING, "get_connection requested for invalid node / path pair: %d with %p (%s).", node->uid, path, path->name);
		for (GList *l = node->paths; l != NULL; l = l->next) {
			struct dijkstra_path *path = (struct dijkstra_path*) l->data;
			report(LL_WARNING, "  Available: %p (%s)", path, path->name);
		}
		getchar();
	}
	return (path->destination == node ? path->source : path->destination);
}

struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *solver,
						      struct position *position) {
	GList *l = NULL;
	GList *pl = NULL;
	struct dijkstra_node *node = NULL;
	struct dijkstra_node *closest_node = NULL;
	struct dijkstra_path *path = NULL;
	float closest_distance;
	float distance;

	for (l = solver->nodes; l != NULL; l = l->next) {
		node = (struct dijkstra_node*) l->data;
		for (pl = node->paths; pl != NULL; pl = pl->next) {
			path = (struct dijkstra_path*) pl->data;
			if (path->net_meta->type == STREET)
				break;
		}
		if (!(path && path->net_meta->type == STREET))
			continue; // Only start on nodes that are part of a street
		distance = dijkstra_position_get_distance(position, &node->position);
		if (!closest_node || distance < closest_distance) {
			closest_node = node;
			closest_distance = distance;
		}
	}

	return closest_node;
}

struct dijkstra_net_meta *dijkstra_net_meta_new(enum dijkstra_net_type net_type,
						char *name,
						char *access)
{
	struct dijkstra_net_meta *net_meta;

	net_meta = malloc(sizeof(*net_meta));
	if (net_meta) {
		net_meta->type = net_type;
		net_meta->name = strdup(name);
		if (access)
			net_meta->access = strdup(access);
		else
			net_meta->access = NULL;
	}

	return net_meta;
}

struct dijkstra_net_meta *dijkstra_net_meta_new_to_list(struct dijkstra_solver *solver,
							enum dijkstra_net_type net_type,
							char *name,
							char *access)
{
	struct dijkstra_net_meta *net_meta;
	GList *l = NULL;

	if (net_type == STREET)
		name = "all";

	for (l = solver->net_metas; l != NULL; l = l->next) {
		net_meta = l->data;
		if (net_meta->type == net_type &&
				!strcmp(net_meta->name, name) &&
				(net_meta->access && (!strcmp(net_meta->access, access)))) {
			return net_meta;
		}
	}

	net_meta = dijkstra_net_meta_new(net_type, name, access);
	solver->net_metas = g_list_append(solver->net_metas, (void *) net_meta);
	return net_meta;
}