Add dependencise and cost evaluator

Signed-off-by: Arjo Chakravarty <[email protected]>

examples/end_to_end_planner.rs

@@ -0,0 +1,85 @@
+use std::ops::Index;
+
+use futures::pending;
+/// This example show cases how to use a* for accounting for optimal parking spot allocation
+/// The core idea is we have a set of robots that need to traverse an area. The goal is to pick up an item
+/// and then have it delivered to a certain location.
+/// 
+/// Along the way we may place a door which serves as a bottle neck.
+use pathfinding::prelude::astar;
+
+fn test_scenario() -> String
+{
+    // Goal is for robot to transport i to I and j to J, with parking spots
+    // "p" using robots 1 and 2.
+
+    let room = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx\n
+                     x.................x................x\n
+                     x.1.....i.........x.........I......x\n
+                     x.................D................x\n
+                     x.......j.....pp..x..........2.....x\n
+                     x.................x................x\n
+                     x.................D.......J........x\n
+                     x.................x................x\n
+                     xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
+                     ";
+    room.to_string()
+}
+
+fn find_character_coordinate(map: &Vec<String>, character: &String) -> Option<(usize, usize)>
+{
+    map.iter().enumerate()
+    .map(|(idx, f)| {(f.find(character), idx)})
+    .fold(None, |num, res| if res.0 == None { num } else {
+     if let Some(row) = res.0 {
+         Some((res.1, row))
+     }
+     else {
+         num
+     }
+    })
+}
+
+fn transform_to_map(map: String) -> Vec<String>
+{
+   map.split("\n").filter(|p| p.len() > 0).map(|f| f.replace(" ", "").to_string()).collect()
+}
+
+
+fn display_path(path: &Vec<(usize, usize)>, m: &Vec<String>)
+{
+    let mut m = m.clone();
+    for p in path {
+        m[p.0].replace_range(p.1..p.1+1, "*");
+    }
+
+    for line in m{
+        println!("{:?}", line);
+    }
+}
+fn main()
+{
+    let m = transform_to_map(test_scenario());
+
+    let Some(end_goal) = find_character_coordinate(&m, &"i".to_string()) else {
+        panic!("couldnt find start");
+    };
+    let Some(start_goal) = find_character_coordinate(&m,  &"I".to_string()) else {
+        panic!("could not find end");
+    };
+
+    let opts = vec![(0,1), (1,0), (0,-1), (-1,0)];
+
+    let res = astar(&start_goal,  |node| { 
+
+        let v:Vec<_> = opts.iter()
+            .map(|(dx, dy)| (node.0 as i32 +  dx.clone(), node.1 as i32 + dy.clone()))
+            .filter(|(x,y)| *x >= 0 && *y >= 0 && *x < m.len() as i32 && *y < m[0].len() as i32)
+            .filter(|(x,y)| m[*x as usize].bytes().nth(*y as usize).unwrap() != b'x')
+            .collect();
+        v.into_iter().map(|(x,y)| ((x as usize, y as usize), 1)) }, 
+     |p| p.0.abs_diff(end_goal.0) + p.1.abs_diff(end_goal.1),
+      |f| *f == end_goal);
+
+    display_path(&res.unwrap().0, &m);
+}

src/algorithms/sat_flexible_time_model.rs

@@ -37,6 +37,14 @@ impl Problem {
         self.requests.push(alternatives);
         return self.requests.len() - 1;
     }
+
+    pub fn implies(&mut self, a: &(usize, usize), b: &(usize, usize)) -> Result<(),String> {
+        if a.0 >= self.requests.len() || a.1 >= self.requests[a.0].len() || b.0 >= self.requests.len() || b.1 >= self.requests[b.0].len() {
+            return Err("Request and alternative was not found.".to_string())
+        }
+        self.dependencies.push((*a,*b));
+        Ok(())
+    } 
 }
 
 /// Snapshot of a solution. A solved schedule contains a list of assingments for each resource
@@ -234,6 +242,17 @@ impl<CS: ClockSource + Clone + std::marker::Send + std::marker::Sync> SATFlexibl
             }
         }
 
+        for dep  in problem.dependencies.iter() {
+            let (x1, x2) = dep;
+            let Some(x_ij) = var_list.get(x1) else {
+                panic!("Could not find variable");
+            };
+            let Some(x_km) = var_list.get(x1) else {
+                panic!("Could not get variable");
+            };
+            formula.add_clause(&[x_ij.negative(), Lit::from_var(*x_km, true)]);
+        }
+
         // Strict Total Order constraints
         for (_, alternatives) in var_by_resource.iter() {
             for i in 0..alternatives.len() {
@@ -375,12 +394,13 @@ impl<CS: ClockSource + Clone + std::marker::Send + std::marker::Sync> SATFlexibl
                             if let Some(alt_ij_shrink) = alt_ij_shrink {
                                 if let Some(alt_km_shrink) = alt_km_shrink {
                                     let Some(list_ij) = comes_after_vars.get(&alt_ij) else {
-                                        panic!("For some reason");
+                                        panic!("For some reason unable to get comes after vars");
                                     };
 
                                     let X_ijkm = list_ij.get(&alt_km).expect("");
                                     let Some(list_km) = comes_after_vars.get(&alt_km) else {
-                                        panic!("For some reason");
+                                        panic!("For some reason unable to get comes after vars
+                                        ");
                                     };
 
                                     let X_kmij = list_km.get(&alt_ij).expect("");