// 
// Decompiled by Procyon v0.6.0
// 

package com.hypixel.hytale.server.npc.navigation;

import com.hypixel.hytale.logger.HytaleLogger;
import com.hypixel.hytale.function.function.BiToFloatFunction;
import it.unimi.dsi.fastutil.objects.ObjectIterator;
import com.hypixel.hytale.function.predicate.BiFloatPredicate;
import com.hypixel.hytale.function.function.ToFloatFunction;
import com.hypixel.hytale.math.util.MathUtil;
import com.hypixel.hytale.component.ComponentAccessor;
import com.hypixel.hytale.server.npc.movement.controllers.ProbeMoveData;
import com.hypixel.hytale.server.npc.movement.controllers.MotionController;
import com.hypixel.hytale.server.core.universe.world.storage.EntityStore;
import com.hypixel.hytale.component.Ref;
import it.unimi.dsi.fastutil.longs.Long2ObjectMaps;
import javax.annotation.Nonnull;
import it.unimi.dsi.fastutil.longs.Long2ObjectOpenHashMap;
import it.unimi.dsi.fastutil.objects.ObjectArrayList;
import javax.annotation.Nullable;
import it.unimi.dsi.fastutil.longs.Long2ObjectMap;
import java.util.List;
import com.hypixel.hytale.math.vector.Vector3d;

public class AStarBase
{
    public static final double FULL_STEP_THRESHOLD = 0.9999999;
    public static final double REQUIRED_TARGET_DISTANCE = 9.999999994736442E-8;
    public static final double HALF_STEP_THRESHOLD = 0.49999995;
    public static final double ON_GRID_THRESHOLD = 0.01;
    protected static final int INDEX_FRACTIONAL_BITS = 1;
    protected static final int POSITION_BITS = 11;
    protected static final int POSITION_OFFSET = 1024;
    protected static final int POSITION_MASK = 2047;
    protected int maxPathLength;
    protected int openNodesLimit;
    protected int totalNodesLimit;
    protected boolean canMoveDiagonal;
    protected boolean optimizedBuildPath;
    protected boolean isAvoidingBlockDamage;
    protected boolean isRelaxedMoveConstraints;
    protected final Vector3d startPosition;
    protected AStarEvaluator evaluator;
    protected double positionToIndexOffsetX;
    protected double positionToIndexOffsetY;
    protected double positionToIndexOffsetZ;
    protected long indexToPositionOffsetX;
    protected long indexToPositionOffsetY;
    protected long indexToPositionOffsetZ;
    protected long startPositionIndex;
    protected boolean is2D;
    protected boolean projectedX;
    protected boolean projectedY;
    protected boolean projectedZ;
    protected final Vector3d searchDirectionsWorldNormal;
    protected boolean searchDirectionIsDiagonalMoves;
    protected boolean searchDirectionIs2D;
    protected Vector3d[] searchDirections;
    protected double[] searchDirectionDistances;
    protected int[] inverseSearchDirections;
    protected int normalsPerDirection;
    protected int[] normalDirections;
    protected AStarNodePool nodePool;
    protected final List<AStarNode> openNodes;
    protected final Long2ObjectMap<AStarNode> visitedBlocks;
    protected int iterations;
    @Nullable
    protected AStarNode path;
    protected Progress progress;
    protected final Vector3d pathEnd;
    protected final Vector3d tempPositionVector;
    protected final Vector3d tempDirectionVector;
    
    public AStarBase() {
        this.maxPathLength = 200;
        this.openNodesLimit = 80;
        this.totalNodesLimit = 400;
        this.canMoveDiagonal = true;
        this.optimizedBuildPath = true;
        this.startPosition = new Vector3d();
        this.searchDirectionsWorldNormal = new Vector3d();
        this.openNodes = new ObjectArrayList<AStarNode>();
        this.visitedBlocks = new Long2ObjectOpenHashMap<AStarNode>();
        this.pathEnd = new Vector3d();
        this.tempPositionVector = new Vector3d();
        this.tempDirectionVector = new Vector3d();
    }
    
    public void setCanMoveDiagonal(final boolean canMoveDiagonal) {
        this.canMoveDiagonal = canMoveDiagonal;
    }
    
    public void setMaxPathLength(final int maxPathLength) {
        this.maxPathLength = maxPathLength;
    }
    
    public void setOpenNodesLimit(final int openNodesLimit) {
        this.openNodesLimit = openNodesLimit;
    }
    
    public void setTotalNodesLimit(final int totalNodesLimit) {
        this.totalNodesLimit = totalNodesLimit;
    }
    
    public void setStartPosition(@Nonnull final Vector3d position) {
        this.startPosition.assign(position);
    }
    
    @Nonnull
    public Vector3d getStartPosition() {
        return this.startPosition;
    }
    
    public void setOptimizedBuildPath(final boolean optimizedBuildPath) {
        this.optimizedBuildPath = optimizedBuildPath;
    }
    
    public AStarEvaluator getEvaluator() {
        return this.evaluator;
    }
    
    @Nonnull
    public List<AStarNode> getOpenNodes() {
        return this.openNodes;
    }
    
    public int getOpenCount() {
        return this.openNodes.size();
    }
    
    @Nonnull
    public Long2ObjectMap<AStarNode> getVisitedBlocks() {
        return this.visitedBlocks;
    }
    
    public long getStartPositionIndex() {
        return this.startPositionIndex;
    }
    
    @Nullable
    public AStarNode getPath() {
        return this.path;
    }
    
    @Nullable
    public Vector3d getPosition() {
        return (this.path != null) ? this.path.getPosition() : null;
    }
    
    public int getLength() {
        return (this.path != null) ? this.path.getLength() : 0;
    }
    
    public int getIterations() {
        return this.iterations;
    }
    
    @Nullable
    public Vector3d getEndPosition() {
        return (this.path != null) ? this.pathEnd : null;
    }
    
    public void clearPath() {
        this.path = null;
        if (!this.visitedBlocks.isEmpty()) {
            Long2ObjectMaps.fastForEach(this.visitedBlocks, nodeEntry -> this.nodePool.deallocate((AStarNode)nodeEntry.getValue()));
            this.visitedBlocks.clear();
        }
        this.openNodes.clear();
        this.setProgress(Progress.UNSTARTED);
    }
    
    public Progress initComputePath(@Nonnull final Ref<EntityStore> ref, @Nonnull final Vector3d start, final AStarEvaluator evaluator, @Nonnull final MotionController motionController, @Nonnull final ProbeMoveData probeMoveData, @Nonnull final AStarNodePoolProvider nodePoolProvider, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.clearPath();
        this.iterations = 0;
        this.evaluator = evaluator;
        this.startPosition.assign(start);
        this.isAvoidingBlockDamage = probeMoveData.isAvoidingBlockDamage;
        this.isRelaxedMoveConstraints = probeMoveData.isRelaxedMoveConstraints;
        final long startBlockX = MathUtil.fastFloor(this.startPosition.x);
        final long startBlockY = MathUtil.fastFloor(this.startPosition.y);
        final long startBlockZ = MathUtil.fastFloor(this.startPosition.z);
        final long twoX = 2L * startBlockX;
        final long twoY = 2L * startBlockY;
        final long twoZ = 2L * startBlockZ;
        final double positionToIndexOffset = -1023.25;
        this.positionToIndexOffsetX = twoX + positionToIndexOffset;
        this.positionToIndexOffsetY = twoY + positionToIndexOffset;
        this.positionToIndexOffsetZ = twoZ + positionToIndexOffset;
        this.indexToPositionOffsetX = twoX + 1L - 1024L;
        this.indexToPositionOffsetY = twoY + 1L - 1024L;
        this.indexToPositionOffsetZ = twoZ + 1L - 1024L;
        this.startPositionIndex = this.positionToIndex(this.startPosition);
        final Vector3d componentSelector = motionController.getComponentSelector();
        this.is2D = motionController.is2D();
        this.projectedX = (this.is2D && componentSelector.x == 0.0);
        this.projectedY = (this.is2D && componentSelector.y == 0.0);
        this.projectedZ = (this.is2D && componentSelector.z == 0.0);
        if (this.searchDirections == null || this.searchDirectionIs2D != this.is2D || this.searchDirectionIsDiagonalMoves != this.canMoveDiagonal || !this.searchDirectionsWorldNormal.equals(motionController.getWorldNormal())) {
            this.searchDirectionIsDiagonalMoves = this.canMoveDiagonal;
            this.searchDirectionIs2D = this.is2D;
            this.searchDirectionsWorldNormal.assign(motionController.getWorldNormal());
            final int searchDirectionCount = this.is2D ? (this.canMoveDiagonal ? 8 : 4) : (this.canMoveDiagonal ? 26 : 6);
            this.searchDirections = new Vector3d[searchDirectionCount];
            this.searchDirectionDistances = new double[searchDirectionCount];
            int directionIndex = 0;
            for (double x = -1.0; x <= 1.0; ++x) {
                if (!this.projectedX || x == 0.0) {
                    for (double y = -1.0; y <= 1.0; ++y) {
                        if (!this.projectedY || y == 0.0) {
                            for (double z = -1.0; z <= 1.0; ++z) {
                                if (!this.projectedZ || z == 0.0) {
                                    if (x != 0.0 || y != 0.0 || z != 0.0) {
                                        final Vector3d direction = new Vector3d(x, y, z);
                                        this.searchDirections[directionIndex] = direction;
                                        this.searchDirectionDistances[directionIndex] = direction.length();
                                        ++directionIndex;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            this.inverseSearchDirections = new int[searchDirectionCount];
            for (int i = 0; i < this.inverseSearchDirections.length; ++i) {
                this.inverseSearchDirections[i] = -1;
            }
            for (int i = 0; i < this.searchDirections.length - 1; ++i) {
                if (this.inverseSearchDirections[i] == -1) {
                    this.tempDirectionVector.assign(this.searchDirections[i]).negate();
                    for (int j = i + 1; j < this.searchDirections.length; ++j) {
                        if (this.searchDirections[j].equals(this.tempDirectionVector)) {
                            this.inverseSearchDirections[i] = j;
                            this.inverseSearchDirections[j] = i;
                            break;
                        }
                    }
                    if (this.inverseSearchDirections[i] == -1) {
                        throw new IllegalStateException("Can't find inverse search direction");
                    }
                }
            }
            if (this.is2D) {
                this.normalsPerDirection = 1;
                this.normalDirections = new int[this.normalsPerDirection * searchDirectionCount];
                directionIndex = 0;
                for (int i = 0; i < this.searchDirections.length; ++i) {
                    final Vector3d direction2 = this.searchDirections[i];
                    final int endIndex = directionIndex + this.normalsPerDirection;
                    Vector3d oneNormal = null;
                    for (int k = 0; k < this.searchDirections.length; ++k) {
                        final Vector3d otherDirection = this.searchDirections[k];
                        if (i != k && direction2.dot(otherDirection) == 0.0) {
                            if (oneNormal == null || oneNormal.dot(otherDirection) == 0.0) {
                                this.normalDirections[directionIndex++] = k;
                                if (directionIndex == endIndex) {
                                    break;
                                }
                                oneNormal = otherDirection;
                            }
                        }
                    }
                    if (directionIndex != endIndex) {
                        throw new IllegalStateException("Can't find correct number of normals");
                    }
                }
            }
            this.nodePool = nodePoolProvider.getPool(searchDirectionCount);
        }
        probeMoveData.setSaveSegments(false);
        this.tempPositionVector.assign(this.projectedX ? start.x : (startBlockX + 0.5), this.projectedY ? start.y : (startBlockY + 0.5), this.projectedZ ? start.z : (startBlockZ + 0.5));
        Vector3d position = this.canAdvance(ref, this.startPosition, this.tempPositionVector, motionController, probeMoveData, componentAccessor);
        if (position != null) {
            this.addStartNode(this.startPosition, position, motionController);
            probeMoveData.setSaveSegments(true);
            return this.setProgress(Progress.COMPUTING);
        }
        this.tempPositionVector.x = (this.projectedX ? start.x : (MathUtil.fastFloor(2.0 * start.x) / 2.0));
        this.tempPositionVector.y = (this.projectedY ? start.y : (MathUtil.fastFloor(2.0 * start.y) / 2.0));
        this.tempPositionVector.z = (this.projectedZ ? start.z : (MathUtil.fastFloor(2.0 * start.z) / 2.0));
        for (double x2 = this.projectedX ? 0.0 : 0.5; x2 >= 0.0; x2 -= 0.5) {
            for (double y2 = this.projectedY ? 0.0 : 0.5; y2 >= 0.0; y2 -= 0.5) {
                for (double z2 = this.projectedZ ? 0.0 : 0.5; z2 >= 0.0; z2 -= 0.5) {
                    this.tempDirectionVector.assign(x2, y2, z2).add(this.tempPositionVector);
                    position = this.canAdvance(ref, this.startPosition, this.tempDirectionVector, motionController, probeMoveData, componentAccessor);
                    if (position != null) {
                        this.addStartNode(this.startPosition, position, motionController);
                    }
                }
            }
        }
        if (this.openNodes.isEmpty()) {
            final double startX = this.tempPositionVector.x + (this.projectedX ? 0 : -1);
            final double endX = this.tempPositionVector.x + (this.projectedX ? 0 : 1);
            final double startY = this.tempPositionVector.y + (this.projectedY ? 0 : -1);
            final double endY = this.tempPositionVector.y + (this.projectedY ? 0 : 1);
            final double startZ = this.tempPositionVector.z + (this.projectedZ ? 0 : -1);
            final double endZ = this.tempPositionVector.z + (this.projectedZ ? 0 : 1);
            for (double x3 = startX; x3 <= endX; x3 += 0.5) {
                for (double y3 = startY; y3 <= endY; y3 += 0.5) {
                    for (double z3 = startZ; z3 <= endZ; z3 += 0.5) {
                        this.tempDirectionVector.assign(x3, y3, z3);
                        position = this.canAdvance(ref, this.startPosition, this.tempDirectionVector, motionController, probeMoveData, componentAccessor);
                        if (position != null) {
                            this.addStartNode(this.startPosition, position, motionController);
                        }
                    }
                }
            }
        }
        probeMoveData.setSaveSegments(true);
        return this.setProgress(this.openNodes.isEmpty() ? Progress.ABORTED : Progress.COMPUTING);
    }
    
    public Progress computePath(@Nonnull final Ref<EntityStore> ref, @Nonnull final MotionController motionController, @Nonnull final ProbeMoveData probeMoveData, int nodesToProcess, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        if (this.progress != Progress.COMPUTING) {
            return this.progress;
        }
        probeMoveData.isAvoidingBlockDamage = this.isAvoidingBlockDamage;
        probeMoveData.isRelaxedMoveConstraints = this.isRelaxedMoveConstraints;
        while (!this.openNodes.isEmpty() && nodesToProcess-- > 0) {
            final int idx = this.openNodes.size() - 1;
            final AStarNode node = this.openNodes.get(idx);
            node.close();
            if (this.evaluator.isGoalReached(ref, this, node, motionController, componentAccessor)) {
                this.buildPath(node);
                return this.setProgress(Progress.ACCOMPLISHED);
            }
            this.openNodes.remove(idx);
            ++this.iterations;
            if (node.getLength() >= this.maxPathLength) {
                continue;
            }
            final Vector3d nodePosition = node.getPosition();
            final AStarNode[] successors = node.getSuccessors();
        Label_0492:
            for (int searchDirectionCount = this.searchDirections.length, directionIndex = 0; directionIndex < searchDirectionCount; ++directionIndex) {
                if (successors[directionIndex] == null) {
                    final double directionLength = this.searchDirectionDistances[directionIndex];
                    probeMoveData.setPosition(nodePosition).setDirection(this.searchDirections[directionIndex]);
                    final double distance = motionController.probeMove(ref, probeMoveData, componentAccessor);
                    final double halfThreshold = directionLength * 0.49999995;
                    if (distance >= halfThreshold) {
                        final double halfDistance = directionLength * 0.5;
                        probeMoveData.computePosition(halfDistance, probeMoveData.targetPosition);
                        final long halfPositionIndex = this.positionToIndex(probeMoveData.targetPosition);
                        if (halfPositionIndex != -1L) {
                            final AStarNode otherNode = this.visitedBlocks.get(halfPositionIndex);
                            if (otherNode != null) {
                                this.updateNode(node, directionIndex, otherNode, motionController);
                            }
                            else {
                                if (this.is2D) {
                                    int normalIndex;
                                    for (int firstNormalIndex = normalIndex = directionIndex * this.normalsPerDirection; normalIndex < firstNormalIndex + this.normalsPerDirection; ++normalIndex) {
                                        final Vector3d normal = this.searchDirections[this.normalDirections[normalIndex]];
                                        final long diagonalVertexIndex = this.addOffsetToIndex(halfPositionIndex, (long)normal.x, (long)normal.y, (long)normal.z);
                                        final AStarNode diagonalNode = this.visitedBlocks.get(diagonalVertexIndex);
                                        if (diagonalNode != null) {
                                            final AStarNode otherDiagonalNode = diagonalNode.getSuccessor(this.inverseSearchDirections[normalIndex]);
                                            if (otherDiagonalNode != null) {
                                                continue Label_0492;
                                            }
                                        }
                                    }
                                }
                                final Vector3d destination = (distance >= directionLength * 0.9999999) ? probeMoveData.probePosition : probeMoveData.targetPosition;
                                this.addOrUpdateNode(node, directionIndex, destination, motionController, componentAccessor);
                            }
                        }
                    }
                }
            }
            if (this.openNodesLimit > 0 && this.openNodes.size() >= this.openNodesLimit) {
                return this.setProgress(Progress.TERMINATED_OPEN_NODE_LIMIT_EXCEEDED);
            }
            if (this.totalNodesLimit > 0 && this.visitedBlocks.size() >= this.totalNodesLimit) {
                return this.setProgress(Progress.TERMINATED_TOTAL_NODE_LIMIT_EXCEEDED);
            }
        }
        return this.setProgress(this.openNodes.isEmpty() ? Progress.TERMINATED : Progress.COMPUTING);
    }
    
    public Progress getProgress() {
        return this.progress;
    }
    
    public boolean isComputing() {
        return this.progress == Progress.COMPUTING;
    }
    
    public float buildLongestPath() {
        final AStarNode node = this.buildBestPath(AStarNode::getTravelCost, (oldV, v) -> v > oldV, 0.0f);
        return (node == null) ? 0.0f : node.getTravelCost();
    }
    
    public float buildFurthestPath() {
        final AStarNode node = this.buildBestPath(n -> (float)n.getPosition().distanceSquaredTo(this.startPosition), (oldV, v) -> v > oldV, 0.0f);
        return (node == null) ? 0.0f : node.getTravelCost();
    }
    
    @Nullable
    public AStarNode buildBestPath(@Nonnull final ToFloatFunction<AStarNode> weight, @Nonnull final BiFloatPredicate predicate, final float initialValue) {
        if (this.path != null) {
            return null;
        }
        final AStarNode node = this.findBestVisitedNode(weight, predicate, initialValue);
        if (node == null) {
            return null;
        }
        this.buildPath(node);
        return node;
    }
    
    @Nullable
    public AStarNode findBestVisitedNode(@Nonnull final ToFloatFunction<AStarNode> weight, @Nonnull final BiFloatPredicate predicate, final float initialValue) {
        if (this.visitedBlocks.isEmpty()) {
            return null;
        }
        final ObjectIterator<Long2ObjectMap.Entry<AStarNode>> iterator = Long2ObjectMaps.fastIterator(this.visitedBlocks);
        float value = initialValue;
        AStarNode resultNode = null;
        while (iterator.hasNext()) {
            final AStarNode node = (AStarNode)iterator.next().getValue();
            final float v = weight.applyAsFloat(node);
            if (!predicate.test(value, v)) {
                continue;
            }
            value = v;
            resultNode = node;
        }
        return resultNode;
    }
    
    @Nullable
    public <T> AStarNode buildBestPath(@Nonnull final BiToFloatFunction<AStarNode, T> weight, @Nonnull final BiFloatPredicate predicate, final float initialValue, final T obj) {
        if (this.path != null) {
            return null;
        }
        final AStarNode node = this.findBestVisitedNode((BiToFloatFunction<AStarNode, Object>)weight, predicate, initialValue, obj);
        if (node == null) {
            return null;
        }
        this.buildPath(node);
        return node;
    }
    
    @Nullable
    public <T> AStarNode findBestVisitedNode(@Nonnull final BiToFloatFunction<AStarNode, T> weight, @Nonnull final BiFloatPredicate predicate, final float initialValue, final T obj) {
        if (this.visitedBlocks.isEmpty()) {
            return null;
        }
        final ObjectIterator<Long2ObjectMap.Entry<AStarNode>> iterator = Long2ObjectMaps.fastIterator(this.visitedBlocks);
        float value = initialValue;
        AStarNode resultNode = null;
        while (iterator.hasNext()) {
            final AStarNode node = (AStarNode)iterator.next().getValue();
            final float v = weight.applyAsFloat(node, obj);
            if (!predicate.test(value, v)) {
                continue;
            }
            value = v;
            resultNode = node;
        }
        return resultNode;
    }
    
    @Nonnull
    public AStarDebugBase createDebugHelper(@Nonnull final HytaleLogger logger) {
        return new AStarDebugBase(this, logger);
    }
    
    public static long indexFromXYZ(final long dx, final long dy, final long dz) {
        if (dx < 0L || dx > 2047L || dy < 0L || dy > 2047L || dz < 0L || dz > 2047L) {
            return -1L;
        }
        return (dx << 22) + (dy << 11) + dz;
    }
    
    public static int zFromIndex(final long index) {
        return (int)(index & 0x7FFL);
    }
    
    public static int yFromIndex(final long index) {
        return zFromIndex(index >> 11);
    }
    
    public static int xFromIndex(final long index) {
        return zFromIndex(index >> 22);
    }
    
    @Nonnull
    public static String positionIndexToString(final long index) {
        final double x = (xFromIndex(index) - 1024) * 0.5 + 0.5;
        final double y = (yFromIndex(index) - 1024) * 0.5 + 0.5;
        final double z = (zFromIndex(index) - 1024) * 0.5 + 0.5;
        return "[" + x + "/" + y + "/" + z;
    }
    
    protected Progress setProgress(final Progress progress) {
        return this.progress = progress;
    }
    
    @Nullable
    protected Vector3d canAdvance(@Nonnull final Ref<EntityStore> ref, @Nonnull final Vector3d startPosition, @Nonnull final Vector3d destination, @Nonnull final MotionController motionController, @Nonnull final ProbeMoveData probeMoveData, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        probeMoveData.setPosition(startPosition).setTargetPosition(destination);
        return probeMoveData.canAdvance(ref, motionController, 0.9999999, componentAccessor) ? probeMoveData.probePosition : null;
    }
    
    protected void addStartNode(final Vector3d startPosition, @Nonnull final Vector3d position, @Nonnull final MotionController motionController) {
        final long positionIndex = this.positionToIndex(position);
        final float cost = this.measureWalkCost(startPosition, position, motionController);
        final AStarNode node = this.nodePool.allocate().initAsStartNode(position, positionIndex, cost, this.evaluator.estimateToGoal(this, position, motionController));
        this.addOpenNode(node, positionIndex);
    }
    
    protected void addOpenNode(@Nonnull final AStarNode parentNode, final int directionIndex, @Nonnull final Vector3d position, final long positionIndex, final float cost, final MotionController motionController) {
        final int inverseSearchDirection = this.inverseSearchDirections[directionIndex];
        final AStarNode node = this.nodePool.allocate().initWithPredecessor(parentNode, directionIndex, position, positionIndex, inverseSearchDirection, cost, this.evaluator.estimateToGoal(this, position, motionController));
        this.addOpenNode(node, positionIndex);
    }
    
    protected void addOpenNode(@Nonnull final AStarNode node, final long index) {
        int predecessor = this.openNodes.size() - 1;
        for (float totalCost = node.getTotalCost(); predecessor >= 0 && this.openNodes.get(predecessor).getTotalCost() < totalCost; --predecessor) {}
        this.openNodes.add(predecessor + 1, node);
        this.visitedBlocks.put(index, node);
    }
    
    protected void updateNode(@Nonnull final AStarNode node, final int directionIndex, @Nonnull final AStarNode targetNode, @Nonnull final MotionController motionController) {
        if (targetNode.isInvalid()) {
            return;
        }
        final float stepCost = this.measureWalkCost(node.getPosition(), targetNode.getPosition(), motionController);
        this.updateNodeCost(node, directionIndex, targetNode, stepCost);
    }
    
    protected void addOrUpdateNode(@Nonnull final AStarNode node, final int directionIndex, @Nonnull final Vector3d position, @Nonnull final MotionController motionController, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final long positionIndex = this.positionToIndex(position);
        if (positionIndex == -1L || positionIndex == node.positionIndex) {
            return;
        }
        final AStarNode targetNode = this.visitedBlocks.get(positionIndex);
        if (targetNode == null) {
            if (!motionController.isValidPosition(position, componentAccessor)) {
                this.visitedBlocks.put(positionIndex, this.nodePool.allocate().initAsInvalid(position, this.positionToIndex(position)));
                return;
            }
            final float travelCostToNode = node.getTravelCost() + this.measureWalkCost(node.getPosition(), position, motionController);
            this.addOpenNode(node, directionIndex, position, positionIndex, travelCostToNode, motionController);
        }
        else {
            if (targetNode.isInvalid()) {
                return;
            }
            final float stepCost = this.measureWalkCost(node.getPosition(), position, motionController);
            this.updateNodeCost(node, directionIndex, targetNode, stepCost);
        }
    }
    
    protected void updateNodeCost(@Nonnull final AStarNode node, final int directionIndex, @Nonnull final AStarNode targetNode, final float stepCost) {
        final float travelCostToNode = node.getTravelCost() + stepCost;
        final float delta = travelCostToNode - targetNode.getTravelCost();
        if (delta < 0.0f) {
            targetNode.adjustOptimalPath(node, delta, directionIndex);
            node.setSuccessor(directionIndex, targetNode, this.inverseSearchDirections[directionIndex], stepCost);
        }
        else {
            node.successors[directionIndex] = AStarNode.ENTRY_NODE_TAG;
            targetNode.successors[this.inverseSearchDirections[directionIndex]] = AStarNode.ENTRY_NODE_TAG;
        }
    }
    
    protected long positionToIndex(@Nonnull final Vector3d position) {
        final long dx = MathUtil.fastFloor(position.x * 2.0 - this.positionToIndexOffsetX);
        final long dy = MathUtil.fastFloor(position.y * 2.0 - this.positionToIndexOffsetY);
        final long dz = MathUtil.fastFloor(position.z * 2.0 - this.positionToIndexOffsetZ);
        return indexFromXYZ(dx, dy, dz);
    }
    
    protected float measureWalkCost(final Vector3d fromPosition, final Vector3d toPosition, @Nonnull final MotionController motionController) {
        return (float)motionController.waypointDistance(fromPosition, toPosition);
    }
    
    protected void buildPath(@Nullable AStarNode endNode) {
        if (endNode == null) {
            this.path = null;
            return;
        }
        this.pathEnd.assign(endNode.getPosition());
        if (this.optimizedBuildPath) {
            AStarNode node = endNode;
            int length = 1;
            int lastDirection = -1;
            AStarNode lastCorner = null;
            while (node != null) {
                node.setNextNode(lastCorner, length);
                if (node.getPredecessorDirection() != lastDirection) {
                    lastDirection = node.getPredecessorDirection();
                    lastCorner = node;
                    ++length;
                }
                this.path = node;
                node = node.getPredecessor();
            }
            return;
        }
        int length2 = 0;
        AStarNode nextNode = null;
        while (endNode != null) {
            endNode.setNextNode(nextNode, ++length2);
            this.path = endNode;
            nextNode = endNode;
            endNode = endNode.getPredecessor();
        }
    }
    
    protected long addOffsetToIndex(final long index, final long xSteps, final long ySteps, final long zSteps) {
        final long x = xFromIndex(index) + xSteps;
        final long y = yFromIndex(index) + ySteps;
        final long z = zFromIndex(index) + zSteps;
        return indexFromXYZ(x, y, z);
    }
    
    public enum Progress
    {
        UNSTARTED, 
        ABORTED, 
        COMPUTING, 
        ACCOMPLISHED, 
        TERMINATED, 
        TERMINATED_OPEN_NODE_LIMIT_EXCEEDED, 
        TERMINATED_TOTAL_NODE_LIMIT_EXCEEDED;
    }
}