blackout/src/navigation.rs

use std::{collections::HashMap, error::Error, fmt::Debug, hash::Hash, marker::PhantomData};

use bevy::prelude::*;
use bevy_input_actionmap::InputMap;
use bevy_rapier2d::{na::UnitComplex, prelude::*};
use bevy_tts::Tts;
use derive_more::{Deref, DerefMut};

use crate::{
    core::{Angle, CardinalDirection, Coordinates, Player, PointLike},
    error::error_handler,
    exploration::{ExplorationFocused, Exploring},
    map::{ITileType, Map},
    pathfinding::Destination,
};

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

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

#[derive(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(Clone, Copy, Debug, Default, Deref, DerefMut, Reflect)]
#[reflect(Component)]
pub struct Speed(pub f32);

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

fn add_map_colliders(mut commands: Commands, maps: Query<(Entity, &Map), Added<Map>>) {
    for (map_entity, map) in maps.iter() {
        let rigid_body_entity = commands
            .entity(map_entity)
            .insert_bundle(RigidBodyBundle {
                body_type: RigidBodyType::Static,
                ..Default::default()
            })
            .id();
        for x in 0..map.width() {
            for y in 0..map.height() {
                let tile = map.base.at(x, y);
                if tile.blocks_motion() {
                    let collider = ColliderBundle {
                        shape: ColliderShape::cuboid(1., 1.),
                        ..Default::default()
                    };
                    let collider_parent = ColliderParent {
                        handle: rigid_body_entity.handle(),
                        pos_wrt_parent: Vec2::new(x as f32 + 0.5, y as f32 + 0.5).into(),
                    };
                    commands
                        .spawn()
                        .insert_bundle(collider)
                        .insert(collider_parent);
                }
            }
        }
    }
}

fn movement_controls<S, A: 'static>(
    mut commands: Commands,
    config: Res<NavigationConfig<S, A>>,
    input: Res<InputMap<A>>,
    time: Res<Time>,
    mut query: Query<
        (
            Entity,
            &mut RigidBodyVelocity,
            &mut Speed,
            &MaxSpeed,
            Option<&RotationSpeed>,
            &mut RigidBodyPosition,
            Option<&Destination>,
        ),
        With<Player>,
    >,
    exploration_focused: Query<(Entity, &ExplorationFocused)>,
) where
    S: bevy::ecs::component::Component + Clone + Debug + Eq + Hash,
    A: Hash + Eq + Clone + Send + Sync,
{
    for (entity, mut velocity, mut speed, max_speed, rotation_speed, mut position, destination) in
        query.iter_mut()
    {
        let sprinting = if let Some(action) = config.action_sprint.clone() {
            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.clone() {
            if input.active(action) {
                direction.x += 1.;
            }
        }
        if let Some(action) = config.action_backward.clone() {
            if input.active(action) {
                direction.x -= 1.;
            }
        }
        if let Some(action) = config.action_left.clone() {
            if input.active(action) {
                direction.y += 1.;
            }
        }
        if let Some(action) = config.action_right.clone() {
            if input.active(action) {
                direction.y -= 1.;
            }
        }
        if let (Some(rotation_speed), Some(rotate_left), Some(rotate_right)) = (
            rotation_speed,
            config.action_rotate_left.clone(),
            config.action_rotate_right.clone(),
        ) {
            let delta = rotation_speed.radians() * time.delta_seconds();
            if input.active(rotate_left) {
                position.position.rotation *= UnitComplex::new(delta);
            }
            if input.active(rotate_right) {
                position.position.rotation *= UnitComplex::new(-delta);
            }
        }
        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.clone(),
                config.action_backward.clone(),
                config.action_left.clone(),
                config.action_right.clone(),
            ) {
                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;
            }
            //direction = transform.compute_matrix().transform_vector3(direction);
            let mut v: Vector<Real> = (direction * **speed).into();
            v = position.position.rotation.transform_vector(&v);
            velocity.linvel = v;
            //velocity.linvel = position.position.rotation.transform_vector(&velocity.linvel);
        } else if destination.is_none() {
            velocity.linvel = Vec2::ZERO.into();
            **speed = 0.;
        } else if sprinting {
            **speed = **max_speed;
        } else {
            **speed = **max_speed / 3.;
        }
    }
}

fn speak_direction(
    mut tts: ResMut<Tts>,
    mut cache: Local<HashMap<Entity, CardinalDirection>>,
    player: Query<(Entity, &Player, &Transform), Changed<Transform>>,
) -> Result<(), Box<dyn Error>> {
    if let Ok((entity, _, transform)) = player.single() {
        let forward = transform.local_x();
        let yaw = Angle::Radians(forward.y.atan2(forward.x));
        if let Some(old_direction) = cache.get(&entity) {
            let old_direction = *old_direction;
            let direction: CardinalDirection = yaw.into();
            if old_direction != direction {
                let direction: String = direction.into();
                tts.speak(direction, true)?;
            }
            cache.insert(entity, direction);
        } else {
            cache.insert(entity, yaw.into());
        }
    }
    Ok(())
}

#[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_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_states: Vec<S>,
    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_sprint: None,
            forward_movement_factor: 1.,
            backward_movement_factor: 1.,
            strafe_movement_factor: 1.,
            sprint_movement_factor: 3.,
            movement_states: vec![],
            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<'a, S, A> Plugin for NavigationPlugin<'a, S, A>
where
    S: bevy::ecs::component::Component + Clone + Debug + Eq + Hash,
    A: Hash + Eq + Clone + Send + Sync,
    'a: 'static,
{
    fn build(&self, app: &mut AppBuilder) {
        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();
        app.register_type::<MaxSpeed>()
            .register_type::<RotationSpeed>()
            .register_type::<Sprinting>()
            .add_system(add_map_colliders.system())
            .add_system(speak_direction.system().chain(error_handler.system()));
        if config.movement_states.is_empty() {
        } else {
            let states = config.movement_states;
            for state in states {}
        }
        if config.movement_control_states.is_empty() {
            app.add_system(movement_controls::<S, A>.system());
        } 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>.system()),
                );
            }
        }
    }
}