"""预训练（方案 §6）：MeanFlow 一步头 + FK 损失 + 平滑 + 价值头 + 子目标头。
  python -m ml.train --samples ml/data/samples --out ml/runs/base [--config ml/configs/base.json]
  吞吐摸底：python -m ml.train --samples ml/data/samples --out ml/runs/bench_base --steps 2000 --no-eval
  通过线（方案 R10）：全程 60000 步 ≤ 3.5 h，即 samples_per_s ≥ 5000（3090 实测 6.2 k/s ≈ 2.8 h）；日志 eta_h 为剩余小时数。
每 eval_every 步做一次闭环验证（ml/sim/closed_loop.py，部署节奏），按 success 存 best.pt；连续 patience_evals 次不升即停。"""
import argparse, json, math, os, time
import torch

from ml.common.kin_torch import Kin
from ml.common.features import STEP, D2R
from ml.data import Samples, load_tasks
from ml.model.policy import build, wta_loss
from ml.model.meanflow import meanflow_loss, one_step
from ml.sim.closed_loop import evaluate


def fk_points(kin, q, idx):
    """选定胶囊端点 + TCP 的世界坐标 [B, 2*len(idx)+1, 3]"""
    xf = kin.cum_xf(q)
    a, b = kin.caps_world(q, xf)
    return torch.cat([a[:, idx], b[:, idx], kin.fk(q, xf)[:, None]], 1)


def q_from_feat(X):
    s, c = X[:, 0:12:2], X[:, 1:12:2]
    return torch.atan2(s, c) / D2R


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--samples', nargs='+', default=['ml/data/samples']); ap.add_argument('--out', default='ml/runs/base')
    ap.add_argument('--config', default='ml/configs/base.json'); ap.add_argument('--steps', type=int, default=None)
    ap.add_argument('--resume', default=None); ap.add_argument('--no-eval', action='store_true')
    ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu'); ap.add_argument('--seed', type=int, default=0)
    a = ap.parse_args()
    cfg = json.load(open(a.config)); tc = cfg['train']
    if a.steps: tc['steps'] = a.steps
    os.makedirs(a.out, exist_ok=True)
    torch.manual_seed(a.seed)
    dev = a.device
    kin = Kin(device=dev)
    data = Samples(a.samples, device=dev, split='train')
    val_tasks = [t for t in load_tasks(a.samples) if t['split'] == 'val'][:tc['eval_tasks']]
    print(f'train samples {data.n}, val tasks {len(val_tasks)}, device {dev}', flush=True)
    model = build(cfg).to(dev)
    print(f'params {sum(p.numel() for p in model.parameters()) / 1e6:.2f} M', flush=True)
    opt = torch.optim.AdamW(model.parameters(), lr=tc['lr'], betas=tuple(tc['betas']), weight_decay=tc['wd'])
    step0, best = 0, -1.0
    if a.resume:
        ck = torch.load(a.resume, map_location=dev); model.load_state_dict(ck['model'])
        if 'opt' in ck: opt.load_state_dict(ck['opt'])
        step0, best = ck.get('step', 0), ck.get('best', -1.0)
    total = tc['steps']
    lr_at = lambda s: tc['lr'] * (s / tc['warmup'] if s < tc['warmup'] else 0.5 * (1 + math.cos(math.pi * (s - tc['warmup']) / max(1, total - tc['warmup']))))
    amp = dev.startswith('cuda') and tc.get('amp') == 'bf16'
    stepv = torch.tensor(STEP, device=dev)
    fk_idx = torch.linspace(0, kin.cap_a.shape[0] - 1, tc['fk_caps'], device=dev).long()
    log = open(os.path.join(a.out, 'log.jsonl'), 'a' if a.resume else 'w')   # 新训练覆盖旧日志，续训才追加
    if not val_tasks and not a.no_eval:
        print('没有 val 任务（样本太少），本次跳过闭环验证', flush=True); a.no_eval = True
    bad_evals, t0, seen = 0, time.time(), 0
    def save(name, step, extra=None):
        d = {'model': model.state_dict(), 'opt': opt.state_dict(), 'cfg': cfg, 'step': step, 'best': best, 'fp': kin.fp}
        if extra: d.update(extra)
        torch.save(d, os.path.join(a.out, name))
    for step in range(step0, total):
        for gpar in opt.param_groups: gpar['lr'] = lr_at(step)
        X, Y, V, SG = data.batch(tc['batch'])
        model.train()
        with torch.autocast('cuda', dtype=torch.bfloat16, enabled=amp):
            mem = model.encode(X)
            if model.head == 'wta':
                l_mf, xh = wta_loss(model, mem, Y); mf_raw = l_mf.detach(); xh = xh.float()
            else:
                l_mf, mf_raw = meanflow_loss(model, mem, Y, p_neq=tc['p_neq'])
                e = torch.randn_like(Y)
                xh = one_step(model, mem, e).float()
            v, s = model.heads(mem)
        l_val = (v.float() - V).abs().mean(); l_sub = (s.float() - SG).abs().mean()
        d2 = xh[:, 2:] - 2 * xh[:, 1:-1] + xh[:, :-2]
        l_sm = d2.pow(2).mean()
        l_fk = torch.zeros((), device=dev)
        if tc['w_fk'] > 0:
            q = q_from_feat(X)
            ks = torch.tensor([3, 7, 11, 15], device=dev)
            qh = q[:, None] + torch.cumsum(xh.clamp(-1, 1) * stepv, 1)[:, ks]
            qt = q[:, None] + torch.cumsum(Y * stepv, 1)[:, ks]
            B4 = qh.shape[0] * 4
            ph = fk_points(kin, qh.reshape(B4, 6), fk_idx); pt = fk_points(kin, qt.reshape(B4, 6), fk_idx)
            l_fk = ((ph - pt).norm(dim=-1) / 10).mean()
        loss = tc['w_mf'] * l_mf + tc['w_fk'] * l_fk + tc['w_smooth'] * l_sm + tc['w_value'] * l_val + tc['w_sub'] * l_sub
        opt.zero_grad(set_to_none=True)
        loss.backward()
        gn = torch.nn.utils.clip_grad_norm_(model.parameters(), tc['grad_clip'])
        if not torch.isfinite(loss):
            print('非有限损失 → 训练发散（M5 门禁：触发 WTA 回退条件）', flush=True); save('diverged.pt', step); break
        opt.step(); seen += X.shape[0]
        if step % tc['log_every'] == 0:
            el = time.time() - t0
            rec = {'step': step, 'loss': loss.item(), 'mf': mf_raw.item(), 'fk_cm': l_fk.item(), 'smooth': l_sm.item(),
                   'value': l_val.item(), 'sub': l_sub.item(), 'gnorm': gn.item(), 'lr': lr_at(step), 'samples_per_s': seen / max(el, 1e-6),
                   'eta_h': (total - step - 1) * tc['batch'] / max(seen / max(el, 1e-6), 1e-6) / 3600}
            print(json.dumps(rec), flush=True); log.write(json.dumps(rec) + '\n'); log.flush()
        if not a.no_eval and (step + 1) % tc['eval_every'] == 0:
            model.eval()
            res, _ = evaluate(model, kin, val_tasks, dev)
            res['step'] = step + 1
            print('EVAL', json.dumps(res), flush=True); log.write(json.dumps({'eval': res}) + '\n'); log.flush()
            save('last.pt', step + 1)
            if res['success'] > best + 1e-4:
                best = res['success']; bad_evals = 0; save('best.pt', step + 1, {'eval': res})
            else:
                bad_evals += 1
                if bad_evals >= tc['patience_evals']: print('闭环到达率不再上升，停止', flush=True); break
    save('last.pt', step + 1)
    if a.no_eval: save('best.pt', step + 1)
    print(f'done, best success {best:.4f}, {(time.time() - t0) / 3600:.2f} h', flush=True)


if __name__ == '__main__':
    main()