import { KdlStructuredError } from "../../kdl/rpc.js";
import { applyOffset } from "../../kdl/poseApi.js";
import type {
  JointTarget,
  JsonObject,
  MoveCRequest,
  MoveJRequest,
  MoveLRequest,
  MotionSegmentRequest,
  PathEventRequest,
  PathPlanRequest,
  Pose,
  PoseTarget,
  SpeedSpec,
  ZoneSpec
} from "../../kdl/types.js";
import type {
  CompiledOperation,
  CompiledPath,
  MotionInstruction,
  OperationActionInstruction,
  OperationExecutionStep,
  PathEventInstruction,
  RunOperationInstruction,
  RunPathInstruction
} from "../ir/index.js";
import { parseGrlExpression } from "../parser/index.js";
import type {
  GrlExpression,
  GrlOperationActionBlock,
  GrlOperationDeclaration,
  GrlOperationProcessBlock,
  GrlPathDeclaration,
  GrlPathDefaultsBlock,
  GrlPathEvent,
  GrlPathPoint,
  GrlPathProperty,
  GrlPathSourceBlock,
  GrlProcedureDeclaration
} from "../ast/index.js";
import type { GrlToken } from "../lexer/index.js";
import {
  compileOffsetExpression,
  compileGrlDataDeclaration,
  compileGrlTargetDeclaration,
  compileSpeedExpression,
  compileTargetExpression,
  compileZoneExpression,
  type CompiledGrlDataValue
} from "./compileData.js";
import type { GrlDataDeclaration, GrlTargetDeclaration } from "../ast/index.js";

export interface GrlMotionContext {
  targets: Map<string, JointTarget | PoseTarget>;
  speeds: Map<string, SpeedSpec>;
  zones: Map<string, ZoneSpec>;
  tools: Map<string, Pose>;
  frames: Map<string, Pose>;
  currentSpeed?: SpeedSpec;
  currentZone?: ZoneSpec;
  currentTool?: Pose;
  currentFrame?: Pose;
}

export interface MotionRequestOptions {
  startJoints: number[];
  sampleTime: number;
}

interface PathDefaults {
  speed?: SpeedSpec;
  zone?: ZoneSpec;
  tool?: Pose;
  frame?: Pose;
}

export interface PathCompileOptions extends MotionRequestOptions {
  speedOverride?: number;
  stopOnError?: boolean;
}

export function buildMotionContext(declarations: Array<GrlDataDeclaration | GrlTargetDeclaration>): GrlMotionContext {
  const context: GrlMotionContext = {
    targets: new Map(),
    speeds: new Map(),
    zones: new Map(),
    tools: new Map(),
    frames: new Map()
  };

  for (const declaration of declarations) {
    if (declaration.kind === "TargetDeclaration") {
      const compiled = compileGrlTargetDeclaration(declaration);
      context.targets.set(compiled.name, compiled.target);
      continue;
    }

    const compiled = compileGrlDataDeclaration(declaration);
    addCompiledData(context, compiled.name, compiled.typeName, compiled.value);
  }

  return context;
}

export function parseProcedureMotionInstructions(
  procedure: GrlProcedureDeclaration,
  context: GrlMotionContext
): MotionInstruction[] {
  const parser = new MotionStatementParser(procedure.bodyTokens, context);
  return parser.parseAll();
}

export function parseProcedureRunPathStatements(procedure: GrlProcedureDeclaration): RunPathInstruction[] {
  const parser = new RunPathStatementParser(procedure.bodyTokens);
  return parser.parseAll();
}

export function parseProcedureRunOperationStatements(procedure: GrlProcedureDeclaration): RunOperationInstruction[] {
  const parser = new RunOperationStatementParser(procedure.bodyTokens);
  return parser.parseAll();
}

export function compilePathToPlanRequest(
  path: GrlPathDeclaration,
  context: GrlMotionContext,
  options: PathCompileOptions
): CompiledPath {
  const defaults = compilePathDefaults(path.items.find((item): item is GrlPathDefaultsBlock => item.kind === "PathDefaultsBlock"), context);
  const source = compilePathSource(path.items.find((item): item is GrlPathSourceBlock => item.kind === "PathSourceBlock"));
  const points = path.items.filter((item): item is GrlPathPoint => item.kind === "PathPoint");
  const events = path.items.filter((item): item is GrlPathEvent => item.kind === "PathEvent");

  if (points.length === 0) {
    throw motionError("GRL_PATH_EMPTY", `Path ${path.name} must contain at least one point`);
  }

  const pointIds = new Set<string>();
  const motions: MotionInstruction[] = [];
  const segments: MotionSegmentRequest[] = [];

  for (const point of points) {
    if (pointIds.has(point.id)) {
      throw motionError("GRL_PATH_POINT_DUPLICATE", `Path ${path.name} contains duplicate point ${point.id}`);
    }
    pointIds.add(point.id);

    const pointContext = cloneMotionContext(context);
    applyPathDefaults(pointContext, defaults);
    const [motion] = new MotionStatementParser(point.motionTokens, pointContext).parseAll();
    if (!motion) {
      throw motionError("GRL_PATH_POINT_MOTION_MISSING", `Path point ${point.id} has no motion`);
    }

    const instruction: MotionInstruction = {
      ...motion,
      id: point.id,
      pathId: path.name,
      pointId: point.id,
      ...(source ? { source } : {})
    };
    motions.push(instruction);
    segments.push(motionToSegment(instruction, path.name, source));
  }

  const compiledEvents = events.map((event, index) => compilePathEvent(event, index, pointIds));
  const request: PathPlanRequest = {
    pathId: path.name,
    startJoints: options.startJoints,
    segments,
    ...(compiledEvents.length > 0 ? { events: compiledEvents } : {}),
    sampleTime: options.sampleTime,
    ...(options.speedOverride !== undefined ? { speedOverride: options.speedOverride } : {}),
    ...(options.stopOnError !== undefined ? { stopOnError: options.stopOnError } : {}),
    ...(source ? { source } : {})
  };

  return {
    pathId: path.name,
    request,
    motions,
    events: compiledEvents
  };
}

export function compileOperation(
  operation: GrlOperationDeclaration,
  paths: Map<string, GrlPathDeclaration>
): CompiledOperation {
  if (!paths.has(operation.pathName)) {
    throw motionError(
      "GRL_OPERATION_PATH_NOT_FOUND",
      `Operation ${operation.name} references unknown path ${operation.pathName}`
    );
  }

  const processBlock = operation.items.find(
    (item): item is GrlOperationProcessBlock => item.kind === "OperationProcessBlock"
  );
  const actionBlocks = operation.items.filter(
    (item): item is GrlOperationActionBlock => item.kind === "OperationActionBlock"
  );

  return {
    operationId: operation.name,
    kind: operation.operationKind,
    pathId: operation.pathName,
    process: processBlock ? compileProcessBlock(processBlock) : {},
    startActions: actionBlocks
      .filter((item) => item.actionKind === "start_action")
      .map((item) => compileOperationAction(operation.name, item)),
    endActions: actionBlocks
      .filter((item) => item.actionKind === "end_action")
      .map((item) => compileOperationAction(operation.name, item))
  };
}

export function expandRunOperation(
  run: RunOperationInstruction,
  operations: Map<string, CompiledOperation>
): OperationExecutionStep[] {
  const operation = operations.get(run.operationId);
  if (!operation) {
    throw motionError("GRL_OPERATION_NOT_FOUND", `Unknown operation ${run.operationId}`);
  }
  return [
    ...operation.startActions,
    {
      kind: "RUN_PATH",
      pathId: operation.pathId,
      ...(run.sourceMap ? { sourceMap: run.sourceMap } : {})
    },
    ...operation.endActions
  ];
}

export function compileMotionToKdlRequest(
  instruction: MotionInstruction,
  options: MotionRequestOptions
): MoveJRequest | MoveLRequest | MoveCRequest {
  if (instruction.kind === "MOVEJ") {
    if (!instruction.target) {
      throw motionError("GRL_MOTION_TARGET_MISSING", "MOVEJ requires target");
    }
    return {
      startJoints: options.startJoints,
      target: instruction.target,
      speed: instruction.speed,
      zone: instruction.zone,
      ...(instruction.tool ? { tool: instruction.tool } : {}),
      ...(instruction.frame ? { frame: instruction.frame } : {}),
      sampleTime: options.sampleTime,
      ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {})
    };
  }

  if (instruction.kind === "MOVEL") {
    if (!instruction.target || !isPoseTarget(instruction.target)) {
      throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEL requires PoseTarget");
    }
    return {
      startJoints: options.startJoints,
      target: instruction.target,
      speed: instruction.speed,
      zone: instruction.zone,
      ...(instruction.tool ? { tool: instruction.tool } : {}),
      ...(instruction.frame ? { frame: instruction.frame } : {}),
      sampleTime: options.sampleTime,
      ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {})
    };
  }

  if (!instruction.via || !instruction.target || !isPoseTarget(instruction.target)) {
    throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEC requires via and target PoseTarget");
  }

  return {
    startJoints: options.startJoints,
    via: instruction.via,
    target: instruction.target,
    speed: instruction.speed,
    zone: instruction.zone,
    ...(instruction.tool ? { tool: instruction.tool } : {}),
    ...(instruction.frame ? { frame: instruction.frame } : {}),
    sampleTime: options.sampleTime,
    ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {})
  };
}

function motionToSegment(
  instruction: MotionInstruction,
  pathId: string,
  source?: Record<string, unknown>
): MotionSegmentRequest {
  if (instruction.kind === "MOVEJ") {
    if (!instruction.target) {
      throw motionError("GRL_MOTION_TARGET_MISSING", "MOVEJ requires target");
    }
    const targetId = targetIdOf(instruction.target);
    return {
      id: instruction.pointId ?? instruction.id ?? `${pathId}_${instruction.kind.toLowerCase()}`,
      motion: "MOVEJ",
      target: instruction.target,
      ...(targetId ? { targetId } : {}),
      speed: instruction.speed,
      zone: instruction.zone,
      ...(instruction.tool ? { tool: instruction.tool } : {}),
      ...(instruction.frame ? { frame: instruction.frame } : {}),
      ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}),
      ...(source ? { source } : {})
    };
  }

  if (instruction.kind === "MOVEL") {
    if (!instruction.target || !isPoseTarget(instruction.target)) {
      throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEL requires PoseTarget");
    }
    const targetId = targetIdOf(instruction.target);
    return {
      id: instruction.pointId ?? instruction.id ?? `${pathId}_${instruction.kind.toLowerCase()}`,
      motion: "MOVEL",
      target: instruction.target,
      ...(targetId ? { targetId } : {}),
      speed: instruction.speed,
      zone: instruction.zone,
      ...(instruction.tool ? { tool: instruction.tool } : {}),
      ...(instruction.frame ? { frame: instruction.frame } : {}),
      ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}),
      ...(source ? { source } : {})
    };
  }

  if (!instruction.via || !instruction.target || !isPoseTarget(instruction.target)) {
    throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEC requires via and target PoseTarget");
  }
  const targetId = targetIdOf(instruction.target);
  return {
    id: instruction.pointId ?? instruction.id ?? `${pathId}_${instruction.kind.toLowerCase()}`,
    motion: "MOVEC",
    via: instruction.via,
    target: instruction.target,
    ...(targetId ? { targetId } : {}),
    speed: instruction.speed,
    zone: instruction.zone,
    ...(instruction.tool ? { tool: instruction.tool } : {}),
    ...(instruction.frame ? { frame: instruction.frame } : {}),
    ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}),
    ...(source ? { source } : {})
  };
}

function addCompiledData(
  context: GrlMotionContext,
  name: string,
  typeName: string,
  value: CompiledGrlDataValue
): void {
  if (typeName === "speed") {
    context.speeds.set(name, value as SpeedSpec);
  } else if (typeName === "zone") {
    context.zones.set(name, value as ZoneSpec);
  } else if (typeName === "tool") {
    context.tools.set(name, (value as { tcp: Pose }).tcp);
  } else if (typeName === "frame") {
    context.frames.set(name, (value as { origin: Pose }).origin);
  }
}

class MotionStatementParser {
  private current = 0;

  constructor(
    private readonly tokens: GrlToken[],
    private readonly context: GrlMotionContext
  ) {}

  parseAll(): MotionInstruction[] {
    const instructions: MotionInstruction[] = [];
    while (!this.isAtEnd()) {
      if (this.matchKeyword("set_tool")) {
        this.context.currentTool = this.resolveNamedPose(this.consumeIdentifier("Expected tool name"), "tool");
        continue;
      }
      if (this.matchKeyword("set_frame")) {
        this.context.currentFrame = this.resolveNamedPose(this.consumeIdentifier("Expected frame name"), "frame");
        continue;
      }
      if (this.matchKeyword("set_speed")) {
        this.context.currentSpeed = this.parseSpeedArgument();
        continue;
      }
      if (this.matchKeyword("set_zone")) {
        this.context.currentZone = this.parseZoneArgument();
        continue;
      }
      if (this.matchKeyword("movej")) {
        instructions.push(this.finishMoveJ(this.previous()));
        continue;
      }
      if (this.matchKeyword("movel")) {
        instructions.push(this.finishMoveL(this.previous()));
        continue;
      }
      if (this.matchKeyword("movec")) {
        instructions.push(this.finishMoveC(this.previous()));
        continue;
      }
      this.advance();
    }
    return instructions;
  }

  private finishMoveJ(start: GrlToken): MotionInstruction {
    const target = this.parseTargetArgument();
    const params = this.parseMotionParams();
    return this.withDefaults({
      kind: "MOVEJ",
      target,
      ...params,
      sourceMap: tokenSourceMap(start)
    });
  }

  private finishMoveL(start: GrlToken): MotionInstruction {
    const target = this.parseTargetArgument();
    if (!isPoseTarget(target)) {
      throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEL requires PoseTarget");
    }
    const params = this.parseMotionParams();
    return this.withDefaults({
      kind: "MOVEL",
      target,
      ...params,
      sourceMap: tokenSourceMap(start)
    });
  }

  private finishMoveC(start: GrlToken): MotionInstruction {
    this.consumeKeyword("via", "Expected via in MOVEC");
    const via = this.parseTargetArgument();
    if (!isPoseTarget(via)) {
      throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEC via requires PoseTarget");
    }
    this.consumeKeyword("target", "Expected target in MOVEC");
    const target = this.parseTargetArgument();
    if (!isPoseTarget(target)) {
      throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEC target requires PoseTarget");
    }
    const params = this.parseMotionParams();
    return this.withDefaults({
      kind: "MOVEC",
      via,
      target,
      ...params,
      sourceMap: tokenSourceMap(start)
    });
  }

  private parseMotionParams(): Partial<MotionInstruction> {
    const params: Partial<MotionInstruction> = {};
    while (!this.isAtEnd() && !this.isMotionStart(this.peek())) {
      if (this.matchKeyword("speed")) {
        params.speed = this.parseSpeedArgument();
      } else if (this.matchKeyword("zone")) {
        params.zone = this.parseZoneArgument();
      } else if (this.matchKeyword("tool")) {
        params.tool = this.resolveNamedPose(this.consumeIdentifier("Expected tool name"), "tool");
      } else if (this.matchKeyword("frame")) {
        params.frame = this.resolveNamedPose(this.consumeIdentifier("Expected frame name"), "frame");
      } else {
        break;
      }
    }
    return params;
  }

  private parseTargetArgument(): JointTarget | PoseTarget {
    const expressionTokens = this.collectExpressionUntilParamKeyword();
    const expression = parseGrlExpression(expressionTokens);
    return this.resolveTargetExpression(expression);
  }

  private parseSpeedArgument(): SpeedSpec {
    const token = this.peek();
    if ((token.kind === "identifier" || token.kind === "keyword") && this.context.speeds.has(token.raw)) {
      this.advance();
      return this.context.speeds.get(token.raw)!;
    }
    return compileSpeedExpression(parseGrlExpression(this.collectExpressionUntilParamKeyword()));
  }

  private parseZoneArgument(): ZoneSpec {
    const token = this.peek();
    if ((token.kind === "identifier" || token.kind === "keyword") && this.context.zones.has(token.raw)) {
      this.advance();
      return this.context.zones.get(token.raw)!;
    }
    return compileZoneExpression(parseGrlExpression(this.collectExpressionUntilParamKeyword()));
  }

  private resolveTargetExpression(expression: GrlExpression): JointTarget | PoseTarget {
    if (expression.kind === "OffsetExpression") {
      const base = this.resolveTargetExpression(expression.base);
      if (!isPoseTarget(base)) {
        throw motionError("GRL_MOTION_TARGET_TYPE", "offset target requires PoseTarget");
      }
      return applyOffset(base, compileOffsetExpression(expression));
    }
    if (expression.kind === "IdentifierExpression") {
      const target = this.context.targets.get(expression.name);
      if (!target) {
        throw motionError("GRL_TARGET_NOT_FOUND", `Unknown target ${expression.name}`);
      }
      return target;
    }
    return compileTargetExpression(expression);
  }

  private withDefaults(instruction: Partial<MotionInstruction> & Pick<MotionInstruction, "kind">): MotionInstruction {
    const speed = instruction.speed ?? this.context.currentSpeed;
    const zone = instruction.zone ?? this.context.currentZone;
    if (!speed) {
      throw motionError("GRL_SPEED_UNRESOLVED", `${instruction.kind} has no speed`);
    }
    if (!zone) {
      throw motionError("GRL_ZONE_UNRESOLVED", `${instruction.kind} has no zone`);
    }
    return {
      kind: instruction.kind,
      ...(instruction.target ? { target: instruction.target } : {}),
      ...(instruction.via ? { via: instruction.via } : {}),
      speed,
      zone,
      ...(instruction.tool ?? this.context.currentTool ? { tool: instruction.tool ?? this.context.currentTool } : {}),
      ...(instruction.frame ?? this.context.currentFrame ? { frame: instruction.frame ?? this.context.currentFrame } : {}),
      ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {})
    };
  }

  private resolveNamedPose(name: string, kind: "tool" | "frame"): Pose {
    const source = kind === "tool" ? this.context.tools : this.context.frames;
    const pose = source.get(name);
    if (!pose) {
      throw motionError(kind === "tool" ? "GRL_TOOL_NOT_FOUND" : "GRL_FRAME_NOT_FOUND", `Unknown ${kind} ${name}`);
    }
    return pose;
  }

  private collectExpressionUntilParamKeyword(): GrlToken[] {
    const tokens: GrlToken[] = [];
    let parenDepth = 0;
    let bracketDepth = 0;
    let braceDepth = 0;
    const startLine = this.peek().range.start.line;

    while (!this.isAtEnd()) {
      const token = this.peek();
      if (tokens.length > 0 && token.range.start.line > startLine && this.isStatementStart(token)) {
        break;
      }
      if (
        parenDepth === 0 &&
        bracketDepth === 0 &&
        braceDepth === 0 &&
        this.isExpressionTerminator(token)
      ) {
        break;
      }

      const consumed = this.advance();
      tokens.push(consumed);
      if (consumed.kind === "punctuation") {
        if (consumed.raw === "(") parenDepth += 1;
        if (consumed.raw === ")") parenDepth -= 1;
        if (consumed.raw === "[") bracketDepth += 1;
        if (consumed.raw === "]") bracketDepth -= 1;
        if (consumed.raw === "{") braceDepth += 1;
        if (consumed.raw === "}") braceDepth -= 1;
      }
    }

    if (tokens.length === 0) {
      throw motionError("GRL_EXPRESSION_MISSING", "Expected motion expression");
    }
    return tokens;
  }

  private isExpressionTerminator(token: GrlToken): boolean {
    return (
      this.isMotionStart(token) ||
      ((token.kind === "keyword" || token.kind === "identifier") &&
        ["speed", "zone", "tool", "frame", "via", "target"].includes(token.raw))
    );
  }

  private isMotionStart(token: GrlToken): boolean {
    return (
      (token.kind === "keyword" || token.kind === "identifier") &&
      ["movej", "movel", "movec", "set_tool", "set_frame", "set_speed", "set_zone"].includes(token.raw)
    );
  }

  private isStatementStart(token: GrlToken): boolean {
    return (
      (token.kind === "keyword" || token.kind === "identifier") &&
      [
        "movej",
        "movel",
        "movec",
        "set_tool",
        "set_frame",
        "set_speed",
        "set_zone",
        "io",
        "wait",
        "pulse",
        "run_path",
        "run_operation",
        "if",
        "elseif",
        "else",
        "while",
        "for",
        "switch",
        "case",
        "default",
        "break",
        "continue",
        "label",
        "jump",
        "call",
        "return",
        "alarm",
        "raise",
        "try",
        "catch",
        "finally",
        "enable",
        "disable",
        "end"
      ].includes(token.raw)
    );
  }

  private consumeIdentifier(message: string): string {
    const token = this.peek();
    if (token.kind === "identifier" || token.kind === "keyword") {
      this.advance();
      return token.raw;
    }
    throw motionError("GRL_IDENTIFIER_EXPECTED", message);
  }

  private consumeKeyword(keyword: string, message: string): void {
    if (!this.matchKeyword(keyword)) {
      throw motionError("GRL_KEYWORD_EXPECTED", message);
    }
  }

  private matchKeyword(keyword: string): boolean {
    const token = this.peek();
    if ((token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword) {
      this.advance();
      return true;
    }
    return false;
  }

  private advance(): GrlToken {
    this.current += 1;
    return this.previous();
  }

  private previous(): GrlToken {
    return this.tokens[this.current - 1]!;
  }

  private peek(): GrlToken {
    return this.tokens[this.current]!;
  }

  private isAtEnd(): boolean {
    return this.current >= this.tokens.length;
  }
}

class RunPathStatementParser {
  private current = 0;

  constructor(private readonly tokens: GrlToken[]) {}

  parseAll(): RunPathInstruction[] {
    const instructions: RunPathInstruction[] = [];
    while (!this.isAtEnd()) {
      if (this.matchKeyword("run_path")) {
        const start = this.previous();
        const path = this.consumeIdentifier("Expected path name after run_path");
        instructions.push({
          kind: "RUN_PATH",
          pathId: path.raw,
          sourceMap: tokenSourceMap(start)
        });
        continue;
      }
      this.advance();
    }
    return instructions;
  }

  private consumeIdentifier(message: string): GrlToken {
    const token = this.peek();
    if (token.kind === "identifier" || token.kind === "keyword") {
      return this.advance();
    }
    throw motionError("GRL_IDENTIFIER_EXPECTED", message);
  }

  private matchKeyword(keyword: string): boolean {
    const token = this.peek();
    if ((token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword) {
      this.advance();
      return true;
    }
    return false;
  }

  private advance(): GrlToken {
    this.current += 1;
    return this.previous();
  }

  private previous(): GrlToken {
    return this.tokens[this.current - 1]!;
  }

  private peek(): GrlToken {
    return this.tokens[this.current]!;
  }

  private isAtEnd(): boolean {
    return this.current >= this.tokens.length;
  }
}

class RunOperationStatementParser {
  private current = 0;

  constructor(private readonly tokens: GrlToken[]) {}

  parseAll(): RunOperationInstruction[] {
    const instructions: RunOperationInstruction[] = [];
    while (!this.isAtEnd()) {
      if (this.matchKeyword("run_operation")) {
        const start = this.previous();
        const operation = this.consumeIdentifier("Expected operation name after run_operation");
        instructions.push({
          kind: "RUN_OPERATION",
          operationId: operation.raw,
          sourceMap: tokenSourceMap(start)
        });
        continue;
      }
      this.advance();
    }
    return instructions;
  }

  private consumeIdentifier(message: string): GrlToken {
    const token = this.peek();
    if (token.kind === "identifier" || token.kind === "keyword") {
      return this.advance();
    }
    throw motionError("GRL_IDENTIFIER_EXPECTED", message);
  }

  private matchKeyword(keyword: string): boolean {
    const token = this.peek();
    if ((token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword) {
      this.advance();
      return true;
    }
    return false;
  }

  private advance(): GrlToken {
    this.current += 1;
    return this.previous();
  }

  private previous(): GrlToken {
    return this.tokens[this.current - 1]!;
  }

  private peek(): GrlToken {
    return this.tokens[this.current]!;
  }

  private isAtEnd(): boolean {
    return this.current >= this.tokens.length;
  }
}

function compileProcessBlock(block: GrlOperationProcessBlock): Record<string, unknown> {
  return Object.fromEntries(block.properties.map((property) => [property.key, compileLiteralValue(property.value)]));
}

function compileOperationAction(
  operationId: string,
  block: GrlOperationActionBlock
): OperationActionInstruction {
  const first = block.actionTokens[0] ?? block.actionTokens[block.actionTokens.length - 1]!;
  return {
    kind: "ACTION",
    actionKind: block.actionKind,
    operationId,
    statement: block.actionTokens.map((token) => token.raw).join(" "),
    tokens: block.actionTokens,
    sourceMap: tokenSourceMap(first)
  };
}

function compilePathDefaults(block: GrlPathDefaultsBlock | undefined, context: GrlMotionContext): PathDefaults {
  const defaults: PathDefaults = {};
  if (!block) {
    return defaults;
  }

  for (const property of block.properties) {
    if (property.key === "speed") {
      defaults.speed = resolveSpeed(property.value, context);
    } else if (property.key === "zone") {
      defaults.zone = resolveZone(property.value, context);
    } else if (property.key === "tool") {
      defaults.tool = resolveNamedPoseFromExpression(property.value, context, "tool");
    } else if (property.key === "frame") {
      defaults.frame = resolveNamedPoseFromExpression(property.value, context, "frame");
    }
  }
  return defaults;
}

function compilePathSource(block: GrlPathSourceBlock | undefined): JsonObject | undefined {
  if (!block) {
    return undefined;
  }
  return Object.fromEntries(block.properties.map((property) => [property.key, compileLiteralValue(property.value)]));
}

function compilePathEvent(
  event: GrlPathEvent,
  index: number,
  pointIds: Set<string>
): PathEventInstruction & PathEventRequest {
  if (!pointIds.has(event.pointId)) {
    throw motionError("GRL_PATH_EVENT_POINT_NOT_FOUND", `Path event references unknown point ${event.pointId}`);
  }

  return {
    id: `event_${index}`,
    timing: event.timing,
    pointId: event.pointId,
    ...(event.distance ? { distance: normalizedNumber(event.distance) } : {}),
    kind: event.actionTokens[0]?.raw ?? "statement",
    sourceMap: tokenSourceMap(event.actionTokens[0] ?? event.actionTokens[event.actionTokens.length - 1]!),
    data: {
      statement: event.actionTokens.map((token) => token.raw).join(" "),
      tokens: event.actionTokens
    }
  };
}

function cloneMotionContext(context: GrlMotionContext): GrlMotionContext {
  return {
    targets: context.targets,
    speeds: context.speeds,
    zones: context.zones,
    tools: context.tools,
    frames: context.frames,
    ...(context.currentSpeed ? { currentSpeed: context.currentSpeed } : {}),
    ...(context.currentZone ? { currentZone: context.currentZone } : {}),
    ...(context.currentTool ? { currentTool: context.currentTool } : {}),
    ...(context.currentFrame ? { currentFrame: context.currentFrame } : {})
  };
}

function applyPathDefaults(context: GrlMotionContext, defaults: PathDefaults): void {
  if (defaults.speed) {
    context.currentSpeed = defaults.speed;
  }
  if (defaults.zone) {
    context.currentZone = defaults.zone;
  }
  if (defaults.tool) {
    context.currentTool = defaults.tool;
  }
  if (defaults.frame) {
    context.currentFrame = defaults.frame;
  }
}

function resolveSpeed(expression: GrlExpression, context: GrlMotionContext): SpeedSpec {
  if (expression.kind === "IdentifierExpression" && context.speeds.has(expression.name)) {
    return context.speeds.get(expression.name)!;
  }
  return compileSpeedExpression(expression);
}

function resolveZone(expression: GrlExpression, context: GrlMotionContext): ZoneSpec {
  if (expression.kind === "IdentifierExpression" && context.zones.has(expression.name)) {
    return context.zones.get(expression.name)!;
  }
  return compileZoneExpression(expression);
}

function resolveNamedPoseFromExpression(
  expression: GrlExpression,
  context: GrlMotionContext,
  kind: "tool" | "frame"
): Pose {
  if (expression.kind !== "IdentifierExpression") {
    throw motionError(kind === "tool" ? "GRL_TOOL_NOT_FOUND" : "GRL_FRAME_NOT_FOUND", `Path ${kind} must reference a named ${kind}`);
  }
  const source = kind === "tool" ? context.tools : context.frames;
  const pose = source.get(expression.name);
  if (!pose) {
    throw motionError(kind === "tool" ? "GRL_TOOL_NOT_FOUND" : "GRL_FRAME_NOT_FOUND", `Unknown ${kind} ${expression.name}`);
  }
  return pose;
}

function compileLiteralValue(expression: GrlExpression): unknown {
  if (expression.kind === "NumberLiteral") {
    return normalizedNumber(expression);
  }
  if (expression.kind === "StringLiteral" || expression.kind === "BooleanLiteral") {
    return expression.value;
  }
  if (expression.kind === "IdentifierExpression") {
    return expression.name;
  }
  if (expression.kind === "ArrayExpression") {
    return expression.elements.map(compileLiteralValue);
  }
  if (expression.kind === "ObjectExpression") {
    return Object.fromEntries(expression.properties.map((property) => [property.key, compileLiteralValue(property.value)]));
  }
  if (expression.kind === "CallExpression") {
    return {
      callee: expression.callee,
      args: expression.args.map(compileLiteralValue)
    };
  }
  return {
    kind: expression.kind
  };
}

function tokenSourceMap(token: GrlToken) {
  return {
    line: token.range.start.line,
    column: token.range.start.column
  };
}

function isPoseTarget(target: JointTarget | PoseTarget): target is PoseTarget {
  return "pose" in target;
}

function targetIdOf(target: JointTarget | PoseTarget): string | undefined {
  return target.id;
}

function normalizedNumber(expression: { value: number; unit?: { normalizedValue: number } }): number {
  return expression.unit?.normalizedValue ?? expression.value;
}

function motionError(code: string, message: string): KdlStructuredError {
  return new KdlStructuredError(code, message);
}
