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

package com.hypixel.hytale.server.npc.corecomponents.movement;

import java.util.function.Supplier;
import com.hypixel.hytale.server.npc.navigation.AStarNode;
import com.hypixel.hytale.server.npc.navigation.IWaypoint;
import com.hypixel.hytale.math.random.RandomExtra;
import com.hypixel.hytale.math.util.MathUtil;
import com.hypixel.hytale.server.npc.movement.NavState;
import com.hypixel.hytale.math.util.TrigMathUtil;
import com.hypixel.hytale.server.npc.movement.steeringforces.SteeringForceWithTarget;
import com.hypixel.hytale.math.vector.Vector3d;
import java.util.logging.Level;
import com.hypixel.hytale.server.npc.NPCPlugin;
import com.hypixel.hytale.server.npc.movement.Steering;
import javax.annotation.Nullable;
import com.hypixel.hytale.server.npc.sensorinfo.InfoProvider;
import com.hypixel.hytale.server.npc.movement.controllers.MotionController;
import com.hypixel.hytale.server.npc.role.RoleDebugFlags;
import com.hypixel.hytale.server.npc.navigation.AStarNodePoolProviderSimple;
import com.hypixel.hytale.server.core.modules.entity.component.TransformComponent;
import com.hypixel.hytale.component.ComponentAccessor;
import com.hypixel.hytale.server.npc.role.Role;
import com.hypixel.hytale.server.core.universe.world.storage.EntityStore;
import com.hypixel.hytale.component.Ref;
import java.util.EnumSet;
import com.hypixel.hytale.logger.HytaleLogger;
import com.hypixel.hytale.server.npc.corecomponents.builders.BuilderBodyMotionBase;
import com.hypixel.hytale.server.npc.asset.builder.BuilderSupport;
import com.hypixel.hytale.server.npc.corecomponents.movement.builders.BuilderBodyMotionFindBase;
import com.hypixel.hytale.server.npc.navigation.AStarNodePoolProvider;
import com.hypixel.hytale.server.npc.movement.controllers.ProbeMoveData;
import com.hypixel.hytale.server.npc.navigation.PathFollower;
import com.hypixel.hytale.server.npc.navigation.AStarDebugBase;
import javax.annotation.Nonnull;
import com.hypixel.hytale.server.npc.navigation.AStarEvaluator;
import com.hypixel.hytale.server.npc.corecomponents.BodyMotionBase;
import com.hypixel.hytale.server.npc.navigation.AStarBase;

public abstract class BodyMotionFindBase<T extends AStarBase> extends BodyMotionBase implements AStarEvaluator
{
    protected final int nodesPerTick;
    protected final boolean useBestPath;
    protected final double throttleDelayMin;
    protected final double throttleDelayMax;
    protected final int throttleIgnoreCount;
    protected final boolean useSteering;
    protected final boolean usePathfinder;
    protected final boolean skipSteering;
    protected final double minPathLength;
    protected final double minPathLengthSquared;
    protected final boolean canSkipSteering;
    protected final boolean isAvoidingBlockDamage;
    protected final boolean isRelaxedMoveConstraints;
    protected final double desiredAltitudeWeight;
    protected final boolean dbgStatus;
    protected final boolean dbgProfile;
    protected final boolean dbgMaps;
    protected final boolean dbgOpens;
    protected final boolean dbgPath;
    protected final boolean dbgRebuild;
    protected final boolean dbgNodes;
    protected final boolean dbgStay;
    protected final boolean dbgMotionState;
    @Nonnull
    protected final T aStar;
    @Nonnull
    protected final AStarDebugBase aStarDebug;
    protected final PathFollower pathFollower;
    protected final ProbeMoveData probeMoveData;
    protected AStarNodePoolProvider sharedNodePoolProvider;
    protected int throttleCount;
    protected double throttleTime;
    protected double targetDeltaSquared;
    protected boolean wasSteering;
    protected double throttleDelay;
    protected boolean passedWaypoint;
    protected boolean wasAvoidingBlockDamage;
    protected boolean dbgDisplayString;
    protected StringBuilder debugString;
    
    public BodyMotionFindBase(@Nonnull final BuilderBodyMotionFindBase builderBodyMotionFindBase, @Nonnull final BuilderSupport support, @Nonnull final T aStar) {
        super(builderBodyMotionFindBase);
        this.pathFollower = new PathFollower();
        this.probeMoveData = new ProbeMoveData();
        this.aStar = aStar;
        this.aStarDebug = aStar.createDebugHelper(HytaleLogger.forEnclosingClass());
        this.useBestPath = builderBodyMotionFindBase.getUseBestPath(support);
        this.nodesPerTick = builderBodyMotionFindBase.getNodesPerTick(support);
        this.usePathfinder = builderBodyMotionFindBase.isUsePathfinder(support);
        this.useSteering = builderBodyMotionFindBase.isUseSteering(support);
        this.skipSteering = builderBodyMotionFindBase.isSkipSteering(support);
        this.canSkipSteering = (this.skipSteering || this.usePathfinder);
        this.minPathLength = builderBodyMotionFindBase.getMinPathLength(support);
        this.minPathLengthSquared = this.minPathLength * this.minPathLength;
        final double[] throttleDelayRange = builderBodyMotionFindBase.getThrottleDelayRange(support);
        this.throttleDelayMin = throttleDelayRange[0];
        this.throttleDelayMax = throttleDelayRange[1];
        this.throttleIgnoreCount = builderBodyMotionFindBase.getThrottleIgnoreCount(support);
        this.isRelaxedMoveConstraints = builderBodyMotionFindBase.isRelaxedMoveConstraints(support);
        this.isAvoidingBlockDamage = builderBodyMotionFindBase.isAvoidingBlockDamage(support);
        this.desiredAltitudeWeight = builderBodyMotionFindBase.getDesiredAltitudeWeight(support);
        this.probeMoveData.setRelaxedMoveConstraints(this.isRelaxedMoveConstraints);
        this.pathFollower.setPathSmoothing(builderBodyMotionFindBase.getPathSmoothing(support));
        this.pathFollower.setRelativeSpeed(builderBodyMotionFindBase.getRelativeSpeed(support));
        this.pathFollower.setRelativeSpeedWaypoint(builderBodyMotionFindBase.getRelativeSpeedWaypoint(support));
        this.pathFollower.setWaypointRadius(builderBodyMotionFindBase.getWaypointRadius(support));
        this.pathFollower.setRejectionWeight(builderBodyMotionFindBase.getRejectionWeight(support));
        this.pathFollower.setBlendHeading(builderBodyMotionFindBase.getBlendHeading(support));
        this.aStar.setMaxPathLength(builderBodyMotionFindBase.getMaxPathLength(support));
        this.aStar.setOpenNodesLimit(builderBodyMotionFindBase.getMaxOpenNodes(support));
        this.aStar.setTotalNodesLimit(builderBodyMotionFindBase.getMaxTotalNodes(support));
        this.aStar.setCanMoveDiagonal(builderBodyMotionFindBase.isDiagonalMoves(support));
        this.aStar.setOptimizedBuildPath(builderBodyMotionFindBase.isBuildOptimisedPath(support));
        final EnumSet<DebugFlags> debugFlags = builderBodyMotionFindBase.getParsedDebugFlags();
        this.dbgRebuild = debugFlags.contains(DebugFlags.Rebuild);
        this.dbgNodes = debugFlags.contains(DebugFlags.Nodes);
        this.dbgProfile = debugFlags.contains(DebugFlags.Profile);
        this.dbgPath = debugFlags.contains(DebugFlags.Path);
        this.dbgOpens = debugFlags.contains(DebugFlags.Opens);
        this.dbgMaps = debugFlags.contains(DebugFlags.Maps);
        this.dbgStatus = debugFlags.contains(DebugFlags.Status);
        this.dbgStay = debugFlags.contains(DebugFlags.Stay);
        this.dbgMotionState = debugFlags.contains(DebugFlags.Motion);
        this.dbgDisplayString = false;
        this.pathFollower.setDebugNodes(this.dbgNodes);
    }
    
    @Override
    public void activate(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
        assert transformComponent != null;
        final MotionController activeMotionController = role.getActiveMotionController();
        this.sharedNodePoolProvider = componentAccessor.getResource(AStarNodePoolProviderSimple.getResourceType());
        this.dbgDisplayString = role.getDebugSupport().getDebugFlags().contains(RoleDebugFlags.Pathfinder);
        this.setNavStateInit(activeMotionController);
        this.wasSteering = false;
        this.wasAvoidingBlockDamage = (this.isAvoidingBlockDamage && activeMotionController.isAvoidingBlockDamage());
        this.aStar.setStartPosition(transformComponent.getPosition());
    }
    
    @Override
    public void deactivate(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.pathFollower.clearPath();
        this.aStar.clearPath();
    }
    
    @Override
    public boolean computeSteering(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nullable final InfoProvider infoProvider, final double dt, @Nonnull final Steering desiredSteering, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        desiredSteering.clear();
        final MotionController activeMotionController = role.getActiveMotionController();
        final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
        assert transformComponent != null;
        final Vector3d position = transformComponent.getPosition();
        if (!this.canComputeMotion(ref, role, infoProvider, componentAccessor)) {
            this.setNavStateAborted(activeMotionController);
            return this.wasSteering = false;
        }
        if (!this.isAvoidingBlockDamage) {
            activeMotionController.setAvoidingBlockDamage(false);
        }
        activeMotionController.setRelaxedMoveConstraints(this.isRelaxedMoveConstraints);
        this.probeMoveData.setAvoidingBlockDamage(activeMotionController.isAvoidingBlockDamage());
        if (this.isGoalReached(ref, activeMotionController, position, componentAccessor)) {
            this.setNavStateAtGoal(role.getActiveMotionController());
            this.wasSteering = false;
            if (this.dbgMotionState) {
                NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: At Goal");
            }
            return false;
        }
        if (this.throttleCount > this.throttleIgnoreCount) {
            this.throttleTime += dt;
        }
        this.throttleDelay -= dt;
        if (this.throttleDelay > 0.0) {
            this.wasSteering = false;
            if (!this.mustAbortThrottling(activeMotionController, ref)) {
                this.onThrottling(activeMotionController, ref, desiredSteering, componentAccessor);
                this.setNavStateThrottling(activeMotionController);
                if (this.dbgMotionState) {
                    NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Delaying");
                }
                return true;
            }
            this.resetThrottleCount();
        }
        final boolean unobstructed = !activeMotionController.isObstructed();
        if (this.passedWaypoint && this.pathFollower.getCurrentWaypoint() != null && this.useSteering && unobstructed) {
            if (this.canSwitchToSteering(ref, activeMotionController, componentAccessor)) {
                if (this.dbgMotionState) {
                    NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Switch to steering");
                }
                this.forceRecomputePath(activeMotionController);
                this.wasSteering = true;
            }
            this.passedWaypoint = false;
        }
        if (this.pathFollower.getCurrentWaypoint() == null && !this.aStar.isComputing() && this.useSteering) {
            if (unobstructed && (this.wasSteering || !this.canSkipSteering || !this.shouldSkipSteering(ref, activeMotionController, position, componentAccessor)) && this.computeSteering(ref, role, position, desiredSteering, componentAccessor)) {
                this.setNavStateSteering(role.getActiveMotionController());
                this.onSteering(activeMotionController, ref, componentAccessor);
                this.wasSteering = true;
                if (this.dbgMotionState) {
                    NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Steering");
                }
                return true;
            }
            if (!this.usePathfinder) {
                this.setNavStateBlocked(role.getActiveMotionController());
                this.wasSteering = false;
                if (this.dbgMotionState) {
                    NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Blocked");
                }
                return false;
            }
            if (this.dbgMotionState) {
                NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: End of steering - Fall through path finder");
            }
            this.forceRecomputePath(activeMotionController);
        }
        this.wasSteering = false;
        final boolean mustRecomputePath = this.mustRecomputePath(activeMotionController);
        final boolean forceRecomputePath = activeMotionController.isForceRecomputePath();
        if (mustRecomputePath || forceRecomputePath) {
            if (this.dbgMotionState) {
                NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Trigger Recomputing path reason mustRecompute=%s forceRecompute=%s", mustRecomputePath, forceRecomputePath);
            }
            this.forceRecomputePath(activeMotionController);
        }
        if (this.pathFollower.getCurrentWaypoint() != null) {
            this.updatePathFollower(ref, position, activeMotionController, componentAccessor);
            if (this.dbgMotionState) {
                NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Updating path follower");
            }
        }
        if (this.pathFollower.pathInFinalStage() && activeMotionController.canAct(ref, componentAccessor)) {
            if (this.pathFollower.getCurrentWaypoint() == null) {
                if (this.aStar.getPath() != null) {
                    this.setPath(ref, position, activeMotionController, componentAccessor);
                    if (this.dbgMotionState) {
                        NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: End of path - Precomputed exists");
                    }
                }
                else if (!this.aStar.isComputing()) {
                    if (unobstructed && (!this.canSkipSteering || !this.shouldSkipSteering(ref, activeMotionController, position, componentAccessor))) {
                        this.setNavStateSteering(activeMotionController);
                        if (this.dbgMotionState) {
                            NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: End of path - Switch back to steering");
                        }
                        return false;
                    }
                    if (this.shouldDeferPathComputation(activeMotionController, position, componentAccessor)) {
                        this.onDeferring(activeMotionController, ref, desiredSteering, componentAccessor);
                        this.setNavStateDeferred(activeMotionController);
                        if (this.dbgMotionState) {
                            NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: End of path - Deferring computations");
                        }
                        return true;
                    }
                    if (!this.startPathFinder(ref, position, role, activeMotionController, componentAccessor)) {
                        this.onNoPathFound(activeMotionController);
                        this.setNavStateAborted(activeMotionController);
                        if (this.dbgMotionState) {
                            NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: End of path - Start path finder failed");
                        }
                        return false;
                    }
                    if (this.dbgMotionState) {
                        NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: End of path - Restarted path finder");
                    }
                }
            }
            else if (this.pathFollower.isWaypointFrozen()) {
                this.aStar.clearPath();
                final Vector3d targetPosition = this.pathFollower.getCurrentWaypointPosition();
                if (targetPosition.distanceSquaredTo(position) < 1.0) {
                    this.pathFollower.setWaypointFrozen(false);
                    if (this.canSkipSteering && this.shouldSkipSteering(ref, activeMotionController, targetPosition, componentAccessor)) {
                        this.startPathFinder(ref, position, role, activeMotionController, componentAccessor);
                        if (this.dbgMotionState) {
                            NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Early start path computation");
                        }
                    }
                    else if (this.dbgMotionState) {
                        NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Skipped early start path computation");
                    }
                }
            }
        }
        if (this.aStar.isComputing() && this.continuePathFinder(ref, activeMotionController, componentAccessor)) {
            if (this.pathFollower.getCurrentWaypoint() == null) {
                this.setNavStateComputing(activeMotionController);
                if (this.dbgMotionState) {
                    NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Computing - no follower");
                }
                return true;
            }
            if (this.dbgMotionState) {
                NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Computing");
            }
        }
        if (this.pathFollower.getCurrentWaypoint() == null) {
            if (this.aStar.getPath() == null) {
                this.setNavStateThrottling(activeMotionController);
                if (this.dbgMotionState) {
                    NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: No path - throttling");
                }
                return false;
            }
            this.setPath(ref, position, activeMotionController, componentAccessor);
            if (this.dbgMotionState) {
                NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: No path - setting new path");
            }
        }
        if (activeMotionController.canAct(ref, componentAccessor) && !this.dbgStay) {
            this.pathFollower.executePath(position, activeMotionController, desiredSteering);
            if (this.dbgMotionState) {
                NPCPlugin.get().getLogger().at(Level.INFO).every(100).log("MotionFindBase: Executing path");
            }
        }
        this.setNavStateFollowing(activeMotionController);
        return true;
    }
    
    public abstract void findBestPath(final AStarBase p0, final MotionController p1);
    
    protected boolean startPathFinder(@Nonnull final Ref<EntityStore> ref, @Nonnull final Vector3d position, final Role role, @Nonnull final MotionController activeMotionController, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        if (this.dbgProfile) {
            long time = System.nanoTime();
            AStarBase.Progress progress = this.startComputePath(ref, role, activeMotionController, position, componentAccessor);
            if (progress == AStarBase.Progress.COMPUTING) {
                progress = this.aStar.computePath(ref, activeMotionController, this.probeMoveData, Integer.MAX_VALUE, componentAccessor);
            }
            time = System.nanoTime() - time;
            NPCPlugin.get().getLogger().at(Level.INFO).log("Path computation profile %s in %s", progress.toString(), time / 1000L);
            if (progress != AStarBase.Progress.ACCOMPLISHED) {
                return false;
            }
            if (this.dbgPath) {
                this.aStarDebug.dumpPath();
                this.aStarDebug.dumpMap(true, activeMotionController);
            }
        }
        else {
            final AStarBase.Progress progress2 = this.startComputePath(ref, role, activeMotionController, position, componentAccessor);
            if (progress2 != AStarBase.Progress.COMPUTING) {
                if (this.dbgStatus) {
                    NPCPlugin.get().getLogger().at(Level.INFO).log("Path computation start failed %s", progress2.toString());
                }
                return false;
            }
        }
        return true;
    }
    
    protected boolean continuePathFinder(@Nonnull final Ref<EntityStore> ref, @Nonnull final MotionController activeMotionController, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        AStarBase.Progress progress = this.aStar.computePath(ref, activeMotionController, this.probeMoveData, this.nodesPerTick, componentAccessor);
        if (progress == AStarBase.Progress.COMPUTING) {
            this.setNavStateComputing(activeMotionController);
            if (this.dbgOpens) {
                this.aStarDebug.dumpOpens(activeMotionController);
            }
            if (this.dbgMaps) {
                this.aStarDebug.dumpMap(true, activeMotionController);
            }
            return true;
        }
        if (progress == AStarBase.Progress.ACCOMPLISHED) {
            this.resetThrottleCount();
        }
        else if (this.useBestPath) {
            this.findBestPath(this.aStar, activeMotionController);
            ++this.throttleCount;
            if (this.aStar.getPath() != null) {
                progress = AStarBase.Progress.ACCOMPLISHED;
            }
        }
        if (progress != AStarBase.Progress.ACCOMPLISHED) {
            if (this.dbgStatus) {
                NPCPlugin.get().getLogger().at(Level.INFO).log("Path computation failed %s", progress.toString());
            }
            this.aStar.clearPath();
            this.onNoPathFound(activeMotionController);
            return false;
        }
        final double pathLengthSquared = this.aStar.getEndPosition().distanceSquaredTo(this.aStar.getPosition());
        if (pathLengthSquared < this.minPathLengthSquared) {
            if (this.dbgStatus) {
                NPCPlugin.get().getLogger().at(Level.INFO).log("Path computation failed. Path to short length=%s", Math.sqrt(pathLengthSquared));
            }
            this.aStar.clearPath();
            this.onNoPathFound(activeMotionController);
            return false;
        }
        if (this.dbgPath) {
            this.aStarDebug.dumpPath();
        }
        if (this.dbgPath || this.dbgMaps) {
            this.aStarDebug.dumpMap(true, activeMotionController);
        }
        return false;
    }
    
    protected boolean updatePathFollower(@Nonnull final Ref<EntityStore> ref, @Nonnull final Vector3d position, @Nonnull final MotionController activeMotionController, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        if (!this.pathFollower.updateCurrentTarget(position, activeMotionController)) {
            return false;
        }
        if (this.pathFollower.shouldSmoothPath()) {
            this.passedWaypoint = true;
            this.pathFollower.smoothPath(ref, position, activeMotionController, this.probeMoveData, componentAccessor);
        }
        return true;
    }
    
    protected boolean canSwitchToSteering(@Nonnull final Ref<EntityStore> ref, final MotionController motionController, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        return false;
    }
    
    protected boolean shouldSkipSteering(@Nonnull final Ref<EntityStore> ref, final MotionController activeMotionController, final Vector3d position, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        return !this.useSteering;
    }
    
    protected boolean computeSteering(@Nonnull final Ref<EntityStore> ref, final Role role, final Vector3d position, final Steering desiredSteering, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        return false;
    }
    
    protected boolean scaleSteering(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nonnull final SteeringForceWithTarget steeringForce, @Nonnull final Steering desiredSteering, final double desiredAltitudeWeight, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final MotionController motionController = role.getActiveMotionController();
        final boolean approachDesiredHeight = !motionController.is2D() && desiredAltitudeWeight > 0.0;
        final boolean withinRange = approachDesiredHeight && motionController.getDesiredVerticalRange(ref, componentAccessor).isWithinRange();
        if (withinRange) {
            steeringForce.setComponentSelector(role.getActiveMotionController().getPlanarComponentSelector());
        }
        else {
            steeringForce.setComponentSelector(role.getActiveMotionController().getComponentSelector());
        }
        if (!steeringForce.compute(desiredSteering)) {
            return false;
        }
        desiredSteering.scaleTranslation(this.getRelativeSpeed());
        if (approachDesiredHeight && !withinRange) {
            final MotionController.VerticalRange desiredAltitudeRange = motionController.getDesiredVerticalRange(ref, componentAccessor);
            if (desiredAltitudeRange.current > desiredAltitudeRange.max) {
                desiredSteering.setY(-this.computeDesiredYTranslation(desiredSteering, motionController.getMaxSinkAngle(), desiredAltitudeWeight));
            }
            else if (desiredAltitudeRange.current < desiredAltitudeRange.min) {
                desiredSteering.setY(this.computeDesiredYTranslation(desiredSteering, motionController.getMaxClimbAngle(), desiredAltitudeWeight));
            }
        }
        motionController.requireDepthProbing();
        return true;
    }
    
    protected double computeDesiredYTranslation(@Nonnull final Steering desiredSteering, final float maxAngle, final double desiredAltitudeWeight) {
        final double dirX = desiredSteering.getX();
        final double dirZ = desiredSteering.getZ();
        final double length = Math.sqrt(dirX * dirX + dirZ * dirZ);
        return length * TrigMathUtil.sin(maxAngle * desiredAltitudeWeight);
    }
    
    protected void onNoPathFound(final MotionController motionController) {
        this.aStar.clearPath();
    }
    
    protected void onBlockedPath() {
    }
    
    protected void onSteering(final MotionController activeMotionController, final Ref<EntityStore> ref, final ComponentAccessor<EntityStore> componentAccessor) {
    }
    
    protected void onThrottling(final MotionController motionController, final Ref<EntityStore> ref, final Steering steering, final ComponentAccessor<EntityStore> componentAccessor) {
    }
    
    protected void onDeferring(final MotionController motionController, final Ref<EntityStore> ref, final Steering steering, final ComponentAccessor<EntityStore> componentAccessor) {
    }
    
    protected boolean mustAbortThrottling(final MotionController motionController, final Ref<EntityStore> ref) {
        return false;
    }
    
    protected abstract boolean isGoalReached(final Ref<EntityStore> p0, final MotionController p1, final Vector3d p2, final ComponentAccessor<EntityStore> p3);
    
    protected void setNavState(final NavState state, final String label, final boolean reset, @Nonnull final MotionController activeMotionController) {
        if (reset) {
            this.resetThrottleCount();
            this.targetDeltaSquared = 0.0;
            this.forceRecomputePath(activeMotionController);
        }
        activeMotionController.setNavState(state, this.throttleTime, this.targetDeltaSquared);
        if (this.dbgDisplayString) {
            if (this.debugString == null) {
                this.debugString = new StringBuilder();
            }
            this.debugString.append(label).append(" TC:").append(this.throttleCount).append(" TT:").append(MathUtil.floor(this.throttleTime * 10.0) / 10.0);
            this.decorateDebugString(this.debugString);
            activeMotionController.getRole().getDebugSupport().setDisplayPathfinderString(this.debugString.toString());
            this.debugString.setLength();
        }
    }
    
    protected void decorateDebugString(final StringBuilder dbgString) {
    }
    
    protected void setNavStateInit(@Nonnull final MotionController motionController) {
        this.setNavState(NavState.INIT, "I-START", true, motionController);
    }
    
    protected void setNavStateComputing(@Nonnull final MotionController motionController) {
        this.setNavState(NavState.PROGRESSING, "P-COMPT", false, motionController);
    }
    
    protected void setNavStateDeferred(@Nonnull final MotionController motionController) {
        this.setNavState(NavState.DEFER, "D-DEFER", false, motionController);
    }
    
    protected void setNavStateAtGoal(@Nonnull final MotionController motionController) {
        this.setNavState(NavState.AT_GOAL, "G-GOAL", true, motionController);
    }
    
    protected void setNavStateFollowing(@Nonnull final MotionController motionController) {
        this.setNavState(NavState.PROGRESSING, "P-FOLLW", false, motionController);
    }
    
    protected void setNavStateSteering(@Nonnull final MotionController motionController) {
        this.setNavState(NavState.PROGRESSING, "P-STEER", false, motionController);
    }
    
    protected void setNavStateBlocked(@Nonnull final MotionController motionController) {
        this.setNavState(NavState.BLOCKED, "B-BLOCK", false, motionController);
    }
    
    protected void setNavStateAborted(@Nonnull final MotionController motionController) {
        this.setNavState(NavState.ABORTED, "A-ABORT", true, motionController);
    }
    
    protected void setNavStateThrottling(@Nonnull final MotionController motionController) {
        if (this.throttleDelay <= 0.0 && this.throttleDelayMax > 0.0 && this.throttleCount > this.throttleIgnoreCount) {
            this.throttleDelay = RandomExtra.randomRange(this.throttleDelayMin, this.throttleDelayMax);
        }
        this.setNavState(NavState.PROGRESSING, "P-THRTL", false, motionController);
    }
    
    protected void setPath(@Nonnull final Ref<EntityStore> ref, @Nonnull final Vector3d position, @Nonnull final MotionController activeMotionController, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final AStarNode aStarPath = this.aStar.getPath();
        this.pathFollower.setPath(aStarPath, position);
        this.passedWaypoint = false;
        this.updatePathFollower(ref, position, activeMotionController, componentAccessor);
    }
    
    protected void resetThrottleCount() {
        this.throttleTime = 0.0;
        this.throttleCount = 0;
        this.throttleDelay = 0.0;
    }
    
    protected AStarBase.Progress startComputePath(@Nonnull final Ref<EntityStore> ref, final Role role, @Nonnull final MotionController activeMotionController, @Nonnull final Vector3d position, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        return this.aStar.initComputePath(ref, position, this, activeMotionController, this.probeMoveData, this.sharedNodePoolProvider, componentAccessor);
    }
    
    protected boolean shouldDeferPathComputation(final MotionController motionController, final Vector3d position, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        return false;
    }
    
    protected boolean canComputeMotion(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nullable final InfoProvider positionProvider, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        return true;
    }
    
    protected boolean mustRecomputePath(@Nonnull final MotionController activeMotionController) {
        if (this.dbgRebuild) {
            return true;
        }
        if (activeMotionController.isObstructed()) {
            if (this.dbgStatus) {
                NPCPlugin.get().getLogger().at(Level.INFO).log("Recomputing Path - Blocked");
            }
            this.onBlockedPath();
            return true;
        }
        final boolean avoidingBlockDamage = activeMotionController.isAvoidingBlockDamage();
        if (this.wasAvoidingBlockDamage != avoidingBlockDamage) {
            this.wasAvoidingBlockDamage = avoidingBlockDamage;
            if (this.dbgStatus) {
                NPCPlugin.get().getLogger().at(Level.INFO).log("Recomputing Path - AvoidBlockDamage changed");
            }
            return true;
        }
        return false;
    }
    
    protected double getRelativeSpeed() {
        return this.pathFollower.getRelativeSpeed();
    }
    
    protected void forceRecomputePath(final MotionController activeMotionController) {
        this.aStar.clearPath();
        this.pathFollower.clearPath();
    }
    
    public enum DebugFlags implements Supplier<String>
    {
        Opens("Display open nodes each step when computing"), 
        Maps("Display map each step when computing"), 
        Path("Display map when path was found"), 
        Status("Display status messages"), 
        Rebuild("Force immediate rebuild of path"), 
        Profile("Measure time for path finding"), 
        Nodes("Display walk node information"), 
        Motion("Display Motion state changes"), 
        Stay("Don't move. Only compute path");
        
        private final String description;
        
        private DebugFlags(final String description) {
            this.description = description;
        }
        
        @Override
        public String get() {
            return this.description;
        }
    }
}