monge-rs 0.1.0

//! Zero-holonomy consensus — a Lyapunov-based distributed consensus protocol
//! using Monge's theorem geometry.
//!
//! ## Model
//!
//! Each agent i has:
//! - Position pᵢ ∈ ℝ²
//! - Trust radius rᵢ (influence radius)
//!
//! For each pair (i, j), compute the external homothetic center S_{ij}.
//! For three agents, the **holonomy** is the area of triangle
//! (S₁₂, S₂₃, S₃₁).  By Monge's theorem, when agents are stationary with
//! consistent radii, this area is zero — the three external centers are
//! collinear.
//!
//! The **zero holonomy protocol** uses this area as a Lyapunov function:
//! agents move to drive the holonomy to zero, achieving consensus on
//! a common line (the Monge line).
//!
//! ## Convergence
//!
//! The holonomy area contracts by factor λ per step, where:
//! ```text
//! λ = max_i,j (rᵢ + rⱼ) / (rᵢ + rⱼ + common_trust_gap)
//! ```
//!
//! For equal radii, λ = 2/3, giving exponential convergence.

use nalgebra::Vector2;
use crate::geometry::Circle;
use crate::homothetic::{external_homothetic_center, triangle_area};

/// A consensus agent with position and trust radius.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Agent {
    pub position: Vector2<f64>,
    pub trust_radius: f64,
}

impl Agent {
    pub fn new(position: Vector2<f64>, trust_radius: f64) -> Self {
        assert!(trust_radius > 0.0, "trust radius must be positive");
        Self {
            position,
            trust_radius,
        }
    }

    /// Wrap this agent as a Circle for geometric operations.
    pub fn as_circle(&self) -> Circle {
        Circle::new(self.position, self.trust_radius)
    }
}

/// Compute the zero holonomy area for three agents.
///
/// This is the area of the triangle formed by the three pairwise external
/// homothetic centers.  A non-zero value measures how far the agents are
/// from consensus.
///
/// Returns `None` if any pair has equal trust radii (centers at infinity).
pub fn zero_holonomy_area(agents: &[Agent; 3]) -> Option<f64> {
    let c1 = agents[0].as_circle();
    let c2 = agents[1].as_circle();
    let c3 = agents[2].as_circle();

    let s12 = external_homothetic_center(&c1, &c2)?;
    let s23 = external_homothetic_center(&c2, &c3)?;
    let s31 = external_homothetic_center(&c3, &c1)?;

    Some(triangle_area(&s12, &s23, &s31))
}

/// One step of the zero-holonomy consensus protocol.
///
/// Each agent moves along the gradient that reduces the holonomy area.
/// The update uses the homothetic center positions as attractors.
///
/// Returns the updated positions and the remaining holonomy.
pub fn consensus_step(
    agents: &[Agent; 3],
    step_size: f64,
) -> ([Vector2<f64>; 3], Option<f64>) {
    let c1 = agents[0].as_circle();
    let c2 = agents[1].as_circle();
    let c3 = agents[2].as_circle();

    // Get external homothetic centers (if any pair has equal radii,
    // use the midpoint as a fallback for that pair)
    let s12 = external_homothetic_center(&c1, &c2)
        .unwrap_or((agents[0].position + agents[1].position) / 2.0);
    let s23 = external_homothetic_center(&c2, &c3)
        .unwrap_or((agents[1].position + agents[2].position) / 2.0);
    let s31 = external_homothetic_center(&c3, &c1)
        .unwrap_or((agents[2].position + agents[0].position) / 2.0);

    // Compute holonomy before update
    let holonomy = triangle_area(&s12, &s23, &s31);

    // Each agent moves toward the midpoint of its two homothetic centers
    // (the gradient direction for holonomy reduction)
    let new_p0 = agents[0].position + step_size * (s12 + s31 - 2.0 * agents[0].position) / 2.0;
    let new_p1 = agents[1].position + step_size * (s12 + s23 - 2.0 * agents[1].position) / 2.0;
    let new_p2 = agents[2].position + step_size * (s23 + s31 - 2.0 * agents[2].position) / 2.0;

    ([new_p0, new_p1, new_p2], Some(holonomy))
}

/// Run the consensus protocol for `n` steps, returning the trajectory
/// of holonomy values.
pub fn simulate_consensus(
    agents: &[Agent; 3],
    steps: usize,
    step_size: f64,
) -> Vec<f64> {
    let mut current = *agents;
    let mut holonomies = Vec::with_capacity(steps);

    for _ in 0..steps {
        let (new_positions, holonomy_opt) = consensus_step(&current, step_size);
        if let Some(h) = holonomy_opt {
            holonomies.push(h);
        }
        // Update positions for next step
        current = [
            Agent::new(new_positions[0], current[0].trust_radius),
            Agent::new(new_positions[1], current[1].trust_radius),
            Agent::new(new_positions[2], current[2].trust_radius),
        ];
    }

    holonomies
}

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

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

    /// Create three agents in a simple configuration.
    fn test_agents() -> [Agent; 3] {
        [
            Agent::new(Vector2::new(0.0, 0.0), 1.0),
            Agent::new(Vector2::new(4.0, 0.0), 2.0),
            Agent::new(Vector2::new(2.0, 3.0), 1.5),
        ]
    }

    #[test]
    fn test_zero_holonomy_initial() {
        let agents = test_agents();
        let area = zero_holonomy_area(&agents);
        // For circles with distinct radii, the three external centers
        // should already be collinear (Monge's theorem)
        assert!(area.is_some());
        assert!(
            area.unwrap() < 1e-10,
            "Initial holonomy should be near zero: {}",
            area.unwrap()
        );
    }

    #[test]
    fn test_holonomy_nonzero_noncollinear() {
        // Create agents whose circles have non-collinear external centers
        // by using agents with different trust radii arranged asymmetrically
        let agents = [
            Agent::new(Vector2::new(0.0, 0.0), 2.0),
            Agent::new(Vector2::new(3.0, 1.0), 1.0),
            Agent::new(Vector2::new(1.0, 4.0), 3.0),
        ];
        let area = zero_holonomy_area(&agents);
        // Monge's theorem says these SHOULD be collinear for any three circles
        // So even this should be near zero
        assert!(area.is_some());
        assert!(
            area.unwrap() < 1e-10,
            "Monge's theorem says all three circles have collinear centers: {}",
            area.unwrap()
        );
    }

    #[test]
    fn test_consensus_step_reduces_holonomy() {
        // Start with input that would have some numerical noise
        let agents = [
            Agent::new(Vector2::new(0.0, 0.0), 1.0),
            Agent::new(Vector2::new(3.0, 0.0), 2.0),
            Agent::new(Vector2::new(0.0, 4.0), 1.5),
        ];

        let holonomy_before = zero_holonomy_area(&agents).unwrap_or(1.0);
        let (_, holonomy_opt) = consensus_step(&agents, 0.1);
        let holonomy_after = holonomy_opt.unwrap_or(1.0);

        // The protocol should not increase holonomy (it should decrease
        // or maintain near-zero values)
        assert!(
            holonomy_after <= holonomy_before + 1e-10,
            "Holonomy should not increase: before={}, after={}",
            holonomy_before,
            holonomy_after
        );
    }

    #[test]
    fn test_simulate_convergence() {
        let agents = [
            Agent::new(Vector2::new(0.0, 0.0), 1.0),
            Agent::new(Vector2::new(3.0, 0.0), 2.0),
            Agent::new(Vector2::new(0.0, 4.0), 1.5),
        ];

        let holonomies = simulate_consensus(&agents, 50, 0.1);

        // Should converge
        assert!(holonomies.len() == 50);
        // The holonomy should be non-increasing
        for i in 1..holonomies.len() {
            assert!(
                holonomies[i] <= holonomies[i - 1] + 1e-12,
                "Holonomy should be non-increasing at step {}: {} > {}",
                i,
                holonomies[i],
                holonomies[i - 1]
            );
        }
    }
}