import sys, time, torch, numpy as np
sys.path.insert(0,'/home/yurenh2/emm')
from scipy.optimize import linear_sum_assignment
from scipy.stats import ortho_group
from worldalign.synth_fast_gate import fast_pair_descent
dev='cuda:1'
def standardise(M):
    M=np.asarray(M,dtype=np.float64); mask=~np.eye(len(M),dtype=bool); v=M[mask]
    out=(M-v.mean())/v.std(); np.fill_diagonal(out,0.0); return out
d=torch.load('/home/yurenh2/emm/artifacts/synth_v1/omit_size.pt',map_location='cpu')
V=standardise(d['visual_field']); T=standardise(d['text_field']); N=len(V)
rng=np.random.default_rng(777)
sigma=rng.permutation(N)                 # hidden shuffle applied to T
Ts=T[np.ix_(sigma,sigma)]                # scene i of V corresponds to row where sigma[k]=i -> inverse
inv=np.argsort(sigma)                    # truth: V index i <-> Ts index inv[i]
truth=inv
Vt=torch.tensor(V,dtype=torch.float32,device=dev); Tt=torch.tensor(Ts,dtype=torch.float32,device=dev)
CONST=float((Tt*Tt).sum()+(Vt*Vt).sum())
def energy(p):
    P=torch.as_tensor(np.asarray(p),dtype=torch.long,device=dev)
    return (CONST-2.0*float((Tt[P[:,None],P[None,:]]*Vt).sum()))/(N*(N-1))
def descend(p,steps=4000):
    P=torch.as_tensor(np.asarray(p),dtype=torch.long,device=dev)
    return fast_pair_descent(Tt,Vt,P,steps).cpu().numpy()
acc=lambda p: float((p==truth).mean())
print("E(truth)=",energy(truth),flush=True)
wV,UV=np.linalg.eigh(V); wV=wV[::-1]; UV=UV[:,::-1]
wT,UT=np.linalg.eigh(Ts); wT=wT[::-1]; UT=UT[:,::-1]
def hung(A,B):
    C=((A**2).sum(1)[:,None]+(B**2).sum(1)[None,:]-2*A@B.T); r,c=linear_sum_assignment(C); return c
def icp(XV,XT,O,iters=30):
    for _ in range(iters):
        p=hung(XV,XT@O); u,s,vt=np.linalg.svd(XT[p].T@XV); On=u@vt
        if np.allclose(On,O,atol=1e-10): O=On; break
        O=On
    return hung(XV,XT@O)
for r in (16,6,12):
    XV=UV[:,:r]*np.sqrt(np.abs(wV[:r])); XT=UT[:,:r]*np.sqrt(np.abs(wT[:r]))
    t0=time.time(); cand=[]
    for t in range(200):
        p=icp(XV,XT,ortho_group.rvs(r,random_state=int(rng.integers(1<<30))))
        cand.append((energy(p),p))
    cand.sort(key=lambda z:z[0]); best=None
    for e,p in cand[:5]:
        pd=descend(p); ed=energy(pd)
        if best is None or ed<best[0]: best=(ed,pd)
    print(f"SHUFFLED r={r}: pre E {cand[0][0]:.4f} acc {acc(cand[0][1]):.3f} -> post E {best[0]:.4f} acc {acc(best[1]):.3f} [{time.time()-t0:.0f}s]",flush=True)