import * as THREE from 'three';

/* RL gait/strike synthesis directly on a model's OWN skeleton.
 *
 * The failure of cross-skeleton retargeting is that we must guess each
 * bone's flexion axis. Here we DON'T guess: each driven joint moves as a
 * rotation about a LEARNABLE axis, and CEM (cross-entropy method, a simple
 * evolution strategy) optimizes all joint axes/amplitudes/phases/biases
 * against a physics rollout reward. The optimizer discovers whatever joint
 * motion actually produces forward, balanced walking in the rig's native
 * coordinates — so the result maps perfectly, no retarget.
 *
 * A joint's per-frame local quaternion:
 *     q = rest * axisAngle(axis, bias + amp*sin(2*pi*phase + phi))
 * Params per joint: [ax, ay, az, amp, phi, bias]  (axis auto-normalized)
 */

const _v = new THREE.Vector3(), _q = new THREE.Quaternion();

export class RigController {
  // joints: [{key, bone}] ; feet: {L:boneL, R:boneR}; hips, head bones
  constructor(rig, jointKeys, refs) {
    this.rig = rig;
    this.joints = jointKeys.map(k => ({ key: k, bone: rig.bones[refs.map[k]], rest: null }));
    this.joints.forEach(j => { if (!j.bone) console.warn('RL: missing bone', refs.map[j.key], 'for', j.key); });
    this.joints = this.joints.filter(j => j.bone);
    this.joints.forEach(j => j.rest = j.bone.quaternion.clone());
    this.refs = refs;
    this.footL = rig.bones[refs.footL]; this.footR = rig.bones[refs.footR];
    this.hips = rig.bones[refs.hips]; this.head = rig.bones[refs.head];
    this.skinned = refs.skinnedRoot;
    this.nJ = this.joints.length;
    this.P = 6;                       // params per joint
  }

  randParams(rng) {
    const p = new Float32Array(this.nJ * this.P);
    for (let j = 0; j < this.nJ; j++) {
      const o = j * this.P;
      p[o] = rng() * 2 - 1; p[o + 1] = rng() * 2 - 1; p[o + 2] = rng() * 2 - 1; // axis
      p[o + 3] = rng() * 0.6;              // amp
      p[o + 4] = rng() * Math.PI * 2;      // phase
      p[o + 5] = (rng() * 2 - 1) * 0.4;    // bias
    }
    return p;
  }

  // set all joints to the pose at a given cycle phase (0..1)
  pose(params, phase) {
    const w = 2 * Math.PI * phase;
    for (let j = 0; j < this.nJ; j++) {
      const o = j * this.P;
      let ax = params[o], ay = params[o + 1], az = params[o + 2];
      const n = Math.hypot(ax, ay, az) || 1; ax /= n; ay /= n; az /= n;
      const ang = params[o + 5] + params[o + 3] * Math.sin(w + params[o + 4]);
      _v.set(ax, ay, az); _q.setFromAxisAngle(_v, ang);
      this.joints[j].bone.quaternion.copy(this.joints[j].rest).multiply(_q);
    }
  }

  restPose() { this.joints.forEach(j => j.bone.quaternion.copy(j.rest)); }

  // physics rollout reward for WALK. Inverted-pendulum locomotion: the lower
  // (stance) foot pins to the ground and its backward slide advances the COM.
  walkReward(params, opt = {}) {
    const T = opt.T || 46, dt = opt.dt || 1 / 30, f = opt.freq || 1.4;
    const skinned = this.skinned;
    let comZ = 0, comX = 0, prevStanceZ = null, prevStance = -1;
    let upPen = 0, latPen = 0, clearL = 0, clearR = 0, fall = 0, footYmin = 1e9;
    let energy = 0;
    for (let j = 0; j < this.nJ; j++) energy += params[j * this.P + 3] ** 2;
    // measure rest torso length
    this.restPose(); skinned.updateWorldMatrix(true, true);
    const restHead = this.head.getWorldPosition(new THREE.Vector3());
    const restHip = this.hips.getWorldPosition(new THREE.Vector3());
    const torso0 = restHead.y - restHip.y;
    for (let t = 0; t < T; t++) {
      const phase = (f * t * dt) % 1;
      this.pose(params, phase);
      skinned.updateWorldMatrix(true, true);
      const fl = this.footL.getWorldPosition(new THREE.Vector3());
      const fr = this.footR.getWorldPosition(new THREE.Vector3());
      const hp = this.hips.getWorldPosition(new THREE.Vector3());
      const hd = this.head.getWorldPosition(new THREE.Vector3());
      // stance = lower foot (in world y)
      const stanceL = fl.y <= fr.y;
      const stanceZ = stanceL ? fl.z : fr.z;
      const stance = stanceL ? 0 : 1;
      if (prevStanceZ !== null && stance === prevStance) {
        comZ += -(stanceZ - prevStanceZ);     // foot slides back -> body forward
      }
      prevStanceZ = stanceZ; prevStance = stance;
      // uprightness: torso should keep length & be vertical
      const torso = hd.y - hp.y;
      upPen += (torso0 - torso) ** 2 + (hd.x - hp.x) ** 2 * 0.5;
      latPen += hp.x * hp.x;
      // swing foot clearance
      clearL += Math.max(0, fl.y); clearR += Math.max(0, fr.y);
      footYmin = Math.min(footYmin, fl.y, fr.y);
      if (torso < 0.45 * torso0) fall += 1;
    }
    this.restPose();
    const speed = comZ / (T * dt);
    // reward forward walking speed, balance, foot alternation clearance
    const clearance = Math.min(clearL, clearR) / T;      // both feet must lift
    return 1.6 * speed
      - 3.0 * upPen / T
      - 2.0 * latPen / T
      + 1.2 * clearance
      - 0.15 * energy
      - 2.5 * fall / T
      - 4.0 * Math.max(0, footYmin);      // feet shouldn't float above ground
  }

  cem(rewardFn, opt = {}) {
    const rng = mulberry32(opt.seed || 1);
    const dim = this.nJ * this.P;
    const pop = opt.pop || 64, elite = opt.elite || 10, iters = opt.iters || 40;
    let mu = this.randParams(() => rng());
    let sig = new Float32Array(dim).fill(0.5);
    const hist = [];
    let best = null, bestR = -1e9;
    for (let it = 0; it < iters; it++) {
      const cands = [], rewards = [];
      for (let p = 0; p < pop; p++) {
        const c = new Float32Array(dim);
        for (let d = 0; d < dim; d++) c[d] = mu[d] + sig[d] * gauss(rng);
        const r = rewardFn(c);
        cands.push(c); rewards.push(r);
        if (r > bestR) { bestR = r; best = c.slice(); }
      }
      const idx = rewards.map((r, i) => [r, i]).sort((a, b) => b[0] - a[0]).slice(0, elite).map(x => x[1]);
      const nmu = new Float32Array(dim), nsig = new Float32Array(dim);
      for (const i of idx) for (let d = 0; d < dim; d++) nmu[d] += cands[i][d] / elite;
      for (const i of idx) for (let d = 0; d < dim; d++) nsig[d] += (cands[i][d] - nmu[d]) ** 2 / elite;
      for (let d = 0; d < dim; d++) { mu[d] = nmu[d]; sig[d] = Math.sqrt(nsig[d]) + 0.02; }
      hist.push(bestR);
      if (opt.onIter) opt.onIter(it, bestR);
    }
    return { params: best, reward: bestR, hist };
  }
}

/* ================= PHYSICS-BASED TRAINER =================
 *
 * DeepMimic-lite: the PhysRig's joint motors track the Mixamo clip
 * (sampled via sampleClipTargets), and CEM learns a per-joint RESIDUAL
 * rotation + motor gain that keeps the clip upright under real gravity,
 * mass and contact. The trained weights ARE the animation controller:
 *   theta_j = bias + amp*sin(2πphase+phi) + K·s      (s = feedback state)
 *   target_j(phase) = clip_j(phase) * axisAngle(axis_j, theta_j)
 * Feedback state s (yaw-relative, so it generalizes across headings):
 *   [pelvis-up x, pelvis-up z, com-vel x, com-vel z - targetSpeed]
 * Params per joint: [ax,ay,az, amp, phi, bias, gain, k0,k1,k2,k3]  (P = 11)
 *
 * Cyclic clips (walk) loop phase; opt.oneshot clips (attack, roll) play
 * phase 0..1 once and hold the final pose.
 */
const _tq = new THREE.Quaternion(), _tv = new THREE.Vector3();
const _com = new THREE.Vector3(), _cv = new THREE.Vector3();
const _aq = new THREE.Quaternion();   // actual qRel — MUST be separate from _tq
                                      // (clipTarget returns _tq; aliasing zeroed imErr)

export class PhysTrainer {
  /* rig: PhysRig ; ref: result of sampleClipTargets(walkClip, ...) */
  constructor(rig, ref, opt = {}) {
    this.rig = rig; this.ref = ref;
    this.P = 11;
    this.oneshot = !!opt.oneshot;
    // joints the policy modulates (arms ride along on pure clip tracking)
    this.driven = opt.driven ||
      ['thighL', 'shinL', 'footL', 'thighR', 'shinR', 'footR', 'torso'];
    this.trackOnly = rig.joints.map(j => j.name).filter(n => !this.driven.includes(n));
    this.nJ = this.driven.length;
    this.dim = this.nJ * this.P;
    this.freq = opt.freq || ref.freq;
    this.dt = opt.dt || 1 / 30;
    this.substeps = opt.substeps || 5;
    this.T = opt.T || (this.oneshot
      ? Math.round(ref.duration / this.dt) + 8      // clip once + recovery
      : Math.round(2.2 / this.dt));                 // ~2 gait cycles
    this.targetSpeed = opt.targetSpeed ?? (this.oneshot ? 0 : ref.speed);
    this.assistScale = opt.assistScale ?? 1;        // weaken balance assists (roll)
    this.imWeight = opt.imWeight ?? 0.35;           // imitation (Mixamo-match) reward weight
    this.speedWeight = opt.speedWeight ?? 2.0;      // forward-speed reward weight
    this.distWeight = opt.distWeight ?? 0;          // net forward-distance reward (traversal)
    this.vertWeight = opt.vertWeight ?? 0;          // peak-height reward (jumps leave the ground)
    this.dirZ = opt.dirZ ?? 1;                       // clip forward sign (+1 fwd, -1 backward)
    this.perturbMag = opt.perturbMag ?? 0;          // combat knockback (m/s) injected mid-move
    this.perturbK = opt.perturbK ?? 3;              // fight scenarios averaged per candidate
    this.contactWeight = opt.contactWeight ?? 0;    // reward matching the clip's foot-contact timing
    this._s = new Float32Array(4);                  // feedback state buffer
  }

  /* reference pelvis height at a phase (the clip's authored root bob). Falls
   * back to rest height if the reference lacks it. */
  refHeight(phase) {
    const p = this.ref.pelvisY; if (!p) return this.rig.restPelvisY;
    const n = p.length, x = (((phase % 1) + 1) % 1) * n;
    const i0 = Math.floor(x) % n, i1 = (i0 + 1) % n;
    return p[i0] + (p[i1] - p[i0]) * (x - i0);
  }

  /* yaw-relative balance state: pelvis tilt + com velocity deviation */
  fbState() {
    const s = this._s, p = this.rig.byName.pelvis;
    _tv.set(0, 0, 1).applyQuaternion(p.quat);
    const yaw = Math.atan2(_tv.x, _tv.z), sy = Math.sin(yaw), cy = Math.cos(yaw);
    _tv.set(0, 1, 0).applyQuaternion(p.quat);                 // pelvis up
    s[0] = cy * _tv.x - sy * _tv.z;
    s[1] = sy * _tv.x + cy * _tv.z;
    this.rig.comVel(_cv);
    s[2] = cy * _cv.x - sy * _cv.z;
    s[3] = (sy * _cv.x + cy * _cv.z) - this.targetSpeed;      // forward vel error
    return s;
  }

  zeroParams() {
    const p = new Float32Array(this.dim);
    for (let j = 0; j < this.nJ; j++) {
      p[j * this.P] = 1;          // axis x (sagittal flexion default)
      p[j * this.P + 6] = 1;      // gain
    }
    return p;
  }

  clipTarget(name, phase) {  // interpolated clip target quat for a joint
    const f = this.ref.frames, n = this.ref.nPhase;
    if (this.oneshot) {      // clamp: play once, hold last frame
      const x = Math.min(Math.max(phase, 0), 1) * (n - 1);
      const i0 = Math.floor(x), i1 = Math.min(i0 + 1, n - 1);
      return _tq.copy(f[i0][name]).slerp(f[i1][name], x - i0);
    }
    const x = ((phase % 1) + 1) % 1 * n;
    const i0 = Math.floor(x) % n, i1 = (i0 + 1) % n;
    return _tq.copy(f[i0][name]).slerp(f[i1][name], x - i0);
  }

  /* Compute this gait's motor target per joint at `phase` WITHOUT applying
   * them, so an AnimController can blend targets across several gaits. Writes
   * {q, gain} into `out[jointName]` (out reused across calls). Reads the live
   * rig state for the feedback term, so all blended gaits see the same pose. */
  computeTargets(params, phase, out = {}) {
    const s = this.fbState();
    const w = 2 * Math.PI * phase;
    for (const name of this.trackOnly) {
      const e = out[name] || (out[name] = { q: new THREE.Quaternion(), gain: 1 });
      e.q.copy(this.clipTarget(name, phase)); e.gain = 1;
    }
    for (let j = 0; j < this.nJ; j++) {
      const o = j * this.P, name = this.driven[j];
      let ax = params[o], ay = params[o + 1], az = params[o + 2];
      const n = Math.hypot(ax, ay, az) || 1; ax /= n; ay /= n; az /= n;
      let ang = params[o + 5] + params[o + 3] * Math.sin(w + params[o + 4])
        + params[o + 7] * s[0] + params[o + 8] * s[1]
        + params[o + 9] * s[2] + params[o + 10] * s[3];
      ang = Math.max(-1.2, Math.min(1.2, ang));   // residual safety clamp
      const gain = Math.max(0.2, Math.min(4, Math.abs(params[o + 6])));
      _tv.set(ax, ay, az);
      const e = out[name] || (out[name] = { q: new THREE.Quaternion(), gain: 1 });
      e.q.copy(this.clipTarget(name, phase)).multiply(_aq.setFromAxisAngle(_tv, ang));
      e.gain = gain;
    }
    return out;
  }

  /* set all motor targets for a phase given params (single-gait playback) */
  drive(params, phase) {
    const rig = this.rig;
    rig.hoverTarget = this.vertWeight > 0 ? null : this.refHeight(phase);  // track clip bob
    for (const name of this.trackOnly)
      rig.setTarget(name, this.clipTarget(name, phase).clone(), 1);
    const s = this.fbState();
    const w = 2 * Math.PI * phase;
    for (let j = 0; j < this.nJ; j++) {
      const o = j * this.P, name = this.driven[j];
      let ax = params[o], ay = params[o + 1], az = params[o + 2];
      const n = Math.hypot(ax, ay, az) || 1; ax /= n; ay /= n; az /= n;
      let ang = params[o + 5] + params[o + 3] * Math.sin(w + params[o + 4])
        + params[o + 7] * s[0] + params[o + 8] * s[1]
        + params[o + 9] * s[2] + params[o + 10] * s[3];
      ang = Math.max(-1.2, Math.min(1.2, ang));   // residual safety clamp
      const gain = Math.max(0.2, Math.min(4, Math.abs(params[o + 6])));
      _tv.set(ax, ay, az);
      const t = this.clipTarget(name, phase).clone()
        .multiply(new THREE.Quaternion().setFromAxisAngle(_tv, ang));
      rig.setTarget(name, t, gain);
    }
  }

  /* physics rollout; returns reward. opt.onFrame(rig, t) for playback/recording */
  rollout(params, opt = {}) {
    const rig = this.rig, T = opt.T || this.T;
    rig.reset();
    const up0 = rig.uprightK, hv0 = rig.hoverK;
    rig.uprightK *= this.assistScale; rig.hoverK *= this.assistScale;
    try { return this._rollout(params, T, opt); }
    finally { rig.uprightK = up0; rig.hoverK = hv0; }
  }
  /* a fight scenario = list of knockback pokes {step, v:[x,y,z]} sampled to
   * mimic getting hit mid-move (magnitude ~ combat knockback). Fixed per
   * training so CEM compares candidates on identical disturbances. */
  makeScenarios(rng) {
    const K = this.perturbK, mag = this.perturbMag, out = [];
    for (let k = 0; k < K; k++) {
      const pokes = [];
      const nP = 1 + Math.floor(rng() * 2);
      for (let p = 0; p < nP; p++) {
        const a = rng() * 2 * Math.PI, s = mag * (0.5 + rng());
        pokes.push({ step: Math.floor(rng() * this.T * 0.7) + 2,
                     v: [Math.cos(a) * s, (rng() - 0.3) * s * 0.4, Math.sin(a) * s] });
      }
      out.push(pokes);
    }
    out[0] = [];                          // one clean scenario (no hit) always
    return out;
  }

  _rollout(params, T, opt) {
    const rig = this.rig;
    const pelvis = rig.byName.pelvis;
    const y0 = rig.restPelvisY;
    const schedule = opt.schedule;        // fight-context knockback pokes
    let imErr = 0, upPen = 0, latPen = 0, fell = 0, contactPen = 0;
    let energy = 0;
    for (let j = 0; j < this.nJ; j++) energy += params[j * this.P + 3] ** 2;
    const start = rig.comPos(new THREE.Vector3());
    const vert = this.vertWeight > 0;                          // jump: reward leaving ground
    let peakY = -1e9, steps = 0;
    for (let t = 0; t < T; t++) {
      let phase = this.freq * t * this.dt;
      if (!this.oneshot) phase %= 1;
      this.drive(params, phase);
      rig.step(this.dt, this.substeps);
      steps++;
      if (schedule) for (const pk of schedule) if (pk.step === t)
        for (const b of rig.bodies) { b.vel.x += pk.v[0]; b.vel.y += pk.v[1]; b.vel.z += pk.v[2]; }
      // imitation error on driven joints (match the Mixamo clip)
      for (const name of this.driven) {
        const j = rig.jointByName[name];
        _aq.copy(j.p.quat).invert().multiply(j.c.quat);       // actual qRel
        const tgt = this.clipTarget(name, phase);             // returns _tq — keep separate
        const d = Math.abs(_aq.dot(tgt));
        imErr += 2 * Math.acos(Math.min(1, d));
      }
      // upright: body-up near world-up; height tracks the clip's authored root
      // trajectory (refHeight) rather than a constant — this is the root-pose
      // imitation term that was missing and caused the bounce.
      _tv.set(0, 1, 0).applyQuaternion(pelvis.quat);
      upPen += (1 - _tv.y) + (vert ? 0 : Math.abs(pelvis.pos.y - this.refHeight(phase)) * 3);
      latPen += Math.abs(pelvis.pos.x - start.x);
      peakY = Math.max(peakY, pelvis.pos.y);
      // contact matching: foot should be planted (low, still) when the clip says
      if (this.contactWeight > 0) {
        const fc = this.ref.footContact, ci = Math.floor(((phase % 1) + 1) % 1 * fc.L.length) % fc.L.length;
        // timing only: foot should be DOWN when the clip says down, UP when up.
        // Do NOT penalize its horizontal velocity — in in-place tracking the
        // stance foot slides backward and that slide IS the propulsion.
        for (const [name, want] of [['footL', fc.L[ci]], ['footR', fc.R[ci]]]) {
          const fb = rig.byName[name];
          const grounded = fb.pos.y < fb.r * 1.7 ? 1 : 0;
          contactPen += want ? (1 - grounded) : grounded * 0.25;
        }
      }
      if (opt.onFrame) opt.onFrame(rig, t);
      if (pelvis.pos.y < y0 * 0.55) {                          // fallen?
        if (!this.oneshot || this.assistScale >= 1) { fell = 1; break; }
        else if (t > T - 10) fell += 0.25;   // tumbling is fine; ending down is not
      }
    }
    const com = rig.comPos(new THREE.Vector3());
    const dist = this.dirZ * (com.z - start.z);               // signed by clip's forward dir
    const dur = steps * this.dt;
    const speed = dist / Math.max(dur, 1e-3);
    const speedR = 1 - Math.min(1.5, Math.abs(speed - this.targetSpeed) / Math.max(this.targetSpeed, 0.3));
    return this.speedWeight * speedR
      + this.distWeight * Math.max(0, Math.min(4, dist))   // net forward traversal
      + this.vertWeight * Math.max(0, peakY - y0)          // jump apex above rest
      + 2.5 * (steps / T)                    // survival
      - this.imWeight * imErr / (steps * this.nJ)
      - 1.2 * upPen / steps
      - 0.6 * latPen / steps
      - this.contactWeight * contactPen / steps
      - 0.08 * energy
      - 2.0 * fell;
  }

  /* Imitation fidelity: fraction of the Mixamo clip's joint motion actually
   * reproduced by the physics rollout. 1.0 = tracks the clip exactly; ~0 =
   * ignores it (stands at rest). meanErr = actual-vs-clip angle; meanMotion =
   * clip-vs-neutral angle (how much the clip itself moves). */
  fidelity(params) {
    const rig = this.rig, T = this.T;
    rig.reset();
    const up0 = rig.uprightK, hv0 = rig.hoverK;
    rig.uprightK *= this.assistScale; rig.hoverK *= this.assistScale;
    let err = 0, motion = 0, cnt = 0;
    try {
      for (let t = 0; t < T; t++) {
        let phase = this.freq * t * this.dt;
        if (!this.oneshot) phase %= 1;
        this.drive(params, phase);
        rig.step(this.dt, this.substeps);
        for (const name of this.driven) {
          const j = rig.jointByName[name];
          _aq.copy(j.p.quat).invert().multiply(j.c.quat);     // actual qRel
          const tgt = this.clipTarget(name, phase);           // clip target (_tq)
          err += 2 * Math.acos(Math.min(1, Math.abs(_aq.dot(tgt))));
          motion += 2 * Math.acos(Math.min(1, Math.abs(tgt.w)));  // clip vs neutral
          cnt++;
        }
      }
    } finally { rig.uprightK = up0; rig.hoverK = hv0; rig.reset(); }
    const meanErr = err / cnt, meanMotion = motion / cnt;
    const fid = Math.max(0, Math.min(1, 1 - meanErr / Math.max(meanMotion, 1e-3)));
    return { fidelity: fid, meanErrDeg: +(meanErr * 180 / Math.PI).toFixed(1),
             meanMotionDeg: +(meanMotion * 180 / Math.PI).toFixed(1) };
  }

  /* CEM over residual params, seeded at zero-residual (= pure clip tracking).
   * Async: yields to the event loop each iteration so a HUD can repaint. */
  async cem(opt = {}) {
    const rng = mulberry32(opt.seed || 1);
    const pop = opt.pop || 40, elite = opt.elite || 8, iters = opt.iters || 30;
    let mu = this.zeroParams();
    const sig = new Float32Array(this.dim);
    for (let j = 0; j < this.nJ; j++) {
      const o = j * this.P;
      sig[o] = sig[o + 1] = sig[o + 2] = 0.4;   // axis
      sig[o + 3] = 0.25;                        // amp
      sig[o + 4] = 1.5;                         // phase
      sig[o + 5] = 0.15;                        // bias
      sig[o + 6] = 0.4;                         // gain
      sig[o + 7] = sig[o + 8] = sig[o + 9] = sig[o + 10] = 0.12; // feedback gains
    }
    // fight-context: average each candidate over fixed knockback scenarios so
    // the learned feedback gains reproduce the animation even while being hit
    const scen = this.perturbMag > 0 ? this.makeScenarios(rng) : [null];
    const evalR = c => { let s = 0; for (const sch of scen) s += this.rollout(c, { schedule: sch }); return s / scen.length; };
    let best = mu.slice(), bestR = evalR(mu);
    const hist = [bestR];
    for (let it = 0; it < iters; it++) {
      const cands = [], rewards = [];
      for (let p = 0; p < pop; p++) {
        const c = new Float32Array(this.dim);
        for (let d = 0; d < this.dim; d++) c[d] = mu[d] + sig[d] * gauss(rng);
        const r = evalR(c);
        cands.push(c); rewards.push(r);
        if (r > bestR) { bestR = r; best = c.slice(); }
      }
      const idx = rewards.map((r, i) => [r, i]).sort((a, b) => b[0] - a[0]).slice(0, elite).map(x => x[1]);
      const nmu = new Float32Array(this.dim), nsig = new Float32Array(this.dim);
      for (const i of idx) for (let d = 0; d < this.dim; d++) nmu[d] += cands[i][d] / elite;
      for (const i of idx) for (let d = 0; d < this.dim; d++) nsig[d] += (cands[i][d] - nmu[d]) ** 2 / elite;
      for (let d = 0; d < this.dim; d++) { mu[d] = nmu[d]; sig[d] = Math.sqrt(nsig[d]) + 0.01; }
      hist.push(bestR);
      if (opt.onIter) opt.onIter(it, bestR);
      // MessageChannel yield: repaints HUD but isn't throttled in background tabs
      await new Promise(r => { const c = new MessageChannel(); c.port1.onmessage = r; c.port2.postMessage(0); });
    }
    return { params: best, reward: bestR, hist };
  }

  /* portable trained-weights blob */
  exportWeights(params, meta = {}) {
    return {
      kind: 'physgait-v1',
      driven: this.driven, P: this.P, oneshot: this.oneshot, assistScale: this.assistScale,
      params: Array.from(params).map(x => +x.toFixed(5)),
      freq: this.freq, dt: this.dt, substeps: this.substeps,
      targetSpeed: this.targetSpeed, ...meta,
    };
  }
}

function mulberry32(a) {
  return function () {
    a |= 0; a = a + 0x6D2B79F5 | 0;
    let t = Math.imul(a ^ a >>> 15, 1 | a);
    t = t + Math.imul(t ^ t >>> 7, 61 | t) ^ t;
    return ((t ^ t >>> 14) >>> 0) / 4294967296;
  };
}
function gauss(rng) {
  let u = 0, v = 0; while (u === 0) u = rng(); while (v === 0) v = rng();
  return Math.sqrt(-2 * Math.log(u)) * Math.cos(2 * Math.PI * v);
}