package net.minecraft.world.level.pathfinder;

import java.util.List;
import com.google.common.collect.Lists;
import java.util.Optional;
import java.util.Iterator;
import java.util.Comparator;
import com.google.common.collect.Sets;
import com.google.common.collect.ImmutableSet;
import javax.annotation.Nullable;
import java.util.stream.Collectors;
import java.util.function.Function;
import java.util.Map;
import net.minecraft.core.BlockPos;
import java.util.Set;
import net.minecraft.world.entity.Mob;
import net.minecraft.world.level.PathNavigationRegion;

public class PathFinder {
    private final Node[] neighbors;
    private final int maxVisitedNodes;
    private final NodeEvaluator nodeEvaluator;
    private final BinaryHeap openSet;
    
    public PathFinder(final NodeEvaluator cze, final int integer) {
        this.neighbors = new Node[32];
        this.openSet = new BinaryHeap();
        this.nodeEvaluator = cze;
        this.maxVisitedNodes = integer;
    }
    
    @Nullable
    public Path findPath(final PathNavigationRegion bql, final Mob aoz, final Set<BlockPos> set, final float float4, final int integer, final float float6) {
        this.openSet.clear();
        this.nodeEvaluator.prepare(bql, aoz);
        final Node czd8 = this.nodeEvaluator.getStart();
        final Map<Target, BlockPos> map9 = set.stream().collect(Collectors.toMap(fu -> this.nodeEvaluator.getGoal(fu.getX(), fu.getY(), fu.getZ()), Function.identity()));
        final Path czf10 = this.findPath(czd8, map9, float4, integer, float6);
        this.nodeEvaluator.done();
        return czf10;
    }
    
    @Nullable
    private Path findPath(final Node czd, final Map<Target, BlockPos> map, final float float3, final int integer, final float float5) {
        final Set<Target> set7 = map.keySet();
        czd.g = 0.0f;
        czd.h = this.getBestH(czd, set7);
        czd.f = czd.h;
        this.openSet.clear();
        this.openSet.insert(czd);
        final Set<Node> set8 = ImmutableSet.of();
        int integer2 = 0;
        final Set<Target> set9 = Sets.newHashSetWithExpectedSize(set7.size());
        final int integer3 = (int)(this.maxVisitedNodes * float5);
        while (!this.openSet.isEmpty() && ++integer2 < integer3) {
            final Node czd2 = this.openSet.pop();
            czd2.closed = true;
            for (final Target czj2 : set7) {
                if (czd2.distanceManhattan(czj2) <= integer) {
                    czj2.setReached();
                    set9.add(czj2);
                }
            }
            if (!set9.isEmpty()) {
                break;
            }
            if (czd2.distanceTo(czd) >= float3) {
                continue;
            }
            for (int integer4 = this.nodeEvaluator.getNeighbors(this.neighbors, czd2), integer5 = 0; integer5 < integer4; ++integer5) {
                final Node czd3 = this.neighbors[integer5];
                final float float6 = czd2.distanceTo(czd3);
                czd3.walkedDistance = czd2.walkedDistance + float6;
                final float float7 = czd2.g + float6 + czd3.costMalus;
                if (czd3.walkedDistance < float3 && (!czd3.inOpenSet() || float7 < czd3.g)) {
                    czd3.cameFrom = czd2;
                    czd3.g = float7;
                    czd3.h = this.getBestH(czd3, set7) * 1.5f;
                    if (czd3.inOpenSet()) {
                        this.openSet.changeCost(czd3, czd3.g + czd3.h);
                    }
                    else {
                        czd3.f = czd3.g + czd3.h;
                        this.openSet.insert(czd3);
                    }
                }
            }
        }
        final Optional<Path> optional12 = set9.isEmpty() ? set7.stream().<Path>map(czj -> this.reconstructPath(czj.getBestNode(), map.get(czj), false)).min(Comparator.comparingDouble(Path::getDistToTarget).thenComparingInt(Path::getSize)) : set9.stream().<Path>map(czj -> this.reconstructPath(czj.getBestNode(), map.get(czj), true)).min(Comparator.comparingInt(Path::getSize));
        if (!optional12.isPresent()) {
            return null;
        }
        final Path czf13 = optional12.get();
        return czf13;
    }
    
    private float getBestH(final Node czd, final Set<Target> set) {
        float float4 = Float.MAX_VALUE;
        for (final Target czj6 : set) {
            final float float5 = czd.distanceTo(czj6);
            czj6.updateBest(float5, czd);
            float4 = Math.min(float5, float4);
        }
        return float4;
    }
    
    private Path reconstructPath(final Node czd, final BlockPos fu, final boolean boolean3) {
        final List<Node> list5 = Lists.newArrayList();
        Node czd2 = czd;
        list5.add(0, czd2);
        while (czd2.cameFrom != null) {
            czd2 = czd2.cameFrom;
            list5.add(0, czd2);
        }
        return new Path(list5, fu, boolean3);
    }
}
