pub use crate::protocol::PropertyTag;
use crate::protocol::{
    CommandTag, FramingPacketType, ResponseTag, COMMAND_FLAG_HAS_DATA, COMMAND_MAX_PARAMS,
    FRAMING_HEADER_SIZE, MIN_MAX_PACKET_SIZE, START_BYTE,
};
pub use crate::transport::Transport;
use anyhow::{anyhow, bail, Context as _, Result};
use std::fmt;
use std::io;
use std::time::{Duration, Instant};

// TODO(jrreinhart): Replace anyhow errors with custom error types using thiserror.

/// Represents a connected target device running the NXP MCU bootloader.
pub struct Device {
    transport: Box<dyn Transport>,
    max_packet_size: usize,
}

// Public API
impl Device {
    pub fn new(transport: Box<dyn Transport>) -> Self {
        Device {
            transport,
            // Initial max packet size is the minimum boot ROM packet size.
            // This is updated to GetProperty(MaxPacketSize).
            max_packet_size: MIN_MAX_PACKET_SIZE,
        }
    }

    pub fn transport(&self) -> &dyn Transport {
        &*self.transport
    }

    pub fn transport_mut(&mut self) -> &mut dyn Transport {
        &mut *self.transport
    }

    pub fn init(&mut self) -> Result<()> {
        self.ping().context("Initial ping")?;

        let max_packet_size = self.get_property_single(PropertyTag::MaxPacketSize)?;
        log::info!("MaxPacketSize: 0x{max_packet_size:X}");
        if (max_packet_size as usize) < self.max_packet_size {
            bail!("MaxPacketSize (0x{max_packet_size:X}) too small!");
        }
        self.max_packet_size = max_packet_size as usize;

        Ok(())
    }

    pub fn ping(&mut self) -> Result<PingResponse> {
        // TODO: Check initialized
        self.write_ping()?;
        self.read_ping_response()
    }

    pub fn get_property_mem_id(
        &mut self,
        prop_tag: PropertyTag,
        memory_id: Option<u32>,
    ) -> Result<Vec<u32>> {
        // TODO: Check initialized

        // Send GetProperty Command
        // RT500RM 18.7.2.7.2 GetProperty command
        let mut args: Vec<u32> = vec![prop_tag.into()];
        if let Some(memory_id) = memory_id {
            args.push(memory_id)
        }
        self.send_command(CommandTag::GetProperty, &args)?;

        // Read GetPropertyResponse
        // RT500RM Table 107. GetPropertyResponse Parameters
        let mut response = self.read_response(ResponseTag::GetPropertyResponse)?;

        // "The parameter count field in the header is set to greater than 1, to always include the
        // status code and one or many property values."
        if response.len() < 2 {
            bail!("Unexpected response param count: {}", response.len());
        }

        let status_code: u32 = response[0];
        let property_values = response.split_off(1);

        if status_code != 0 {
            bail!("Command returned status {status_code}");
        }

        Ok(property_values)
    }

    pub fn get_property(&mut self, prop_tag: PropertyTag) -> Result<Vec<u32>> {
        self.get_property_mem_id(prop_tag, None)
    }

    pub fn get_property_single(&mut self, prop_tag: PropertyTag) -> Result<u32> {
        let property_values = self.get_property(prop_tag)?;
        if property_values.len() != 1 {
            bail!(
                "GetProperty returned {} values; expected one.",
                property_values.len()
            );
        }
        Ok(property_values[0])
    }

    pub fn set_property(&mut self, prop_tag: PropertyTag, value: u32) -> Result<()> {
        // TODO: Check initialized

        // Send SetProperty command
        let args = [prop_tag.into(), value];
        self.send_command(CommandTag::SetProperty, &args)?;

        self.read_generic_response(CommandTag::SetProperty)?;
        log::debug!("Got generic response");

        Ok(())
    }

    pub fn write_memory(
        &mut self,
        data: &[u8],
        address: u32,
        memory_id: Option<u32>,
    ) -> Result<()> {
        // TODO: Check initialized

        const COMMAND: CommandTag = CommandTag::WriteMemory;
        let data_size = u32::try_from(data.len())?;

        // RT500RM Figure 64. Protocol Sequence for WriteMemory Command

        // Write command
        let mut args: Vec<u32> = vec![address, data_size];
        if let Some(memory_id) = memory_id {
            args.push(memory_id)
        }
        self.send_command_has_data(COMMAND, &args)?;

        // Read initial generic response
        self.read_generic_response(COMMAND)
            .context("Reading initial response")?;

        // Write data loop
        for chunk in data.chunks(self.max_packet_size) {
            self.write_data_packet(chunk)?;
        }

        // Read final generic response
        self.read_generic_response(COMMAND)
            .context("Reading final response")?;

        Ok(())
    }

    pub fn read_memory(
        &mut self,
        address: u32,
        byte_count: u32,
        memory_id: Option<u32>,
    ) -> Result<Vec<u8>> {
        // TODO: Check initialized

        const COMMAND: CommandTag = CommandTag::ReadMemory;

        // RT500RM Figure 63. Command sequence for ReadMemory

        // Write command
        let mut args: Vec<u32> = vec![address, byte_count];
        if let Some(memory_id) = memory_id {
            args.push(memory_id)
        }
        self.send_command(COMMAND, &args)?;

        // Read ReadMemoryResponse
        // RT500RM Table 108. ReadMemoryResponse Parameters
        let response = self.read_response_expect_data(ResponseTag::ReadMemoryResponse, true)?;

        // "The parameter count set to 2 for the status code and the data byte count parameters"
        if response.len() != 2 {
            bail!("Unexpected response param count: {}", response.len());
        }

        let status_code: u32 = response[0];
        if status_code != 0 {
            bail!("Command returned status {status_code}");
        }

        let ret_byte_count = response[1] as usize;
        if ret_byte_count > byte_count as usize {
            bail!("Returned byte count {ret_byte_count} > requested {byte_count}");
        }

        // Read data loop
        let mut result: Vec<u8> = Vec::with_capacity(ret_byte_count);
        while result.len() < ret_byte_count {
            let mut data = self.read_data_packet()?;

            // "The sending side may abort the data phase early by sending
            // a zero-length data packet."
            if data.is_empty() {
                log::warn!("Data phase aborted by sender");
                break;
            }

            result.append(&mut data);
        }

        // Read final generic response
        self.read_generic_response(COMMAND)
            .context("Reading final response")?;

        Ok(result)
    }

    pub fn configure_memory(&mut self, memory_id: u32, config_address: u32) -> Result<()> {
        // TODO: Check initialized
        const COMMAND: CommandTag = CommandTag::ConfigureMemory;
        self.send_command(COMMAND, &[memory_id, config_address])?;
        self.read_generic_response(COMMAND)?;
        Ok(())
    }

    pub fn execute(&mut self, address: u32, argument: u32, stack_ptr: u32) -> Result<()> {
        // TODO: check initialized
        self.send_command(CommandTag::Execute, &[address, argument, stack_ptr])?;
        self.read_generic_response(CommandTag::Execute)?;
        Ok(())
    }

    pub fn flash_erase_region_until(
        &mut self,
        address: u32,
        byte_count: u32,
        memory_id: Option<u32>,
        deadline: Instant,
    ) -> Result<()> {
        // TODO: check initialized
        const COMMAND: CommandTag = CommandTag::FlashEraseRegion;

        // Send FlashEraseRegion Command
        // RT500RM 18.7.2.7.5 FlashEraseRegion command
        let mut args: Vec<u32> = vec![address, byte_count];
        if let Some(memory_id) = memory_id {
            args.push(memory_id)
        }
        self.send_command_until(COMMAND, &args, deadline)?;

        self.read_generic_response(COMMAND)?;
        Ok(())
    }

    pub fn flash_erase_region(
        &mut self,
        address: u32,
        byte_count: u32,
        memory_id: Option<u32>,
    ) -> Result<()> {
        let deadline = Instant::now() + Self::DEFAULT_TIMEOUT;
        self.flash_erase_region_until(address, byte_count, memory_id, deadline)
    }

    /// Enables the IRQ notify function on the given port/pin tuple
    pub fn set_irqn_pin(&mut self, port: u8, pin: u8) -> Result<()> {
        // RT500RM 18.7.2.9 SPI In-System Programming
        // RT500RM 18.7.2.7.17 Supported properties in GetProperty and SetProperty
        // IRQ notifier pin:
        // bit[7:0] pin
        // bit[15:8] port
        // bit[31] enable
        let value: u32 = (1 << 31) | ((port as u32) << 8) | (pin as u32);
        self.set_property(PropertyTag::IrqNotifierPin, value)
    }

    /// Sets up the IRQ notify pin.
    ///
    /// This does two things:
    ///
    /// 1. Configures the MCU bootrom ISP to use the given port/pin for its IRQ notify output pin.
    /// 2. Configures the host-side transport to watch for IRQ notifications on the gpio line.
    ///
    /// # Arguments
    ///
    /// * `port` - The MCU GPIO port number of the IRQ pin. E.g. for PIO0_7, this is 0.
    /// * `pin` - The MCU GPIO pin number of the IRQ pin. E.g. for PIO0_7, this is 7.
    /// * `irq_line` - The GPIO line to monitor for IRQ notifications.
    pub fn setup_irqn(&mut self, port: u8, pin: u8, irq_line: &gpio_cdev::Line) -> Result<()> {
        // First, configure the MCU to use the IRQ Notifier Pin.
        self.set_irqn_pin(port, pin)
            .context("Setting IRQ notifier pin")?;

        // Then tell the transport to respect it.
        self.transport.set_irq_line(irq_line)?;

        // Verify it works before going any further.
        self.ping().context("Verifying irq line")?;

        Ok(())
    }
}

// Internal functions
impl Device {
    const DEFAULT_TIMEOUT: Duration = Duration::from_secs(1);

    fn read_until(&mut self, buf: &mut [u8], deadline: Instant) -> io::Result<()> {
        self.transport.read(buf, deadline)
    }

    fn read_start_until(&mut self, buf: &mut [u8], deadline: Instant) -> io::Result<()> {
        self.transport.read_start(buf, deadline)
    }

    fn write_until(&mut self, buf: &[u8], deadline: Instant) -> io::Result<()> {
        self.transport.write(buf, deadline)
    }

    fn write(&mut self, buf: &[u8]) -> io::Result<()> {
        let deadline = Instant::now() + Self::DEFAULT_TIMEOUT;
        self.write_until(buf, deadline)
    }

    /// Reads a packet which starts with a sync byte and a packet type.
    ///
    /// # Arguments
    ///
    /// * `size` - The size of the packet. Must be at least 2 as it includes
    ///   the start byte and packet type.
    fn read_sync_packet_until(&mut self, size: usize, deadline: Instant) -> Result<SyncPacket> {
        // The buffer must be at least two bytes:
        // * Start byte (0x5A)
        // * Packet type
        assert!(size >= 2);
        let mut buf: Vec<u8> = vec![0; size];

        // Read the start byte and packet type byte
        let start_buf = buf.get_mut(0..2).unwrap(); // length checked on entry
        self.read_start_until(start_buf, deadline)
            .context("Reading packet start")?;
        assert_eq!(start_buf[0], START_BYTE);
        if start_buf[1] == START_BYTE {
            bail!("Duplicate start byte");
        }

        // Read the remainder of the requested packet
        let rest = buf.get_mut(2..).unwrap(); // length checked on entry
        if !rest.is_empty() {
            self.read_until(rest, deadline)
                .context("Reading rest of packet")?;
        }

        Ok(SyncPacket::new(buf))
    }

    fn read_sync_packet(&mut self, size: usize) -> Result<SyncPacket> {
        let deadline = Instant::now() + Self::DEFAULT_TIMEOUT;
        self.read_sync_packet_until(size, deadline)
    }

    /// Writes an ACK packet to the device.
    fn write_ack(&mut self) -> Result<()> {
        let buf: [u8; 2] = [START_BYTE, FramingPacketType::Ack.into()];
        self.write(&buf).context("Writing ack")
    }

    /// Reads an ACK packet from the device with a deadline.
    fn read_ack_until(&mut self, deadline: Instant) -> Result<()> {
        let packet = self.read_sync_packet_until(2, deadline).context("Reading ACK")?;
        packet.check_packet_type(FramingPacketType::Ack)?;
        // TODO: Handle FramingPacketType::Nak (will require removing ? from previous line).
        Ok(())
    }

    /// Reads an ACK packet from the device.
    fn read_ack(&mut self) -> Result<()> {
        let deadline = Instant::now() + Self::DEFAULT_TIMEOUT;
        self.read_ack_until(deadline)
    }

    /// Writes a ping packet to the device.
    fn write_ping(&mut self) -> Result<()> {
        let buf: [u8; 2] = [START_BYTE, FramingPacketType::Ping.into()];
        self.write(&buf).context("Writing ping")
    }

    /// Reads a ping response packet from the device.
    fn read_ping_response(&mut self) -> Result<PingResponse> {
        const PING_RESPONSE_SIZE: usize = 10;
        let packet = self
            .read_sync_packet(PING_RESPONSE_SIZE)
            .context("Reading ping response")?;
        packet.check_packet_type(FramingPacketType::PingResponse)?;

        let _options = u16::from_le_bytes(packet.data[6..8].try_into().unwrap());
        let hdr_crc = u16::from_le_bytes(packet.data[8..10].try_into().unwrap());

        // Validate CRC
        let calc_crc = CRC.checksum(&packet.data[0..8]);
        if calc_crc != hdr_crc {
            // TODO: specific error
            bail!(
                "Ping response CRC (0x{:04X}) incorrect (expected 0x{:04X})",
                calc_crc,
                hdr_crc
            );
        }

        let response = PingResponse {
            protocol_name: packet.data[5] as char,
            protocol_major: packet.data[4],
            protocol_minor: packet.data[3],
            protocol_bugfix: packet.data[2],
        };
        log::info!("Got ping response: {}", response);
        Ok(response)
    }

    /// Reads and ACKs a framing packet with an expected packet type and returns the payload bytes.
    fn read_framing_packet(&mut self, expected_packet_type: FramingPacketType) -> Result<Vec<u8>> {
        let deadline = Instant::now() + Self::DEFAULT_TIMEOUT;

        // First read the framing packet header
        // RT500RM Table 99. Framing Packet Format
        let header = self
            .read_sync_packet_until(FRAMING_HEADER_SIZE, deadline)
            .context("Reading framing packet")?;

        // Decode the framing header
        let length = u16::from_le_bytes(header.data[2..4].try_into().unwrap());
        let hdr_crc = u16::from_le_bytes(header.data[4..6].try_into().unwrap());

        // Now read the payload
        let mut payload = vec![0; length.into()];
        if length > 0 {
            self.read_until(payload.as_mut_slice(), deadline)
                .context("Reading payload")?;
        }

        // Validate packet type
        header.check_packet_type(expected_packet_type)?;

        // Validate CRC
        let calc_crc = framing_packet_crc(&header.data, &payload);
        if calc_crc != hdr_crc {
            // TODO: Send NAK?
            // TODO: specific error
            bail!(
                "Framing packet CRC (0x{:04X}) incorrect (expected 0x{:04X})",
                calc_crc,
                hdr_crc
            );
        }

        self.write_ack()?;

        Ok(payload)
    }

    /// Writes a framing packet with an arbitrary payload.
    fn write_framing_packet(
        &mut self,
        packet_type: FramingPacketType,
        payload: &[u8],
    ) -> Result<()> {
        // Header without CRC
        let partial_header = [
            [START_BYTE, packet_type.into()],
            (payload.len() as u16).to_le_bytes(),
        ]
        .concat();

        // Header + CRC + payload
        let buf = [
            partial_header.as_slice(),
            &framing_packet_crc(&partial_header, payload).to_le_bytes(),
            payload,
        ]
        .concat();

        self.write(&buf).context("Writing framing packet")
    }

    /// Writes a command with a given tag, flags, and set of parameters.
    fn write_command_flags(&mut self, tag: CommandTag, flags: u8, params: &[u32]) -> Result<()> {
        if params.len() > COMMAND_MAX_PARAMS {
            bail!(
                "Parameter count ({}) exceeds limit ({})",
                params.len(),
                COMMAND_MAX_PARAMS
            );
        }
        log::debug!(
            "Writing command={:?} flags={} params={:?}",
            tag,
            flags,
            params
        );

        // RT500RM Table 102. Command Packet Format
        let payload = [
            tag.into(),         // 0: command or response tag
            flags,              // 1: flags
            0,                  // 2: reserved
            params.len() as u8, // 3: parameter count
        ]
        .into_iter()
        .chain(params.iter().flat_map(|param| param.to_le_bytes()))
        .collect::<Vec<u8>>();

        self.write_framing_packet(FramingPacketType::Command, payload.as_slice())
    }

    /// Sends a command and reads the ACK, with a deadline.
    fn send_command_until(&mut self, tag: CommandTag, params: &[u32], deadline: Instant) -> Result<()> {
        // TODO: This naming feels inconsistent
        let flags: u8 = 0;
        self.write_command_flags(tag, flags, params)?;
        self.read_ack_until(deadline)
    }

    /// Sends a command and reads the ACK.
    fn send_command(&mut self, tag: CommandTag, params: &[u32]) -> Result<()> {
        let deadline = Instant::now() + Self::DEFAULT_TIMEOUT;
        self.send_command_until(tag, params, deadline)
    }

    /// Sends a command, indicating a data phase, and reads the ACK.
    fn send_command_has_data(&mut self, tag: CommandTag, params: &[u32])&nbsp;-> Result<()> {
        // TODO: This naming feels inconsistent
        let flags: u8 = COMMAND_FLAG_HAS_DATA;
        self.write_command_flags(tag, flags, params)?;
        self.read_ack()
    }

    /// Reads a response packet from the device and returns the "parameters".
    fn read_response_expect_data(
        &mut self,
        expected_tag: ResponseTag,
        expect_data: bool,
    ) -> Result<Vec<u32>> {
        // RT500RM 18.7.2.6.6 Response packet
        // "The responses are carried using the same command packet format wrapped with framing
        // packet data."
        let response = self
            .read_framing_packet(FramingPacketType::Command)
            .context("Reading response packet")?;

        // Decode the response packet header.
        // RT500RM 18.7.2.6.6 Response packet: "same command packet format":
        // RT500RM 18.7.2.6.5 Command packet
        let resp_tag = ResponseTag::try_from(response[0])
            .map_err(|_| anyhow!("Unknown response tag: 0x{:02X}", response[0]))?;
        let flags = response[1];
        let has_data: bool = (flags & COMMAND_FLAG_HAS_DATA) != 0;
        // [2] reserved
        let param_count = response[3] as usize;

        if resp_tag != expected_tag {
            bail!("Unexpected response tag {resp_tag:?}, expected {expected_tag:?}");
        }
        if has_data != expect_data {
            bail!(
                "Response {} have data; {} expect data",
                if has_data { "does" } else { "doesn't" },
                if expect_data { "did" } else { "didn't" }
            );
        }

        // Unpack the params
        let param_bytes = &response[4..];
        if param_count * size_of::<u32>() != param_bytes.len() {
            bail!(
                "Param count ({}) / param bytes ({}) mismatch",
                param_count,
                param_bytes.len()
            );
        }
        let params = param_bytes
            .chunks(size_of::<u32>())
            .map(|bytes| u32::from_le_bytes(bytes.try_into().unwrap()))
            .collect();

        Ok(params)
    }

    fn read_response(&mut self, expected_tag: ResponseTag) -> Result<Vec<u32>> {
        self.read_response_expect_data(expected_tag, false)
    }

    fn read_generic_response(&mut self, expected_command: CommandTag) -> Result<()> {
        let response = self
            .read_response(ResponseTag::GenericResponse)
            .context("Reading generic response")?;

        // Table 106. GenericResponse Parameters
        // "The parameter count field in the header is always set to 2, for status
        // code and command tag parameters."
        if response.len() != 2 {
            bail!("Unexpected response param count: {}", response.len())
        }

        let status_code: u32 = response[0];

        let command_tag_val = u8::try_from(response[1]).map_err(|_| {
            anyhow!(
                "Generic response command tag too large: 0x{:08X}",
                response[1]
            )
        })?;
        let command_tag = CommandTag::try_from(command_tag_val).map_err(|_| {
            anyhow!(
                "Unknown generic response command tag: 0x{:02X}",
                response[1]
            )
        })?;

        if command_tag != expected_command {
            bail!(
                "Unexpected generic response command tag {:?}, expected {:?}",
                command_tag,
                expected_command
            );
        }

        if status_code != 0 {
            // TODO: Return this an a custom error type
            bail!(
                "Command {:?} returned status 0x{:08X}",
                command_tag,
                status_code
            );
        }

        Ok(())
    }

    /// Writes a data packet and reads the ACK.
    fn write_data_packet(&mut self, data: &[u8]) -> Result<()> {
        self.write_framing_packet(FramingPacketType::Data, data)?;
        self.read_ack()
    }

    /// Reads a data packet and sends an ACK.
    fn read_data_packet(&mut self) -> Result<Vec<u8>> {
        self.read_framing_packet(FramingPacketType::Data)
            .context("Reading data packet")
    }
}

struct SyncPacket {
    data: Vec<u8>,
}

impl SyncPacket {
    fn new(data: Vec<u8>) -> Self {
        assert!(data.len() >= 2);
        SyncPacket { data }
    }

    fn packet_type(&self) -> Result<FramingPacketType> {
        let ptype = self.data[1];
        FramingPacketType::try_from(ptype)
            .map_err(|_| anyhow!("Unknown packet type: 0x{:02X}", ptype))
    }

    fn check_packet_type(&self, expected: FramingPacketType) -> Result<()> {
        let actual = self.packet_type()?;
        if actual != expected {
            bail!(
                "Unexpected packet type: {:?}, expected {:?}",
                actual,
                expected
            );
        }
        Ok(())
    }
}

// RT500RM 18.7.2.6.4 CRC16 algorithm
// "The CRC is computed over each byte in the framing packet header, excluding the crc16 field
// itself, plus all of the payload bytes. The CRC algorithm is the XMODEM variant of CRC-16."
// poly=0x1021 init=0x0000 refin=false refout=false xorout=0x0000 check=0x31c3
const CRC: crc::Crc<u16> = crc::Crc::<u16>::new(&crc::CRC_16_XMODEM);

fn framing_packet_crc(header: &[u8], payload: &[u8]) -> u16 {
    let mut digest = CRC.digest();
    digest.update(&header[0..4]);
    digest.update(payload);
    digest.finalize()
}

#[derive(Debug)]
pub struct PingResponse {
    protocol_name: char,
    protocol_major: u8,
    protocol_minor: u8,
    protocol_bugfix: u8,
}

impl fmt::Display for PingResponse {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(
            f,
            "{}{}.{}.{}",
            self.protocol_name, self.protocol_major, self.protocol_minor, self.protocol_bugfix
        )
    }
}

// -------------------------------------------------------------------------------------------------
// Tests

#[cfg(test)]
mod tests {
    use super::*;
    use std::collections::VecDeque;
    use std::io;

    fn init_logger() {
        let _ = env_logger::builder()
            // Include all events in tests
            .filter_level(log::LevelFilter::max())
            // Ensure events are captured by `cargo test`
            .is_test(true)
            // Ignore errors initializing the logger if tests race to configure it
            .try_init();
    }

    #[derive(Default)]
    struct TestTransport {
        // Data is popped by read()
        // push_back(), pop_front()
        read_q: VecDeque<u8>,

        // Data is pushed by write()
        // push_back(), pop_front()
        write_q: VecDeque<u8>,
    }

    impl TestTransport {
        pub fn new() -> Self {
            TestTransport::default()
        }

        // Read

        pub fn push_for_read(&mut self, buf: &[u8]) {
            for b in buf {
                self.read_q.push_back(*b);
            }
        }

        pub fn push_ack(&mut self) {
            self.push_for_read(&[0x5a, 0xa1]); // Ack
        }

        // Write

        pub fn take_written(&mut self, n: usize) -> Result<Vec<u8>> {
            if n > self.write_q.len() {
                bail!(
                    "Failed to take {} bytes from write queue; only {} avail: {:X?}",
                    n,
                    self.write_q.len(),
                    self.write_q
                );
            }
            Ok(self.write_q.drain(0..n).collect::<Vec<u8>>())
        }

        pub fn assert_written(&mut self, expected: &[u8]) {
            let ack = self.take_written(expected.len()).unwrap();
            assert_eq!(ack, expected);
        }

        pub fn assert_ack_written(&mut self) {
            self.assert_written(&[0x5a, 0xa1]);
        }
    }

    impl Drop for TestTransport {
        fn drop(&mut self) {
            log::warn!("TestTransport::drop: read_q: {:?}", self.read_q);
            assert!(self.read_q.is_empty());
        }
    }

    impl Transport for TestTransport {
        fn read(&mut self, buf: &mut [u8], _deadline: Instant) -> io::Result<()> {
            assert_ne!(buf.len(), 0);
            for i in 0..buf.len() {
                buf[i] = self.read_q.pop_front().ok_or_else(|| {
                    io::Error::new(
                        io::ErrorKind::Other,
                        format!("read_q empty at {}/{}", i, buf.len()),
                    )
                })?;
            }
            log::debug!("TestTransport::read() read {} bytes", buf.len());
            Ok(())
        }

        fn write(&mut self, buf: &[u8], _deadline: Instant) -> io::Result<()> {
            assert_ne!(buf.len(), 0);
            for b in buf {
                self.write_q.push_back(*b);
            }
            Ok(())
        }
    }

    impl Device {
        fn new_test() -> Self {
            let transport = Box::new(TestTransport::new());
            Device::new(transport)
        }

        fn test_transport(&mut self) -> &mut TestTransport {
            self.transport_mut()
                .downcast_mut::<TestTransport>()
                .unwrap()
        }
    }

    #[test]
    fn read_sync_packet_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();
        device.test_transport().push_ack();

        let packet = device.read_sync_packet(2)?;
        packet.check_packet_type(FramingPacketType::Ack)?;

        Ok(())
    }

    #[test]
    fn read_ack_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();
        device.test_transport().push_ack();

        device.read_ack()?;

        Ok(())
    }

    #[test]
    fn write_ack_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();

        device.write_ack()?;
        device.test_transport().assert_ack_written();

        Ok(())
    }

    #[test]
    fn ping_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();

        // RT500RM Figure 56. Ping Packet Protocol Sequence
        device
            .test_transport()
            .push_for_read(&[0x5a, 0xa7, 0x00, 0x02, 0x01, 0x50, 0x00, 0x00, 0xaa, 0xea]);

        let resp = device.ping()?;

        device.test_transport().assert_written(&[0x5a, 0xa6]);

        assert_eq!(resp.protocol_name, 'P');
        assert_eq!(resp.protocol_major, 1);
        assert_eq!(resp.protocol_minor, 2);
        assert_eq!(resp.protocol_bugfix, 0);

        Ok(())
    }

    #[test]
    fn ping_detects_bad_crc() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();

        // RT500RM Figure 56. Ping Packet Protocol Sequence
        device
            .test_transport()
            .push_for_read(&[0x5a, 0xa7, 0x00, 0x02, 0x01, 0x50, 0x00, 0x00, 0xdd, 0xba]);

        let result = device.ping();
        assert!(result.is_err());
        // TODO: Assert specific error

        Ok(())
    }

    static TEST_DATA: [u8; 16] = [
        0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee,
        0xff,
    ];

    static TEST_DATA_FRAMING_HEADER: [u8; FRAMING_HEADER_SIZE] = [
        // RT500RM 18.7.2.7.7 WriteMemory command
        0x5a, // Start
        0xa5, // Packet Type: Data
        0x10, 0x00, // Length: 16
        0x25, 0x84, // CRC: 0x8425
    ];

    #[test]
    fn read_framing_packet_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();
        device
            .test_transport()
            .push_for_read(&TEST_DATA_FRAMING_HEADER);
        device.test_transport().push_for_read(&TEST_DATA);

        let data = device.read_framing_packet(FramingPacketType::Data)?;
        assert_eq!(data, &TEST_DATA);

        device.test_transport().assert_ack_written();

        Ok(())
    }

    #[test]
    fn read_framing_packet_detects_incorrect_packet_type() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();

        device.test_transport().push_for_read(&[
            0x5a, // Start
            0x66, // Packet Type: (0x66 is bogus)
            0x00, 0x00, // Length: 16
            0x5b, 0x34, // CRC: 0x345B
        ]);

        let result = device.read_framing_packet(FramingPacketType::Data);
        assert!(result.is_err());
        // TODO: Assert specific error

        Ok(())
    }

    #[test]
    fn read_framing_packet_detects_bad_crc() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();
        device.test_transport().push_for_read(&[
            // RT500RM 18.7.2.7.7 WriteMemory command
            0x5a, // Start
            0xa5, // Packet Type: Data
            0x10, 0x00, // Length: 16
            0xdd, 0xba, // CRC: 0xBADD (wrong, should be 0x8425)
        ]);
        device.test_transport().push_for_read(&TEST_DATA);

        let result = device.read_framing_packet(FramingPacketType::Data);
        assert!(result.is_err());
        // TODO: Assert specific error

        Ok(())
    }

    #[test]
    fn write_framing_packet_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();

        device.write_framing_packet(FramingPacketType::Data, &TEST_DATA)?;

        device
            .test_transport()
            .assert_written(&TEST_DATA_FRAMING_HEADER);
        device.test_transport().assert_written(&TEST_DATA);

        Ok(())
    }

    // RT500RM Figure 65. Protocol Sequence for FillMemory Command
    static TEST_FILL_MEMORY_PARAMS: [u32; 3] = [0x0, 4, 0x12345678];
    static TEST_FILL_MEMORY_COMMAND: [u8; 22] = [
        0x5a, 0xa4, 0x10, 0x00, 0xb6, 0x70, 0x05, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x04,
        0x00, 0x00, 0x00, 0x78, 0x56, 0x34, 0x12,
    ];

    #[test]
    fn write_command_flags_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();

        device.write_command_flags(CommandTag::FillMemory, 0, &TEST_FILL_MEMORY_PARAMS)?;
        device
            .test_transport()
            .assert_written(&TEST_FILL_MEMORY_COMMAND);

        Ok(())
    }

    #[test]
    fn send_command_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();

        device.test_transport().push_ack();

        device.send_command(CommandTag::FillMemory, &TEST_FILL_MEMORY_PARAMS)?;
        device
            .test_transport()
            .assert_written(&TEST_FILL_MEMORY_COMMAND);

        Ok(())
    }

    static TEST_CALL_COMMAND_RESPONSE: [u8; 18] = [
        0x5a, 0xa4, 0x0c, 0x00, 0x79, 0xd0, 0xa0, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x0a,
        0x00, 0x00, 0x00,
    ];

    #[test]
    fn read_response_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();

        // RT500RM Figure 66. Protocol sequence for Call command
        device
            .test_transport()
            .push_for_read(&TEST_CALL_COMMAND_RESPONSE);

        let resp = device.read_response(ResponseTag::GenericResponse)?;
        assert_eq!(resp, &[0, 10]);

        device.test_transport().assert_ack_written();

        Ok(())
    }

    #[test]
    fn read_generic_response_works() -> Result<()> {
        init_logger();

        let mut device = Device::new_test();

        // RT500RM Figure 66. Protocol sequence for Call command
        device
            .test_transport()
            .push_for_read(&TEST_CALL_COMMAND_RESPONSE);

        device.read_generic_response(CommandTag::Call)?;

        device.test_transport().assert_ack_written();

        Ok(())
    }

    #[test]
    fn read_data_packet_works() -> Result<()> {
        init_logger();

        // RT500RM Figure 63. Command sequence for ReadMemory
        const TEST_READ_DATA_PACKET: [u8; 22] = [
            0x5a, 0xa5, 0x10, 0x00, 0x99, 0x5b, 0xd1, 0x00, 0x20, 0x40, 0x11, 0x2e, 0x01, 0x20,
            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        ];

        let mut device = Device::new_test();

        device
            .test_transport()
            .push_for_read(&TEST_READ_DATA_PACKET);

        let result = device.read_data_packet()?;
        assert_eq!(result, TEST_READ_DATA_PACKET[6..]);

        Ok(())
    }

    #[test]
    fn get_property_works() -> Result<()> {
        init_logger();

        // GetProperty(MaxPacketSize)
        const EXPECTED_GET_PROP_COMMAND: [u8; 14] = [
            0x5a, 0xa4, 0x08, 0x00, 0xd8, 0xbc, 0x07, 0x00, 0x00, 0x01, 0x0B, 0x00, 0x00, 0x00,
        ];
        const GET_PROP_RESPONSE: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0xf9, 0xde, 0xa7, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x02, 0x00, 0x00,
        ];

        let mut device = Device::new_test();

        device.test_transport().push_ack(); // Ack the GetProperty command
        device.test_transport().push_for_read(&GET_PROP_RESPONSE);

        let result = device.get_property_single(PropertyTag::MaxPacketSize)?;

        device
            .test_transport()
            .assert_written(&EXPECTED_GET_PROP_COMMAND);
        device.test_transport().assert_ack_written();

        assert_eq!(result, 0x200);

        Ok(())
    }

    #[test]
    fn set_property_works() -> Result<()> {
        init_logger();

        // RT500RM Figure 60. Parameters for SetProperty Command
        // (with corrected CRC in response)
        const EXPECTED_SET_PROP_COMMAND: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0x67, 0x8d, 0x0c, 0x00, 0x00, 0x02, 0x0a, 0x00, 0x00, 0x00,
            0x01, 0x00, 0x00, 0x00,
        ];
        const SET_PROP_RESPONSE: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0xe0, 0xf7, 0xa0, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
            0x0c, 0x00, 0x00, 0x00,
        ];

        let mut device = Device::new_test();

        device.test_transport().push_ack(); // Ack the SetProperty command
        device.test_transport().push_for_read(&SET_PROP_RESPONSE);

        device.set_property(PropertyTag::VerifyWrites, 1)?;

        device
            .test_transport()
            .assert_written(&EXPECTED_SET_PROP_COMMAND);
        device.test_transport().assert_ack_written();

        Ok(())
    }

    #[test]
    fn write_memory_works() -> Result<()> {
        init_logger();

        // RT500RM Figure 64. Protocol Sequence for WriteMemory Command
        const ADDRESS: u32 = 0x20000400;

        const EXPECTED_WRITE_MEM_COMMAND: [u8; 22] = [
            0x5a, 0xa4, 0x10, 0x00, 0xf6, 0x62, 0x04, 0x01, 0x00, 0x03, 0x00, 0x04, 0x00, 0x20,
            0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        ];

        const GENERIC_RESPONSE: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0x23, 0x72, 0xa0, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
            0x04, 0x00, 0x00, 0x00,
        ];

        let mut device = Device::new_test();

        device.test_transport().push_ack(); // Ack the WriteMemory command
        device.test_transport().push_for_read(&GENERIC_RESPONSE); // Initial
        device.test_transport().push_ack(); // Ack the data packet
        device.test_transport().push_for_read(&GENERIC_RESPONSE); // Final

        device.write_memory(&TEST_DATA, ADDRESS, Some(0x0))?;

        device
            .test_transport()
            .assert_written(&EXPECTED_WRITE_MEM_COMMAND);
        device.test_transport().assert_ack_written(); // Ack the initial generic response
        device
            .test_transport()
            .assert_written(&TEST_DATA_FRAMING_HEADER);
        device.test_transport().assert_written(&TEST_DATA);
        device.test_transport().assert_ack_written(); // Ack the final generic response

        Ok(())
    }

    #[test]
    fn read_memory_works() -> Result<()> {
        init_logger();

        // RT500RM Figure 63. Command sequence for ReadMemory
        const EXPECTED_READ_MEM_COMMAND: [u8; 22] = [
            0x5a, 0xa4, 0x10, 0x00, 0x95, 0xa4, 0x03, 0x00, 0x00, 0x03, 0x00, 0x04, 0x00, 0x20,
            0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        ];
        const READ_MEM_RESPONSE: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0xa3, 0x7e, 0xa3, 0x01, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
            0x10, 0x00, 0x00, 0x00,
        ];
        const DATA_PACKET: [u8; 22] = [
            0x5a, 0xa5, 0x10, 0x00, 0x99, 0x5b, 0xd1, 0x00, 0x20, 0x40, 0x11, 0x2e, 0x01, 0x20,
            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        ];
        const FINAL_GENERIC_RESPONSE: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0x0e, 0x23, 0xa0, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
            0x03, 0x00, 0x00, 0x00,
        ];

        let mut device = Device::new_test();

        device.test_transport().push_ack(); // Ack the ReadMemory command
        device.test_transport().push_for_read(&READ_MEM_RESPONSE);
        device.test_transport().push_for_read(&DATA_PACKET);
        device
            .test_transport()
            .push_for_read(&FINAL_GENERIC_RESPONSE);

        let result = device.read_memory(0x20000400, 0x10, Some(0))?;

        device
            .test_transport()
            .assert_written(&EXPECTED_READ_MEM_COMMAND);
        device.test_transport().assert_ack_written(); // Ack the read memory response
        device.test_transport().assert_ack_written(); // Ack the data packet
        device.test_transport().assert_ack_written(); // Ack the final generic response

        assert_eq!(result, DATA_PACKET[6..]);
        Ok(())
    }

    #[test]
    fn configure_memory_works() -> Result<()> {
        init_logger();

        // RT500RM Figure 70. Protocol Sequence for ConfigureMemory Command
        const MEMORY_ID: u32 = 0x9;
        const CONFIG_BLOCK_ADDR: u32 = 0x2000;

        const EXPECTED_CONFIG_MEM_COMMAND: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0x39, 0x97, 0x11, 0x00, 0x00, 0x02, 0x09, 0x00, 0x00, 0x00,
            0x00, 0x20, 0x00, 0x00,
        ];

        const RESPONSE: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0xc1, 0xd5, 0xa0, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
            0x11, 0x00, 0x00, 0x00,
        ];

        let mut device = Device::new_test();

        device.test_transport().push_ack(); // Ack the ConfigureMemory command
        device.test_transport().push_for_read(&RESPONSE);

        device.configure_memory(MEMORY_ID, CONFIG_BLOCK_ADDR)?;

        device
            .test_transport()
            .assert_written(&EXPECTED_CONFIG_MEM_COMMAND);
        device.test_transport().assert_ack_written(); // We should ack the generic response

        Ok(())
    }

    #[test]
    fn execute_works() -> Result<()> {
        init_logger();

        const ADDRESS: u32 = 0x95aa1;
        const ARGUMENT: u32 = 0x80000;
        const STACK_PTR: u32 = 0x20500000;
        const EXPECTED_EXECUTE_CMD: [u8; 22] = [
            0x5A, 0xA4, 0x10, 0x00, 0x46, 0xE8, 0x09, 0x00, 0x00, 0x03, 0xA1, 0x5A, 0x09, 0x00,
            0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x50, 0x20,
        ];
        const GENERIC_RESPONSE: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0xa5, 0x4b, 0xa0, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
            0x09, 0x00, 0x00, 0x00,
        ];

        let mut device = Device::new_test();

        device.test_transport().push_ack(); // Ack the Execute command
        device.test_transport().push_for_read(&GENERIC_RESPONSE);

        device.execute(ADDRESS, ARGUMENT, STACK_PTR)?;

        device
            .test_transport()
            .assert_written(&EXPECTED_EXECUTE_CMD);
        device.test_transport().assert_ack_written(); // Ack the generic response

        Ok(())
    }

    #[test]
    fn flash_erase_region_works() -> Result<()> {
        init_logger();

        // RT500RM Figure 62. Protocol Sequence for FlashEraseRegion Command
        const ADDRESS: u32 = 0x0;
        const BYTE_COUNT: u32 = 0x1000;
        const MEMORY_ID: u32 = 0;

        const EXPECTED_CMD: [u8; 22] = [
            0x5a, 0xa4, 0x10, 0x00, 0xc5, 0xf0, 0x02, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00,
            0x00, /* corrected: */ 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        ];
        const GENERIC_RESPONSE: [u8; 18] = [
            0x5a, 0xa4, 0x0c, 0x00, 0xba, 0x55, 0xa0, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
            0x02, 0x00, 0x00, 0x00,
        ];

        let mut device = Device::new_test();

        device.test_transport().push_ack(); // Ack the FlashEraseRegion command
        device.test_transport().push_for_read(&GENERIC_RESPONSE);

        device.flash_erase_region(ADDRESS, BYTE_COUNT, Some(MEMORY_ID))?;

        device.test_transport().assert_written(&EXPECTED_CMD);
        device.test_transport().assert_ack_written(); // Ack the generic response

        Ok(())
    }
}