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

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

import javax.annotation.Nullable;
import com.hypixel.hytale.math.shape.Box;
import com.hypixel.hytale.server.core.asset.type.model.config.Model;
import com.hypixel.hytale.server.core.modules.collision.BlockCollisionData;
import com.hypixel.hytale.protocol.BlockMaterial;
import com.hypixel.hytale.logger.HytaleLogger;
import com.hypixel.hytale.server.core.modules.collision.CollisionModule;
import com.hypixel.hytale.server.core.modules.physics.component.Velocity;
import java.util.logging.Level;
import com.hypixel.hytale.server.core.modules.physics.util.PhysicsMath;
import com.hypixel.hytale.server.npc.util.NPCPhysicsMath;
import com.hypixel.hytale.server.npc.movement.Steering;
import com.hypixel.hytale.server.npc.role.Role;
import com.hypixel.hytale.server.core.modules.entity.component.TransformComponent;
import com.hypixel.hytale.component.ComponentAccessor;
import com.hypixel.hytale.server.core.universe.world.storage.EntityStore;
import com.hypixel.hytale.component.Ref;
import com.hypixel.hytale.math.util.MathUtil;
import com.hypixel.hytale.server.npc.movement.controllers.builders.BuilderMotionControllerBase;
import com.hypixel.hytale.server.npc.movement.controllers.builders.BuilderMotionControllerDive;
import javax.annotation.Nonnull;
import com.hypixel.hytale.server.npc.asset.builder.BuilderSupport;
import com.hypixel.hytale.server.npc.movement.MotionKind;
import java.util.EnumSet;
import com.hypixel.hytale.math.vector.Vector3d;
import com.hypixel.hytale.server.npc.util.PositionProbeWater;

public class MotionControllerDive extends MotionControllerBase
{
    public static final String TYPE = "Dive";
    public static final int COLLISION_MATERIALS_ACTIVE = 5;
    public static final int COLLISION_MATERIALS_PASSIVE = 4;
    public static final double DEFAULT_SWIM_DEPTH = 0.5;
    protected static double DAMPING_FACTOR;
    protected final double maxVerticalSpeed;
    protected final double acceleration;
    protected final double maxFallSpeed;
    protected final double maxSinkSpeed;
    protected final double maxRotationSpeed;
    protected final float maxMoveTurnAngle;
    protected final double minDiveDepth;
    protected final double maxDiveDepth;
    protected final double minWaterDepth;
    protected final double maxWaterDepth;
    protected final double minDepthAboveGround;
    protected final double minDepthBelowSurface;
    protected final double relativeSwimDepth;
    protected final double sinkRatio;
    protected final double fastDiveThreshold;
    protected final double minSpeedAfterForceSquared;
    protected final double desiredDepthWeight;
    protected double swimDepth;
    protected double climbSpeed;
    protected double currentRelativeSpeed;
    protected boolean collisionWithSolid;
    protected final MotionController.VerticalRange verticalRange;
    protected final PositionProbeWater moveProbe;
    protected final PositionProbeWater probeMoveProbe;
    protected final Vector3d tempPosition;
    protected final Vector3d tempDirection;
    private static final EnumSet<MotionKind> VALID_MOTIONS;
    
    public MotionControllerDive(@Nonnull final BuilderSupport builderSupport, @Nonnull final BuilderMotionControllerDive builder) {
        super(builderSupport, builder);
        this.verticalRange = new MotionController.VerticalRange();
        this.moveProbe = new PositionProbeWater();
        this.probeMoveProbe = new PositionProbeWater();
        this.tempPosition = new Vector3d();
        this.tempDirection = new Vector3d();
        this.setGravity(builder.getGravity());
        this.componentSelector.assign(1.0, 1.0, 1.0);
        this.maxVerticalSpeed = builder.getMaxVerticalSpeed();
        this.acceleration = builder.getAcceleration();
        this.maxSinkSpeed = builder.getMaxSinkSpeed();
        this.maxFallSpeed = builder.getMaxFallSpeed();
        this.maxRotationSpeed = builder.getMaxRotationSpeed();
        this.maxMoveTurnAngle = builder.getMaxMoveTurnAngle();
        this.minDiveDepth = builder.getMinDiveDepth();
        this.maxDiveDepth = builder.getMaxDiveDepth();
        this.minWaterDepth = builder.getMinWaterDepth();
        this.maxWaterDepth = builder.getMaxWaterDepth();
        this.minDepthAboveGround = builder.getMinDepthAboveGround();
        this.minDepthBelowSurface = builder.getMinDepthBelowSurface();
        this.relativeSwimDepth = builder.getSwimDepth();
        this.sinkRatio = builder.getSinkRatio();
        this.fastDiveThreshold = builder.getFastDiveThreshold();
        this.desiredDepthWeight = builder.getDesiredDepthWeight();
        final double minSpeedAfterForceSquared = MathUtil.minValue(this.maxVerticalSpeed, this.maxSinkSpeed, this.maxFallSpeed);
        this.minSpeedAfterForceSquared = minSpeedAfterForceSquared * minSpeedAfterForceSquared;
    }
    
    @Override
    public void activate() {
        super.activate();
        this.collisionResult.disableSlides();
    }
    
    @Override
    public double getWanderVerticalMovementRatio() {
        return this.sinkRatio;
    }
    
    @Nonnull
    @Override
    public MotionController.VerticalRange getDesiredVerticalRange(@Nonnull final Ref<EntityStore> ref, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final double waterLevel = this.moveProbe.getWaterLevel() + 1.0;
        double maxY = waterLevel - this.swimDepth - this.minDepthBelowSurface;
        final double groundY = this.moveProbe.getGroundLevel() + this.minDepthAboveGround;
        final double lowY = waterLevel - this.maxDiveDepth;
        double minY = Math.max(groundY, lowY);
        final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
        assert transformComponent != null;
        final double y = transformComponent.getPosition().y;
        if (this.onGround()) {
            minY = y;
        }
        if (this.moveProbe.isTouchCeil()) {
            maxY = y;
        }
        if (minY > maxY) {
            minY = y;
            maxY = y;
        }
        this.verticalRange.assign(y, minY, maxY);
        return this.verticalRange;
    }
    
    @Override
    protected double computeMove(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nonnull final Steering steering, final double dt, @Nonnull final Vector3d translation, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.saveMotionKind();
        this.moveProbe.probePosition(ref, this.collisionBoundingBox, this.position, this.collisionResult, this.swimDepth, componentAccessor);
        this.setMotionKind((!this.moveProbe.isInWater() && this.moveProbe.isOnGround()) ? MotionKind.MOVING : MotionKind.SWIMMING);
        this.currentRelativeSpeed = steering.getSpeed();
        final Vector3d dir = steering.getTranslation();
        float heading = this.getYaw();
        float pitch = this.getPitch();
        if (this.collisionWithSolid) {
            this.moveSpeed = 0.0;
            this.climbSpeed = 0.0;
            this.forceVelocity.assign(Vector3d.ZERO);
            this.appliedVelocities.clear();
        }
        if (this.canAct(ref, componentAccessor)) {
            this.tempDirection.assign(dir.x, 0.0, dir.z);
            final double maxVerticalSpeed = this.maxVerticalSpeed * this.effectHorizontalSpeedMultiplier;
            final double hSpeed = this.tempDirection.length() * this.getMaximumSpeed();
            final double vSpeed = dir.y * maxVerticalSpeed;
            this.moveSpeed = NPCPhysicsMath.accelerateToTargetSpeed(this.moveSpeed, hSpeed, dt, this.acceleration, this.getMaximumSpeed());
            this.climbSpeed = NPCPhysicsMath.accelerateToTargetSpeed(this.climbSpeed, vSpeed, dt, this.acceleration, this.acceleration, -maxVerticalSpeed, maxVerticalSpeed);
            final float maxRotation = (float)(dt * this.getCurrentMaxBodyRotationSpeed());
            final boolean isMoving = this.moveSpeed * this.moveSpeed + this.climbSpeed * this.climbSpeed > 1.0E-12 && steering.hasTranslation();
            float newHeading;
            float newPitch;
            if (this.moveSpeed * this.moveSpeed > 1.0000000000000002E-10 && steering.hasTranslation()) {
                newHeading = PhysicsMath.normalizeTurnAngle(PhysicsMath.headingFromDirection(dir.x, dir.z));
                newPitch = PhysicsMath.normalizeTurnAngle(PhysicsMath.pitchFromDirection(dir.x, dir.y, dir.z));
            }
            else {
                translation.assign(Vector3d.ZERO);
                if (steering.hasYaw()) {
                    newHeading = steering.getYaw();
                }
                else {
                    newHeading = heading;
                }
                steering.clearYaw();
                if (steering.hasPitch()) {
                    newPitch = steering.getPitch();
                }
                else {
                    newPitch = pitch;
                }
                steering.clearPitch();
            }
            float turnAngle = NPCPhysicsMath.turnAngle(heading, newHeading);
            float inclinationAngle = NPCPhysicsMath.turnAngle(pitch, newPitch);
            if (Math.abs(turnAngle) > this.maxMoveTurnAngle) {
                this.moveSpeed = 0.0;
            }
            if (this.isNearZero(turnAngle)) {
                heading = newHeading;
                turnAngle = 0.0f;
            }
            else {
                turnAngle = MathUtil.clamp(turnAngle, -maxRotation, maxRotation);
                heading += turnAngle;
                if (!isMoving) {
                    this.setMotionKind(MotionKind.SWIMMING_TURNING);
                }
            }
            heading = PhysicsMath.normalizeTurnAngle(heading);
            if (this.isNearZero(inclinationAngle)) {
                pitch = newPitch;
            }
            else {
                inclinationAngle = MathUtil.clamp(inclinationAngle, -maxRotation, maxRotation);
                pitch += inclinationAngle;
            }
            pitch = PhysicsMath.normalizeTurnAngle(pitch);
            if (!steering.hasYaw()) {
                steering.setYaw(heading);
            }
            if (!steering.hasPitch()) {
                steering.setPitch(pitch);
            }
            if (this.debugModeSteer) {
                MotionControllerDive.LOGGER.at(Level.INFO).log("=== Compute = t =%.4f v =%.4f h =%.4f nh=%.4f dh=%.4f", dt, this.moveSpeed, 57.295776f * heading, 57.295776f * newHeading, 57.295776f * turnAngle);
            }
            this.computeTranslation(translation, dt, heading, this.moveSpeed, this.climbSpeed);
            return dt;
        }
        final double maxSpeed = this.moveProbe.isInWater() ? this.maxSinkSpeed : this.maxFallSpeed;
        final boolean onGround = this.onGround();
        if (!this.isAlive(ref, componentAccessor)) {
            steering.setYaw(this.getYaw());
            steering.setPitch(onGround ? 0.0f : this.getPitch());
            this.forceVelocity.assign(Vector3d.ZERO);
            this.appliedVelocities.clear();
            this.moveSpeed = 0.0;
            this.climbSpeed = 0.0;
            if (onGround) {
                translation.assign(Vector3d.ZERO);
            }
            else {
                final Velocity velocityComponent = componentAccessor.getComponent(ref, Velocity.getComponentType());
                double sinkSpeed = velocityComponent.getVelocity().y;
                sinkSpeed = NPCPhysicsMath.gravityDrag(sinkSpeed, 5.0 * this.gravity, dt, maxSpeed);
                translation.assign(0.0, sinkSpeed, 0.0).scale(dt);
            }
            return dt;
        }
        if (!this.forceVelocity.equals(Vector3d.ZERO) || !this.appliedVelocities.isEmpty()) {
            this.moveSpeed = 0.0;
            this.climbSpeed = 0.0;
            if (!this.isObstructed()) {
                translation.assign(this.forceVelocity);
                for (int i = 0; i < this.appliedVelocities.size(); ++i) {
                    final AppliedVelocity entry = this.appliedVelocities.get(i);
                    if (entry.velocity.y + this.forceVelocity.y <= 0.0 || entry.velocity.y < 0.0) {
                        entry.canClear = true;
                    }
                    if (onGround && entry.canClear) {
                        entry.velocity.y = 0.0;
                    }
                    translation.add(entry.velocity);
                }
            }
            else {
                translation.assign(Vector3d.ZERO);
                this.appliedVelocities.clear();
                this.forceVelocity.assign(Vector3d.ZERO);
            }
            translation.y = NPCPhysicsMath.gravityDrag(this.forceVelocity.y, 5.0 * this.gravity, dt, maxSpeed);
            translation.scale(dt);
            steering.setYaw(this.getYaw());
            steering.setPitch(this.getPitch());
            return dt;
        }
        if (!steering.hasYaw()) {
            steering.setYaw(heading);
        }
        if (!steering.hasPitch()) {
            steering.setPitch(this.onGround() ? 0.0f : pitch);
        }
        this.climbSpeed = NPCPhysicsMath.gravityDrag(this.climbSpeed, 5.0 * this.gravity, dt, maxSpeed);
        this.computeTranslation(translation, dt, heading, this.moveSpeed, this.climbSpeed);
        return dt;
    }
    
    @Override
    protected boolean shouldDampenAppliedVelocitiesY() {
        return true;
    }
    
    @Override
    protected boolean shouldAlwaysUseGroundResistance() {
        return true;
    }
    
    private void computeTranslation(@Nonnull final Vector3d translation, final double dt, final float heading, final double moveSpeed, final double climbSpeed) {
        translation.x = moveSpeed * dt * PhysicsMath.headingX(heading);
        translation.z = moveSpeed * dt * PhysicsMath.headingZ(heading);
        translation.y = climbSpeed * dt;
        translation.clipToZero(this.getEpsilonSpeed());
    }
    
    private boolean isNearZero(final float angle) {
        final float epsilonAngle = this.getEpsilonAngle();
        return angle >= -epsilonAngle && angle <= epsilonAngle;
    }
    
    @Override
    public void setMotionKind(MotionKind motionKind) {
        if (!MotionControllerDive.VALID_MOTIONS.contains(motionKind)) {
            motionKind = MotionKind.SWIMMING;
        }
        super.setMotionKind(motionKind);
    }
    
    @Override
    protected double executeMove(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, final double dt, @Nonnull final Vector3d translation, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        if (this.debugModeValidatePositions && !this.isValidPosition(this.position, this.collisionResult, componentAccessor)) {
            throw new IllegalStateException("Invalid position");
        }
        final boolean canAct = this.canAct(ref, componentAccessor);
        this.collisionResult.setCollisionByMaterial(canAct ? 5 : 4);
        if (this.debugModeBlockCollisions) {
            this.collisionResult.setLogger(MotionControllerDive.LOGGER);
        }
        CollisionModule.get();
        CollisionModule.findCollisions(this.collisionBoundingBox, this.position, translation, this.collisionResult, componentAccessor);
        if (this.debugModeBlockCollisions) {
            this.collisionResult.setLogger(null);
        }
        if (this.debugModeCollisions) {
            this.dumpCollisionResults();
        }
        this.lastValidPosition.assign(this.position);
        this.isObstructed = false;
        this.collisionWithSolid = false;
        final BlockCollisionData collision = this.collisionResult.getFirstBlockCollision();
        if (collision == null) {
            double time = dt;
            this.position.add(translation);
            if (!this.moveProbe.probePosition(ref, this.collisionBoundingBox, this.position, this.collisionResult, this.swimDepth, componentAccessor) || (canAct && !this.moveProbe.isInWater())) {
                time = this.bisect(ref, this.lastValidPosition, 0.0, this.position, dt, this.position, componentAccessor);
                this.isObstructed = true;
                if (this.debugModeMove) {
                    MotionControllerDive.LOGGER.at(Level.INFO).log("Move - Dive: No Collision, Bisect pos=%s, blocked=%s", Vector3d.formatShortString(this.position), this.isObstructed);
                }
            }
            else if (this.debugModeMove) {
                MotionControllerDive.LOGGER.at(Level.INFO).log("Move - Dive: No collision, pos=%s, blocked=%s", Vector3d.formatShortString(this.position), this.isObstructed);
            }
            if (this.debugModeValidatePositions && !this.isValidPosition(this.position, this.collisionResult, componentAccessor)) {
                throw new IllegalStateException("Invalid position");
            }
            this.processTriggers(ref, this.collisionResult, time / dt, componentAccessor);
            return time;
        }
        else {
            if (this.debugModeValidatePositions && !this.isValidPosition(collision.collisionPoint, this.collisionResult, componentAccessor)) {
                throw new IllegalStateException("Invalid position");
            }
            double collisionStart = collision.collisionStart;
            this.position.assign(collision.collisionPoint);
            this.isObstructed = true;
            this.collisionWithSolid = (collision.blockMaterial == BlockMaterial.Solid);
            if (!this.moveProbe.probePosition(ref, this.collisionBoundingBox, this.position, this.collisionResult, this.swimDepth, componentAccessor) || (canAct && !this.moveProbe.isInWater())) {
                collisionStart = this.bisect(ref, this.lastValidPosition, 0.0, this.position, collisionStart, this.position, componentAccessor);
                if (this.debugModeMove) {
                    MotionControllerDive.LOGGER.at(Level.INFO).log("Move - Dive: Collision with solid=%s, Bisect pos=%s, blocked=%s", this.collisionWithSolid, Vector3d.formatShortString(this.position), this.isObstructed);
                }
            }
            else if (this.debugModeMove) {
                MotionControllerDive.LOGGER.at(Level.INFO).log("Move - Dive: Collision with solid=%s, pos=%s, blocked=%s", this.collisionWithSolid, Vector3d.formatShortString(this.position), this.isObstructed);
            }
            if (this.debugModeValidatePositions && !this.isValidPosition(this.position, this.collisionResult, componentAccessor)) {
                throw new IllegalStateException("Invalid position");
            }
            this.processTriggers(ref, this.collisionResult, collisionStart, componentAccessor);
            return dt * collisionStart;
        }
    }
    
    @Override
    public void constrainRotations(final Role role, @Nonnull final TransformComponent transform) {
        transform.getRotation().setRoll(0.0f);
    }
    
    @Override
    public double getCurrentMaxBodyRotationSpeed() {
        return this.maxRotationSpeed * this.effectHorizontalSpeedMultiplier;
    }
    
    @Override
    public boolean canAct(@Nonnull final Ref<EntityStore> ref, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        return super.canAct(ref, componentAccessor) && this.moveProbe.isInWater();
    }
    
    @Override
    public boolean inAir() {
        return !this.moveProbe.isOnGround();
    }
    
    @Override
    public boolean inWater() {
        return this.moveProbe.isInWater();
    }
    
    @Override
    public boolean onGround() {
        return this.moveProbe.isOnGround();
    }
    
    @Nonnull
    @Override
    public String getType() {
        return "Dive";
    }
    
    public double bisect(@Nonnull final Ref<EntityStore> ref, @Nonnull final Vector3d validPosition, final double validDistance, @Nonnull final Vector3d invalidPosition, final double invalidDistance, @Nonnull final Vector3d result, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        return NPCPhysicsMath.lerp(validDistance, invalidDistance, this.bisect(ref, validPosition, invalidPosition, result, componentAccessor));
    }
    
    public double bisect(@Nonnull final Ref<EntityStore> ref, @Nonnull final Vector3d validPosition, @Nonnull final Vector3d invalidPosition, @Nonnull final Vector3d result, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        return this.bisect(validPosition, invalidPosition, this, (_this, position) -> _this.probeMoveProbe.probePosition(ref, _this.collisionBoundingBox, position, _this.collisionResult, _this.swimDepth, componentAccessor), result);
    }
    
    @Override
    public double probeMove(@Nonnull final Ref<EntityStore> ref, @Nonnull final ProbeMoveData probeMoveData, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final boolean saveSegments = probeMoveData.startProbing();
        this.collisionResult.setCollisionByMaterial(5);
        final Vector3d probePosition = probeMoveData.probePosition;
        final Vector3d probeMovement = probeMoveData.probeDirection;
        final CollisionModule collisionModule = CollisionModule.get();
        if (saveSegments) {
            probeMoveData.addStartSegment(probePosition, false);
        }
        if (!this.probeMoveProbe.probePosition(ref, this.collisionBoundingBox, probePosition, this.collisionResult, this.swimDepth, componentAccessor)) {
            if (saveSegments) {
                probeMoveData.addEndSegment(probePosition, false, 0.0);
            }
            return 0.0;
        }
        final double maxDistance = probeMovement.length();
        CollisionModule.findCollisions(this.collisionBoundingBox, probePosition, probeMovement, this.collisionResult, componentAccessor);
        if (this.debugModeMove) {
            MotionControllerDive.LOGGER.at(Level.INFO).log("Probe Step: pos=%s mov=%s left=%s", Vector3d.formatShortString(probePosition), Vector3d.formatShortString(probeMovement), maxDistance);
        }
        if (this.debugModeCollisions) {
            this.dumpCollisionResults();
        }
        final BlockCollisionData collision = this.collisionResult.getFirstBlockCollision();
        this.tempPosition.assign(probePosition);
        if (collision == null) {
            probePosition.add(probeMovement);
            probeMovement.assign(Vector3d.ZERO);
            double distanceTravelled;
            if (!this.probeMoveProbe.probePosition(ref, this.collisionBoundingBox, probePosition, this.collisionResult, this.swimDepth, componentAccessor) || !this.probeMoveProbe.isInWater()) {
                distanceTravelled = this.bisect(ref, this.tempPosition, 0.0, probePosition, maxDistance, probePosition, componentAccessor);
                if (this.debugModeMove) {
                    MotionControllerDive.LOGGER.at(Level.INFO).log("Probe - Dive: No Collision, Bisect pos=%s, distanceLeft=%s", Vector3d.formatShortString(probePosition), maxDistance - distanceTravelled);
                }
            }
            else {
                distanceTravelled = maxDistance;
                if (this.debugModeMove) {
                    MotionControllerDive.LOGGER.at(Level.INFO).log("Probe - Dive: No Collision, valid pos=%s, distanceLeft=%s", Vector3d.formatShortString(probePosition), maxDistance - distanceTravelled);
                }
            }
            if (this.debugModeValidatePositions && !this.isValidPosition(this.tempPosition, this.collisionResult, componentAccessor)) {
                throw new IllegalStateException("Invalid position");
            }
            if (saveSegments) {
                probeMoveData.addMoveSegment(probePosition, false, distanceTravelled);
                probeMoveData.addEndSegment(probePosition, false, distanceTravelled);
            }
            if (this.debugModeMove) {
                MotionControllerDive.LOGGER.at(Level.INFO).log("Probe Move done: No collision - maxDistance=%s distanceLeft=%s", maxDistance, maxDistance - distanceTravelled);
            }
            return distanceTravelled;
        }
        else {
            final double collisionStart = collision.collisionStart;
            double distanceTravelled = maxDistance * collisionStart;
            probePosition.assign(collision.collisionPoint);
            if (!this.probeMoveProbe.probePosition(ref, this.collisionBoundingBox, probePosition, this.collisionResult, this.swimDepth, componentAccessor) || !this.probeMoveProbe.isInWater()) {
                distanceTravelled = this.bisect(ref, this.tempPosition, 0.0, probePosition, distanceTravelled, probePosition, componentAccessor);
                if (this.debugModeMove) {
                    MotionControllerDive.LOGGER.at(Level.INFO).log("Probe - Dive: Collision, Bisect pos=%s, distanceLeft=%s, collision start=%s", Vector3d.formatShortString(probePosition), maxDistance - distanceTravelled, collisionStart);
                }
            }
            else if (this.debugModeMove) {
                MotionControllerDive.LOGGER.at(Level.INFO).log("Probe - Dive: Collision, valid pos=%s, distanceLeft=%s, collision start=%s", Vector3d.formatShortString(probePosition), maxDistance - distanceTravelled, collisionStart);
            }
            if (this.debugModeValidatePositions && !this.isValidPosition(probePosition, this.collisionResult, componentAccessor)) {
                throw new IllegalStateException("Invalid position");
            }
            if (saveSegments) {
                if (this.getWorldNormal().equals(collision.collisionNormal)) {
                    probeMoveData.addHitGroundSegment(probePosition, distanceTravelled, collision.collisionNormal, collision.blockId);
                }
                else {
                    probeMoveData.addHitWallSegment(probePosition, false, distanceTravelled, collision.collisionNormal, collision.blockId);
                }
            }
            if (saveSegments) {
                probeMoveData.addEndSegment(probePosition, false, distanceTravelled);
            }
            if (this.debugModeMove) {
                MotionControllerDive.LOGGER.at(Level.INFO).log("Probe Move done: maxDistance=%s distanceLeft=%s", maxDistance, maxDistance - distanceTravelled);
            }
            return distanceTravelled;
        }
    }
    
    @Override
    public void spawned() {
    }
    
    @Override
    public double getCurrentSpeed() {
        return this.moveSpeed;
    }
    
    @Override
    public double getCurrentTurnRadius() {
        return 0.0;
    }
    
    @Override
    public float getMaxClimbAngle() {
        return 1.5707964f;
    }
    
    @Override
    public float getMaxSinkAngle() {
        return 1.5707964f;
    }
    
    @Override
    public double getMaximumSpeed() {
        return this.maxHorizontalSpeed * this.effectHorizontalSpeedMultiplier;
    }
    
    public boolean isFastMotionKind(final double speed) {
        return this.currentRelativeSpeed > this.fastDiveThreshold;
    }
    
    @Override
    public boolean is2D() {
        return false;
    }
    
    @Override
    public boolean canRestAtPlace() {
        return true;
    }
    
    @Override
    public double getDesiredAltitudeWeight() {
        return this.desiredDepthWeight;
    }
    
    @Override
    public double getHeightOverGround() {
        return this.probeMoveProbe.getHeightOverGround();
    }
    
    @Override
    public boolean estimateVelocity(final Steering steering, @Nonnull final Vector3d velocityOut) {
        velocityOut.assign(Vector3d.ZERO);
        return false;
    }
    
    @Override
    public void updateModelParameters(final Ref<EntityStore> ref, final Model model, @Nonnull final Box boundingBox, final ComponentAccessor<EntityStore> componentAccessor) {
        super.updateModelParameters(ref, model, boundingBox, componentAccessor);
        this.swimDepth = relativeSwimDepthToHeight(ref, this.relativeSwimDepth, model, boundingBox, componentAccessor);
    }
    
    @Override
    protected void dampForceVelocity(@Nonnull final Vector3d forceVelocity, final double forceVelocityDamping, final double interval, final ComponentAccessor<EntityStore> componentAccessor) {
        if (forceVelocity.squaredLength() < this.minSpeedAfterForceSquared) {
            forceVelocity.assign(Vector3d.ZERO);
            return;
        }
        NPCPhysicsMath.deccelerateToStop(forceVelocity, this.getDampingDeceleration(), interval);
    }
    
    public static double relativeSwimDepthToBoundingBox(final double swimDepth, @Nullable final Box boundingBox, final float eyeHeight) {
        if (boundingBox == null) {
            return 0.5;
        }
        if (swimDepth < 0.0) {
            return NPCPhysicsMath.lerp(eyeHeight, boundingBox.getMin().getY(), -swimDepth);
        }
        return NPCPhysicsMath.lerp(eyeHeight, boundingBox.getMax().getY(), swimDepth);
    }
    
    public static double relativeSwimDepthToHeight(final double swimDepth, @Nullable final Box boundingBox, final float eyeHeight) {
        if (boundingBox == null) {
            return 0.5;
        }
        return relativeSwimDepthToBoundingBox(swimDepth, boundingBox, eyeHeight) - boundingBox.getMin().getY();
    }
    
    public static double relativeSwimDepthToHeight(@Nullable final Ref<EntityStore> ref, final double swimDepth, final Model model, final Box boundingBox, @Nullable final ComponentAccessor<EntityStore> componentAccessor) {
        return relativeSwimDepthToHeight(swimDepth, boundingBox, (model != null) ? model.getEyeHeight(ref, componentAccessor) : 0.0f);
    }
    
    public double getDampingDeceleration() {
        return this.forceVelocityDamping * MotionControllerDive.DAMPING_FACTOR;
    }
    
    static {
        MotionControllerDive.DAMPING_FACTOR = 20.0;
        VALID_MOTIONS = EnumSet.of(MotionKind.SWIMMING, MotionKind.SWIMMING_TURNING, MotionKind.MOVING);
    }
}