improv-setup/src/serial.rs

use anyhow::{
    bail,
    Context,
    Result,
};
use log::{
    debug,
    log_enabled,
    Level,
};

use crate::improv::{
    IMPROV_HEADER,
    RawPacket,
    ImprovDataToPacket,
};
use crate::utils::{
    to_ascii_debug,
};

pub fn find_begin_of_improv_packet(buffer: &[u8]) -> Result<usize> {
    let mut improv_header_char: usize = 0;

    for (i, b) in buffer.iter().enumerate() {
        if b == &IMPROV_HEADER[improv_header_char] {
            improv_header_char += 1;

            if improv_header_char >= IMPROV_HEADER.len() {
                return Ok(i - (IMPROV_HEADER.len() - 1));
            }
        }
        else {
            improv_header_char = 0;
            if b == &IMPROV_HEADER[improv_header_char] {
                improv_header_char += 1;
            }
        }
    }

    bail!("Improv header not found");
}

pub struct PacketBounds {
    begin: usize,
    end: usize,
}

pub fn find_packet(buffer: &[u8]) -> Result<PacketBounds> {
        let improv_packet_offset = find_begin_of_improv_packet(buffer)
            .context("Failed to find improv header in received bytes from serial device")?;

        let improv_packet_len_position = improv_packet_offset + 8;
        if buffer.len() <= improv_packet_len_position {
            bail!("Byte that contains length of improv packet not available in buffer");
        }

        let improv_packet_end = improv_packet_offset + 10 + <u8 as Into<usize>>::into(buffer[improv_packet_len_position]);
        if buffer.len() <= improv_packet_end {
            bail!("Buffer stops before improv packet ends");
        }

        Ok(PacketBounds {
            begin: improv_packet_offset,
            end: improv_packet_end,
        })
}

pub struct SerialInterface {
    interface: Box<dyn tokio_serial::SerialPort>,
    buffer: Vec<u8>,
}

impl SerialInterface {

    pub fn new(path: &str, baud_rate: u32) -> Result<Self> {
        let interface = tokio_serial::new(path, baud_rate).open().context("Failed to open serial device")?;

        return Ok(Self {
            interface: interface,
            buffer: Vec::new(),
        });
    }

    pub fn send_bytes(&mut self, packet_bytes: &[u8]) -> Result<()> {
        if log_enabled!(Level::Debug) {
            debug!("Sending packet: \n{}\n{}", hex::encode(packet_bytes), to_ascii_debug(packet_bytes));
        }
        self.interface.write(packet_bytes).context("Unable to write bytes to serial interface")?;

        Ok(())
    }

    pub fn send_raw_packet(&mut self, raw_packet: RawPacket) -> Result<()> {
        self.send_bytes(&raw_packet.to_bytes())?;

        Ok(())
    }

    pub fn send(&mut self, packet: &impl ImprovDataToPacket) -> Result<()> {
        self.send_raw_packet(packet.to_raw_packet())?;

        Ok(())
    }

    pub fn fill_buffer(&mut self) -> Result<()> {
        let available_bytes = self.interface.bytes_to_read()
            .context("Failed to figure out how many bytes are available to read")?
            .try_into()?;

        debug!("Available bytes to read: {}", available_bytes);
        let mut buffer: Vec<u8> = vec![0; available_bytes];

        if buffer.len() <= 0 {
            debug!("No bytes available to read");
            return Ok(());
        }

        self.interface.read(&mut buffer)
            .context("Failed to read bytes from serial device")?;

        if log_enabled!(Level::Debug) {
            debug!("Received bytes: \n{}\n{}", hex::encode(&buffer), to_ascii_debug(&buffer));
        }

        self.buffer.append(&mut buffer);

        Ok(())
    }

    pub fn drain_buffer(&mut self) {
        let _ = self.fill_buffer();
        self.buffer = Vec::new();
    }

    pub fn recv_bytes(&mut self) -> Result<Vec<u8>> {
        let retry_counter = 0;

        let packet_bounds: PacketBounds;
        let mut buffer: Vec<u8>;

        loop {
            if retry_counter > 300 {
                bail!("Failed to fetch more data, timed out");
            }

            self.fill_buffer()
                .context("Failed to fill read buffer")?;

            buffer = self.buffer.clone();

            if let Ok(pb) = find_packet(&buffer) {
                packet_bounds = pb;
                break;
            }

            std::thread::sleep(std::time::Duration::from_millis(10));
        }

        let packet_bytes = buffer[packet_bounds.begin..packet_bounds.end].to_vec();

        self.buffer = buffer[packet_bounds.end..].to_vec();

        if log_enabled!(Level::Debug) {
            debug!("Received packet: \n{}\n{}", hex::encode(&packet_bytes), to_ascii_debug(&packet_bytes));
        }

        return Ok(packet_bytes);
    }
}