15_train_crossing_demo.py

import os
import sys
from pathlib import Path

import matplotlib.pyplot as plt
import numpy as np

# Add src to path
sys.path.append(str(Path(__file__).parent.parent.parent / "src"))

from tnfr.dynamics.symplectic import TNFRSymplecticIntegrator


def ensure_dir(path):
    if not os.path.exists(path):
        os.makedirs(path)


def zero_force(q):
    """
    Represents a 'Free Particle' regime in TNFR.
    Delta NFR = 0 (No structural pressure).
    The node propagates with constant momentum (Coherence Inertia).
    """
    return np.zeros_like(q)


def run_train_crossing_demo():
    print("--- TNFR Classical Kinematics: The Two Trains Problem ---")

    # Problem Parameters
    # Train A: Madrid -> Barcelona
    # Train B: Barcelona -> Madrid
    DISTANCE_KM = 600.0
    SPEED_A_KMH = 300.0  # AVE
    SPEED_B_KMH = 250.0  # Alvia

    # Convert to SI units (meters, seconds) for the physics engine
    # Though the engine is unit-agnostic, consistency helps
    dist_m = DISTANCE_KM * 1000.0
    v_a_ms = SPEED_A_KMH / 3.6
    v_b_ms = -SPEED_B_KMH / 3.6  # Moving left

    print(f"Distance: {dist_m/1000:.1f} km")
    print(f"Train A Speed: {v_a_ms:.2f} m/s")
    print(f"Train B Speed: {v_b_ms:.2f} m/s")

    # Analytical Solution
    # t = d / (v1 + v2)
    # x = v1 * t
    t_analytical = dist_m / (v_a_ms + abs(v_b_ms))
    x_analytical = v_a_ms * t_analytical

    print(f"\nAnalytical Prediction:")
    print(f"Time to cross: {t_analytical:.2f} s ({t_analytical/60:.2f} min)")
    print(f"Crossing point: {x_analytical/1000:.2f} km from Madrid")

    # --- TNFR Simulation ---

    # Initialize Nodes
    # State vector q = [x, y] (we only use x)
    q_a = np.array([0.0, 0.0])
    p_a = np.array([v_a_ms, 0.0])  # Momentum (assuming mass=1 for simplicity)

    q_b = np.array([dist_m, 0.0])
    p_b = np.array([v_b_ms, 0.0])

    # Integrator
    # Mass = 1.0 (Inverse Structural Frequency nu_f = 1.0)
    # The current implementation of TNFRSymplecticIntegrator is a static class or uses node objects.
    # Let's check how we used it in the classical mechanics demo.
    # It seems I implemented a standalone version there or the class has changed.
    # Let's implement a simple standalone Verlet here to avoid dependency issues if the class signature is different.

    class SimpleVerlet:
        def __init__(self, mass=1.0):
            self.mass = mass

        def velocity_verlet(self, q, p, force_func, dt):
            # p = m * v -> v = p / m
            v = p / self.mass
            f = force_func(q)
            a = f / self.mass

            # Half kick
            v_half = v + 0.5 * a * dt

            # Drift
            q_new = q + v_half * dt

            # Re-eval force
            f_new = force_func(q_new)
            a_new = f_new / self.mass

            # Half kick
            v_new = v_half + 0.5 * a_new * dt

            # Return new q, new p
            return q_new, v_new * self.mass

    integrator = SimpleVerlet(mass=1.0)

    dt = 1.0  # 1 second steps
    time = 0.0

    history_a = []
    history_b = []
    times = []

    print(f"\nRunning Nodal Dynamics Simulation (dt={dt}s)...")

    crossing_detected = False
    crossing_time = 0.0
    crossing_pos = 0.0

    # Run loop
    # We run a bit past the expected time to show the crossing
    max_steps = int(t_analytical * 1.2)

    for step in range(max_steps):
        # Store history
        history_a.append(q_a[0])
        history_b.append(q_b[0])
        times.append(time)

        # Check crossing
        # If A was behind B and now is ahead (or equal)
        if not crossing_detected and q_a[0] >= q_b[0]:
            crossing_detected = True
            # Linear interpolation for precise time
            # xA_prev, xB_prev at t-dt
            # xA_curr, xB_curr at t
            # Find tau where xA(tau) = xB(tau)

            xA_prev = history_a[-2]
            xB_prev = history_b[-2]
            xA_curr = q_a[0]
            xB_curr = q_b[0]

            # Relative distance D(t) = xB - xA
            # D_prev = xB_prev - xA_prev (> 0)
            # D_curr = xB_curr - xA_curr (<= 0)
            # Fraction f = D_prev / (D_prev - D_curr)

            D_prev = xB_prev - xA_prev
            D_curr = xB_curr - xA_curr
            fraction = D_prev / (D_prev - D_curr)

            crossing_time = time - dt + fraction * dt
            crossing_pos = xA_prev + v_a_ms * (fraction * dt)

            print(f"-> Crossing Detected at Step {step}!")

        # Evolve Nodes
        # Force = 0 (Inertial Motion)
        q_a, p_a = integrator.velocity_verlet(q_a, p_a, zero_force, dt)
        q_b, p_b = integrator.velocity_verlet(q_b, p_b, zero_force, dt)

        time += dt

    # --- Results ---
    print(f"\nTNFR Simulation Results:")
    print(f"Time to cross: {crossing_time:.2f} s")
    print(f"Crossing point: {crossing_pos/1000:.2f} km")

    error_t = abs(crossing_time - t_analytical)
    error_x = abs(crossing_pos - x_analytical)

    print(f"\nAccuracy:")
    print(f"Time Error: {error_t:.6f} s")
    print(f"Position Error: {error_x:.6f} m")

    if error_t < 1e-3:
        print("SUCCESS: Nodal Dynamics perfectly reproduces Classical Kinematics.")
    else:
        print("WARNING: Discrepancy detected.")

    # --- Visualization ---
    results_dir = Path("results/kinematics_demo")
    ensure_dir(results_dir)

    plt.figure(figsize=(10, 6))

    # Convert to km and min for plotting
    times_min = np.array(times) / 60.0
    pos_a_km = np.array(history_a) / 1000.0
    pos_b_km = np.array(history_b) / 1000.0

    plt.plot(
        times_min, pos_a_km, label="Train A (Madrid -> BCN)", color="blue", linewidth=2
    )
    plt.plot(
        times_min, pos_b_km, label="Train B (BCN -> Madrid)", color="red", linewidth=2
    )

    # Mark crossing
    cross_t_min = crossing_time / 60.0
    cross_x_km = crossing_pos / 1000.0

    plt.plot(cross_t_min, cross_x_km, "ko", markersize=10, label="Crossing Point")
    plt.annotate(
        f"  t={cross_t_min:.1f} min\n  x={cross_x_km:.1f} km",
        (cross_t_min, cross_x_km),
        xytext=(10, -20),
        textcoords="offset points",
    )

    plt.title("TNFR Kinematics: Two Trains Problem")
    plt.xlabel("Time (minutes)")
    plt.ylabel("Position (km)")
    plt.grid(True, alpha=0.3)
    plt.legend()

    plt.savefig(results_dir / "01_train_crossing.png")
    print(f"Saved plot to {results_dir / '01_train_crossing.png'}")


if __name__ == "__main__":
    run_train_crossing_demo()