import { describe, expect, it } from "vitest";
import type { GrlDataDeclaration, GrlPathDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js";
import { parseGrl } from "../../src/grl/parser/index.js";
import { postProcessAllBrands } from "../../src/grl/post/index.js";
import {
  buildMotionContext,
  compilePathToPlanRequest,
  parseProcedureRunPathStatements
} from "../../src/grl/semantic/index.js";
import { importBrandProgram } from "../../src/importers/index.js";
import type { KdlRuntimeHandlers } from "../../src/kdl/rpc.js";
import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
import type {
  FkResult,
  IkResult,
  JacobianResult,
  KdlApiMethod,
  LimitCheckResult,
  MoveJRequest,
  PathPlanRequest,
  PathPlanResult,
  RobotInfo,
  SingularityResult,
  TrajectoryResult
} from "../../src/kdl/types.js";
import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
import { KdlRuntimeBridge, MotionQueue, type MotionPlanner } from "../../src/runtime/index.js";
import {
  ABB_IRB120_3_58_URDF,
  ABB_IRB120_HOME_TO_PICK_JOINTS,
  ABB_IRB120_LOAD_OPTIONS,
  ABB_IRB120_PICK_JOINTS,
  ABB_IRB120_PLACE_JOINTS,
  ABB_IRB120_URDF_SOURCE,
  ABB_IRB120_ZERO_JOINTS
} from "../fixtures/abbIrb120.js";

const ABB120_GRL_PROGRAM = `language grl 0.1
module Abb120Cell
  const speed v_fast = joint(40 %)
  const speed v_slow = joint(20 %)
  const speed v_linear = linear(150 mm/s)
  const zone z10 = z(10 mm)
  target home = joint_target {
    joints: [0 deg, 0 deg, 0 deg, 0 deg, 0 deg, 0 deg]
  }
  target pick = joint_target {
    joints: [11.459156 deg, -20.053523 deg, 25.783101 deg, 5.729578 deg, -11.459156 deg, 17.188734 deg]
  }
  target place = joint_target {
    joints: [-20.053523 deg, -14.323945 deg, 20.053523 deg, -14.323945 deg, 8.594367 deg, -22.918312 deg]
  }
  target unreachable_pose = pose_target {
    pose: pose(300 mm, 100 mm, 500 mm, 0 deg, 90 deg, 0 deg)
  }
  path joint_pick_place {
    defaults {
      speed: v_fast,
      zone: z10
    }
    point p_home movej home zone fine
    point p_pick movej pick speed v_slow
    point p_place movej place
    event before p_pick io.do[1] = true
    event after p_place io.do[1] = false
  }
  proc main()
    run_path joint_pick_place
    movej unreachable_pose speed v_linear zone fine
  end
end
`;

const ABB120_RAPID = `MODULE ABB120_PICK_PLACE
  CONST jointtarget jHome := [[0,0,0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
  CONST jointtarget jPick := [[11.459156,-20.053523,25.783101,5.729578,-11.459156,17.188734],[9E9,9E9,9E9,9E9,9E9,9E9]];
  CONST jointtarget jPlace := [[-20.053523,-14.323945,20.053523,-14.323945,8.594367,-22.918312],[9E9,9E9,9E9,9E9,9E9,9E9]];
  CONST robtarget pScan := [[300,100,500],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
  PROC main()
    MoveJ jHome,v100,fine,tool0;
    MoveJ jPick,v50,z10,tool0;
    MoveJ jPlace,v50,fine,tool0;
    MoveL pScan,v100,fine,tool0;
  ENDPROC
ENDMODULE`;

async function createAbb120Robot() {
  const runtime = createKdlWorkerRuntime();
  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
  const response = await dispatchKdlRpcRequest(runtime, {
    id: 2,
    method: "loadRobotFromUrdf",
    payload: [ABB_IRB120_3_58_URDF, ABB_IRB120_LOAD_OPTIONS]
  });
  expect(response.ok).toBe(true);
  return { runtime, handle: response.result as number };
}

async function rpc<T>(
  runtime: KdlRuntimeHandlers,
  id: number,
  method: KdlApiMethod,
  payload: unknown[]
): Promise<T> {
  const response = await dispatchKdlRpcRequest(runtime, { id, method, payload });
  expect(response.ok).toBe(true);
  return response.result as T;
}

function abb120PathRequest(): PathPlanRequest {
  const program = parseGrl(ABB120_GRL_PROGRAM);
  const declarations = program.module.declarations;
  const path = declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!;
  const context = buildMotionContext(
    declarations.filter(
      (decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
        decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
    )
  );
  return compilePathToPlanRequest(path, context, {
    startJoints: [...ABB_IRB120_ZERO_JOINTS],
    sampleTime: 0.02
  }).request;
}

function moveJRequest(targetJoints: readonly number[]): MoveJRequest {
  return {
    startJoints: [...ABB_IRB120_ZERO_JOINTS],
    target: {
      id: "abb120_pick",
      joints: [...targetJoints]
    },
    speed: {
      kind: "joint_percent",
      value: 0.35
    },
    zone: {
      kind: "fine"
    },
    sampleTime: 0.02
  };
}

function expectCloseArray(actual: number[] | undefined, expected: readonly number[], digits = 6): void {
  expect(actual).toHaveLength(expected.length);
  for (const [index, expectedValue] of expected.entries()) {
    expect(actual?.[index]).toBeCloseTo(expectedValue, digits);
  }
}

describe("ABB IRB120 programs with ROS-Industrial URDF", () => {
  it("loads the ABB120 URDF fixture with source provenance and expected 6R chain", async () => {
    const { runtime, handle } = await createAbb120Robot();
    const info = await rpc<RobotInfo>(runtime, 3, "getRobotInfo", [handle]);

    expect(ABB_IRB120_URDF_SOURCE).toMatchObject({
      repository: "https://github.com/ros-industrial/abb",
      entrypoint: "abb_irb120_support/urdf/irb120_3_58.xacro"
    });
    expect(info).toMatchObject({
      robotId: "abb_irb120_3_58",
      name: "abb_irb120_3_58",
      baseLink: "base_link",
      tipLink: "tool0",
      dof: 6,
      jointNames: ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"]
    });
    expect(info.limits.map((limit) => [limit.name, limit.lower, limit.upper])).toEqual([
      ["joint_1", -2.87979, 2.87979],
      ["joint_2", -1.91986, 1.91986],
      ["joint_3", -1.91986, 1.22173],
      ["joint_4", -2.79253, 2.79253],
      ["joint_5", -2.094395, 2.094395],
      ["joint_6", -6.98132, 6.98132]
    ]);
  });

  it("runs FK, link poses, Jacobian, limit checks, and singularity diagnostics", async () => {
    const { runtime, handle } = await createAbb120Robot();
    const zeroFk = await rpc<FkResult>(runtime, 4, "fk", [handle, [...ABB_IRB120_ZERO_JOINTS]]);
    const pickFk = await rpc<FkResult>(runtime, 5, "fk", [handle, [...ABB_IRB120_PICK_JOINTS]]);
    const links = await rpc<{ linkPoses: Array<{ link: string }> }>(runtime, 6, "fkAllLinks", [
      handle,
      [...ABB_IRB120_ZERO_JOINTS]
    ]);
    const jacobian = await rpc<JacobianResult>(runtime, 7, "jacobian", [handle, [...ABB_IRB120_PICK_JOINTS]]);
    const limits = await rpc<LimitCheckResult>(runtime, 8, "checkJointLimits", [
      handle,
      [3.2, 0, 0, 0, 0, 0]
    ]);
    const singularity = await rpc<SingularityResult>(runtime, 9, "checkSingularity", [
      handle,
      [...ABB_IRB120_ZERO_JOINTS]
    ]);

    expect(zeroFk.tcp.position[0]).toBeCloseTo(0.374);
    expect(zeroFk.tcp.position[1]).toBeCloseTo(0);
    expect(zeroFk.tcp.position[2]).toBeCloseTo(0.63);
    expect(zeroFk.tcp.quaternion[1]).toBeCloseTo(Math.SQRT1_2);
    expect(zeroFk.tcp.quaternion[3]).toBeCloseTo(Math.SQRT1_2);
    expect(pickFk.tcp.position).not.toEqual(zeroFk.tcp.position);
    expect(links.linkPoses.map((entry) => entry.link)).toEqual([
      "base_link",
      "link_1",
      "link_2",
      "link_3",
      "link_4",
      "link_5",
      "link_6",
      "flange",
      "tool0"
    ]);
    expect(jacobian).toMatchObject({ ok: true, rows: 6, cols: 6 });
    expect(Array.from(jacobian.data)).toHaveLength(36);
    expect(limits.ok).toBe(false);
    expect(limits.diagnostics[0]).toMatchObject({
      code: "KDL_JOINT_LIMIT",
      severity: "error"
    });
    expect(singularity.ok).toBe(true);
    expect(typeof singularity.nearSingularity).toBe("boolean");
    expect(Number.isFinite(singularity.manipulability)).toBe(true);
    expect(Number.isFinite(singularity.conditionNumber)).toBe(true);
  });

  it("plans ABB120 MoveJ and multi-segment joint path programs", async () => {
    const { runtime, handle } = await createAbb120Robot();
    const moveJ = await rpc<TrajectoryResult>(runtime, 10, "planMoveJ", [
      handle,
      moveJRequest(ABB_IRB120_PICK_JOINTS)
    ]);
    const path = await rpc<PathPlanResult>(runtime, 11, "planPath", [handle, abb120PathRequest()]);
    const validation = await rpc<{ ok: boolean; reachable: boolean; cycleTime?: number }>(runtime, 12, "validatePath", [
      handle,
      abb120PathRequest()
    ]);

    expect(moveJ).toMatchObject({
      ok: true,
      motion: "MOVEJ",
      meta: {
        targetType: "joint",
        qStart: [...ABB_IRB120_ZERO_JOINTS],
        qEnd: [...ABB_IRB120_PICK_JOINTS]
      }
    });
    expect(moveJ.points.length).toBeGreaterThan(2);
    expectCloseArray(moveJ.points.at(-1)?.joints, ABB_IRB120_PICK_JOINTS);
    expect(path.ok).toBe(true);
    expect(path.segments.map((segment) => segment.motion)).toEqual(["MOVEJ", "MOVEJ", "MOVEJ"]);
    expectCloseArray(path.points.at(-1)?.joints, ABB_IRB120_PLACE_JOINTS);
    expect(validation).toMatchObject({
      ok: true,
      reachable: true
    });
    expect(validation.cycleTime).toBeGreaterThan(0);
  });

  it("compiles GRL path/procedure, posts all brands, and replays through MotionQueue", async () => {
    const { runtime, handle } = await createAbb120Robot();
    const program = parseGrl(ABB120_GRL_PROGRAM);
    const pathRequest = abb120PathRequest();
    const ir = await import("../../src/grl/semantic/index.js").then(({ compileSemanticProgram }) =>
      compileSemanticProgram(program, {
        startJoints: [...ABB_IRB120_ZERO_JOINTS],
        sampleTime: 0.02
      })
    );
    const procedure = program.module.declarations.find((decl) => decl.kind === "ProcedureDeclaration");
    expect(procedure && parseProcedureRunPathStatements(procedure).map((instruction) => instruction.pathId)).toEqual([
      "joint_pick_place"
    ]);
    expect(ir.kdlBridge.pathRequests[0]).toMatchObject({
      pathId: "joint_pick_place",
      segments: [
        { id: "p_home", motion: "MOVEJ" },
        { id: "p_pick", motion: "MOVEJ" },
        { id: "p_place", motion: "MOVEJ" }
      ]
    });
    expect(ir.procedures[0]?.instructions.map((instruction) => instruction.kind)).toEqual([
      "RUN_PATH",
      "MOVEJ"
    ]);

    const posted = postProcessAllBrands(ir);
    expect(posted.outputs.abb.text).toContain("MoveJ unreachable_pose,v150,fine,tool0;");
    expect(posted.outputs.fanuc.text).toContain("J unreachable_pose 150mm/sec FINE");
    expect(posted.outputs.kuka.text).toContain("PTP unreachable_pose Vel=0.150m/s");

    const planner: MotionPlanner = {
      planPath: (_robotHandle, request) =>
        dispatchKdlRpcRequest(runtime, {
          id: 20,
          method: "planPath",
          payload: [handle, request]
        }).then((response) => response.result as PathPlanResult)
    };
    const queue = new MotionQueue({
      startJoints: [...ABB_IRB120_ZERO_JOINTS],
      sampleTime: 0.02,
      robotHandle: handle,
      planner
    });
    const bridge = new KdlRuntimeBridge(ir, queue);
    const pathId = pathRequest.pathId;
    expect(pathId).toBe("joint_pick_place");
    const items = bridge.enqueueInstruction({ kind: "RUN_PATH", pathId: pathId! });
    await queue.planAll();
    const plannedDuration = queue.snapshot().items[0]?.planned?.duration ?? 0;
    const halfway = queue.advance(plannedDuration / 2);

    expect(items).toHaveLength(1);
    expect(items[0]).toMatchObject({
      kind: "path",
      source: {
        pathId: "joint_pick_place"
      }
    });
    expect(queue.snapshot().items[0]?.planned).toMatchObject({
      ok: true
    });
    expect(halfway?.source).toMatchObject({
      pathId: "joint_pick_place"
    });
    expect(halfway?.point.joints).toHaveLength(6);
  });

  it("solves 6R ABB120 pose IK and reports straight-line MOVEL sampling limits", async () => {
    const { runtime, handle } = await createAbb120Robot();
    const pickPose = await rpc<FkResult>(runtime, 30, "fk", [handle, [...ABB_IRB120_HOME_TO_PICK_JOINTS]]);
    const ik = await rpc<IkResult>(runtime, 31, "ik", [
      handle,
      [...ABB_IRB120_ZERO_JOINTS],
      pickPose.tcp,
      { positionTolerance: 1e-6 }
    ]);
    const moveL = await rpc<TrajectoryResult>(runtime, 32, "planMoveL", [
      handle,
      {
        startJoints: [...ABB_IRB120_ZERO_JOINTS],
        target: {
          id: "fk_pose_target",
          pose: pickPose.tcp
        },
        speed: {
          kind: "linear",
          velocity: 0.1
        },
        zone: {
          kind: "fine"
        },
        sampleTime: 0.02
      }
    ]);

    expect(ik).toMatchObject({
      ok: true,
      diagnostics: []
    });
    expect(ik.joints).toHaveLength(6);
    expect(ik.residualPosition).toBeLessThanOrEqual(1e-6);
    expect(moveL).toMatchObject({
      ok: false,
      motion: "MOVEL",
      meta: {
        targetId: "fk_pose_target"
      }
    });
    expect(moveL.diagnostics).toContainEqual(
      expect.objectContaining({
        severity: "error"
      })
    );
  });

  it("imports an ABB RAPID program with ABB120-style joint targets and reports unreachable pose moves", async () => {
    const { runtime, handle } = await createAbb120Robot();
    const imported = importBrandProgram("abb", [{ name: "abb120.mod", text: ABB120_RAPID }], {
      projectName: "Abb120Imported",
      generatedAt: "2026-06-27T00:00:00.000Z"
    });
    const pathRequest = imported.ir.kdlBridge.pathRequests[0]!;
    const planned = await rpc<PathPlanResult>(runtime, 40, "planPath", [
      handle,
      {
        ...pathRequest,
        startJoints: [...ABB_IRB120_ZERO_JOINTS],
        sampleTime: 0.02,
        stopOnError: false
      }
    ]);

    expect(imported.report.status).toBe("warn");
    expect(imported.ir.paths[0]?.motions.map((motion) => motion.kind)).toEqual([
      "MOVEJ",
      "MOVEJ",
      "MOVEJ",
      "MOVEL"
    ]);
    expect(postProcessAllBrands(imported.ir).outputs.abb.text).toContain("MoveL pScan");
    expect(planned.segments.map((segment) => [segment.motion, segment.ok])).toEqual([
      ["MOVEJ", true],
      ["MOVEJ", true],
      ["MOVEJ", true],
      ["MOVEL", false]
    ]);
    expect(planned.diagnostics).toContainEqual(
      expect.objectContaining({
        code: "KDL_TARGET_UNREACHABLE",
        segmentId: "import_p03"
      })
    );
  });
});