#include "common.hpp"

namespace world {

static const v2f_t path_margin(5, 5);
static const tile_index_t path_offsets[8] = {
	{+1, 0}, {+1, +1}, {0, +1}, {-1, +1},
	{-1, 0}, {-1, -1}, {0, -1}, {+1, -1}
};

path_finder_t::~path_finder_t()
{
	delete nodes;
}

void path_finder_t::setup_nodes(v2f_t src_, v2f_t dst_)
{
	rectf_t src_margin, dst_margin, bounds;
	tile_index_t end;

	src = src_;
	dst = dst_;

	tile_center = v2f_t(0.5, 0.5);
	shortest_dist = INFINITY;

	src_margin[0] = src - path_margin;
	src_margin[1] = src + path_margin;
	dst_margin[0] = dst - path_margin;
	dst_margin[1] = dst + path_margin;
	bounds = src_margin | dst_margin;

	base = tile_index_at(bounds[0]);
	end = tile_index_at(bounds[1]);

	width = end[0] - base[0] + 1;
	height = end[1] - base[1] + 1;

	nodes = new path_node_t[width * height];
	for (size_t i = 0; i < width * height; i++)
		nodes[i].dist = INFINITY;
}

void path_finder_t::eliminate_nodes(rectf_t bounds)
{
	rect_t<coord_t, 2> index_bounds;
	tile_index_t index;

	std::cout << "eliminate " << bounds << "\n";

	index_bounds[0] = tile_index_t(bounds[0].floor()) - base;
	index_bounds[1] = tile_index_t(bounds[1].ceil()) - base;

	for (index[1] = index_bounds[0][1]; index[1] <= index_bounds[1][1]; index[1]++)
	for (index[0] = index_bounds[0][0]; index[0] <= index_bounds[1][0]; index[0]++) {
		path_node_t *node;

		node = nodes + index[1] * width + index[0];
		node->accessible = false;
	}
}

void path_finder_t::find_r(tile_index_t index, float dist, float limit)
{
	path_node_t *node;
	float dist_to_dst;

	node = nodes + index[1] * width + index[0];

	if (!node->accessible)
		return;
	if (node->dist <= dist)
		return;

	path.push_back(index);
	node->dist = dist;

	dist_to_dst = (v2f_t(base + index) + tile_center - dst).len();
	if (dist_to_dst < 1.0f && dist + dist_to_dst < shortest_dist) {
		shortest_path = path;
		shortest_dist = dist + dist_to_dst;
	}

	for (size_t i = 0; i < 8; i++) {
		tile_index_t offset, next;

		offset = path_offsets[i];
		next = index + offset;

		if (next[0] < 0 || next[1] < 0 ||
		    next[0] >= (int64_t)width || next[1] >= (int64_t)height)
			continue;

		if (dist + v2f_t(offset).len() > limit)
			continue;

		find_r(next, dist + v2f_t(offset).len(), limit);
	}

	path.pop_back();
}

bool path_finder_t::find(void)
{
	tile_index_t start;
	float dist_simple;

	dist_simple = (src - dst).len();

	if (dist_simple < 1.0f)
		return true;

	start = tile_index_at(src) - base;
	nodes[start[1] * width + start[0]].accessible = false;

	for (size_t i = 0; i < 8; i++) {
		tile_index_t next;
		v2f_t offset;

		next = start + path_offsets[i];
		offset = (src - v2f_t(base)) - v2f_t(next) - tile_center;

		if (next[0] < 0 || next[1] < 0 ||
		    next[0] >= (int64_t)width || next[1] >= (int64_t)height)
			continue;

		find_r(next, v2f_t(offset).len(), dist_simple * 3);
	}

	return shortest_path.size() > 0;
}

void path_finder_t::export_path(std::list<v2f_t> *list)
{
	list->clear();

	for (tile_index_t &index : shortest_path)
		list->push_back(v2f_t(index + base) + tile_center);

	list->push_back(dst);
}

} // namespace world