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

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

import java.util.UUID;
import com.hypixel.hytale.server.core.modules.physics.component.Velocity;
import com.hypixel.hytale.server.core.entity.UUIDComponent;
import com.hypixel.hytale.server.core.modules.entity.component.TransformComponent;
import com.hypixel.hytale.component.CommandBuffer;
import com.hypixel.hytale.server.npc.util.NPCPhysicsMath;
import java.util.logging.Level;
import com.hypixel.hytale.server.npc.NPCPlugin;
import com.hypixel.hytale.server.npc.movement.Steering;
import com.hypixel.hytale.component.ComponentAccessor;
import javax.annotation.Nonnull;
import javax.annotation.Nullable;
import com.hypixel.hytale.server.core.universe.world.storage.EntityStore;
import com.hypixel.hytale.component.Ref;
import com.hypixel.hytale.server.npc.role.Role;
import com.hypixel.hytale.math.vector.Vector3d;

public class SteeringForceAvoidCollision extends SteeringForceWithGroup
{
    private final Vector3d selfVelocity;
    private double selfRadius;
    private double collisionTime;
    private final Vector3d colliderPosition;
    private final double[] tempTime;
    private final Vector3d tempPos;
    private final Vector3d tempVel;
    private double maxDistance;
    private double falloff;
    private double strength;
    private Role.AvoidanceMode avoidanceMode;
    private Ref<EntityStore> selfReference;
    @Nullable
    private Ref<EntityStore> otherReference;
    private double velocity;
    private double maxTime;
    private boolean canSlowDown;
    private boolean overlap;
    @Nonnull
    protected final Vector3d lastSteeringDirection;
    private boolean debug;
    
    public SteeringForceAvoidCollision() {
        this.selfVelocity = new Vector3d();
        this.collisionTime = Double.MAX_VALUE;
        this.colliderPosition = new Vector3d();
        this.tempTime = new double[2];
        this.tempPos = new Vector3d();
        this.tempVel = new Vector3d();
        this.falloff = 2.0;
        this.strength = 1.0;
        this.avoidanceMode = Role.AvoidanceMode.Slowdown;
        this.lastSteeringDirection = new Vector3d();
    }
    
    public void setDebug(final boolean debug) {
        this.debug = debug;
    }
    
    public Role.AvoidanceMode getAvoidanceMode() {
        return this.avoidanceMode;
    }
    
    public void setAvoidanceMode(final Role.AvoidanceMode avoidanceMode) {
        this.avoidanceMode = avoidanceMode;
    }
    
    @Override
    public void setSelf(@Nonnull final Ref<EntityStore> ref, @Nonnull final Vector3d position, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.setSelf(ref, position, null, -1.0, componentAccessor);
    }
    
    public void setSelf(@Nonnull final Ref<EntityStore> ref, @Nonnull final Vector3d position, @Nullable final Vector3d velocity, final double radius, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        super.setSelf(ref, position, componentAccessor);
        this.selfReference = ref;
        this.otherReference = null;
        if (velocity != null) {
            this.selfVelocity.assign(velocity);
        }
        else {
            this.setVelocityFromEntity(this.selfReference, componentAccessor);
        }
        if (radius < 0.0) {
            this.setRadiusFromEntity(this.selfReference, componentAccessor);
        }
        else {
            this.selfRadius = radius;
        }
    }
    
    @Override
    public void reset() {
        this.collisionTime = Double.MAX_VALUE;
        this.otherReference = null;
        this.canSlowDown = true;
        this.overlap = false;
        final double velocitySquared = this.selfVelocity.squaredLength();
        if (velocitySquared > 0.001) {
            this.velocity = Math.sqrt(velocitySquared);
            this.maxTime = this.maxDistance / velocitySquared;
        }
        else {
            this.velocity = 0.0;
            this.maxTime = 0.0;
        }
    }
    
    @Override
    public boolean compute(@Nonnull final Steering output) {
        this.lastSteeringDirection.assign(Vector3d.ZERO);
        if (this.velocity == 0.0) {
            return false;
        }
        if (super.compute(output)) {
            final double l = output.getTranslation().length();
            if (l > 1.0E-4) {
                final double distance = this.collisionTime * this.selfVelocity.length();
                if (distance <= this.maxDistance) {
                    switch (this.avoidanceMode) {
                        case Slowdown: {
                            this.canSlowDown = true;
                            break;
                        }
                        case Evade: {
                            this.canSlowDown = this.overlap;
                            break;
                        }
                    }
                    if (this.canSlowDown) {
                        final double s = Math.pow(distance / this.maxDistance, 1.0 / this.falloff);
                        output.scaleTranslation(s);
                        if (this.debug) {
                            NPCPlugin.get().getLogger().at(Level.INFO).log("--> Avoidance slowdown=%s dist=%s maxDist=%s", s, distance, this.maxDistance);
                        }
                    }
                    else {
                        this.tempPos.assign(this.colliderPosition).subtract(this.selfPosition);
                        NPCPhysicsMath.rejection(this.selfVelocity, this.tempPos, this.tempVel);
                        this.tempVel.negate();
                        if (this.tempVel.squaredLength() < 0.001) {
                            this.selfVelocity.cross(Vector3d.UP, this.tempVel);
                            if (this.tempVel.squaredLength() < 0.001) {
                                this.selfVelocity.cross(Vector3d.RIGHT, this.tempVel);
                            }
                        }
                        final double s = Math.pow(1.0 - distance / this.maxDistance, 1.0 / this.falloff);
                        this.tempVel.setLength(l * s * this.strength).scale(this.componentSelector);
                        this.lastSteeringDirection.assign(this.tempVel);
                        output.scaleTranslation(1.0 - s);
                        output.getTranslation().add(this.tempVel).setLength(l);
                        if (this.debug) {
                            NPCPlugin.get().getLogger().at(Level.INFO).log("--> Avoidance dist=%.2f l=%.2f s=%.2f maxDist=%.2f", distance, l, s, this.maxDistance);
                        }
                        if (!output.getTranslation().isFinite()) {
                            if (this.debug) {
                                NPCPlugin.get().getLogger().at(Level.WARNING).log("Denormalized avoidance steering dist=%s l=%s s=%s", distance, l, s);
                            }
                            output.clearTranslation();
                            return false;
                        }
                    }
                    return true;
                }
            }
        }
        return false;
    }
    
    @Override
    public void add(@Nonnull final Ref<EntityStore> ref, @Nonnull final CommandBuffer<EntityStore> commandBuffer) {
        if (this.velocity == 0.0 || this.collisionTime == 0.0) {
            return;
        }
        final TransformComponent transformComponent = commandBuffer.getComponent(ref, TransformComponent.getComponentType());
        assert transformComponent != null;
        final Vector3d position = transformComponent.getPosition();
        final UUIDComponent uuidComponent = commandBuffer.getComponent(ref, UUIDComponent.getComponentType());
        assert uuidComponent != null;
        final UUID uuid = uuidComponent.getUuid();
        this.tempPos.assign(position);
        final boolean departing = this.tempVel.assign(this.tempPos).subtract(this.selfPosition).dot(this.selfVelocity) <= 0.0;
        if (departing) {
            if (this.debug) {
                NPCPlugin.get().getLogger().at(Level.INFO).log("Avoidance add: Entity %s - Moving away, ignoring", uuid);
            }
            return;
        }
        final double entityRadius = NPCPhysicsMath.collisionSphereRadius(ref, commandBuffer);
        final double sumRadius = this.selfRadius + entityRadius;
        this.overlap = (this.selfPosition.distanceSquaredTo(this.tempPos) <= sumRadius * sumRadius);
        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", uuid);
            }
            return;
        }
        final Velocity velocityComponent = commandBuffer.getComponent(ref, Velocity.getComponentType());
        velocityComponent.assignVelocityTo(this.tempVel);
        final int solutions = NPCPhysicsMath.intersectSweptSpheresFootpoint(this.selfPosition, this.selfVelocity, this.selfRadius, this.tempPos, this.tempVel, entityRadius, Vector3d.ALL_ONES, this.tempTime);
        if (this.debug && solutions > 0) {
            NPCPlugin.get().getLogger().at(Level.INFO).log("Avoidance add: Solutions with %s=%s, time=[%s, %s], maxTime=%s, collisionTime=%s", uuid, solutions, this.tempTime[0], this.tempTime[1], this.maxTime, this.collisionTime);
        }
        if (solutions == 0 || this.tempTime[0] < 0.0 || this.tempTime[0] > this.maxTime || this.tempTime[0] >= this.collisionTime) {
            return;
        }
        final double tempVelocity = this.tempVel.length();
        final double dot = (tempVelocity > 0.0) ? (this.selfVelocity.dot(this.tempVel) / (tempVelocity * this.velocity)) : 0.0;
        final boolean antiParallel = dot < -0.8;
        if (this.debug && solutions > 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", uuid, this.tempTime[0], this.tempTime[1], this.maxTime, antiParallel, dot, departing, this.collisionTime);
        }
        this.colliderPosition.assign(position);
        this.collisionTime = this.tempTime[0];
        this.otherReference = ref;
        this.canSlowDown = (!antiParallel && this.otherReference.getIndex() < this.selfReference.getIndex());
    }
    
    public void setVelocityFromEntity(@Nonnull final Ref<EntityStore> ref, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final Velocity velocityComponent = componentAccessor.getComponent(ref, Velocity.getComponentType());
        velocityComponent.assignVelocityTo(this.selfVelocity);
    }
    
    public void setRadiusFromEntity(@Nonnull final Ref<EntityStore> ref, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.selfRadius = NPCPhysicsMath.collisionSphereRadius(ref, componentAccessor);
    }
    
    public void setMaxDistance(final double distance) {
        this.maxDistance = distance;
    }
    
    public void setFalloff(final double falloff) {
        this.falloff = falloff;
    }
    
    public void setSelfVelocity(@Nonnull final Vector3d selfVelocity) {
        this.selfVelocity.assign(selfVelocity);
    }
    
    @Nonnull
    public Vector3d getSelfVelocity() {
        return this.selfVelocity;
    }
    
    public double getSelfRadius() {
        return this.selfRadius;
    }
    
    public void setSelfRadius(final double selfRadius) {
        this.selfRadius = selfRadius;
    }
    
    public double getStrength() {
        return this.strength;
    }
    
    public void setStrength(final double strength) {
        this.strength = strength;
    }
    
    @Nonnull
    public Vector3d getLastSteeringDirection() {
        return this.lastSteeringDirection;
    }
}