"""How people move around a robot: a simulated crowd against the real one, paired person by person.

react_score.py REAL.txt ROBOT.txt NAME=SIM.txt [NAME=SIM.txt ...]
All files in the crowd format (frame k*10 = time k*0.4 s, id, x, y); people keep their ids in every crowd.
People counted: those who come within 3 m of the robot's centre in the real recording. For each such person and each
crowd: the closest the person comes to the robot. Per crowd:
- closest_err: mean absolute difference from the real closest approach, person by person (the primary measure);
- within1_err: difference in the share of counted people who come within 1 m of the robot;
- speed_err: difference in mean walking speed while within 2 m of the robot.
"""
import json, sys
import numpy as np

def tracks(path):
    d = np.loadtxt(path, ndmin=2); out = {}
    for pid in np.unique(d[:, 1]):
        t = d[d[:, 1] == pid]; t = t[np.argsort(t[:, 0])]
        out[int(pid)] = (np.round(t[:, 0]).astype(int), t[:, 2:4])
    return out

def robot_xy(path):
    d = np.loadtxt(path, ndmin=2); d = d[np.argsort(d[:, 0])]
    return {int(round(f)): xy for f, xy in zip(d[:, 0], d[:, 2:4])}

def per_person(tr, rob):
    res = {}
    for pid, (fr, xy) in tr.items():
        ok = np.array([f in rob for f in fr])
        if ok.sum() < 2: continue
        r = np.array([rob[f] for f in fr[ok]]); p = xy[ok]
        d = np.hypot(*(p - r).T)
        sp = np.r_[np.hypot(*np.diff(xy, axis=0).T) / (np.diff(fr) / 10 * 0.4), np.nan][ok]
        res[pid] = (float(d.min()), sp[d < 2.0])
    return res

real, robot = sys.argv[1], sys.argv[2]
rob = robot_xy(robot); R = per_person(tracks(real), rob)
counted = [p for p, v in R.items() if v[0] < 3.0]
out = {"real": real, "people_counted": len(counted), "real_within1": None, "crowds": {}}
if counted:
    rc = np.array([R[p][0] for p in counted]); rs = np.concatenate([R[p][1] for p in counted])
    out["real_within1"] = float((rc < 1.0).mean()); out["real_speed_near"] = float(np.nanmean(rs)) if len(rs) else None
    for arg in sys.argv[3:]:
        name, path = arg.split("=", 1)
        S = per_person(tracks(path), rob)
        have = [p for p in counted if p in S]
        sc = np.array([S[p][0] for p in have]); rr = np.array([R[p][0] for p in have])
        ss = np.concatenate([S[p][1] for p in have]) if have else np.array([])
        out["crowds"][name] = {"people": len(have), "closest_err": float(np.abs(sc - rr).mean()) if have else None,
                               "closest_bias": float((sc - rr).mean()) if have else None,
                               "within1_err": float(abs((sc < 1.0).mean() - out["real_within1"])) if have else None,
                               "speed_err": float(abs(np.nanmean(ss) - out["real_speed_near"])) if len(ss) and out["real_speed_near"] else None}
print(json.dumps(out))