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

const SIMPLE_URDF = `
<robot name="simple6">
  <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.14" upper="3.14" 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 1.57"/>
    <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>
`;

describe("URDF to NormalizedRobotModel", () => {
  it("parses links, joints, origins, axes, limits, stable active joint names, and source hash", () => {
    const model = loadRobotFromUrdfModel(SIMPLE_URDF, {
      robotId: "r1",
      baseLink: "base_link",
      tipLink: "tool0"
    });

    expect(model).toMatchObject({
      robotId: "r1",
      name: "simple6",
      baseLink: "base_link",
      tipLink: "tool0",
      activeJointNames: ["joint_1", "joint_2"],
      source: { type: "urdf" }
    });
    expect(model.source.urdfHash).toHaveLength(64);
    expect(model.links.map((link) => link.name)).toEqual(["base_link", "link_1", "link_2", "tool0"]);
    expect(model.joints[0]).toMatchObject({
      name: "joint_1",
      type: "revolute",
      parent: "base_link",
      child: "link_1",
      origin: { xyz: [0, 0, 0.1], rpy: [0, 0, 0] },
      axis: [0, 0, 1]
    });
    expect(model.limits).toEqual([
      { name: "joint_1", lower: -3.14, upper: 3.14, velocity: 2.5, acceleration: 5 },
      { name: "joint_2", lower: 0, upper: 0.4, velocity: 0.3, acceleration: 1.2 }
    ]);
  });

  it("applies joint order and limit overrides", () => {
    const model = loadRobotFromUrdfModel(SIMPLE_URDF, {
      robotId: "r1",
      baseLink: "base_link",
      tipLink: "tool0",
      jointOrder: ["joint_2", "joint_1"],
      overrideLimits: [{ name: "joint_2", velocity: 0.2 }]
    });

    expect(model.activeJointNames).toEqual(["joint_2", "joint_1"]);
    expect(model.limits[0]).toMatchObject({ name: "joint_2", velocity: 0.2 });
  });

  it("returns structured diagnostics for disconnected base and tip links", () => {
    let thrown: unknown;
    try {
      loadRobotFromUrdfModel(SIMPLE_URDF, {
        robotId: "r1",
        baseLink: "tool0",
        tipLink: "base_link"
      });
    } catch (error) {
      thrown = error;
    }

    expect(thrown).toBeInstanceOf(KdlStructuredError);
    expect(thrown).toMatchObject({
      code: "KDL_INVALID_MODEL",
      diagnostics: [
        {
          severity: "error",
          code: "KDL_INVALID_MODEL"
        }
      ]
    });
  });

  it("rejects unsupported joint types", () => {
    const urdf = SIMPLE_URDF.replace('type="prismatic"', 'type="floating"');
    let thrown: unknown;
    try {
      loadRobotFromUrdfModel(urdf, {
        robotId: "r1",
        baseLink: "base_link",
        tipLink: "tool0"
      });
    } catch (error) {
      thrown = error;
    }

    expect(thrown).toBeInstanceOf(KdlStructuredError);
    expect(thrown).toMatchObject({
      code: "KDL_INVALID_MODEL",
      message: expect.stringContaining("Unsupported joint type")
    });
  });

  it("supports RobotHandle lifecycle through the worker runtime", async () => {
    const runtime = createKdlWorkerRuntime();
    await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
    const createResponse = await dispatchKdlRpcRequest(runtime, {
      id: 2,
      method: "loadRobotFromUrdf",
      payload: [
        SIMPLE_URDF,
        {
          robotId: "r1",
          baseLink: "base_link",
          tipLink: "tool0"
        }
      ]
    });

    expect(createResponse).toMatchObject({ ok: true, result: 1 });

    const infoResponse = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "getRobotInfo",
      payload: [1]
    });
    expect(infoResponse.result).toMatchObject({
      handle: 1,
      robotId: "r1",
      name: "simple6",
      dof: 2,
      jointNames: ["joint_1", "joint_2"]
    });

    const limitsResponse = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "getJointLimits",
      payload: [1]
    });
    expect(limitsResponse.result).toEqual([
      { name: "joint_1", lower: -3.14, upper: 3.14, velocity: 2.5, acceleration: 5 },
      { name: "joint_2", lower: 0, upper: 0.4, velocity: 0.3, acceleration: 1.2 }
    ]);

    const destroyResponse = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "destroyRobot",
      payload: [1]
    });
    expect(destroyResponse.ok).toBe(true);

    const afterDestroy = await dispatchKdlRpcRequest(runtime, {
      id: 6,
      method: "getRobotInfo",
      payload: [1]
    });
    expect(afterDestroy).toMatchObject({
      ok: false,
      error: { code: "KDL_INVALID_HANDLE" }
    });
  });
});