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

const PLANAR_URDF = `
<robot name="movel_planar">
  <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="10"/>
  </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="1" acceleration="10"/>
  </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: "movel",
        baseLink: "base_link",
        tipLink: "tool0"
      }
    ]
  });
  expect(response.ok).toBe(true);
  return { runtime, handle: response.result as number };
}

function request(overrides: Partial<MoveLRequest>): MoveLRequest {
  return {
    startJoints: [Math.PI / 2, 0.2],
    target: {
      id: "line_goal",
      pose: {
        position: [0, 0.6, 0],
        quaternion: [0, 0, 0, 1]
      }
    },
    speed: {
      kind: "linear",
      velocity: 0.2,
      acceleration: 1
    },
    zone: {
      kind: "fine"
    },
    sampleTime: 0.05,
    ...overrides
  };
}

describe("planMoveL", () => {
  it("plans a TCP straight-line trajectory with continuous IK seeds", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "planMoveL",
      payload: [handle, request({})]
    });

    expect(response.ok).toBe(true);
    const trajectory = response.result as TrajectoryResult;
    expect(trajectory.ok).toBe(true);
    expect(trajectory.motion).toBe("MOVEL");
    expect(trajectory.points.length).toBeGreaterThan(2);
    expect(trajectory.points[0]).toMatchObject({
      index: 0,
      time: 0,
      s: 0,
      motion: "MOVEL",
      targetId: "line_goal"
    });
    expect(trajectory.points.at(-1)?.s).toBe(1);
    expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 6);
    expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.6, 6);
    expect(trajectory.meta).toMatchObject({
      targetId: "line_goal",
      orientationMode: "fixed"
    });
    expect(trajectory.meta?.length as number).toBeCloseTo(0.4);

    for (const point of trajectory.points) {
      expect(point.tcp.position[0]).toBeCloseTo(0, 5);
      expect(point.tcp.position[2]).toBeCloseTo(0, 5);
      expect(point.tcp.position[1]).toBeGreaterThanOrEqual(0.2 - 1e-9);
      expect(point.tcp.position[1]).toBeLessThanOrEqual(0.6 + 1e-9);
    }
  });

  it("returns a failed trajectory when a sampled pose is unreachable", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "planMoveL",
      payload: [
        handle,
        request({
          target: {
            id: "far_goal",
            pose: {
              position: [0, 2, 0],
              quaternion: [0, 0, 0, 1]
            }
          }
        })
      ]
    });

    const trajectory = response.result as TrajectoryResult;
    expect(trajectory.ok).toBe(false);
    expect(trajectory.motion).toBe("MOVEL");
    expect(trajectory.diagnostics).toContainEqual(
      expect.objectContaining({
        severity: "error",
        code: "KDL_JOINT_LIMIT"
      })
    );
    expect(trajectory.meta).toMatchObject({
      targetId: "far_goal"
    });
  });

  it("reports zone approximation and joint-speed approximation warnings", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "planMoveL",
      payload: [
        handle,
        request({
          speed: {
            kind: "joint_abs",
            velocity: 0.2,
            acceleration: 1
          },
          zone: {
            kind: "distance",
            value: 0.01
          }
        })
      ]
    });

    const trajectory = response.result as TrajectoryResult;
    expect(trajectory.ok).toBe(true);
    expect(trajectory.diagnostics).toContainEqual(
      expect.objectContaining({
        severity: "warning",
        code: "KDL_MOVEL_JOINT_SPEED_APPROX"
      })
    );
    expect(trajectory.diagnostics).toContainEqual(
      expect.objectContaining({
        severity: "warning",
        code: "KDL_ZONE_APPROX_FINE"
      })
    );
  });
});