import { describe, expect, it } from "vitest";
import type { GrlDataDeclaration, GrlProcedureDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js";
import { parseGrl } from "../../src/grl/parser/index.js";
import {
  buildMotionContext,
  compileMotionToKdlRequest,
  parseProcedureMotionInstructions
} from "../../src/grl/semantic/index.js";

const PROGRAM = `language grl 0.1
module Main
  persistent tool gripper = tool {
    tcp: pose(0 mm, 0 mm, 100 mm, 0 deg, 0 deg, 0 deg),
    mass: 1 kg
  }
  persistent frame fixture = frame {
    origin: pose(800 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
  }
  const speed v_joint = joint(60 %)
  const speed v_linear = linear(300 mm/s)
  const zone z10 = z(10 mm)
  target home = joint_target {
    joints: [0 deg, 0 deg]
  }
  target pick = pose_target {
    pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg),
    tool: gripper,
    frame: fixture
  }
  target mid = pose_target {
    pose: pose(550 mm, 50 mm, 0 mm, 0 deg, 0 deg, 0 deg)
  }
  target arc_end = pose_target {
    pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
  }
  proc main()
    set_tool gripper
    set_frame fixture
    set_speed v_linear
    set_zone z10
    movej home speed v_joint zone fine
    movel pick
    movec via mid target arc_end speed linear(150 mm/s) zone fine
  end
end
`;

function declarations() {
  return parseGrl(PROGRAM).module.declarations;
}

describe("GRL motion instruction compilation", () => {
  it("parses movej, movel, and movec from procedure body tokens", () => {
    const decls = declarations();
    const context = buildMotionContext(
      decls.filter(
        (decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
          decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
      )
    );
    const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
    const instructions = parseProcedureMotionInstructions(procedure, context);

    expect(instructions.map((instruction) => instruction.kind)).toEqual(["MOVEJ", "MOVEL", "MOVEC"]);
    expect(instructions[0]).toMatchObject({
      kind: "MOVEJ",
      speed: { kind: "joint_percent", value: 0.6 },
      zone: { kind: "fine" },
      target: { joints: [0, 0] },
      sourceMap: { line: 32 }
    });
    expect(instructions[1]).toMatchObject({
      kind: "MOVEL",
      speed: { kind: "linear", velocity: 0.3 },
      zone: { kind: "distance", value: 0.01 }
    });
    expect(instructions[1]?.tool?.position).toEqual([0, 0, 0.1]);
    expect(instructions[1]?.frame?.position).toEqual([0.8, 0, 0]);
    expect(instructions[2]).toMatchObject({
      kind: "MOVEC",
      speed: { kind: "linear", velocity: 0.15 },
      zone: { kind: "fine" }
    });
  });

  it("compiles motion instructions to KDL request shapes", () => {
    const decls = declarations();
    const context = buildMotionContext(
      decls.filter(
        (decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
          decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
      )
    );
    const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
    const [movej, movel, movec] = parseProcedureMotionInstructions(procedure, context);

    expect(compileMotionToKdlRequest(movej!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({
      startJoints: [0, 0],
      target: { joints: [0, 0] },
      speed: { kind: "joint_percent", value: 0.6 },
      zone: { kind: "fine" },
      sampleTime: 0.004
    });

    expect(compileMotionToKdlRequest(movel!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({
      startJoints: [0, 0],
      target: { pose: { position: [0.5, 0, 0] } },
      speed: { kind: "linear", velocity: 0.3 },
      zone: { kind: "distance", value: 0.01 },
      tool: { position: [0, 0, 0.1] },
      frame: { position: [0.8, 0, 0] },
      sampleTime: 0.004
    });

    expect(compileMotionToKdlRequest(movec!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({
      startJoints: [0, 0],
      via: { pose: { position: [0.55, 0.05, 0] } },
      target: { pose: { position: [0.6, 0, 0] } },
      speed: { kind: "linear", velocity: 0.15 },
      zone: { kind: "fine" },
      sampleTime: 0.004
    });
  });
});