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

const SIMPLE_URDF = `
<robot name="simple_fk">
  <link name="base_link"/>
  <link name="link_1"/>
  <link name="link_2"/>
  <link name="tool0"/>
  <joint name="joint_1" type="revolute">
    <parent link="base_link"/>
    <child link="link_1"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
  </joint>
  <joint name="joint_2" type="prismatic">
    <parent link="link_1"/>
    <child link="link_2"/>
    <origin xyz="0 0 0.2" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="0" upper="0.4" velocity="0.3" acceleration="1.2"/>
  </joint>
  <joint name="tool_fixed" type="fixed">
    <parent link="link_2"/>
    <child link="tool0"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
  </joint>
</robot>
`;

async function createRuntimeRobot() {
  const runtime = createKdlWorkerRuntime();
  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
  const response = await dispatchKdlRpcRequest(runtime, {
    id: 2,
    method: "loadRobotFromUrdf",
    payload: [
      SIMPLE_URDF,
      {
        robotId: "fk",
        baseLink: "base_link",
        tipLink: "tool0"
      }
    ]
  });
  expect(response.ok).toBe(true);
  return { runtime, handle: response.result as number };
}

describe("FK and fkAllLinks", () => {
  it("computes flange and tcp poses for the zero joint state", async () => {
    const { runtime, handle } = await createRuntimeRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "fk",
      payload: [handle, [0, 0]]
    });

    expect(response.ok).toBe(true);
    const result = response.result as {
      ok: boolean;
      joints: number[];
      diagnostics: unknown[];
      flange: { position: number[]; quaternion: number[] };
      tcp: { position: number[]; quaternion: number[] };
    };
    expect(result.ok).toBe(true);
    expect(result.joints).toEqual([0, 0]);
    expect(result.diagnostics).toEqual([]);
    expect(result.flange.position[0]).toBeCloseTo(0);
    expect(result.flange.position[1]).toBeCloseTo(0);
    expect(result.flange.position[2]).toBeCloseTo(0.35);
    expect(result.flange.quaternion).toEqual([0, 0, 0, 1]);
    expect(result.tcp.position[0]).toBeCloseTo(0);
    expect(result.tcp.position[1]).toBeCloseTo(0);
    expect(result.tcp.position[2]).toBeCloseTo(0.35);
    expect(result.tcp.quaternion).toEqual([0, 0, 0, 1]);
  });

  it("applies revolute and prismatic joint motion in chain order", async () => {
    const { runtime, handle } = await createRuntimeRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "fk",
      payload: [handle, [Math.PI / 2, 0.2]]
    });

    expect(response.ok).toBe(true);
    const result = response.result as { flange: { position: number[]; quaternion: number[] } };
    expect(result.flange.position[0]).toBeCloseTo(0);
    expect(result.flange.position[1]).toBeCloseTo(0.2);
    expect(result.flange.position[2]).toBeCloseTo(0.35);
    expect(result.flange.quaternion[2]).toBeCloseTo(Math.SQRT1_2);
    expect(result.flange.quaternion[3]).toBeCloseTo(Math.SQRT1_2);
  });

  it("returns link poses in base-to-tip order", async () => {
    const { runtime, handle } = await createRuntimeRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "fkAllLinks",
      payload: [handle, [0, 0.1]]
    });

    expect(response.ok).toBe(true);
    const result = response.result as {
      linkPoses: Array<{ link: string; pose: { position: number[] } }>;
    };
    expect(result.linkPoses.map((entry) => entry.link)).toEqual(["base_link", "link_1", "link_2", "tool0"]);
    expect(result.linkPoses[0]?.pose.position).toEqual([0, 0, 0]);
    expect(result.linkPoses[3]?.pose.position[0]).toBeCloseTo(0.1);
    expect(result.linkPoses[3]?.pose.position[2]).toBeCloseTo(0.35);
  });

  it("applies tool offset to tcp without changing flange", async () => {
    const { runtime, handle } = await createRuntimeRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "fk",
      payload: [
        handle,
        [0, 0],
        {
          tool: {
            position: [0, 0, 0.1],
            quaternion: [0, 0, 0, 1]
          }
        }
      ]
    });

    expect(response.ok).toBe(true);
    const result = response.result as {
      flange: { position: number[] };
      tcp: { position: number[] };
    };
    expect(result.flange.position[2]).toBeCloseTo(0.35);
    expect(result.tcp.position[2]).toBeCloseTo(0.45);
  });

  it("writes tcp pose into a reusable Float64Array", async () => {
    const { runtime, handle } = await createRuntimeRobot();
    const out = new Float64Array(7);
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 51,
      method: "fkPose7",
      payload: [handle, new Float64Array([Math.PI / 2, 0.2]), out]
    });

    expect(response.ok).toBe(true);
    expect(response.result).toBe(out);
    expect(out[0]).toBeCloseTo(0);
    expect(out[1]).toBeCloseTo(0.2);
    expect(out[2]).toBeCloseTo(0.35);
    expect(out[5]).toBeCloseTo(Math.SQRT1_2);
    expect(out[6]).toBeCloseTo(Math.SQRT1_2);
  });

  it("returns a structured error for undersized fkPose7 output buffers", async () => {
    const { runtime, handle } = await createRuntimeRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 52,
      method: "fkPose7",
      payload: [handle, [0, 0], new Float64Array(6)]
    });

    expect(response).toMatchObject({
      ok: false,
      error: {
        code: "KDL_OUTPUT_DIMENSION_MISMATCH"
      }
    });
  });

  it("returns a structured dimension diagnostic", async () => {
    const { runtime, handle } = await createRuntimeRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 6,
      method: "fk",
      payload: [handle, [0]]
    });

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