Rewrote path selection for conflict avoidance.

src/belt_finding/common.rs

@@ -1,3 +1,8 @@
+use std::io::Write;
+
+use base64::write;
+use termcolor::{ColorSpec, StandardStream, WriteColor};
+
 pub type PositionType = i32;
 
 #[derive(Clone, Copy, PartialEq, Eq, Debug)]
@@ -108,6 +113,16 @@ impl Position {
     }
 }
 
+impl std::ops::Sub for Position {
+    type Output = Position;
+
+    fn sub(mut self, rhs: Self) -> Self::Output {
+        self.x -= rhs.x;
+        self.y -= rhs.y;
+        self
+    }
+}
+
 #[derive(Clone, Debug, Copy)]
 pub enum PathField {
     Belt {
@@ -167,3 +182,73 @@ impl PathField {
         }
     }
 }
+
+pub fn print_map<F>(width: i32, height: i32, f: F)
+where
+    F: Fn(i32, i32) -> (ColorSpec, &'static str),
+{
+    let stdout = &mut StandardStream::stdout(termcolor::ColorChoice::Always);
+
+    let width_digits = width.ilog10() + 1;
+    let height_digits = height.ilog10() + 1;
+
+    // print header
+    for i in 0..width_digits {
+        let d = width_digits - i - 1;
+
+        //padding
+        for _ in 0..height_digits {
+            write!(stdout, " ");
+        }
+
+        for x in 0..width {
+            let digits = x / (i32::pow(10, d));
+            if digits == 0 && d > 0 {
+                write!(stdout, " ");
+            } else {
+                write!(
+                    stdout,
+                    "{}",
+                    char::from_u32((digits % 10) as u32 + 48).unwrap()
+                );
+            }
+        }
+        writeln!(stdout);
+    }
+
+    for y in 0..height {
+        write!(stdout, "{:1$}", y, height_digits as usize);
+
+        for x in 0..width {
+            let (c, s) = f(x, y);
+            stdout.set_color(&c);
+            write!(stdout, "{:1}", s);
+            stdout.reset();
+        }
+
+        writeln!(stdout, "{:1$}", y, height_digits as usize);
+    }
+
+    for i in 0..width_digits {
+        let d = width_digits - i - 1;
+
+        //padding
+        for _ in 0..height_digits {
+            write!(stdout, " ");
+        }
+
+        for x in 0..width {
+            let digits = x / (i32::pow(10, d));
+            if digits == 0 && d > 0 {
+                write!(stdout, " ");
+            } else {
+                write!(
+                    stdout,
+                    "{}",
+                    char::from_u32((digits % 10) as u32 + 48).unwrap()
+                );
+            }
+        }
+        writeln!(stdout);
+    }
+}