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

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

import java.util.function.Supplier;
import com.hypixel.hytale.server.core.universe.world.path.IPathWaypoint;
import com.hypixel.hytale.server.npc.role.support.WorldSupport;
import com.hypixel.hytale.server.npc.movement.controllers.MotionController;
import com.hypixel.hytale.server.core.universe.world.path.IPath;
import com.hypixel.hytale.math.random.RandomExtra;
import com.hypixel.hytale.server.npc.util.NPCPhysicsMath;
import com.hypixel.hytale.math.util.MathUtil;
import com.hypixel.hytale.server.core.modules.entity.component.TransformComponent;
import java.util.Random;
import it.unimi.dsi.fastutil.ints.IntLists;
import java.util.concurrent.ThreadLocalRandom;
import com.hypixel.hytale.server.npc.sensorinfo.IPathProvider;
import com.hypixel.hytale.server.npc.movement.Steering;
import com.hypixel.hytale.server.npc.sensorinfo.InfoProvider;
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 it.unimi.dsi.fastutil.ints.IntArrayList;
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.world.builders.BuilderBodyMotionPath;
import com.hypixel.hytale.server.npc.movement.steeringforces.SteeringForcePursue;
import com.hypixel.hytale.server.npc.movement.steeringforces.SteeringForceRotate;
import it.unimi.dsi.fastutil.ints.IntList;
import com.hypixel.hytale.math.vector.Vector3d;
import javax.annotation.Nullable;
import com.hypixel.hytale.server.npc.corecomponents.BodyMotionBase;

public class BodyMotionPath extends BodyMotionBase
{
    public static final double MIN_GUARD_POINT_WAIT_TIME = 1.0;
    public static final boolean TESTING = false;
    protected final Shape shape;
    protected final double pathWidth;
    protected final double nodeWidth;
    protected final double minRelativeSpeed;
    protected final double maxRelativeSpeed;
    protected final double minWalkDistance;
    protected final double maxWalkDistance;
    protected final boolean startAtNearestNode;
    protected final Direction direction;
    protected final double minNodeDelay;
    protected final double maxNodeDelay;
    protected final int viewSegments;
    protected final boolean useNodeViewDirection;
    protected final boolean pickRandomAngle;
    protected final double minDelayScale;
    protected final double maxDelayScale;
    protected final double minPercentage;
    protected final double maxPercentage;
    protected int currentWaypointIndex;
    @Nullable
    protected Direction currentDirection;
    protected final Vector3d currentWaypointPosition;
    protected final Vector3d lastWaypointPosition;
    protected final IntList visitOrder;
    protected int visitIndex;
    protected final SteeringForceRotate steeringForceRotate;
    protected final SteeringForcePursue steeringForcePursue;
    protected double currentSpeed;
    protected final Vector3d currentPosition;
    protected final Vector3d nextPosition;
    protected boolean nextPositionValid;
    protected double currentNodeDelay;
    protected boolean pendingNodeDelay;
    protected boolean rotatingToView;
    protected float nodeViewDirection;
    protected double nodeWaitTime;
    protected float observationSector;
    protected double currentObservationDelay;
    protected boolean rotating;
    protected final Vector3d previousSteeringTranslation;
    protected int currentViewSegment;
    
    public BodyMotionPath(@Nonnull final BuilderBodyMotionPath builder, @Nonnull final BuilderSupport support) {
        super(builder);
        this.currentWaypointIndex = -1;
        this.currentWaypointPosition = new Vector3d();
        this.lastWaypointPosition = new Vector3d();
        this.visitOrder = new IntArrayList();
        this.steeringForceRotate = new SteeringForceRotate();
        this.steeringForcePursue = new SteeringForcePursue();
        this.currentPosition = new Vector3d();
        this.nextPosition = new Vector3d();
        this.previousSteeringTranslation = new Vector3d(Vector3d.MIN);
        this.shape = builder.getShape(support);
        this.pathWidth = builder.getPathWidth();
        this.nodeWidth = builder.getNodeWidth();
        this.minRelativeSpeed = builder.getMinRelativeSpeed();
        this.maxRelativeSpeed = builder.getMaxRelativeSpeed();
        this.minWalkDistance = builder.getMinWalkDistance();
        this.maxWalkDistance = builder.getMaxWalkDistance();
        this.startAtNearestNode = builder.isStartAtNearestNode();
        this.direction = builder.getDirection();
        this.minNodeDelay = builder.getMinNodeDelay();
        this.maxNodeDelay = builder.getMaxNodeDelay();
        this.useNodeViewDirection = builder.isUseNodeViewDirection();
        final double[] delayScaleRange = builder.getDelayScaleRange(support);
        this.minDelayScale = delayScaleRange[0];
        this.maxDelayScale = delayScaleRange[1];
        final double[] delayPercentRange = builder.getPercentDelayRange(support);
        this.minPercentage = delayPercentRange[0];
        this.maxPercentage = delayPercentRange[1];
        this.pickRandomAngle = builder.isPickRandomAngle();
        this.viewSegments = builder.getViewSegments(support);
    }
    
    @Override
    public void activate(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.reset();
    }
    
    @Override
    public void loaded(final Role role) {
        this.invalidateWaypoint();
        this.reset();
    }
    
    @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) {
        desiredSteering.clear();
        if (!role.getActiveMotionController().canAct(ref, componentAccessor)) {
            return true;
        }
        final IPathProvider info = sensorInfo.getExtraInfo(IPathProvider.class);
        if (info == null || !info.hasPath()) {
            return false;
        }
        final IPath<?> path = info.getPath();
        final int numWaypoints = path.length();
        if (this.visitOrder.size() != numWaypoints) {
            this.visitOrder.clear();
            for (int i = 0; i < numWaypoints; ++i) {
                this.visitOrder.add(i);
            }
            IntLists.shuffle(this.visitOrder, ThreadLocalRandom.current());
            this.visitIndex = 0;
            this.invalidateWaypoint();
        }
        final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
        assert transformComponent != null;
        final Vector3d position = transformComponent.getPosition();
        if (this.currentWaypointIndex == -1) {
            if (!this.getFirstWaypoint(ref, role, path, position, componentAccessor)) {
                return false;
            }
            this.nextPositionValid = false;
            this.currentNodeDelay = 0.0;
        }
        final float heading = transformComponent.getRotation().getYaw();
        if (this.currentNodeDelay > 0.0) {
            this.currentNodeDelay -= dt;
            if (this.observationSector != 0.0f || numWaypoints == 1) {
                this.steeringForceRotate.setHeading(heading);
                if (this.steeringForceRotate.compute(desiredSteering)) {
                    return true;
                }
                desiredSteering.setYaw(heading);
                if (this.tickObservationDelay(dt)) {
                    return true;
                }
                this.pickNextObservationAngle();
            }
            return true;
        }
        final MotionController activeMotionController = role.getActiveMotionController();
        final Vector3d componentSelector = activeMotionController.getComponentSelector();
        final WorldSupport worldSupport = role.getWorldSupport();
        this.currentPosition.assign(position.getX(), position.getY(), position.getZ());
        final int lastIndex = this.currentWaypointIndex;
        this.lastWaypointPosition.assign(this.currentWaypointPosition);
        while (this.closeToPosition(this.currentWaypointPosition, activeMotionController)) {
            if (this.nextPositionValid || numWaypoints == 1) {
                final IPathWaypoint wayPoint = (IPathWaypoint)path.get(this.currentWaypointIndex);
                if (wayPoint == null) {
                    return false;
                }
                this.nodeViewDirection = wayPoint.getWaypointRotation(componentAccessor).getYaw();
                this.nodeWaitTime = wayPoint.getPauseTime();
                this.observationSector = wayPoint.getObservationAngle() / 2.0f;
                this.currentViewSegment = 0;
                if (numWaypoints == 1) {
                    this.currentNodeDelay = MathUtil.maxValue(this.nodeWaitTime, 1.0);
                    this.pickNextObservationAngle();
                    desiredSteering.setYaw(heading);
                    return true;
                }
            }
            this.nextPositionValid = false;
            if (!this.nextWayPoint(path, worldSupport, componentAccessor)) {
                if (worldSupport.hasRequestedNewPath()) {
                    desiredSteering.setTranslation(this.previousSteeringTranslation);
                }
                return false;
            }
            if (this.currentWaypointIndex == lastIndex) {
                return false;
            }
        }
        if (!this.nextPositionValid || this.closeToPosition(this.nextPosition, activeMotionController)) {
            if (this.pathWidth == 0.0) {
                this.nextPosition.assign(this.currentWaypointPosition);
            }
            else {
                final double maxDistance = NPCPhysicsMath.dotProduct(this.currentWaypointPosition, this.lastWaypointPosition, this.currentPosition, componentSelector);
                final double distance = Math.min(RandomExtra.randomRange(this.minWalkDistance, this.maxWalkDistance), maxDistance);
                if (distance >= maxDistance - this.nodeWidth) {
                    this.nextPosition.assign(this.currentWaypointPosition);
                }
                else {
                    NPCPhysicsMath.orthoComposition(this.lastWaypointPosition, this.currentWaypointPosition, distance, Vector3d.UP, RandomExtra.randomRange(-this.pathWidth / 2.0, this.pathWidth / 2.0), this.nextPosition);
                }
            }
            this.nextPositionValid = true;
            this.currentSpeed = RandomExtra.randomRange(this.minRelativeSpeed, this.maxRelativeSpeed);
            this.steeringForcePursue.setTargetPosition(this.nextPosition);
            this.steeringForcePursue.setDistances(this.nodeWidth * 1.0, 0.1);
            this.steeringForcePursue.setComponentSelector(componentSelector);
            if ((this.useNodeViewDirection && this.minNodeDelay > 0.0) || this.nodeWaitTime > 0.0) {
                this.pickNextObservationAngle();
                this.rotatingToView = true;
                this.pendingNodeDelay = false;
            }
            else {
                this.steeringForceRotate.setDesiredHeading(NPCPhysicsMath.lookatHeading(this.currentPosition, this.nextPosition, heading));
                this.pendingNodeDelay = (this.minNodeDelay > 0.0);
            }
            this.rotating = true;
        }
        if (this.rotating) {
            this.steeringForceRotate.setHeading(heading);
            if (this.useNodeViewDirection && this.rotatingToView) {
                if (this.steeringForceRotate.compute(desiredSteering)) {
                    return true;
                }
                desiredSteering.setYaw(heading);
                if (this.tickObservationDelay(dt)) {
                    return true;
                }
                this.rotatingToView = false;
                this.steeringForceRotate.setDesiredHeading(NPCPhysicsMath.lookatHeading(this.currentPosition, this.nextPosition, heading));
                if (this.minNodeDelay > 0.0) {
                    this.currentNodeDelay = RandomExtra.randomRange(this.minNodeDelay, this.maxNodeDelay);
                }
                if (this.nodeWaitTime > this.currentNodeDelay) {
                    this.currentNodeDelay = this.nodeWaitTime;
                }
                if (this.currentNodeDelay > 0.0) {
                    this.pickNextObservationAngle();
                }
                return true;
            }
            else {
                if (this.steeringForceRotate.compute(desiredSteering)) {
                    return true;
                }
                if (this.pendingNodeDelay) {
                    this.currentNodeDelay = RandomExtra.randomRange(this.minNodeDelay, this.maxNodeDelay);
                    this.pendingNodeDelay = false;
                    return true;
                }
                this.rotating = false;
            }
        }
        this.steeringForcePursue.setSelfPosition(this.currentPosition);
        this.steeringForcePursue.setComponentSelector(activeMotionController.getComponentSelector());
        this.nextPositionValid = this.steeringForcePursue.compute(desiredSteering);
        if (desiredSteering.hasTranslation()) {
            desiredSteering.scaleTranslation(this.currentSpeed);
        }
        this.previousSteeringTranslation.assign(desiredSteering.getTranslation());
        return true;
    }
    
    protected boolean tickObservationDelay(final double dt) {
        if (this.currentObservationDelay > 0.0) {
            this.currentObservationDelay -= dt;
            return true;
        }
        return false;
    }
    
    protected void pickNextObservationAngle() {
        if (this.pickRandomAngle) {
            final float angle = RandomExtra.randomRange(-this.observationSector, this.observationSector);
            this.steeringForceRotate.setDesiredHeading(this.nodeViewDirection + angle);
        }
        else if (this.viewSegments > 1) {
            final float fullSector = this.observationSector * 2.0f;
            final float start = this.nodeViewDirection - this.observationSector;
            final float segment = fullSector / (this.viewSegments - 1);
            final int thisSegment = this.currentViewSegment++;
            this.currentViewSegment %= this.viewSegments;
            this.steeringForceRotate.setDesiredHeading(start + thisSegment * segment);
        }
        else {
            this.steeringForceRotate.setDesiredHeading(this.nodeViewDirection + this.observationSector);
            this.observationSector *= -1.0f;
        }
        this.currentObservationDelay = this.nodeWaitTime * RandomExtra.randomRange(this.minDelayScale, this.maxDelayScale);
        this.currentObservationDelay += this.currentObservationDelay * RandomExtra.randomRange(this.minPercentage, this.maxPercentage);
    }
    
    protected boolean closeToPosition(final Vector3d position, @Nonnull final MotionController motionController) {
        return motionController.waypointDistanceSquared(this.currentPosition, position) <= this.nodeWidth * this.nodeWidth;
    }
    
    protected void invalidateWaypoint() {
        this.currentWaypointIndex = -1;
        this.currentDirection = null;
    }
    
    protected boolean nextWayPoint(@Nonnull final IPath<?> path, @Nonnull final WorldSupport support, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        if (this.currentWaypointIndex == -1) {
            return false;
        }
        final int numWaypoints = path.length();
        switch (this.shape.ordinal()) {
            case 0:
            case 1:
            case 3: {
                if (this.direction == Direction.RANDOM || this.currentDirection == null) {
                    this.currentDirection = (RandomExtra.randomBoolean() ? Direction.FORWARD : Direction.BACKWARD);
                }
                this.currentWaypointIndex += ((this.currentDirection == Direction.FORWARD) ? 1 : -1);
                if (this.currentWaypointIndex >= 0 && this.currentWaypointIndex < numWaypoints) {
                    break;
                }
                if (this.shape == Shape.LOOP) {
                    this.currentWaypointIndex = (this.currentWaypointIndex + numWaypoints) % numWaypoints;
                    break;
                }
                if (this.currentWaypointIndex < 0) {
                    if (this.shape == Shape.CHAIN) {
                        this.currentWaypointIndex = -1;
                        support.requestNewPath();
                        return false;
                    }
                    this.currentWaypointIndex = 1;
                    this.currentDirection = Direction.FORWARD;
                    break;
                }
                else {
                    if (this.currentWaypointIndex < numWaypoints) {
                        break;
                    }
                    if (this.shape == Shape.CHAIN) {
                        this.currentWaypointIndex = -1;
                        support.requestNewPath();
                        return false;
                    }
                    this.currentWaypointIndex = numWaypoints - 2;
                    this.currentDirection = Direction.BACKWARD;
                    break;
                }
                break;
            }
            case 2: {
                if (this.direction == Direction.RANDOM) {
                    int index = RandomExtra.randomRange(numWaypoints);
                    if (index == this.currentWaypointIndex) {
                        index = (index + RandomExtra.randomRange(2) * 2 - 1 + numWaypoints) % numWaypoints;
                    }
                    this.currentWaypointIndex = index;
                }
                else {
                    ++this.visitIndex;
                    if (this.visitIndex >= this.visitOrder.size()) {
                        this.visitIndex = 0;
                        IntLists.shuffle(this.visitOrder, ThreadLocalRandom.current());
                    }
                    if (this.visitOrder.getInt(this.visitIndex) == this.currentWaypointIndex) {
                        ++this.visitIndex;
                    }
                    this.currentWaypointIndex = this.visitOrder.getInt(this.visitIndex);
                }
                this.currentDirection = Direction.FORWARD;
                break;
            }
        }
        this.waypointIndexUpdated(path, componentAccessor);
        return true;
    }
    
    protected boolean getFirstWaypoint(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nullable final IPath<?> path, @Nonnull final Vector3d lastPos, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.invalidateWaypoint();
        if (path == null || path.length() == 0) {
            return false;
        }
        this.initializeCurrentDirection();
        if (this.startAtNearestNode) {
            double distanceSquared = Double.MAX_VALUE;
            final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
            assert transformComponent != null;
            final Vector3d pos = transformComponent.getPosition();
            final MotionController activeMotionController = role.getActiveMotionController();
            for (int i = 0; i < path.length(); ++i) {
                final IPathWaypoint pathWaypoint = (IPathWaypoint)path.get(i);
                if (pathWaypoint == null) {
                    return false;
                }
                final double distance = activeMotionController.waypointDistanceSquared(pos, pathWaypoint.getWaypointPosition(componentAccessor));
                if (distance < distanceSquared) {
                    this.currentWaypointIndex = i;
                    distanceSquared = distance;
                }
            }
            switch (this.shape.ordinal()) {
                case 0:
                case 3: {
                    if (this.currentWaypointIndex == 0) {
                        this.currentDirection = Direction.FORWARD;
                        break;
                    }
                    if (this.currentWaypointIndex == path.length() - 1) {
                        this.currentDirection = Direction.BACKWARD;
                        break;
                    }
                    break;
                }
                case 2: {
                    if (this.direction != Direction.RANDOM) {
                        IntLists.shuffle(this.visitOrder, ThreadLocalRandom.current());
                        this.visitIndex = this.visitOrder.indexOf(this.currentWaypointIndex);
                        break;
                    }
                    break;
                }
            }
        }
        else {
            switch (this.shape.ordinal()) {
                case 0:
                case 3: {
                    this.currentWaypointIndex = ((this.currentDirection == Direction.FORWARD) ? 0 : (path.length() - 1));
                    break;
                }
                case 1: {
                    this.currentWaypointIndex = 0;
                    break;
                }
                case 2: {
                    if (this.direction != Direction.RANDOM) {
                        IntLists.shuffle(this.visitOrder, ThreadLocalRandom.current());
                        this.visitIndex = 0;
                        this.currentWaypointIndex = this.visitOrder.getInt(this.visitIndex);
                        break;
                    }
                    this.currentWaypointIndex = RandomExtra.randomRange(path.length());
                    break;
                }
            }
        }
        this.waypointIndexUpdated(path, componentAccessor);
        this.lastWaypointPosition.assign(lastPos.getX(), lastPos.getY(), lastPos.getZ());
        return true;
    }
    
    protected void waypointIndexUpdated(@Nonnull final IPath<?> path, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final IPathWaypoint pathWaypoint = (IPathWaypoint)path.get(this.currentWaypointIndex);
        if (pathWaypoint == null) {
            return;
        }
        final Vector3d pathWaypointPosition = pathWaypoint.getWaypointPosition(componentAccessor);
        this.currentWaypointPosition.assign(pathWaypointPosition);
    }
    
    protected void initializeCurrentDirection() {
        if (this.direction == Direction.RANDOM || this.direction == Direction.ANY) {
            this.currentDirection = (RandomExtra.randomBoolean() ? Direction.FORWARD : Direction.BACKWARD);
        }
        else {
            this.currentDirection = this.direction;
        }
    }
    
    protected void reset() {
        this.pendingNodeDelay = false;
        this.currentNodeDelay = 0.0;
        this.nextPositionValid = false;
        this.currentViewSegment = 0;
        this.nodeWaitTime = 0.0;
        this.rotating = false;
    }
    
    public enum Direction implements Supplier<String>
    {
        FORWARD("Start visiting nodes in order"), 
        BACKWARD("Start visiting nodes in reverse order "), 
        RANDOM("Can change direction between nodes and randomly pick target node in Points shape mode"), 
        ANY("Pick any start direction");
        
        private final String description;
        
        private Direction(final String description) {
            this.description = description;
        }
        
        @Override
        public String get() {
            return this.description;
        }
    }
    
    public enum Shape implements Supplier<String>
    {
        LINE("Nodes form an open path of line segments"), 
        LOOP("Nodes form a closed loop of line segments (last node leads to first node)"), 
        POINTS("Any path between nodes is possible"), 
        CHAIN("Nodes form an open path of line segments and will chain together with the next nearest path upon reaching the final node");
        
        private final String description;
        
        private Shape(final String description) {
            this.description = description;
        }
        
        @Override
        public String get() {
            return this.description;
        }
    }
}