///|
/// Encode a MAVLink v1 or v2 packet into wire bytes, computing the X.25 CRC-16 with `crc_extra`
/// and optionally appending a 13-byte MAVLink v2 signature when `signing` is provided.
pub fn encode_frame(
  header : MavHeader,
  raw_payload : Bytes,
  crc_extra : Byte,
  truncate_v2_zeros~ : Bool = true,
  signing~ : SigningContext? = None,
) -> Result[(Bytes, MavFrame), MavError] {
  if raw_payload.length() > MAX_PAYLOAD_LEN {
    return Err(PayloadTooLarge(raw_payload.length()))
  }
  match header.version {
    V1 => {
      if header.msgid > 255U {
        return Err(UnsupportedV1MessageId(header.msgid))
      }
      let payload_len = raw_payload.length()
      let body_writer = PayloadWriter::new(capacity=V1_HEADER_LEN + payload_len)
      body_writer.write_uint8(payload_len.to_byte())
      body_writer.write_uint8(header.seq)
      body_writer.write_uint8(header.sysid)
      body_writer.write_uint8(header.compid)
      body_writer.write_uint8((header.msgid & 0xFFU).to_byte())
      body_writer.write_bytes(raw_payload)
      let body_bytes = body_writer.to_bytes()
      let checksum = crc16_calculate_with_extra(body_bytes, crc_extra)

      let wire_writer = PayloadWriter::new(capacity=1 + body_bytes.length() + 2)
      wire_writer.write_uint8(MAVLINK_STX_V1)
      wire_writer.write_bytes(body_bytes)
      wire_writer.write_uint16(checksum)
      let frame : MavFrame = {
        header,
        payload: raw_payload,
        checksum,
        signature: None,
      }
      Ok((wire_writer.to_bytes(), frame))
    }
    V2 => {
      if header.msgid > 0xFFFFFFU {
        return Err(UnsupportedV1MessageId(header.msgid))
      }
      let payload = if truncate_v2_zeros {
        truncate_trailing_zeros(raw_payload)
      } else {
        raw_payload
      }
      let payload_len = payload.length()
      let effective_incompat = match signing {
        Some(_) =>
          (header.incompat_flags.to_uint() | IFLAG_SIGNED.to_uint()).to_byte()
        None => header.incompat_flags
      }
      let body_writer = PayloadWriter::new(capacity=V2_HEADER_LEN + payload_len)
      body_writer.write_uint8(payload_len.to_byte())
      body_writer.write_uint8(effective_incompat)
      body_writer.write_uint8(header.compat_flags)
      body_writer.write_uint8(header.seq)
      body_writer.write_uint8(header.sysid)
      body_writer.write_uint8(header.compid)
      body_writer.write_uint24(header.msgid)
      body_writer.write_bytes(payload)
      let body_bytes = body_writer.to_bytes()
      let checksum = crc16_calculate_with_extra(body_bytes, crc_extra)

      let unsigned_writer = PayloadWriter::new(
        capacity=1 + body_bytes.length() + 2,
      )
      unsigned_writer.write_uint8(MAVLINK_STX_V2)
      unsigned_writer.write_bytes(body_bytes)
      unsigned_writer.write_uint16(checksum)
      let unsigned_bytes = unsigned_writer.to_bytes()

      let effective_header : MavHeader = {
        ..header,
        incompat_flags: effective_incompat,
      }
      match signing {
        Some(ctx) => {
          let sig = ctx.sign_frame_bytes(unsigned_bytes)
          let signed_writer = PayloadWriter::new(
            capacity=unsigned_bytes.length() + V2_SIGNATURE_LEN,
          )
          signed_writer.write_bytes(unsigned_bytes)
          signed_writer.write_uint8(sig.link_id)
          for shift = 0; shift < 48; shift = shift + 8 {
            signed_writer.write_uint8(
              ((sig.timestamp >> shift) & 0xFFUL).to_byte(),
            )
          }
          for i = 0; i < 6; i = i + 1 {
            signed_writer.write_uint8(sig.signature[i])
          }
          let frame : MavFrame = {
            header: effective_header,
            payload,
            checksum,
            signature: Some(sig),
          }
          Ok((signed_writer.to_bytes(), frame))
        }
        None => {
          let frame : MavFrame = {
            header: effective_header,
            payload,
            checksum,
            signature: None,
          }
          Ok((unsigned_bytes, frame))
        }
      }
    }
  }
}

///|
/// Decode a single MAVLink v1 or v2 frame from `wire` starting at byte offset `start`.
/// Returns `(decoded_frame, bytes_consumed)` on success.
pub fn decode_frame(
  wire : Bytes,
  crc_extra_lookup : (UInt32) -> Byte?,
  start~ : Int32 = 0,
  signing~ : SigningContext? = None,
) -> Result[(MavFrame, Int32), MavError] {
  let avail = wire.length() - start
  if avail <= 0 {
    return Err(IncompleteFrame(1, 0))
  }
  let stx = wire[start]
  if stx == MAVLINK_STX_V1 {
    if avail < 1 + V1_HEADER_LEN + 2 {
      return Err(IncompleteFrame(1 + V1_HEADER_LEN + 2, avail))
    }
    let payload_len = wire[start + 1].to_int() & 0xFF
    let total_len = 1 + V1_HEADER_LEN + payload_len + 2
    if avail < total_len {
      return Err(IncompleteFrame(total_len, avail))
    }
    let seq = wire[start + 2]
    let sysid = wire[start + 3]
    let compid = wire[start + 4]
    let msgid = wire[start + 5].to_uint() & 0xFFU

    let crc_extra = match crc_extra_lookup(msgid) {
      Some(ce) => ce
      None => return Err(UnknownMessageId(msgid))
    }
    let crc = X25Crc::new()
    crc.accumulate_bytes(wire, start=start + 1, len=V1_HEADER_LEN + payload_len)
    crc.accumulate_byte(crc_extra)
    let crc_offset = start + 1 + V1_HEADER_LEN + payload_len
    let wire_crc = wire[crc_offset].to_uint() |
      (wire[crc_offset + 1].to_uint() << 8)
    if crc.value != wire_crc {
      return Err(CrcMismatch(crc.value, wire_crc))
    }
    let payload_arr : Array[Byte] = Array(capacity=payload_len)
    for i = 0; i < payload_len; i = i + 1 {
      payload_arr.push(wire[start + 1 + V1_HEADER_LEN + i])
    }
    let header = MavHeader::new_v1(seq~, sysid~, compid~, msgid~)
    let frame : MavFrame = {
      header,
      payload: Bytes::from_array(payload_arr),
      checksum: wire_crc,
      signature: None,
    }
    Ok((frame, total_len))
  } else if stx == MAVLINK_STX_V2 {
    if avail < 1 + V2_HEADER_LEN + 2 {
      return Err(IncompleteFrame(1 + V2_HEADER_LEN + 2, avail))
    }
    let payload_len = wire[start + 1].to_int() & 0xFF
    let incompat_flags = wire[start + 2]
    let compat_flags = wire[start + 3]
    if (incompat_flags.to_uint() & IFLAG_SIGNED.to_uint().lnot() & 0xFFU) != 0U {
      return Err(UnsupportedIncompatFlags(incompat_flags))
    }
    let is_signed = (incompat_flags.to_uint() & IFLAG_SIGNED.to_uint()) != 0U
    let sig_len = if is_signed { V2_SIGNATURE_LEN } else { 0 }
    let unsigned_len = 1 + V2_HEADER_LEN + payload_len + 2
    let total_len = unsigned_len + sig_len
    if avail < total_len {
      return Err(IncompleteFrame(total_len, avail))
    }
    let seq = wire[start + 4]
    let sysid = wire[start + 5]
    let compid = wire[start + 6]
    let msgid = wire[start + 7].to_uint() |
      (wire[start + 8].to_uint() << 8) |
      (wire[start + 9].to_uint() << 16)

    let crc_extra = match crc_extra_lookup(msgid) {
      Some(ce) => ce
      None => return Err(UnknownMessageId(msgid))
    }
    let crc = X25Crc::new()
    crc.accumulate_bytes(wire, start=start + 1, len=V2_HEADER_LEN + payload_len)
    crc.accumulate_byte(crc_extra)
    let crc_offset = start + 1 + V2_HEADER_LEN + payload_len
    let wire_crc = wire[crc_offset].to_uint() |
      (wire[crc_offset + 1].to_uint() << 8)
    if crc.value != wire_crc {
      return Err(CrcMismatch(crc.value, wire_crc))
    }
    let payload_arr : Array[Byte] = Array(capacity=payload_len)
    for i = 0; i < payload_len; i = i + 1 {
      payload_arr.push(wire[start + 1 + V2_HEADER_LEN + i])
    }
    let signature = if is_signed {
      let sig_offset = start + unsigned_len
      let link_id = wire[sig_offset]
      let mut ts = 0UL
      for i = 0; i < 6; i = i + 1 {
        ts = ts | (wire[sig_offset + 1 + i].to_uint64() << (i * 8))
      }
      let sig_bytes = FixedArray::make(6, b'\x00')
      for i = 0; i < 6; i = i + 1 {
        sig_bytes[i] = wire[sig_offset + 7 + i]
      }
      let parsed_sig : MavSignature = {
        link_id,
        timestamp: ts,
        signature: sig_bytes,
      }
      match signing {
        Some(ctx) => {
          let unsigned_arr : Array[Byte] = Array(capacity=unsigned_len)
          for i = 0; i < unsigned_len; i = i + 1 {
            unsigned_arr.push(wire[start + i])
          }
          match
            ctx.verify_frame_signature(
              Bytes::from_array(unsigned_arr),
              parsed_sig,
            ) {
            Ok(_) => ()
            Err(e) => return Err(e)
          }
        }
        None => ()
      }
      Some(parsed_sig)
    } else {
      match signing {
        Some(_) => return Err(SignatureMissing)
        None => None
      }
    }
    let header = MavHeader::new_v2(
      seq~,
      sysid~,
      compid~,
      msgid~,
      incompat_flags~,
      compat_flags~,
    )
    let frame : MavFrame = {
      header,
      payload: Bytes::from_array(payload_arr),
      checksum: wire_crc,
      signature,
    }
    Ok((frame, total_len))
  } else {
    Err(InvalidMagic(stx))
  }
}

///|
/// Streaming parser event emitted when feeding arbitrary byte chunks.
pub(all) enum ParseStreamEvent {
  FrameDecoded(MavFrame)
  FrameError(MavError)
} derive(Eq, Debug)

///|
pub extend ParseStreamEvent with Eq::{not_equal, equal}

///|
pub extend ParseStreamEvent with @debug.Debug::{to_repr}

///|
/// Stateful streaming MAVLink v1/v2 frame parser with noise recovery,
/// partial-packet buffering, and sequence-gap packet-drop tracking.
pub(all) struct FrameParser {
  buffer : Array[Byte]
  mut packets_received : UInt32
  mut packets_dropped : UInt32
  mut crc_errors : UInt32
  mut noise_bytes_skipped : UInt32
  mut last_seq : Int32
} derive(Eq, Debug)

///|
pub extend FrameParser with Eq::{not_equal, equal}

///|
pub extend FrameParser with @debug.Debug::{to_repr}

///|
/// Create a new streaming `FrameParser`.
pub fn FrameParser::new() -> FrameParser {
  {
    buffer: [],
    packets_received: 0U,
    packets_dropped: 0U,
    crc_errors: 0U,
    noise_bytes_skipped: 0U,
    last_seq: -1,
  }
}

///|
fn FrameParser::discard_prefix(self : FrameParser, count : Int32) -> Unit {
  if count <= 0 {
    return
  }
  let remaining = self.buffer.length() - count
  if remaining <= 0 {
    self.buffer.clear()
    return
  }
  for i = 0; i < remaining; i = i + 1 {
    self.buffer[i] = self.buffer[i + count]
  }
  while self.buffer.length() > remaining {
    let _ = self.buffer.pop()
  }
}

///|
/// Feed a chunk of incoming bytes (e.g. from UART or UDP) into the parser and return
/// all decoded frames and protocol errors encountered.
pub fn FrameParser::push_bytes(
  self : FrameParser,
  chunk : Bytes,
  crc_extra_lookup : (UInt32) -> Byte?,
  signing~ : SigningContext? = None,
) -> Array[ParseStreamEvent] {
  for i = 0; i < chunk.length(); i = i + 1 {
    self.buffer.push(chunk[i])
  }
  let events : Array[ParseStreamEvent] = []
  while self.buffer.length() > 0 {
    let stx = self.buffer[0]
    if stx != MAVLINK_STX_V1 && stx != MAVLINK_STX_V2 {
      self.noise_bytes_skipped = self.noise_bytes_skipped + 1U
      self.discard_prefix(1)
      continue
    }
    let snapshot = Bytes::from_array(self.buffer)
    match decode_frame(snapshot, crc_extra_lookup, signing~) {
      Ok((frame, consumed)) => {
        let cur_seq = frame.header.seq.to_int() & 0xFF
        if self.last_seq >= 0 {
          let expected_seq = (self.last_seq + 1) & 0xFF
          if cur_seq != expected_seq {
            let gap = (cur_seq - expected_seq + 256) & 0xFF
            self.packets_dropped = self.packets_dropped +
              gap.reinterpret_as_uint()
          }
        }
        self.last_seq = cur_seq
        self.packets_received = self.packets_received + 1U
        self.discard_prefix(consumed)
        events.push(FrameDecoded(frame))
      }
      Err(IncompleteFrame(_, _)) => break
      Err(CrcMismatch(expected, actual)) => {
        self.crc_errors = self.crc_errors + 1U
        self.discard_prefix(1)
        events.push(FrameError(CrcMismatch(expected, actual)))
      }
      Err(other_err) => {
        self.discard_prefix(1)
        events.push(FrameError(other_err))
      }
    }
  }
  events
}