blackout/src/navigation.rs

use std::{collections::HashMap, error::Error, f32::consts::PI, fmt::Debug, hash::Hash};

use bevy::prelude::*;
use bevy_rapier2d::prelude::*;
use bevy_tts::Tts;
use leafwing_input_manager::{axislike::DualAxisData, prelude::*};

use crate::{
    commands::RunIfExistsExt,
    core::{Angle, Area, CardinalDirection, GlobalTransformExt, Player},
    error::error_handler,
    exploration::{ExplorationFocused, Exploring},
    log::Log,
    utils::target_and_other,
};

#[derive(Actionlike, PartialEq, Eq, Clone, Copy, Hash, Debug)]
pub enum NavigationAction {
    Move,
    Translate,
    Rotate,
    SetLinearVelocity,
    SetAngularVelocity,
    SnapLeft,
    SnapRight,
    SnapCardinal,
    SnapReverse,
}

#[derive(Component, Clone, Copy, Debug, Deref, DerefMut, Reflect)]
#[reflect(Component)]
pub struct BackwardMovementFactor(pub f32);

impl Default for BackwardMovementFactor {
    fn default() -> Self {
        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.)
    }
}

#[derive(Component, Clone, Copy, Default, Debug, Deref, DerefMut, Reflect)]
#[reflect(Component)]
pub struct RotationSpeed(pub Angle);

#[derive(Deref, DerefMut)]
struct SnapTimer(Timer);

impl Default for SnapTimer {
    fn default() -> Self {
        Self(Timer::from_seconds(0.2, TimerMode::Once))
    }
}

#[derive(Resource, Default, Deref, DerefMut)]
struct SnapTimers(HashMap<Entity, SnapTimer>);

#[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,
    time: Res<Time>,
    snap_timers: Res<SnapTimers>,
    mut query: Query<(
        Entity,
        &mut ActionState<NavigationAction>,
        &mut Velocity,
        &Speed,
        Option<&RotationSpeed>,
        Option<&BackwardMovementFactor>,
        Option<&ForwardMovementFactor>,
        Option<&StrafeMovementFactor>,
        &Transform,
        Option<&mut KinematicCharacterController>,
    )>,
    exploration_focused: Query<Entity, With<ExplorationFocused>>,
) {
    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) {
            if let Some(pair) = actions.clamped_axis_pair(NavigationAction::Move) {
                cleanup = true;
                let mut direction = pair.xy();
                if direction.x.abs() < 0.5 {
                    direction.x = 0.;
                }
                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. {
                    forward_movement_factor
                } else if direction.y < 0. {
                    backward_movement_factor
                } else {
                    1.
                };
                let movement_factor = if direction.x != 0. && direction.y != 0. {
                    strafe_movement_factor.min(forward_backward_movement_factor)
                } else if direction.x != 0. {
                    strafe_movement_factor
                } else {
                    forward_backward_movement_factor
                };
                trace!("{entity:?}: move: {direction:?}");
                direction = Vec2::new(direction.y, -direction.x);
                direction = transform
                    .compute_matrix()
                    .transform_vector3(direction.extend(0.))
                    .truncate();
                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));
                }
            }
        } 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 {
                if actions.pressed(NavigationAction::Rotate) {
                    cleanup = true;
                    let delta =
                        -rotation_speed.radians() * actions.clamped_value(NavigationAction::Rotate);
                    actions.press(NavigationAction::SetAngularVelocity);
                    actions
                        .action_data_mut(NavigationAction::SetAngularVelocity)
                        .value = delta;
                }
            }
        }
        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::<Exploring>();
            for entity in exploration_focused.iter() {
                commands.entity(entity).remove::<ExplorationFocused>();
            }
        }
    }
}
fn snap(
    mut tts: ResMut<Tts>,
    mut snap_timers: ResMut<SnapTimers>,
    mut query: Query<
        (
            Entity,
            &ActionState<NavigationAction>,
            &mut Transform,
            &CardinalDirection,
        ),
        With<Player>,
    >,
) -> Result<(), Box<dyn Error>> {
    for (entity, actions, mut transform, direction) in &mut query {
        if snap_timers.contains_key(&entity) {
            continue;
        } else if actions.just_pressed(NavigationAction::SnapLeft) {
            snap_timers.insert(entity, SnapTimer::default());
            transform.rotation = Quat::from_rotation_z(match direction {
                CardinalDirection::North => PI,
                CardinalDirection::East => PI / 2.,
                CardinalDirection::South => 0.,
                CardinalDirection::West => -PI / 2.,
            });
        } else if actions.just_pressed(NavigationAction::SnapRight) {
            snap_timers.insert(entity, SnapTimer::default());
            transform.rotation = Quat::from_rotation_z(match direction {
                CardinalDirection::North => 0.,
                CardinalDirection::East => -PI / 2.,
                CardinalDirection::South => PI,
                CardinalDirection::West => PI / 2.,
            });
        } else if actions.just_pressed(NavigationAction::SnapReverse) {
            snap_timers.insert(entity, SnapTimer::default());
            transform.rotate(Quat::from_rotation_z(PI));
        } else if actions.just_pressed(NavigationAction::SnapCardinal) {
            let yaw: Angle = direction.into();
            let yaw = yaw.radians();
            transform.rotation = Quat::from_rotation_z(yaw);
            tts.speak(direction.to_string(), true)?;
        }
    }
    Ok(())
}

fn tick_snap_timers(time: Res<Time>, mut snap_timers: ResMut<SnapTimers>) {
    for timer in snap_timers.values_mut() {
        timer.tick(time.delta());
    }
    snap_timers.retain(|_, v| !v.finished());
}

fn update_direction(
    mut commands: Commands,
    mut query: Query<
        (Entity, &GlobalTransform, Option<&mut CardinalDirection>),
        (With<Player>, Changed<Transform>),
    >,
) {
    for (entity, transform, direction) in query.iter_mut() {
        let yaw = transform.yaw();
        let new_direction: CardinalDirection = yaw.into();
        if let Some(mut direction) = direction {
            if *direction != new_direction {
                *direction = new_direction;
            }
        } else {
            commands.entity(entity).insert(new_direction);
        }
    }
}

fn remove_direction(
    mut commands: Commands,
    removed: RemovedComponents<Transform>,
    directions: Query<&CardinalDirection>,
) {
    for entity in removed.iter() {
        if directions.contains(entity) {
            commands.run_if_exists(entity, |mut entity| {
                entity.remove::<CardinalDirection>();
            });
        }
    }
}

fn speak_direction(
    mut tts: ResMut<Tts>,
    player: Query<
        (&CardinalDirection, ChangeTrackers<CardinalDirection>),
        (With<Player>, Changed<CardinalDirection>),
    >,
) -> Result<(), Box<dyn Error>> {
    if let Ok((direction, change)) = player.get_single() {
        if !change.is_added() {
            let direction: String = (*direction).into();
            tts.speak(direction, true)?;
        }
    }
    Ok(())
}

fn add_speed(mut commands: Commands, query: Query<Entity, (Added<Speed>, Without<Velocity>)>) {
    for entity in query.iter() {
        commands.entity(entity).insert(Velocity {
            linvel: Vec2::ZERO,
            ..default()
        });
    }
}

fn log_area_descriptions<State>(
    mut events: EventReader<CollisionEvent>,
    areas: Query<(&Area, Option<&Name>)>,
    players: Query<&Player>,
    config: Res<NavigationPlugin<State>>,
    mut log: Query<&mut Log>,
) where
    State: 'static + Send + Sync,
{
    if !config.log_area_descriptions {
        return;
    }
    for event in events.iter() {
        let (entity1, entity2, started) = match event {
            CollisionEvent::Started(collider1, collider2, _) => (collider1, collider2, true),
            CollisionEvent::Stopped(collider1, collider2, _) => (collider1, collider2, false),
        };
        if let Some((area, other)) = target_and_other(*entity1, *entity2, &|v| areas.get(v).is_ok())
        {
            if players.get(other).is_ok() {
                if let Ok((aabb, area_name)) = areas.get(area) {
                    let name = if let Some(name) = area_name {
                        Some(name.to_string())
                    } else if config.describe_undescribed_areas {
                        Some(format!("{}-by-{} area", aabb.extents().x, aabb.extents().y))
                    } else {
                        None
                    };
                    if let Some(name) = name {
                        if let Ok(mut log) = log.get_single_mut() {
                            if started {
                                log.push(format!("Entering {name}."));
                            } else {
                                log.push(format!("Leaving {name}."));
                            }
                        }
                    }
                }
            }
        }
    }
}

pub const MOVEMENT_CONTROLS: &str = "MOVEMENT_CONTROLS";

#[derive(Resource, Clone, Debug)]
pub struct NavigationPlugin<State> {
    pub states: Vec<State>,
    pub describe_undescribed_areas: bool,
    pub log_area_descriptions: bool,
}

impl<State> Default for NavigationPlugin<State> {
    fn default() -> Self {
        Self {
            states: vec![],
            describe_undescribed_areas: false,
            log_area_descriptions: true,
        }
    }
}

impl<State> Plugin for NavigationPlugin<State>
where
    State: 'static + Clone + Copy + Debug + Eq + Hash + Send + Sync,
{
    fn build(&self, app: &mut App) {
        app.insert_resource(self.clone());
        app.init_resource::<SnapTimers>()
            .register_type::<BackwardMovementFactor>()
            .register_type::<ForwardMovementFactor>()
            .register_type::<StrafeMovementFactor>()
            .register_type::<RotationSpeed>()
            .register_type::<Speed>()
            .add_plugin(InputManagerPlugin::<NavigationAction>::default())
            .add_system_to_stage(CoreStage::PreUpdate, update_direction)
            .add_system_to_stage(CoreStage::PostUpdate, remove_direction)
            .add_system(tick_snap_timers)
            .add_system(speak_direction.pipe(error_handler))
            .add_system(add_speed)
            .add_system_to_stage(CoreStage::PostUpdate, log_area_descriptions::<State>);
        if self.states.is_empty() {
            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(controls.label(MOVEMENT_CONTROLS))
                        .with_system(snap.pipe(error_handler).before(MOVEMENT_CONTROLS)),
                );
            }
        }
    }
}