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

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

function baseRequest(overrides: Partial<MoveJRequest>): MoveJRequest {
  return {
    startJoints: [0, 0],
    target: {
      id: "joint_goal",
      joints: [0.5, 0.25]
    },
    speed: {
      kind: "joint_abs",
      velocity: 0.5,
      acceleration: 1
    },
    zone: {
      kind: "fine"
    },
    sampleTime: 0.1,
    ...overrides
  };
}

describe("planMoveJ", () => {
  it("plans a synchronized joint trajectory to a joint target", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "planMoveJ",
      payload: [handle, baseRequest({})]
    });

    expect(response.ok).toBe(true);
    const trajectory = response.result as TrajectoryResult;
    expect(trajectory.ok).toBe(true);
    expect(trajectory.motion).toBe("MOVEJ");
    expect(trajectory.points.length).toBeGreaterThan(2);
    expect(trajectory.points[0]).toMatchObject({
      index: 0,
      time: 0,
      s: 0,
      joints: [0, 0],
      motion: "MOVEJ",
      targetId: "joint_goal"
    });
    expect(trajectory.points.at(-1)?.s).toBe(1);
    expect(trajectory.points.at(-1)?.joints[0]).toBeCloseTo(0.5);
    expect(trajectory.points.at(-1)?.joints[1]).toBeCloseTo(0.25);
    expect(trajectory.points.at(-1)?.jointVelocity[0]).toBeCloseTo(0);
    expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0.25 * Math.cos(0.5));
    expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.25 * Math.sin(0.5));
    expect(trajectory.meta).toMatchObject({
      targetType: "joint",
      qStart: [0, 0],
      qEnd: [0.5, 0.25]
    });
  });

  it("uses IK for pose targets and warns when zone is approximated as fine", async () => {
    const { runtime, handle } = await createRobot();
    const target: PoseTarget = {
      id: "pose_goal",
      pose: {
        position: [0, 0.3, 0],
        quaternion: [0, 0, 0, 1]
      }
    };
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "planMoveJ",
      payload: [
        handle,
        baseRequest({
          target,
          zone: { kind: "distance", value: 0.01 }
        })
      ]
    });

    const trajectory = response.result as TrajectoryResult;
    expect(trajectory.ok).toBe(true);
    expect(trajectory.points.at(-1)?.joints[0]).toBeCloseTo(Math.PI / 2);
    expect(trajectory.points.at(-1)?.joints[1]).toBeCloseTo(0.3);
    expect(trajectory.diagnostics).toContainEqual(
      expect.objectContaining({
        severity: "warning",
        code: "KDL_ZONE_APPROX_FINE"
      })
    );
    expect(trajectory.meta).toMatchObject({
      targetType: "pose"
    });
  });

  it("returns a failed trajectory result for endpoint joint limit violations", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "planMoveJ",
      payload: [
        handle,
        baseRequest({
          target: {
            id: "bad_goal",
            joints: [0, 2]
          }
        })
      ]
    });

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

  it("keeps velocity and acceleration within joint limits", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 6,
      method: "planMoveJ",
      payload: [
        handle,
        baseRequest({
          speed: { kind: "joint_percent", value: 1 },
          target: {
            id: "limit_goal",
            joints: [1, 0.5]
          }
        })
      ]
    });

    const trajectory = response.result as TrajectoryResult;
    expect(trajectory.ok).toBe(true);
    for (const point of trajectory.points) {
      expect(Math.abs(point.jointVelocity[0]!)).toBeLessThanOrEqual(1 + 1e-9);
      expect(Math.abs(point.jointVelocity[1]!)).toBeLessThanOrEqual(0.5 + 1e-9);
      expect(Math.abs(point.jointAcceleration[0]!)).toBeLessThanOrEqual(2 + 1e-9);
      expect(Math.abs(point.jointAcceleration[1]!)).toBeLessThanOrEqual(1 + 1e-9);
    }
  });
});