blackout/src/navigation.rs

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

use bevy::prelude::*;
use bevy_input_actionmap::InputMap;
use bevy_rapier2d::prelude::*;
use bevy_tts::Tts;

use crate::{
    commands::RunIfExistsExt,
    core::{Angle, CardinalDirection, Player},
    error::error_handler,
    exploration::{ExplorationFocused, Exploring},
    pathfinding::Destination,
};

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

impl Default for MaxSpeed {
    fn default() -> Self {
        MaxSpeed(2.)
    }
}

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

impl Default for RotationSpeed {
    fn default() -> Self {
        Self(Angle::Radians(0.))
    }
}

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

#[derive(Component, Clone, Copy, Debug, Default, Reflect)]
#[reflect(Component)]
pub struct Sprinting;

#[derive(Default, Deref, DerefMut)]
struct Snapping(bool);

fn movement_controls<S, A: 'static>(
    mut commands: Commands,
    config: Res<NavigationConfig<S, A>>,
    input: Res<InputMap<A>>,
    time: Res<Time>,
    mut snapping: ResMut<Snapping>,
    mut query: Query<
        (
            Entity,
            &mut Velocity,
            &mut Speed,
            &MaxSpeed,
            Option<&RotationSpeed>,
            &mut Transform,
            Option<&Destination>,
        ),
        With<Player>,
    >,
    exploration_focused: Query<Entity, With<ExplorationFocused>>,
) where
    S: 'static + Clone + Debug + Eq + Hash + Send + Sync,
    A: Hash + Eq + Copy + Send + Sync,
{
    for (entity, mut velocity, mut speed, max_speed, rotation_speed, mut transform, destination) in
        query.iter_mut()
    {
        if **snapping {
            if let (Some(_), Some(rotate_left), Some(rotate_right)) = (
                rotation_speed,
                config.action_rotate_left,
                config.action_rotate_right,
            ) {
                if input.active(rotate_left) {
                    continue;
                }
                if input.active(rotate_right) {
                    continue;
                }
            }
            **snapping = false;
            continue;
        } else {
            let sprinting = if let Some(action) = config.action_sprint {
                input.active(action)
            } else {
                false
            };
            if sprinting {
                commands.entity(entity).insert(Sprinting::default());
            } else {
                commands.entity(entity).remove::<Sprinting>();
            }
            let mut direction = Vec2::default();
            if let Some(action) = config.action_forward {
                if input.active(action) {
                    direction.x += 1.;
                }
            }
            if let Some(action) = config.action_backward {
                if input.active(action) {
                    direction.x -= 1.;
                }
            }
            if let Some(action) = config.action_left {
                if input.active(action) {
                    direction.y += 1.;
                }
            }
            if let Some(action) = config.action_right {
                if input.active(action) {
                    direction.y -= 1.;
                }
            }
            if let (Some(rotation_speed), Some(rotate_left), Some(rotate_right)) = (
                rotation_speed,
                config.action_rotate_left,
                config.action_rotate_right,
            ) {
                let delta = rotation_speed.radians() * time.delta_seconds();
                if input.active(rotate_left) {
                    transform.rotate(Quat::from_rotation_z(delta));
                }
                if input.active(rotate_right) {
                    transform.rotate(Quat::from_rotation_z(-delta));
                }
                if !input.active(rotate_left) && !input.active(rotate_right) {
                    velocity.angvel = 0.;
                }
            } else {
                velocity.angvel = 0.;
            }
            if direction.length_squared() != 0. {
                commands.entity(entity).remove::<Destination>();
                commands.entity(entity).remove::<Exploring>();
                for entity in exploration_focused.iter() {
                    commands.entity(entity).remove::<ExplorationFocused>();
                }
                direction = direction.normalize();
                if direction.x > 0. {
                    direction.x *= config.forward_movement_factor;
                } else if direction.x < 0. {
                    direction.x *= config.backward_movement_factor;
                }
                if direction.y != 0. {
                    direction.y *= config.strafe_movement_factor;
                }
                let strength = if let (Some(forward), Some(backward), Some(left), Some(right)) = (
                    config.action_forward,
                    config.action_backward,
                    config.action_left,
                    config.action_right,
                ) {
                    let forward_x = input.strength(forward).abs();
                    let backward_x = input.strength(backward).abs();
                    let x = if forward_x > backward_x {
                        forward_x
                    } else {
                        backward_x
                    };
                    let right_y = input.strength(right).abs();
                    let left_y = input.strength(left).abs();
                    let y = if right_y > left_y { right_y } else { left_y };
                    Some(Vec2::new(x, y))
                } else {
                    None
                };
                let s = if sprinting {
                    **max_speed
                } else {
                    **max_speed / config.sprint_movement_factor
                };
                **speed = s;
                if let Some(strength) = strength {
                    direction *= strength;
                }
                let mut v = direction * **speed;
                v = transform
                    .compute_matrix()
                    .transform_vector3(v.extend(0.))
                    .truncate();
                velocity.linvel = v;
            } else if destination.is_none() {
                velocity.linvel = Vec2::ZERO;
                **speed = 0.;
            } else if sprinting {
                **speed = **max_speed;
            } else {
                velocity.linvel = Vec2::ZERO;
                **speed = **max_speed / 3.;
            }
        }
    }
}

fn snap<S, A: 'static>(
    mut tts: ResMut<Tts>,
    input: Res<InputMap<A>>,
    config: Res<NavigationConfig<S, A>>,
    mut snapping: ResMut<Snapping>,
    mut query: Query<(&mut Transform, &mut Velocity, &CardinalDirection), With<Player>>,
) -> Result<(), Box<dyn Error>>
where
    S: 'static + Copy + Debug + Eq + Hash + Send + Sync,
    A: Hash + Eq + Clone + Send + Sync,
{
    if let Some(action) = config.action_snap_left.clone() {
        if input.just_active(action) {
            for (mut position, mut velocity, direction) in query.iter_mut() {
                **snapping = true;
                position.rotation = Quat::from_rotation_z(match direction {
                    CardinalDirection::North => PI,
                    CardinalDirection::East => PI / 2.,
                    CardinalDirection::South => 0.,
                    CardinalDirection::West => PI * 1.5,
                });
                velocity.angvel = 0.;
            }
        }
    }
    if let Some(action) = config.action_snap_right.clone() {
        if input.just_active(action) {
            for (mut transform, mut velocity, direction) in query.iter_mut() {
                **snapping = true;
                transform.rotation = Quat::from_rotation_z(match direction {
                    CardinalDirection::North => 0.,
                    CardinalDirection::East => PI * 1.5,
                    CardinalDirection::South => PI,
                    CardinalDirection::West => PI / 2.,
                });
                velocity.angvel = 0.;
            }
        }
    }
    if let Some(action) = config.action_snap_cardinal.clone() {
        if input.just_active(action) {
            for (mut transform, mut velocity, direction) in query.iter_mut() {
                **snapping = true;
                let yaw: Angle = direction.into();
                let yaw = yaw.radians();
                transform.rotation = Quat::from_rotation_z(yaw);
                velocity.angvel = 0.;
                tts.speak(direction.to_string(), true)?;
            }
        }
    }
    if let Some(action) = config.action_snap_reverse.clone() {
        if input.just_active(action) {
            for (mut transform, mut velocity, _) in query.iter_mut() {
                **snapping = true;
                transform.rotate(Quat::from_rotation_z(PI));
                velocity.angvel = 0.;
            }
        }
    }
    Ok(())
}

fn update_direction(
    mut commands: Commands,
    mut query: Query<
        (Entity, &Transform, Option<&mut CardinalDirection>),
        (With<Player>, Changed<Transform>),
    >,
) {
    for (entity, transform, direction) in query.iter_mut() {
        let forward = transform.local_x();
        let yaw = Angle::Radians(forward.y.atan2(forward.x));
        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>) {
    for entity in removed.iter() {
        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 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;
        }
    }
}

#[derive(Clone, Debug)]
pub struct NavigationConfig<S, A> {
    pub action_backward: Option<A>,
    pub action_forward: Option<A>,
    pub action_left: Option<A>,
    pub action_right: Option<A>,
    pub action_rotate_left: Option<A>,
    pub action_rotate_right: Option<A>,
    pub action_snap_left: Option<A>,
    pub action_snap_right: Option<A>,
    pub action_snap_reverse: Option<A>,
    pub action_snap_cardinal: Option<A>,
    pub action_sprint: Option<A>,
    pub forward_movement_factor: f32,
    pub backward_movement_factor: f32,
    pub strafe_movement_factor: f32,
    pub sprint_movement_factor: f32,
    pub movement_control_states: Vec<S>,
}

impl<S, A> Default for NavigationConfig<S, A> {
    fn default() -> Self {
        Self {
            action_backward: None,
            action_forward: None,
            action_left: None,
            action_right: None,
            action_rotate_left: None,
            action_rotate_right: None,
            action_snap_left: None,
            action_snap_right: None,
            action_snap_reverse: None,
            action_snap_cardinal: None,
            action_sprint: None,
            forward_movement_factor: 1.,
            backward_movement_factor: 1.,
            strafe_movement_factor: 1.,
            sprint_movement_factor: 3.,
            movement_control_states: vec![],
        }
    }
}

pub struct NavigationPlugin<'a, S, A>(PhantomData<&'a S>, PhantomData<&'a A>);

impl<'a, S, A> Default for NavigationPlugin<'a, S, A> {
    fn default() -> Self {
        Self(PhantomData, PhantomData)
    }
}

impl<S, A> Plugin for NavigationPlugin<'static, S, A>
where
    S: 'static + Clone + Copy + Debug + Eq + Hash + Send + Sync,
    A: Hash + Eq + Copy + Send + Sync,
{
    fn build(&self, app: &mut App) {
        if !app.world.contains_resource::<NavigationConfig<S, A>>() {
            app.insert_resource(NavigationConfig::<S, A>::default());
        }
        let config = app
            .world
            .get_resource::<NavigationConfig<S, A>>()
            .unwrap()
            .clone();
        const SNAP: &str = "SNAP";
        app.init_resource::<Snapping>()
            .register_type::<MaxSpeed>()
            .register_type::<RotationSpeed>()
            .register_type::<Sprinting>()
            .add_system(update_direction.before(SNAP))
            .add_system_to_stage(CoreStage::PostUpdate, remove_direction)
            .add_system(speak_direction.chain(error_handler))
            .add_system(add_speed)
            .add_system_to_stage(CoreStage::PostUpdate, remove_speed);
        const MOVEMENT_CONTROLS: &str = "MOVEMENT_CONTROLS";
        if config.movement_control_states.is_empty() {
            app.add_system(movement_controls::<S, A>.label(MOVEMENT_CONTROLS))
                .add_system(
                    snap::<S, A>
                        .chain(error_handler)
                        .label(SNAP)
                        .before(MOVEMENT_CONTROLS),
                );
        } else {
            let states = config.movement_control_states;
            for state in states {
                app.add_system_set(
                    SystemSet::on_update(state)
                        .with_system(movement_controls::<S, A>.label(MOVEMENT_CONTROLS))
                        .with_system(
                            snap::<S, A>
                                .chain(error_handler)
                                .label(SNAP)
                                .before(MOVEMENT_CONTROLS),
                        ),
                );
            }
        }
    }
}