///|
/// Learned routing table entry mapping a MAVLink `(sysid, compid)` address to a physical/logical link port.
pub(all) struct RouteEntry {
  sysid : Byte
  compid : Byte
  mut port_id : Int32
  mut frames_routed : Int32
} derive(Eq, Debug)

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

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

///|
/// Extract the destination `(target_system, target_component)` pair from a `MavMessage`
/// if the message type carries explicit target addressing; returns `None` for broadcast
/// telemetry messages (such as `HEARTBEAT`, `ATTITUDE`, `GPS_RAW_INT`, `STATUSTEXT`).
pub fn extract_message_target(msg : MavMessage) -> (Byte, Byte)? {
  match msg {
    MsgPing(m) => Some((m.target_system, m.target_component))
    MsgSetMode(m) => Some((m.target_system, b'\x00'))
    MsgParamRequestRead(m) => Some((m.target_system, m.target_component))
    MsgParamRequestList(m) => Some((m.target_system, m.target_component))
    MsgParamSet(m) => Some((m.target_system, m.target_component))
    MsgMissionWritePartialList(m) => Some((m.target_system, m.target_component))
    MsgMissionItem(m) => Some((m.target_system, m.target_component))
    MsgMissionRequest(m) => Some((m.target_system, m.target_component))
    MsgMissionSetCurrent(m) => Some((m.target_system, m.target_component))
    MsgMissionRequestList(m) => Some((m.target_system, m.target_component))
    MsgMissionCount(m) => Some((m.target_system, m.target_component))
    MsgMissionClearAll(m) => Some((m.target_system, m.target_component))
    MsgMissionAck(m) => Some((m.target_system, m.target_component))
    MsgMissionRequestInt(m) => Some((m.target_system, m.target_component))
    MsgManualControl(m) => Some((m.target, b'\x00'))
    MsgMissionItemInt(m) => Some((m.target_system, m.target_component))
    MsgCommandInt(m) => Some((m.target_system, m.target_component))
    MsgCommandLong(m) => Some((m.target_system, m.target_component))
    MsgCommandAck(m) => Some((m.target_system, m.target_component))
    MsgCommandCancel(m) => Some((m.target_system, m.target_component))
    MsgSetAttitudeTarget(m) => Some((m.target_system, m.target_component))
    MsgSetPositionTargetLocalNed(m) =>
      Some((m.target_system, m.target_component))
    MsgFileTransferProtocol(m) => Some((m.target_system, m.target_component))
    MsgTimesync(m) => Some((m.target_system, m.target_component))
    MsgLoggingData(m) => Some((m.target_system, m.target_component))
    MsgLoggingAck(m) => Some((m.target_system, m.target_component))
    MsgParamExtRequestRead(m) => Some((m.target_system, m.target_component))
    MsgParamExtRequestList(m) => Some((m.target_system, m.target_component))
    MsgParamExtSet(m) => Some((m.target_system, m.target_component))
    _ => None
  }
}

///|
/// Multi-link MAVLink v2 packet router (modeled after `mavlink-router` / `MAVProxy`)
/// with automatic source-address learning, unicast target resolution, and split-horizon loop prevention.
pub(all) struct MavRouter {
  ports : Array[Int32]
  routes : Array[RouteEntry]
  mut unicast_forwarded : Int32
  mut broadcast_flooded : Int32
} derive(Eq, Debug)

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

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

///|
/// Create a new `MavRouter` configured with a list of link port IDs (e.g. `[0, 1, 2]` for UART0, UART1, UDP0).
pub fn MavRouter::new(port_ids : Array[Int32]) -> MavRouter {
  let ports : Array[Int32] = []
  for p in port_ids {
    ports.push(p)
  }
  { ports, routes: [], unicast_forwarded: 0, broadcast_flooded: 0, }
}

///|
/// Learn or update the ingress `port_id` for `(sysid, compid)`.
pub fn MavRouter::learn_source(
  self : MavRouter,
  sysid : Byte,
  compid : Byte,
  port_id : Int32,
) -> Unit {
  for i = 0; i < self.routes.length(); i = i + 1 {
    if self.routes[i].sysid == sysid && self.routes[i].compid == compid {
      self.routes[i].port_id = port_id
      self.routes[i].frames_routed = self.routes[i].frames_routed + 1
      return ()
    }
  }
  self.routes.push({ sysid, compid, port_id, frames_routed: 1, })
}

///|
fn flood_other_ports(
  ports : Array[Int32],
  ingress_port : Int32,
) -> Array[Int32] {
  let out : Array[Int32] = []
  for p in ports {
    if p != ingress_port {
      out.push(p)
    }
  }
  out
}

///|
/// Compute the list of egress link port IDs to which an incoming `MavFrame` arriving on `ingress_port`
/// should be forwarded.
///
/// Rules:
/// 1. Records `(frame.header.sysid, frame.header.compid) -> ingress_port` in the routing table.
/// 2. If the message is broadcast (`target_system == 0` or untargeted telemetry), returns all ports except `ingress_port`.
/// 3. If the message targets a learned `(target_system, target_component)` (or `target_component == 0`),
///    forwards only to the matching egress port(s) (excluding `ingress_port`).
/// 4. If the target address has not yet been learned, falls back to flooding all non-ingress ports.
pub fn MavRouter::route_frame(
  self : MavRouter,
  ingress_port : Int32,
  frame : MavFrame,
) -> Array[Int32] {
  self.learn_source(frame.header.sysid, frame.header.compid, ingress_port)

  let target_opt = match MavMessage::decode_from_frame(frame) {
    Ok(msg) => extract_message_target(msg)
    Err(_) => None
  }

  match target_opt {
    None => {
      self.broadcast_flooded = self.broadcast_flooded + 1
      flood_other_ports(self.ports, ingress_port)
    }
    Some((target_sys, target_comp)) => {
      if target_sys == b'\x00' {
        self.broadcast_flooded = self.broadcast_flooded + 1
        return flood_other_ports(self.ports, ingress_port)
      }
      let matched_ports : Array[Int32] = []
      let mut target_known = false
      for r in self.routes {
        if r.sysid == target_sys &&
          (target_comp == b'\x00' || r.compid == target_comp) {
          target_known = true
          if r.port_id != ingress_port {
            let mut already = false
            for mp in matched_ports {
              if mp == r.port_id {
                already = true
                break
              }
            }
            if !already {
              matched_ports.push(r.port_id)
            }
          }
        }
      }
      if target_known {
        self.unicast_forwarded = self.unicast_forwarded + 1
        matched_ports
      } else {
        self.broadcast_flooded = self.broadcast_flooded + 1
        flood_other_ports(self.ports, ingress_port)
      }
    }
  }
}