// Copyright Marcus Del Favero 2025
// Licensed under the GNU AGPLv3 with an exception, see `README.md` for details
mod components;
mod searcher;

use std::{collections::{BinaryHeap, HashMap, hash_map::Entry}, cmp::Ordering, f32::consts::PI, time::Instant};

use glam::{Vec2, IVec2};
use arrayvec::ArrayVec;

use components::Components;
use searcher::{Searcher, Goal, Avoid};

use crate::world::{World, blocks::{Blocks, BlockBorder}};

pub struct Pathfinding {
	border: BlockBorder,
	components: Components,
}

struct FringeItem {
	node: IVec2,
	prev: IVec2,
	dist: i32,
	est: i32,
}

impl PartialEq for FringeItem {
	fn eq(&self, other: &FringeItem) -> bool {
		self.cmp(other).is_eq()
	}
}

impl Eq for FringeItem {}

impl PartialOrd for FringeItem {
	fn partial_cmp(&self, other: &FringeItem) -> Option<Ordering> {
		Some(self.cmp(other))
	}
}

impl Ord for FringeItem {
	fn cmp(&self, other: &FringeItem) -> Ordering {
		other.est.cmp(&self.est) // Reverse ordering to use a min heap
	}
}

struct Neighbour {
	pos: IVec2,
	blocked: bool,
	cardinal: bool,
}

/*
 * One optimisation to A* is to avoid using floating-point arithmetic for
 * calculating distances and estimations.
 *
 * Using 70 and 99 for the costs of diagonal and cardinal neighbours is very
 * similar to using the exact distances of 1 and sqrt(2).
 *
 * Source: https://github.com/riscy/a_star_on_grids/
 */
const C_COST: i32 = 70;
const D_COST: i32 = 99;

pub const DEFAULT_MAX_DIST: u32 = 256;

impl Pathfinding {
	pub fn new(world: &World) -> Pathfinding {
		let blocks = world.get_blocks();
		let border = blocks.world_size_to_block_border(world.get_size());
		let components = Components::new(blocks, &border);
		Pathfinding { border, components }
	}

	/**
	 * Attempts to find a path from `start` to `goal`, both in world coordinates.
	 *
	 * Returns `Some(list of points on path)` if a path was successfully found,
	 * or `None` if no path was found.
	 */
	pub fn find_path_to_goal(&self, start: Vec2, goal: Vec2, blocks: &Blocks, max_dist: u32) -> Option<Vec<Vec2>> {
		Self::benchmark(|| self.find_path::<Goal>(start, goal, blocks, max_dist))
	}

	/**
	 * Finds a path from `start` to as far away from `avoid` as possible.
	 *
	 * Return type is the same as `find_path_towards`.
	 */
	pub fn find_path_avoiding(&self, start: Vec2, avoid: Vec2, blocks: &Blocks) -> Option<Vec<Vec2>> {
		Self::benchmark(|| self.find_path::<Avoid>(start, avoid, blocks, DEFAULT_MAX_DIST))
	}

	fn benchmark(f: impl FnOnce() -> Option<Vec<Vec2>>) -> Option<Vec<Vec2>> {
		let begin = Instant::now();
		let path = f();
		log::debug!("pathfinding time = {} μs", begin.elapsed().as_micros());
		path
	}

	fn find_path<S: Searcher>(&self, start: Vec2, goal: Vec2, blocks: &Blocks, max_dist: u32) -> Option<Vec<Vec2>> {
		let (start, goal) = (blocks.world_to_block_pos(start), blocks.world_to_block_pos(goal));

		// Unreachable
		if self.border.outside(start) || self.border.outside(goal) || !self.components.reachable(start, goal) { return None; }

		// Don't bother searching if too far as searching might be too expensive
		if searcher::octile_distance(start, goal) > (max_dist as i32).saturating_mul(C_COST) { return None; }

		/*
		 * Implements weighted A*. The exact shortest path isn't needed, any
		 * somewhat fast path is sufficient.
		 *
		 * I could use something else like HPA* but there isn't yet any need to
		 * optimise for that.
		 */
		let mut fringe = BinaryHeap::new();
		fringe.push(FringeItem { node: start, prev: IVec2::MAX, dist: 0, est: S::heuristic(start, goal) });

		let mut searcher = S::new(start, goal);
		let mut backpointers = HashMap::new();

		let visited_upper_bound = (PI * (max_dist as f32 + 4.0).powi(2)) as usize; // Not the least upper bound

		while let Some(item) = fringe.pop() {
			if let Entry::Vacant(e) = backpointers.entry(item.node) {
				e.insert(item.prev);

				if let Some(goal) = searcher.found_goal(item.node, item.est) {
					return Some(Self::backtrack(goal, backpointers, blocks));
				}

				let mut neighbours: ArrayVec<Neighbour, 8> = ArrayVec::new();
				for iy in -1..=1 {
					for ix in -1..=1 {
						if ix == 0 && iy == 0 { continue; }

						let pos = item.node + IVec2::new(ix, iy);
						let blocked = blocks.get(pos).unwrap_or_default() || self.border.outside(pos);
						let cardinal = ix == 0 || iy == 0;
						neighbours.push(Neighbour { pos, blocked, cardinal });
					}
				}

				/*
				 * To be able to directly travel diagonally, the diagonal neighbour
				 * needs to not be a block and the two cardinal neighbours adjacent
				 * to that diagonal neighbour also cannot be blocks.
				 *
				 * If any one of these adjacent neighbours is blocked, the diagonal
				 * neighbour is blocked, which is what each of these four lines
				 * does.
				 */
				neighbours[0].blocked |= neighbours[1].blocked | neighbours[3].blocked;
				neighbours[2].blocked |= neighbours[1].blocked | neighbours[4].blocked;
				neighbours[5].blocked |= neighbours[3].blocked | neighbours[6].blocked;
				neighbours[7].blocked |= neighbours[4].blocked | neighbours[6].blocked;

				for neighbour in neighbours {
					if neighbour.blocked || backpointers.contains_key(&neighbour.pos) { continue; }

					let edge_dist = if neighbour.cardinal { C_COST } else { D_COST };
					let dist = item.dist + edge_dist;
					let est = dist + S::heuristic(neighbour.pos, goal);
					fringe.push(FringeItem { node: neighbour.pos, prev: item.node, dist, est });
				}
			}

			if backpointers.len() > visited_upper_bound { // Further protection against infinite loops in case there are bugs in the previous mitigations
				log::warn!("pathfinding expected to terminate before visiting {visited_upper_bound} nodes");
				return None;
			}
		}

		searcher.not_found().map(|node| Self::backtrack(node, backpointers, blocks))
	}

	fn backtrack(mut node: IVec2, backpointers: HashMap<IVec2, IVec2>, blocks: &Blocks) -> Vec<Vec2> {
		let mut path = Vec::new();
		loop {
			path.push(blocks.block_to_world_pos(node));
			if let Some(prev) = backpointers.get(&node).copied() { node = prev; }
			else { break; }
		}
		path.pop(); // Removes IVec2::MAX
		path.reverse();
		path
	}
}
