codecraft 0.1.2

//! What a game sends *to* a pad: rumble, adaptive triggers, lightbar.
//! Layout follows the Linux `hid-playstation` driver plus community-documented trigger effects.

/// A shape for a trigger to hold. Positions are tenths of travel (0 to 9), strengths 1 to 8; out-of-range values are clamped.
#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
pub enum Trigger {
    /// A plain trigger.
    #[default]
    Free,
    /// Pushes back from `from` onwards, the whole way down.
    Resist { from: u8, strength: u8 },
    /// Resists from `from` and gives way at `to`, with nothing behind it.
    Weapon { from: u8, to: u8, strength: u8 },
    /// Pushes back harder from `from` up to `peak` at `to`, and not at all past `to`.
    Spring { from: u8, to: u8, peak: u8 },
    /// Shakes from `from` onwards at roughly `frequency` hertz.
    Vibrate {
        from: u8,
        strength: u8,
        frequency: u8,
    },
}

/// Everything a game can tell a pad in one go; sent whole every frame. `Default` is a pad doing nothing.
#[derive(Clone, Copy, Debug, PartialEq, Default)]
pub struct Feedback {
    /// The heavy actuator, 0 to 1.
    pub strong: f32,
    /// The light actuator, 0 to 1.
    pub weak: f32,
    pub left: Trigger,
    pub right: Trigger,
    /// Lightbar RGB; `None` leaves it to the pad.
    pub lightbar: Option<[u8; 3]>,
    /// Player number, 1 to 5, shown on the touchpad lights.
    pub player: Option<u8>,
    /// Speaker volume, 0 to 1; `None` leaves it unchanged.
    pub speaker: Option<f32>,
}

/// How a pad is plugged in, which decides the shape of the report.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Bus {
    Usb,
    Bluetooth,
}

const DUALSENSE_USB: usize = 48;
const DUALSENSE_BT: usize = 78;
const USB_BODY: usize = 1;
const BT_BODY: usize = 3;

/// Seed the pad mixes into every Bluetooth CRC.
const CRC_SEED: u8 = 0xA2;

/// The DualSense output report as bytes for `HidDevice::write`; `seq` (four bits) only matters over Bluetooth.
pub fn dualsense(feedback: &Feedback, bus: Bus, seq: u8) -> Vec<u8> {
    let (mut report, body) = match bus {
        Bus::Usb => {
            let mut report = vec![0u8; DUALSENSE_USB];
            report[0] = 0x02;
            (report, USB_BODY)
        }
        Bus::Bluetooth => {
            let mut report = vec![0u8; DUALSENSE_BT];
            report[0] = 0x31;
            report[1] = (seq & 0x0F) << 4;
            report[2] = 0x10;
            (report, BT_BODY)
        }
    };
    fill_dualsense(&mut report[body..], feedback);
    if bus == Bus::Bluetooth {
        let over = DUALSENSE_BT - 4;
        let crc = crc32(std::iter::once(CRC_SEED).chain(report[..over].iter().copied()));
        report[over..].copy_from_slice(&crc.to_le_bytes());
    }
    report
}

/// The 47-byte body both transports share.
fn fill_dualsense(body: &mut [u8], feedback: &Feedback) {
    // Old firmware reads the rumble flag in byte 0, new firmware in byte 38; set both.
    body[0] |= 0x01 | 0x02;
    body[38] |= 0x04;
    body[2] = level(feedback.weak);
    body[3] = level(feedback.strong);

    body[0] |= 0x04 | 0x08;
    trigger(&mut body[10..21], feedback.right);
    trigger(&mut body[21..32], feedback.left);

    if let Some(speaker) = feedback.speaker {
        body[0] |= 0x20;
        body[5] = level(speaker);
    }
    if let Some([r, g, b]) = feedback.lightbar {
        body[1] |= 0x04;
        body[44] = r;
        body[45] = g;
        body[46] = b;
    }
    if let Some(player) = feedback.player {
        body[1] |= 0x10;
        body[43] = player_leds(player);
    }
}

/// The DualShock 4's USB report: motors and lightbar only.
pub fn dualshock4(feedback: &Feedback) -> Vec<u8> {
    let mut report = vec![0u8; 32];
    report[0] = 0x05;
    report[1] = 0x07;
    report[4] = level(feedback.weak);
    report[5] = level(feedback.strong);
    if let Some([r, g, b]) = feedback.lightbar {
        report[6] = r;
        report[7] = g;
        report[8] = b;
    }
    report
}

fn level(value: f32) -> u8 {
    (value.clamp(0.0, 1.0) * 255.0).round() as u8
}

/// Touchpad lights lit outwards from the middle, as the console does.
fn player_leds(player: u8) -> u8 {
    match player {
        1 => 0b00100,
        2 => 0b01010,
        3 => 0b10101,
        4 => 0b11011,
        _ => 0b11111,
    }
}

/// One trigger's eleven bytes: a mode, then a 16-bit zone mask and 3 bits of strength per zone.
fn trigger(bytes: &mut [u8], effect: Trigger) {
    bytes.fill(0);
    match effect {
        Trigger::Free => bytes[0] = 0x05,
        Trigger::Resist { from, strength } => {
            bytes[0] = 0x21;
            let (active, force) = zones(from, 9, |_| strength);
            bytes[1..3].copy_from_slice(&active.to_le_bytes());
            bytes[3..7].copy_from_slice(&force.to_le_bytes());
        }
        Trigger::Spring { from, to, peak } => {
            // Resistance mode with climbing strengths; zones past `to` are left out so the pad reads nothing there.
            bytes[0] = 0x21;
            let peak = peak.clamp(1, 8) as f32;
            let (active, force) =
                zones(from, to, |along| (1.0 + (peak - 1.0) * along).round() as u8);
            bytes[1..3].copy_from_slice(&active.to_le_bytes());
            bytes[3..7].copy_from_slice(&force.to_le_bytes());
        }
        Trigger::Weapon { from, to, strength } => {
            bytes[0] = 0x25;
            let from = from.clamp(2, 7);
            let to = to.clamp(from + 1, 8);
            let ends = (1u16 << from) | (1u16 << to);
            bytes[1..3].copy_from_slice(&ends.to_le_bytes());
            bytes[3] = strength.clamp(1, 8) - 1;
        }
        Trigger::Vibrate {
            from,
            strength,
            frequency,
        } => {
            bytes[0] = 0x26;
            let (active, amplitude) = zones(from, 9, |_| strength);
            bytes[1..3].copy_from_slice(&active.to_le_bytes());
            bytes[3..7].copy_from_slice(&amplitude.to_le_bytes());
            bytes[9] = frequency;
        }
    }
}

/// Zone mask and packed strengths for `from..=to`, with `strength` given the fraction along; strength 1..=8 is sent as 0..=7.
fn zones(from: u8, to: u8, strength: impl Fn(f32) -> u8) -> (u16, u32) {
    let from = from.min(9);
    let to = to.clamp(from, 9);
    let mut active = 0u16;
    let mut packed = 0u32;
    for zone in from..=to {
        let along = match to > from {
            true => (zone - from) as f32 / (to - from) as f32,
            false => 1.0,
        };
        active |= 1 << zone;
        packed |= ((strength(along).clamp(1, 8) - 1) as u32) << (3 * zone);
    }
    (active, packed)
}

/// The ordinary CRC-32 (as in zip), which the pad checks Bluetooth reports against.
pub fn crc32(bytes: impl IntoIterator<Item = u8>) -> u32 {
    let mut crc = 0xFFFF_FFFFu32;
    for byte in bytes {
        crc ^= byte as u32;
        for _ in 0..8 {
            let low = crc & 1;
            crc >>= 1;
            if low != 0 {
                crc ^= 0xEDB8_8320;
            }
        }
    }
    !crc
}

#[cfg(test)]
mod tests {
    use super::*;

    fn body(report: &[u8], bus: Bus) -> &[u8] {
        match bus {
            Bus::Usb => &report[USB_BODY..USB_BODY + 47],
            Bus::Bluetooth => &report[BT_BODY..BT_BODY + 47],
        }
    }

    #[test]
    fn the_crc_is_the_ordinary_one() {
        assert_eq!(crc32(b"123456789".iter().copied()), 0xCBF4_3926);
        assert_eq!(crc32(std::iter::empty()), 0);
    }

    #[test]
    fn the_two_transports_carry_the_same_body() {
        let feedback = Feedback {
            strong: 1.0,
            weak: 0.5,
            lightbar: Some([10, 20, 30]),
            player: Some(2),
            right: Trigger::Weapon {
                from: 2,
                to: 6,
                strength: 8,
            },
            ..Feedback::default()
        };
        let usb = dualsense(&feedback, Bus::Usb, 0);
        let bt = dualsense(&feedback, Bus::Bluetooth, 5);

        assert_eq!(usb.len(), 48);
        assert_eq!(usb[0], 0x02);
        assert_eq!(bt.len(), 78);
        assert_eq!(bt[0], 0x31);
        assert_eq!(bt[1], 5 << 4, "the sequence number sits in the high bits");
        assert_eq!(bt[2], 0x10);
        assert_eq!(body(&usb, Bus::Usb), body(&bt, Bus::Bluetooth));

        let over = bt.len() - 4;
        let crc = crc32(std::iter::once(CRC_SEED).chain(bt[..over].iter().copied()));
        assert_eq!(
            &bt[over..],
            &crc.to_le_bytes(),
            "signed over the seed and the rest"
        );
    }

    #[test]
    fn rumble_is_set_for_old_and_new_firmware_alike() {
        let report = dualsense(
            &Feedback {
                strong: 1.0,
                weak: 0.25,
                ..Feedback::default()
            },
            Bus::Usb,
            0,
        );
        let body = body(&report, Bus::Usb);
        assert_eq!(
            body[0] & 0x03,
            0x03,
            "compatible vibration, and haptics selected"
        );
        assert_eq!(body[38] & 0x04, 0x04, "and the newer flag");
        assert_eq!(body[3], 255, "the strong actuator is the 'left motor'");
        assert_eq!(body[2], 64, "and the weak one the 'right'");
    }

    #[test]
    fn a_default_feedback_is_a_pad_at_rest() {
        let report = dualsense(&Feedback::default(), Bus::Usb, 0);
        let body = body(&report, Bus::Usb);
        assert_eq!(body[2], 0);
        assert_eq!(body[3], 0);
        assert_eq!(body[1], 0, "no lightbar, no player lights");
        assert_eq!(body[10], 0x05, "the triggers are told to be free");
        assert_eq!(body[21], 0x05);
        assert!(body[11..21].iter().all(|b| *b == 0));
    }

    #[test]
    fn a_resisting_trigger_covers_every_zone_from_its_start() {
        let mut bytes = [0xFFu8; 11];
        trigger(
            &mut bytes,
            Trigger::Resist {
                from: 3,
                strength: 8,
            },
        );
        assert_eq!(bytes[0], 0x21);
        let active = u16::from_le_bytes([bytes[1], bytes[2]]);
        assert_eq!(active, 0b11_1111_1000, "zones three to nine");
        let force = u32::from_le_bytes([bytes[3], bytes[4], bytes[5], bytes[6]]);
        for zone in 0..10u32 {
            let expected = if zone >= 3 { 7 } else { 0 };
            assert_eq!((force >> (3 * zone)) & 7, expected, "zone {zone}");
        }
        assert!(bytes[7..].iter().all(|b| *b == 0), "and nothing after");
    }

    #[test]
    fn a_spring_climbs_to_its_peak_and_is_gone_past_it() {
        let mut bytes = [0xFFu8; 11];
        trigger(
            &mut bytes,
            Trigger::Spring {
                from: 0,
                to: 4,
                peak: 8,
            },
        );
        assert_eq!(bytes[0], 0x21, "a strength per zone, like a resistance");
        let active = u16::from_le_bytes([bytes[1], bytes[2]]);
        assert_eq!(active, 0b1_1111, "zones nought to four, and none past");
        let force = u32::from_le_bytes([bytes[3], bytes[4], bytes[5], bytes[6]]);
        let strengths: Vec<u32> = (0..10).map(|zone| (force >> (3 * zone)) & 7).collect();
        assert_eq!(
            strengths,
            [0, 2, 4, 5, 7, 0, 0, 0, 0, 0],
            "one, three, five, six, eight, sent as nought to seven",
        );
        assert!(bytes[7..].iter().all(|b| *b == 0), "and nothing after");
    }

    #[test]
    fn a_spring_of_one_zone_is_its_peak() {
        let mut bytes = [0u8; 11];
        trigger(
            &mut bytes,
            Trigger::Spring {
                from: 5,
                to: 5,
                peak: 40,
            },
        );
        assert_eq!(u16::from_le_bytes([bytes[1], bytes[2]]), 1 << 5);
        let force = u32::from_le_bytes([bytes[3], bytes[4], bytes[5], bytes[6]]);
        assert_eq!(force, 7 << 15, "the one zone, at the most the pad does");
    }

    #[test]
    fn a_weapon_trigger_is_two_ends_and_a_strength() {
        let mut bytes = [0u8; 11];
        trigger(
            &mut bytes,
            Trigger::Weapon {
                from: 2,
                to: 5,
                strength: 6,
            },
        );
        assert_eq!(bytes[0], 0x25);
        assert_eq!(
            u16::from_le_bytes([bytes[1], bytes[2]]),
            (1 << 2) | (1 << 5)
        );
        assert_eq!(
            bytes[3], 5,
            "strength one to eight is sent as nought to seven"
        );
    }

    #[test]
    fn impossible_triggers_are_brought_within_reach() {
        let mut bytes = [0u8; 11];
        trigger(
            &mut bytes,
            Trigger::Weapon {
                from: 9,
                to: 1,
                strength: 40,
            },
        );
        assert_eq!(
            u16::from_le_bytes([bytes[1], bytes[2]]),
            (1 << 7) | (1 << 8)
        );
        assert_eq!(bytes[3], 7);

        trigger(
            &mut bytes,
            Trigger::Vibrate {
                from: 12,
                strength: 0,
                frequency: 40,
            },
        );
        assert_eq!(bytes[0], 0x26);
        assert_eq!(
            u16::from_le_bytes([bytes[1], bytes[2]]),
            1 << 9,
            "the last zone at least"
        );
        assert_eq!(bytes[9], 40);

        trigger(
            &mut bytes,
            Trigger::Spring {
                from: 7,
                to: 2,
                peak: 3,
            },
        );
        assert_eq!(u16::from_le_bytes([bytes[1], bytes[2]]), 1 << 7);
        let force = u32::from_le_bytes([bytes[3], bytes[4], bytes[5], bytes[6]]);
        assert_eq!(force, 2 << 21, "zone seven at three");
    }

    #[test]
    fn the_player_lights_count_outwards_from_the_middle() {
        assert_eq!(player_leds(1), 0b00100);
        assert_eq!(player_leds(2), 0b01010);
        assert_eq!(player_leds(3), 0b10101);
        let report = dualsense(
            &Feedback {
                player: Some(2),
                ..Feedback::default()
            },
            Bus::Usb,
            0,
        );
        let body = body(&report, Bus::Usb);
        assert_eq!(body[1] & 0x10, 0x10);
        assert_eq!(body[43], 0b01010);
    }

    #[test]
    fn levels_are_bytes_with_the_ends_held() {
        assert_eq!(level(-1.0), 0);
        assert_eq!(level(0.5), 128);
        assert_eq!(level(2.0), 255);
    }
}