export class NavigationGrid {
    width;
    height;
    origin;
    cellSize;
    allowDiagonal;
    blocked = new Set();
    costs = new Map();
    constructor(options) {
        if (!Number.isInteger(options.width) || options.width < 2 || !Number.isInteger(options.height) || options.height < 2) {
            throw new RangeError("NavigationGrid width and height must be integers >= 2.");
        }
        this.width = options.width;
        this.height = options.height;
        this.origin = options.origin ?? [0, 0];
        this.cellSize = options.cellSize ?? 1;
        this.allowDiagonal = options.allowDiagonal ?? false;
        if (!Number.isFinite(this.cellSize) || this.cellSize <= 0) {
            throw new RangeError("NavigationGrid cellSize must be a finite positive number.");
        }
        for (const cell of options.blocked ?? []) {
            if (this.containsCell(cell))
                this.blocked.add(cellKey(cell));
        }
        for (const entry of options.costs ?? []) {
            if (!this.containsCell(entry.cell))
                continue;
            if (!Number.isFinite(entry.cost) || entry.cost <= 0) {
                throw new RangeError("NavigationGrid traversal costs must be finite positive numbers.");
            }
            this.costs.set(cellKey(entry.cell), entry.cost);
        }
    }
    isBlocked(cell) {
        return this.blocked.has(cellKey(cell));
    }
    cellCost(cell) {
        return this.costs.get(cellKey(cell)) ?? 1;
    }
    containsCell(cell) {
        return Number.isInteger(cell[0]) && Number.isInteger(cell[1]) && cell[0] >= 0 && cell[1] >= 0 && cell[0] < this.width && cell[1] < this.height;
    }
    worldToCell(point) {
        return [
            clampInt(Math.floor((point[0] - this.origin[0]) / this.cellSize), 0, this.width - 1),
            clampInt(Math.floor((point[1] - this.origin[1]) / this.cellSize), 0, this.height - 1),
        ];
    }
    cellToWorld(cell) {
        return [
            this.origin[0] + (cell[0] + 0.5) * this.cellSize,
            this.origin[1] + (cell[1] + 0.5) * this.cellSize,
        ];
    }
    findPath(start, goal) {
        const startCell = this.nearestWalkableCell(this.worldToCell(start));
        const goalCell = this.nearestWalkableCell(this.worldToCell(goal));
        if (!startCell || !goalCell)
            return failedPath();
        const result = this.search(startCell, goalCell);
        if (!result)
            return failedPath();
        const cells = reconstruct(result.node);
        const waypoints = simplifyPath(cells).map((cell) => this.cellToWorld(cell));
        const lastWaypoint = result.complete ? goal : this.cellToWorld(cells[cells.length - 1]);
        const normalizedWaypoints = [
            start,
            ...waypoints.slice(1, -1),
            lastWaypoint
        ];
        return {
            status: result.complete ? "success" : "partial",
            cells,
            waypoints: normalizedWaypoints,
            length: pathLength(normalizedWaypoints),
            cost: round3(result.node.g),
            visitedCells: result.visitedCells,
        };
    }
    search(startCell, goalCell) {
        const open = [node(startCell, undefined, 0, manhattan(startCell, goalCell))];
        const bestByCell = new Map([[cellKey(startCell), open[0]]]);
        const closed = new Set();
        let best = open[0];
        while (open.length > 0) {
            open.sort((left, right) => left.f - right.f || left.h - right.h);
            const current = open.shift();
            if (closed.has(current.key))
                continue;
            closed.add(current.key);
            if (current.h < best.h)
                best = current;
            if (sameCell(current.cell, goalCell)) {
                return { node: current, complete: true, visitedCells: closed.size };
            }
            for (const nextCell of neighbors(current.cell, this.allowDiagonal)) {
                if (!this.containsCell(nextCell) || this.isBlocked(nextCell))
                    continue;
                const key = cellKey(nextCell);
                if (closed.has(key))
                    continue;
                const stepCost = current.cell[0] !== nextCell[0] && current.cell[1] !== nextCell[1] ? Math.SQRT2 : 1;
                const g = current.g + stepCost * this.cellCost(nextCell);
                const h = heuristic(nextCell, goalCell, this.allowDiagonal);
                const existing = bestByCell.get(key);
                if (existing && existing.g <= g)
                    continue;
                const next = node(nextCell, current, g, h);
                bestByCell.set(key, next);
                open.push(next);
            }
        }
        return best ? { node: best, complete: false, visitedCells: closed.size } : undefined;
    }
    nearestWalkableCell(cell) {
        if (this.containsCell(cell) && !this.isBlocked(cell))
            return cell;
        for (let radius = 1; radius < Math.max(this.width, this.height); radius += 1) {
            for (let y = cell[1] - radius; y <= cell[1] + radius; y += 1) {
                for (let x = cell[0] - radius; x <= cell[0] + radius; x += 1) {
                    const candidate = [x, y];
                    if (this.containsCell(candidate) && !this.isBlocked(candidate))
                        return candidate;
                }
            }
        }
        return undefined;
    }
}
export class NavigationAgent {
    position;
    path = failedPath();
    waypointIndex = 0;
    distanceTraveled = 0;
    speed;
    waypointRadius;
    constructor(options) {
        this.position = options.position;
        this.speed = options.speed ?? 1;
        this.waypointRadius = options.waypointRadius ?? 0.05;
        if (!Number.isFinite(this.speed) || this.speed <= 0 || !Number.isFinite(this.waypointRadius) || this.waypointRadius < 0) {
            throw new RangeError("NavigationAgent speed must be positive and waypointRadius must be non-negative.");
        }
    }
    setPath(path) {
        this.path = path;
        this.waypointIndex = path.waypoints.length > 1 ? 1 : 0;
        this.distanceTraveled = 0;
    }
    update(deltaSeconds) {
        if (this.path.status === "failed")
            return this.snapshot("blocked");
        if (this.waypointIndex >= this.path.waypoints.length)
            return this.snapshot("arrived");
        const target = this.path.waypoints[this.waypointIndex];
        const dx = target[0] - this.position[0];
        const dy = target[1] - this.position[1];
        const distance = Math.hypot(dx, dy);
        if (distance <= this.waypointRadius) {
            this.position = target;
            this.waypointIndex += 1;
            return this.update(deltaSeconds);
        }
        const step = Math.min(distance, Math.max(0, deltaSeconds) * this.speed);
        if (step >= distance) {
            this.position = target;
            this.distanceTraveled += distance;
            this.waypointIndex += 1;
            return this.snapshot("moving");
        }
        this.position = [
            this.position[0] + (dx / distance) * step,
            this.position[1] + (dy / distance) * step,
        ];
        this.distanceTraveled += step;
        return this.snapshot("moving");
    }
    snapshot(state = this.path.status === "failed" ? "blocked" : "idle") {
        const resolvedState = this.path.status === "failed" ? "blocked" : this.waypointIndex >= this.path.waypoints.length ? "arrived" : state;
        return {
            position: [round3(this.position[0]), round3(this.position[1])],
            state: resolvedState,
            waypointIndex: this.waypointIndex,
            remainingWaypoints: Math.max(0, this.path.waypoints.length - this.waypointIndex),
            pathLength: round3(this.path.length),
            distanceTraveled: round3(this.distanceTraveled),
        };
    }
}
function node(cell, parent, g, h) {
    return { cell, key: cellKey(cell), parent, g, h, f: g + h };
}
function neighbors(cell, allowDiagonal) {
    const cardinal = [[cell[0] + 1, cell[1]], [cell[0] - 1, cell[1]], [cell[0], cell[1] + 1], [cell[0], cell[1] - 1]];
    return allowDiagonal
        ? [
            ...cardinal,
            [cell[0] + 1, cell[1] + 1],
            [cell[0] + 1, cell[1] - 1],
            [cell[0] - 1, cell[1] + 1],
            [cell[0] - 1, cell[1] - 1],
        ]
        : cardinal;
}
function reconstruct(node) {
    const cells = [];
    let current = node;
    while (current) {
        cells.push(current.cell);
        current = current.parent;
    }
    return cells.reverse();
}
function simplifyPath(cells) {
    if (cells.length <= 2)
        return cells;
    const result = [cells[0]];
    let previousDirection = [cells[1][0] - cells[0][0], cells[1][1] - cells[0][1]];
    for (let index = 2; index < cells.length; index += 1) {
        const direction = [cells[index][0] - cells[index - 1][0], cells[index][1] - cells[index - 1][1]];
        if (!sameCell(direction, previousDirection)) {
            result.push(cells[index - 1]);
            previousDirection = direction;
        }
    }
    result.push(cells[cells.length - 1]);
    return result;
}
function failedPath() {
    return { status: "failed", cells: [], waypoints: [], length: 0, cost: 0, visitedCells: 0 };
}
function pathLength(points) {
    let length = 0;
    for (let index = 1; index < points.length; index += 1) {
        length += Math.hypot(points[index][0] - points[index - 1][0], points[index][1] - points[index - 1][1]);
    }
    return round3(length);
}
function cellKey(cell) {
    return `${cell[0]}:${cell[1]}`;
}
function sameCell(left, right) {
    return left[0] === right[0] && left[1] === right[1];
}
function manhattan(left, right) {
    return Math.abs(left[0] - right[0]) + Math.abs(left[1] - right[1]);
}
function heuristic(left, right, allowDiagonal) {
    if (!allowDiagonal)
        return manhattan(left, right);
    const dx = Math.abs(left[0] - right[0]);
    const dy = Math.abs(left[1] - right[1]);
    return Math.max(dx, dy) + (Math.SQRT2 - 1) * Math.min(dx, dy);
}
function clampInt(value, min, max) {
    return Math.max(min, Math.min(max, value));
}
function round3(value) {
    return Number(value.toFixed(3));
}
//# sourceMappingURL=Navigation.js.map