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.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Random;
import java.util.Set;

public class PathExecutor {

    // ══════════════════════════════════════════════════════════════════════════
    // STAŁE
    // ══════════════════════════════════════════════════════════════════════════

    private static final double SPRINT_ANGLE    = 35.0;   // max kąt yaw do sprintu
    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;

    // Lookahead
    private static final int   HOLD_TICKS     = 10;
    private static final float NEAR_THRESHOLD = 3.5f;

    // Stuck detection
    private static final int   STUCK_TICKS    = 26;   // ~1.3s przy 20 TPS
    private static final float STUCK_MIN_DIST = 0.3f;

    // ══════════════════════════════════════════════════════════════════════════
    // INTERPOLACJA KAMERY (atomic frame dla render threadu)
    // ══════════════════════════════════════════════════════════════════════════

    private record YawPitchFrame(float prevYaw, float prevPitch,
                                 float nextYaw, float nextPitch) {}

    private volatile YawPitchFrame frame = new YawPitchFrame(0, 0, 0, 0);

    // ══════════════════════════════════════════════════════════════════════════
    // STAN KAMERY
    // ══════════════════════════════════════════════════════════════════════════

    private float   cameraYaw         = 0f;
    private float   cameraPitch       = 0f;
    private boolean cameraInitialized = false;
    private float   currentMoveYaw    = 0f;  // yaw do aktualnego waypointu (nie lookahead)

    private float yawVelocity   = 0f;
    private float pitchVelocity = 0f;
    private Vec3d smoothLookPoint = null;

    // Lookahead state
    private int lookTargetIndex = 0;
    private int lookHoldTicks   = 0;

    // ══════════════════════════════════════════════════════════════════════════
    // STAN PATHFINDINGU
    // ══════════════════════════════════════════════════════════════════════════

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

    private volatile PathInput currentInput = PathInput.NONE;

    // Waypoint offsety (randomizacja pozycji w bloku)
    private final Random              rng             = new Random();
    private final Map<BlockPos, Vec3d> waypointOffsets = new HashMap<>();

    // ══════════════════════════════════════════════════════════════════════════
    // STUCK DETECTION
    // ══════════════════════════════════════════════════════════════════════════

    private Vec3d lastStuckCheckPos = null;
    private int   stuckTickCounter  = 0;
    private final Set<BlockPos> blacklistedNodes = new HashSet<>();

    // ══════════════════════════════════════════════════════════════════════════
    // PUBLIC API
    // ══════════════════════════════════════════════════════════════════════════

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

    /** Zatrzymaj pathfinding — blacklista zostaje (potrzebna przy retry po stucku). */
    public synchronized void stop(MinecraftClient client) {
        active            = false;
        currentInput      = PathInput.NONE;
        yawVelocity       = 0f;
        pitchVelocity     = 0f;
        smoothLookPoint   = null;
        cameraInitialized = false;
        currentMoveYaw    = 0f;
        lookTargetIndex   = 0;
        lookHoldTicks     = 0;
        waypointIndex     = 0;
        waypointOffsets.clear();
        lastStuckCheckPos = null;
        stuckTickCounter  = 0;
        if (client.player != null) client.player.setSprinting(false);
    }

    /** Pełny reset — wywołuj tylko przy ręcznym zatrzymaniu przez gracza. */
    public synchronized void reset(MinecraftClient client) {
        stop(client);
        blacklistedNodes.clear();
    }

    public synchronized void clearBlacklist() {
        blacklistedNodes.clear();
    }

    public Set<BlockPos> getBlacklist() { return blacklistedNodes; }

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

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

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

    // ══════════════════════════════════════════════════════════════════════════
    // TICK GŁÓWNY
    // ══════════════════════════════════════════════════════════════════════════

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

        if (flying) {
            client.player.getAbilities().flying      = true;
            client.player.getAbilities().allowFlying = true;
        }

        advanceWaypoints(client.player);

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

        checkStuck(client.player, client);

        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());
        client.player.setSprinting(shouldSprint);
    }

    // ══════════════════════════════════════════════════════════════════════════
    // KAMERA — rotacja i interpolacja
    // ══════════════════════════════════════════════════════════════════════════

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

        // ── Look target (lookahead) ───────────────────────────────────────────
        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);
        }

        // ── Oblicz target yaw/pitch z smoothLookPoint ─────────────────────────
        Vec3d playerEye = new Vec3d(player.getX(), player.getEyeY(), player.getZ());
        double dx = smoothLookPoint.x - playerEye.x;
        double dz = smoothLookPoint.z - playerEye.z;
        double dy = smoothLookPoint.y - playerEye.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));

        // ── Spring damper — yaw ───────────────────────────────────────────────
        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));

        // ── Spring damper — pitch ─────────────────────────────────────────────
        float pitchError = targetPitch - cameraPitch;
        float pitchAccel = SPRING_STIFFNESS * pitchError - SPRING_DAMPING * pitchVelocity;
        pitchVelocity   += pitchAccel;
        pitchVelocity    = Math.max(-MAX_VELOCITY, Math.min(MAX_VELOCITY, pitchVelocity));

        float oldYaw   = cameraYaw;
        float oldPitch = cameraPitch;

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

        // Atomowy zapis dla render threadu
        frame = new YawPitchFrame(oldYaw, oldPitch, cameraYaw, cameraPitch);

        player.setYaw(cameraYaw);
        player.setPitch(cameraPitch);

        // ── moveYaw — osobny kąt dla nóg (do aktualnego waypointu, nie lookahead) ──
        Vec3d playerFeet = player.getEntityPos();
        Vec3d wpTarget   = getWaypointTarget(path.get(waypointIndex));
        double mdx = wpTarget.x - playerFeet.x;
        double mdz = wpTarget.z - playerFeet.z;
        float rawMoveYaw = (float) Math.toDegrees(Math.atan2(-mdx, mdz));
        float moveDiff   = (float) angleDiff(rawMoveYaw, currentMoveYaw);
        currentMoveYaw  += moveDiff * 0.3f; // lerp 30% — wygładza drobne wahania
    }

    public float getInterpolatedYaw(float tickDelta) {
        YawPitchFrame f = frame;
        float diff = (float) angleDiff(f.nextYaw(), f.prevYaw());
        return f.prevYaw() + diff * tickDelta;
    }

    public float getInterpolatedPitch(float tickDelta) {
        YawPitchFrame f = frame;
        return f.prevPitch() + (f.nextPitch() - f.prevPitch()) * tickDelta;
    }

    // ══════════════════════════════════════════════════════════════════════════
    // LOOKAHEAD — wybór punktu do patrzenia
    // ══════════════════════════════════════════════════════════════════════════

    private BlockPos chooseLookTarget(ClientPlayerEntity player, MinecraftClient client) {
        Vec3d eyePos    = player.getEyePos();
        Vec3d playerPos = player.getEntityPos();
        int   MAX_LOOKAHEAD = 4;
        int   bestIdx       = waypointIndex;

        for (int i = 1; i <= MAX_LOOKAHEAD; i++) {
            int idx = waypointIndex + i;
            if (idx >= path.size()) break;

            BlockPos candidate    = path.get(idx);
            Vec3d   candidateVec  = Vec3d.ofCenter(candidate).add(0, 0.5, 0);
            double  distToCandidate = eyePos.distanceTo(candidateVec);

            // ── Sprawdź czy między aktualnym a kandydatem jest zmiana wysokości ──
            // Jeśli którykolwiek waypoint po drodze ma inną Y — zatrzymaj lookahead
            boolean heightChangeOnPath = false;
            for (int j = waypointIndex; j < idx; j++) {
                int yA = path.get(j).getY();
                int yB = path.get(j + 1).getY();
                if (Math.abs(yA - yB) >= 1) {
                    heightChangeOnPath = true;
                    break;
                }
            }

            // Jeśli na drodze do kandydata jest zmiana wysokości —
            // patrz tylko na pierwszy waypoint tej zmiany, nie dalej
            if (heightChangeOnPath) {
                // Znajdź pierwszy waypoint ze zmianą Y i patrz na niego
                for (int j = waypointIndex; j < idx; j++) {
                    if (Math.abs(path.get(j).getY() - path.get(j + 1).getY()) >= 1) {
                        bestIdx = j + 1; // patrz na wyższy/niższy waypoint
                        break;
                    }
                }
                break; // nie idź dalej w lookahead
            }

            // Brak zmiany wysokości — normalna logika
            if (distToCandidate < 2.5) continue;

            double angleToCandidate = angleToTarget(player, candidateVec);
            if (Math.abs(angleToCandidate) > 70.0) break;

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

            if (hit.getType() == HitResult.Type.MISS) {
                bestIdx = idx;
            } else {
                break;
            }
        }

        return path.get(bestIdx);
    }

    /** Kąt między aktualnym kierunkiem ruchu gracza a punktem docelowym. */
    private double angleToTarget(ClientPlayerEntity player, Vec3d target) {
        Vec3d pos = player.getEntityPos();
        double dx = target.x - pos.x;
        double dz = target.z - pos.z;
        double targetYaw = Math.toDegrees(Math.atan2(-dx, dz));
        return angleDiff(targetYaw, currentMoveYaw);
    }

    // ══════════════════════════════════════════════════════════════════════════
    // RUCH — waypoints i input
    // ══════════════════════════════════════════════════════════════════════════

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

            if (flying) {
                double dist = pPos.distanceTo(center);
                if (dist < 2.5) waypointIndex++;
                else break;
            } else {
                double horizDist = horizontalDist(pPos, center);
                double vertDiff  = pPos.y - center.y; // dodatnie = gracz wyżej niż cel

                // ── Cel jest WYŻEJ niż gracz (skok w górę) ───────────────────────
                // Nie zaliczaj dopóki gracz faktycznie nie wejdzie na ten poziom
                if (wpBlock.getY() > Math.floor(pPos.y) + 0.1) {
                    // Gracz musi być na tym samym Y i poziomo blisko
                    if (horizDist < 1.0 && Math.abs(vertDiff) < 0.6) {
                        waypointIndex++;
                    } else {
                        break;
                    }
                    continue;
                }

                // ── Cel jest NIŻEJ niż gracz (schodzenie/spadanie) ───────────────
                if (vertDiff > 0.8 && horizDist < 1.2) {
                    waypointIndex++;
                    continue;
                }

                // ── Ten sam poziom — normalne zaliczanie ─────────────────────────
                double reach = Math.min(1.5, 0.5 + horizDist * 0.1);
                if (horizDist < reach) waypointIndex++;
                else break;
            }
        }
    }

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

        // Jeśli cel jest niżej i gracz jest poziomo nad nim — patrz na następny
        // waypoint żeby nie wchodzić w ścianę przy schodzeniu
        double horizDist = horizontalDist(playerPos, targetPos);
        double vertDiff  = playerPos.y - targetPos.y;
        if (vertDiff > 0.8 && horizDist < 1.5 && waypointIndex + 1 < path.size()) {
            targetPos = getWaypointTarget(path.get(waypointIndex + 1));
        }

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

        double moveYaw = Math.toDegrees(Math.atan2(-dx, dz));

        // Użyj currentMoveYaw (kąt do waypointu) zamiast player.getYaw()
        // (który jest ustawiony przez lookahead kamery)
        double diff     = angleDiff(moveYaw, currentMoveYaw);
        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;

        // ── Hamowanie przed zakrętem ──────────────────────────────────────────
        float speedMult = 1.0f;

        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, moveYaw));

            if (turnAngle > 30.0) {
                float  turnMult   = (float) Math.max(0.3, 1.0 - (turnAngle - 30.0) / 120.0);
                double distToTurn = flying
                        ? playerPos.distanceTo(targetPos)
                        : horizontalDist(playerPos, targetPos);
                if (distToTurn < 5.0) speedMult *= turnMult;
            }
        }

        // ── Lot — sterowanie pionowe i hamowanie przy dużej zmianie Y ────────
        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;
            if (Math.abs(dy) > 3.0) speedMult *= 0.5f;
        } else {
            jumping = target.getY() > Math.floor(playerPos.y) + 0.5;
        }

        speedMult = Math.max(0.0f, Math.min(1.0f, speedMult));

        boolean isLastWaypoint = (waypointIndex == path.size() - 1);
        boolean sprint = !isLastWaypoint
                && Math.abs(diff) < SPRINT_ANGLE
                && forward > 0.5f
                && speedMult > 0.7f;

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

    // ══════════════════════════════════════════════════════════════════════════
    // STUCK DETECTION
    // ══════════════════════════════════════════════════════════════════════════

    private void checkStuck(ClientPlayerEntity player, MinecraftClient client) {
        Vec3d pos = player.getEntityPos();

        if (lastStuckCheckPos == null) {
            lastStuckCheckPos = pos;
            return;
        }

        double moved = horizontalDist(pos, lastStuckCheckPos);

        if (moved < STUCK_MIN_DIST) {
            stuckTickCounter++;
            if (stuckTickCounter >= STUCK_TICKS) {
                stuckTickCounter  = 0;
                lastStuckCheckPos = pos;
                handleStuck(client);
            }
        } else {
            stuckTickCounter  = 0;
            lastStuckCheckPos = pos;
        }
    }

    private void handleStuck(MinecraftClient client) {
        if (path == null || client.player == null) return;

        BlockPos goal    = path.get(path.size() - 1);
        BlockPos stuckAt = client.player.getBlockPos();

        for (int dx = -1; dx <= 1; dx++) {
            for (int dz = -1; dz <= 1; dz++) {
                BlockPos candidate = stuckAt.add(dx, 0, dz);
                if (candidate.isWithinDistance(goal, 3.0)) continue;
                blacklistedNodes.add(candidate);
                blacklistedNodes.add(candidate.up());
                blacklistedNodes.add(candidate.down());
            }
        }

        stop(client);

        PathfindingController.INSTANCE.goTo(
                goal.getX(), goal.getY(), goal.getZ(),
                flying, null
        );
    }

    // ══════════════════════════════════════════════════════════════════════════
    // POMOCNICZE
    // ══════════════════════════════════════════════════════════════════════════

    /** Losowy offset w bloku — zapobiega chodzeniu przez idealny środek. */
    private Vec3d getWaypointTarget(BlockPos pos) {
        return waypointOffsets.computeIfAbsent(pos, p -> {
            double ox = (rng.nextDouble() - 0.5) * 0.6;
            double oz = (rng.nextDouble() - 0.5) * 0.6;
            return new Vec3d(p.getX() + 0.5 + ox, p.getY(), p.getZ() + 0.5 + oz);
        });
    }

    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;
    }
}