gmqtt 0.1.1

use core::slice::Iter;

use crate::MqttError;

pub(crate) fn read_str<'a>(buf: &'a [u8], offset: &mut usize) -> Result<&'a str, MqttError> {
    core::str::from_utf8(read_bytes(buf, offset)?).map_err(MqttError::InvalidString)
}

pub(crate) fn read_bytes<'a>(buf: &'a [u8], offset: &mut usize) -> Result<&'a [u8], MqttError> {
    if buf[*offset..].len() < 2 {
        return Err(MqttError::InvalidLength);
    }

    let len = ((buf[*offset] as usize) << 8) | buf[*offset + 1] as usize;
    *offset += 2;
    if len > buf[*offset..].len() {
        Err(MqttError::InvalidLength)
    } else {
        let bytes = &buf[*offset..*offset + len];
        *offset += len;
        Ok(bytes)
    }
}

/// Check wether buffer has `len` bytes of write capacity left. Use this to return a clean
/// Result::MqttError instead of panicking.
pub(crate) fn check_remaining(
    buf: &mut [u8],
    offset: &mut usize,
    len: usize,
) -> Result<(), MqttError> {
    if buf[*offset..].len() < len {
        Err(MqttError::InsufficientBufferSize)
    } else {
        Ok(())
    }
}

/// http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718023
pub(crate) fn write_length(
    buf: &mut [u8],
    offset: &mut usize,
    len: usize,
) -> Result<usize, MqttError> {
    let write_len = match len {
        0..=127 => {
            check_remaining(buf, offset, len + 1)?;
            len + 1
        }
        128..=16383 => {
            check_remaining(buf, offset, len + 2)?;
            len + 2
        }
        16384..=2097151 => {
            check_remaining(buf, offset, len + 3)?;
            len + 3
        }
        2097152..=268435455 => {
            check_remaining(buf, offset, len + 4)?;
            len + 4
        }
        _ => return Err(MqttError::InvalidLength),
    };

    let mut done = false;
    let mut x = len;
    while !done {
        let mut byte = (x % 128) as u8;
        x /= 128;
        if x > 0 {
            byte |= 128;
        }
        write_u8(buf, offset, byte);
        done = x == 0;
    }

    Ok(write_len)
}

/// Writes remaining length and returns number of bytes for remaining length
pub(crate) fn write_remaining_length(
    buf: &mut [u8],
    offset: &mut usize,
    len: usize,
) -> Result<usize, MqttError> {
    if len > 268_435_455 {
        return Err(MqttError::PayloadTooLong);
    }

    let mut done = false;
    let mut x = len;
    let mut count = 0;

    while !done {
        let mut byte = (x % 128) as u8;
        x /= 128;
        if x > 0 {
            byte |= 128;
        }

        write_u8(buf, offset, byte);
        count += 1;
        done = x == 0;
    }

    Ok(count)
}

pub(crate) fn write_u8(buf: &mut [u8], offset: &mut usize, val: u8) {
    buf[*offset] = val;
    *offset += 1;
}

pub(crate) fn write_u16(buf: &mut [u8], offset: &mut usize, val: u16) {
    write_u8(buf, offset, (val >> 8) as u8);
    write_u8(buf, offset, (val & 0xFF) as u8);
}

/// Writes u8 buffer, preceeded by two bytes representing the size of written bytes
pub(crate) fn write_bytes_with_len(buf: &mut [u8], offset: &mut usize, bytes: &[u8]) {
    write_u16(buf, offset, bytes.len() as u16);

    for &byte in bytes {
        write_u8(buf, offset, byte);
    }
}
pub(crate) fn write_bytes(buf: &mut [u8], offset: &mut usize, bytes: &[u8]) {
    for &byte in bytes {
        write_u8(buf, offset, byte);
    }
}

pub(crate) fn read_u16(buf: &[u8], offset: usize) -> u16 {
    u16::from_be_bytes([buf[offset], buf[offset + 1]])
}

pub(crate) fn read_u32(buf: &[u8], offset: usize) -> u32 {
    u32::from_be_bytes([
        buf[offset],
        buf[offset + 1],
        buf[offset + 2],
        buf[offset + 3],
    ])
}

/// Return number of remaining length bytes required for encoding length
pub(crate) fn len_len(len: usize) -> usize {
    if len >= 2_097_152 {
        4
    } else if len >= 16_384 {
        3
    } else if len >= 128 {
        2
    } else {
        1
    }
}

/// Parses variable byte integer in the stream and returns the length
/// and number of bytes that make it. Used for remaining length calculation
/// as well as for calculating property lengths
pub(crate) fn varint_length(stream: Iter<u8>) -> Result<(usize, usize), MqttError> {
    let mut len: usize = 0;
    let mut len_len = 0;
    let mut done = false;
    let mut shift = 0;

    // Use continuation bit at position 7 to continue reading next
    // byte to frame 'length'.
    for byte in stream {
        len_len += 1;
        let byte = *byte as usize;
        len += (byte & 0x7F) << shift;

        // stop when continue bit is 0
        done = (byte & 0x80) == 0;
        if done {
            break;
        }

        shift += 7;

        // Only a max of 4 bytes allowed for remaining length
        // more than 4 shifts (0, 7, 14, 21) implies bad length
        if shift > 21 {
            return Err(MqttError::MalformedRemainingLength);
        }
    }

    // Not enough bytes to frame remaining length.
    if !done {
        return Err(MqttError::InsufficientBytes);
    }

    Ok((len_len, len))
}