lemur/src/controller.rs

use std::{
    collections::HashMap,
    sync::{Arc, RwLock},
};

use gilrs::{Event as GamepadEvent, EventType, GamepadId, ev::Code};
use winit::{
    event::{ElementState, KeyEvent},
    keyboard::PhysicalKey,
};

use crate::{
    emulator::{EmulatorClient, EmulatorCommand, SimId, VBKey},
    input::{AxisMapping, InputMapping, MappingProvider},
};

pub struct Controller {
    pub sim_id: SimId,
    state: VBKey,
    axis_values: HashMap<(GamepadId, Code), f32>,
    mapping: Arc<RwLock<InputMapping>>,
}

impl Controller {
    pub fn new(sim_id: SimId, mappings: &MappingProvider) -> Self {
        Self {
            sim_id,
            state: VBKey::SGN,
            axis_values: HashMap::new(),
            mapping: mappings.for_sim(sim_id).clone(),
        }
    }

    pub fn key_event(&mut self, event: &KeyEvent) -> Option<VBKey> {
        let keys = self.map_keys(&event.physical_key)?;
        match event.state {
            ElementState::Pressed => self.update_state(keys, VBKey::empty()),
            ElementState::Released => self.update_state(VBKey::empty(), keys),
        }
    }

    pub fn gamepad_event(&mut self, event: &GamepadEvent) -> Option<VBKey> {
        let (pressed, released) = match event.event {
            EventType::ButtonPressed(_, code) => {
                let mappings = self.map_button(&event.id, &code)?;
                (mappings, VBKey::empty())
            }
            EventType::ButtonReleased(_, code) => {
                let mappings = self.map_button(&event.id, &code)?;
                (VBKey::empty(), mappings)
            }
            EventType::AxisChanged(_, value, code) => {
                let mapping = self.map_axis(&event.id, &code)?;
                self.axis_values.insert((event.id, code), value);

                let pair_value = mapping
                    .pair
                    .and_then(|p| self.axis_values.get(&(event.id, p)))
                    .copied()
                    .unwrap_or_default();
                let neg = mapping.neg;
                let pos = mapping.pos;

                let (mut pressed, mut released) = axis_presses(value, pair_value, neg, pos);
                if let Some(other) = mapping.pair.and_then(|p| self.map_axis(&event.id, &p)) {
                    let (other_pressed, other_released) =
                        axis_presses(pair_value, value, other.neg, other.pos);
                    pressed = pressed.union(other_pressed);
                    released = released.union(other_released);
                }
                (pressed, released)
            }
            _ => {
                return None;
            }
        };
        self.update_state(pressed, released)
    }

    pub fn low_battery(&mut self, pwr: bool) -> Option<VBKey> {
        let (pressed, released) = if pwr {
            (VBKey::PWR, VBKey::empty())
        } else {
            (VBKey::empty(), VBKey::PWR)
        };
        self.update_state(pressed, released)
    }

    fn update_state(&mut self, pressed: VBKey, released: VBKey) -> Option<VBKey> {
        let old_state = self.state;
        self.state = self.state.union(pressed).difference(released);
        if self.state != old_state {
            Some(self.state)
        } else {
            None
        }
    }

    fn map_keys(&self, key: &PhysicalKey) -> Option<VBKey> {
        self.mapping.read().unwrap().map_keyboard(key)
    }

    fn map_button(&self, id: &GamepadId, code: &Code) -> Option<VBKey> {
        self.mapping.read().unwrap().map_button(id, code)
    }

    fn map_axis(&self, id: &GamepadId, code: &Code) -> Option<AxisMapping> {
        self.mapping.read().unwrap().map_axis(id, code)
    }
}

pub struct ControllerManager {
    client: EmulatorClient,
    controllers: [Controller; 2],
}

impl ControllerManager {
    pub fn new(client: EmulatorClient, mappings: &MappingProvider) -> Self {
        Self {
            client,
            controllers: [
                Controller::new(SimId::Player1, mappings),
                Controller::new(SimId::Player2, mappings),
            ],
        }
    }

    pub fn handle_key_event(&mut self, event: &KeyEvent) {
        for controller in &mut self.controllers {
            if let Some(pressed) = controller.key_event(event) {
                self.client
                    .send_command(EmulatorCommand::SetKeys(controller.sim_id, pressed));
            }
        }
    }

    pub fn handle_gamepad_event(&mut self, event: &GamepadEvent) {
        for controller in &mut self.controllers {
            if let Some(pressed) = controller.gamepad_event(event) {
                self.client
                    .send_command(EmulatorCommand::SetKeys(controller.sim_id, pressed));
            }
        }
    }

    pub fn handle_low_battery(&mut self, sim_id: SimId, pwr: bool) {
        let controller = &mut self.controllers[sim_id.to_index()];
        if let Some(pressed) = controller.low_battery(pwr) {
            self.client
                .send_command(EmulatorCommand::SetKeys(controller.sim_id, pressed));
        }
    }
}

fn axis_presses(value: f32, pair_value: f32, neg: VBKey, pos: VBKey) -> (VBKey, VBKey) {
    use std::f32::consts::FRAC_PI_3;

    let mut pressed = VBKey::empty();
    let mut released = VBKey::empty();

    let magnitude = value.hypot(pair_value);
    let abs_angle = pair_value.atan2(value).abs();

    if magnitude < 0.65 {
        released = released.union(neg).union(pos);
    } else if abs_angle <= FRAC_PI_3 {
        // stick tilted towards positive
        released = released.union(neg);

        if magnitude >= 0.75 {
            pressed = pressed.union(pos);
        }
    } else if abs_angle >= 2.0 * FRAC_PI_3 {
        // stick tilted towards negative
        released = released.union(pos);

        if magnitude >= 0.75 {
            pressed = pressed.union(neg);
        }
    } else {
        released = released.union(neg).union(pos);
    }

    (pressed, released)
}

#[cfg(test)]
mod tests {
    use super::{VBKey, axis_presses};

    const NEG: VBKey = VBKey::LL;
    const POS: VBKey = VBKey::LR;

    const NONE: VBKey = VBKey::empty();
    const BOTH: VBKey = NEG.union(POS);

    #[test]
    fn detects_no_input() {
        let (pressed, released) = axis_presses(0.0, 0.0, NEG, POS);
        assert_eq!(pressed, NONE);
        assert_eq!(released, BOTH);
    }

    #[test]
    fn detects_pos_input() {
        let (pressed, released) = axis_presses(1.0, 0.0, NEG, POS);
        assert_eq!(pressed, POS);
        assert_eq!(released, NEG);
    }

    #[test]
    fn detects_neg_input() {
        let (pressed, released) = axis_presses(-1.0, 0.0, NEG, POS);
        assert_eq!(pressed, NEG);
        assert_eq!(released, POS);
    }

    #[test]
    fn respects_dead_zone() {
        let (pressed, released) = axis_presses(0.70, 0.0, NEG, POS);
        assert_eq!(pressed, NONE);
        assert_eq!(released, NEG);
    }

    #[test]
    fn handles_diagonals_ok() {
        let (pressed, released) = axis_presses(0.6, 0.6, NEG, POS);
        assert_eq!(pressed, POS);
        assert_eq!(released, NEG);
    }

    #[test]
    fn ignores_diagonal_noise() {
        let (pressed, released) = axis_presses(-1.0, -0.1484245, NEG, POS);
        assert_eq!(pressed, NEG);
        assert_eq!(released, POS);
    }

    #[test]
    fn ignores_small_values() {
        let (pressed, released) = axis_presses(-0.1484245, -1.0, NEG, POS);
        assert_eq!(pressed, NONE);
        assert_eq!(released, BOTH);
    }
}