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

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

function request(overrides: Partial<MoveCRequest>): MoveCRequest {
  return {
    startJoints: [0, 0.5],
    via: {
      id: "via",
      pose: {
        position: [0.5, 0.5, 0],
        quaternion: [0, 0, 0, 1]
      }
    },
    target: {
      id: "arc_goal",
      pose: {
        position: [0, 0.5, 0],
        quaternion: [0, 0, 0, 1]
      }
    },
    speed: {
      kind: "linear",
      velocity: 0.25,
      acceleration: 1
    },
    zone: {
      kind: "fine"
    },
    sampleTime: 0.05,
    ...overrides
  };
}

describe("planMoveC", () => {
  it("plans a circular TCP arc with circle metadata", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "planMoveC",
      payload: [handle, request({})]
    });

    expect(response.ok).toBe(true);
    const trajectory = response.result as TrajectoryResult;
    expect(trajectory.ok).toBe(true);
    expect(trajectory.motion).toBe("MOVEC");
    expect(trajectory.points.length).toBeGreaterThan(2);
    expect(trajectory.points[0]).toMatchObject({
      index: 0,
      time: 0,
      s: 0,
      motion: "MOVEC",
      targetId: "arc_goal"
    });
    expect(trajectory.points.at(-1)?.s).toBe(1);
    expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 5);
    expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.5, 5);

    const circle = trajectory.meta?.circle as {
      center: number[];
      radius: number;
      angle: number;
      length: number;
      direction: "cw" | "ccw";
      maxArcError: number;
    };
    expect(circle.center[0]).toBeCloseTo(0.25);
    expect(circle.center[1]).toBeCloseTo(0.25);
    expect(circle.radius).toBeCloseTo(Math.SQRT1_2 / 2);
    expect(circle.angle).toBeCloseTo(Math.PI);
    expect(circle.length).toBeCloseTo((Math.SQRT1_2 / 2) * Math.PI);
    expect(circle.direction).toBe("ccw");
    expect(circle.maxArcError).toBeLessThan(1e-6);
  });

  it("returns KDL_ARC_DEGENERATE for collinear points", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "planMoveC",
      payload: [
        handle,
        request({
          via: {
            id: "line_mid",
            pose: {
              position: [0.25, 0, 0],
              quaternion: [0, 0, 0, 1]
            }
          },
          target: {
            id: "line_end",
            pose: {
              position: [0.75, 0, 0],
              quaternion: [0, 0, 0, 1]
            }
          }
        })
      ]
    });

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

  it("reports zone approximation and joint-speed approximation warnings", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "planMoveC",
      payload: [
        handle,
        request({
          speed: {
            kind: "joint_abs",
            velocity: 0.25,
            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_MOVEC_JOINT_SPEED_APPROX"
      })
    );
    expect(trajectory.diagnostics).toContainEqual(
      expect.objectContaining({
        severity: "warning",
        code: "KDL_ZONE_APPROX_FINE"
      })
    );
  });
});