import { expect, test } from '@playwright/test';
import { writeFile } from 'node:fs/promises';

// Isolated development-only kinematic oracle; no UI/production pose-write API.
test('真实 WASM 双相机：腕部随臂运动，前置保持底盘固连，转动底盘两者同步', async ({
  page,
}, info) => {
  await page.goto('/physics/runner.html');
  const result = await page.evaluate(async () => {
    const paths = [
      '/src/simulation/PhysicsAdapter.ts',
      '/src/project/importer.ts',
      '/src/project/builtinRobots.ts',
    ];
    const { MainThreadPhysicsAdapter } = (await import(
      /* @vite-ignore */ paths[0]
    )) as typeof import('../src/simulation/PhysicsAdapter');
    const { importBrowserFiles } = (await import(
      /* @vite-ignore */ paths[1]
    )) as typeof import('../src/project/importer');
    const { BUILTIN_ROBOTS, builtinRobotEnhancements, downloadBuiltinRobot } = (await import(
      /* @vite-ignore */ paths[2]
    )) as typeof import('../src/project/builtinRobots');
    const robot = BUILTIN_ROBOTS[0];
    const source = await importBrowserFiles([
      await downloadBuiltinRobot(robot, new AbortController().signal),
    ]);
    const adapter = new MainThreadPhysicsAdapter();
    try {
      await adapter.load(source, robot.entryPath, {
        robotProfileId: robot.profileId,
        enhancements: builtinRobotEnhancements(robot.id),
      });
      const session = adapter.session!;
      const { model, data, module } = session;
      const jointAddress = (name: string) => {
        for (let id = 0; id < model.njnt; id++) {
          const joint = model.jnt(id);
          try {
            if (joint.name === name) return Number(model.jnt_qposadr[id]);
          } finally {
            joint.delete();
          }
        }
        throw new Error(`Missing joint ${name}`);
      };
      const read = () =>
        Array.from({ length: model.ncam }, (_, id) => {
          const camera = model.cam(id);
          const body = model.body(Number(model.cam_bodyid[id]));
          try {
            return {
              name: camera.name,
              body: body.name,
              position: Array.from(data.cam_xpos.slice(id * 3, id * 3 + 3), Number),
              forward: [2, 5, 8].map((offset) => -Number(data.cam_xmat[id * 9 + offset])),
            };
          } finally {
            camera.delete();
            body.delete();
          }
        });
      const before = read();
      data.qpos[jointAddress('arm_wrist_flex')] += 0.35;
      module.mj_forward(model, data);
      const wristMoved = read();
      const base = jointAddress('__platform_base_freejoint__');
      data.qpos[base + 3] = Math.SQRT1_2;
      data.qpos[base + 4] = 0;
      data.qpos[base + 5] = 0;
      data.qpos[base + 6] = Math.SQRT1_2;
      module.mj_forward(model, data);
      return { before, wristMoved, baseTurned: read(), time: Number(data.time) };
    } finally {
      adapter.dispose();
    }
  });
  expect(result.before).toHaveLength(2);
  const front = (items: typeof result.before) =>
    items.find((camera) => camera.body === 'Camera-Model-v3')!;
  const wrist = (items: typeof result.before) =>
    items.find((camera) => camera.body === 'Camera-Model-v3-1')!;
  const distance = (a: number[], b: number[]) => Math.hypot(...a.map((value, i) => value - b[i]));
  expect(distance(front(result.before).position, front(result.wristMoved).position)).toBeLessThan(
    1e-10,
  );
  expect(distance(front(result.before).forward, front(result.wristMoved).forward)).toBeLessThan(
    1e-10,
  );
  expect(
    distance(wrist(result.before).position, wrist(result.wristMoved).position),
  ).toBeGreaterThan(0.005);
  expect(distance(wrist(result.before).forward, wrist(result.wristMoved).forward)).toBeGreaterThan(
    0.1,
  );
  for (const before of result.wristMoved) {
    const after = result.baseTurned.find((camera) => camera.name === before.name)!;
    const turn = ([x, y, z]: number[]) => [-y, x, z];
    expect(distance(after.position, turn(before.position))).toBeLessThan(1e-8);
    expect(distance(after.forward, turn(before.forward))).toBeLessThan(1e-8);
  }
  expect(result.time).toBe(0);
  await writeFile(info.outputPath('dual-camera-kinematics.json'), JSON.stringify(result, null, 2));
});