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

class="kw">import com.hypixel.hytale.component.ComponentAccessor;
class="kw">import com.hypixel.hytale.component.Ref;
class="kw">import com.hypixel.hytale.math.vector.Vector3dUtil;
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.movement.Steering;
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.util.NPCPhysicsMath;
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 PathFollower {
   @Nullable
   class="kw">protected IWaypoint currentWaypoint;
   class="kw">protected double currentWaypointDistanceSquared;
   class="kw">protected PathFollower.FrozenWaypoint frozenWaypoint;
   class="kw">protected boolean isWaypointFrozen;
   class="kw">protected class="kw">final Vector3d lastWaypointPosition = new Vector3d();
   class="kw">protected class="kw">final Vector3d direction = new Vector3d();
   class="kw">protected class="kw">final Vector3d tempVector = new Vector3d();
   class="kw">protected class="kw">final Vector3d tempPath = new Vector3d();
   class="kw">protected class="kw">final Vector3d projection = new Vector3d();
   class="kw">protected class="kw">final Vector3d rejection = new Vector3d();
   class="kw">protected int pathSmoothing;
   class="kw">protected double blendHeading;
   class="kw">protected double relativeSpeed;
   class="kw">protected double relativeSpeedWaypoint;
   class="kw">protected double waypointRadius;
   class="kw">protected double rejectionWeight = 3.0;
   class="kw">protected double waypointRadiusSquared;
   class="kw">protected boolean debugNodes = false;
   class="kw">protected boolean shouldSmoothPath = true;

   class="kw">public PathFollower() {
   }

   class="kw">public void setPathSmoothing(int var1) {
      this.pathSmoothing = var1;
   }

   class="kw">public double getRelativeSpeed() {
      class="kw">return this.relativeSpeed;
   }

   class="kw">public void setRelativeSpeed(double var1) {
      this.relativeSpeed = var1;
   }

   class="kw">public void setRelativeSpeedWaypoint(double var1) {
      this.relativeSpeedWaypoint = var1;
   }

   class="kw">public void setWaypointRadius(double var1) {
      this.waypointRadius = var1;
      this.waypointRadiusSquared = var1 * var1;
   }

   class="kw">public void setDebugNodes(boolean var1) {
      this.debugNodes = var1;
   }

   class="kw">public boolean shouldSmoothPath() {
      class="kw">return this.shouldSmoothPath;
   }

   class="kw">public void setRejectionWeight(double var1) {
      this.rejectionWeight = var1;
   }

   class="kw">public void setBlendHeading(double var1) {
      if (var1 < 0.0 || var1 > 1.0) {
         var1 = 0.0;
      }

      this.blendHeading = var1;
   }

   @Nullable
   class="kw">public IWaypoint getCurrentWaypoint() {
      class="kw">return this.currentWaypoint;
   }

   @Nullable
   class="kw">public Vector3d getCurrentWaypointPosition() {
      class="kw">return this.currentWaypoint == null ? null : this.currentWaypoint.getPosition();
   }

   @Nullable
   class="kw">public IWaypoint getNextWaypoint() {
      class="kw">return this.currentWaypoint == null ? null : this.currentWaypoint.next();
   }

   @Nullable
   class="kw">public Vector3d getNextWaypointPosition() {
      IWaypoint var1 = this.getNextWaypoint();
      class="kw">return var1 == null ? null : var1.getPosition();
   }

   class="kw">public void setPath(IWaypoint var1, @Nonnull Vector3d var2) {
      this.currentWaypoint = var1;
      this.lastWaypointPosition.set(var2);
      this.currentWaypointDistanceSquared = Double.MAX_VALUE;
      this.shouldSmoothPath = true;
      this.isWaypointFrozen = false;
   }

   class="kw">public void clearPath() {
      this.currentWaypoint = null;
      this.isWaypointFrozen = false;
   }

   class="kw">public boolean pathInFinalStage() {
      if (this.currentWaypoint == null) {
         class="kw">return true;
      }

      if (this.currentWaypoint.next() != null) {
         class="kw">return false;
      }

      this.freezeWaypoint();
      class="kw">return true;
   }

   class="kw">public boolean freezeWaypoint() {
      if (this.currentWaypoint == null) {
         class="kw">return false;
      }

      if (this.frozenWaypoint == null) {
         this.frozenWaypoint = new PathFollower.FrozenWaypoint();
      }

      if (this.currentWaypoint == this.frozenWaypoint) {
         class="kw">return true;
      }

      this.frozenWaypoint.position.set(this.currentWaypoint.getPosition());
      this.currentWaypoint = this.frozenWaypoint;
      this.isWaypointFrozen = true;
      class="kw">return true;
   }

   class="kw">public boolean isWaypointFrozen() {
      class="kw">return this.isWaypointFrozen;
   }

   class="kw">public void setWaypointFrozen(boolean var1) {
      this.isWaypointFrozen = var1;
   }

   class="kw">public void executePath(@Nonnull Vector3d var1, @Nonnull MotionController var2, @Nonnull Steering var3) {
      Vector3d var4 = this.getCurrentWaypointPosition();
      if (var4 != null) {
         this.tempVector.set(var4).sub(var1);
         double var5 = this.tempVector.length();
         var3.setMaxDistance(var5);
         if (var5 > this.waypointRadius) {
            this.direction.set(this.tempVector);
            this.computeRejection(var1, var4, var2);
            this.direction.sub(this.rejection);
            var3.setTranslation(this.direction.mul(this.relativeSpeed / var5));
         } else {
            if (var5 > 0.1) {
               this.direction.set(this.tempVector);
            }

            var3.setTranslation(this.direction.mul(this.relativeSpeedWaypoint / var5));
         }
      }
   }

   class="kw">public void computeRejection(@Nonnull Vector3d var1, @Nonnull Vector3d var2, @Nonnull MotionController var3) {
      this.tempPath.set(var2).sub(this.lastWaypointPosition).mul(var3.getComponentSelector());
      this.tempVector.set(var1).sub(this.lastWaypointPosition).mul(var3.getComponentSelector());
      double var4 = this.tempPath.lengthSquared();
      double var6 = this.tempPath.dot(this.tempVector);
      this.projection.set(this.tempPath).mul(var6 / var4);
      this.rejection.set(this.tempVector).sub(this.projection).mul(this.rejectionWeight);
   }

   class="kw">public boolean updateCurrentTarget(@Nonnull Vector3d var1, @Nonnull MotionController var2) {
      if (this.currentWaypoint == null) {
         class="kw">return false;
      }

      Vector3d var3 = this.currentWaypoint.getPosition();
      double var4 = var2.waypointDistanceSquared(var3, var1);
      double var6 = 0.0;
      boolean var8;
      if (!(var4 <= 1.0000000000000002E-10) && (!(var4 < 0.01) || !(var4 > this.currentWaypointDistanceSquared))) {
         var6 = NPCPhysicsMath.dotProduct(var3, this.lastWaypointPosition, var1, var2.getComponentSelector());
         var8 = var6 < 0.0;
      } else {
         var8 = true;
      }

      this.currentWaypointDistanceSquared = var4;
      if (this.debugNodes) {
         NPCPlugin.get()
            .getLogger()
            .at(Level.INFO)
            .log(
               "=== Target len=%s before=%s targetdist=%s  proj=%s pos=%s tgt=%s",
               this.currentWaypoint.getLength(),
               !var8,
               Math.sqrt(var4),
               var6,
               Vector3dUtil.formatShortString(var1),
               Vector3dUtil.formatShortString(var3)
            );
      }

      if (var8) {
         this.lastWaypointPosition.set(var3);
         this.currentWaypoint = this.currentWaypoint.next();
         if (this.currentWaypoint == null) {
            this.isWaypointFrozen = false;
            class="kw">return false;
         }

         this.currentWaypointDistanceSquared = Double.MAX_VALUE;
         this.shouldSmoothPath = true;
         var3 = this.currentWaypoint.getPosition();
         if (this.blendHeading > 0.0) {
            var4 = var2.waypointDistanceSquared(var3, var1);
         }
      }

      var2.requirePreciseMovement(var3);
      if (this.blendHeading <= 0.0) {
         class="kw">return true;
      }

      var2.enableHeadingBlending();
      if (var4 > this.waypointRadiusSquared) {
         class="kw">return true;
      }

      IWaypoint var9 = this.getNextWaypoint();
      if (var9 == null) {
         class="kw">return true;
      }

      this.tempVector.set(var9.getPosition()).sub(var3);
      var4 = NPCPhysicsMath.projectedLengthSquared(this.tempVector, var2.getComponentSelector());
      if (var4 < 0.001) {
         class="kw">return true;
      }

      float var10 = PhysicsMath.headingFromDirection(this.tempVector.x, this.tempVector.z);
      var2.enableHeadingBlending(var10, var3, this.blendHeading);
      class="kw">return true;
   }

   class="kw">public void smoothPath(
      @Nonnull Ref<EntityStore> var1,
      @Nonnull Vector3d var2,
      @Nonnull MotionController var3,
      @Nonnull ProbeMoveData var4,
      @Nonnull ComponentAccessor<EntityStore> var5
   ) {
      this.shouldSmoothPath = false;
      if (this.pathSmoothing > 0) {
         IWaypoint var6 = this.currentWaypoint;
         if (var6 != null) {
            int var7 = var6.getLength();

            IWaypoint var8;
            int var10;
            do {
               var8 = var6;
               int var9 = var8.getLength();
               if (var9 == 1) {
                  var10 = 0;
                  break;
               }

               var10 = Math.min(this.pathSmoothing, var9 - 1);
               var6 = var8.advance(var10);
            } while (this.canMoveTo(var1, var3, var2, var6.getPosition(), var4, var5));

            while (var10 > 1) {
               int var11 = var10 / 2;
               IWaypoint var12 = var8.advance(var11);
               if (this.canMoveTo(var1, var3, var2, var12.getPosition(), var4, var5)) {
                  var8 = var12;
                  var10 -= var11;
               } else {
                  var10 = var11;
               }
            }

            if (this.debugNodes) {
               int var13 = var8.getLength();
               NPCPlugin.get()
                  .getLogger()
                  .at(Level.INFO)
                  .log(
                     "=== New Target len=%s skipped=%s pos=%s tgt=%s dist=%s",
                     var13,
                     var7 - var13,
                     Vector3dUtil.formatShortString(var2),
                     Vector3dUtil.formatShortString(var8.getPosition()),
                     var2.distance(var8.getPosition())
                  );
            }

            this.currentWaypoint = var8;
            this.currentWaypointDistanceSquared = Double.MAX_VALUE;
         }
      }
   }

   class="kw">protected boolean canMoveTo(
      @Nonnull Ref<EntityStore> var1,
      @Nonnull MotionController var2,
      @Nonnull Vector3d var3,
      @Nonnull Vector3d var4,
      @Nonnull ProbeMoveData var5,
      @Nonnull ComponentAccessor<EntityStore> var6
   ) {
      var5.setPosition(var3).setTargetPosition(var4);
      class="kw">return var5.canMoveTo(var1, var2, 9.999999994736442E-8, 0.5, var6);
   }

   class="kw">private class="kw">static class FrozenWaypoint class="kw">implements IWaypoint {
      class="kw">protected class="kw">final Vector3d position = new Vector3d();

      class="kw">private FrozenWaypoint() {
      }

      @Override
      class="kw">public int getLength() {
         class="kw">return 1;
      }

      @Nonnull
      @Override
      class="kw">public Vector3d getPosition() {
         class="kw">return this.position;
      }

      @Nullable
      @Override
      class="kw">public IWaypoint advance(int var1) {
         class="kw">return null;
      }

      @Nullable
      @Override
      class="kw">public IWaypoint next() {
         class="kw">return null;
      }
   }
}