From 3fc74d10d99890d9657456a9fc7a959bb3390d83 Mon Sep 17 00:00:00 2001
From: Nolan Darilek <nolan@thewordnerd.info>
Date: Tue, 31 Jan 2023 16:48:26 -0600
Subject: [PATCH] Refactor navigation/pathfinding to actions.

---
 Cargo.toml         |   2 +-
 src/core.rs        |   4 +-
 src/navigation.rs  | 209 +++++++++++++++++++++++++++------------------
 src/pathfinding.rs | 107 +++++++++++++++++------
 4 files changed, 209 insertions(+), 113 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index f9efb42..6a1ac53 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -33,7 +33,7 @@ coord_2d = "0.3"
 futures-lite = "1"
 gilrs = "0.10"
 here_be_dragons = "0.2"
-leafwing-input-manager = "0.7"
+leafwing-input-manager = "0.8"
 maze_generator = "2"
 once_cell = "1"
 pathfinding = "4"
diff --git a/src/core.rs b/src/core.rs
index 08f271a..29e4223 100644
--- a/src/core.rs
+++ b/src/core.rs
@@ -298,11 +298,11 @@ pub trait PointLike {
     fn y(&self) -> f32;
 
     fn x_i32(&self) -> i32 {
-        self.x().trunc() as i32
+        self.x().round() as i32
     }
 
     fn y_i32(&self) -> i32 {
-        self.y().trunc() as i32
+        self.y().round() as i32
     }
 
     fn x_usize(&self) -> usize {
diff --git a/src/navigation.rs b/src/navigation.rs
index 9bb7e80..e214918 100644
--- a/src/navigation.rs
+++ b/src/navigation.rs
@@ -11,13 +11,13 @@ use crate::{
     error::error_handler,
     exploration::{ExplorationFocused, Exploring},
     log::Log,
-    pathfinding::Destination,
     utils::target_and_other,
 };
 
 #[derive(Actionlike, PartialEq, Eq, Clone, Copy, Hash, Debug)]
 pub enum NavigationAction {
     Move,
+    Translate,
     Rotate,
     SetLinearVelocity,
     SetAngularVelocity,
@@ -29,11 +29,31 @@ pub enum NavigationAction {
 
 #[derive(Component, Clone, Copy, Debug, Deref, DerefMut, Reflect)]
 #[reflect(Component)]
-pub struct MaxSpeed(pub f32);
+pub struct BackwardMovementFactor(pub f32);
 
-impl Default for MaxSpeed {
+impl Default for BackwardMovementFactor {
     fn default() -> Self {
-        MaxSpeed(2.)
+        Self(1.)
+    }
+}
+
+#[derive(Component, Clone, Copy, Debug, Deref, DerefMut, Reflect)]
+#[reflect(Component)]
+pub struct ForwardMovementFactor(pub f32);
+
+impl Default for ForwardMovementFactor {
+    fn default() -> Self {
+        Self(1.)
+    }
+}
+
+#[derive(Component, Clone, Copy, Debug, Deref, DerefMut, Reflect)]
+#[reflect(Component)]
+pub struct StrafeMovementFactor(pub f32);
+
+impl Default for StrafeMovementFactor {
+    fn default() -> Self {
+        Self(1.)
     }
 }
 
@@ -41,10 +61,6 @@ impl Default for MaxSpeed {
 #[reflect(Component)]
 pub struct RotationSpeed(pub Angle);
 
-#[derive(Component, Clone, Copy, Debug, Default, Deref, DerefMut, Reflect)]
-#[reflect(Component)]
-pub struct Speed(pub f32);
-
 #[derive(Deref, DerefMut)]
 struct SnapTimer(Timer);
 
@@ -57,25 +73,46 @@ impl Default for SnapTimer {
 #[derive(Resource, Default, Deref, DerefMut)]
 struct SnapTimers(HashMap<Entity, SnapTimer>);
 
-fn movement_controls<State>(
+#[derive(Component, Clone, Copy, Debug, Deref, DerefMut, Reflect)]
+#[reflect(Component)]
+pub struct Speed(pub f32);
+
+impl Default for Speed {
+    fn default() -> Self {
+        Self(1.)
+    }
+}
+
+fn controls(
     mut commands: Commands,
-    config: Res<NavigationPlugin<State>>,
+    time: Res<Time>,
     snap_timers: Res<SnapTimers>,
     mut query: Query<(
         Entity,
         &mut ActionState<NavigationAction>,
         &mut Velocity,
-        &mut Speed,
-        &MaxSpeed,
+        &Speed,
         Option<&RotationSpeed>,
+        Option<&BackwardMovementFactor>,
+        Option<&ForwardMovementFactor>,
+        Option<&StrafeMovementFactor>,
         &Transform,
+        Option<&mut KinematicCharacterController>,
     )>,
     exploration_focused: Query<Entity, With<ExplorationFocused>>,
-) where
-    State: 'static + Clone + Debug + Eq + Hash + Send + Sync,
-{
-    for (entity, mut actions, mut velocity, mut speed, max_speed, rotation_speed, transform) in
-        &mut query
+) {
+    for (
+        entity,
+        mut actions,
+        mut velocity,
+        speed,
+        rotation_speed,
+        backward_movement_factor,
+        forward_movement_factor,
+        strafe_movement_factor,
+        transform,
+        character_controller,
+    ) in &mut query
     {
         let mut cleanup = false;
         if actions.pressed(NavigationAction::Move) {
@@ -88,51 +125,81 @@ fn movement_controls<State>(
                 if direction.y.abs() < 0.5 {
                     direction.y = 0.;
                 }
+                let forward_movement_factor =
+                    forward_movement_factor.map(|v| v.0).unwrap_or_else(|| 1.);
+                let backward_movement_factor =
+                    backward_movement_factor.map(|v| v.0).unwrap_or_else(|| 1.);
+                let strafe_movement_factor =
+                    strafe_movement_factor.map(|v| v.0).unwrap_or_else(|| 1.);
                 let forward_backward_movement_factor = if direction.y > 0. {
-                    config.forward_movement_factor
+                    forward_movement_factor
                 } else if direction.y < 0. {
-                    config.backward_movement_factor
+                    backward_movement_factor
                 } else {
-                    0.
+                    1.
                 };
                 let movement_factor = if direction.x != 0. && direction.y != 0. {
-                    config
-                        .strafe_movement_factor
-                        .min(forward_backward_movement_factor)
+                    strafe_movement_factor.min(forward_backward_movement_factor)
                 } else if direction.x != 0. {
-                    config.strafe_movement_factor
+                    strafe_movement_factor
                 } else {
                     forward_backward_movement_factor
                 };
-                let mut s = **max_speed;
-                s *= movement_factor;
-                **speed = s;
-                let mut v = direction * **speed;
-                v = Vec2::new(v.y, -v.x);
-                v = transform
+                trace!("{entity:?}: move: {direction:?}");
+                direction = Vec2::new(direction.y, -direction.x);
+                direction = transform
                     .compute_matrix()
-                    .transform_vector3(v.extend(0.))
+                    .transform_vector3(direction.extend(0.))
                     .truncate();
-                actions.press(NavigationAction::SetLinearVelocity);
-                actions
-                    .action_data_mut(NavigationAction::SetLinearVelocity)
-                    .axis_pair = Some(DualAxisData::from_xy(v));
+                let mut speed = **speed;
+                speed *= movement_factor;
+                let velocity = direction * speed;
+                if character_controller.is_some() {
+                    let translation = velocity * time.delta_seconds();
+                    actions.press(NavigationAction::Translate);
+                    actions
+                        .action_data_mut(NavigationAction::Translate)
+                        .axis_pair = Some(DualAxisData::from_xy(translation));
+                } else {
+                    actions.press(NavigationAction::SetLinearVelocity);
+                    actions
+                        .action_data_mut(NavigationAction::SetLinearVelocity)
+                        .axis_pair = Some(DualAxisData::from_xy(velocity));
+                }
             }
-        }
-        if actions.released(NavigationAction::Move)
-            && actions.axis_pair(NavigationAction::Move).is_some()
-        {
-            actions.press(NavigationAction::SetLinearVelocity);
-            actions
-                .action_data_mut(NavigationAction::SetLinearVelocity)
-                .axis_pair = None;
+        } else if actions.just_released(NavigationAction::Move) {
+            trace!("{entity:?}: Stopped moving");
+            actions.release(NavigationAction::SetLinearVelocity);
+            actions.release(NavigationAction::Translate);
+            actions.action_data_mut(NavigationAction::Move).axis_pair = None;
         }
         if actions.pressed(NavigationAction::SetLinearVelocity) {
             if let Some(pair) = actions.axis_pair(NavigationAction::SetLinearVelocity) {
+                trace!("{entity:?}: SetLinearVelocity: {pair:?}");
                 velocity.linvel = pair.into();
             } else {
                 velocity.linvel = Vec2::ZERO;
             }
+        } else if actions.just_released(NavigationAction::SetLinearVelocity) {
+            trace!("{entity:?}: Released velocity");
+            velocity.linvel = Vec2::ZERO;
+            actions
+                .action_data_mut(NavigationAction::SetLinearVelocity)
+                .axis_pair = None;
+        }
+        if actions.pressed(NavigationAction::Translate) {
+            if let Some(pair) = actions.axis_pair(NavigationAction::Translate) {
+                if let Some(mut character_controller) = character_controller {
+                    character_controller.translation = Some(pair.xy());
+                }
+            }
+        } else if actions.just_released(NavigationAction::Translate) {
+            if let Some(mut character_controller) = character_controller {
+                character_controller.translation = None;
+            }
+            actions
+                .action_data_mut(NavigationAction::Translate)
+                .axis_pair = None;
         }
         if !snap_timers.contains_key(&entity) {
             if let Some(rotation_speed) = rotation_speed {
@@ -147,17 +214,19 @@ fn movement_controls<State>(
                 }
             }
         }
-        if actions.released(NavigationAction::Rotate) {
-            actions.press(NavigationAction::SetAngularVelocity);
-            actions
-                .action_data_mut(NavigationAction::SetAngularVelocity)
-                .value = 0.;
+        if actions.just_released(NavigationAction::Rotate) {
+            actions.release(NavigationAction::SetAngularVelocity);
+            actions.action_data_mut(NavigationAction::Rotate).value = 0.;
         }
         if actions.pressed(NavigationAction::SetAngularVelocity) {
             velocity.angvel = actions.value(NavigationAction::SetAngularVelocity);
+        } else if actions.just_released(NavigationAction::SetAngularVelocity) {
+            actions
+                .action_data_mut(NavigationAction::SetAngularVelocity)
+                .value = 0.;
+            velocity.angvel = 0.;
         }
         if cleanup {
-            commands.entity(entity).remove::<Destination>();
             commands.entity(entity).remove::<Exploring>();
             for entity in exploration_focused.iter() {
                 commands.entity(entity).remove::<ExplorationFocused>();
@@ -165,7 +234,6 @@ fn movement_controls<State>(
         }
     }
 }
-
 fn snap(
     mut tts: ResMut<Tts>,
     mut snap_timers: ResMut<SnapTimers>,
@@ -277,22 +345,6 @@ fn add_speed(mut commands: Commands, query: Query<Entity, (Added<Speed>, Without
     }
 }
 
-pub(crate) fn limit_speed(mut query: Query<(&mut Speed, &MaxSpeed)>) {
-    for (mut speed, max_speed) in &mut query {
-        if **speed > **max_speed {
-            **speed = **max_speed;
-        }
-    }
-}
-
-fn remove_speed(removed: RemovedComponents<Speed>, mut query: Query<&mut Velocity>) {
-    for entity in removed.iter() {
-        if let Ok(mut velocity) = query.get_mut(entity) {
-            velocity.linvel = Vec2::ZERO;
-        }
-    }
-}
-
 fn log_area_descriptions<State>(
     mut events: EventReader<CollisionEvent>,
     areas: Query<(&Area, Option<&Name>)>,
@@ -336,11 +388,10 @@ fn log_area_descriptions<State>(
     }
 }
 
+pub const MOVEMENT_CONTROLS: &str = "MOVEMENT_CONTROLS";
+
 #[derive(Resource, Clone, Debug)]
 pub struct NavigationPlugin<State> {
-    pub forward_movement_factor: f32,
-    pub backward_movement_factor: f32,
-    pub strafe_movement_factor: f32,
     pub states: Vec<State>,
     pub describe_undescribed_areas: bool,
     pub log_area_descriptions: bool,
@@ -349,9 +400,6 @@ pub struct NavigationPlugin<State> {
 impl<State> Default for NavigationPlugin<State> {
     fn default() -> Self {
         Self {
-            forward_movement_factor: 1.,
-            backward_movement_factor: 1.,
-            strafe_movement_factor: 1.,
             states: vec![],
             describe_undescribed_areas: false,
             log_area_descriptions: true,
@@ -366,7 +414,9 @@ where
     fn build(&self, app: &mut App) {
         app.insert_resource(self.clone());
         app.init_resource::<SnapTimers>()
-            .register_type::<MaxSpeed>()
+            .register_type::<BackwardMovementFactor>()
+            .register_type::<ForwardMovementFactor>()
+            .register_type::<StrafeMovementFactor>()
             .register_type::<RotationSpeed>()
             .register_type::<Speed>()
             .add_plugin(InputManagerPlugin::<NavigationAction>::default())
@@ -375,22 +425,15 @@ where
             .add_system(tick_snap_timers)
             .add_system(speak_direction.pipe(error_handler))
             .add_system(add_speed)
-            .add_system(limit_speed)
-            .add_system_to_stage(CoreStage::PostUpdate, remove_speed)
             .add_system_to_stage(CoreStage::PostUpdate, log_area_descriptions::<State>);
-        const MOVEMENT_CONTROLS: &str = "MOVEMENT_CONTROLS";
         if self.states.is_empty() {
-            app.add_system(
-                movement_controls::<State>
-                    .label(MOVEMENT_CONTROLS)
-                    .after(limit_speed),
-            )
-            .add_system(snap.pipe(error_handler).before(MOVEMENT_CONTROLS));
+            app.add_system(controls.label(MOVEMENT_CONTROLS))
+                .add_system(snap.pipe(error_handler).before(MOVEMENT_CONTROLS));
         } else {
             for state in &self.states {
                 app.add_system_set(
                     SystemSet::on_update(*state)
-                        .with_system(movement_controls::<State>.label(MOVEMENT_CONTROLS))
+                        .with_system(controls.label(MOVEMENT_CONTROLS))
                         .with_system(snap.pipe(error_handler).before(MOVEMENT_CONTROLS)),
                 );
             }
diff --git a/src/pathfinding.rs b/src/pathfinding.rs
index a64aa01..d8f09df 100644
--- a/src/pathfinding.rs
+++ b/src/pathfinding.rs
@@ -9,16 +9,35 @@ use bevy_rapier2d::{
     rapier::prelude::{ColliderHandle, ColliderSet, QueryPipeline, RigidBodySet},
 };
 use futures_lite::future;
-
+use leafwing_input_manager::{axislike::DualAxisData, plugin::InputManagerSystem, prelude::*};
 use pathfinding::prelude::*;
 
 use crate::{
     commands::RunIfExistsExt,
     core::PointLike,
     map::{Map, MapObstruction},
-    navigation::{RotationSpeed, Speed},
+    navigation::{NavigationAction, RotationSpeed},
 };
 
+#[derive(PartialEq, Eq, Clone, Copy, Hash, Debug)]
+pub struct NegotiatePathAction;
+
+impl Actionlike for NegotiatePathAction {
+    const N_VARIANTS: usize = 1;
+
+    fn get_at(index: usize) -> Option<Self> {
+        if index == 0 {
+            Some(Self)
+        } else {
+            None
+        }
+    }
+
+    fn index(&self) -> usize {
+        0
+    }
+}
+
 #[derive(Component, Debug, Deref, DerefMut)]
 struct Calculating(Task<Option<Path>>);
 
@@ -182,8 +201,7 @@ fn calculate_path(
                 .remove::<Path>()
                 .remove::<NoPath>()
                 .remove::<Calculating>()
-                .remove::<Destination>()
-                .remove::<Speed>();
+                .remove::<Destination>();
             continue;
         }
         let coordinates_clone = *coordinates;
@@ -264,49 +282,52 @@ fn negotiate_path(
     mut commands: Commands,
     mut query: Query<(
         Entity,
+        &mut ActionState<NavigationAction>,
         &mut Path,
         &mut Transform,
-        &mut Velocity,
-        &Speed,
         Option<&RotationSpeed>,
     )>,
 ) {
-    for (entity, mut path, mut transform, mut velocity, speed, rotation_speed) in &mut query {
+    for (entity, mut actions, mut path, mut transform, rotation_speed) in &mut query {
         let start_i32 = (transform.translation.x, transform.translation.y).i32();
+        trace!(
+            "{entity:?}: start pathfinding from {start_i32:?} to {:?}: {:?}",
+            path.last(),
+            transform.translation.truncate()
+        );
         let mut cleanup = false;
         if let Some(last) = path.last() {
             if last == &start_i32 {
-                trace!("{:?}: path: at destination", entity);
+                trace!("{entity:?}: path: at destination");
                 cleanup = true;
             }
         }
         if !cleanup {
             if let Some(position) = path.iter().position(|v| v == &start_i32) {
-                trace!("{:?}: Path contains start", entity);
+                trace!("{entity:?}: Path contains start");
                 let (_, new_path) = path.split_at(position + 1);
                 **path = new_path.to_vec();
             }
             if let Some(next) = path.first() {
-                trace!(
-                    "{:?}: path: moving from {:?} to {:?}",
-                    entity,
-                    start_i32,
-                    next
-                );
-                let start = transform.translation.truncate();
-                let next = Vec2::new(next.0 as f32 + 0.5, next.1 as f32 + 0.5);
+                trace!("{entity:?}: path: moving from {start_i32:?} to {next:?}");
+                let start = Vec2::new(start_i32.x(), start_i32.y());
+                let next = Vec2::new(next.0 as f32, next.1 as f32);
                 let mut direction = next - start;
                 direction = direction.normalize();
-                trace!("{:?}: path: direction: {:?}", entity, direction);
-                direction *= **speed;
-                velocity.linvel = direction;
-                trace!("{:?}: path: velocity: {:?}", entity, velocity.linvel);
+                trace!("{entity:?}: path: direction: {direction:?}");
+                actions.press(NavigationAction::Move);
+                actions.action_data_mut(NavigationAction::Move).axis_pair =
+                    Some(DualAxisData::from_xy(Vec2::new(-direction.y, direction.x)));
+                trace!(
+                    "{entity:?}: path: move: {:?}",
+                    Vec2::new(direction.y, -direction.x)
+                );
                 if rotation_speed.is_some() {
-                    let v = next - start;
-                    let angle = v.y.atan2(v.x);
+                    let angle = direction.y.atan2(direction.x);
                     transform.rotation = Quat::from_rotation_z(angle);
                 }
             } else {
+                trace!("{entity:?}: empty path, cleaning");
                 cleanup = true;
             }
         }
@@ -315,8 +336,35 @@ fn negotiate_path(
                 .entity(entity)
                 .remove::<Path>()
                 .remove::<NoPath>()
-                .remove::<Destination>()
-                .remove::<Speed>();
+                .remove::<Destination>();
+            actions.release(NavigationAction::Move);
+            trace!("{entity:?}: pathfinding: cleaned up");
+        }
+    }
+}
+
+fn actions(
+    mut commands: Commands,
+    mut query: Query<(
+        Entity,
+        &ActionState<NegotiatePathAction>,
+        Option<&mut Destination>,
+    )>,
+) {
+    for (entity, actions, destination) in &mut query {
+        if actions.pressed(NegotiatePathAction) {
+            if let Some(pair) = actions.axis_pair(NegotiatePathAction) {
+                let dest = Destination((pair.x() as i32, pair.y() as i32));
+                if let Some(mut current_dest) = destination {
+                    if *current_dest != dest {
+                        *current_dest = dest;
+                    }
+                } else {
+                    commands.entity(entity).insert(dest);
+                }
+            } else if destination.is_some() {
+                commands.entity(entity).remove::<Destination>();
+            }
         }
     }
 }
@@ -325,13 +373,18 @@ pub struct PathfindingPlugin;
 
 impl Plugin for PathfindingPlugin {
     fn build(&self, app: &mut App) {
-        app.register_type::<Destination>()
+        app.add_plugin(InputManagerPlugin::<NegotiatePathAction>::default())
+            .register_type::<Destination>()
             .register_type::<NoPath>()
             .register_type::<Path>()
             .register_type::<CostMap>()
             .add_system(calculate_path)
             .add_system_to_stage(CoreStage::PostUpdate, remove_destination)
             .add_system(poll_tasks)
-            .add_system_to_stage(CoreStage::PostUpdate, negotiate_path);
+            .add_system_to_stage(
+                CoreStage::PreUpdate,
+                negotiate_path.after(InputManagerSystem::Tick),
+            )
+            .add_system(actions);
     }
 }