import { KdlStructuredError, type KdlRuntimeHandlers } from "./rpc.js";
import type { NativeKdlModule, NativeKdlModuleLoader } from "./nativeModule.js";
import {
  applyOffset as applyOffsetToTarget,
  applyToolAndFrame as applyToolAndFrameToTarget,
  composePose as composeRuntimePose,
  inversePose as inverseRuntimePose,
  normalizePose as normalizeRuntimePose
} from "./poseApi.js";
import {
  makeTrapProfile as makeRuntimeTrapProfile,
  sampleTrapProfile as sampleRuntimeTrapProfile
} from "./trapProfile.js";
import {
  estimateCycleTime as estimateRuntimeCycleTime,
  resampleTrajectory as resampleRuntimeTrajectory
} from "./trajectoryUtils.js";
import { loadRobotFromUrdfModel } from "../robot/urdfParser.js";
import { RobotModelRegistry } from "../robot/normalizedRobotModel.js";
import type {
  FkOptions,
  IkOptions,
  JacobianOptions,
  KdlInitOptions,
  KdlRuntimeInfo,
  MoveCRequest,
  MoveJRequest,
  MoveLRequest,
  NormalizedRobotModel,
  OffsetSpec,
  PathPlanRequest,
  PathPlanResult,
  Pose,
  PoseLike,
  PoseNormalizeOptions,
  PoseTarget,
  RobotHandle,
  TrapProfileOptions,
  TrajectoryResult,
  UrdfLoadOptions
} from "./types.js";

export class KdlWorkerRuntime {
  private initialized = false;
  private nativeModule: NativeKdlModule | undefined;
  private readonly robots = new RobotModelRegistry();

  constructor(private readonly loadNativeModule?: NativeKdlModuleLoader) {}

  async init(options?: KdlInitOptions): Promise<KdlRuntimeInfo> {
    if (this.loadNativeModule) {
      await this.initNativeModule(options ?? {});
    }

    this.initialized = true;

    return {
      version: "0.1.0",
      wasmBuild: options?.wasmBuild ?? (this.nativeModule ? "wasm" : "stub"),
      supportsThreads: options?.useThreads ?? false,
      supportsWasmFs: false
    };
  }

  async dispose(): Promise<void> {
    this.initialized = false;
    this.nativeModule = undefined;
  }

  assertInitialized(method: string): void {
    if (!this.initialized) {
      throw new KdlStructuredError(
        "KDL_NOT_INITIALIZED",
        `KDL runtime must be initialized before calling ${method}`
      );
    }
  }

  async loadRobotFromUrdf(urdfXml: string, options: UrdfLoadOptions): Promise<RobotHandle> {
    this.assertInitialized("loadRobotFromUrdf");
    const model = loadRobotFromUrdfModel(urdfXml, options);
    return this.createRobotFromModel(model);
  }

  async createRobotFromModel(model: NormalizedRobotModel): Promise<RobotHandle> {
    this.assertInitialized("createRobotFromModel");
    return this.robots.create(model);
  }

  async destroyRobot(handle: RobotHandle): Promise<void> {
    this.assertInitialized("destroyRobot");
    this.robots.destroy(handle);
  }

  async getRobotInfo(handle: RobotHandle) {
    this.assertInitialized("getRobotInfo");
    return this.robots.getInfo(handle);
  }

  async getJointLimits(handle: RobotHandle) {
    this.assertInitialized("getJointLimits");
    return this.robots.getJointLimits(handle);
  }

  async normalizePose(input: PoseLike, _options?: PoseNormalizeOptions) {
    this.assertInitialized("normalizePose");
    return normalizeRuntimePose(input);
  }

  async composePose(a: Pose, b: Pose) {
    this.assertInitialized("composePose");
    return composeRuntimePose(a, b);
  }

  async inversePose(pose: Pose) {
    this.assertInitialized("inversePose");
    return inverseRuntimePose(pose);
  }

  async applyToolAndFrame(target: PoseTarget, tool: Pose, frame: Pose) {
    this.assertInitialized("applyToolAndFrame");
    return applyToolAndFrameToTarget(target, tool, frame);
  }

  async applyOffset(target: PoseTarget, offset: OffsetSpec) {
    this.assertInitialized("applyOffset");
    return applyOffsetToTarget(target, offset);
  }

  async makeTrapProfile(length: number, options: TrapProfileOptions) {
    this.assertInitialized("makeTrapProfile");
    return makeRuntimeTrapProfile(length, options);
  }

  async sampleTrapProfile(length: number, options: TrapProfileOptions) {
    this.assertInitialized("sampleTrapProfile");
    return sampleRuntimeTrapProfile(length, options);
  }

  async fk(handle: RobotHandle, joints: Float64Array | number[], options?: FkOptions) {
    this.assertInitialized("fk");
    return this.robots.fk(handle, joints, options);
  }

  async fkPose7(handle: RobotHandle, joints: Float64Array | number[], out?: Float64Array, options?: FkOptions) {
    this.assertInitialized("fkPose7");
    return this.robots.fkPose7(handle, joints, out, options);
  }

  async fkAllLinks(handle: RobotHandle, joints: Float64Array | number[], options?: FkOptions) {
    this.assertInitialized("fkAllLinks");
    return this.robots.fkAllLinks(handle, joints, options);
  }

  async ik(handle: RobotHandle, seed: Float64Array | number[], target: Pose, options?: IkOptions) {
    this.assertInitialized("ik");
    return this.robots.ik(handle, seed, target, options);
  }

  async ikBatch(
    handle: RobotHandle,
    seeds: Array<Float64Array | number[]>,
    targets: Pose[],
    options?: IkOptions
  ) {
    this.assertInitialized("ikBatch");
    return this.robots.ikBatch(handle, seeds, targets, options);
  }

  async jacobian(handle: RobotHandle, joints: Float64Array | number[], options?: JacobianOptions) {
    this.assertInitialized("jacobian");
    return this.robots.jacobian(handle, joints, options);
  }

  async checkSingularity(handle: RobotHandle, joints: Float64Array | number[]) {
    this.assertInitialized("checkSingularity");
    return this.robots.checkSingularity(handle, joints);
  }

  async checkJointLimits(handle: RobotHandle, joints: Float64Array | number[]) {
    this.assertInitialized("checkJointLimits");
    return this.robots.checkJointLimits(handle, joints);
  }

  async checkVelocityLimits(handle: RobotHandle, trajectory: TrajectoryResult) {
    this.assertInitialized("checkVelocityLimits");
    return this.robots.checkVelocityLimits(handle, trajectory);
  }

  async checkReachability(handle: RobotHandle, target: PoseTarget, options?: IkOptions) {
    this.assertInitialized("checkReachability");
    return this.robots.checkReachability(handle, target, options);
  }

  async checkReachabilityBatch(handle: RobotHandle, targets: PoseTarget[], options?: IkOptions) {
    this.assertInitialized("checkReachabilityBatch");
    return this.robots.checkReachabilityBatch(handle, targets, options);
  }

  async planMoveJ(handle: RobotHandle, request: MoveJRequest) {
    this.assertInitialized("planMoveJ");
    return this.robots.planMoveJ(handle, request);
  }

  async planMoveL(handle: RobotHandle, request: MoveLRequest) {
    this.assertInitialized("planMoveL");
    return this.robots.planMoveL(handle, request);
  }

  async planMoveC(handle: RobotHandle, request: MoveCRequest) {
    this.assertInitialized("planMoveC");
    return this.robots.planMoveC(handle, request);
  }

  async planPath(handle: RobotHandle, request: PathPlanRequest) {
    this.assertInitialized("planPath");
    return this.robots.planPath(handle, request);
  }

  async validatePath(handle: RobotHandle, request: PathPlanRequest) {
    this.assertInitialized("validatePath");
    return this.robots.validatePath(handle, request);
  }

  async estimateCycleTime(input: TrajectoryResult | PathPlanResult) {
    this.assertInitialized("estimateCycleTime");
    return estimateRuntimeCycleTime(input);
  }

  async resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number) {
    this.assertInitialized("resampleTrajectory");
    return resampleRuntimeTrajectory(trajectory, sampleTime);
  }

  private async initNativeModule(options: KdlInitOptions): Promise<void> {
    try {
      const nativeModule = await this.loadNativeModule?.(options);
      if (!nativeModule) {
        throw new Error("No KDL native module was returned");
      }

      const result = nativeModule.ccall("kdl_init", "number", ["string"], [JSON.stringify(options)]);
      if (Number(result) !== 0) {
        throw new Error(`kdl_init returned ${String(result)}`);
      }

      this.nativeModule = nativeModule;
    } catch (error) {
      const message = error instanceof Error ? error.message : String(error);
      throw new KdlStructuredError(
        "KDL_WASM_INIT_FAILED",
        `Failed to initialize KDL WASM runtime: ${message}`
      );
    }
  }
}

export function createKdlWorkerRuntime(loadNativeModule?: NativeKdlModuleLoader): KdlRuntimeHandlers {
  const runtime = new KdlWorkerRuntime(loadNativeModule);

  return {
    init: (options?: unknown) => runtime.init(options as KdlInitOptions | undefined),
    dispose: () => runtime.dispose(),
    loadRobotFromUrdf: (urdfXml: unknown, options: unknown) =>
      runtime.loadRobotFromUrdf(urdfXml as string, options as UrdfLoadOptions),
    createRobotFromModel: (model: unknown) => runtime.createRobotFromModel(model as NormalizedRobotModel),
    destroyRobot: (handle: unknown) => runtime.destroyRobot(handle as RobotHandle),
    getRobotInfo: (handle: unknown) => runtime.getRobotInfo(handle as RobotHandle),
    getJointLimits: (handle: unknown) => runtime.getJointLimits(handle as RobotHandle),
    normalizePose: (input: unknown, options: unknown) =>
      runtime.normalizePose(input as PoseLike, options as PoseNormalizeOptions | undefined),
    composePose: (a: unknown, b: unknown) => runtime.composePose(a as Pose, b as Pose),
    inversePose: (pose: unknown) => runtime.inversePose(pose as Pose),
    applyToolAndFrame: (target: unknown, tool: unknown, frame: unknown) =>
      runtime.applyToolAndFrame(target as PoseTarget, tool as Pose, frame as Pose),
    applyOffset: (target: unknown, offset: unknown) =>
      runtime.applyOffset(target as PoseTarget, offset as OffsetSpec),
    makeTrapProfile: (length: unknown, options: unknown) =>
      runtime.makeTrapProfile(length as number, options as TrapProfileOptions),
    sampleTrapProfile: (length: unknown, options: unknown) =>
      runtime.sampleTrapProfile(length as number, options as TrapProfileOptions),
    fk: (handle: unknown, joints: unknown, options: unknown) =>
      runtime.fk(handle as RobotHandle, joints as Float64Array | number[], options as FkOptions | undefined),
    fkPose7: (handle: unknown, joints: unknown, out: unknown, options: unknown) =>
      runtime.fkPose7(
        handle as RobotHandle,
        joints as Float64Array | number[],
        out as Float64Array | undefined,
        options as FkOptions | undefined
      ),
    fkAllLinks: (handle: unknown, joints: unknown, options: unknown) =>
      runtime.fkAllLinks(handle as RobotHandle, joints as Float64Array | number[], options as FkOptions | undefined),
    ik: (handle: unknown, seed: unknown, target: unknown, options: unknown) =>
      runtime.ik(handle as RobotHandle, seed as Float64Array | number[], target as Pose, options as IkOptions | undefined),
    ikBatch: (handle: unknown, seeds: unknown, targets: unknown, options: unknown) =>
      runtime.ikBatch(
        handle as RobotHandle,
        seeds as Array<Float64Array | number[]>,
        targets as Pose[],
        options as IkOptions | undefined
      ),
    jacobian: (handle: unknown, joints: unknown, options: unknown) =>
      runtime.jacobian(handle as RobotHandle, joints as Float64Array | number[], options as JacobianOptions | undefined),
    checkSingularity: (handle: unknown, joints: unknown) =>
      runtime.checkSingularity(handle as RobotHandle, joints as Float64Array | number[]),
    checkJointLimits: (handle: unknown, joints: unknown) =>
      runtime.checkJointLimits(handle as RobotHandle, joints as Float64Array | number[]),
    checkVelocityLimits: (handle: unknown, trajectory: unknown) =>
      runtime.checkVelocityLimits(handle as RobotHandle, trajectory as TrajectoryResult),
    checkReachability: (handle: unknown, target: unknown, options: unknown) =>
      runtime.checkReachability(handle as RobotHandle, target as PoseTarget, options as IkOptions | undefined),
    checkReachabilityBatch: (handle: unknown, targets: unknown, options: unknown) =>
      runtime.checkReachabilityBatch(handle as RobotHandle, targets as PoseTarget[], options as IkOptions | undefined),
    planMoveJ: (handle: unknown, request: unknown) =>
      runtime.planMoveJ(handle as RobotHandle, request as MoveJRequest),
    planMoveL: (handle: unknown, request: unknown) =>
      runtime.planMoveL(handle as RobotHandle, request as MoveLRequest),
    planMoveC: (handle: unknown, request: unknown) =>
      runtime.planMoveC(handle as RobotHandle, request as MoveCRequest),
    planPath: (handle: unknown, request: unknown) =>
      runtime.planPath(handle as RobotHandle, request as PathPlanRequest),
    validatePath: (handle: unknown, request: unknown) =>
      runtime.validatePath(handle as RobotHandle, request as PathPlanRequest),
    estimateCycleTime: (input: unknown) =>
      runtime.estimateCycleTime(input as TrajectoryResult | PathPlanResult),
    resampleTrajectory: (trajectory: unknown, sampleTime: unknown) =>
      runtime.resampleTrajectory(trajectory as TrajectoryResult, sampleTime as number)
  };
}
