use arrayvec::ArrayVec;
use bitvec::prelude::*;

pub(crate) fn interleave<const N: usize>(data: &ArrayVec<u8, N>) -> ArrayVec<u8, N> {
    let bv = data.view_bits::<Msb0>();

    let mut out: ArrayVec<u8, N> = ArrayVec::new();
    for _ in 0..data.len() {
        out.push(0);
    }
    let bv_out = out.view_bits_mut::<Msb0>();

    let mut out_i = 0;
    for i in 0..32 {
        for j in (0..bv.len()).step_by(32) {
            if i + j >= bv.len() {
                continue;
            }

            bv_out.set(out_i, bv[i + j]);

            out_i += 1;
        }
    }

    out
}

pub(crate) fn uninterleave<const N: usize>(data: &ArrayVec<u8, N>) -> ArrayVec<u8, N> {
    let bv = data.view_bits::<Msb0>();

    let mut out: ArrayVec<u8, N> = ArrayVec::new();
    for _ in 0..data.len() {
        out.push(0);
    }
    let bv_out = out.view_bits_mut::<Msb0>();

    let mut out_i = 0;
    for i in 0..32 {
        for j in (0..bv.len()).step_by(32) {
            if i + j >= bv.len() {
                continue;
            }

            bv_out.set(i + j, bv[out_i]);

            out_i += 1;
        }
    }

    out
}

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

    #[test]
    fn interleaver_works() {
        let orig =
            ArrayVec::try_from([0x84, 0x73, 0x12, 0xA3, 0xFF, 0x00, 0xC2, 0x1B, 0x77]).unwrap();
        let interleaved = interleave(&orig);

        let expected = [0xCD, 0xB5, 0xDB, 0x2A, 0x0A, 0x52, 0x0C, 0x89, 0x4F];
        assert_eq!(expected, interleaved[..]);

        let uninterleaved = uninterleave(&interleaved);

        assert_eq!(orig, uninterleaved);
    }
}