#!/usr/bin/env python
"""闭环碰撞定位：第一次碰撞发生在第几秒、碰撞时最近的是哪两条连杆（J几-J几）。

用法（仓库根目录）：
  PYTHONUTF8=1 python -m ml.eval.diag_collision --ckpt ml/runs/base/last.pt --n 512 --out ml/runs/diag_collision.json

口径与 closed_loop.rollout 一致：每步同时检查当前状态 q 与相邻状态中点 mid 的间隙（cap=0.5 mm）；
间隙组对索引 = gA*7+gB（gA<gB，g 为连杆组 0..5 → J1..J6）。
"""
import argparse, json, math, os, sys, time
from collections import Counter

import torch

from ml.common.kin_torch import Kin
from ml.data import load_tasks
from ml.model.policy import build
from ml.sim.closed_loop import DT, rollout


def pair_names():
    # kin.json 有 7 个连杆组：g=0 是固定基座/立柱，g=1..6 是 J1..J6
    def nm(g):
        return '基座' if g == 0 else f'J{g}'
    return {a * 7 + b: f'{nm(a)}-{nm(b)}' for a in range(7) for b in range(a + 1, 7)}


@torch.no_grad()
def replay_first_hits(path, out=None):
    saved = torch.load(path, map_location='cpu', weights_only=True)
    k32, k64 = Kin(), Kin(dtype=torch.float64)
    rows, offset = [], 0
    for _, per, count in saved['batches']:
        for b in (per['col'] > 0).nonzero().flatten().tolist():
            it = int(round(float(per['first_hit'][b]) / DT))
            prev, q = per['traj'][b, it - 1:it + 1]
            mid = prev + 0.5 * (q - prev)
            points = torch.stack([per['traj'][b, 0], prev, mid, q])
            gm32 = k32.gaps(points, 0.5)['gm']
            gm64 = k64.gaps(points.double(), 0.5)['gm']
            gap32, gp32 = gm32.min(-1)
            gap64, gp64 = gm64.min(-1)
            nseg = max(1, math.ceil(float((q - prev).abs().max()) / 0.01))
            dense = prev.double() + torch.linspace(0, 1, nseg + 1, dtype=torch.float64)[:, None] * (q.double() - prev.double())
            dense_gap = torch.cat([k64.gaps(chunk, 0.5)['gm'].min(-1).values for chunk in dense.split(32)])
            boundary = (~k32.cont) & ((q - k32.lo).abs().lt(1e-5) | (q - k32.hi).abs().lt(1e-5))
            row = {
                'i': offset + b, 'task': saved['tasks'][offset + b],
                'first_hit_s': float(per['first_hit'][b]), 'dls_it': int(per['dls_it'][b]),
                'point_order': ['q0', 'previous', 'midpoint', 'endpoint'],
                'gap32_mm': gap32.tolist(), 'gap64_mm': gap64.tolist(),
                'pair32': gp32.tolist(), 'pair64': gp64.tolist(),
                'boundary_joints': (boundary.nonzero().flatten() + 1).tolist(),
                'previous': prev.tolist(), 'endpoint': q.tolist(),
                'dense_nsegments': nseg, 'dense_gap64_min_mm': float(dense_gap.min()),
            }
            if 'plans' in per:
                plans = per['plans']
                pi = int(torch.searchsorted(plans['it'], it - 1, right=True)) - 1
                step = it - 1 - int(plans['it'][pi])
                ncommit = plans['commit'].shape[2]
                command = plans['commit'][b, pi, step]
                cp, cm = plans['points'][b, pi, step], plans['midpoints'][b, pi, step]
                checked = bool(plans['certified'][b, pi, step])
                predicted = k32.clamp_box((prev + command)[None])[0]
                row['certificate'] = {
                    'plan_start_it': int(plans['it'][pi]), 'step': step, 'checked': checked,
                    'endpoint': cp.tolist() if checked else None,
                    'midpoint': cm.tolist() if checked else None,
                    'recorded_gap32_mm': plans['gaps'][b, pi, [step, ncommit + step]].tolist() if checked else None,
                    'replayed_gap64_mm': k64.gaps(torch.stack([cp, cm]).double(), 0.5)['gm'].min(-1).values.tolist() if checked else None,
                    'command_delta': command.tolist(),
                    'clamped_reconstruction_max_abs_deg': float((predicted - q).abs().max()),
                    'certified_vs_executed_max_abs_deg': float((cp - q).abs().max()) if checked else None,
                }
            rows.append(row)
            print('task %d first_hit %.2fs gap32 %s gap64 %s boundary %s' %
                  (row['i'], row['first_hit_s'], row['gap32_mm'], row['gap64_mm'], row['boundary_joints']), flush=True)
        offset += count
    result = {
        'trace': path, 'n': offset, 'reported_collision_tasks': len(rows),
        'replayed_first_hits32': sum(min(r['gap32_mm'][2:]) <= 0 for r in rows),
        'replayed_first_hits64': sum(min(r['gap64_mm'][2:]) <= 0 for r in rows),
        'scope': 'Replay reported first hits only; same geometry kernel at float32/float64, not a full safety audit.',
        'rows': rows,
    }
    if out:
        with open(out, 'w', encoding='utf-8') as f:
            json.dump(result, f, indent=2, allow_nan=False)
    print('replayed first hits: %d/%d float32, %d/%d float64' %
          (result['replayed_first_hits32'], len(rows), result['replayed_first_hits64'], len(rows)), flush=True)
    return result


@torch.no_grad()
def replay_dense(path, indices, max_step_deg=0.05, device='cpu', out=None):
    saved = torch.load(path, map_location='cpu', weights_only=True)
    kin = Kin(device=device, dtype=torch.float64)
    if saved.get('kin_fp') != kin.fp:
        raise ValueError('trace/kin fingerprint mismatch')
    if not indices or min(indices) < 0 or max(indices) >= len(saved['tasks']) or max_step_deg <= 0:
        raise ValueError('valid task indices and positive max_step_deg required')
    started = time.perf_counter()
    rows, offset = [], 0
    for _, per, count in saved['batches']:
        for index in indices:
            if not offset <= index < offset + count:
                continue
            b = index - offset
            nstep = min(per['traj'].shape[1] - 1, math.floor((float(per['limit'][b]) + 1e-6) / DT))
            if torch.isfinite(per['t_done'][b]):
                nstep = min(nstep, round(float(per['t_done'][b]) / DT))
            trajectory = per['traj'][b, :nstep + 1].double()
            points, segment_ids, fractions = [trajectory[:1]], [torch.tensor([-1])], [torch.zeros(1)]
            for it in range(nstep):
                prev, end = trajectory[it:it + 2]
                nseg = max(2, 2 * math.ceil(float((end - prev).abs().max()) / (2 * max_step_deg)))
                alpha = torch.arange(1, nseg + 1, dtype=torch.float64) / nseg
                points.append(prev + alpha[:, None] * (end - prev))
                segment_ids.append(torch.full((nseg,), it))
                fractions.append(alpha)
            points = torch.cat(points)
            segments, alpha = torch.cat(segment_ids), torch.cat(fractions)
            print('dense task %d steps %d points %d start' % (index, nstep, len(points)), flush=True)
            gaps = []
            for start in range(0, len(points), 64):
                gaps.append(kin.gaps(points[start:start + 64].to(device), 0.5)['gm'].min(-1).values.cpu())
                if (start // 64 + 1) % 64 == 0:
                    print('dense task %d points %d/%d wall %.1fs' %
                          (index, min(start + 64, len(points)), len(points), time.perf_counter() - started), flush=True)
            gaps = torch.cat(gaps)
            hit = gaps <= 0
            first = int(hit.nonzero()[0]) if hit.any() else None
            rows.append({
                'i': index, 'task': saved['tasks'][index], 'steps': nstep, 'sampled_points': len(points),
                'initial_gap64_mm': float(gaps[0]), 'min_gap64_mm': float(gaps.min()),
                'collision_segments': int(segments[hit & (segments >= 0)].unique().numel()),
                'first_collision_s': max(0.0, float(segments[first] + alpha[first]) * DT) if first is not None else None,
            })
            print('dense task %d min_gap %.9f mm collision_segments %d' %
                  (index, rows[-1]['min_gap64_mm'], rows[-1]['collision_segments']), flush=True)
        offset += count
    result = {
        'trace': str(path), 'tasks_sha256': saved.get('tasks_sha256'), 'kin_fp': kin.fp,
        'indices': indices, 'max_step_deg': max_step_deg, 'dtype': 'float64', 'device': device,
        'scope': 'Executed trajectory through each task deadline or arrival, including initial state; finite sampling, not continuous certification.',
        'collision_tasks': sum(r['min_gap64_mm'] <= 0 for r in rows), 'rows': rows,
        'audit_wall_s': time.perf_counter() - started,
    }
    if out:
        with open(out, 'w', encoding='utf-8') as f:
            json.dump(result, f, indent=2, allow_nan=False)
    return result


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--ckpt')
    ap.add_argument('--trace')
    ap.add_argument('--dense_indices', help='逗号分隔的任务下标；回放实际轨迹，不依赖原 col 标记')
    ap.add_argument('--max_step_deg', type=float, default=0.05)
    ap.add_argument('--samples', default='ml/data/samples')
    ap.add_argument('--split', default='val')
    ap.add_argument('--n', type=int, default=512)
    ap.add_argument('--bs', type=int, default=64)
    ap.add_argument('--chunk', type=int, default=64, help='回放轨迹分块步数（显存小就调小）')
    ap.add_argument('--K', type=int, default=8)
    ap.add_argument('--cap', type=float, default=0.5)
    ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu')
    ap.add_argument('--out', default=None)
    a = ap.parse_args()
    if a.trace:
        if a.dense_indices:
            replay_dense(a.trace, [int(i) for i in a.dense_indices.split(',')], a.max_step_deg, a.device, a.out)
        else:
            replay_first_hits(a.trace, a.out)
        return
    if a.dense_indices:
        ap.error('--dense_indices requires --trace')
    if not a.ckpt:
        ap.error('--ckpt or --trace is required')

    ck = torch.load(a.ckpt, map_location=a.device)
    model = build(ck['cfg']).to(a.device)
    model.load_state_dict(ck['model'])
    model.eval()
    kin = Kin(device=a.device)
    if ck.get('fp') and ck['fp'] != kin.fp:
        sys.exit(f'checkpoint 指纹 {ck["fp"]} 与 kin.json {kin.fp} 不符')
    tasks = [t for t in load_tasks([a.samples]) if t['split'] == a.split][:a.n]
    PN = pair_names()
    rows = []
    tau = [0, 0.5, 1.0, 2.0, 3.0, 5.0, 1e9]
    hist = Counter()
    pair_first = Counter(); pair_steps = Counter()
    with torch.no_grad():
        for i in range(0, len(tasks), a.bs):
            chunk = tasks[i:i + a.bs]
            res, per = rollout(model, kin, chunk, a.device, K=a.K, safety=False, seed=i, return_traj=True)
            traj = per['traj'].to(a.device)              # [B,T,6]
            B, Tn, _ = traj.shape
            first_t = torch.full((B,), float('nan'), device=a.device)
            first_pair = torch.full((B,), -1, dtype=torch.long, device=a.device)
            first_gap = torch.full((B,), float('nan'), device=a.device)
            min_gap = torch.full((B,), float('inf'), device=a.device)
            col_steps = torch.zeros(B, device=a.device)
            for k in range(Tn):
                qk = traj[:, k]
                mid = 0.5 * (traj[:, max(0, k - 1)] + qk)
                pts = torch.stack([qk, mid], 1).reshape(2 * B, 6)   # 每帧只喂 2B 个点（与 rollout 同量级；一次 2048 点会爆显存）
                gm = kin.gaps(pts, a.cap)['gm'].view(B, 2, 49)
                gmin, gpair = gm.min(-1)                             # [B,2]
                g1 = gmin.min(1).values
                min_gap = torch.minimum(min_gap, g1)
                newly = (g1 <= 0) & torch.isnan(first_t)
                if bool(newly.any()):
                    for b in newly.nonzero().flatten().tolist():
                        pos = 0 if float(gmin[b, 0]) <= 0 else 1
                        first_t[b] = k * DT
                        first_pair[b] = gpair[b, pos]
                        first_gap[b] = gmin[b, pos]
                if not bool(torch.isnan(first_t).any()):
                    break
            for b, t in enumerate(chunk):
                ft = float(first_t[b])
                fp = int(first_pair[b])
                name = PN.get(fp, '—')
                if not math.isnan(ft):
                    pair_first[name] += 1
                    for j in range(len(tau) - 1):
                        if tau[j] <= ft < tau[j + 1]:
                            hist[f'{tau[j]}–{tau[j + 1]} s' if tau[j + 1] < 1e8 else '> 5 s'] += 1
                            break
                rows.append({
                    'tid': t.get('tid'), 'cat': t.get('cat'), 'mode': t.get('mode'),
                    'collided': bool(per['col'][b] > 0),
                    'first_collision_s': None if math.isnan(ft) else round(ft, 2),
                    'first_pair': None if name == '—' else name, 'first_pair_idx': None if fp < 0 else fp,
                    'first_gap_mm': None if math.isnan(float(first_gap[b])) else round(float(first_gap[b]), 3),
                    'min_gap_mm': round(float(min_gap[b]), 3),
                    'collision_steps': int(per['col'][b]),
                })
            print(f'[diag] {min(i + a.bs, len(tasks))}/{len(tasks)} 已回放', flush=True)

    collided = [r for r in rows if r['collided']]
    def pct(v, p):
        v = sorted(v); return v[min(len(v) - 1, int(len(v) * p))] if v else None
    times = [r['first_collision_s'] for r in collided]
    out = {
        'ckpt': a.ckpt, 'step': ck.get('step'), 'n': len(rows), 'n_collided': len(collided),
        'collision_rate': len(collided) / max(1, len(rows)),
        'first_time_s': {'p10': pct(times, .1), 'p50': pct(times, .5), 'p90': pct(times, .9)},
        'first_time_hist': dict(hist),
        'first_pair_top': pair_first.most_common(),
        'min_gap_mm_p10/p50': [pct([r['min_gap_mm'] for r in rows], .1), pct([r['min_gap_mm'] for r in rows], .5)],
        'by_cat': {},
        'tasks': rows,
    }
    for k in sorted({f"{r['cat']}/{r['mode']}" for r in rows}):
        g = [r for r in rows if f"{r['cat']}/{r['mode']}" == k]
        out['by_cat'][k] = {'n': len(g), 'first_t_p50': pct([r['first_collision_s'] for r in g if r['collided']], .5),
                            'first_pairs': Counter([r['first_pair'] for r in g if r['first_pair']]).most_common(3)}
    s = json.dumps(out, indent=1, ensure_ascii=False)
    if a.out:
        open(a.out + '.tmp', 'w', encoding='utf-8').write(s); os.replace(a.out + '.tmp', a.out)
    print('== 汇总 ==')
    print('题数 %d，撞过 %d（%.1f%%）' % (len(rows), len(collided), 100 * out['collision_rate']))
    print('首次碰撞时间 p10/p50/p90 = %s / %s / %s s' % (
        out['first_time_s']['p10'], out['first_time_s']['p50'], out['first_time_s']['p90']))
    print('首次碰撞时间分布：', out['first_time_hist'])
    print('首次碰撞的连杆对 Top：', pair_first.most_common(8))
    print('全程最小间隙 p10/p50 = %s / %s mm' % tuple(out['min_gap_mm_p10/p50']))
    if a.out: print('已写', a.out)


if __name__ == '__main__':
    main()