#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::find_r(tile_index_t index, float dist)
{
	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;

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

	path.pop_back();
}

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

	if ((src - dst).len() < 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());
	}

	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