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

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

function pathRequest(overrides: Partial<PathPlanRequest> = {}): PathPlanRequest {
  return {
    startJoints: [0, 0],
    sampleTime: 0.05,
    segments: [
      {
        id: "move-home",
        motion: "MOVEJ",
        target: {
          id: "joint_goal",
          joints: [Math.PI / 2, 0.2]
        },
        speed: { kind: "joint_abs", velocity: 1, acceleration: 4 },
        zone: { kind: "fine" },
        sourceMap: { line: 10, column: 5 }
      },
      {
        id: "line-out",
        motion: "MOVEL",
        target: {
          id: "line_goal",
          pose: {
            position: [0, 0.4, 0],
            quaternion: [0, 0, 0, 1]
          }
        },
        speed: { kind: "linear", velocity: 0.2, acceleration: 1 },
        zone: { kind: "fine" },
        sourceMap: { line: 11, column: 5 }
      }
    ],
    ...overrides
  };
}

describe("planPath and validatePath", () => {
  it("plans multiple motion segments and merges points with segment metadata", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 3,
      method: "planPath",
      payload: [handle, pathRequest()]
    });

    expect(response.ok).toBe(true);
    const result = response.result as PathPlanResult;
    expect(result.ok).toBe(true);
    expect(result.segments).toHaveLength(2);
    expect(result.points.length).toBeGreaterThan(result.segments[0]!.points.length);
    expect(result.points[0]).toMatchObject({
      index: 0,
      time: 0,
      segmentId: "move-home",
      targetId: "joint_goal",
      sourceMap: { line: 10 }
    });
    expect(result.points.at(-1)).toMatchObject({
      segmentId: "line-out",
      targetId: "line_goal",
      sourceMap: { line: 11 }
    });
    expect(result.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 5);
    expect(result.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.4, 5);
    expect(result.duration).toBeCloseTo(result.segments[0]!.duration + result.segments[1]!.duration);
    for (let index = 1; index < result.points.length; index += 1) {
      expect(result.points[index]!.time).toBeGreaterThan(result.points[index - 1]!.time);
      expect(result.points[index]!.index).toBe(index);
    }
  });

  it("validates a path and returns per-segment reports", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 4,
      method: "validatePath",
      payload: [handle, pathRequest()]
    });

    expect(response.ok).toBe(true);
    const result = response.result as PathValidationResult;
    expect(result.ok).toBe(true);
    expect(result.reachable).toBe(true);
    expect(result.cycleTime).toBeGreaterThan(0);
    expect(result.segmentReports.map((report) => report.segmentId)).toEqual(["move-home", "line-out"]);
    expect(result.segmentReports[0]).toMatchObject({
      ok: true,
      motion: "MOVEJ"
    });
    expect(result.segmentReports[1]).toMatchObject({
      ok: true,
      motion: "MOVEL",
      maxCartesianError: 0
    });
  });

  it("returns KDL_PATH_EMPTY for empty path requests", async () => {
    const { runtime, handle } = await createRobot();
    const response = await dispatchKdlRpcRequest(runtime, {
      id: 5,
      method: "planPath",
      payload: [
        handle,
        pathRequest({
          segments: []
        })
      ]
    });

    expect(response.ok).toBe(true);
    expect(response.result).toMatchObject({
      ok: false,
      duration: 0,
      segments: [],
      points: [],
      diagnostics: [
        {
          severity: "error",
          code: "KDL_PATH_EMPTY"
        }
      ]
    });
  });
});