rustsim_spaces/

link.rs

//! Generic link-based network space.
//!
//! [`LinkSpace<P>`] represents a directed network where agents move along links
//! (edges) rather than residing at nodes. The space is intentionally domain-
//! neutral: it models topology, FIFO occupancy, link blocking, and agent
//! transfer between links, while parameterizing per-link semantics via `P`.
//!
//! Transport-specific semantics such as speed-density relationships and lane
//! counts live in `rustsim-traffic` and can use `LinkSpace<LinkProperties>`.

use rustsim_core::{
    space::Space,
    types::{AgentId, EdgeId, NodeId},
};
use std::collections::HashMap;
use thiserror::Error;

/// Unique identifier for a link (directed edge) in the network.
pub type LinkId = EdgeId;

/// Errors returned by link-geometry validation.
#[derive(Debug, Clone, Copy, PartialEq, Error)]
pub enum LinkGeometryError {
    /// Link length must be strictly positive.
    #[error("link length must be positive")]
    InvalidLength,
}

/// Minimal geometric properties of a generic link.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct LinkGeometry {
    /// Link length in meters.
    pub length: f64,
}

impl LinkGeometry {
    /// Create generic link geometry from a length in meters.
    pub fn new(length: f64) -> Result<Self, LinkGeometryError> {
        if length <= 0.0 {
            return Err(LinkGeometryError::InvalidLength);
        }
        Ok(Self { length })
    }
}

/// Errors returned by link space operations.
#[derive(Debug, Clone, PartialEq, Error)]
pub enum LinkSpaceError {
    /// The node index is out of range.
    #[error("invalid node index {0}")]
    InvalidNode(NodeId),
    /// The link index is out of range.
    #[error("invalid link index {0}")]
    InvalidLink(LinkId),
    /// Agent is not on any link.
    #[error("agent {0} not found on any link")]
    AgentNotFound(AgentId),
    /// Agent already exists on a link.
    #[error("agent {0} already exists on link {1}")]
    AgentAlreadyOnLink(AgentId, LinkId),
}

#[derive(Debug, Clone, Copy)]
struct AgentOnLink {
    link_id: LinkId,
    position: f64,
}

/// Internal link data.
#[derive(Debug, Clone)]
struct LinkData<P> {
    from: NodeId,
    to: NodeId,
    geometry: LinkGeometry,
    properties: P,
    agents: Vec<AgentId>,
    exit_blocked: bool,
}

/// Generic link-based network space.
#[derive(Debug, Clone)]
pub struct LinkSpace<P = ()> {
    nodes: Vec<NodeData>,
    links: Vec<LinkData<P>>,
    agent_state: HashMap<AgentId, AgentOnLink>,
}

#[derive(Debug, Clone, Default)]
struct NodeData {
    outgoing: Vec<LinkId>,
    incoming: Vec<LinkId>,
}

impl<P> LinkSpace<P> {
    /// Create an empty link space with no nodes or links.
    pub fn new() -> Self {
        Self {
            nodes: Vec::new(),
            links: Vec::new(),
            agent_state: HashMap::new(),
        }
    }

    /// Number of nodes in the network.
    pub fn num_nodes(&self) -> usize {
        self.nodes.len()
    }

    /// Number of links in the network.
    pub fn num_links(&self) -> usize {
        self.links.len()
    }

    /// Add a node and return its index.
    pub fn add_node(&mut self) -> NodeId {
        let id = self.nodes.len();
        self.nodes.push(NodeData::default());
        id
    }

    /// Add a directed link from `from` to `to` with geometry and per-link properties.
    pub fn add_link(
        &mut self,
        from: NodeId,
        to: NodeId,
        geometry: LinkGeometry,
        properties: P,
    ) -> Result<LinkId, LinkSpaceError> {
        if from >= self.nodes.len() {
            return Err(LinkSpaceError::InvalidNode(from));
        }
        if to >= self.nodes.len() {
            return Err(LinkSpaceError::InvalidNode(to));
        }

        let link_id = self.links.len();
        self.links.push(LinkData {
            from,
            to,
            geometry,
            properties,
            agents: Vec::new(),
            exit_blocked: false,
        });
        self.nodes[from].outgoing.push(link_id);
        self.nodes[to].incoming.push(link_id);
        Ok(link_id)
    }

    /// Immutable access to link geometry.
    pub fn link_geometry(&self, link_id: LinkId) -> Option<&LinkGeometry> {
        self.links.get(link_id).map(|l| &l.geometry)
    }

    /// Mutable access to link geometry.
    pub fn link_geometry_mut(&mut self, link_id: LinkId) -> Option<&mut LinkGeometry> {
        self.links.get_mut(link_id).map(|l| &mut l.geometry)
    }

    /// Immutable access to per-link properties.
    pub fn link_properties(&self, link_id: LinkId) -> Option<&P> {
        self.links.get(link_id).map(|l| &l.properties)
    }

    /// Mutable access to per-link properties.
    pub fn link_properties_mut(&mut self, link_id: LinkId) -> Option<&mut P> {
        self.links.get_mut(link_id).map(|l| &mut l.properties)
    }

    /// Source and destination nodes of a link as `(from, to)`.
    pub fn link_endpoints(&self, link_id: LinkId) -> Option<(NodeId, NodeId)> {
        self.links.get(link_id).map(|l| (l.from, l.to))
    }

    /// Outgoing link IDs from a node.
    pub fn outgoing_links(&self, node: NodeId) -> &[LinkId] {
        &self.nodes[node].outgoing
    }

    /// Incoming link IDs to a node.
    pub fn incoming_links(&self, node: NodeId) -> &[LinkId] {
        &self.nodes[node].incoming
    }

    /// Number of agents currently on a link.
    pub fn agents_on_link(&self, link_id: LinkId) -> usize {
        self.links.get(link_id).map_or(0, |l| l.agents.len())
    }

    /// Agent IDs currently on a link, ordered by position (upstream first).
    pub fn agent_ids_on_link(&self, link_id: LinkId) -> &[AgentId] {
        self.links
            .get(link_id)
            .map_or(&[] as &[AgentId], |l| &l.agents)
    }

    /// Link length in meters.
    pub fn link_length(&self, link_id: LinkId) -> Option<f64> {
        self.link_geometry(link_id).map(|g| g.length)
    }

    /// Set whether a link's exit is blocked by downstream logic.
    pub fn set_link_exit_blocked(&mut self, link_id: LinkId, blocked: bool) {
        if let Some(link) = self.links.get_mut(link_id) {
            link.exit_blocked = blocked;
        }
    }

    /// Whether a link's exit is currently blocked.
    pub fn link_exit_blocked(&self, link_id: LinkId) -> bool {
        self.links.get(link_id).is_some_and(|l| l.exit_blocked)
    }

    /// Get an agent's current link and position.
    pub fn agent_position(&self, id: AgentId) -> Option<(LinkId, f64)> {
        self.agent_state.get(&id).map(|s| (s.link_id, s.position))
    }

    /// Place an agent on a link at a specific position.
    pub fn add_agent_to_link(
        &mut self,
        id: AgentId,
        link_id: LinkId,
        position: f64,
    ) -> Result<(), LinkSpaceError> {
        if link_id >= self.links.len() {
            return Err(LinkSpaceError::InvalidLink(link_id));
        }
        if self.agent_state.contains_key(&id) {
            return Err(LinkSpaceError::AgentAlreadyOnLink(id, link_id));
        }

        let length = self.links[link_id].geometry.length;
        let pos = position.clamp(0.0, length);

        self.agent_state.insert(
            id,
            AgentOnLink {
                link_id,
                position: pos,
            },
        );

        let link = &mut self.links[link_id];
        let insert_idx = link
            .agents
            .iter()
            .position(|&aid| match self.agent_state.get(&aid) {
                None => true,
                Some(state) => state.position > pos,
            })
            .unwrap_or(link.agents.len());
        link.agents.insert(insert_idx, id);

        Ok(())
    }

    /// Remove an agent from the network.
    pub fn remove_agent(&mut self, id: AgentId) -> Result<(), LinkSpaceError> {
        let state = self
            .agent_state
            .remove(&id)
            .ok_or(LinkSpaceError::AgentNotFound(id))?;
        let link = &mut self.links[state.link_id];
        if let Some(i) = link.agents.iter().position(|&a| a == id) {
            link.agents.remove(i);
        }
        Ok(())
    }

    /// Advance an agent's position on its current link by `distance` meters.
    pub fn advance_agent(&mut self, id: AgentId, distance: f64) -> Result<f64, LinkSpaceError> {
        let state = self
            .agent_state
            .get(&id)
            .copied()
            .ok_or(LinkSpaceError::AgentNotFound(id))?;

        let link = &self.links[state.link_id];
        let length = link.geometry.length;
        let my_idx = link.agents.iter().position(|&a| a == id).expect(
            "invariant: agent present in agent_state must also appear on its link's agent list",
        );

        let max_pos = if my_idx + 1 < link.agents.len() {
            let ahead_id = link.agents[my_idx + 1];
            let ahead_pos = self.agent_state[&ahead_id].position;
            (ahead_pos - 5.0).max(state.position)
        } else {
            length
        };

        let new_pos = (state.position + distance).min(max_pos).min(length);
        self.agent_state
            .get_mut(&id)
            .expect("invariant: agent existed at function entry and has not been removed")
            .position = new_pos;
        Ok(new_pos)
    }

    /// Check if an agent has reached the downstream end of its link.
    pub fn agent_at_link_end(&self, id: AgentId) -> bool {
        if let Some(state) = self.agent_state.get(&id) {
            let length = self.links[state.link_id].geometry.length;
            state.position >= length - 0.01
        } else {
            false
        }
    }

    /// Transfer an agent from the end of its current link to the start of a new link.
    pub fn transfer_agent(&mut self, id: AgentId, to_link: LinkId) -> Result<(), LinkSpaceError> {
        let state = self
            .agent_state
            .get(&id)
            .copied()
            .ok_or(LinkSpaceError::AgentNotFound(id))?;

        if to_link >= self.links.len() {
            return Err(LinkSpaceError::InvalidLink(to_link));
        }

        let current_to = self.links[state.link_id].to;
        let next_from = self.links[to_link].from;

        if current_to != next_from {
            return Err(LinkSpaceError::InvalidLink(to_link));
        }

        let old_link = &mut self.links[state.link_id];
        if let Some(i) = old_link.agents.iter().position(|&a| a == id) {
            old_link.agents.remove(i);
        }

        let entry = self
            .agent_state
            .get_mut(&id)
            .expect("invariant: agent existed at function entry and has not been removed");
        entry.link_id = to_link;
        entry.position = 0.0;

        let new_link = &mut self.links[to_link];
        new_link.agents.insert(0, id);

        Ok(())
    }

    /// Total number of agents in the network.
    pub fn total_agents(&self) -> usize {
        self.agent_state.len()
    }

    /// All agent IDs in the network.
    pub fn all_agent_ids(&self) -> Vec<AgentId> {
        self.agent_state.keys().copied().collect()
    }
}

impl<P> Default for LinkSpace<P> {
    fn default() -> Self {
        Self::new()
    }
}

impl<P: Send + Sync> Space for LinkSpace<P> {}

#[cfg(test)]
mod tests {
    use super::*;

    fn simple_network() -> LinkSpace<()> {
        let mut space = LinkSpace::new();
        let a = space.add_node();
        let b = space.add_node();
        let c = space.add_node();
        space
            .add_link(a, b, LinkGeometry::new(500.0).unwrap(), ())
            .unwrap();
        space
            .add_link(b, c, LinkGeometry::new(300.0).unwrap(), ())
            .unwrap();
        space
    }

    #[test]
    fn network_topology() {
        let space = simple_network();
        assert_eq!(space.num_nodes(), 3);
        assert_eq!(space.num_links(), 2);
        assert_eq!(space.link_endpoints(0), Some((0, 1)));
        assert_eq!(space.link_endpoints(1), Some((1, 2)));
        assert_eq!(space.outgoing_links(0), &[0]);
        assert_eq!(space.incoming_links(1), &[0]);
        assert_eq!(space.link_length(0), Some(500.0));
    }

    #[test]
    fn add_and_remove_agent() {
        let mut space = simple_network();
        space.add_agent_to_link(1, 0, 100.0).unwrap();

        assert_eq!(space.agents_on_link(0), 1);
        assert_eq!(space.agent_position(1), Some((0, 100.0)));

        space.remove_agent(1).unwrap();
        assert_eq!(space.agents_on_link(0), 0);
        assert!(space.agent_position(1).is_none());
    }

    #[test]
    fn advance_agent_basic() {
        let mut space = simple_network();
        space.add_agent_to_link(1, 0, 0.0).unwrap();

        let new_pos = space.advance_agent(1, 50.0).unwrap();
        assert!((new_pos - 50.0).abs() < 1e-9);

        let new_pos = space.advance_agent(1, 1000.0).unwrap();
        assert!((new_pos - 500.0).abs() < 1e-9);
    }

    #[test]
    fn fifo_ordering() {
        let mut space = simple_network();
        space.add_agent_to_link(1, 0, 100.0).unwrap();
        space.add_agent_to_link(2, 0, 200.0).unwrap();

        let new_pos = space.advance_agent(1, 200.0).unwrap();
        assert!(new_pos <= 195.0 + 1e-9);
    }

    #[test]
    fn transfer_agent() {
        let mut space = simple_network();
        space.add_agent_to_link(1, 0, 490.0).unwrap();

        space.advance_agent(1, 100.0).unwrap();
        assert!(space.agent_at_link_end(1));

        space.transfer_agent(1, 1).unwrap();
        assert_eq!(space.agent_position(1), Some((1, 0.0)));
        assert_eq!(space.agents_on_link(0), 0);
        assert_eq!(space.agents_on_link(1), 1);
    }

    #[test]
    fn exit_blocked_flag() {
        let mut space = simple_network();
        assert!(!space.link_exit_blocked(0));
        space.set_link_exit_blocked(0, true);
        assert!(space.link_exit_blocked(0));
    }

    #[test]
    fn agent_already_on_link_error() {
        let mut space = simple_network();
        space.add_agent_to_link(1, 0, 0.0).unwrap();
        let err = space.add_agent_to_link(1, 0, 50.0).unwrap_err();
        assert_eq!(err, LinkSpaceError::AgentAlreadyOnLink(1, 0));
    }

    #[test]
    fn agent_not_found_error() {
        let mut space = simple_network();
        let err = space.advance_agent(99, 10.0).unwrap_err();
        assert_eq!(err, LinkSpaceError::AgentNotFound(99));
    }
}