blackout/src/navigation.rs

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

use bevy::{math::CompassQuadrant, prelude::*};
use bevy_rapier2d::prelude::*;
use bevy_tts::Tts;
use leafwing_input_manager::prelude::*;

use crate::{
    core::{Area, CardinalDirection, GlobalTransformExt, Player, TransformExt},
    error::error_handler,
    log::Log,
    utils::target_and_other,
};

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

impl Actionlike for NavigationAction {
    fn input_control_kind(&self) -> InputControlKind {
        match &self {
            NavigationAction::Move
            | NavigationAction::Translate
            | NavigationAction::SetLinearVelocity => InputControlKind::DualAxis,
            NavigationAction::Rotate | NavigationAction::SetAngularVelocity => {
                InputControlKind::Axis
            }
            NavigationAction::SnapLeft
            | NavigationAction::SnapRight
            | NavigationAction::SnapCardinal
            | NavigationAction::SnapReverse => InputControlKind::Button,
        }
    }
}

#[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 Rot2);

#[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 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.0 {
                CompassQuadrant::North => PI,
                CompassQuadrant::East => PI / 2.,
                CompassQuadrant::South => 0.,
                CompassQuadrant::West => -PI / 2.,
            });
        } else if actions.just_pressed(&NavigationAction::SnapRight) {
            snap_timers.insert(entity, SnapTimer::default());
            transform.rotation = Quat::from_rotation_z(match direction.0 {
                CompassQuadrant::North => 0.,
                CompassQuadrant::East => -PI / 2.,
                CompassQuadrant::South => PI,
                CompassQuadrant::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) {
            println!("Direction: {direction:?}");
            let yaw: Rot2 = direction.into();
            let yaw = yaw.as_radians();
            println!("Yaw: {yaw}");
            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 controls(
    rapier_context: Res<RapierContext>,
    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>,
        &mut Transform,
        &Collider,
    )>,
) {
    for (
        entity,
        mut actions,
        mut velocity,
        speed,
        rotation_speed,
        backward_movement_factor,
        forward_movement_factor,
        strafe_movement_factor,
        mut transform,
        collider,
    ) in &mut query
    {
        if !actions.action_disabled(&NavigationAction::Move) {
            let mut direction = actions.clamped_axis_pair(&NavigationAction::Move);
            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.x > 0. {
                forward_movement_factor
            } else if direction.x < 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.y != 0. {
                strafe_movement_factor
            } else {
                forward_backward_movement_factor
            };
            trace!("{entity:?}: move: {direction:?}");
            direction = transform
                .compute_matrix()
                .transform_vector3(direction.extend(0.))
                .truncate();
            let mut speed = **speed;
            speed *= movement_factor;
            let move_velocity = direction * speed;
            // println!("{entity:?}: SetLinearVelocity: {velocity:?}");
            actions.set_axis_pair(&NavigationAction::SetLinearVelocity, move_velocity);
        }
        if velocity.linvel != actions.axis_pair(&NavigationAction::SetLinearVelocity) {
            velocity.linvel = actions.axis_pair(&NavigationAction::SetLinearVelocity);
        }
        if actions.axis_pair(&NavigationAction::Translate) != Vec2::ZERO {
            let pair = actions.axis_pair(&NavigationAction::Translate);
            if rapier_context
                .cast_shape(
                    transform.translation.truncate(),
                    transform.yaw().as_radians(),
                    pair,
                    collider,
                    ShapeCastOptions {
                        max_time_of_impact: 1.,
                        ..default()
                    },
                    QueryFilter::new()
                        .exclude_sensors()
                        .exclude_collider(entity),
                )
                .is_none()
            {
                transform.translation += pair.extend(0.);
            }
            actions.set_axis_pair(&NavigationAction::Translate, Vec2::ZERO);
        }
        if !snap_timers.contains_key(&entity) {
            if let Some(rotation_speed) = rotation_speed {
                let delta =
                    rotation_speed.as_radians() * actions.clamped_value(&NavigationAction::Rotate);
                actions.set_value(&NavigationAction::SetAngularVelocity, delta);
            }
        }
        if actions.value(&NavigationAction::SetAngularVelocity) != 0. {
            // velocity.angvel =
            // actions.value(&NavigationAction::SetAngularVelocity);
            transform.rotation *= Quat::from_rotation_z(
                actions.value(&NavigationAction::SetAngularVelocity) * time.delta_seconds(),
            );
        }
    }
}

fn update_direction(
    mut commands: Commands,
    mut query: Query<
        (Entity, &GlobalTransform, Option<&mut CardinalDirection>),
        (With<Player>, Changed<Transform>),
    >,
) {
    for (entity, transform, direction) in &mut query {
        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,
    mut removed: RemovedComponents<Transform>,
    directions: Query<&CardinalDirection>,
) {
    for entity in removed.read() {
        if directions.contains(entity) {
            commands.entity(entity).remove::<CardinalDirection>();
        }
    }
}

fn speak_direction(
    mut tts: ResMut<Tts>,
    player: Query<&CardinalDirection, (With<Player>, Changed<CardinalDirection>)>,
) -> Result<(), Box<dyn Error>> {
    if let Ok(direction) = player.get_single() {
        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 {
        commands.entity(entity).insert(Velocity {
            linvel: Vec2::ZERO,
            ..default()
        });
    }
}

fn log_area_descriptions(
    mut events: EventReader<CollisionEvent>,
    areas: Query<(&Area, Option<&Name>)>,
    players: Query<&Player>,
    config: Res<NavigationPlugin>,
    mut log: Query<&mut Log>,
) {
    if !config.log_area_descriptions {
        return;
    }
    for event in events.read() {
        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}."));
                            }
                        }
                    }
                }
            }
        }
    }
}

#[derive(Debug, Hash, PartialEq, Eq, Clone, SystemSet)]
pub struct Movement;

#[derive(Resource, Clone, Debug)]
pub struct NavigationPlugin {
    pub describe_undescribed_areas: bool,
    pub log_area_descriptions: bool,
}

impl Default for NavigationPlugin {
    fn default() -> Self {
        Self {
            describe_undescribed_areas: false,
            log_area_descriptions: true,
        }
    }
}

impl Plugin for NavigationPlugin {
    fn build(&self, app: &mut App) {
        app.insert_resource(self.clone())
            .init_resource::<SnapTimers>()
            .register_type::<BackwardMovementFactor>()
            .register_type::<ForwardMovementFactor>()
            .register_type::<StrafeMovementFactor>()
            .register_type::<RotationSpeed>()
            .register_type::<Speed>()
            .add_plugins(InputManagerPlugin::<NavigationAction>::default())
            .add_systems(PreUpdate, (update_direction, add_speed))
            .add_systems(
                Update,
                (snap.pipe(error_handler), controls)
                    .chain()
                    .in_set(Movement),
            )
            .add_systems(
                FixedUpdate,
                (tick_snap_timers, speak_direction.pipe(error_handler)),
            )
            .add_systems(PostUpdate, (remove_direction, log_area_descriptions));
    }
}