Add initial conflict resolution

examples/brute_force2.rs

@@ -37,6 +37,10 @@ fn main() {
     // b.set_blocked_range(7, 3, 10, 4, true);
 
     b.set_blocked_range(3, 2, 10, 3, true);
+    b.set_blocked(2, 0, true);
+    b.set_blocked(11, 0, true);
+    b.set_blocked(2, 5, true);
+    b.set_blocked(11, 5, true);
 
     let mut b = b.build();
 

src/belt_finding/brute_force.rs

@@ -129,7 +129,7 @@ pub struct Bruteforce {
 impl Bruteforce {
     pub fn modify_pointer(&self) -> usize {
         self.pointer_stack
-            .get(self.pointer_stack.len() - 2)
+            .get(self.pointer_stack.len().saturating_sub(2))
             .copied()
             .unwrap_or(0)
     }

src/belt_finding/mod.rs

@@ -1,3 +1,4 @@
+use crate::belt_finding::brute_force::BruteforceBuilder;
 use crate::graph::wheighted_graph::shortest_path::dijkstra;
 use crate::graph::wheighted_graph::WheightedGraph;
 use crate::priority_queue::BinaryHeap;
@@ -17,6 +18,21 @@ pub enum Direction {
 }
 
 impl Direction {
+    fn from_neghbors(pos: &Position, neighbor: &Position) -> Self {
+        let x_diff = pos.x as i64 - neighbor.x as i64;
+        let y_diff = pos.y as i64 - neighbor.y as i64;
+
+        match (x_diff, y_diff) {
+            (1, 0) => Direction::Left,
+            (0, 1) => Direction::Up,
+            (-1, 0) => Direction::Right,
+            (0, -1) => Direction::Down,
+            _ => {
+                unreachable!()
+            }
+        }
+    }
+
     fn vertical(&self) -> bool {
         match self {
             Direction::Up => true,
@@ -274,5 +290,137 @@ impl Problem {
                 self.path[i] = p;
             }
         }
+
+        let mut conflicts: Map<usize> = Map::new(self.map.width, self.map.height);
+
+        for x in 0..self.map.width {
+            for y in 0..self.map.height {
+                if self.map.get(x, y).blocked {
+                    *conflicts.get_mut(x, y) += 1;
+                }
+            }
+        }
+
+        for path in &self.path {
+            for pos in path {
+                *conflicts.get_mut(pos.x, pos.y) += 1;
+            }
+        }
+
+        for y in 0..self.map.height {
+            for x in 0..self.map.width {
+                if *conflicts.get(x, y) > 1 {
+                    print!("#");
+                } else {
+                    print!(" ");
+                }
+            }
+            println!();
+        }
+
+        println!("{self}");
+
+        loop {
+            let mut c = None;
+            for y in 0..self.map.height {
+                for x in 0..self.map.width {
+                    if *conflicts.get(x, y) > 1 {
+                        c = Some((x, y));
+                        break;
+                    }
+                }
+            }
+
+            dbg!(c);
+
+            let xoffset = 1;
+            let yoffset = 1;
+
+            if let Some((cx, cy)) = c {
+                *conflicts.get_mut(cx, cy) = 1;
+
+                let xrange = cx..=cx;
+                let yrange = cy..=cy;
+
+                let xrange = xrange.start().saturating_sub(xoffset)
+                    ..=usize::min(xrange.end() + xoffset, self.map.width - 1);
+                let yrange = yrange.start().saturating_sub(yoffset)
+                    ..=usize::min(yrange.end() + yoffset, self.map.height - 1);
+                // dbg!(&xrange, &yrange);
+
+                let xsize = xrange.end() - xrange.start() + 1;
+                let ysize = yrange.end() - yrange.start() + 1;
+
+                // dbg!(xsize, ysize);
+
+                let mut b = BruteforceBuilder::new(xsize + 2, ysize + 2);
+                b.set_blocked_range(0, 0, xsize + 1, 0, true);
+                b.set_blocked_range(0, ysize + 1, xsize + 1, ysize + 1, true);
+                b.set_blocked_range(0, 0, 0, ysize + 1, true);
+                b.set_blocked_range(xsize + 1, 0, xsize + 1, ysize + 1, true);
+
+                for x in 0..xsize {
+                    for y in 0..ysize {
+                        b.set_blocked(
+                            x + 1,
+                            y + 1,
+                            self.map.get(xrange.start() + x, yrange.start() + y).blocked,
+                        );
+                    }
+                }
+
+                for path in &self.path {
+                    let s = path
+                        .iter()
+                        .position(|p| xrange.contains(&p.x) && yrange.contains(&p.y))
+                        .map(|i| i.saturating_sub(1));
+                    let e = path
+                        .iter()
+                        .rev()
+                        .position(|p| xrange.contains(&p.x) && yrange.contains(&p.y))
+                        .map(|i| usize::min(path.len() - 1, path.len() - i));
+
+                    if let Some((start, end)) = s.zip(e) {
+                        // dbg!(start, end);
+
+                        let start_pos = path[start];
+                        let start_dir = Direction::from_neghbors(&path[start], &path[start + 1]);
+                        let end_pos = path[end];
+                        let end_dir = Direction::from_neghbors(&path[end - 1], &path[end]);
+
+                        // dbg!(start_pos, end_pos);
+
+                        b.add_path(
+                            (
+                                Position::new(
+                                    start_pos.x - (xrange.start() - 1),
+                                    start_pos.y - (yrange.start() - 1),
+                                ),
+                                start_dir,
+                            ),
+                            (
+                                Position::new(
+                                    end_pos.x - (xrange.start() - 1),
+                                    end_pos.y - (yrange.start() - 1),
+                                ),
+                                end_dir,
+                            ),
+                        );
+                    }
+                }
+
+                let mut b = b.build();
+
+                println!("{}", b);
+
+                while b.next_finish_state() {
+                    println!("{}", b);
+                }
+
+                println!("{}", b.solution_count());
+            } else {
+                break;
+            }
+        }
     }
 }