use std::collections::{BTreeSet, BTreeMap};
use std::usize;
use std::fmt;


type Cell = (usize, usize);


#[derive(Debug, PartialEq, Copy, Clone)]
pub enum Space {
    Unknown,
    Open,
    Wall
}

pub struct Office {
    seed: usize,
    floor: Vec<Vec<Space>>
}

impl Office {
    pub fn new(seed: usize) -> Self {
        Office {
            seed: seed,
            floor: Vec::new()
        }
    }

    pub fn cell_type(&mut self, cell: Cell) -> Space {
        while cell.1 >= self.floor.len() {
            self.floor.push(Vec::new());
        }

        while cell.0 >= self.floor[cell.1].len() {
            self.floor[cell.1].push(Space::Unknown);
        }

        match self.floor[cell.1][cell.0] {
            Space::Unknown => {
                let integer = (cell.0 * cell.0) + (3 * cell.0) + (2 * cell.0 * cell.1) + cell.1 + (cell.1 * cell.1) + self.seed;

                let binary = format!("{:b}", integer);

                let ones = binary.chars()
                    .filter(|x| *x == '1')
                    .count();

                if ones % 2 == 0 {
                    self.floor[cell.1][cell.0] = Space::Open;
                } else {
                    self.floor[cell.1][cell.0] = Space::Wall;
                }

                self.floor[cell.1][cell.0]
            }
            s @ _ => {
                s
            }
        }
    }

    fn cell_neighbors(&mut self, cell: Cell) -> Vec<Cell> {
        let mut neighbors = Vec::new();

        // Over.
        if cell.1 > 0 {
            if let Space::Open = self.cell_type((cell.0, cell.1 - 1)) {
                neighbors.push((cell.0, cell.1 - 1));
            }
        }

        // Below.
        if let Space::Open = self.cell_type((cell.0, cell.1 + 1)) {
            neighbors.push((cell.0, cell.1 + 1));
        }

        // Left.
        if cell.0 > 0 {
            if let Space::Open = self.cell_type((cell.0 - 1, cell.1)) {
                neighbors.push((cell.0 - 1, cell.1));
            }
        }

        // Right.
        if let Space::Open = self.cell_type((cell.0 + 1, cell.1)) {
            neighbors.push((cell.0 + 1, cell.1));
        }

        neighbors
    }

    pub fn path_between(&mut self, start: Cell, goal: Cell) -> Result<Vec<Cell>, ()> {
        let mut closed_set = BTreeSet::new();
        let mut open_set = BTreeSet::new();

        open_set.insert(start);

        let mut came_from = BTreeMap::new();;

        let mut g_score = BTreeMap::new();

        g_score.insert(start, 0);

        let mut f_score = BTreeMap::new();

        f_score.insert(start, heuristic_cost_estimate(start, goal));

        while !open_set.is_empty() {
            let (current, _) = f_score.iter()
                .filter(|&(&(x, y), _)| open_set.contains(&(x, y)))
                .fold(((0, 0), usize::MAX), |((min_x, min_y), min_score), (&(x, y), &score)| {
                    if score < min_score {
                        ((x, y), score)
                    } else {
                        ((min_x, min_y), min_score)
                    }
                });

            if current == goal {
                return Ok(reconstruct_path(&came_from, current));
            }

            open_set.remove(&current);
            closed_set.insert(current);

            for neighbor in self.cell_neighbors(current) {
                if closed_set.contains(&neighbor) {
                    continue;
                }

                let tentative_g_score = *g_score.entry(current).or_insert(usize::MAX) + dist_between(current, neighbor);

                if !open_set.contains(&neighbor) {
                    open_set.insert(neighbor);
                } else if tentative_g_score >= *g_score.entry(neighbor).or_insert(usize::MAX) {
                    continue
                }

                came_from.insert(neighbor, current);

                g_score.insert(neighbor, tentative_g_score);
                f_score.insert(neighbor, tentative_g_score + heuristic_cost_estimate(neighbor, goal));
            }
        }

        Err(())
    }

}

impl fmt::Display for Office {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        for row in self.floor.iter() {
            for cell in row {
                match *cell {
                    Space::Unknown => { write!(f, " ")? },
                    Space::Open    => { write!(f, ".")? },
                    Space::Wall    => { write!(f, "#")? },
                }
            }

            writeln!(f, "")?
        }

        Ok(())
    }
}


fn reconstruct_path(came_from: &BTreeMap<Cell, Cell>, current: Cell) -> Vec<Cell> {
    let mut current = current;
    let mut path = Vec::new();

    path.push(current);

    while came_from.contains_key(&current) {
        if let Some(next) = came_from.get(&current) {
            current = *next;

            path.push(*next);
        }
    }

    path
}

fn dist_between(start: Cell, goal: Cell) -> usize {
    let delta_x = goal.0 as i64 - start.0 as i64;
    let delta_y = goal.1 as i64 - start.1 as i64;

    (delta_x.abs() + delta_y.abs()) as usize
}

fn heuristic_cost_estimate(start: Cell, goal: Cell) -> usize {
    dist_between(start, goal)
}


fn main() {
    let mut office = Office::new(1352);

    let path = office.path_between((1, 1), (31, 39)).unwrap();

    for (y, row) in office.floor.iter().enumerate() {
        for (x, cell) in row.iter().enumerate() {
            if path.contains(&(x, y)) {
                print!("\x1b[32m");
            }

            match *cell {
                Space::Unknown => { print!(" "); },
                Space::Open    => { print!("."); },
                Space::Wall    => { print!("#"); },
            }

            if path.contains(&(x, y)) {
                print!("\x1b[0m");
            }
        }

        println!("");
    }

    println!("length={}", path.len() - 1);


    let mut office = Office::new(1352);
    let mut reachable = BTreeSet::new();

    for x in 0..50 + 2 {
        for y in 0..50 + 2 {
            if let Ok(path) = office.path_between((1, 1), (x, y)) {
                for cell in path.iter().rev().take(51) {
                    reachable.insert(cell.clone());
                }
            }
        }
    }

    for (y, row) in office.floor.iter().enumerate() {
        for (x, cell) in row.iter().enumerate() {
            if reachable.contains(&(x, y)) {
                print!("\x1b[32m");
            }

            match *cell {
                Space::Unknown => { print!(" "); },
                Space::Open    => { print!("."); },
                Space::Wall    => { print!("#"); },
            }

            if reachable.contains(&(x, y)) {
                print!("\x1b[0m");
            }
        }

        println!("");
    }

    println!("length={}", reachable.len());
}


#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_cell_space() {
        let mut office = Office::new(10);

        assert_eq!(Space::Open, office.cell_type((0, 0)));
        assert_eq!(Space::Wall, office.cell_type((1, 0)));
        assert_eq!(Space::Open, office.cell_type((2, 0)));

        assert_eq!(Space::Open, office.cell_type((0, 1)));
        assert_eq!(Space::Open, office.cell_type((1, 1)));
        assert_eq!(Space::Wall, office.cell_type((2, 1)));

        assert_eq!(Space::Wall, office.cell_type((0, 2)));
        assert_eq!(Space::Open, office.cell_type((1, 2)));
        assert_eq!(Space::Open, office.cell_type((2, 2)));
    }

    #[test]
    fn test_path_between() {
        let mut office = Office::new(10);

        assert_eq!(Ok(vec![(7, 4), (6, 4), (6, 5), (5, 5), (4, 5), (4, 4), (3, 4), (3, 3), (3, 2), (2, 2), (1, 2), (1, 1)]), office.path_between((1, 1), (7, 4)));
    }
}