import {
  KdlStructuredError,
  rpcErrorToException,
  type KdlRpcRequest,
  type KdlRpcResponse
} from "./rpc.js";
import type {
  CycleTimeResult,
  JointLimits,
  FkOptions,
  FkResult,
  IkOptions,
  IkResult,
  JacobianOptions,
  JacobianResult,
  KdlApiMethod,
  KdlInitOptions,
  KdlRuntimeInfo,
  KdlWasmApi,
  LinkPoseResult,
  LimitCheckResult,
  MoveCRequest,
  MoveJRequest,
  MoveLRequest,
  NormalizedRobotModel,
  OffsetSpec,
  PathPlanRequest,
  PathPlanResult,
  PathValidationResult,
  Pose,
  PoseLike,
  PoseNormalizeOptions,
  PoseTarget,
  ReachabilityResult,
  RobotHandle,
  RobotInfo,
  SingularityResult,
  TrapProfileOptions,
  TrapProfileResult,
  TrapSample,
  TrajectoryResult,
  UrdfLoadOptions
} from "./types.js";

export interface KdlWorkerLike {
  postMessage(message: KdlRpcRequest<unknown[]>, transfer?: Transferable[]): void;
  terminate?: () => void;
  addEventListener(type: "message", listener: (event: MessageEvent<KdlRpcResponse>) => void): void;
  addEventListener(type: "error", listener: (event: ErrorEvent) => void): void;
  removeEventListener(type: "message", listener: (event: MessageEvent<KdlRpcResponse>) => void): void;
  removeEventListener(type: "error", listener: (event: ErrorEvent) => void): void;
}

interface PendingCall {
  method: KdlApiMethod;
  resolve: (value: unknown) => void;
  reject: (reason?: unknown) => void;
}

export class KdlWorkerClient
  implements
    Pick<
      KdlWasmApi,
      | "init"
      | "dispose"
      | "loadRobotFromUrdf"
      | "createRobotFromModel"
      | "destroyRobot"
      | "getRobotInfo"
      | "getJointLimits"
      | "normalizePose"
      | "composePose"
      | "inversePose"
      | "applyToolAndFrame"
      | "applyOffset"
      | "makeTrapProfile"
      | "sampleTrapProfile"
      | "fk"
      | "fkPose7"
      | "fkAllLinks"
      | "ik"
      | "ikBatch"
      | "jacobian"
      | "checkSingularity"
      | "checkJointLimits"
      | "checkVelocityLimits"
      | "checkReachability"
      | "checkReachabilityBatch"
      | "planMoveJ"
      | "planMoveL"
      | "planMoveC"
      | "planPath"
      | "validatePath"
      | "estimateCycleTime"
      | "resampleTrajectory"
    >
{
  private nextId = 1;
  private worker: KdlWorkerLike | undefined;
  private readonly pending = new Map<number, PendingCall>();
  private readonly handleMessage = (event: MessageEvent<KdlRpcResponse>) => {
    this.acceptResponse(event.data);
  };
  private readonly handleError = (event: ErrorEvent) => {
    this.failWorker(event.error instanceof Error ? event.error : new Error(event.message));
  };

  constructor(private readonly createWorker: () => KdlWorkerLike) {}

  async init(options?: KdlInitOptions): Promise<KdlRuntimeInfo> {
    return this.call("init", options) as Promise<KdlRuntimeInfo>;
  }

  async dispose(): Promise<void> {
    if (!this.worker) {
      return;
    }

    try {
      await this.call("dispose");
    } finally {
      this.detachWorker();
    }
  }

  async loadRobotFromUrdf(urdfXml: string, options: UrdfLoadOptions): Promise<RobotHandle> {
    return this.call("loadRobotFromUrdf", urdfXml, options) as Promise<RobotHandle>;
  }

  async createRobotFromModel(model: NormalizedRobotModel): Promise<RobotHandle> {
    return this.call("createRobotFromModel", model) as Promise<RobotHandle>;
  }

  async destroyRobot(handle: RobotHandle): Promise<void> {
    await this.call("destroyRobot", handle);
  }

  async getRobotInfo(handle: RobotHandle): Promise<RobotInfo> {
    return this.call("getRobotInfo", handle) as Promise<RobotInfo>;
  }

  async getJointLimits(handle: RobotHandle): Promise<JointLimits[]> {
    return this.call("getJointLimits", handle) as Promise<JointLimits[]>;
  }

  async normalizePose(input: PoseLike, options?: PoseNormalizeOptions): Promise<Pose> {
    return this.call("normalizePose", input, options) as Promise<Pose>;
  }

  async composePose(a: Pose, b: Pose): Promise<Pose> {
    return this.call("composePose", a, b) as Promise<Pose>;
  }

  async inversePose(pose: Pose): Promise<Pose> {
    return this.call("inversePose", pose) as Promise<Pose>;
  }

  async applyToolAndFrame(target: PoseTarget, tool: Pose, frame: Pose): Promise<Pose> {
    return this.call("applyToolAndFrame", target, tool, frame) as Promise<Pose>;
  }

  async applyOffset(target: PoseTarget, offset: OffsetSpec): Promise<PoseTarget> {
    return this.call("applyOffset", target, offset) as Promise<PoseTarget>;
  }

  async makeTrapProfile(length: number, options: TrapProfileOptions): Promise<TrapProfileResult> {
    return this.call("makeTrapProfile", length, options) as Promise<TrapProfileResult>;
  }

  async sampleTrapProfile(length: number, options: TrapProfileOptions): Promise<TrapSample[]> {
    return this.call("sampleTrapProfile", length, options) as Promise<TrapSample[]>;
  }

  async fk(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise<FkResult> {
    return this.call("fk", handle, joints, options) as Promise<FkResult>;
  }

  async fkPose7(handle: RobotHandle, joints: Float64Array, out?: Float64Array, options?: FkOptions): Promise<Float64Array> {
    return this.call("fkPose7", handle, joints, out, options) as Promise<Float64Array>;
  }

  async fkAllLinks(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise<LinkPoseResult> {
    return this.call("fkAllLinks", handle, joints, options) as Promise<LinkPoseResult>;
  }

  async ik(handle: RobotHandle, seed: Float64Array, target: Pose, options?: IkOptions): Promise<IkResult> {
    return this.call("ik", handle, seed, target, options) as Promise<IkResult>;
  }

  async ikBatch(handle: RobotHandle, seeds: Float64Array[], targets: Pose[], options?: IkOptions): Promise<IkResult[]> {
    return this.call("ikBatch", handle, seeds, targets, options) as Promise<IkResult[]>;
  }

  async jacobian(handle: RobotHandle, joints: Float64Array, options?: JacobianOptions): Promise<JacobianResult> {
    return this.call("jacobian", handle, joints, options) as Promise<JacobianResult>;
  }

  async checkSingularity(handle: RobotHandle, joints: Float64Array): Promise<SingularityResult> {
    return this.call("checkSingularity", handle, joints) as Promise<SingularityResult>;
  }

  async checkJointLimits(handle: RobotHandle, joints: Float64Array): Promise<LimitCheckResult> {
    return this.call("checkJointLimits", handle, joints) as Promise<LimitCheckResult>;
  }

  async checkVelocityLimits(handle: RobotHandle, trajectory: TrajectoryResult): Promise<LimitCheckResult> {
    return this.call("checkVelocityLimits", handle, trajectory) as Promise<LimitCheckResult>;
  }

  async checkReachability(handle: RobotHandle, target: PoseTarget, options?: IkOptions): Promise<ReachabilityResult> {
    return this.call("checkReachability", handle, target, options) as Promise<ReachabilityResult>;
  }

  async checkReachabilityBatch(
    handle: RobotHandle,
    targets: PoseTarget[],
    options?: IkOptions
  ): Promise<ReachabilityResult[]> {
    return this.call("checkReachabilityBatch", handle, targets, options) as Promise<ReachabilityResult[]>;
  }

  async planMoveJ(handle: RobotHandle, request: MoveJRequest): Promise<TrajectoryResult> {
    return this.call("planMoveJ", handle, request) as Promise<TrajectoryResult>;
  }

  async planMoveL(handle: RobotHandle, request: MoveLRequest): Promise<TrajectoryResult> {
    return this.call("planMoveL", handle, request) as Promise<TrajectoryResult>;
  }

  async planMoveC(handle: RobotHandle, request: MoveCRequest): Promise<TrajectoryResult> {
    return this.call("planMoveC", handle, request) as Promise<TrajectoryResult>;
  }

  async planPath(handle: RobotHandle, request: PathPlanRequest): Promise<PathPlanResult> {
    return this.call("planPath", handle, request) as Promise<PathPlanResult>;
  }

  async validatePath(handle: RobotHandle, request: PathPlanRequest): Promise<PathValidationResult> {
    return this.call("validatePath", handle, request) as Promise<PathValidationResult>;
  }

  async estimateCycleTime(input: TrajectoryResult | PathPlanResult): Promise<CycleTimeResult> {
    return this.call("estimateCycleTime", input) as Promise<CycleTimeResult>;
  }

  async resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number): Promise<TrajectoryResult> {
    return this.call("resampleTrajectory", trajectory, sampleTime) as Promise<TrajectoryResult>;
  }

  async call(method: KdlApiMethod, ...args: unknown[]): Promise<unknown> {
    const worker = this.ensureWorker();
    const id = this.nextId++;
    const request: KdlRpcRequest<unknown[]> = {
      id,
      method,
      payload: args
    };

    return new Promise((resolve, reject) => {
      this.pending.set(id, { method, resolve, reject });
      worker.postMessage(request);
    });
  }

  private ensureWorker(): KdlWorkerLike {
    if (this.worker) {
      return this.worker;
    }

    const worker = this.createWorker();
    worker.addEventListener("message", this.handleMessage);
    worker.addEventListener("error", this.handleError);
    this.worker = worker;
    return worker;
  }

  private acceptResponse(response: KdlRpcResponse): void {
    const pending = this.pending.get(response.id);
    if (!pending) {
      return;
    }

    this.pending.delete(response.id);

    if (response.ok) {
      pending.resolve(response.result);
    } else {
      pending.reject(
        rpcErrorToException(
          response.error ?? {
            code: "KDL_RPC_MISSING_ERROR",
            message: `KDL worker returned a failed response for ${pending.method} without error details`
          }
        )
      );
    }
  }

  private failWorker(error: Error): void {
    const structured = new KdlStructuredError("KDL_WORKER_CRASHED", error.message);
    for (const pending of this.pending.values()) {
      pending.reject(structured);
    }
    this.pending.clear();
    this.detachWorker();
  }

  private detachWorker(): void {
    if (!this.worker) {
      return;
    }

    this.worker.removeEventListener("message", this.handleMessage);
    this.worker.removeEventListener("error", this.handleError);
    this.worker.terminate?.();
    this.worker = undefined;
  }
}
