import json, pathlib, copy, math
ROOT=pathlib.Path('/mnt/data/karaoke_v423e/root')
p=ROOT/'assets/js/data/nex-nexdatah-full-bank-physio-ik-keypose-specs.json'
D=json.loads(p.read_text())
prof=next(x for x in D['specs'] if x.get('profileId')=='warmup_karaoke_ik_v1')
base=copy.deepcopy(prof['physioTruthSpec']['keyPositions'][0]['ikControllerTargetsMeters'])
BASE_PELVIS_Y=base['Pelvis'][1]

def body(px, py=.92):
    t=copy.deepcopy(base); dy=py-BASE_PELVIS_Y
    for k,v in t.items():
        if isinstance(v,list) and len(v)==3:
            v[0]+=px-base['Pelvis'][0]
            if not (k.endswith('_Foot') or k.endswith('_ToeBase') or k.endswith('_Calf')):
                v[1]+=dy
    t['L_Thigh']=[px-.0475,.70+dy,.04]
    t['R_Thigh']=[px+.0475,.70+dy,.04]
    return t

def knee_for(thigh, foot, knee_y, fwd=.03):
    den=thigh[1]-foot[1]
    u=(thigh[1]-knee_y)/den if abs(den)>1e-9 else .35
    u=max(0,min(1,u))
    return [thigh[0]+u*(foot[0]-thigh[0]), knee_y, thigh[2]+u*(foot[2]-thigh[2])+fwd]

def set_leg(t, side, foot, kind='land'):
    foot=list(foot); t[f'{side}_Foot']=foot
    t[f'{side}_ToeBase']=[foot[0],max(.02,foot[1]),foot[2]+.12]
    if kind=='front_peak': ky,fw=.705,.100
    elif kind=='front_over': ky,fw=.690,.095
    elif kind=='behind_peak': ky,fw=.615,.060
    elif kind=='side_swing': ky,fw=.595,.055
    elif kind=='toeoff': ky,fw=.535,.040
    elif kind=='load': ky,fw=.465,.025
    else: ky,fw=.495,.020
    t[f'{side}_Calf']=knee_for(t[f'{side}_Thigh'],foot,ky,fw)

def set_arms(t,state):
    s=t['Hip'][0]
    P={
      'neutral':{
       'L_Forearm':[s-.235,1.285,.035],'L_Hand':[s-.205,1.155,.095],
       'R_Forearm':[s+.235,1.285,.035],'R_Hand':[s+.205,1.155,.095]},
      # outward front-cross uses L leg -> right hand is the explosive helper
      'front_pre_R':{
       'L_Forearm':[s-.250,1.280,-.080],'L_Hand':[s-.235,1.105,-.180],
       'R_Forearm':[s+.205,1.335,.105],'R_Hand':[s+.165,1.300,.185]},
      'front_up_R':{
       'L_Forearm':[s-.270,1.270,-.135],'L_Hand':[s-.252,1.070,-.245],
       'R_Forearm':[s+.182,1.420,.150],'R_Hand':[s+.125,1.420,.240]},
      'front_glide_R':{
       'L_Forearm':[s-.257,1.278,-.100],'L_Hand':[s-.240,1.095,-.200],
       'R_Forearm':[s+.205,1.330,.105],'R_Hand':[s+.165,1.255,.175]},
      'front_down_R':{
       'L_Forearm':[s-.242,1.282,-.055],'L_Hand':[s-.222,1.115,-.135],
       'R_Forearm':[s+.223,1.270,.055],'R_Hand':[s+.200,1.145,.100]},
      # outward behind-cross uses L leg -> left hand comes forward gently, no punch
      'behind_pre_L':{
       'L_Forearm':[s-.220,1.305,.065],'L_Hand':[s-.185,1.205,.125],
       'R_Forearm':[s+.242,1.245,-.050],'R_Hand':[s+.225,1.105,-.120]},
      'behind_soft_L':{
       'L_Forearm':[s-.190,1.345,.110],'L_Hand':[s-.145,1.290,.180],
       'R_Forearm':[s+.255,1.235,-.090],'R_Hand':[s+.235,1.080,-.170]},
      'behind_exit_L':{
       'L_Forearm':[s-.205,1.315,.080],'L_Hand':[s-.170,1.235,.140],
       'R_Forearm':[s+.245,1.255,-.050],'R_Hand':[s+.225,1.110,-.120]},
    }
    # mirror arm states for return direction
    if state.endswith('_L') and state.startswith('front'):
        src=P[state.replace('_L','_R')]; out={}
        pairs=[('L_Forearm','R_Forearm'),('L_Hand','R_Hand')]
        for a,b in pairs:
            va=src[b][:]; vb=src[a][:]
            va[0]=2*s-va[0]; vb[0]=2*s-vb[0]
            out[a]=va; out[b]=vb
        P[state]=out
    if state.endswith('_R') and state.startswith('behind'):
        src=P[state.replace('_R','_L')]; out={}
        pairs=[('L_Forearm','R_Forearm'),('L_Hand','R_Hand')]
        for a,b in pairs:
            va=src[b][:]; vb=src[a][:]
            va[0]=2*s-va[0]; vb[0]=2*s-vb[0]
            out[a]=va; out[b]=vb
        P[state]=out
    for k,v in P[state].items(): t[k]=v

def px_from(lf,rf,support,d):
    if len(support)==1:
        f=lf if support[0]=='L' else rf
        return f[0]+d*.025
    return (lf[0]+rf[0])/2

K=[]
def add(id,sec,lf,rf,support,moving=None,kind='land',arm='neutral',d=1,py=None):
    if py is None:
        py=.965 if kind in ('front_peak','front_over') else (.94 if kind in ('behind_peak','side_swing') else (.89 if kind=='load' else .91))
    px=px_from(lf,rf,support,d)
    t=body(px,py)
    set_leg(t,'L',lf,kind if moving=='L' else ('load' if kind=='load' else 'land'))
    set_leg(t,'R',rf,kind if moving=='R' else ('load' if kind=='load' else 'land'))
    set_arms(t,arm)
    K.append({'id':id,'sec':round(sec,4),'targets':t,'support':support,'moving':moving,'kind':kind,'arm':arm})

def step_side(prefix,t0,lf,rf,lead,d,step,arm='neutral'):
    support='L' if lead=='R' else 'R'; cur=list(rf if lead=='R' else lf)
    target=[cur[0]+d*step,.02,.04]
    toe=[cur[0]+d*.02,.055,.05]; swing=[(cur[0]+target[0])*.5,.145,.07]
    def pair(f): return (lf,f) if lead=='R' else (f,rf)
    a,b=pair(tuple(toe)); add(prefix+'_toeoff',t0+.01,a,b,[support],lead,'toeoff',arm,d)
    a,b=pair(tuple(swing)); add(prefix+'_swing',t0+.08,a,b,[support],lead,'side_swing',arm,d)
    a,b=pair(tuple(target)); add(prefix+'_land',t0+.16,a,b,['L','R'],None,'land',arm,d)
    add(prefix+'_accept',t0+.20,a,b,['L','R'],None,'load',arm,d)
    return a,b

def step_cross(prefix,t0,lf,rf,cross,d,step,front=True):
    support='R' if cross=='L' else 'L'; cur=list(lf if cross=='L' else rf)
    sup=rf if cross=='L' else lf
    tx=sup[0]+d*step
    if front:
        zpeak=.23; zland=.18; armPre=('front_pre_R' if cross=='L' else 'front_pre_L'); armUp=('front_up_R' if cross=='L' else 'front_up_L'); armGlide=('front_glide_R' if cross=='L' else 'front_glide_L'); armDown=('front_down_R' if cross=='L' else 'front_down_L')
        toe=[cur[0]+d*.015,.060,.07]; knee=[cur[0]+d*(tx-cur[0])*.55,.225,.16]; over=[tx,.245,zpeak]; land=[tx,.02,zland]
        times=(.01,.09,.15,.25,.31)
        states=(armPre,armUp,armUp,armGlide,armDown)
        kinds=('toeoff','front_peak','front_over','land','load')
    else:
        zpeak=-.16; zland=-.12; armPre=('behind_pre_L' if cross=='L' else 'behind_pre_R'); armSoft=('behind_soft_L' if cross=='L' else 'behind_soft_R'); armExit=('behind_exit_L' if cross=='L' else 'behind_exit_R')
        toe=[cur[0]+d*.015,.055,.02]; knee=[cur[0]+d*(tx-cur[0])*.55,.165,-.03]; over=[tx,.185,zpeak]; land=[tx,.02,zland]
        times=(.01,.09,.17,.25,.30)
        states=(armPre,armSoft,armSoft,armExit,armExit)
        kinds=('toeoff','behind_peak','behind_peak','land','load')
    poses=[toe,knee,over,land,land]
    def pair(f): return (tuple(f),rf) if cross=='L' else (lf,tuple(f))
    last=None
    for suf,dt,pose,state,kind in zip(['toeoff','knee','over','land','accept'],times,poses,states,kinds):
        a,b=pair(pose); add(prefix+'_'+suf,t0+dt,a,b,[support] if suf in ('toeoff','knee','over') else ['L','R'],cross if suf in ('toeoff','knee','over') else None,kind,state,d); last=(a,b)
    return last

def cycle(prefix,cs,lf,rf,d,lead,cross,side_step=.10,cross_step=.06):
    # Source cadence: soft behind step first, then explosive front-cross at ~cs+0.82 s.
    lf,rf=step_side(prefix+'_s1',cs+0.00,lf,rf,lead,d,side_step,'neutral')
    lf,rf=step_cross(prefix+'_behind',cs+0.22,lf,rf,cross,d,cross_step,front=False)
    lf,rf=step_side(prefix+'_s2',cs+0.53,lf,rf,lead,d,side_step,'neutral')
    lf,rf=step_cross(prefix+'_front',cs+0.735,lf,rf,cross,d,cross_step,front=True)
    return lf,rf

# SOURCE cadence: front-cross knee peaks around 0.81, 2.06, 3.31 s on outward travel.
lf=(-.18,.02,.04); rf=(.18,.02,.04)
add('ready',0.00,lf,rf,['L','R'],None,'land','neutral',1,.92)
for n,cs in enumerate([0.00,1.25,2.50],1):
    lf,rf=cycle(f'out{n}',cs,lf,rf,1,'R','L',.10,.06)
# settle/turn phase seen in source; no fake high-knee accent during the turn.
add('out_settle',3.62,lf,rf,['L','R'],None,'land','neutral',1,.92)
add('turn_load',4.15,lf,rf,['L','R'],None,'load','neutral',-1,.90)
add('turn_ready',4.82,lf,rf,['L','R'],None,'land','neutral',-1,.92)
# Return cadence: source front-cross peaks ~5.9 and ~7.15 s.
# choose cycle starts 5.05 and 6.30 => peaks 5.86 and 7.11.
for n,cs in enumerate([5.05,6.30],1):
    lf,rf=cycle(f'ret{n}',cs,lf,rf,-1,'L','R',.13,.07)
# finish with a soft open recovery toward a loopable stance, no extra explosive cross.
lf,rf=step_side('finish_open',7.55,lf,rf,'L',-1,.10,'neutral')
TOTAL=7.93
add('finish_or_loop',7.92,lf,rf,['L','R'],None,'land','neutral',-1,.92)

# Safety monotonic times / unique ids
assert all(K[i]['sec']<K[i+1]['sec'] for i in range(len(K)-1)), [(K[i]['id'],K[i]['sec'],K[i+1]['id'],K[i+1]['sec']) for i in range(len(K)-1) if K[i]['sec']>=K[i+1]['sec']]
assert len({k['id'] for k in K})==len(K)
kp=[]
for e in K:
    joints=sum(([f'{s}_Foot',f'{s}_ToeBase'] for s in e['support']),[])
    kp.append({'id':e['id'],'t':round(e['sec']/TOTAL,7),
      'physioRequirement':'Karaoke source cadence: alternating lead/cross gait. Behind-cross is soft with gentle same-side hand-forward counter-rotation; front-cross is a faster accented phase with slightly higher knee and compact contralateral hand-up drive. Support foot stays planted until opposite foot lands.',
      'ikControllerTargetsMeters':e['targets'],
      'detectors':['SUPPORT_POINT_AUTHORITY','NEX_REACHABILITY','SOURCE_FIDELITY_BLOCKING','V423G_KARAOKE_SOURCE_CADENCE_ACCENT'],
      'contactRules':[{'type':'support_contact','joints':joints,'toleranceMeters':.045,'policy':'V423G source-cadence authority: planted support contact through opposite-foot toe-off/swing/plant; behind step soft, front cross accented.'}],
      'mirrorRule':'Source-authored full MOV cadence; direction-specific gait rather than synthetic half-cycle mirroring.',
      'forbiddenBehaviors':['actual_nex_not_posed_to_ik','same_foot_consecutive_steps','support_foot_slides_during_opposite_swing','front_cross_not_accented','front_cross_knee_too_low','arm_windmill_phase','root_static_in_travel_drill','manual_bone_mapping_not_applied']})
prof['physioTruthSpec']['keyposeCount']=len(kp)
prof['physioTruthSpec']['keyPositions']=kp
prof['nexProd01']['testCases']=[{'caseId':'v423g_'+e['id'],'phase':round(e['sec']/TOTAL,7),'expectedKeypose':e['id']} for e in K]
p.write_text(json.dumps(D,indent=2)+'\n')
(ROOT/'assets/js/data/nex-nexdatah-full-bank-physio-ik-keypose-specs.js').write_text('window.NEXT11_NEXDATAH_FULL_BANK_PHYSIO_IK_KEYPOSE_SPECS = '+json.dumps(D,indent=2)+';\n')
print(json.dumps({'keyposes':len(kp),'duration':TOTAL,'frontPeaks':[.81,2.06,3.31,5.86,7.11]},indent=2))
