Herobrine-Rewoven/src/main/java/com/thebrokenrail/herobrine/entity/ai/stage/IdleStage.java

package com.thebrokenrail.herobrine.entity.ai.stage;

import com.thebrokenrail.herobrine.config.HardcodedConfig;
import com.thebrokenrail.herobrine.entity.HerobrineEntity;
import com.thebrokenrail.herobrine.entity.ai.AIStage;
import net.minecraft.nbt.CompoundTag;
import net.minecraft.server.network.ServerPlayerEntity;
import net.minecraft.util.math.MathHelper;
import net.minecraft.util.math.Vec3d;

public class IdleStage extends AIStage {
    private int time = 0;

    public IdleStage(HerobrineEntity entity) {
        super(entity);
    }

    @Override
    public void tick() {
        if (time < HardcodedConfig.HEROBRINE_MINIMUM_IDLE_TIME) {
            time++;
        } else if (getEntity().getRandom().nextDouble() < HardcodedConfig.HEROBRINE_IDLE_END_CHANCE) {
            nextStage();
            return;
        }

        ServerPlayerEntity nearest = getEntity().getNearestPlayer();

        boolean moving = false;
        if (nearest != null) {
            float maxDiff = Math.abs(target(getTargetYaw(nearest.getPos()), getTargetPitch(nearest.getPos())));

            if (maxDiff < HardcodedConfig.HEROBRINE_FOLLOW_ANGLE_THRESHOLD) {
                Vec3d diff = getEntity().getRotationVec(1.0f);
                diff = diff.multiply(HardcodedConfig.HEROBRINE_FLY_SPEED);

                float distance = Math.abs(getEntity().distanceTo(nearest));
                boolean go = false;
                if (distance > (HardcodedConfig.HEROBRINE_RECOMMENDED_DISTANCE_CENTER + HardcodedConfig.HEROBRINE_RECOMMENDED_DISTANCE_DIFF)) {
                    go = true;
                } else if (distance < (HardcodedConfig.HEROBRINE_RECOMMENDED_DISTANCE_CENTER - HardcodedConfig.HEROBRINE_RECOMMENDED_DISTANCE_DIFF)) {
                    go = true;
                    // Prefer Moving On X And Z When Backing Away
                    diff = new Vec3d(diff.getX(), 0, diff.getZ()).multiply(-1);
                }
                if (go) {
                    getEntity().addVelocity(diff.getX(), diff.getY(), diff.getZ());
                    moving = true;
                }
            }
        }
        if (!moving) {
            getEntity().setVelocity(getEntity().getVelocity().multiply(0.25d));
            getEntity().velocityDirty = true;
        }
    }

    private Vec3d getPosVec() {
        return getEntity().getPos();
    }

    private static final double RAD2DEG = 180d / Math.PI;

    private float getTargetPitch(Vec3d targetPos) {
        Vec3d pos = getPosVec();
        double diffX = targetPos.getX() - pos.getX();
        double diffY = targetPos.getY() - pos.getY();
        double diffZ = targetPos.getZ() - pos.getZ();
        double g = MathHelper.sqrt(diffX * diffX + diffZ * diffZ);
        return (float) -(MathHelper.atan2(diffY, g) * RAD2DEG);
    }

    private float getTargetYaw(Vec3d targetPos) {
        Vec3d pos = getPosVec();
        double diffX = targetPos.getX() - pos.getX();
        double diffZ = targetPos.getZ() - pos.getZ();
        return (float) (MathHelper.atan2(diffZ, diffX) * RAD2DEG) - 90f;
    }

    private static final float ROTATION_INCREMENT = 4;

    private void change(float diffYaw, float diffPitch) {
        getEntity().yaw = diffYaw;
        getEntity().headYaw = diffYaw;
        getEntity().pitch = diffPitch;
    }

    private float changeAngle(float from, float to, float max) {
        float f = MathHelper.subtractAngles(from, to);
        float g = MathHelper.clamp(f, -max, max);
        return from + g;
    }

    private float[] getAngleChange(float from, float to) {
        float diff = MathHelper.subtractAngles(from, to);
        float diff2 = changeAngle(from, to, ROTATION_INCREMENT);
        return new float[]{diff2, diff};
    }

    private float target(float targetYaw, float targetPitch) {
        float[] yawChange = getAngleChange(getEntity().headYaw, targetYaw);
        float[] pitchChange = getAngleChange(getEntity().pitch, targetPitch);
        change(yawChange[0], pitchChange[0]);
        return Math.max(yawChange[1], pitchChange[1]);
    }

    @Override
    public void fromTag(CompoundTag tag) {
        time = tag.getInt("Time");
    }

    @Override
    public CompoundTag toTag() {
        CompoundTag tag = new CompoundTag();
        tag.putInt("Time", time);
        return tag;
    }
}