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

class="kw">import com.hypixel.hytale.component.CommandBuffer;
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.entity.UUIDComponent;
class="kw">import com.hypixel.hytale.server.core.modules.entity.component.TransformComponent;
class="kw">import com.hypixel.hytale.server.core.modules.physics.component.Velocity;
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.role.Role;
class="kw">import com.hypixel.hytale.server.npc.util.NPCPhysicsMath;
class="kw">import java.util.UUID;
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 SteeringForceAvoidCollision class="kw">extends SteeringForceWithGroup {
   class="kw">private class="kw">final Vector3d selfVelocity = new Vector3d();
   class="kw">private double selfRadius;
   class="kw">private double collisionTime = Double.MAX_VALUE;
   class="kw">private class="kw">final Vector3d colliderPosition = new Vector3d();
   class="kw">private class="kw">final double[] tempTime = new double[2];
   class="kw">private class="kw">final Vector3d tempPos = new Vector3d();
   class="kw">private class="kw">final Vector3d tempVel = new Vector3d();
   class="kw">private double maxDistance;
   class="kw">private double falloff = 2.0;
   class="kw">private double strength = 1.0;
   class="kw">private Role.AvoidanceMode avoidanceMode = Role.AvoidanceMode.Slowdown;
   class="kw">private Ref<EntityStore> selfReference;
   @Nullable
   class="kw">private Ref<EntityStore> otherReference;
   class="kw">private double velocity;
   class="kw">private double maxTime;
   class="kw">private boolean canSlowDown;
   class="kw">private boolean overlap;
   @Nonnull
   class="kw">protected class="kw">final Vector3d lastSteeringDirection = new Vector3d();
   class="kw">private boolean debug;

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

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

   class="kw">public Role.AvoidanceMode getAvoidanceMode() {
      class="kw">return this.avoidanceMode;
   }

   class="kw">public void setAvoidanceMode(Role.AvoidanceMode var1) {
      this.avoidanceMode = var1;
   }

   @Override
   class="kw">public void setSelf(@Nonnull Ref<EntityStore> var1, @Nonnull Vector3d var2, @Nonnull ComponentAccessor<EntityStore> var3) {
      this.setSelf(var1, var2, null, -1.0, var3);
   }

   class="kw">public void setSelf(
      @Nonnull Ref<EntityStore> var1, @Nonnull Vector3d var2, @Nullable Vector3d var3, double var4, @Nonnull ComponentAccessor<EntityStore> var6
   ) {
      super.setSelf(var1, var2, var6);
      this.selfReference = var1;
      this.otherReference = null;
      if (var3 != null) {
         this.selfVelocity.set(var3);
      } else {
         this.setVelocityFromEntity(this.selfReference, var6);
      }

      if (var4 < 0.0) {
         this.setRadiusFromEntity(this.selfReference, var6);
      } else {
         this.selfRadius = var4;
      }
   }

   @Override
   class="kw">public void reset() {
      this.collisionTime = Double.MAX_VALUE;
      this.otherReference = null;
      this.canSlowDown = true;
      this.overlap = false;
      double var1 = this.selfVelocity.lengthSquared();
      if (var1 > 0.001) {
         this.velocity = Math.sqrt(var1);
         this.maxTime = this.maxDistance / this.velocity;
      } else {
         this.velocity = 0.0;
         this.maxTime = 0.0;
      }
   }

   @Override
   class="kw">public boolean compute(@Nonnull Steering var1) {
      this.lastSteeringDirection.zero();
      if (this.velocity == 0.0) {
         class="kw">return false;
      }

      if (super.compute(var1)) {
         double var2 = var1.getTranslation().length();
         if (var2 > 1.0E-4) {
            double var4 = this.collisionTime * this.selfVelocity.length();
            if (var4 <= this.maxDistance) {
               class="kw">switch (this.avoidanceMode) {
                  case Slowdown:
                     this.canSlowDown = true;
                     break;
                  case Evade:
                     this.canSlowDown = this.overlap;
                  case Any:
               }

               if (this.canSlowDown) {
                  double var6 = Math.pow(var4 / this.maxDistance, 1.0 / this.falloff);
                  var1.scaleTranslation(var6);
                  if (this.debug) {
                     NPCPlugin.get().getLogger().at(Level.INFO).log("--> Avoidance slowdown=%s dist=%s maxDist=%s", var6, var4, this.maxDistance);
                  }
               } else {
                  this.tempPos.set(this.colliderPosition).sub(this.selfPosition);
                  NPCPhysicsMath.rejection(this.selfVelocity, this.tempPos, this.tempVel);
                  this.tempVel.negate();
                  if (this.tempVel.lengthSquared() < 0.001) {
                     this.selfVelocity.cross(Vector3dUtil.UP, this.tempVel);
                     if (this.tempVel.lengthSquared() < 0.001) {
                        this.selfVelocity.cross(Vector3dUtil.RIGHT, this.tempVel);
                     }
                  }

                  double var8 = Math.pow(1.0 - var4 / this.maxDistance, 1.0 / this.falloff);
                  this.tempVel.normalize(var2 * var8 * this.strength).mul(this.componentSelector);
                  this.lastSteeringDirection.set(this.tempVel);
                  var1.scaleTranslation(1.0 - var8);
                  var1.getTranslation().add(this.tempVel).normalize(var2);
                  if (this.debug) {
                     NPCPlugin.get().getLogger().at(Level.INFO).log("--> Avoidance dist=%.2f l=%.2f s=%.2f maxDist=%.2f", var4, var2, var8, this.maxDistance);
                  }

                  if (!var1.getTranslation().isFinite()) {
                     if (this.debug) {
                        NPCPlugin.get().getLogger().at(Level.WARNING).log("Denormalized avoidance steering dist=%s l=%s s=%s", var4, var2, var8);
                     }

                     var1.clearTranslation();
                     class="kw">return false;
                  }
               }

               class="kw">return true;
            }
         }
      }

      class="kw">return false;
   }

   @Override
   class="kw">public void add(@Nonnull Ref<EntityStore> var1, @Nonnull CommandBuffer<EntityStore> var2) {
      if (this.velocity != 0.0 && this.collisionTime != 0.0) {
         TransformComponent var3 = var2.getComponent(var1, TransformComponent.getComponentType());
         assert var3 != null;
         Vector3d var4 = var3.getPosition();
         UUIDComponent var5 = var2.getComponent(var1, UUIDComponent.getComponentType());
         assert var5 != null;
         UUID var6 = var5.getUuid();
         this.tempPos.set(var4);
         boolean var7 = this.tempVel.set(this.tempPos).sub(this.selfPosition).dot(this.selfVelocity) <= 0.0;
         if (var7) {
            if (this.debug) {
               NPCPlugin.get().getLogger().at(Level.INFO).log("Avoidance add: Entity %s - Moving away, ignoring", var6);
            }
         } else {
            double var8 = NPCPhysicsMath.collisionSphereRadius(var1, var2);
            double var10 = this.selfRadius + var8;
            this.overlap = this.selfPosition.distanceSquared(this.tempPos) <= var10 * var10;
            if (this.overlap) {
               this.collisionTime = 0.0;
               this.canSlowDown = true;
               if (this.debug) {
                  NPCPlugin.get().getLogger().at(Level.INFO).log("Avoidance add: Overlap with %s - Stopping", var6);
               }
            } else {
               Velocity var12 = var2.getComponent(var1, Velocity.getComponentType());
               var12.assignVelocityTo(this.tempVel);
               int var13 = NPCPhysicsMath.intersectSweptSpheresFootpoint(
                  this.selfPosition, this.selfVelocity, this.selfRadius, this.tempPos, this.tempVel, var8, Vector3dUtil.ALL_ONES, this.tempTime
               );
               if (this.debug && var13 > 0) {
                  NPCPlugin.get()
                     .getLogger()
                     .at(Level.INFO)
                     .log(
                        "Avoidance add: Solutions with %s=%s, time=[%s, %s], maxTime=%s, collisionTime=%s",
                        var6,
                        var13,
                        this.tempTime[0],
                        this.tempTime[1],
                        this.maxTime,
                        this.collisionTime
                     );
               }

               if (var13 != 0 && !(this.tempTime[0] < 0.0) && !(this.tempTime[0] > this.maxTime) && !(this.tempTime[0] >= this.collisionTime)) {
                  double var14 = this.tempVel.length();
                  double var16 = var14 > 0.0 ? this.selfVelocity.dot(this.tempVel) / (var14 * this.velocity) : 0.0;
                  boolean var18 = var16 < -0.8;
                  if (this.debug && var13 > 0) {
                     NPCPlugin.get()
                        .getLogger()
                        .at(Level.INFO)
                        .log(
                           "Avoidance add: New solution with %s, time=[%s, %s], maxTime=%s, antiParallel=%s, dot=%s, departing=%s, collisionTime=%s",
                           var6,
                           this.tempTime[0],
                           this.tempTime[1],
                           this.maxTime,
                           var18,
                           var16,
                           var7,
                           this.collisionTime
                        );
                  }

                  this.colliderPosition.set(var4);
                  this.collisionTime = this.tempTime[0];
                  this.otherReference = var1;
                  this.canSlowDown = !var18 && this.otherReference.getIndex() < this.selfReference.getIndex();
               }
            }
         }
      }
   }

   class="kw">public void setVelocityFromEntity(@Nonnull Ref<EntityStore> var1, @Nonnull ComponentAccessor<EntityStore> var2) {
      Velocity var3 = var2.getComponent(var1, Velocity.getComponentType());
      var3.assignVelocityTo(this.selfVelocity);
   }

   class="kw">public void setRadiusFromEntity(@Nonnull Ref<EntityStore> var1, @Nonnull ComponentAccessor<EntityStore> var2) {
      this.selfRadius = NPCPhysicsMath.collisionSphereRadius(var1, var2);
   }

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

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

   class="kw">public void setSelfVelocity(@Nonnull Vector3d var1) {
      this.selfVelocity.set(var1);
   }

   @Nonnull
   class="kw">public Vector3d getSelfVelocity() {
      class="kw">return this.selfVelocity;
   }

   class="kw">public double getSelfRadius() {
      class="kw">return this.selfRadius;
   }

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

   class="kw">public double getStrength() {
      class="kw">return this.strength;
   }

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

   @Nonnull
   class="kw">public Vector3d getLastSteeringDirection() {
      class="kw">return this.lastSteeringDirection;
   }
}