round5 0.1.2

pub fn pack(packed: &mut [u8], m: &[u16], els: usize, nr_bits: u8) -> usize {
    let mask = (1 << nr_bits as u16) - 1;
    if nr_bits == 8 {
        for i in 0usize..els {
            packed[i] = m[i] as u8;
        }
    } else {
        let mut idx: usize;
        let mut bits_done = 0usize;
        let mut val: u16;
        let mut bit_idx: usize;
        for m_el in m.iter().take(els) {
            idx = bits_done >> 3;
            bit_idx = bits_done & 7;
            val = *m_el & mask;
            packed[idx] = (packed[idx] as u16 | (val << bit_idx)) as u8;
            if bit_idx + nr_bits as usize > 8 {
                // Spill over to next packed byte
                packed[idx + 1] = (packed[idx + 1] as u16| (val >> (8 - bit_idx))) as u8;
                if bit_idx + nr_bits as usize > 16 {
                    // Spill over to next packed byte
                    packed[idx + 2] = (packed[idx + 2] as u16 | (val >> (16 - bit_idx))) as u8;
                }
            }
            bits_done += nr_bits as usize;
        }
    }
    bits_to_bytes!(els * nr_bits as usize) as usize
}

fn unpack(m: &mut [u16], packed: &[u8], els: usize, nr_bits: u8) -> usize {
    let mut val: u16;
    let mut bits_done: usize = 0;
    let mut idx: usize;
    let mut bit_idx: usize;
    let bitmask = (1 << nr_bits as u16) - 1;

    if nr_bits == 8 {
        for i in 0usize..els {
            m[i] = packed[i] as u16;
        }
    } else {
        for el_m in m.iter_mut().take(els) {
            idx = bits_done >> 3;
            bit_idx = bits_done & 7;
            val = (packed[idx] >> bit_idx as u8) as u16;
            if bit_idx + nr_bits as usize > 8 {
                /* Get spill over from next packed byte */
                val |= (packed[idx + 1] as u16) << (8 - bit_idx);
                if bit_idx + nr_bits as usize > 16 {
                    /* Get spill over from next packed byte */
                    val |= (packed[idx + 2] << (16 - bit_idx)) as u16;
                }
            }
            *el_m = val & bitmask as u16;
            bits_done += nr_bits as usize;
        }
    }
    bits_to_bytes!(els * nr_bits as usize) as usize
}

pub fn pack_pk(pk: &mut [u8], sigma: &[u8], b: &[u16], elements: usize, nr_bits: u8) {
    pk[..sigma.len()].copy_from_slice(sigma);
    pack(&mut pk[sigma.len()..], b, elements, nr_bits);
}

pub fn unpack_pk(sigma: &mut [u8], b: &mut [u16], packed_pk: &[u8], sigma_len: usize, elements: usize, nr_bits: u8) {
    sigma.copy_from_slice(&packed_pk[..sigma_len]);
    unpack(b, &packed_pk[sigma_len..], elements, nr_bits);   
}

pub fn pack_ct(packed_ct: &mut [u8], u_t: &[u16], u_bits: u8, v: &[u16], v_bits: u8) {
    let idx = pack(packed_ct, u_t, u_t.len(), u_bits);
    pack(&mut packed_ct[idx..], v, v.len(), v_bits);
}

pub fn unpack_ct(u_t: &mut [u16], v: &mut [u16], packed_ct: &[u8], u_bits: usize, v_bits: usize) {
    let idx = unpack(u_t, packed_ct, u_t.len(), u_bits as u8);
    unpack(v, &packed_ct[idx..], v.len(), v_bits as u8);
}