diff --git a/src/entities/tasks/demo.rs b/src/entities/tasks/demo.rs index 06504cc..2ac17c7 100644 --- a/src/entities/tasks/demo.rs +++ b/src/entities/tasks/demo.rs @@ -22,6 +22,8 @@ use bevy::prelude::*; use rustc_hash::FxHashSet; +use smallvec::SmallVec; +use std::collections::VecDeque; use crate::constants::ITILE_SIZE; use crate::entities::cargo::{Cargo, Haulable}; @@ -48,11 +50,21 @@ pub enum DemoState { /// A ChopTree task has been assigned to `chopper`. /// `trunk_pos` is the lowest trunk tile of the target tree. Chopping { trunk_pos: IVec3, chopper: Entity }, - /// Tree has been felled. Waiting for all visible logs to be claimed/hauled. - /// Tracks which log entities have been assigned haul tasks. + /// Tree has been felled. Working through the haul queue. + /// + /// `unassigned` — logs not yet claimed, ordered nearest-to-origin first. + /// `in_progress` — log entities currently assigned to a dorf. + /// + /// Each tick: assign idle dorfs from unassigned. When a log entity + /// disappears from cargo_tiles it has been dropped at destination — + /// remove it from in_progress. When both are empty, go Idle. Hauling { felled_trunk_pos: IVec3, - assigned_logs: FxHashSet, + /// Queue of (log_entity, log_tile_pos) not yet assigned. + /// Front = highest priority (nearest to origin). + unassigned: VecDeque<(Entity, IVec3)>, + /// Log entities currently being hauled by a dorf. + in_progress: FxHashSet, }, } @@ -61,7 +73,7 @@ pub enum DemoState { /// State transitions: /// Idle → Chopping: found a tree, assigned ChopTree to one idle dorf /// Chopping → Hauling: trunk no longer in fixture_tiles (tree felled) -/// Hauling → Idle: all assigned logs cleared from cargo_tiles (hauled) +/// Hauling → Idle: both unassigned and in_progress are empty /// /// Performance: scans fixture_tiles once per state transition (infrequent), /// not per tick. During Chopping and Hauling states the system does O(1) checks. @@ -73,7 +85,7 @@ pub fn demo_system( cargo_query: Query<(Entity, &Cargo), With>, mut dorf_query: Query<(Entity, &mut TaskQueue, &mut TaskState, &Transform)>, ) { - match &*demo_state { + match &mut *demo_state { DemoState::Idle => { // Find the nearest standing tree to (0,0). // A "tree" is identified by a TreePart with is_trunk=true whose @@ -114,7 +126,6 @@ pub fn demo_system( }; // Find the lowest trunk tile (minimum z) for this tree's XY column. - // fell_tree is called with the lowest trunk pos. let lowest_trunk = tree_parts .iter() .filter(|(_, p)| { @@ -148,7 +159,6 @@ pub fn demo_system( // Assign ChopTree task if let Ok((_, mut queue, mut state, _)) = dorf_query.get_mut(chopper) { - // Clear current idle task and push ChopTree queue.clear(); queue.push(Task::ChopTree { trunk_pos: lowest_trunk, @@ -182,7 +192,6 @@ pub fn demo_system( |t| matches!(t, Task::ChopTree { trunk_pos: tp, .. } if *tp == trunk_pos), ); if !still_chopping && queue.is_empty() { - // Chopper abandoned — reassign *demo_state = DemoState::Idle; warn!("Demo: chopper {:?} abandoned ChopTree — resetting", chopper); } @@ -195,101 +204,129 @@ pub fn demo_system( // Find all Cargo logs near the trunk position let search_world = LOG_SEARCH_RADIUS_TILES * ITILE_SIZE; - let logs: Vec<(Entity, IVec3)> = cargo_query + let mut logs: SmallVec<[(Entity, IVec3, i32); 8]> = cargo_query .iter() .filter(|(_, cargo)| { cargo.name == "log" && (cargo.tile_pos.x - trunk_pos.x).abs() <= search_world && (cargo.tile_pos.y - trunk_pos.y).abs() <= search_world }) - .map(|(e, cargo)| (e, cargo.tile_pos)) + .map(|(e, cargo)| { + // Sort key: Chebyshev distance from origin + let dx = cargo.tile_pos.x.abs() / ITILE_SIZE; + let dy = cargo.tile_pos.y.abs() / ITILE_SIZE; + (e, cargo.tile_pos, dx.max(dy)) + }) .collect(); + // Nearest to origin first — dorfs haul the closest logs first + logs.sort_by_key(|(_, _, dist)| *dist); + if logs.is_empty() { - // No logs found — tree may have had no trunks or logs all orphaned warn!("Demo: no logs found after felling {:?}", trunk_pos); *demo_state = DemoState::Idle; return; } - // Find haul destination — nearest standable tile to (0,0) - let dest = tilemap - .find_nearest_free_cargo_tile(IVec3::ZERO, HAUL_DEST_SEARCH_RADIUS) - .unwrap_or(IVec3::ZERO); + // Convert to VecDeque, dropping sort key + let unassigned: VecDeque<(Entity, IVec3)> = + logs.into_iter().map(|(e, pos, _)| (e, pos)).collect(); - // Assign one HaulCargo task per log to idle dorfs - let mut assigned_logs: FxHashSet = FxHashSet::default(); - let mut logs_iter = logs.iter(); - - // Re-collect idle dorfs (mutable query needed) - // Must collect entities first to avoid double-borrow - let idle_dorfs: Vec = dorf_query - .iter() - .filter(|(_, queue, state, _)| is_idle_dorf(queue, state)) - .map(|(e, _, _, _)| e) - .collect(); - - for dorf_entity in idle_dorfs { - let Some(&(log_entity, log_pos)) = logs_iter.next() else { - break; // more dorfs than logs — remaining dorfs stay idle - }; - if let Ok((_, mut queue, mut state, _)) = dorf_query.get_mut(dorf_entity) { - queue.clear(); - queue.push(Task::HaulCargo { - cargo_entity: log_entity, - cargo_pos: log_pos, - dest, - step: HaulStep::MovingToCargo, - }); - *state = TaskState::Pending; - assigned_logs.insert(log_entity); - } - } - - // Any remaining unassigned logs stay on the floor — future dorfs - // will be assigned when they become idle if the demo loops. - // For simplicity: log unassigned count but don't retry this tick. - let unassigned = logs.len().saturating_sub(assigned_logs.len()); - if unassigned > 0 { - info!( - "Demo: {} logs unassigned (not enough idle dorfs)", - unassigned - ); - } + info!( + "Demo: {} logs to haul from {:?}", + unassigned.len(), + trunk_pos + ); *demo_state = DemoState::Hauling { felled_trunk_pos: trunk_pos, - assigned_logs, + unassigned, + in_progress: FxHashSet::default(), }; } DemoState::Hauling { felled_trunk_pos, - assigned_logs, + unassigned, + in_progress, } => { let felled_trunk_pos = *felled_trunk_pos; - // Check if all assigned logs have been hauled (removed from cargo_tiles) - // Check via cargo_query: if the Cargo entity still exists - // and is still in cargo_tiles, it hasn't been hauled yet. - let remaining = assigned_logs - .iter() - .filter(|&&log_entity| { - cargo_query - .get(log_entity) - .map(|(_, cargo)| tilemap.cargo_tiles.contains_key(&cargo.tile_pos)) - .unwrap_or(false) - }) - .count(); + // Remove completed hauls from in_progress — + // a log is done when it's no longer in cargo_tiles (picked up by dorf). + // This is correct: once picked up, the haul is committed from demo's perspective. + in_progress.retain(|&log_entity| { + cargo_query + .get(log_entity) + .map(|(_, cargo)| tilemap.cargo_tiles.contains_key(&cargo.tile_pos)) + .unwrap_or(false) + }); - if remaining == 0 { + // Assign idle dorfs to unassigned logs + if !unassigned.is_empty() { + // Collect idle dorfs sorted by proximity to front of log queue + let next_log_pos = unassigned.front().map(|(_, p)| *p).unwrap_or(IVec3::ZERO); + + let mut idle_dorfs: SmallVec<[(Entity, i32); 8]> = dorf_query + .iter() + .filter(|(_, queue, state, _)| is_idle_dorf(queue, state)) + .map(|(e, _, _, transform)| { + let pos = transform.translation.as_ivec3(); + let dx = (pos.x - next_log_pos.x).abs() / ITILE_SIZE; + let dy = (pos.y - next_log_pos.y).abs() / ITILE_SIZE; + (e, dx.max(dy)) + }) + .collect(); + // Sort by distance — nearest dorf gets nearest log + idle_dorfs.sort_by_key(|(_, d)| *d); + // Drop distance, keep entity + let idle_dorfs: SmallVec<[Entity; 8]> = + idle_dorfs.into_iter().map(|(e, _)| e).collect(); + + for dorf_entity in idle_dorfs { + // Compute haul destination fresh for each assignment — + // as logs accumulate near origin, each new assignment correctly + // finds the next free tile. + let dest = tilemap + .find_nearest_free_cargo_tile(IVec3::ZERO, HAUL_DEST_SEARCH_RADIUS) + .unwrap_or(IVec3::ZERO); + + let Some((log_entity, log_pos)) = unassigned.pop_front() else { + break; + }; + + // Verify log still exists and is in cargo_tiles before assigning + if !tilemap.cargo_tiles.contains_key(&log_pos) { + // Log already picked up by someone else — skip it + continue; + } + + if let Ok((_, mut queue, mut state, _)) = dorf_query.get_mut(dorf_entity) { + queue.clear(); + queue.push(Task::HaulCargo { + cargo_entity: log_entity, + cargo_pos: log_pos, + dest, + step: HaulStep::MovingToCargo, + }); + *state = TaskState::Pending; + in_progress.insert(log_entity); + } else { + // Couldn't assign — put log back at front of queue + unassigned.push_front((log_entity, log_pos)); + break; + } + } + } + + // Check if all work is done + if unassigned.is_empty() && in_progress.is_empty() { info!( "Demo: all logs hauled from {:?}, finding next tree", felled_trunk_pos ); *demo_state = DemoState::Idle; } - // else: still hauling, check again next tick (cheap) } } } diff --git a/src/entities/tasks/executor.rs b/src/entities/tasks/executor.rs index 364e817..8b66854 100644 --- a/src/entities/tasks/executor.rs +++ b/src/entities/tasks/executor.rs @@ -180,7 +180,8 @@ pub fn task_executor_system( let chopper_pos = transform.translation.truncate(); // XY only let trunk_xy = Vec2::new(trunk_pos.x as f32, trunk_pos.y as f32); let mut fall_dir = (trunk_xy - chopper_pos).normalize_or_zero(); - // If dorf is standing on the trunk (distance ~0), fall direction is arbitrary + // If dorf is standing on the trunk (distance ~0), normalize_or_zero returns ZERO. + // Safe to use == here because normalize_or_zero produces exact zero components. if fall_dir == Vec2::ZERO { fall_dir = Vec2::new(1.0, 0.0); // default: fall east }