blackout/src/pathfinding.rs

use std::collections::HashMap;

use bevy::{prelude::*, tasks::prelude::*};
use bevy_rapier2d::{na::UnitComplex, prelude::*};
use crossbeam_channel::{unbounded, Receiver};
use derive_more::{Deref, DerefMut};
use pathfinding::prelude::*;

use crate::{
    core::{Coordinates, PointLike},
    map::Map,
    navigation::{RotationSpeed, Speed},
};

#[derive(Clone, Copy, Debug, Default, Deref, DerefMut, Eq, Hash, PartialEq, Reflect)]
#[reflect(Component)]
pub struct Destination(pub (i32, i32));

impl_pointlike_for_tuple_component!(Destination);

#[derive(Clone, Debug, Default, Deref, DerefMut, Reflect)]
#[reflect(Component)]
pub struct Path(pub Vec<(i32, i32)>);

pub fn find_path(
    start: &dyn PointLike,
    destination: &dyn PointLike,
    map: &Map,
) -> Option<(Vec<(i32, i32)>, u32)> {
    astar(
        &start.into(),
        |p| {
            let mut successors: Vec<((i32, i32), u32)> = vec![];
            for tile in map.get_available_exits(p.0 as usize, p.1 as usize) {
                successors.push(((tile.0 as i32, tile.1 as i32), (tile.2 * 100.) as u32));
            }
            successors
        },
        |p| (p.distance_squared(destination) * 100.) as u32,
        |p| *p == destination.into(),
    )
}

fn calculate_path(
    mut commands: Commands,
    pool: Res<AsyncComputeTaskPool>,
    mut calculating: Local<HashMap<Entity, Receiver<Path>>>,
    query: Query<(Entity, &Destination, &Coordinates), Changed<Destination>>,
    destinations: Query<&Destination>,
    map: Query<&Map>,
) {
    let calculating_clone = calculating.clone();
    for (entity, rx) in calculating_clone.iter() {
        if destinations.get(*entity).is_ok() {
            if let Ok(path) = rx.try_recv() {
                commands.entity(*entity).insert(path);
                calculating.remove(&entity);
            }
        } else {
            calculating.remove(&entity);
        }
    }
    for (entity, destination, coordinates) in query.iter() {
        if !calculating.contains_key(&entity) {
            let (tx, rx) = unbounded();
            calculating.insert(entity, rx);
            for map in map.iter() {
                let start_clone = *coordinates;
                let destination_clone = *destination;
                let map_clone = map.clone();
                let tx_clone = tx.clone();
                pool.spawn(async move {
                    if let Some(result) = find_path(&start_clone, &destination_clone, &map_clone) {
                        tx_clone.send(Path(result.0)).expect("Channel should exist");
                    }
                })
                .detach();
            }
        }
    }
}

fn negotiate_path(
    mut commands: Commands,
    mut query: Query<(
        Entity,
        &mut Path,
        &mut RigidBodyPosition,
        &mut RigidBodyVelocity,
        &Speed,
        Option<&RotationSpeed>,
    )>,
) {
    for (entity, mut path, mut position, mut velocity, speed, rotation_speed) in query.iter_mut() {
        let mut new_path = path.0.clone();
        let start_i32 = (
            position.position.translation.x,
            position.position.translation.y,
        )
            .i32();
        let new_path_clone = new_path.clone();
        let mut iter = new_path_clone.split(|p| *p == start_i32);
        if iter.next().is_some() {
            if let Some(upcoming) = iter.next() {
                new_path = vec![start_i32];
                new_path.append(&mut upcoming.to_vec());
            }
        }
        **path = new_path;
        if path.len() >= 2 {
            let start = Vec2::new(
                position.position.translation.x,
                position.position.translation.y,
            );
            let next = path[1];
            let next = Vec2::new(next.0 as f32, next.1 as f32);
            if rotation_speed.is_some() {
                let v = next - start;
                let angle = v.y.atan2(v.x);
                position.position.rotation = UnitComplex::new(angle);
            }
            let mut direction = next - start;
            direction = direction.normalize();
            direction *= speed.0;
            velocity.linvel = direction.into();
        } else {
            commands.entity(entity).remove::<Path>();
            commands.entity(entity).remove::<Destination>();
            velocity.linvel = Vec2::ZERO.into();
        }
    }
}

pub struct PathfindingPlugin;

impl Plugin for PathfindingPlugin {
    fn build(&self, app: &mut AppBuilder) {
        app.add_system_to_stage(CoreStage::PostUpdate, calculate_path.system())
            .add_system(negotiate_path.system());
    }
}