"""Observed endpoint motion relative to a common anatomical root and scale."""
from math import dist,hypot,isfinite,acos,pi

LIMBS=(('LW','hand',5,7,9),('RW','hand',6,8,10),('LF','foot',11,13,15),('RF','foot',12,14,16))
SEGMENTS=((5,6),(5,11),(6,12),(11,12))


def sub(a,b):return (a[0]-b[0],a[1]-b[1])
def dot(a,b):return a[0]*b[0]+a[1]*b[1]
def norm(a):return hypot(*a)


def closest(p,a,b):
    v=sub(b,a);den=dot(v,v)
    f=max(0,min(1,dot(sub(p,a),v)/den)) if den>1e-9 else 0
    return (a[0]+v[0]*f,a[1]+v[1]*f)


def region_distance(p,joints,unit):
    choices=[]
    for a,b in SEGMENTS:
        if a in joints and b in joints:
            q=closest(p,joints[a],joints[b]);choices.append((max(0,dist(p,q)-.055*unit),q,'torso'))
    for a in (0,1,2,3,4):
        if a in joints:choices.append((max(0,dist(p,joints[a][:2])-.065*unit),joints[a][:2],'head'))
    return min(choices,key=lambda x:x[0]) if choices else None


def angle(a,b,c):
    x,y=sub(a,b),sub(c,b);den=norm(x)*norm(y)
    return acos(max(-1,min(1,dot(x,y)/den)))*180/pi if den>1e-8 else None


def extract(old,current,elapsed):
    result=[];fail=[]
    assert 0<elapsed<=.35+1e-8
    for actor,(a,b) in enumerate(zip(old,current)):
        other=1-actor;oa,ob=old[other],current[other]
        aj,bj=a['joints'],b['joints'];unit=max(1,(a['height']+b['height'])/2)
        target_unit=max(1,(oa['height']+ob['height'])/2)
        scale=b['height']/a['height']
        if not .65<=scale<=1.55:fail.append(dict(actor=actor,reason='SCALE_DISCONTINUITY'));continue
        common_target=set(oa['joints'])&set(ob['joints'])
        old_target={k:oa['joints'][k] for k in common_target};new_target={k:ob['joints'][k] for k in common_target}
        for limb,kind,root,middle,end in LIMBS:
            if not all(j in aj and j in bj for j in (root,end)):
                fail.append(dict(actor=actor,limb=limb,reason='LIMB_MISSING'));continue
            before=sub(aj[end],aj[root]);after=sub(bj[end],bj[root])
            delta=(after[0]-before[0]*scale,after[1]-before[1]*scale)
            movement=norm(delta)/unit;speed=movement/elapsed
            if max(norm(before)/max(1,a['height']),norm(after)/max(1,b['height']))>1.15 or speed>4.0:
                fail.append(dict(actor=actor,limb=limb,reason='GEOMETRY_OR_SPEED_JUMP'));continue
            old_d=region_distance(aj[end][:2],old_target,target_unit);new_d=region_distance(bj[end][:2],new_target,target_unit)
            if old_d is None or new_d is None:
                fail.append(dict(actor=actor,limb=limb,reason='TARGET_REGION_MISSING'));continue
            direction=sub(new_d[1],bj[root]);den=norm(direction)
            toward=dot(delta,direction)/max(1e-8,den)/unit
            cosine=dot(delta,direction)/max(1e-8,norm(delta)*den)
            extension=(norm(after)-norm(before)*scale)/unit
            angular=None
            if middle in aj and middle in bj:
                x=angle(aj[root],aj[middle],aj[end]);y=angle(bj[root],bj[middle],bj[end])
                if x is not None and y is not None:angular=y-x
            result.append(dict(actor=actor,limb=limb,kind=kind,root=root,end=end,elapsed=elapsed,
                motion=movement,speed=speed,toward=toward,cosine=cosine,
                approach=(old_d[0]-new_d[0])/target_unit,near=new_d[0]/target_unit,
                extension=extension,angular=angular,vertical=(bj[end][1]-bj[root][1])/unit,
                relative_end=[after[0]/unit,after[1]/unit],delta=[delta[0]/unit,delta[1]/unit],
                target_region=new_d[2],old_endpoint=aj[end][:2],endpoint=bj[end][:2],target_point=list(new_d[1]),
                root_point=bj[root][:2],confidence=min(aj[root][2],aj[end][2],bj[root][2],bj[end][2])))
    return result,fail


def pair_features(pair,t):
    result=[];fail=[];used=set()
    for lag in (.1,.2):
        options=[(abs(t-ts-lag),ts,bs) for ts,bs in pair['history'] if .055-1e-8<=t-ts<=.30+1e-8 and abs(t-ts-lag)<=.055+1e-8]
        if not options:continue
        _,ts,bs=min(options,key=lambda x:(x[0],-x[1]))
        if ts in used:continue
        used.add(ts);features,invalid=extract(bs,pair['bodies'],t-ts)
        for v in features:v.update(input_start=ts,input_end=t,lag_requested=lag)
        result+=features;fail+=invalid
    return result,fail