rustsim-traffic 0.0.1

//! Explicit transport queue and control policies.
//!
//! These policies lift the small built-in link behavior into named,
//! configurable contracts. The default queue policy preserves the historical
//! FIFO/gap-limited behavior used by [`TransportLinkOps`](crate::TransportLinkOps).

use rustsim_core::types::AgentId;
use rustsim_spaces::link::{LinkId, LinkSpace, LinkSpaceError};

use crate::{LinkProperties, SignalPhase, SignalTiming, TrafficControlType, TransportLinkOps};

/// Why a policy constrained an agent's speed.
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum SpeedConstraint {
    /// No downstream or leader constraint applied.
    Unconstrained,
    /// A leading agent capped this agent's speed.
    Leader {
        /// Leading agent identifier.
        leader: AgentId,
        /// Current bumper-to-bumper gap used by the policy, in meters.
        gap_m: f64,
    },
    /// The downstream end of the link is blocked.
    BlockedExit {
        /// Remaining distance to the link end, in meters.
        remaining_m: f64,
    },
}

/// Speed chosen by a queue policy.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SpeedDecision {
    /// Selected speed in meters per second.
    pub speed: f64,
    /// Constraint that determined the selected speed.
    pub constraint: SpeedConstraint,
}

/// Policy for choosing an agent's link speed from ordered occupancy.
pub trait QueuePolicy {
    /// Compute the speed decision for `agent` in `space`.
    fn speed_for(
        &self,
        space: &LinkSpace<LinkProperties>,
        agent: AgentId,
    ) -> Result<SpeedDecision, LinkSpaceError>;
}

/// FIFO policy with a fixed gap to the leader and fixed exit clearance.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct FifoGapPolicy {
    /// Minimum gap maintained behind a leading agent, in meters.
    pub min_gap_m: f64,
    /// Clearance maintained before a blocked downstream exit, in meters.
    pub exit_clearance_m: f64,
}

impl Default for FifoGapPolicy {
    fn default() -> Self {
        Self {
            min_gap_m: 5.0,
            exit_clearance_m: 0.5,
        }
    }
}

impl FifoGapPolicy {
    /// Create a FIFO/gap policy with explicit distances in meters.
    pub fn new(min_gap_m: f64, exit_clearance_m: f64) -> Self {
        Self {
            min_gap_m,
            exit_clearance_m,
        }
    }
}

impl QueuePolicy for FifoGapPolicy {
    fn speed_for(
        &self,
        space: &LinkSpace<LinkProperties>,
        agent: AgentId,
    ) -> Result<SpeedDecision, LinkSpaceError> {
        let (link_id, position) = space
            .agent_position(agent)
            .ok_or(LinkSpaceError::AgentNotFound(agent))?;
        let link_speed = space.link_speed(link_id);
        let ids = space.agent_ids_on_link(link_id);
        let my_idx = ids.iter().position(|&id| id == agent).expect(
            "invariant: agent_position returned link_id whose agent list must contain the agent",
        );
        let min_gap_m = self.min_gap_m.max(0.0);
        let exit_clearance_m = self.exit_clearance_m.max(0.0);

        if my_idx + 1 < ids.len() {
            let leader = ids[my_idx + 1];
            let leader_pos = space
                .agent_position(leader)
                .expect("invariant: leader came from the same link's agent list and must resolve")
                .1;
            let gap_m = leader_pos - position;
            let safe_speed = (gap_m - min_gap_m).max(0.0);
            return Ok(SpeedDecision {
                speed: link_speed.min(safe_speed),
                constraint: SpeedConstraint::Leader { leader, gap_m },
            });
        }

        if space.link_exit_blocked(link_id) {
            let remaining_m = space.link_length(link_id).unwrap_or(0.0) - position;
            let safe_speed = (remaining_m - exit_clearance_m).max(0.0);
            return Ok(SpeedDecision {
                speed: link_speed.min(safe_speed),
                constraint: SpeedConstraint::BlockedExit { remaining_m },
            });
        }

        Ok(SpeedDecision {
            speed: link_speed,
            constraint: SpeedConstraint::Unconstrained,
        })
    }
}

/// Signal/control context for an approach or movement.
#[derive(Debug, Clone, Copy)]
pub struct ControlContext<'a> {
    /// Simulation time in seconds.
    pub sim_time_s: f64,
    /// Coarse traffic-control type for the movement.
    pub traffic_control: TrafficControlType,
    /// Optional fixed-time signal timing when `traffic_control` is signalized.
    pub signal_timing: Option<&'a SignalTiming>,
    /// Optional approach link identifier for downstream telemetry.
    pub approach_link: Option<LinkId>,
}

impl<'a> ControlContext<'a> {
    /// Create a control context at `sim_time_s`.
    pub fn new(sim_time_s: f64, traffic_control: TrafficControlType) -> Self {
        Self {
            sim_time_s,
            traffic_control,
            signal_timing: None,
            approach_link: None,
        }
    }

    /// Attach fixed-time signal timing.
    pub fn with_signal_timing(mut self, signal_timing: &'a SignalTiming) -> Self {
        self.signal_timing = Some(signal_timing);
        self
    }

    /// Attach an approach link identifier.
    pub fn with_approach_link(mut self, approach_link: LinkId) -> Self {
        self.approach_link = Some(approach_link);
        self
    }
}

/// Control decision for a movement.
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum ControlDecision {
    /// The movement may proceed now.
    Proceed,
    /// The movement must wait for the supplied duration.
    Hold {
        /// Time remaining before the movement may be reconsidered, in seconds.
        wait_s: f64,
    },
}

impl ControlDecision {
    /// True if the decision allows immediate movement.
    pub fn can_proceed(self) -> bool {
        matches!(self, Self::Proceed)
    }

    /// Waiting time implied by this decision, in seconds.
    pub fn wait_s(self) -> f64 {
        match self {
            Self::Proceed => 0.0,
            Self::Hold { wait_s } => wait_s,
        }
    }
}

/// Policy that decides whether a movement may proceed through a control point.
pub trait ControlPolicy {
    /// Evaluate a movement under the supplied context.
    fn decide(&self, context: ControlContext<'_>) -> ControlDecision;
}

/// Fixed-rule control policy for uncontrolled, yield, stop, and signal controls.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct FixedControlPolicy {
    /// Delay applied to stop-controlled movements before they may proceed.
    pub stop_delay_s: f64,
    /// Delay applied to yield-controlled movements before they may proceed.
    pub yield_delay_s: f64,
}

impl Default for FixedControlPolicy {
    fn default() -> Self {
        Self {
            stop_delay_s: 0.0,
            yield_delay_s: 0.0,
        }
    }
}

impl FixedControlPolicy {
    /// Create a fixed-rule control policy.
    pub fn new(stop_delay_s: f64, yield_delay_s: f64) -> Self {
        Self {
            stop_delay_s,
            yield_delay_s,
        }
    }
}

impl ControlPolicy for FixedControlPolicy {
    fn decide(&self, context: ControlContext<'_>) -> ControlDecision {
        match context.traffic_control {
            TrafficControlType::Uncontrolled => ControlDecision::Proceed,
            TrafficControlType::Yield => hold_or_proceed(self.yield_delay_s),
            TrafficControlType::Stop => hold_or_proceed(self.stop_delay_s),
            TrafficControlType::Signal => match context.signal_timing {
                Some(timing) => {
                    let (phase, remaining_s) = timing.phase_at(context.sim_time_s);
                    match phase {
                        SignalPhase::Green => ControlDecision::Proceed,
                        SignalPhase::Red => ControlDecision::Hold {
                            wait_s: remaining_s.max(0.0),
                        },
                    }
                }
                None => ControlDecision::Proceed,
            },
        }
    }
}

fn hold_or_proceed(delay_s: f64) -> ControlDecision {
    if delay_s > 0.0 {
        ControlDecision::Hold { wait_s: delay_s }
    } else {
        ControlDecision::Proceed
    }
}

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

    fn one_link_space() -> (LinkSpace<LinkProperties>, LinkId) {
        let mut space = LinkSpace::new();
        let a = space.add_node();
        let b = space.add_node();
        let (geom, props) = LinkProperties::urban(100.0, 36.0, 1).unwrap();
        let link = space.add_link(a, b, geom, props).unwrap();
        (space, link)
    }

    #[test]
    fn fifo_gap_policy_limits_trailing_agent_by_leader_gap() {
        let (mut space, link) = one_link_space();
        space.add_agent_to_link(1, link, 10.0).unwrap();
        space.add_agent_to_link(2, link, 18.0).unwrap();

        let decision = FifoGapPolicy::default().speed_for(&space, 1).unwrap();

        assert_eq!(decision.speed, 3.0);
        assert_eq!(
            decision.constraint,
            SpeedConstraint::Leader {
                leader: 2,
                gap_m: 8.0
            }
        );
    }

    #[test]
    fn default_transport_agent_speed_uses_fifo_gap_policy() {
        let (mut space, link) = one_link_space();
        space.add_agent_to_link(1, link, 10.0).unwrap();
        space.add_agent_to_link(2, link, 18.0).unwrap();

        assert_eq!(space.agent_speed(1).unwrap(), 3.0);
        assert_eq!(
            space
                .agent_speed_decision(1, &FifoGapPolicy::default())
                .unwrap()
                .speed,
            3.0
        );
    }

    #[test]
    fn blocked_exit_policy_limits_last_agent() {
        let (mut space, link) = one_link_space();
        space.add_agent_to_link(1, link, 98.0).unwrap();
        space.set_link_exit_blocked(link, true);

        let decision = FifoGapPolicy::default().speed_for(&space, 1).unwrap();

        assert_eq!(decision.speed, 1.5);
        assert_eq!(
            decision.constraint,
            SpeedConstraint::BlockedExit { remaining_m: 2.0 }
        );
    }

    #[test]
    fn fixed_control_policy_blocks_red_signal_and_releases_green() {
        let policy = FixedControlPolicy::default();
        let timing = SignalTiming::new(60.0, 0.0, vec![30.0]);

        let red = policy.decide(
            ControlContext::new(45.0, TrafficControlType::Signal).with_signal_timing(&timing),
        );
        assert_eq!(red, ControlDecision::Hold { wait_s: 15.0 });

        let green = policy.decide(
            ControlContext::new(5.0, TrafficControlType::Signal).with_signal_timing(&timing),
        );
        assert_eq!(green, ControlDecision::Proceed);
    }
}