"""Sandbox check of the linear-theory choice law (NOT a test point: theta = 0.004 and 0.02 at N = 1000).
Mean-field (TWA) trajectories of the two-beam model; P(yang) = fraction of samples whose beam-A transverse
displacement points to the same side as the noiseless trajectory, at the time t* where <|u|^2> = 0.1.
Prediction: P = Phi(kappa x), kappa = sqrt(2/mu), x = sin(2 theta) sqrt(N/2).
Slaving: v/u -> (alpha + i lambda)/beta, modulus 1, phase atan2(lambda, alpha)."""
import numpy as np
from math import erf, sqrt, atan2, degrees
Phi = lambda z: 0.5 * (1 + erf(z / sqrt(2)))
def rhs(a, b, B, mu):
    s = 0.5 * mu * (a + b)
    return np.cross(B + s, a), np.cross(-B + s, b)
def run(theta, N, mu, M=20000, dt=0.01, seed=1):
    rng = np.random.default_rng(seed); sg = np.sqrt(2.0 / N)
    B = np.array([np.sin(2 * theta), 0.0, np.cos(2 * theta)])
    a = np.zeros((M + 1, 3)); b = np.zeros((M + 1, 3)); a[:, 2] = 1; b[:, 2] = -1
    a[1:, :2] = rng.normal(0, sg, (M, 2)); b[1:, :2] = rng.normal(0, sg, (M, 2))   # row 0: noiseless
    t = 0.0
    while True:
        k1 = rhs(a, b, B, mu); k2 = rhs(a + dt/2*k1[0], b + dt/2*k1[1], B, mu)
        k3 = rhs(a + dt/2*k2[0], b + dt/2*k2[1], B, mu); k4 = rhs(a + dt*k3[0], b + dt*k3[1], B, mu)
        a = a + dt/6*(k1[0] + 2*k2[0] + 2*k3[0] + k4[0]); b = b + dt/6*(k1[1] + 2*k2[1] + 2*k3[1] + k4[1]); t += dt
        u = a[:, 0] + 1j * a[:, 1]; v = b[:, 0] + 1j * b[:, 1]
        # deviation from the fixed point (a* = B, b* = -B): u* = sin2theta, v* = -sin2theta
        du = u - np.sin(2*theta); dv = v + np.sin(2*theta)
        if np.mean(np.abs(du[1:])**2) >= 0.1: break
    d0 = du[0] / abs(du[0])
    P = np.mean(np.real(du[1:] * np.conj(d0)) > 0)
    r = np.mean(dv[1:] * np.conj(du[1:])) / np.mean(np.abs(du[1:])**2)
    return t, P, r
for theta, N in ((0.004, 1000), (0.02, 1000)):
    x = np.sin(2*theta) * np.sqrt(N/2)
    for mu in (1.5, 2.0, 2.5):
        lam = np.sqrt(mu - 1); al, be = 1 - mu/2, mu/2
        t, P, r = run(theta, N, mu)
        print(f"theta={theta} N={N} x={x:.3f} mu={mu}: t*={t:.2f}  P={P:.4f}  "
              f"Phi(sqrt(2/mu)x)={Phi(np.sqrt(2/mu)*x):.4f}  Phi(x)={Phi(x):.4f} | "
              f"|v/u|={abs(r):.3f} (pred 1)  phase={degrees(np.angle(r)):.1f} (pred {degrees(atan2(lam, al)):.1f})")
