KDL_WORK/kdl-wasm/web/src/controller/virtualController.ts

import type { ExecutableInstruction, SemanticProgramIr } from "../grl/ir/index.js";
import { IoImageRuntime, type IoRuntimeOptions } from "../runtime/ioRuntime.js";
import { KdlRuntimeBridge } from "../runtime/kdlBridge.js";
import { MotionQueue, type MotionPlanner } from "../runtime/motionQueue.js";
import type { RobotHandle } from "../kdl/types.js";
import { IrExecutionRuntime, type IrExecutionRuntimeSnapshot, type RuntimeStepResult } from "./runtime.js";
import { VirtualControllerStateMachine } from "./stateMachine.js";
import type { RuntimeTraceEvent } from "./trace.js";

export interface VirtualControllerLoadOptions {
  entryProcedure?: string;
  startJoints: number[];
  sampleTime: number;
  planner?: MotionPlanner;
  robotHandle?: RobotHandle;
  io?: IoRuntimeOptions;
}

export interface VirtualControllerSnapshot {
  state: ReturnType<VirtualControllerStateMachine["snapshot"]>;
  runtime: IrExecutionRuntimeSnapshot;
  motion?: ReturnType<MotionQueue["snapshot"]>;
  io: ReturnType<IoImageRuntime["snapshot"]>;
  bridge?: ReturnType<KdlRuntimeBridge["snapshot"]>;
}

export class VirtualController {
  readonly stateMachine = new VirtualControllerStateMachine();
  private ioValue = new IoImageRuntime();
  private runtimeValue: IrExecutionRuntime | undefined;
  private motionQueueValue: MotionQueue | undefined;
  private bridgeValue: KdlRuntimeBridge | undefined;
  private loadedOptions: VirtualControllerLoadOptions | undefined;

  get io(): IoImageRuntime {
    return this.ioValue;
  }

  get runtime(): IrExecutionRuntime | undefined {
    return this.runtimeValue;
  }

  get motionQueue(): MotionQueue | undefined {
    return this.motionQueueValue;
  }

  get bridge(): KdlRuntimeBridge | undefined {
    return this.bridgeValue;
  }

  load(program: SemanticProgramIr, options: VirtualControllerLoadOptions): RuntimeTraceEvent | undefined {
    this.loadedOptions = options;
    this.ioValue = new IoImageRuntime(options.io);
    this.motionQueueValue = new MotionQueue({
      startJoints: options.startJoints,
      sampleTime: options.sampleTime,
      ...(options.planner ? { planner: options.planner } : {}),
      ...(options.robotHandle !== undefined ? { robotHandle: options.robotHandle } : {})
    });
    this.runtimeValue = new IrExecutionRuntime({
      ...(options.entryProcedure ? { entryProcedure: options.entryProcedure } : {}),
      hooks: {
        onMotion: (instruction, runtime) => {
          this.bridgeValue?.enqueueInstruction(instruction);
          runtime.trace("motion", {
            instruction,
            data: { queued: true }
          });
        },
        onIo: (instruction, runtime) => {
          const result = this.ioValue.executeInstruction(instruction);
          runtime.trace("io", {
            instruction,
            data: { result }
          });
        },
        onWait: (instruction, runtime) => {
          const waitKey = `$wait:${runtime.currentFrame?.procedure ?? "main"}:${runtime.currentFrame?.pc ?? 0}`;
          if (runtime.getVariable(waitKey) === undefined) {
            runtime.setVariable(waitKey, runtime.virtualTime, "global");
          }
          const elapsed = runtime.virtualTime - Number(runtime.getVariable(waitKey) ?? runtime.virtualTime);
          const result = this.ioValue.evaluateWait(instruction, elapsed);
          runtime.trace("wait", {
            instruction,
            data: { status: result.status, elapsed }
          });
          if (result.status === "satisfied" || result.status === "timeout") {
            if (result.onTimeout?.kind === "alarm") {
              runtime.pushAlarm(result.onTimeout.value, result.onTimeout.value, "warning", runtime.locateInstruction(instruction));
            }
            return undefined;
          }
          if (result.status === "hold-stop") {
            this.stateMachine.dispatch("hold");
          }
          return {
            status: "blocked",
            instruction
          };
        }
      }
    });
    this.runtimeValue.load(program, options.entryProcedure);
    this.bridgeValue = new KdlRuntimeBridge(program, this.motionQueueValue);
    this.stateMachine.dispatch("load");
    return this.runtimeValue.snapshot().trace.at(-1);
  }

  run(maxSteps?: number): RuntimeStepResult {
    const transition = this.stateMachine.dispatch("run");
    if (!transition.ok) {
      return blockedTransitionResult(transition.diagnostic);
    }
    const result = this.requireRuntime().run(maxSteps);
    if (result.status === "completed") {
      this.stateMachine.dispatch("stop");
    } else if (result.status === "breakpoint") {
      this.stateMachine.dispatch("pause");
    }
    return result;
  }

  pause(): void {
    this.stateMachine.dispatch("pause");
  }

  stop(): void {
    this.stateMachine.dispatch("stop");
  }

  reset(): void {
    const options = this.loadedOptions;
    this.stateMachine.dispatch("reset");
    if (!options || !this.runtimeValue || !this.runtimeValue.program) {
      return;
    }
    this.runtimeValue.reset(options.entryProcedure);
    this.motionQueueValue?.clear();
  }

  step(): RuntimeStepResult {
    const transition = this.stateMachine.dispatch("step");
    if (!transition.ok) {
      return blockedTransitionResult(transition.diagnostic);
    }
    return this.requireRuntime().step();
  }

  advance(deltaTime: number): void {
    this.io.advance(deltaTime);
    this.motionQueueValue?.advance(deltaTime);
    if (this.runtimeValue) {
      this.runtimeValue.virtualTime += deltaTime;
    }
  }

  snapshot(): VirtualControllerSnapshot {
    return {
      state: this.stateMachine.snapshot(),
      runtime: this.requireRuntime().snapshot(),
      ...(this.motionQueueValue ? { motion: this.motionQueueValue.snapshot() } : {}),
      io: this.ioValue.snapshot(),
      ...(this.bridgeValue ? { bridge: this.bridgeValue.snapshot() } : {})
    };
  }

  private requireRuntime(): IrExecutionRuntime {
    if (!this.runtimeValue) {
      throw new Error("Controller has not loaded a program");
    }
    return this.runtimeValue;
  }
}

function blockedTransitionResult(diagnostic: RuntimeStepResult["diagnostic"]): RuntimeStepResult {
  return {
    status: "blocked",
    ...(diagnostic ? { diagnostic } : {})
  };
}

export function isMotionInstruction(
  instruction: ExecutableInstruction
): instruction is Extract<ExecutableInstruction, { kind: "MOVEJ" | "MOVEL" | "MOVEC" | "RUN_PATH" | "RUN_OPERATION" }> {
  return (
    instruction.kind === "MOVEJ" ||
    instruction.kind === "MOVEL" ||
    instruction.kind === "MOVEC" ||
    instruction.kind === "RUN_PATH" ||
    instruction.kind === "RUN_OPERATION"
  );
}