class="kw">package com.hypixel.hytale.server.npc.corecomponents.movement;

class="kw">import com.hypixel.hytale.common.util.ArrayUtil;
class="kw">import com.hypixel.hytale.component.ComponentAccessor;
class="kw">import com.hypixel.hytale.component.Ref;
class="kw">import com.hypixel.hytale.logger.HytaleLogger;
class="kw">import com.hypixel.hytale.math.random.RandomExtra;
class="kw">import com.hypixel.hytale.math.util.MathUtil;
class="kw">import com.hypixel.hytale.math.vector.Rotation3f;
class="kw">import com.hypixel.hytale.server.core.entity.nameplate.Nameplate;
class="kw">import com.hypixel.hytale.server.core.modules.entity.component.TransformComponent;
class="kw">import com.hypixel.hytale.server.core.modules.physics.util.PhysicsMath;
class="kw">import com.hypixel.hytale.server.core.universe.world.storage.EntityStore;
class="kw">import com.hypixel.hytale.server.npc.NPCPlugin;
class="kw">import com.hypixel.hytale.server.npc.asset.builder.BuilderSupport;
class="kw">import com.hypixel.hytale.server.npc.corecomponents.BodyMotionBase;
class="kw">import com.hypixel.hytale.server.npc.corecomponents.movement.builders.BuilderBodyMotionWanderBase;
class="kw">import com.hypixel.hytale.server.npc.entities.NPCEntity;
class="kw">import com.hypixel.hytale.server.npc.instructions.ExecutionSupport;
class="kw">import com.hypixel.hytale.server.npc.movement.Steering;
class="kw">import com.hypixel.hytale.server.npc.movement.constraints.RelaxedConstraint;
class="kw">import com.hypixel.hytale.server.npc.movement.controllers.MotionController;
class="kw">import com.hypixel.hytale.server.npc.movement.controllers.ProbeMoveData;
class="kw">import com.hypixel.hytale.server.npc.movement.steeringforces.SteeringForcePursue;
class="kw">import com.hypixel.hytale.server.npc.role.RoleDebugFlags;
class="kw">import com.hypixel.hytale.server.npc.sensorinfo.InfoProvider;
class="kw">import com.hypixel.hytale.server.npc.util.NPCPhysicsMath;
class="kw">import java.util.Arrays;
class="kw">import java.util.EnumSet;
class="kw">import java.util.concurrent.ThreadLocalRandom;
class="kw">import java.util.logging.Level;
class="kw">import javax.annotation.Nonnull;
class="kw">import javax.annotation.Nullable;
class="kw">import org.joml.Vector3d;

class="kw">public class="kw">abstract class BodyMotionWanderBase class="kw">extends BodyMotionBase {
   class="kw">public class="kw">static class="kw">final HytaleLogger LOGGER = NPCPlugin.get().getLogger();
   class="kw">public class="kw">static class="kw">final int DIRECTION_COUNT = 32;
   class="kw">public class="kw">static class="kw">final float SEGMENT_ANGLE = (float) (Math.PI / 16);
   class="kw">public class="kw">static class="kw">final double MIN_DISTANCE_SHRINK = 0.3;
   class="kw">public class="kw">static class="kw">final double MIN_DISTANCE_SHRINK_SCALE = -1.4;
   class="kw">protected class="kw">final double minWalkTime;
   class="kw">protected class="kw">final double maxWalkTime;
   class="kw">protected class="kw">final float minHeadingChange;
   class="kw">protected class="kw">final float maxHeadingChange;
   class="kw">protected class="kw">final byte minDirection;
   class="kw">protected class="kw">final byte maxDirection;
   class="kw">protected class="kw">final boolean relaxHeadingChange;
   class="kw">protected class="kw">final double relativeSpeed;
   class="kw">protected class="kw">final double minMoveDistance;
   class="kw">protected class="kw">final double stopDistance;
   class="kw">protected class="kw">final int testsPerTick;
   class="kw">protected class="kw">final double desiredAltitudeWeight;
   class="kw">protected class="kw">final byte[] preOrderedDirections = new byte[32];
   class="kw">protected class="kw">final int insideConeCount;
   class="kw">protected class="kw">final Vector3d targetPosition = new Vector3d();
   class="kw">protected class="kw">final Vector3d probeDirection = new Vector3d();
   class="kw">protected class="kw">final Vector3d probePosition = new Vector3d();
   class="kw">protected class="kw">final SteeringForcePursue seekTarget = new SteeringForcePursue();
   class="kw">protected class="kw">final ProbeMoveData probeMoveData = new ProbeMoveData();
   class="kw">protected boolean debugSteer;
   class="kw">protected BodyMotionWanderBase.State state;
   class="kw">protected float angleOffset;
   class="kw">protected double probeDY;
   class="kw">protected double maxDistanceAbove;
   class="kw">protected double maxDistanceBelow;
   class="kw">protected double walkTime;
   class="kw">protected float walkHeading;
   class="kw">protected double walkDistance;
   class="kw">protected int directionIndex;
   class="kw">protected double desiredWalkDistance;
   class="kw">protected boolean searchUsesEscapeConstraints;
   @Nonnull
   class="kw">protected class="kw">final EnumSet<RelaxedConstraint> cachedEffectiveConstraints;
   class="kw">protected class="kw">final double[] walkDistances = new double[32];
   class="kw">protected class="kw">final byte[] walkDirections = new byte[32];

   class="kw">public BodyMotionWanderBase(@Nonnull BuilderBodyMotionWanderBase var1, @Nonnull BuilderSupport var2) {
      super(var1);
      this.minWalkTime = var1.getMinWalkTime(var2);
      this.maxWalkTime = var1.getMaxWalkTime(var2);
      this.minHeadingChange = (float) (Math.PI / 180.0) * var1.getMinHeadingChange(var2);
      this.maxHeadingChange = (float) (Math.PI / 180.0) * var1.getMaxHeadingChange(var2);
      this.relaxHeadingChange = var1.isRelaxHeadingChange(var2);
      this.minDirection = (byte)MathUtil.fastFloor(this.minHeadingChange / (float) (Math.PI / 16));
      this.maxDirection = (byte)MathUtil.fastCeil(this.maxHeadingChange / (float) (Math.PI / 16));
      this.relativeSpeed = var1.getRelativeSpeed(var2);
      this.minMoveDistance = var1.getMinMoveDistance(var2);
      this.stopDistance = var1.getStopDistance(var2);
      this.testsPerTick = var1.getTestsPerTick(var2);
      this.desiredAltitudeWeight = var1.getDesiredAltitudeWeight(var2);
      boolean var3 = var1.isLegacyRelaxedMoveConstraints(var2);
      boolean var4 = !var1.isRelaxedConstraintsPresent();
      EnumSet var5 = var1.getRelaxedConstraints(var2);
      boolean var6 = var1.isAvoidingBlockDamage(var2);
      this.cachedEffectiveConstraints = computeEffectiveRelaxedConstraints(var4, var5, var3, var6);
      this.probeMoveData.setRelaxedConstraints(this.cachedEffectiveConstraints);
      int var8 = 0;

      for (int var7 = this.minDirection; var7 <= this.maxDirection; var7++) {
         var8 = this.addPreOrderedDirection(var7, var8);
      }

      this.insideConeCount = var8;

      for (int var9 = 0; var9 < this.minDirection; var9++) {
         var8 = this.addPreOrderedDirection(var9, var8);
      }

      for (int var10 = this.maxDirection + 1; var10 <= 16; var10++) {
         var8 = this.addPreOrderedDirection(var10, var8);
      }
   }

   @Nullable
   @Override
   class="kw">public EnumSet<RelaxedConstraint> getRelaxedConstraints() {
      class="kw">return this.cachedEffectiveConstraints;
   }

   @Override
   class="kw">public void activate(@Nonnull Ref<EntityStore> var1, @Nonnull ExecutionSupport var2, @Nonnull ComponentAccessor<EntityStore> var3) {
      this.debugSteer = var2.getDebugSupport().isDebugFlagSet(RoleDebugFlags.MotionControllerSteer);
      NPCEntity var4 = var3.getComponent(var1, NPCEntity.getComponentType());
      assert var4 != null;
      this.restartSearch(var1, var4, var2.getMotionContextSupport().getActiveMotionController(), var3);
   }

   @Override
   class="kw">public void deactivate(@Nonnull Ref<EntityStore> var1, @Nonnull ExecutionSupport var2, @Nonnull ComponentAccessor<EntityStore> var3) {
      if (this.debugSteer) {
         var3.removeComponent(var1, Nameplate.getComponentType());
      }
   }

   @Override
   class="kw">public void motionControllerChanged(
      @Nullable Ref<EntityStore> var1, @Nonnull NPCEntity var2, @Nonnull MotionController var3, @Nullable ComponentAccessor<EntityStore> var4
   ) {
      this.restartSearch(var1, var2, var3, var4);
   }

   @Override
   class="kw">public boolean computeSteering(
      @Nonnull Ref<EntityStore> var1,
      @Nonnull ExecutionSupport var2,
      @Nullable InfoProvider var3,
      double var4,
      @Nonnull Steering var6,
      @Nonnull ComponentAccessor<EntityStore> var7
   ) {
      NPCEntity var8 = var7.getComponent(var1, NPCEntity.getComponentType());
      assert var8 != null;
      MotionController var9 = var2.getMotionContextSupport().getActiveMotionController();
      if (this.debugSteer) {
         LOGGER.at(Level.INFO)
            .log(
               "Wander compute: state=%s canSteer=%s blocked=%s walkTime=%s",
               this.state.toString(),
               var9.canSteer(var1, var7),
               var9.isObstructed(),
               this.walkTime
            );
         String var10 = this.state.toString();
         var7.putComponent(var1, Nameplate.getComponentType(), new Nameplate(var10));
      }

      var6.clear();
      float var19 = var8.getCurrentHorizontalSpeedMultiplier(var1, var7);
      if (var19 == 0.0F) {
         this.state = BodyMotionWanderBase.State.STOPPED;
         class="kw">return true;
      }

      if (this.state == BodyMotionWanderBase.State.STOPPED) {
         this.restartSearch(var1, var8, var9, var7);
      }

      this.probeMoveData.setRelaxedConstraints(this.cachedEffectiveConstraints);
      boolean var11 = applyEscapeConstraints(var2, this.probeMoveData);
      if (this.state == BodyMotionWanderBase.State.SEARCHING && this.directionIndex == 0) {
         this.searchUsesEscapeConstraints = var11;
      } else if (this.searchUsesEscapeConstraints && !var11) {
         this.restartSearch(var1, var8, var9, var7);
      }

      if (var9.isInProgress()) {
         if (this.state == BodyMotionWanderBase.State.WALKING) {
            this.walkTime -= var4;
            var9.setRelaxedMoveConstraints(this.cachedEffectiveConstraints);
         }

         class="kw">return true;
      } else {
         if (!var9.canSteer(var1, var7)) {
            class="kw">return true;
         }

         TransformComponent var12 = var7.getComponent(var1, TransformComponent.getComponentType());
         assert var12 != null;
         Rotation3f var13 = var12.getRotation();
         if (var9.isObstructed() && this.state == BodyMotionWanderBase.State.WALKING) {
            this.restartSearch(var1, var8, var9, var7);
            if (this.debugSteer) {
               LOGGER.at(Level.INFO)
                  .log(
                     "Wander: Blocked state=%s directionIndex=%s walkTime=%s yaw=%s newYaw=%s",
                     this.state.toString(),
                     this.directionIndex,
                     this.walkTime,
                     (180.0F / (float)Math.PI) * var13.yaw(),
                     (180.0F / (float)Math.PI) * this.walkHeading
                  );
            }
         }

         if (this.state == BodyMotionWanderBase.State.SEARCHING) {
            int var14 = 0;

            while (true) {
               if (this.directionIndex == 32 || !this.relaxHeadingChange && this.directionIndex == this.insideConeCount) {
                  if (this.findBestDirection(var1, var7)) {
                     break;
                  }

                  this.restartSearch(var1, var8, var9, var7);
               } else {
                  if (this.probeDirection(var1, this.directionIndex, var2, var7)) {
                     break;
                  }

                  this.directionIndex++;
               }

               if (++var14 >= this.testsPerTick) {
                  class="kw">return true;
               }
            }

            double var15 = Math.min(Math.max(this.stopDistance, var9.getCurrentTurnRadius()), this.walkDistance);
            double var17 = Math.min(2.0 * var15, this.walkDistance);
            this.seekTarget.setDistances(var17, var15);
            this.seekTarget.setComponentSelector(var9.getComponentSelector());
            this.seekTarget.setTargetPosition(this.targetPosition);
            this.state = BodyMotionWanderBase.State.TURNING;
            if (this.debugSteer) {
               LOGGER.at(Level.INFO)
                  .log(
                     "Wander: Found move state=%s directionIndex=%s yaw=%s newYaw=%s",
                     this.state.toString(),
                     this.directionIndex,
                     (180.0F / (float)Math.PI) * var13.yaw(),
                     (180.0F / (float)Math.PI) * this.walkHeading
                  );
            }
         }

         if (this.state == BodyMotionWanderBase.State.TURNING) {
            float var20 = var13.yaw();
            double var21 = NPCPhysicsMath.turnAngle(this.walkHeading, var20);
            if (!(Math.abs(var21) < 0.05235988F)) {
               var6.setYaw(this.walkHeading);
               if (this.debugSteer) {
                  LOGGER.at(Level.INFO)
                     .log(
                        "Wander: Turn state=%s turnAngle=%s heading=%s walkHeading=%s",
                        this.state.toString(),
                        180.0F / (float)Math.PI * var21,
                        (180.0F / (float)Math.PI) * var20,
                        (180.0F / (float)Math.PI) * this.walkHeading
                     );
               }

               class="kw">return true;
            }

            if (this.debugSteer) {
               LOGGER.at(Level.INFO)
                  .log(
                     "Wander: Walk state=%s yaw=%s desiredYaw=%s walkTime=%s",
                     this.state.toString(),
                     (180.0F / (float)Math.PI) * var13.yaw(),
                     (180.0F / (float)Math.PI) * this.walkHeading,
                     this.walkTime
                  );
            }

            this.state = BodyMotionWanderBase.State.WALKING;
         }

         if (this.state == BodyMotionWanderBase.State.WALKING) {
            this.seekTarget.setSelfPosition(var12.getPosition());
            this.walkTime -= var4;
            if (!this.seekTarget.compute(var6) || this.walkTime <= 0.0) {
               this.restartSearch(var1, var8, var9, var7);
               if (this.debugSteer) {
                  LOGGER.at(Level.INFO)
                     .log(
                        "Wander: Walk done state=%s directionIndex=%s yaw=%s desiredYaw=%s",
                        this.state.toString(),
                        this.directionIndex,
                        (180.0F / (float)Math.PI) * var13.yaw(),
                        (180.0F / (float)Math.PI) * this.walkHeading
                     );
               }
            }

            var9.setRelaxedMoveConstraints(this.cachedEffectiveConstraints);
            this.probeMoveData.setRelaxedConstraints(this.cachedEffectiveConstraints);
            applyEscapeConstraints(var2, this.probeMoveData);
            var6.scaleTranslation(this.relativeSpeed);
         }

         class="kw">return true;
      }
   }

   class="kw">protected boolean findBestDirection(@Nonnull Ref<EntityStore> var1, @Nonnull ComponentAccessor<EntityStore> var2) {
      int var3 = -1;
      double var4 = 0.0;
      double var6 = 0.0;
      double var8 = 0.0;

      for (int var10 = 0; var10 < this.directionIndex; var10++) {
         double var11 = this.walkDistances[var10];
         if (var11 > 0.0) {
            var6++;
            var8 += var11;
         }
      }

      if (var6 > 0.0) {
         var8 /= var6;
         ThreadLocalRandom var19 = ThreadLocalRandom.current();

         for (int var20 = 0; var20 < this.directionIndex; var20++) {
            double var12 = this.walkDistances[var20];
            if (var12 > var4) {
               var4 = var12;
               var3 = var20;
               if (!(var12 < var8)) {
                  double var14 = var19.nextDouble();
                  if (var14 <= 0.5) {
                     double var16 = var14 * -1.4 + 1.0;
                     var4 *= var16;
                     break;
                  }
               }
            }
         }
      }

      if (var3 == -1) {
         class="kw">return false;
      }

      this.walkHeading = this.toAngle(var1, this.walkDirections[var3], var2);
      this.walkDistance = var4;
      this.computeTargetPosition(var1, this.walkHeading, this.walkDistance, var2);
      class="kw">return true;
   }

   class="kw">protected class="kw">abstract double constrainMove(
      @Nonnull Ref<EntityStore> var1,
      @Nonnull ExecutionSupport var2,
      @Nonnull Vector3d var3,
      @Nonnull Vector3d var4,
      double var5,
      @Nonnull ComponentAccessor<EntityStore> var7
   );

   class="kw">protected void restartSearch(
      @Nullable Ref<EntityStore> var1, @Nonnull NPCEntity var2, @Nonnull MotionController var3, @Nullable ComponentAccessor<EntityStore> var4
   ) {
      this.state = BodyMotionWanderBase.State.SEARCHING;
      this.searchUsesEscapeConstraints = false;
      float var5 = var2.getCurrentHorizontalSpeedMultiplier(var1, var4);
      this.walkTime = RandomExtra.randomRange(this.minWalkTime, this.maxWalkTime) / var5;
      this.desiredWalkDistance = this.relativeSpeed * var3.getMaximumSpeed() * this.walkTime;
      this.directionIndex = 0;
      this.angleOffset = this.relaxHeadingChange ? ThreadLocalRandom.current().nextFloat() * (float) (Math.PI / 16) : 0.0F;
      if (var1 != null && var4 != null) {
         this.computeHeightRange(var1, var3, var4);
      }

      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);
   }

   class="kw">protected void computeHeightRange(@Nonnull Ref<EntityStore> var1, @Nonnull MotionController var2, @Nonnull ComponentAccessor<EntityStore> var3) {
      this.maxDistanceAbove = 0.0;
      this.maxDistanceBelow = 0.0;
      if (!var2.is2D()) {
         double var4 = var2.getWanderVerticalMovementRatio();
         double var6 = var4 * this.desiredWalkDistance;
         if (var6 != 0.0) {
            MotionController.VerticalRange var8 = var2.getDesiredVerticalRange(var1, var3);
            double var9 = this.desiredAltitudeWeight >= 0.0 ? this.desiredAltitudeWeight : var2.getDesiredAltitudeWeight();
            if (var9 > 0.0 && !var8.isWithinRange()) {
               var6 = this.desiredWalkDistance * (var4 + (1.0 - var4) * var9);
            }

            double var11 = var8.current;
            double var13 = -var6 * var9;
            this.maxDistanceAbove = MathUtil.clamp(var8.max - var11, var13, var6);
            this.maxDistanceBelow = MathUtil.clamp(var11 - var8.min, var13, var6);
         }
      }
   }

   class="kw">protected boolean probeDirection(@Nonnull Ref<EntityStore> var1, int var2, @Nonnull ExecutionSupport var3, @Nonnull ComponentAccessor<EntityStore> var4) {
      int var5 = this.walkDirections[var2];
      MotionController var6 = var3.getMotionContextSupport().getActiveMotionController();
      float var7 = this.toAngle(var1, var5, var4);
      this.computeTargetPosition(var1, var7, this.desiredWalkDistance, var4);
      double var8 = this.constrainMove(var1, var3, this.probePosition, this.targetPosition, this.desiredWalkDistance, var4);
      if (var8 < 1.0E-5) {
         class="kw">return false;
      }

      if (var8 < this.desiredWalkDistance) {
         this.probeDirection.mul(var8 / this.desiredWalkDistance);
      }

      if (var6.willReceiveBlockDamage()) {
         this.probeMoveData.getRelaxedConstraints().add(RelaxedConstraint.DAMAGE);
      }

      double var10 = var6.probeMove(var1, this.probePosition, this.probeDirection, this.probeMoveData, var4);
      if (var10 < 1.0E-5) {
         class="kw">return false;
      }

      this.walkDistances[var2] = var10;
      if (var10 < this.desiredWalkDistance) {
         class="kw">return false;
      }

      if (var10 < var8) {
         this.probeDirection.mul(var10 / var8);
      }

      this.walkDistance = var10;
      this.walkHeading = var7;
      this.targetPosition.set(this.probePosition).add(this.probeDirection);
      class="kw">return true;
   }

   class="kw">private void computeTargetPosition(@Nonnull Ref<EntityStore> var1, float var2, double var3, @Nonnull ComponentAccessor<EntityStore> var5) {
      TransformComponent var6 = var5.getComponent(var1, TransformComponent.getComponentType());
      assert var6 != null;
      this.probePosition.set(var6.getPosition());
      this.probeDirection.x = PhysicsMath.headingX(var2) * var3;
      this.probeDirection.y = this.probeDY * var3 / this.desiredWalkDistance;
      this.probeDirection.z = PhysicsMath.headingZ(var2) * var3;
      this.targetPosition.set(this.probePosition).add(this.probeDirection);
   }

   class="kw">protected float toAngle(@Nonnull Ref<EntityStore> var1, int var2, @Nonnull ComponentAccessor<EntityStore> var3) {
      TransformComponent var4 = var3.getComponent(var1, TransformComponent.getComponentType());
      assert var4 != null;
      class="kw">return PhysicsMath.normalizeAngle(var4.getRotation().yaw() + var2 * (float) (Math.PI / 16) + this.angleOffset);
   }

   class="kw">private int addPreOrderedDirection(int var1, int var2) {
      this.preOrderedDirections[var2++] = (byte)var1;
      if (var1 != 0 && var1 != 16) {
         this.preOrderedDirections[var2++] = (byte)(32 - var1);
      }

      class="kw">return var2;
   }

   class="kw">public enum State {
      SEARCHING,
      TURNING,
      WALKING,
      STOPPED;

      State() {
      }
   }
}