import { describe, expect, it } from "vitest";
import type { GrlDataDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js";
import { parseGrl } from "../../src/grl/parser/index.js";
import {
  compileGrlDataDeclaration,
  compileGrlTargetDeclaration,
  compileOffsetExpression
} from "../../src/grl/semantic/index.js";

function dataDeclarations(source: string): GrlDataDeclaration[] {
  return parseGrl(source).module.declarations.filter(
    (declaration): declaration is GrlDataDeclaration => declaration.kind === "DataDeclaration"
  );
}

function targetDeclarations(source: string): GrlTargetDeclaration[] {
  return parseGrl(source).module.declarations.filter(
    (declaration): declaration is GrlTargetDeclaration => declaration.kind === "TargetDeclaration"
  );
}

describe("GRL data declarations and target compilation", () => {
  it("compiles tool and frame declarations into shared structures", () => {
    const [toolDecl, frameDecl] = dataDeclarations(`language grl 0.1
module Main
  persistent tool gripper = tool {
    tcp: pose(0 mm, 0 mm, 180 mm, 0 deg, 0 deg, 0 deg),
    mass: 2.5 kg,
    cog: [0 mm, 0 mm, 80 mm]
  }
  persistent frame fixture = frame {
    origin: pose(800 mm, 0 mm, 200 mm, 0 deg, 0 deg, 0 deg)
  }
end
`);

    expect(toolDecl).toMatchObject({
      storage: "persistent",
      typeName: "tool",
      name: "gripper",
      initializer: { kind: "ObjectExpression", typeName: "tool" }
    });
    expect(compileGrlDataDeclaration(toolDecl!)).toMatchObject({
      name: "gripper",
      value: {
        tcp: {
          position: [0, 0, 0.18],
          quaternion: [0, 0, 0, 1]
        },
        mass: 2.5,
        cog: [0, 0, 0.08]
      }
    });
    expect(compileGrlDataDeclaration(frameDecl!)).toMatchObject({
      name: "fixture",
      value: {
        origin: {
          position: [0.8, 0, 0.2],
          quaternion: [0, 0, 0, 1]
        }
      }
    });
  });

  it("compiles speed and zone declarations", () => {
    const declarations = dataDeclarations(`language grl 0.1
module Main
  const speed v_joint = joint(80 %)
  const speed v_pick = linear(300 mm/s)
  const speed v_slow = linear(100 mm/s, acc 500 mm/s2)
  const zone z_fine = fine
  const zone z10 = z(10 mm)
  const zone z_cnt = cnt(30)
  const zone z_cont = continuous
end
`);
    const compiled = declarations.map(compileGrlDataDeclaration);

    expect(compiled).toMatchObject([
      { name: "v_joint", value: { kind: "joint_percent", value: 0.8 } },
      { name: "v_pick", value: { kind: "linear", velocity: 0.3 } },
      { name: "v_slow", value: { kind: "linear", velocity: 0.1, acceleration: 0.5 } },
      { name: "z_fine", value: { kind: "fine" } },
      { name: "z10", value: { kind: "distance", value: 0.01 } },
      { name: "z_cnt", value: { kind: "cnt", value: 30 } },
      { name: "z_cont", value: { kind: "continuous" } }
    ]);
  });

  it("compiles joint_target and pose_target declarations", () => {
    const [home, pick] = targetDeclarations(`language grl 0.1
module Main
  target home = joint_target {
    joints: [0 deg, -30 deg, 60 deg, 0 deg, 60 deg, 0 deg]
  }
  target pick = pose_target {
    pose: pose(500 mm, 120 mm, 300 mm, 180 deg, 0 deg, 90 deg),
    config: robot_config(0, 0, 1),
    tool: gripper,
    frame: fixture
  }
end
`);

    expect(compileGrlTargetDeclaration(home!)).toMatchObject({
      name: "home",
      target: {
        joints: [0, -Math.PI / 6, Math.PI / 3, 0, Math.PI / 3, 0]
      }
    });

    const compiledPick = compileGrlTargetDeclaration(pick!);
    expect(compiledPick.name).toBe("pick");
    expect("pose" in compiledPick.target).toBe(true);
    if ("pose" in compiledPick.target) {
      expect(compiledPick.target.pose.position).toEqual([0.5, 0.12, 0.3]);
      expect(compiledPick.target.config).toEqual({ shoulder: 0, elbow: 0, wrist: 1 });
    }
  });

  it("parses and compiles offset expressions", () => {
    const [declFrame, declTool] = dataDeclarations(`language grl 0.1
module Main
  var pose_target p2 = pick offset x 20 mm y -10 mm z 50 mm
  var pose_target p3 = pick offset_in tool z -50 mm
end
`);

    expect(declFrame?.initializer).toMatchObject({
      kind: "OffsetExpression",
      mode: "frame",
      axes: [
        { axis: "x" },
        { axis: "y" },
        { axis: "z" }
      ]
    });
    if (declFrame?.initializer.kind === "OffsetExpression") {
      expect(compileOffsetExpression(declFrame.initializer)).toEqual({
        mode: "frame",
        xyz: [0.02, -0.01, 0.05]
      });
    }
    if (declTool?.initializer.kind === "OffsetExpression") {
      expect(compileOffsetExpression(declTool.initializer)).toEqual({
        mode: "tool",
        xyz: [0, 0, -0.05]
      });
    }
  });
});