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

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

import com.hypixel.hytale.server.npc.NPCPlugin;
import com.hypixel.hytale.server.core.modules.physics.util.PhysicsMath;
import java.util.Arrays;
import java.util.Random;
import com.hypixel.hytale.common.util.ArrayUtil;
import com.hypixel.hytale.math.random.RandomExtra;
import java.util.concurrent.ThreadLocalRandom;
import com.hypixel.hytale.math.vector.Vector3f;
import com.hypixel.hytale.server.npc.util.NPCPhysicsMath;
import com.hypixel.hytale.server.core.modules.entity.component.TransformComponent;
import java.util.logging.Level;
import com.hypixel.hytale.server.npc.movement.Steering;
import com.hypixel.hytale.server.npc.sensorinfo.InfoProvider;
import com.hypixel.hytale.server.npc.movement.controllers.MotionController;
import javax.annotation.Nullable;
import com.hypixel.hytale.server.core.entity.nameplate.Nameplate;
import com.hypixel.hytale.server.npc.entities.NPCEntity;
import com.hypixel.hytale.server.npc.role.RoleDebugFlags;
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 com.hypixel.hytale.math.util.MathUtil;
import com.hypixel.hytale.server.npc.corecomponents.builders.BuilderBodyMotionBase;
import com.hypixel.hytale.server.npc.asset.builder.BuilderSupport;
import javax.annotation.Nonnull;
import com.hypixel.hytale.server.npc.corecomponents.movement.builders.BuilderBodyMotionWanderBase;
import com.hypixel.hytale.server.npc.movement.controllers.ProbeMoveData;
import com.hypixel.hytale.server.npc.movement.steeringforces.SteeringForcePursue;
import com.hypixel.hytale.math.vector.Vector3d;
import com.hypixel.hytale.logger.HytaleLogger;
import com.hypixel.hytale.server.npc.corecomponents.BodyMotionBase;

public abstract class BodyMotionWanderBase extends BodyMotionBase
{
    public static final HytaleLogger LOGGER;
    public static final int DIRECTION_COUNT = 32;
    public static final float SEGMENT_ANGLE = 0.19634955f;
    public static final double MIN_DISTANCE_SHRINK = 0.3;
    public static final double MIN_DISTANCE_SHRINK_SCALE = -1.4;
    protected final double minWalkTime;
    protected final double maxWalkTime;
    protected final float minHeadingChange;
    protected final float maxHeadingChange;
    protected final byte minDirection;
    protected final byte maxDirection;
    protected final boolean relaxHeadingChange;
    protected final double relativeSpeed;
    protected final double minMoveDistance;
    protected final double stopDistance;
    protected final int testsPerTick;
    protected final boolean isAvoidingBlockDamage;
    protected final boolean isRelaxedMoveConstraints;
    protected final double desiredAltitudeWeight;
    protected final byte[] preOrderedDirections;
    protected final int insideConeCount;
    protected final Vector3d targetPosition;
    protected final Vector3d probeDirection;
    protected final Vector3d probePosition;
    protected final SteeringForcePursue seekTarget;
    protected final ProbeMoveData probeMoveData;
    protected boolean debugSteer;
    protected State state;
    protected float angleOffset;
    protected double probeDY;
    protected double maxDistanceAbove;
    protected double maxDistanceBelow;
    protected double walkTime;
    protected float walkHeading;
    protected double walkDistance;
    protected int directionIndex;
    protected double desiredWalkDistance;
    protected final double[] walkDistances;
    protected final byte[] walkDirections;
    
    public BodyMotionWanderBase(@Nonnull final BuilderBodyMotionWanderBase builder, @Nonnull final BuilderSupport builderSupport) {
        super(builder);
        this.preOrderedDirections = new byte[32];
        this.targetPosition = new Vector3d();
        this.probeDirection = new Vector3d();
        this.probePosition = new Vector3d();
        this.seekTarget = new SteeringForcePursue();
        this.probeMoveData = new ProbeMoveData();
        this.walkDistances = new double[32];
        this.walkDirections = new byte[32];
        this.minWalkTime = builder.getMinWalkTime(builderSupport);
        this.maxWalkTime = builder.getMaxWalkTime(builderSupport);
        this.minHeadingChange = 0.017453292f * builder.getMinHeadingChange(builderSupport);
        this.maxHeadingChange = 0.017453292f * builder.getMaxHeadingChange(builderSupport);
        this.relaxHeadingChange = builder.isRelaxHeadingChange(builderSupport);
        this.minDirection = (byte)MathUtil.fastFloor(this.minHeadingChange / 0.19634955f);
        this.maxDirection = (byte)MathUtil.fastCeil(this.maxHeadingChange / 0.19634955f);
        this.relativeSpeed = builder.getRelativeSpeed(builderSupport);
        this.minMoveDistance = builder.getMinMoveDistance(builderSupport);
        this.stopDistance = builder.getStopDistance(builderSupport);
        this.testsPerTick = builder.getTestsPerTick(builderSupport);
        this.desiredAltitudeWeight = builder.getDesiredAltitudeWeight(builderSupport);
        final boolean avoidingBlockDamage = builder.isAvoidingBlockDamage(builderSupport);
        this.isAvoidingBlockDamage = avoidingBlockDamage;
        this.probeMoveData.setAvoidingBlockDamage(avoidingBlockDamage);
        final boolean relaxedMoveConstraints = builder.isRelaxedMoveConstraints(builderSupport);
        this.isRelaxedMoveConstraints = relaxedMoveConstraints;
        this.probeMoveData.setRelaxedMoveConstraints(relaxedMoveConstraints);
        int count = 0;
        for (int i = this.minDirection; i <= this.maxDirection; ++i) {
            count = this.addPreOrderedDirection(i, count);
        }
        this.insideConeCount = count;
        for (int i = 0; i < this.minDirection; ++i) {
            count = this.addPreOrderedDirection(i, count);
        }
        for (int i = this.maxDirection + 1; i <= 16; ++i) {
            count = this.addPreOrderedDirection(i, count);
        }
    }
    
    @Override
    public void activate(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.debugSteer = role.getDebugSupport().isDebugFlagSet(RoleDebugFlags.MotionControllerSteer);
        final NPCEntity npcComponent = componentAccessor.getComponent(ref, NPCEntity.getComponentType());
        assert npcComponent != null;
        this.restartSearch(ref, npcComponent, role.getActiveMotionController(), componentAccessor);
    }
    
    @Override
    public void deactivate(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        if (this.debugSteer) {
            componentAccessor.removeComponent(ref, Nameplate.getComponentType());
        }
    }
    
    @Override
    public void motionControllerChanged(@Nullable final Ref<EntityStore> ref, @Nonnull final NPCEntity npcComponent, @Nonnull final MotionController motionController, @Nullable final ComponentAccessor<EntityStore> componentAccessor) {
        this.restartSearch(ref, npcComponent, motionController, componentAccessor);
    }
    
    @Override
    public boolean computeSteering(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nullable final InfoProvider sensorInfo, final double dt, @Nonnull final Steering desiredSteering, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final NPCEntity npcComponent = componentAccessor.getComponent(ref, NPCEntity.getComponentType());
        assert npcComponent != null;
        final MotionController activeMotionController = role.getActiveMotionController();
        if (this.debugSteer) {
            BodyMotionWanderBase.LOGGER.at(Level.INFO).log("Wander compute: state=%s canAct=%s blocked=%s walkTime=%s", this.state.toString(), activeMotionController.canAct(ref, componentAccessor), activeMotionController.isObstructed(), this.walkTime);
            final String headline = this.state.toString();
            componentAccessor.putComponent(ref, Nameplate.getComponentType(), new Nameplate(headline));
        }
        desiredSteering.clear();
        final float currentHorizontalSpeedMultiplier = npcComponent.getCurrentHorizontalSpeedMultiplier(ref, componentAccessor);
        if (currentHorizontalSpeedMultiplier == 0.0f) {
            this.state = State.STOPPED;
            return true;
        }
        if (this.state == State.STOPPED) {
            this.restartSearch(ref, npcComponent, activeMotionController, componentAccessor);
        }
        if (activeMotionController.isInProgress()) {
            if (this.state == State.WALKING) {
                this.walkTime -= dt;
                activeMotionController.setRelaxedMoveConstraints(this.isRelaxedMoveConstraints);
                activeMotionController.setAvoidingBlockDamage(this.isAvoidingBlockDamage && activeMotionController.isAvoidingBlockDamage());
            }
            return true;
        }
        if (!activeMotionController.canAct(ref, componentAccessor)) {
            return true;
        }
        final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
        assert transformComponent != null;
        final Vector3f bodyRotation = transformComponent.getRotation();
        if (activeMotionController.isObstructed() && this.state == State.WALKING) {
            this.restartSearch(ref, npcComponent, activeMotionController, componentAccessor);
            if (this.debugSteer) {
                BodyMotionWanderBase.LOGGER.at(Level.INFO).log("Wander: Blocked state=%s directionIndex=%s walkTime=%s yaw=%s newYaw=%s", this.state.toString(), this.directionIndex, this.walkTime, 57.295776f * bodyRotation.getYaw(), 57.295776f * this.walkHeading);
            }
        }
        Label_0663: {
            if (this.state == State.SEARCHING) {
                int testCount = 0;
                do {
                    if (this.directionIndex == 32 || (!this.relaxHeadingChange && this.directionIndex == this.insideConeCount)) {
                        if (!this.findBestDirection(ref, componentAccessor)) {
                            this.restartSearch(ref, npcComponent, activeMotionController, componentAccessor);
                            continue;
                        }
                    }
                    else if (!this.probeDirection(ref, this.directionIndex, role, componentAccessor)) {
                        ++this.directionIndex;
                        continue;
                    }
                    final double stopDistance = Math.min(Math.max(this.stopDistance, activeMotionController.getCurrentTurnRadius()), this.walkDistance);
                    final double slowdownDistance = Math.min(2.0 * stopDistance, this.walkDistance);
                    this.seekTarget.setDistances(slowdownDistance, stopDistance);
                    this.seekTarget.setComponentSelector(activeMotionController.getComponentSelector());
                    this.seekTarget.setTargetPosition(this.targetPosition);
                    this.state = State.TURNING;
                    if (this.debugSteer) {
                        BodyMotionWanderBase.LOGGER.at(Level.INFO).log("Wander: Found move state=%s directionIndex=%s yaw=%s newYaw=%s", this.state.toString(), this.directionIndex, 57.295776f * bodyRotation.getYaw(), 57.295776f * this.walkHeading);
                    }
                    break Label_0663;
                } while (++testCount < this.testsPerTick);
                return true;
            }
        }
        if (this.state == State.TURNING) {
            final float heading = bodyRotation.getYaw();
            final double turnAngle = NPCPhysicsMath.turnAngle(this.walkHeading, heading);
            if (Math.abs(turnAngle) >= 0.05235987901687622) {
                desiredSteering.setYaw(this.walkHeading);
                if (this.debugSteer) {
                    BodyMotionWanderBase.LOGGER.at(Level.INFO).log("Wander: Turn state=%s turnAngle=%s heading=%s walkHeading=%s", this.state.toString(), 57.2957763671875 * turnAngle, 57.295776f * heading, 57.295776f * this.walkHeading);
                }
                return true;
            }
            if (this.debugSteer) {
                BodyMotionWanderBase.LOGGER.at(Level.INFO).log("Wander: Walk state=%s yaw=%s desiredYaw=%s walkTime=%s", this.state.toString(), 57.295776f * bodyRotation.getYaw(), 57.295776f * this.walkHeading, this.walkTime);
            }
            this.state = State.WALKING;
        }
        if (this.state == State.WALKING) {
            this.seekTarget.setSelfPosition(transformComponent.getPosition());
            this.walkTime -= dt;
            if (!this.seekTarget.compute(desiredSteering) || this.walkTime <= 0.0) {
                this.restartSearch(ref, npcComponent, activeMotionController, componentAccessor);
                if (this.debugSteer) {
                    BodyMotionWanderBase.LOGGER.at(Level.INFO).log("Wander: Walk done state=%s directionIndex=%s yaw=%s desiredYaw=%s", this.state.toString(), this.directionIndex, 57.295776f * bodyRotation.getYaw(), 57.295776f * this.walkHeading);
                }
            }
            activeMotionController.setRelaxedMoveConstraints(this.isRelaxedMoveConstraints);
            activeMotionController.setAvoidingBlockDamage(this.isAvoidingBlockDamage && activeMotionController.isAvoidingBlockDamage());
            desiredSteering.scaleTranslation(this.relativeSpeed);
        }
        return true;
    }
    
    protected boolean findBestDirection(@Nonnull final Ref<EntityStore> ref, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        int index = -1;
        double distance = 0.0;
        double count = 0.0;
        double average = 0.0;
        for (int i = 0; i < this.directionIndex; ++i) {
            final double d = this.walkDistances[i];
            if (d > 0.0) {
                ++count;
                average += d;
            }
        }
        if (count > 0.0) {
            average /= count;
            final ThreadLocalRandom random = ThreadLocalRandom.current();
            for (int j = 0; j < this.directionIndex; ++j) {
                final double d2 = this.walkDistances[j];
                if (d2 > distance) {
                    distance = d2;
                    index = j;
                    if (d2 >= average) {
                        final double r = random.nextDouble();
                        if (r <= 0.5) {
                            final double scale = r * -1.4 + 1.0;
                            distance *= scale;
                            break;
                        }
                    }
                }
            }
        }
        if (index == -1) {
            return false;
        }
        this.walkHeading = this.toAngle(ref, this.walkDirections[index], componentAccessor);
        this.walkDistance = distance;
        this.computeTargetPosition(ref, this.walkHeading, this.walkDistance, componentAccessor);
        return true;
    }
    
    protected abstract double constrainMove(@Nonnull final Ref<EntityStore> p0, @Nonnull final Role p1, @Nonnull final Vector3d p2, @Nonnull final Vector3d p3, final double p4, @Nonnull final ComponentAccessor<EntityStore> p5);
    
    protected void restartSearch(@Nullable final Ref<EntityStore> ref, @Nonnull final NPCEntity npcComponent, @Nonnull final MotionController activeMotionController, @Nullable final ComponentAccessor<EntityStore> componentAccessor) {
        this.state = State.SEARCHING;
        final float currentHorizontalSpeedMultiplier = npcComponent.getCurrentHorizontalSpeedMultiplier(ref, componentAccessor);
        this.walkTime = RandomExtra.randomRange(this.minWalkTime, this.maxWalkTime) / currentHorizontalSpeedMultiplier;
        this.desiredWalkDistance = this.relativeSpeed * activeMotionController.getMaximumSpeed() * this.walkTime;
        this.directionIndex = 0;
        this.angleOffset = (this.relaxHeadingChange ? (ThreadLocalRandom.current().nextFloat() * 0.19634955f) : 0.0f);
        if (ref != null && componentAccessor != null) {
            this.computeHeightRange(ref, activeMotionController, componentAccessor);
        }
        this.probeDY = RandomExtra.randomRange(-this.maxDistanceBelow, this.maxDistanceAbove);
        System.arraycopy(this.preOrderedDirections, 0, this.walkDirections, 0, 32);
        ArrayUtil.shuffleArray(this.walkDirections, 0, this.insideConeCount, ThreadLocalRandom.current());
        if (this.insideConeCount < 31) {
            ArrayUtil.shuffleArray(this.walkDirections, this.insideConeCount, 32, ThreadLocalRandom.current());
        }
        Arrays.fill(this.walkDistances, 0.0);
    }
    
    protected void computeHeightRange(@Nonnull final Ref<EntityStore> ref, @Nonnull final MotionController motionController, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.maxDistanceAbove = 0.0;
        this.maxDistanceBelow = 0.0;
        if (motionController.is2D()) {
            return;
        }
        final double wanderVerticalMovementRatio = motionController.getWanderVerticalMovementRatio();
        double maxVerticalDistance = wanderVerticalMovementRatio * this.desiredWalkDistance;
        if (maxVerticalDistance == 0.0) {
            return;
        }
        final MotionController.VerticalRange verticalRange = motionController.getDesiredVerticalRange(ref, componentAccessor);
        final double desiredAltitudeWeight = (this.desiredAltitudeWeight >= 0.0) ? this.desiredAltitudeWeight : motionController.getDesiredAltitudeWeight();
        if (desiredAltitudeWeight > 0.0 && !verticalRange.isWithinRange()) {
            maxVerticalDistance = this.desiredWalkDistance * (wanderVerticalMovementRatio + (1.0 - wanderVerticalMovementRatio) * desiredAltitudeWeight);
        }
        final double y = verticalRange.current;
        final double negativeMaxVerticalDistance = -maxVerticalDistance * desiredAltitudeWeight;
        this.maxDistanceAbove = MathUtil.clamp(verticalRange.max - y, negativeMaxVerticalDistance, maxVerticalDistance);
        this.maxDistanceBelow = MathUtil.clamp(y - verticalRange.min, negativeMaxVerticalDistance, maxVerticalDistance);
    }
    
    protected boolean probeDirection(@Nonnull final Ref<EntityStore> ref, final int dirIndex, @Nonnull final Role role, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final int direction = this.walkDirections[dirIndex];
        final MotionController motionController = role.getActiveMotionController();
        final float heading = this.toAngle(ref, direction, componentAccessor);
        this.computeTargetPosition(ref, heading, this.desiredWalkDistance, componentAccessor);
        final double constrainDistance = this.constrainMove(ref, role, this.probePosition, this.targetPosition, this.desiredWalkDistance, componentAccessor);
        if (constrainDistance < 1.0E-5) {
            return false;
        }
        if (constrainDistance < this.desiredWalkDistance) {
            this.probeDirection.scale(constrainDistance / this.desiredWalkDistance);
        }
        this.probeMoveData.setAvoidingBlockDamage(!motionController.willReceiveBlockDamage());
        final double moveDistance = motionController.probeMove(ref, this.probePosition, this.probeDirection, this.probeMoveData, componentAccessor);
        if (moveDistance < 1.0E-5) {
            return false;
        }
        this.walkDistances[dirIndex] = moveDistance;
        if (moveDistance < this.desiredWalkDistance) {
            return false;
        }
        if (moveDistance < constrainDistance) {
            this.probeDirection.scale(moveDistance / constrainDistance);
        }
        this.walkDistance = moveDistance;
        this.walkHeading = heading;
        this.targetPosition.assign(this.probePosition).add(this.probeDirection);
        return true;
    }
    
    private void computeTargetPosition(@Nonnull final Ref<EntityStore> ref, final float heading, final double distance, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
        assert transformComponent != null;
        this.probePosition.assign(transformComponent.getPosition());
        this.probeDirection.x = PhysicsMath.headingX(heading) * distance;
        this.probeDirection.y = this.probeDY * distance / this.desiredWalkDistance;
        this.probeDirection.z = PhysicsMath.headingZ(heading) * distance;
        this.targetPosition.assign(this.probePosition).add(this.probeDirection);
    }
    
    protected float toAngle(@Nonnull final Ref<EntityStore> ref, final int direction, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
        assert transformComponent != null;
        return PhysicsMath.normalizeAngle(transformComponent.getRotation().getYaw() + direction * 0.19634955f + this.angleOffset);
    }
    
    private int addPreOrderedDirection(final int direction, int count) {
        this.preOrderedDirections[count++] = (byte)direction;
        if (direction != 0 && direction != 16) {
            this.preOrderedDirections[count++] = (byte)(32 - direction);
        }
        return count;
    }
    
    static {
        LOGGER = NPCPlugin.get().getLogger();
    }
    
    public enum State
    {
        SEARCHING, 
        TURNING, 
        WALKING, 
        STOPPED;
    }
}