package net.minecraft.world.level.pathfinder;

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

public class PathFinder {
    private final BinaryHeap openSet;
    private final Set<Node> closedSet;
    private final Node[] neighbors;
    private final int maxVisitedNodes;
    private final NodeEvaluator nodeEvaluator;
    
    public PathFinder(final NodeEvaluator cqp, final int integer) {
        this.openSet = new BinaryHeap();
        this.closedSet = Sets.newHashSet();
        this.neighbors = new Node[32];
        this.nodeEvaluator = cqp;
        this.maxVisitedNodes = integer;
    }
    
    @Nullable
    public Path findPath(final PathNavigationRegion bkh, final Mob ala, final Set<BlockPos> set, final float float4, final int integer, final float float6) {
        this.openSet.clear();
        this.nodeEvaluator.prepare(bkh, ala);
        final Node cqo8 = this.nodeEvaluator.getStart();
        final Map<Target, BlockPos> map9 = set.stream().collect(Collectors.toMap(fk -> this.nodeEvaluator.getGoal(fk.getX(), fk.getY(), fk.getZ()), Function.identity()));
        final Path cqq10 = this.findPath(cqo8, map9, float4, integer, float6);
        this.nodeEvaluator.done();
        return cqq10;
    }
    
    @Nullable
    private Path findPath(final Node cqo, final Map<Target, BlockPos> map, final float float3, final int integer, final float float5) {
        final Set<Target> set7 = map.keySet();
        cqo.g = 0.0f;
        cqo.h = this.getBestH(cqo, set7);
        cqo.f = cqo.h;
        this.openSet.clear();
        this.closedSet.clear();
        this.openSet.insert(cqo);
        int integer2 = 0;
        final int integer3 = (int)(this.maxVisitedNodes * float5);
        while (!this.openSet.isEmpty() && ++integer2 < integer3) {
            final Node cqo2 = this.openSet.pop();
            cqo2.closed = true;
            set7.stream().filter(cqu -> cqo2.distanceManhattan(cqu) <= integer).forEach(Target::setReached);
            if (set7.stream().anyMatch(Target::isReached)) {
                break;
            }
            if (cqo2.distanceTo(cqo) >= float3) {
                continue;
            }
            for (int integer4 = this.nodeEvaluator.getNeighbors(this.neighbors, cqo2), integer5 = 0; integer5 < integer4; ++integer5) {
                final Node cqo3 = this.neighbors[integer5];
                final float float6 = cqo2.distanceTo(cqo3);
                cqo3.walkedDistance = cqo2.walkedDistance + float6;
                final float float7 = cqo2.g + float6 + cqo3.costMalus;
                if (cqo3.walkedDistance < float3 && (!cqo3.inOpenSet() || float7 < cqo3.g)) {
                    cqo3.cameFrom = cqo2;
                    cqo3.g = float7;
                    cqo3.h = this.getBestH(cqo3, set7) * 1.5f;
                    if (cqo3.inOpenSet()) {
                        this.openSet.changeCost(cqo3, cqo3.g + cqo3.h);
                    }
                    else {
                        cqo3.f = cqo3.g + cqo3.h;
                        this.openSet.insert(cqo3);
                    }
                }
            }
        }
        Stream<Path> stream10;
        if (set7.stream().anyMatch(Target::isReached)) {
            stream10 = set7.stream().filter(Target::isReached).<Path>map(cqu -> this.reconstructPath(cqu.getBestNode(), map.get(cqu), true)).sorted(Comparator.comparingInt(Path::getSize));
        }
        else {
            stream10 = set7.stream().<Path>map(cqu -> this.reconstructPath(cqu.getBestNode(), map.get(cqu), false)).sorted(Comparator.comparingDouble(Path::getDistToTarget).thenComparingInt(Path::getSize));
        }
        final Optional<Path> optional11 = stream10.findFirst();
        if (!optional11.isPresent()) {
            return null;
        }
        final Path cqq12 = optional11.get();
        return cqq12;
    }
    
    private float getBestH(final Node cqo, final Set<Target> set) {
        float float4 = Float.MAX_VALUE;
        for (final Target cqu6 : set) {
            final float float5 = cqo.distanceTo(cqu6);
            cqu6.updateBest(float5, cqo);
            float4 = Math.min(float5, float4);
        }
        return float4;
    }
    
    private Path reconstructPath(final Node cqo, final BlockPos fk, final boolean boolean3) {
        final List<Node> list5 = Lists.newArrayList();
        Node cqo2 = cqo;
        list5.add(0, cqo2);
        while (cqo2.cameFrom != null) {
            cqo2 = cqo2.cameFrom;
            list5.add(0, cqo2);
        }
        return new Path(list5, fk, boolean3);
    }
}
