From 594131a372fa2d65dff0515f9b62843516a6c69f Mon Sep 17 00:00:00 2001
From: StephenAdamson <stephen.h.adamson@gmail.com>
Date: Fri, 17 Jan 2025 23:58:19 +0000
Subject: [PATCH] movement redo part 1

---
 src/citizen.rs | 140 +++++++++++++++++++++++++++++++++----------------
 src/main.rs    |   2 +-
 2 files changed, 96 insertions(+), 46 deletions(-)

diff --git a/src/citizen.rs b/src/citizen.rs
index 5276d8e..3e4f038 100644
--- a/src/citizen.rs
+++ b/src/citizen.rs
@@ -32,7 +32,10 @@ impl Citizen {
 use crate::constants::TILE_SIZE;
 use crate::tile::TileMap;
 use rand::{seq::SliceRandom, Rng};
-use std::collections::{BinaryHeap, HashMap, HashSet};
+use std::{
+    collections::{BinaryHeap, HashMap, HashSet},
+    process::exit,
+};
 
 #[derive(Component)]
 pub struct Ambulatory {
@@ -105,6 +108,8 @@ pub fn update_citizen_targets(
                                 target_y as f32 * TILE_SIZE,
                                 z as f32 * TILE_SIZE,
                             ));
+                            println!("target: {:?}", ambulatory.target);
+                            exit(0);
                             ambulatory.current_path = None;
                             ambulatory.path_index = 0;
                             break;
@@ -119,9 +124,24 @@ pub fn update_citizen_targets(
 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() {
+        // Check if current position is valid
+        let current_pos = transform.translation;
+        let below_pos = Vec3::new(
+            current_pos.x,
+            current_pos.y,
+            current_pos.z - TILE_SIZE - 0.1,
+        );
+
+        // Must have solid ground below
+        if let Some(floor_tile) = tilemap.floor_tiles.get(&below_pos.as_ivec3()) {
+            if !floor_tile.1 {
+                transform.translation.z -= TILE_SIZE;
+                continue;
+            }
+        }
+
         if let Some(target) = ambulatory.target {
             // Convert world position to tile coordinates
             let current_tile = IVec3::new(
@@ -153,7 +173,7 @@ pub fn citizen_movement(
                     );
 
                     let direction = (next_world_pos - transform.translation).normalize();
-                    transform.translation += direction * ambulatory.speed * time.delta_secs();
+                    transform.translation += direction * ambulatory.speed;
 
                     // Update sprite direction (only for x movement)
                     if direction.x != 0.0 {
@@ -170,6 +190,22 @@ pub fn citizen_movement(
     }
 }
 
+fn is_valid_move(tilemap: &TileMap, pos: IVec3) -> bool {
+    // Check if the tile at current position is not blocking
+    if let Some(current_tile) = tilemap.floor_tiles.get(&pos) {
+        if current_tile.1 {
+            return false;
+        }
+    }
+
+    // Check if there's solid ground below
+    let pos_below = pos - IVec3::new(0, 0, 1);
+    if let Some(below_tile) = tilemap.floor_tiles.get(&pos_below) {
+        return below_tile.1;
+    }
+    false
+}
+
 fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec<Vec3> {
     let mut open_set = BinaryHeap::new();
     let mut came_from = HashMap::new();
@@ -185,6 +221,20 @@ fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec<Vec3> {
     open_set.push(start_node.clone());
     g_scores.insert(start, 0);
 
+    // Define allowed movements: orthogonal + diagonal on same level, and up/down
+    let allowed_moves = vec![
+        // Cardinal directions (same level)
+        IVec3::new(1, 0, 0),
+        IVec3::new(-1, 0, 0),
+        IVec3::new(0, 1, 0),
+        IVec3::new(0, -1, 0),
+        // Diagonal directions (same level)
+        IVec3::new(1, 1, 0),
+        IVec3::new(1, -1, 0),
+        IVec3::new(-1, 1, 0),
+        IVec3::new(-1, -1, 0),
+    ];
+
     while let Some(current) = open_set.pop() {
         if current.position == goal {
             return reconstruct_path(came_from, current.position);
@@ -192,52 +242,51 @@ fn calculate_path(tilemap: &TileMap, start: IVec3, goal: IVec3) -> Vec<Vec3> {
 
         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;
-                    }
+        for move_dir in &allowed_moves {
+            // Try same level
+            let mut neighbor_pos = current.position + *move_dir;
 
-                    let neighbor_pos = current.position + IVec3::new(dx, dy, dz);
+            // Try one up if same level is invalid
+            let up_pos = neighbor_pos + IVec3::new(0, 0, 1);
+            if !is_valid_move(tilemap, neighbor_pos) && is_valid_move(tilemap, up_pos) {
+                neighbor_pos = up_pos;
+            }
 
-                    // Skip if not walkable or already closed
-                    if !is_walkable(tilemap, neighbor_pos) || closed_set.contains(&neighbor_pos) {
-                        continue;
-                    }
+            // Try one down if same level is invalid
+            let down_pos = neighbor_pos - IVec3::new(0, 0, 1);
+            if !is_valid_move(tilemap, neighbor_pos) && is_valid_move(tilemap, down_pos) {
+                neighbor_pos = down_pos;
+            }
 
-                    // 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!(),
-                    };
+            if !is_valid_move(tilemap, neighbor_pos) || closed_set.contains(&neighbor_pos) {
+                continue;
+            }
 
-                    // 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
-                        };
+            // Calculate movement cost
+            let movement_cost = match (
+                move_dir.x.abs() + move_dir.y.abs(),
+                neighbor_pos.z - current.position.z,
+            ) {
+                (1, 0) => 10,  // Orthogonal movement
+                (2, 0) => 14,  // Diagonal movement
+                (_, 1) => 20,  // Moving up one level
+                (_, -1) => 15, // Moving down one level
+                _ => continue, // Invalid movement
+            };
 
-                    let new_g = g_scores[&current.position] + movement_cost + tile_weight;
+            let new_g = g_scores[&current.position] + movement_cost;
 
-                    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;
+            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);
-                    }
-                }
+                open_set.push(PathNode {
+                    position: neighbor_pos,
+                    f_score: f,
+                    g_score: new_g,
+                });
+                came_from.insert(neighbor_pos, current.position);
             }
         }
     }
@@ -286,9 +335,10 @@ pub fn spawn_citizens(mut commands: Commands, asset_server: Res<AssetServer>) {
 
     // Spawn a handful of citizens
     for _ in 0..10 {
-        let x = rng.gen_range(-15.0..10.0);
-        let y = rng.gen_range(-10.0..10.0);
-        let position = Vec3::new(x, y, 0.1) * TILE_SIZE;
+        let x: f32 = rng.gen_range(-15.0..10.0);
+        let y: f32 = rng.gen_range(-10.0..10.0);
+        let mut position = Vec3::new(x.round(), y.round(), 5.0) * TILE_SIZE;
+        position.z += 0.1;
 
         commands.spawn(Citizen::new(&asset_server, position));
     }
diff --git a/src/main.rs b/src/main.rs
index 876639a..4d312d5 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -54,7 +54,7 @@ fn main() {
                 tile::camera_z_movement,
                 tile::update_tile_visibility
                     .run_if(|camera_moved: Res<tile::CameraMoved>| camera_moved.0),
-                log_fps_system,
+                // log_fps_system,
             ),
         )
         .run();