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

const PLANAR_URDF = `
<robot name="planar_checks">
  <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 0 1"/>
    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2" acceleration="4"/>
  </joint>
  <joint name="joint_2" type="prismatic">
    <parent link="link_1"/>
    <child link="tool0"/>
    <axis xyz="1 0 0"/>
    <limit lower="0" upper="1" velocity="0.5" acceleration="1"/>
  </joint>
</robot>
`;

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

function poseTarget(id: string, x: number, y: number): PoseTarget {
  return {
    id,
    pose: {
      position: [x, y, 0],
      quaternion: [0, 0, 0, 1]
    }
  };
}

describe("Jacobian, singularity, limits, and reachability checks", () => {
  it("computes a 6xdof Jacobian with expected linear components", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "jacobian",
      payload: [handle, [Math.PI / 2, 0.4]]
    });

    expect(response.ok).toBe(true);
    const jacobian = response.result as JacobianResult;
    expect(jacobian.rows).toBe(6);
    expect(jacobian.cols).toBe(2);
    expect(jacobian.data).toHaveLength(12);
    expect(jacobian.data[0]).toBeCloseTo(-0.4, 4);
    expect(jacobian.data[1]).toBeCloseTo(0, 4);
    expect(jacobian.data[2]).toBeCloseTo(0, 4);
    expect(jacobian.data[3]).toBeCloseTo(1, 4);
  });

  it("reports singularity warning for collapsed planar reach", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "checkSingularity",
      payload: [handle, [0, 0]]
    });

    expect(response.result).toMatchObject({
      ok: true,
      nearSingularity: true,
      diagnostics: [
        {
          severity: "warning",
          code: "KDL_SINGULARITY"
        }
      ]
    });
  });

  it("checks joint and velocity limits with structured diagnostics", async () => {
    const { runtime, handle } = await createRobot();
    const jointResponse = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "checkJointLimits",
      payload: [handle, [0, 2]]
    });
    expect(jointResponse.result).toMatchObject({
      ok: false,
      diagnostics: [
        {
          severity: "error",
          code: "KDL_JOINT_LIMIT"
        }
      ]
    });

    const velocityResponse = await dispatchKdlRpcRequest(runtime, {
      id: 6,
      method: "checkVelocityLimits",
      payload: [
        handle,
        {
          points: [
            {
              jointVelocity: [1, 0.75],
              jointAcceleration: [1, 1.5]
            }
          ]
        }
      ]
    });
    expect(velocityResponse.result).toMatchObject({
      ok: false,
      maxJointVelocityRatio: 1.5,
      maxJointAccelerationRatio: 1.5,
      diagnostics: [
        {
          severity: "error",
          code: "KDL_VELOCITY_LIMIT",
          pointIndex: 0
        },
        {
          severity: "error",
          code: "KDL_ACCEL_LIMIT",
          pointIndex: 0
        }
      ]
    });
  });

  it("checks reachability and preserves batch order", async () => {
    const { runtime, handle } = await createRobot();
    const reachable = await dispatchKdlRpcRequest(runtime, {
      id: 7,
      method: "checkReachability",
      payload: [handle, poseTarget("ok", 0, 0.3), { positionTolerance: 1e-9 }]
    });
    expect(reachable.result).toMatchObject({
      ok: true,
      reachable: true,
      targetId: "ok",
      joints: [Math.PI / 2, 0.3]
    });

    const batch = await dispatchKdlRpcRequest(runtime, {
      id: 8,
      method: "checkReachabilityBatch",
      payload: [handle, [poseTarget("a", 0.2, 0), poseTarget("b", 2, 0)], {}]
    });
    const results = batch.result as ReachabilityResult[];
    expect(results.map((result) => result.targetId)).toEqual(["a", "b"]);
    expect(results[0]?.reachable).toBe(true);
    expect(results[1]).toMatchObject({
      reachable: false,
      diagnostics: [
        {
          severity: "error",
          code: "KDL_JOINT_LIMIT"
        }
      ]
    });
  });
});