From 9486a1144ef0ca19c6ebdabc11793422381c9d6d Mon Sep 17 00:00:00 2001
From: Rostyslav Hnatyshyn <rostyslav.hnatyshyn@gmail.com>
Date: Tue, 2 Jan 2024 22:47:26 -0700
Subject: [PATCH] naive astar

---
 src/lib/ai.rs    |  1 -
 src/lib/point.rs | 74 ++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 74 insertions(+), 1 deletion(-)

diff --git a/src/lib/ai.rs b/src/lib/ai.rs
index bfb45ce..2b53bfa 100644
--- a/src/lib/ai.rs
+++ b/src/lib/ai.rs
@@ -1,4 +1,3 @@
-
 use crate::lib::screen::{Screen, BoardCommand};
 use crate::World;
 use crate::Point;
diff --git a/src/lib/point.rs b/src/lib/point.rs
index dbf85d6..dc117ff 100644
--- a/src/lib/point.rs
+++ b/src/lib/point.rs
@@ -1,3 +1,5 @@
+use std::collections::{HashMap, HashSet};
+
 #[derive(Hash, PartialEq, Eq, Clone, Debug, Copy)]
 pub struct Point(pub i32, pub i32);
 
@@ -11,3 +13,75 @@ impl Point {
         ]
     }
 }
+
+fn manhattan(c1: &Point, c2: &Point) -> i32 {
+    return (c2.0 - c1.0).abs() + (c2.1 - c1.1).abs();
+}
+
+fn astar(start: &Point, goal: &Point) -> Vec<Point> {
+    let mut open_set : HashSet<Point> = HashSet::new();
+    open_set.insert(*start);
+
+    let mut came_from: HashMap<Point, Point> = HashMap::new();
+
+    let mut gscore: HashMap<Point, i32> = HashMap::new();
+    gscore.insert(*start, 0);
+
+    let mut fscore: HashMap<Point, i32> = HashMap::new();
+    fscore.insert(*start, manhattan(&start, &goal));
+
+    let mut answer = Vec::new();
+
+    let mut current: Point = *start;
+    while !open_set.is_empty() {
+        let mut min_score = i32::MAX;
+        for c in open_set.iter() {
+            let val = fscore[c];
+            current = if val < min_score { c.clone() } else { current };
+            min_score = if val < min_score { val } else { min_score };
+        }
+
+        if current == *goal {
+            answer.push(current.clone());
+            while came_from.contains_key(&current) {
+                current = came_from[&current].clone();
+                answer.push(current.clone());
+            }
+            break;
+        }
+
+        open_set.remove(&current);
+
+        let current_gscore = gscore[&current];
+
+        let all_neighbors: Vec<Point> = current.get_neighbors();
+        for neighbor in all_neighbors.iter() {
+            let neighbor_score = if gscore.contains_key(&neighbor) {
+                gscore[neighbor]
+            } else {
+                i32::MAX
+            };
+            let score = current_gscore + 1;
+            if score < neighbor_score {
+                gscore.insert(neighbor.clone(), score);
+                fscore.insert(neighbor.clone(), score + manhattan(&neighbor, &goal));
+                came_from.insert(neighbor.clone(), current.clone());
+                open_set.insert(neighbor.clone());
+            }
+        }
+    }
+
+    answer
+}
+
+#[test]
+fn test_astar() {
+    let start = Point(0, 0);
+
+    let goal = Point(10, 10);
+    let answers: Vec<Point> = astar(&start, &goal);
+
+    for a in answers.iter() {
+        println!("{:?}", &a);
+    }
+}