// This Source Code Form is subject to the terms of the Mozilla Public
// License, v2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/
import { wrap, transfer } from "comlink";
import { estimateObjectSize } from "@foxglove/studio-base/players/messageMemoryEstimation.js";
import { Logger } from "../../../utils/logger";
import { parseMessage } from "../../utils.js";
const logger = Logger.getLogger();
let lastDepthWorkersBusyLogTime = 0;
const DEPTH_WORKER_BUSY_LOG_INTERVAL_MS = 2000;
let workerIndex = 0;
const WORKER_COUNT = 3;
const workerPool = Array.from({ length: WORKER_COUNT }, () => ({
    remote: wrap(new Worker(new URL("./depth-coloring.worker", import.meta.url), { type: "module" })),
    busy: false,
}));
function getDepthWorker() {
    for (let i = 0; i < workerPool.length; i++) {
        const idx = (workerIndex + i) % workerPool.length;
        const slot = workerPool[idx];
        if (!slot?.busy) {
            slot.busy = true;
            workerIndex = (idx + 1) % workerPool.length;
            return slot.remote;
        }
    }
    return null;
}
function releaseWorker(remote) {
    const slot = workerPool.find((s) => s.remote === remote);
    if (slot) {
        slot.busy = false;
    }
}
export async function deserializeDepthFrame({ topic, message, callback, maxStereoDepth, }) {
    const { receiveTime, encoding, width, height } = parseMessage(message);
    const worker = getDepthWorker();
    if (!worker) {
        const now = Date.now();
        if (now - lastDepthWorkersBusyLogTime >= DEPTH_WORKER_BUSY_LOG_INTERVAL_MS) {
            lastDepthWorkersBusyLogTime = now;
            logger.warn(`All depth workers busy, dropping frame for topic ${topic}.`, {
                width,
                height,
                workerCount: WORKER_COUNT,
            });
        }
        return;
    }
    let colorizedData;
    try {
        colorizedData = await worker.colorDepht(transfer(message.data, [message.data.buffer]), encoding, width, height, maxStereoDepth);
    }
    catch (err) {
        logger.error("Error in depth worker:", err);
        releaseWorker(worker);
        throw err;
    }
    finally {
        releaseWorker(worker);
    }
    const foxgloveMessage = {
        timestamp: receiveTime,
        frame_id: `depth-${topic}-frame`,
        width,
        height,
        encoding: "rgb8",
        step: width * 3,
        data: colorizedData,
    };
    callback({
        topic: topic === "Point Cloud" ? "Depth Stream" : topic,
        receiveTime,
        message: foxgloveMessage,
        sizeInBytes: estimateObjectSize(foxgloveMessage),
        schemaName: "foxglove.RawImage",
        depthaiTransformation: message.transformation,
    });
}
export function getDistanceFromDepthDataForOffset(depthData, width, height, xOffset, yOffset) {
    if (xOffset < 0 || xOffset > 1 || yOffset < 0 || yOffset > 1) {
        logger.error("Offsets must be between 0 and 1", { xOffset, yOffset });
        return undefined;
    }
    const xCenter = Math.min(width - 1, Math.max(0, Math.floor(xOffset * width)));
    const yCenter = Math.min(height - 1, Math.max(0, Math.floor(yOffset * height)));
    const pointStride = 16;
    const zByteOffset = 8;
    const view = new DataView(depthData.buffer, depthData.byteOffset, depthData.byteLength);
    const radius = 2;
    let sum = 0;
    let count = 0;
    for (let dy = -radius; dy <= radius; dy++) {
        const y = yCenter + dy;
        if (y < 0 || y >= height) {
            continue;
        }
        for (let dx = -radius; dx <= radius; dx++) {
            const x = xCenter + dx;
            if (x < 0 || x >= width) {
                continue;
            }
            const idx = y * width + x;
            const offset = idx * pointStride + zByteOffset;
            if (offset + 4 > depthData.byteLength) {
                continue;
            }
            const z = view.getFloat32(offset, true);
            if (Number.isFinite(z)) {
                sum += z;
                count++;
            }
        }
    }
    if (count === 0) {
        return undefined;
    }
    return sum / count;
}
//# sourceMappingURL=depth.js.map