"""
Circular-cross-section criterion (spec Category D "не подходит для сортировки без доупаковки").

K = r_inscribed / R_circumscribed for a cross-section outline; an object "has a circle in
section" when K > 0.8 in ANY of its principal cross-sections. Implemented so it works on a
(partial) point cloud measured under the dimension-estimation cameras: the circle CENTRE is
fit to the boundary (Kasa) so an occluded-bottom arc still reads correctly, and a low
circle-fit residual is required so an elongated rounded blob is not mistaken for a circle.

This module both (a) exposes `circular_section_K(points)` for the pipeline and (b) validates
it against ground truth on every mesh, including the ones that are NOT round (boxes, etc.).
Run inside Isaac Sim via isaacsim_send.py (reads /World/CVObjects mesh geometry).
"""
import omni.usd, numpy as np, json
from pxr import UsdGeom, Usd, Gf
stage=omni.usd.get_context().get_stage()
K_ROUND=0.80   # spec threshold

def _kasa(P):
    x,y=P[:,0],P[:,1]; A=np.c_[2*x,2*y,np.ones(len(x))]; b=x*x+y*y
    s,*_=np.linalg.lstsq(A,b,rcond=None); cx,cy,cc=s; r=np.sqrt(max(cc+cx*cx+cy*cy,1e-12))
    return cx,cy,r,np.abs(np.hypot(x-cx,y-cy)-r).mean()

def section_K(xy):
    """True r_inscribed/R_circumscribed of a full cross-section outline via the convex-hull
    incenter (largest inscribed circle) and the circumscribed radius from that centre.
    K=1 for a circle, b/a for an ellipse, 0.707 for a square, short/long for a rectangle."""
    from scipy.spatial import ConvexHull
    if len(xy)<20: return None,0.0,1.0
    try: h=ConvexHull(xy)
    except Exception: return None,0.0,1.0
    V=xy[h.vertices]                                   # CCW hull vertices
    A=V; B=np.roll(V,-1,axis=0); E=B-A; L=np.linalg.norm(E,axis=1)+1e-12
    mn=xy.min(0); mx=xy.max(0)
    G=np.stack(np.meshgrid(np.linspace(mn[0],mx[0],40),np.linspace(mn[1],mx[1],40)),-1).reshape(-1,2)
    # signed distance from each grid point to each hull edge (CCW -> interior side positive)
    d=(E[:,0][None,:]*(G[:,1][:,None]-A[:,1][None,:]) - E[:,1][None,:]*(G[:,0][:,None]-A[:,0][None,:]))/L[None,:]
    inside=(d>0).all(1)
    if inside.sum()<3: return 0.0,1.0,1.0
    rin=float(d[inside].min(1).max())                          # max inscribed circle radius (its own centre)
    # min enclosing circle radius (its own centre): grid centre minimising max distance to hull vertices
    Gd=np.stack(np.meshgrid(np.linspace(mn[0],mx[0],48),np.linspace(mn[1],mx[1],48)),-1).reshape(-1,2)
    Rout=float(np.linalg.norm(Gd[:,None,:]-V[None,:,:],axis=2).max(1).min())
    return rin/max(Rout,1e-9),1.0,0.0

def circular_section_K(points):
    """Max K over cross-sections sampled along each principal axis (a circle in ANY section
    -> round). Returns (max_K, is_round, best_section)."""
    if len(points)<60: return 0.0,False,None
    c=points.mean(0); Q=points-c; _,_,V=np.linalg.svd(Q,full_matrices=False); proj=Q@V.T
    best=0.0; best_sec=None
    for a in range(3):
        o=[i for i in range(3) if i!=a]; ca=proj[:,a]; sp=np.ptp(ca)+1e-9
        for frac in (0.25,0.375,0.5,0.625,0.75):                 # sample slices along the axis
            lvl=np.percentile(ca,frac*100)
            sl=proj[np.abs(ca-lvl)<0.07*sp][:,o]
            K,cov,rr=section_K(sl)
            if K is not None and rr<0.15 and K>best:
                best=K; best_sec=(a,round(frac,2),round(cov,2),rr)
    return round(best,3), (best>K_ROUND), best_sec

# ---------- validate on all meshes (full GT geometry) ----------
def mesh_points(nm, nsamp=40000):
    """Dense, uniform surface sample (barycentric) so thin cross-sections are well populated."""
    root=stage.GetPrimAtPath(f"/World/CVObjects/{nm}"); rng=np.random.default_rng(0); out=[]
    for m in Usd.PrimRange(root):
        if m.GetTypeName()!="Mesh": continue
        P=np.array(UsdGeom.Mesh(m).GetPointsAttr().Get(),dtype=np.float64)
        idx=np.array(UsdGeom.Mesh(m).GetFaceVertexIndicesAttr().Get())
        if len(idx)%3: continue
        tris=P[idx].reshape(-1,3,3); v0,v1,v2=tris[:,0],tris[:,1],tris[:,2]
        area=0.5*np.linalg.norm(np.cross(v1-v0,v2-v0),axis=1); s=area.sum()
        if s<=0: continue
        ti=rng.choice(len(tris),nsamp,p=area/s)
        r1=np.sqrt(rng.random(nsamp)); r2=rng.random(nsamp)
        out.append(((1-r1)[:,None]*v0[ti]+(r1*(1-r2))[:,None]*v1[ti]+(r1*r2)[:,None]*v2[ti]))
    return np.concatenate(out) if out else np.zeros((0,3))

objs=[p.GetName() for p in stage.GetPrimAtPath("/World/CVObjects").GetChildren()]
rows=[]; tp=fp=tn=fn=0
for nm in sorted(objs):
    mp=stage.GetPrimAtPath(f"/World/CVObjects/{nm}/Mesh")
    gt_k=mp.GetCustomDataByKey("gt_k_round"); gt_zone=mp.GetCustomDataByKey("gt_zone")
    P=mesh_points(nm)
    maxK,is_round,sec=circular_section_K(P)
    gt_round=(gt_k is not None and gt_k>0.80)
    ok = (is_round==gt_round)
    if is_round and gt_round: tp+=1
    elif is_round and not gt_round: fp+=1
    elif not is_round and not gt_round: tn+=1
    else: fn+=1
    rows.append((nm,gt_zone,round(gt_k,3) if gt_k else None,maxK,is_round,gt_round,ok))
    print(f"  {nm:16s} zone={gt_zone} GTk={gt_k:.2f} measuredK={maxK:.2f} round={str(is_round):5s} GTround={str(gt_round):5s} {'OK' if ok else 'MISS'}")
prec=tp/(tp+fp) if tp+fp else 0; rec=tp/(tp+fn) if tp+fn else 0
print(f"\ncircle-in-section detection vs GT k_round>0.8:")
print(f"  round: precision={prec:.2f} recall={rec:.2f}  |  TP={tp} FP={fp} TN={tn} FN={fn}  acc={(tp+tn)/len(rows):.2f} ({tp+tn}/{len(rows)})")
non_round=[r for r in rows if not r[5]]
print(f"  non-round objects correctly rejected: {sum(1 for r in non_round if not r[4])}/{len(non_round)}  ({[r[0] for r in non_round if r[4]]} wrongly flagged)")
json.dump([list(r) for r in rows],open("/home/dasha/isaac_assets/cv/circular_section_results.json","w"),indent=2)