/*
    Hurry Curry! - a game about cooking
    Copyright 2024 metamuffin

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as published by
    the Free Software Foundation, version 3 of the License only.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.

*/
use hurrycurry_protocol::glam::{IVec2, Vec2};
use log::trace;
use std::{
    cmp::Ordering,
    collections::{BinaryHeap, HashMap, HashSet},
};

#[derive(Debug, Clone)]
pub struct Path(Vec<Vec2>);

impl Path {
    pub fn next_direction(&mut self, position: Vec2) -> Vec2 {
        if let Some(next) = self.0.last().copied() {
            trace!("next {next}");
            if next.distance(position) < if self.0.len() == 1 { 0.1 } else { 0.6 } {
                self.0.pop();
            }
            (next - position).normalize_or_zero() * 0.5
        } else {
            Vec2::ZERO
        }
    }
    pub fn is_done(&self) -> bool {
        self.0.is_empty()
    }
}

pub fn find_path(walkable: &HashSet<IVec2>, from: IVec2, to: IVec2) -> Option<Path> {
    #[derive(Debug, PartialEq, Eq)]
    struct Open(i32, IVec2, IVec2, i32);
    impl PartialOrd for Open {
        fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
            Some(self.0.cmp(&other.0))
        }
    }
    impl Ord for Open {
        fn cmp(&self, other: &Self) -> Ordering {
            self.0.cmp(&other.0)
        }
    }

    let mut visited = HashMap::new();
    let mut open = BinaryHeap::new();
    open.push(Open(1, from, from, 0));

    loop {
        let Open(_, pos, f, distance) = open.pop()?;
        if visited.contains_key(&pos) {
            continue;
        }
        visited.insert(pos, f);
        if pos == to {
            break;
        }
        for dir in [IVec2::NEG_X, IVec2::NEG_Y, IVec2::X, IVec2::Y] {
            let next = pos + dir;
            if walkable.contains(&next) {
                open.push(Open(
                    -(distance + next.distance_squared(to).isqrt()),
                    next,
                    pos,
                    distance + 1,
                ));
            }
        }
    }

    let mut path = Vec::new();
    let mut c = to;
    loop {
        path.push(c.as_vec2() + 0.5);
        let cn = visited[&c];
        if cn == c {
            break;
        }
        c = cn
    }
    Some(Path(path))
}