import { describe, expect, it } from "vitest";
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
import type { Pose, PoseTarget } from "../../src/kdl/types.js";

async function createRuntime() {
  const runtime = createKdlWorkerRuntime();
  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
  return runtime;
}

function pose(x: number, y: number, z: number): Pose {
  return {
    position: [x, y, z],
    quaternion: [0, 0, 0, 1]
  };
}

describe("pose transform and offset API", () => {
  it("normalizes pose inputs from rpy and quaternion forms", async () => {
    const runtime = await createRuntime();
    const rpyResponse = await dispatchKdlRpcRequest(runtime, {
      id: 2,
      method: "normalizePose",
      payload: [{ xyz: [1, 2, 3], rpy: [0, 0, Math.PI / 2] }]
    });
    const quatResponse = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "normalizePose",
      payload: [{ xyz: [0, 0, 0], quat: [0, 0, 0, 2] }]
    });

    expect(rpyResponse.ok).toBe(true);
    expect((rpyResponse.result as Pose).position).toEqual([1, 2, 3]);
    expect((rpyResponse.result as Pose).quaternion[2]).toBeCloseTo(Math.SQRT1_2);
    expect((rpyResponse.result as Pose).quaternion[3]).toBeCloseTo(Math.SQRT1_2);
    expect(quatResponse.result).toMatchObject({
      position: [0, 0, 0],
      quaternion: [0, 0, 0, 1]
    });
  });

  it("composes poses and computes an inverse pose", async () => {
    const runtime = await createRuntime();
    const composeResponse = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "composePose",
      payload: [pose(1, 0, 0), pose(0, 2, 0)]
    });
    expect(composeResponse.result).toMatchObject({
      position: [1, 2, 0],
      quaternion: [0, 0, 0, 1]
    });

    const inverseResponse = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "inversePose",
      payload: [pose(1, 2, 3)]
    });
    expect(inverseResponse.result).toMatchObject({
      position: [-1, -2, -3],
      quaternion: [0, 0, 0, 1]
    });

    const identityResponse = await dispatchKdlRpcRequest(runtime, {
      id: 6,
      method: "composePose",
      payload: [pose(1, 2, 3), inverseResponse.result]
    });
    expect((identityResponse.result as Pose).position[0]).toBeCloseTo(0);
    expect((identityResponse.result as Pose).position[1]).toBeCloseTo(0);
    expect((identityResponse.result as Pose).position[2]).toBeCloseTo(0);
  });

  it("applies frame, target, and tool using the same order as FK", async () => {
    const runtime = await createRuntime();
    const target: PoseTarget = {
      id: "pick",
      pose: pose(0.5, 0.1, 0.2)
    };
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 7,
      method: "applyToolAndFrame",
      payload: [target, pose(0, 0, 0.18), pose(0.8, 0, 0.2)]
    });

    const result = response.result as Pose;
    expect(result.position[0]).toBeCloseTo(1.3);
    expect(result.position[1]).toBeCloseTo(0.1);
    expect(result.position[2]).toBeCloseTo(0.58);
    expect(result.quaternion).toEqual([0, 0, 0, 1]);
  });

  it("applies offset in frame/world by left composition and tool by right composition", async () => {
    const runtime = await createRuntime();
    const target: PoseTarget = {
      id: "pick",
      pose: {
        position: [1, 2, 3],
        quaternion: [0, 0, Math.SQRT1_2, Math.SQRT1_2]
      },
      frame: pose(10, 0, 0)
    };

    const frameOffset = await dispatchKdlRpcRequest(runtime, {
      id: 8,
      method: "applyOffset",
      payload: [target, { mode: "frame", xyz: [0.1, 0, 0] }]
    });
    const worldOffset = await dispatchKdlRpcRequest(runtime, {
      id: 9,
      method: "applyOffset",
      payload: [target, { mode: "world", xyz: [0, 0.2, 0] }]
    });
    const toolOffset = await dispatchKdlRpcRequest(runtime, {
      id: 10,
      method: "applyOffset",
      payload: [target, { mode: "tool", xyz: [0.1, 0, 0] }]
    });

    expect((frameOffset.result as PoseTarget).id).toBe("pick");
    expect((frameOffset.result as PoseTarget).frame).toEqual(target.frame);
    expect((frameOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1.1);
    expect((frameOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2);
    expect((worldOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1);
    expect((worldOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2.2);
    expect((toolOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1);
    expect((toolOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2.1);
  });

  it("returns structured diagnostics for invalid pose inputs", async () => {
    const runtime = await createRuntime();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 11,
      method: "normalizePose",
      payload: [{ xyz: [1, 2], rpy: [0, 0, 0] }]
    });

    expect(response).toMatchObject({
      ok: false,
      error: {
        code: "KDL_INVALID_POSE",
        diagnostics: [
          {
            severity: "error",
            code: "KDL_INVALID_POSE"
          }
        ]
      }
    });
  });
});