help/PathExecutor.java

package xyz.nodrop.farmingtools.client.pathfinding;

import net.minecraft.client.MinecraftClient;
import net.minecraft.client.network.ClientPlayerEntity;
import net.minecraft.util.math.BlockPos;
import net.minecraft.util.math.Vec3d;
import net.minecraft.world.RaycastContext;
import net.minecraft.util.hit.BlockHitResult;
import net.minecraft.util.hit.HitResult;

import java.util.List;

public class PathExecutor {
    private static final double SPRINT_ANGLE = 35.0;   // max yaw diff to allow sprint

    // ── Pola ──────────────────────────────────────────────────────────────────
    private float yawVelocity     = 0f;
    private float pitchVelocity   = 0f;
    private Vec3d smoothLookPoint = null;
    private boolean flying = false;

    // ── Stałe ─────────────────────────────────────────────────────────────────
    private static final float SPRING_STIFFNESS  = 0.18f;
    private static final float SPRING_DAMPING    = 0.72f;
    private static final float MAX_VELOCITY      = 18.0f;
    private static final float PITCH_LOOK_AHEAD  = 6.0f;
    private static final float LOOK_POINT_LERP   = 0.08f;
    private volatile float prevYaw   = 0f;
    private volatile float prevPitch = 0f;
    private volatile float nextYaw   = 0f;
    private volatile float nextPitch = 0f;

    private float cameraYaw        = 0f;
    private float cameraPitch      = 0f;
    private boolean cameraInitialized = false;

    private List<BlockPos> path;
    private int waypointIndex = 0;
    private boolean active = false;

    // Read by MixinKeyboardInput each tick
    private volatile PathInput currentInput = PathInput.NONE;

    // ── Public API ─────────────────────────────────────────────────────────

    public synchronized void start(List<BlockPos> newPath) {
        this.path          = newPath;
        this.waypointIndex = 0;
        this.active        = !newPath.isEmpty();
        this.currentInput  = PathInput.NONE;
    }

    public synchronized void stop(MinecraftClient client) {
        active        = false;
        currentInput  = PathInput.NONE;
        yawVelocity   = 0f;  // ← dodaj
        pitchVelocity = 0f;  // ← dodaj
        smoothLookPoint = null;  // ← dodaj
        if (client.player != null) client.player.setSprinting(false);
    }

    public synchronized boolean isActive()        { return active; }
    public synchronized int     getWaypointIndex(){ return waypointIndex; }
    public synchronized int     getPathLength()   { return path != null ? path.size() : 0; }
    public synchronized BlockPos getGoal() {
        return (path != null && !path.isEmpty()) ? path.get(path.size() - 1) : null;
    }

    public synchronized List<BlockPos> getPath()  { return path; }

    public PathInput getCurrentInput() {
        return active ? currentInput : null;
    }

    public synchronized void tick(MinecraftClient client) {
        if (!active || path == null || client.player == null) {
            currentInput = PathInput.NONE;
            return;
        }

        // ── Włącz/wyłącz latanie ──────────────────────────────────────────────
        if (flying) {
            client.player.getAbilities().flying = true;
            client.player.getAbilities().allowFlying = true;
        }

        advanceWaypoints(client.player);

        if (waypointIndex >= path.size()) {
            stop(client);
            return;
        }

        BlockPos target = path.get(waypointIndex);
        rotateCamera(client.player, target, client);
        currentInput = computeInput(client.player, target);

        boolean shouldSprint = currentInput.sprint()
                && !currentInput.jumping()
                && (flying || client.player.isOnGround()); // ← flying nie wymaga gruntu
        client.player.setSprinting(shouldSprint);
    }

    private void rotateCamera(ClientPlayerEntity player, BlockPos target, MinecraftClient client) {
        if (!cameraInitialized) {
            cameraYaw         = player.getYaw();
            cameraPitch       = player.getPitch();
            cameraInitialized = true;
        }

        BlockPos lookTarget = chooseLookTarget(player, client);
        Vec3d rawLookPoint  = Vec3d.ofCenter(lookTarget).add(0, 0.5, 0);

        if (smoothLookPoint == null) {
            smoothLookPoint = rawLookPoint;
        } else {
            smoothLookPoint = smoothLookPoint.lerp(rawLookPoint, LOOK_POINT_LERP);
        }

        Vec3d playerPos = new Vec3d(player.getX(), player.getEyeY(), player.getZ());
        double dx = smoothLookPoint.x - playerPos.x;
        double dz = smoothLookPoint.z - playerPos.z;
        double dy = smoothLookPoint.y - playerPos.y;

        float targetYaw   = (float) Math.toDegrees(Math.atan2(-dx, dz));
        float targetPitch = (float) -Math.toDegrees(
                Math.atan2(dy, Math.sqrt(dx * dx + dz * dz)));
        targetPitch = Math.max(-90f, Math.min(90f, targetPitch - PITCH_LOOK_AHEAD));

        float yawError  = (float) angleDiff(targetYaw, cameraYaw);
        float yawAccel  = SPRING_STIFFNESS * yawError - SPRING_DAMPING * yawVelocity;
        yawVelocity    += yawAccel;
        yawVelocity     = Math.max(-MAX_VELOCITY, Math.min(MAX_VELOCITY, yawVelocity));

        float pitchError  = targetPitch - cameraPitch;
        float pitchAccel  = SPRING_STIFFNESS * pitchError - SPRING_DAMPING * pitchVelocity;
        pitchVelocity    += pitchAccel;
        pitchVelocity     = Math.max(-MAX_VELOCITY, Math.min(MAX_VELOCITY, pitchVelocity));

        prevYaw   = cameraYaw;
        prevPitch = cameraPitch;

        cameraYaw   += yawVelocity;
        cameraPitch += pitchVelocity;
        cameraPitch  = Math.max(-90f, Math.min(90f, cameraPitch));

        nextYaw   = cameraYaw;
        nextPitch = cameraPitch;

        player.setYaw(cameraYaw);
        player.setPitch(cameraPitch);
    }
    public float getInterpolatedYaw(float tickDelta) {
        float diff = (float) angleDiff(nextYaw, prevYaw);
        return prevYaw + diff * tickDelta;
    }
    public float getInterpolatedPitch(float tickDelta) {
        return prevPitch + (nextPitch - prevPitch) * tickDelta;
    }


    private BlockPos chooseLookTarget(ClientPlayerEntity player, MinecraftClient client) {
        Vec3d eyePos = player.getEyePos();

        int LOOKAHEAD_STEPS = 3;
        float MIN_LOOKAHEAD_DIST = 4.0f;
        for (int i = LOOKAHEAD_STEPS; i >= 1; i--) {
            int idx = Math.min(waypointIndex + i, path.size() - 1);
            BlockPos candidate = path.get(idx);
            Vec3d candidateVec = Vec3d.ofCenter(candidate).add(0, 0.5, 0);

            if (eyePos.distanceTo(candidateVec) < MIN_LOOKAHEAD_DIST) continue;

            BlockHitResult hit = client.world.raycast(new RaycastContext(
                    eyePos,
                    candidateVec,
                    RaycastContext.ShapeType.COLLIDER,
                    RaycastContext.FluidHandling.NONE,
                    player
            ));

            if (hit.getType() == HitResult.Type.MISS) {
                return candidate;
            }
        }

        return path.get(waypointIndex);
    }

    // ── Internal ───────────────────────────────────────────────────────────

    private void advanceWaypoints(ClientPlayerEntity player) {
        while (waypointIndex < path.size()) {
            Vec3d center = Vec3d.ofCenter(path.get(waypointIndex));
            Vec3d pPos   = player.getEntityPos();

            double dist;
            double reach;

            if (flying) {
                dist  = pPos.distanceTo(center);           // pełny 3D
                reach = 2.5;                               // większy reach w locie
            } else {
                dist  = horizontalDist(pPos, center);      // poziomy 2D
                reach = Math.min(1.8, 0.55 + dist * 0.15);
            }

            if (dist < reach) waypointIndex++;
            else break;
        }
    }

    private PathInput computeInput(ClientPlayerEntity player, BlockPos target) {
        Vec3d playerPos = new Vec3d(player.getX(), player.getY(), player.getZ());
        Vec3d targetPos = Vec3d.ofCenter(target);

        double dx = targetPos.x - playerPos.x;
        double dz = targetPos.z - playerPos.z;

        double targetYaw = Math.toDegrees(Math.atan2(-dx, dz));
        double diff      = angleDiff(targetYaw, player.getYaw());

        double rad      = Math.toRadians(diff);
        float  forward  = (float) Math.cos(rad);
        float  sideways = (float) -Math.sin(rad);

        if (Math.abs(forward)  < 0.05f) forward  = 0;
        if (Math.abs(sideways) < 0.05f) sideways = 0;

        // ── Oblicz mnożnik prędkości ──────────────────────────────────────────
        float speedMult = 1.0f;

        // 1. Hamowanie przed celem (ostatnie N bloków)z
        boolean isLastWaypoint = (waypointIndex == path.size() - 1);
       // if (flying) {
            //double distToTarget = playerPos.distanceTo(targetPos);
           // if (isLastWaypoint) {
                // Hamuj płynnie na ostatnich 6 blokach
                //speedMult *= (float) Math.min(1.0, distToTarget / 0.1);
         //   }
        //} else {
            //double distToTarget = horizontalDist(playerPos, targetPos);
            //if (isLastWaypoint) {
                //speedMult *= (float) Math.min(1.0, distToTarget / 4.0);
            //}
        //}

        // 2. Hamowanie przed zakrętem — patrz na kąt do NASTĘPNEGO waypointu
        if (waypointIndex + 1 < path.size()) {
            BlockPos nextTarget = path.get(waypointIndex + 1);
            Vec3d nextPos = Vec3d.ofCenter(nextTarget);

            double ndx = nextPos.x - targetPos.x;
            double ndz = nextPos.z - targetPos.z;
            double nextYaw = Math.toDegrees(Math.atan2(-ndx, ndz));
            double turnAngle = Math.abs(angleDiff(nextYaw, targetYaw));

            // Im większy zakręt tym mocniej hamuj (powyżej 30° zaczyna hamować)
            if (turnAngle > 30.0) {
                float turnMult = (float) Math.max(0.3, 1.0 - (turnAngle - 30.0) / 120.0);

                // Hamuj tylko gdy jesteś blisko zakrętu
                double distToTurn = flying
                        ? playerPos.distanceTo(targetPos)
                        : horizontalDist(playerPos, targetPos);

                if (distToTurn < 5.0) {
                    speedMult *= turnMult;
                }
            }
        }

        // 3. Hamowanie przy zmianie wysokości w locie
        float upward   = 0f;
        boolean jumping  = false;
        boolean sneaking = false;

        if (flying) {
            double dy = targetPos.y - playerPos.y;
            if      (dy >  0.4) upward =  1.0f;
            else if (dy < -0.4) upward = -1.0f;

            // Jeśli duża zmiana wysokości — zwolnij ruch poziomy
            if (Math.abs(dy) > 3.0) {
                speedMult *= 0.5f;
            }
        } else {
            jumping = target.getY() > Math.floor(playerPos.y) + 0.5;
        }

        // Clamp speedMult żeby nie był ujemny
        speedMult = Math.max(0.0f, Math.min(1.0f, speedMult));

        // Zastosuj mnożnik do forward/sideways
        forward  *= speedMult;
        sideways *= speedMult;

        boolean sprint = !isLastWaypoint
                && Math.abs(diff) < SPRINT_ANGLE
                && forward > 0.5f
                && speedMult > 0.7f; // nie sprintuj gdy hamujesz

        return new PathInput(forward, sideways, upward, jumping, sprint, sneaking);
    }

    // ── Utilities ──────────────────────────────────────────────────────────

    private static double horizontalDist(Vec3d a, Vec3d b) {
        double dx = a.x - b.x;
        double dz = a.z - b.z;
        return Math.sqrt(dx * dx + dz * dz);
    }

    private static double angleDiff(double target, double current) {
        double diff = target - current;
        while (diff >  180) diff -= 360;
        while (diff < -180) diff += 360;
        return diff;
    }

    public boolean isFlying() {
        return flying;
    }

    public void setFlying(boolean flying) {
        this.flying = flying;
    }
}