use std::{
	collections::{BinaryHeap, HashMap},
	hash::Hash,
};

use crate::{map::Floor, pos::Pos};

#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
struct Node {
	estimated_cost: u16,
	cost: u16,
	pos: Pos,
}
impl PartialOrd for Node {
	fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
		Some(self.cmp(other))
	}
}
impl Ord for Node {
	fn cmp(&self, other: &Self) -> std::cmp::Ordering {
		self.estimated_cost
			.cmp(&other.estimated_cost)
			.then(self.cost.cmp(&other.cost))
			.reverse()
	}
}

/// A* pathfinding implementation
///
/// # Arguments
/// * `start` - Starting node (owned)
/// * `goal` - Goal node (borrowed reference)
/// * `floor` - Dungeon floor contaning all tiles
///
/// # Returns
/// * `Some((path, total_cost))` if path found
/// * `None` if no path exists
pub fn astar(start: Pos, goal: Pos, floor: &Floor) -> Option<(Vec<Pos>, u16)> {
	// list of nodes we need to look at next (BinaryHeap for priority queue behavior)
	let mut open_set = BinaryHeap::new();

	// The best known source node to get to a
	// destination node with minimum weight cost (HashMap for O(1) access)
	let mut came_from: HashMap<Pos, Pos> = HashMap::new();

	// The current known cost to get to a given node from the start node
	// (HashMap for O(1) access)
	let mut known_cost_map: HashMap<Pos, u16> = HashMap::new();

	// Initialize with start node (add to known cost map and nodes to explore)
	known_cost_map.insert(start, 0);
	open_set.push(Node {
		cost: 0,
		estimated_cost: 0,
		pos: start,
	});

	// Explore the nodes until we run out of nodes
	while let Some(Node { cost, pos, .. }) = open_set.pop() {
		// Goal reached! Reconstruct the path
		if pos == goal {
			return Some((reconstruct_path(&came_from, pos), cost));
		}

		// Explore neighbors
		for neighbor in floor.neighbors(&pos) {
			let tentative_cost = cost + 1;

			// If the path we are about to take is worse then the best known so far
			// for out neighbor, then we should not take it!
			if let Some(known_cost) = known_cost_map.get(&neighbor)
				&& *known_cost <= tentative_cost
			{
				continue;
			}

			// We have found a better path, we should update, so
			// our structures to reflect this
			came_from.insert(neighbor, pos);
			known_cost_map.insert(neighbor, tentative_cost);
			open_set.push(Node {
				cost: tentative_cost,
				estimated_cost: tentative_cost + goal.manhattan(neighbor),
				pos: neighbor,
			});
		}
	}

	None // No path found
}

/// Reconstruct path from goal to start using came_from map
fn reconstruct_path(came_from: &HashMap<Pos, Pos>, mut current: Pos) -> Vec<Pos> {
	let mut path = vec![current];

	// Backtrack from goal to start
	while let Some(prev) = came_from.get(&current) {
		current = *prev;
		path.push(current);
	}

	path.reverse();
	path
}