"""Measure one unchanged anatomical target at two actual cached times."""
from math import dist,hypot,isfinite

SEGMENTS=((5,6),(5,11),(6,12),(11,12))


def closest(p,a,b):
    v=[b[i]-a[i] for i in (0,1)];den=sum(x*x for x in v)
    k=max(0.,min(1.,sum((p[i]-a[i])*v[i] for i in (0,1))/den)) if den>1e-12 else 0.
    return [a[i]+k*v[i] for i in (0,1)]


def joints(body):return {int(k):v for k,v in body['joints'].items()}


def measure(feature,old,current):
    actor=feature['actor'];other=1-actor;root=feature['root'];end=feature['end']
    a,b=old[actor],current[actor];ta,tb=old[other],current[other]
    ja,jb,jta,jtb=map(joints,(a,b,ta,tb))
    if any(k not in ja or k not in jb for k in (root,end)):
        return dict(anchor_reason='OWN_JOINT_MISSING',anchors={},primary_anchor=None)
    heights=[x['height'] for x in (a,b,ta,tb)]
    if not all(isfinite(x) and x>0 for x in heights):
        return dict(anchor_reason='INVALID_SCALE',anchors={},primary_anchor=None)
    points=[]
    for key in range(5):
        if key in jta and key in jtb:points.append(('H'+str(key),jta[key][:2],jtb[key][:2],.065))
    for x,y in SEGMENTS:
        if all(k in jta and k in jtb for k in (x,y)):
            points.append((f'T{x}:{y}',closest(ja[end],jta[x],jta[y]),closest(jb[end],jtb[x],jtb[y]),.055))
    anchors={}
    for key,pa,pb,pad in points:
        da=dist(ja[end][:2],pa)/ta['height'];db=dist(jb[end][:2],pb)/tb['height']
        anchors[key]=dict(start=max(0.,da-pad),end=max(0.,db-pad),raw_start=da,raw_end=db,
                          start_target=pa,end_target=pb,padding=pad)
    primary=min(anchors,key=lambda k:(anchors[k]['start'],anchors[k]['raw_start'],k)) if anchors else None
    old_rel=[(ja[end][i]-ja[root][i])/a['height'] for i in (0,1)]
    new_rel=[(jb[end][i]-jb[root][i])/b['height'] for i in (0,1)]
    delta=[new_rel[i]-old_rel[i] for i in (0,1)]
    return dict(anchor_reason='OBSERVED' if anchors else 'TARGET_ANCHOR_MISSING',anchors=anchors,
        primary_anchor=primary,old_relative=old_rel,current_relative=new_rel,relative_motion=hypot(*delta),
        relative_delta=delta,measurement_refs=[x['ref'] for x in current],
        measurement_previous_refs=[x['ref'] for x in old])


class AnchorBuilder:
    def __init__(self):self.past={};self.last_time=None

    def step(self,row):
        t=row['seconds'];assert self.last_time is None or t>self.last_time;self.last_time=t
        for p in row['pairs']:
            for f in p['features']:
                assert f['input_start']<f['input_end']==t
                old=self.past.get((p['token'],f['input_start']))
                if old is None:f.update(anchor_reason='HISTORY_UNAVAILABLE',anchors={},primary_anchor=None)
                else:f.update(measure(f,old,p['bodies']))
            self.past[(p['token'],t)]=p['bodies']
        self.past={k:v for k,v in self.past.items() if t-k[1]<=.35+1e-8}
        return row