use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap};

struct Node {
    position: (i32, i32),
    cost: i32,
    estimated_total: i32,
}

impl Eq for Node {}

impl PartialEq for Node {
    fn eq(&self, other: &Self) -> bool {
        self.estimated_total == other.estimated_total
    }
}

impl Ord for Node {
    fn cmp(&self, other: &Self) -> Ordering {
        other.estimated_total.cmp(&self.estimated_total)
    }
}

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

fn manhattan(a: (i32, i32), b: (i32, i32)) -> i32 {
    (a.0 - b.0).abs() + (a.1 - b.1).abs()
}

fn chebyshev(a: (i32, i32), b: (i32, i32)) -> i32 {
    (a.0 - b.0).abs().max((a.1 - b.1).abs())
}

const CARDINAL_NEIGHBORS: [(i32, i32); 4] = [(0, -1), (1, 0), (0, 1), (-1, 0)];

const ALL_NEIGHBORS: [(i32, i32); 8] = [
    (0, -1),
    (1, 0),
    (0, 1),
    (-1, 0),
    (-1, -1),
    (1, -1),
    (-1, 1),
    (1, 1),
];

pub fn astar(
    start: (i32, i32),
    goal: (i32, i32),
    is_walkable: impl Fn(i32, i32) -> bool,
    allow_diagonal: bool,
) -> Option<Vec<(i32, i32)>> {
    if start == goal {
        return Some(vec![start]);
    }

    if !is_walkable(start.0, start.1) || !is_walkable(goal.0, goal.1) {
        return None;
    }

    let heuristic: fn((i32, i32), (i32, i32)) -> i32 =
        if allow_diagonal { chebyshev } else { manhattan };

    let mut open = BinaryHeap::new();
    let mut came_from: HashMap<(i32, i32), (i32, i32)> = HashMap::new();
    let mut cost_so_far: HashMap<(i32, i32), i32> = HashMap::new();

    open.push(Node {
        position: start,
        cost: 0,
        estimated_total: heuristic(start, goal),
    });
    cost_so_far.insert(start, 0);

    let neighbors: &[(i32, i32)] = if allow_diagonal {
        &ALL_NEIGHBORS
    } else {
        &CARDINAL_NEIGHBORS
    };

    while let Some(current) = open.pop() {
        if current.position == goal {
            let mut path = Vec::new();
            let mut position = goal;
            while position != start {
                path.push(position);
                position = came_from[&position];
            }
            path.push(start);
            path.reverse();
            return Some(path);
        }

        let current_cost = cost_so_far[&current.position];

        if current.cost > current_cost {
            continue;
        }

        for &(delta_column, delta_row) in neighbors {
            let neighbor = (
                current.position.0 + delta_column,
                current.position.1 + delta_row,
            );

            if !is_walkable(neighbor.0, neighbor.1) {
                continue;
            }

            let new_cost = current_cost + 1;

            if !cost_so_far.contains_key(&neighbor) || new_cost < cost_so_far[&neighbor] {
                cost_so_far.insert(neighbor, new_cost);
                came_from.insert(neighbor, current.position);
                open.push(Node {
                    position: neighbor,
                    cost: new_cost,
                    estimated_total: new_cost + heuristic(neighbor, goal),
                });
            }
        }
    }

    None
}
