From 0af88c213d03354468cc2cb33ba3aa8958520963 Mon Sep 17 00:00:00 2001
From: StephenAdamson <stephen.h.adamson@gmail.com>
Date: Fri, 17 Jan 2025 20:05:08 +0000
Subject: [PATCH] partial a*

---
 src/citizen.rs | 283 ++++++++++++++++++++++++++++++++++++++++++++-----
 src/main.rs    |   1 +
 src/tile.rs    |   4 +-
 src/tilemap.rs |  41 ++++---
 src/tiles.rs   |   4 +
 5 files changed, 288 insertions(+), 45 deletions(-)

diff --git a/src/citizen.rs b/src/citizen.rs
index d2f6ab6..5276d8e 100644
--- a/src/citizen.rs
+++ b/src/citizen.rs
@@ -1,12 +1,8 @@
-use crate::constants::*;
+use crate::{
+    constants::*,
+    tilemap::{generate_surface_noise, ChunkMap, CHUNK_SIZE},
+};
 use bevy::prelude::*;
-use rand::prelude::*;
-
-#[derive(Component)]
-#[require(Transform)]
-pub struct Ambulatory {
-    speed: f32,
-}
 
 #[derive(Bundle)]
 pub struct Citizen {
@@ -18,7 +14,12 @@ pub struct Citizen {
 impl Citizen {
     pub fn new(asset_server: &Res<AssetServer>, position: Vec3) -> Self {
         Citizen {
-            walkness: Ambulatory { speed: TILE_SIZE },
+            walkness: Ambulatory {
+                speed: TILE_SIZE,
+                target: None,
+                current_path: None,
+                path_index: 0,
+            },
             sprite: Sprite {
                 image: asset_server.load("dorf.png"),
                 ..Default::default()
@@ -28,34 +29,258 @@ impl Citizen {
     }
 }
 
-pub fn citizen_movement(mut query: Query<(&Ambulatory, &mut Transform), With<Ambulatory>>) {
-    for (citizen, mut transform) in query.iter_mut() {
-        // Skip processing if the random check fails
-        if rand::random::<f32>() >= 0.66666 {
-            continue;
-        }
+use crate::constants::TILE_SIZE;
+use crate::tile::TileMap;
+use rand::{seq::SliceRandom, Rng};
+use std::collections::{BinaryHeap, HashMap, HashSet};
 
-        // Generate random directions
-        let direction_x = (rand::random::<f32>() - rand::random::<f32>()).round();
-        let direction_y = (rand::random::<f32>() - rand::random::<f32>()).round();
+#[derive(Component)]
+pub struct Ambulatory {
+    pub speed: f32,
+    pub current_path: Option<Vec<Vec3>>,
+    pub path_index: usize,
+    pub target: Option<Vec3>,
+}
 
-        // Skip if there's no movement
-        if direction_x == 0.0 && direction_y == 0.0 {
-            continue;
-        }
+#[derive(Clone, Eq, PartialEq)]
+struct PathNode {
+    position: IVec3,
+    f_score: i32,
+    g_score: i32,
+}
 
-        // Calculate movement only once
-        let speed = citizen.speed;
-        transform.translation.x += direction_x * speed;
-        transform.translation.y += direction_y * speed;
+impl Ord for PathNode {
+    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
+        other.f_score.cmp(&self.f_score)
+    }
+}
 
-        // Adjust scale for x direction
-        if direction_x != 0.0 {
-            transform.scale.x = PIXEL_RATIO * direction_x.signum();
+impl PartialOrd for PathNode {
+    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+pub struct PathfindingPlugin;
+
+impl Plugin for PathfindingPlugin {
+    fn build(&self, app: &mut App) {
+        app.add_systems(FixedUpdate, (update_citizen_targets, citizen_movement));
+    }
+}
+
+pub fn update_citizen_targets(
+    mut query: Query<(&mut Ambulatory, &Transform)>,
+    tilemap: Res<TileMap>,
+    chunk_map: Res<ChunkMap>,
+) {
+    let mut rng = rand::thread_rng();
+
+    for (mut ambulatory, transform) in query.iter_mut() {
+        if ambulatory.target.is_none()
+            || (ambulatory.current_path.is_some()
+                && ambulatory.path_index >= ambulatory.current_path.as_ref().unwrap().len())
+        {
+            // Find a random loaded chunk
+            let loaded_chunks: Vec<&IVec2> = chunk_map.loaded_chunks.keys().collect();
+            if let Some(&chunk_pos) = loaded_chunks.choose(&mut rng) {
+                // Generate random position within chunk
+                let chunk_x = chunk_pos.x * CHUNK_SIZE;
+                let chunk_y = chunk_pos.y * CHUNK_SIZE;
+
+                let target_x = chunk_x + rng.gen_range(0..CHUNK_SIZE);
+                let target_y = chunk_y + rng.gen_range(0..CHUNK_SIZE);
+
+                // Get height at position
+                let surface_height = generate_surface_noise(target_x, target_y).round() as i32;
+
+                // Find a valid z-level near the surface
+                for z in (surface_height - 2)..=(surface_height + 2) {
+                    let target_pos = IVec3::new(target_x, target_y, z);
+                    if let Some(floor_tile) = tilemap.floor_tiles.get(&target_pos) {
+                        // If tile is walkable, set target
+                        if floor_tile.2 {
+                            ambulatory.target = Some(Vec3::new(
+                                target_x as f32 * TILE_SIZE,
+                                target_y as f32 * TILE_SIZE,
+                                z as f32 * TILE_SIZE,
+                            ));
+                            ambulatory.current_path = None;
+                            ambulatory.path_index = 0;
+                            break;
+                        }
+                    }
+                }
+            }
         }
     }
 }
 
+pub fn citizen_movement(
+    mut query: Query<(&mut Ambulatory, &mut Transform)>,
+    tilemap: Res<TileMap>,
+    time: Res<Time>,
+) {
+    for (mut ambulatory, mut transform) in query.iter_mut() {
+        if let Some(target) = ambulatory.target {
+            // Convert world position to tile coordinates
+            let current_tile = IVec3::new(
+                (transform.translation.x / TILE_SIZE).floor() as i32,
+                (transform.translation.y / TILE_SIZE).floor() as i32,
+                (transform.translation.z / TILE_SIZE).floor() as i32,
+            );
+
+            let target_tile = IVec3::new(
+                (target.x / TILE_SIZE).floor() as i32,
+                (target.y / TILE_SIZE).floor() as i32,
+                (target.z / TILE_SIZE).floor() as i32,
+            );
+
+            // Calculate path if needed
+            if ambulatory.current_path.is_none() {
+                ambulatory.current_path = Some(calculate_path(&tilemap, current_tile, target_tile));
+                ambulatory.path_index = 0;
+            }
+
+            // Follow the current path
+            if let Some(path) = &ambulatory.current_path {
+                if ambulatory.path_index < path.len() {
+                    let next_point = path[ambulatory.path_index];
+                    let next_world_pos = Vec3::new(
+                        next_point.x * TILE_SIZE + TILE_SIZE / 2.0,
+                        next_point.y * TILE_SIZE + TILE_SIZE / 2.0,
+                        next_point.z * TILE_SIZE + TILE_SIZE / 2.0,
+                    );
+
+                    let direction = (next_world_pos - transform.translation).normalize();
+                    transform.translation += direction * ambulatory.speed * time.delta_secs();
+
+                    // Update sprite direction (only for x movement)
+                    if direction.x != 0.0 {
+                        transform.scale.x = transform.scale.x.abs() * direction.x.signum();
+                    }
+
+                    // Check if we've reached the next point
+                    if transform.translation.distance(next_world_pos) < ambulatory.speed {
+                        ambulatory.path_index += 1;
+                    }
+                }
+            }
+        }
+    }
+}
+
+fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec<Vec3> {
+    let mut open_set = BinaryHeap::new();
+    let mut came_from = HashMap::new();
+    let mut g_scores = HashMap::new();
+    let mut closed_set = HashSet::new();
+
+    let start_node = PathNode {
+        position: start,
+        f_score: 0,
+        g_score: 0,
+    };
+
+    open_set.push(start_node.clone());
+    g_scores.insert(start, 0);
+
+    while let Some(current) = open_set.pop() {
+        if current.position == goal {
+            return reconstruct_path(came_from, current.position);
+        }
+
+        closed_set.insert(current.position);
+
+        // Check neighbors in 3D (26 possible directions)
+        for dx in -1..=1 {
+            for dy in -1..=1 {
+                for dz in -1..=1 {
+                    if dx == 0 && dy == 0 && dz == 0 {
+                        continue;
+                    }
+
+                    let neighbor_pos = current.position + IVec3::new(dx, dy, dz);
+
+                    // Skip if not walkable or already closed
+                    if !is_walkable(tilemap, neighbor_pos) || closed_set.contains(&neighbor_pos) {
+                        continue;
+                    }
+
+                    // Calculate base movement cost (diagonal/vertical movement costs more)
+                    let movement_cost = match dx.abs() + dy.abs() + dz.abs() {
+                        1 => 10, // Orthogonal movement
+                        2 => 14, // Diagonal movement on one plane
+                        3 => 17, // Diagonal movement across planes
+                        _ => unreachable!(),
+                    };
+
+                    // Add tile-specific weight
+                    let tile_weight =
+                        if let Some(floor_tile) = tilemap.floor_tiles.get(&neighbor_pos) {
+                            floor_tile.3 as i32 // astar_weight
+                        } else {
+                            100 // Default weight if tile not found
+                        };
+
+                    let new_g = g_scores[&current.position] + movement_cost + tile_weight;
+
+                    if !g_scores.contains_key(&neighbor_pos) || new_g < g_scores[&neighbor_pos] {
+                        g_scores.insert(neighbor_pos, new_g);
+                        let h = manhattan_distance_3d(neighbor_pos, goal);
+                        let f = new_g + h;
+
+                        open_set.push(PathNode {
+                            position: neighbor_pos,
+                            f_score: f,
+                            g_score: new_g,
+                        });
+                        came_from.insert(neighbor_pos, current.position);
+                    }
+                }
+            }
+        }
+    }
+
+    // If no path found, return direct line
+    vec![
+        Vec3::new(start.x as f32, start.y as f32, start.z as f32),
+        Vec3::new(goal.x as f32, goal.y as f32, goal.z as f32),
+    ]
+}
+
+fn is_walkable(tilemap: &TileMap, position: IVec3) -> bool {
+    if let Some(floor_tile) = tilemap.floor_tiles.get(&position) {
+        floor_tile.2 // walkable
+    } else {
+        false
+    }
+}
+
+fn manhattan_distance_3d(a: IVec3, b: IVec3) -> i32 {
+    (a.x - b.x).abs() + (a.y - b.y).abs() + (a.z - b.z).abs()
+}
+
+fn reconstruct_path(came_from: HashMap<IVec3, IVec3>, mut current: IVec3) -> Vec<Vec3> {
+    let mut path = vec![Vec3::new(
+        current.x as f32,
+        current.y as f32,
+        current.z as f32,
+    )];
+
+    while let Some(&previous) = came_from.get(&current) {
+        path.push(Vec3::new(
+            previous.x as f32,
+            previous.y as f32,
+            previous.z as f32,
+        ));
+        current = previous;
+    }
+
+    path.reverse();
+    path
+}
+
 pub fn spawn_citizens(mut commands: Commands, asset_server: Res<AssetServer>) {
     let mut rng = rand::thread_rng();
 
diff --git a/src/main.rs b/src/main.rs
index 99452c4..876639a 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -41,6 +41,7 @@ fn main() {
             FixedUpdate,
             (
                 camera::camera_movement,
+                citizen::update_citizen_targets,
                 citizen::citizen_movement,
                 cursor::move_cursor,
                 // citizen::check_citizen_positions_for_chunks,
diff --git a/src/tile.rs b/src/tile.rs
index b1d6a40..164212a 100644
--- a/src/tile.rs
+++ b/src/tile.rs
@@ -72,8 +72,8 @@ pub fn camera_z_movement(
 
 #[derive(Resource, Default, Clone)]
 pub struct TileMap {
-    pub floors: HashMap<IVec3, (u32, bool, bool, u8, [u32; 8])>,
-    pub fixtures: HashMap<IVec3, (u32, bool, [u32; 8])>,
+    pub floor_tiles: HashMap<IVec3, (u32, bool, bool, u8, [u32; 8])>, // id, opaque, walkable, astar_weight, visible_range
+    pub fixture_tiles: HashMap<IVec3, (u32, bool, [u32; 8])>,
 }
 
 pub fn update_tile_visibility(
diff --git a/src/tilemap.rs b/src/tilemap.rs
index 663ddfb..7649847 100644
--- a/src/tilemap.rs
+++ b/src/tilemap.rs
@@ -76,7 +76,7 @@ pub fn calculate_visibility(pos: IVec3, tilemap: &TileMap) -> [u32; 8] {
 
     let mut visible_range = [0u32; 8];
 
-    for mut camera_z in -150..100 {
+    for mut camera_z in -15..5 {
         camera_z *= tile_size;
         let mut is_visible = false;
 
@@ -89,7 +89,7 @@ pub fn calculate_visibility(pos: IVec3, tilemap: &TileMap) -> [u32; 8] {
         'vertical_check: for z_offset in 1..=35 {
             let above_pos = IVec3::new(pos.x, pos.y, pos.z + (z_offset * tile_size));
             if above_pos.z <= camera_z {
-                if let Some(&(_, opaque, _, _, _)) = tilemap.floors.get(&above_pos) {
+                if let Some(&(_, opaque, _, _, _)) = tilemap.floor_tiles.get(&above_pos) {
                     if opaque {
                         is_occluded = true;
                         break 'vertical_check;
@@ -118,7 +118,7 @@ pub fn calculate_visibility(pos: IVec3, tilemap: &TileMap) -> [u32; 8] {
                             pos.z + z_offset * tile_size,
                         );
 
-                        if let Some(&(id, _, _, _, _)) = tilemap.floors.get(&neighbor_pos) {
+                        if let Some(&(id, _, _, _, _)) = tilemap.floor_tiles.get(&neighbor_pos) {
                             if id == 0 {
                                 is_visible = true;
                                 break 'neighbor_check;
@@ -196,7 +196,7 @@ fn handle_chunk_loading(
                 );
 
                 // Spawn tiles and add them to tilemap
-                for z in -150..50 {
+                for z in -15..5 {
                     let position = Vec3::new(
                         (world_x as f32 * TILE_SIZE).round(),
                         (world_y as f32 * TILE_SIZE).round(),
@@ -212,17 +212,21 @@ fn handle_chunk_loading(
                         ]);
                         if noise_value < -0.5 {
                             FloorTilePrefab::air(position, &asset_server).spawn(&mut commands);
-                            tilemap.floors.insert(pos_ivec, (0, false, true, 1, [0; 8])); // Air tile
+                            tilemap
+                                .floor_tiles
+                                .insert(pos_ivec, (0, false, true, 0, [0; 8])); // Air tile
                             floor_positions.push((position, "air"));
                         } else if noise_value < 0.8 {
                             FloorTilePrefab::rock(position, &asset_server).spawn(&mut commands);
                             tilemap
-                                .floors
-                                .insert(pos_ivec, (2, true, false, 255, [0; 8])); // Rock tile
+                                .floor_tiles
+                                .insert(pos_ivec, (2, true, false, 50, [0; 8])); // Rock tile
                             floor_positions.push((position, "rock"));
                         } else {
                             FloorTilePrefab::dirt(position, &asset_server).spawn(&mut commands);
-                            tilemap.floors.insert(pos_ivec, (1, true, true, 1, [0; 8])); // Dirt tile
+                            tilemap
+                                .floor_tiles
+                                .insert(pos_ivec, (1, true, true, 85, [0; 8])); // Dirt tile
                             floor_positions.push((position, "dirt"));
                         }
                     } else if noise_position.z > position.z {
@@ -230,16 +234,22 @@ fn handle_chunk_loading(
                             <= position.z + TILE_SIZE
                         {
                             FloorTilePrefab::grass(position, &asset_server).spawn(&mut commands);
-                            tilemap.floors.insert(pos_ivec, (1, true, true, 1, [0; 8])); // Dirt tile (grass)
+                            tilemap
+                                .floor_tiles
+                                .insert(pos_ivec, (1, true, true, 100, [0; 8])); // Dirt tile (grass)
                             floor_positions.push((position, "dirt"));
                         } else {
                             FloorTilePrefab::dirt(position, &asset_server).spawn(&mut commands);
-                            tilemap.floors.insert(pos_ivec, (1, true, true, 1, [0; 8])); // Dirt tile
+                            tilemap
+                                .floor_tiles
+                                .insert(pos_ivec, (1, true, true, 85, [0; 8])); // Dirt tile
                             floor_positions.push((position, "dirt"));
                         }
                     } else {
                         FloorTilePrefab::air(position, &asset_server).spawn(&mut commands);
-                        tilemap.floors.insert(pos_ivec, (0, false, true, 1, [0; 8])); // Air tile
+                        tilemap
+                            .floor_tiles
+                            .insert(pos_ivec, (0, false, true, 0, [0; 8])); // Air tile
                         floor_positions.push((position, "air"));
                     }
                 }
@@ -254,15 +264,18 @@ fn handle_chunk_loading(
             match *floor_type {
                 "dirt" => {
                     FixtureTilePrefab::dirt_wall(above_pos, &asset_server).spawn(&mut commands);
-                    tilemap.fixtures.insert(above_ivec, (1, true, [0; 8])); // Dirt wall
+                    tilemap.fixture_tiles.insert(above_ivec, (1, true, [0; 8]));
+                    // Dirt wall
                 }
                 "rock" => {
                     FixtureTilePrefab::rock_wall(above_pos, &asset_server).spawn(&mut commands);
-                    tilemap.fixtures.insert(above_ivec, (2, true, [0; 8])); // Rock wall
+                    tilemap.fixture_tiles.insert(above_ivec, (2, true, [0; 8]));
+                    // Rock wall
                 }
                 "bedrock" => {
                     FixtureTilePrefab::bedrock_wall(above_pos, &asset_server).spawn(&mut commands);
-                    tilemap.fixtures.insert(above_ivec, (3, true, [0; 8])); // Bedrock wall
+                    tilemap.fixture_tiles.insert(above_ivec, (3, true, [0; 8]));
+                    // Bedrock wall
                 }
                 _ => {}
             }
diff --git a/src/tiles.rs b/src/tiles.rs
index 803a292..84f707f 100644
--- a/src/tiles.rs
+++ b/src/tiles.rs
@@ -22,6 +22,7 @@ impl FloorTilePrefab {
             },
             tile: FloorTile {
                 id: 1,
+                astar_weight: 100,
                 ..Default::default()
             },
             tile_state: TileState {
@@ -43,6 +44,7 @@ impl FloorTilePrefab {
             },
             tile: FloorTile {
                 id: 2,
+                astar_weight: 85,
                 ..Default::default()
             },
             tile_state: TileState {
@@ -64,6 +66,7 @@ impl FloorTilePrefab {
             },
             tile: FloorTile {
                 id: 3,
+                astar_weight: 50,
                 ..Default::default()
             },
             tile_state: TileState {
@@ -107,6 +110,7 @@ impl FloorTilePrefab {
             },
             tile: FloorTile {
                 id: 4,
+                astar_weight: 150,
                 ..Default::default()
             },
             tile_state: TileState {