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

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

import com.hypixel.hytale.server.core.modules.physics.util.PhysicsMath;
import com.hypixel.hytale.server.npc.movement.Steering;
import javax.annotation.Nullable;
import com.hypixel.hytale.server.npc.sensorinfo.InfoProvider;
import com.hypixel.hytale.server.npc.entities.NPCEntity;
import com.hypixel.hytale.math.util.MathUtil;
import com.hypixel.hytale.server.core.modules.entity.component.TransformComponent;
import com.hypixel.hytale.server.npc.role.RoleDebugFlags;
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 com.hypixel.hytale.server.npc.corecomponents.builders.BuilderBodyMotionBase;
import javax.annotation.Nonnull;
import com.hypixel.hytale.server.npc.corecomponents.debug.builders.BuilderBodyMotionTestProbe;
import com.hypixel.hytale.server.npc.movement.controllers.ProbeMoveData;
import com.hypixel.hytale.math.vector.Vector3d;
import com.hypixel.hytale.server.npc.corecomponents.BodyMotionBase;

public class BodyMotionTestProbe extends BodyMotionBase
{
    protected final double adjustX;
    protected final double adjustZ;
    protected final double adjustDistance;
    protected final float snapAngle;
    protected boolean displayText;
    protected final Vector3d direction;
    protected final ProbeMoveData probeMoveData;
    
    public BodyMotionTestProbe(@Nonnull final BuilderBodyMotionTestProbe builderBodyMotionTestProbe) {
        super(builderBodyMotionTestProbe);
        this.direction = new Vector3d();
        (this.probeMoveData = new ProbeMoveData()).setSaveSegments(true);
        this.adjustX = builderBodyMotionTestProbe.getAdjustX();
        this.adjustZ = builderBodyMotionTestProbe.getAdjustZ();
        this.adjustDistance = builderBodyMotionTestProbe.getAdjustDistance();
        this.snapAngle = builderBodyMotionTestProbe.getSnapAngle() * 0.017453292f;
        this.probeMoveData.setAvoidingBlockDamage(builderBodyMotionTestProbe.isAvoidingBlockDamage());
        this.probeMoveData.setRelaxedMoveConstraints(builderBodyMotionTestProbe.isRelaxedMoveConstraints());
    }
    
    @Override
    public void activate(@Nonnull final Ref<EntityStore> ref, @Nonnull final Role role, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        this.displayText = role.getDebugSupport().isDebugFlagSet(RoleDebugFlags.DisplayCustom);
        if (this.adjustX < 0.0 && this.adjustZ < 0.0) {
            return;
        }
        final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
        if (transformComponent == null) {
            return;
        }
        final Vector3d position = transformComponent.getPosition();
        double x = position.x;
        double z = position.z;
        if (this.adjustX >= 0.0) {
            x = MathUtil.fastFloor(x) + this.adjustX;
        }
        if (this.adjustZ >= 0.0) {
            z = MathUtil.fastFloor(z) + this.adjustZ;
        }
        final NPCEntity npcComponent = componentAccessor.getComponent(ref, NPCEntity.getComponentType());
        assert npcComponent != null;
        npcComponent.moveTo(ref, x, position.y, z, componentAccessor);
    }
    
    @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 (sensorInfo == null || !sensorInfo.getPositionProvider().providePosition(this.direction)) {
            return false;
        }
        final TransformComponent transformComponent = componentAccessor.getComponent(ref, TransformComponent.getComponentType());
        if (transformComponent == null) {
            return false;
        }
        final Vector3d position = transformComponent.getPosition();
        this.direction.subtract(position);
        if (!this.displayText) {
            return true;
        }
        double length = this.direction.length();
        if (length <= 1.0E-6) {
            return true;
        }
        if (this.adjustDistance > 0.0) {
            length = this.adjustDistance;
            this.direction.setLength(this.adjustDistance);
        }
        final double x = this.direction.getX();
        final double y = this.direction.getY();
        final double z = this.direction.getZ();
        float yaw = PhysicsMath.normalizeTurnAngle(PhysicsMath.headingFromDirection(x, z));
        final float pitch = PhysicsMath.pitchFromDirection(x, y, z);
        if (this.snapAngle > 0.0f && this.snapAngle < 3.1415927f) {
            yaw = MathUtil.fastRound(yaw / this.snapAngle) * this.snapAngle;
            PhysicsMath.vectorFromAngles(yaw, pitch, this.direction).setLength(length);
        }
        desiredSteering.setYaw(yaw);
        desiredSteering.setPitch(pitch);
        final double distance = role.getActiveMotionController().probeMove(ref, position, this.direction, this.probeMoveData, componentAccessor);
        role.getDebugSupport().setDisplayCustomString(String.format("PR: %.2f DX: %.2f DZ: %.2f", distance, this.direction.x, this.direction.z));
        return true;
    }
}