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

const PLANAR_URDF = `
<robot name="planar_ik">
  <link name="base_link"/>
  <link name="link_1"/>
  <link name="tool0"/>
  <joint name="joint_1" type="revolute">
    <parent link="base_link"/>
    <child link="link_1"/>
    <origin xyz="0 0 0" 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="tool0"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="0" upper="1" velocity="0.3" acceleration="1.2"/>
  </joint>
</robot>
`;

const UNSUPPORTED_URDF = `
<robot name="unsupported_ik">
  <link name="base_link"/>
  <link name="link_1"/>
  <link name="tool0"/>
  <joint name="joint_1" type="revolute">
    <parent link="base_link"/>
    <child link="link_1"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.14" upper="3.14" velocity="2.5" acceleration="5"/>
  </joint>
  <joint name="joint_2" type="revolute">
    <parent link="link_1"/>
    <child link="tool0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" velocity="2.5" acceleration="5"/>
  </joint>
</robot>
`;

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

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

describe("IK and ikBatch", () => {
  it("solves a reachable planar target and FK back-substitution is within tolerance", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "ik",
      payload: [handle, [0, 0], pose(0, 0.4), { positionTolerance: 1e-9 }]
    });

    expect(response.ok).toBe(true);
    const result = response.result as IkResult;
    expect(result.ok).toBe(true);
    expect(result.joints?.[0]).toBeCloseTo(Math.PI / 2);
    expect(result.joints?.[1]).toBeCloseTo(0.4);
    expect(result.residualPosition).toBeLessThan(1e-9);

    const fk = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "fk",
      payload: [handle, result.joints]
    });
    const fkResult = fk.result as { tcp: { position: number[] } };
    expect(fkResult.tcp.position[0]).toBeCloseTo(0);
    expect(fkResult.tcp.position[1]).toBeCloseTo(0.4);
  });

  it("keeps ikBatch results in input order", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "ikBatch",
      payload: [
        handle,
        [
          [0, 0],
          [0, 0]
        ],
        [pose(0.2, 0), pose(0, 0.3)],
        {}
      ]
    });

    expect(response.ok).toBe(true);
    const results = response.result as IkResult[];
    expect(results).toHaveLength(2);
    expect(results[0]?.joints?.[0]).toBeCloseTo(0);
    expect(results[0]?.joints?.[1]).toBeCloseTo(0.2);
    expect(results[1]?.joints?.[0]).toBeCloseTo(Math.PI / 2);
    expect(results[1]?.joints?.[1]).toBeCloseTo(0.3);
  });

  it("returns joint_limit reason when the candidate exceeds limits", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 6,
      method: "ik",
      payload: [handle, [0, 0], pose(2, 0), {}]
    });

    expect(response.ok).toBe(true);
    expect(response.result).toMatchObject({
      ok: false,
      reason: "joint_limit",
      diagnostics: [
        {
          severity: "error",
          code: "KDL_JOINT_LIMIT"
        }
      ]
    });
  });

  it("returns invalid_model reason for unsupported IK chains", async () => {
    const { runtime, handle } = await createRobot(UNSUPPORTED_URDF);
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 7,
      method: "ik",
      payload: [handle, [0, 0], pose(0.2, 0), {}]
    });

    expect(response.ok).toBe(true);
    expect(response.result).toMatchObject({
      ok: false,
      reason: "invalid_model",
      diagnostics: [
        {
          severity: "error",
          code: "KDL_IK_UNSUPPORTED_MODEL"
        }
      ]
    });
  });
});