package com.tiedup.remake.entities.ai;

import com.tiedup.remake.core.TiedUpMod;
import java.util.ArrayList;
import java.util.List;
import net.minecraft.core.BlockPos;
import net.minecraft.world.entity.Mob;
import org.jetbrains.annotations.Nullable;

/**
 * Utility class for navigating through ordered waypoints.
 * Used by Kidnapper and Maid AI to navigate complex structures like
 * multi-story buildings, corridors, and staircases.
 *
 * Waypoints are processed in order (0, 1, 2...) until the final destination.
 * The navigator tracks current position in the waypoint list and provides
 * the current target position for pathfinding.
 *
 * Usage:
 * 1. Create with entity and waypoint list
 * 2. Call tick() each AI tick
 * 3. Call navigateToCurrentWaypoint() to pathfind to current target
 * 4. Check isComplete() to know when done
 */
public class WaypointNavigator {

    private final Mob entity;
    private final List<BlockPos> waypoints;
    private int currentIndex = 0;

    /** Distance (squared) at which a waypoint is considered reached */
    private static final double WAYPOINT_REACH_DISTANCE_SQ = 2.5 * 2.5;

    /** Vertical tolerance for reaching a waypoint */
    private static final double WAYPOINT_Y_TOLERANCE = 2.0;

    /** Speed modifier for navigation */
    private double speedModifier = 1.0;

    /**
     * Create a new waypoint navigator.
     *
     * @param entity    The entity to navigate
     * @param waypoints The ordered list of waypoints to follow
     */
    public WaypointNavigator(Mob entity, List<BlockPos> waypoints) {
        this.entity = entity;
        this.waypoints = new ArrayList<>(waypoints);
    }

    /**
     * Create a navigator with a custom speed modifier.
     *
     * @param entity        The entity to navigate
     * @param waypoints     The ordered list of waypoints
     * @param speedModifier Speed multiplier for navigation
     */
    public WaypointNavigator(
        Mob entity,
        List<BlockPos> waypoints,
        double speedModifier
    ) {
        this(entity, waypoints);
        this.speedModifier = speedModifier;
    }

    /**
     * Check if this navigator has any waypoints.
     *
     * @return true if there are waypoints to follow
     */
    public boolean hasWaypoints() {
        return !waypoints.isEmpty();
    }

    /**
     * Check if all waypoints have been reached.
     *
     * @return true if navigation is complete
     */
    public boolean isComplete() {
        return currentIndex >= waypoints.size();
    }

    /**
     * Get the current waypoint to navigate to.
     *
     * @return Current waypoint position, or null if complete
     */
    @Nullable
    public BlockPos getCurrentWaypoint() {
        if (isComplete()) {
            return null;
        }
        return waypoints.get(currentIndex);
    }

    /**
     * Get the current waypoint index (0-based).
     *
     * @return Current index in the waypoint list
     */
    public int getCurrentIndex() {
        return currentIndex;
    }

    /**
     * Get the total number of waypoints.
     *
     * @return Total waypoint count
     */
    public int getTotalWaypoints() {
        return waypoints.size();
    }

    /**
     * Tick the navigator to check for waypoint advancement.
     * Should be called each AI tick.
     * Automatically advances to next waypoint when current is reached.
     */
    public void tick() {
        if (isComplete()) {
            return;
        }

        BlockPos target = getCurrentWaypoint();
        if (target == null) {
            return;
        }

        // Check XZ distance
        double dx = entity.getX() - (target.getX() + 0.5);
        double dz = entity.getZ() - (target.getZ() + 0.5);
        double distXZSq = dx * dx + dz * dz;

        // Check Y distance
        double distY = Math.abs(entity.getY() - target.getY());

        // If close enough in both XZ and Y, advance to next waypoint
        if (
            distXZSq < WAYPOINT_REACH_DISTANCE_SQ &&
            distY < WAYPOINT_Y_TOLERANCE
        ) {
            TiedUpMod.LOGGER.debug(
                "[WaypointNavigator] {} reached waypoint {}/{} at {}",
                entity.getName().getString(),
                currentIndex + 1,
                waypoints.size(),
                target.toShortString()
            );
            currentIndex++;
        }
    }

    /**
     * Navigate to the current waypoint.
     * Should be called periodically (e.g., every 10-20 ticks) to update pathfinding.
     *
     * @return true if a path was found, false if pathfinding failed
     */
    public boolean navigateToCurrentWaypoint() {
        BlockPos target = getCurrentWaypoint();
        if (target == null) {
            return false;
        }

        return entity
            .getNavigation()
            .moveTo(
                target.getX() + 0.5,
                target.getY(),
                target.getZ() + 0.5,
                speedModifier
            );
    }

    /**
     * Reset navigation to start from the beginning.
     */
    public void reset() {
        currentIndex = 0;
    }

    /**
     * Skip to a specific waypoint index.
     *
     * @param index The index to skip to
     */
    public void skipTo(int index) {
        currentIndex = Math.min(index, waypoints.size());
    }

    /**
     * Get the speed modifier.
     *
     * @return Current speed modifier
     */
    public double getSpeedModifier() {
        return speedModifier;
    }

    /**
     * Set the speed modifier.
     *
     * @param speedModifier New speed modifier
     */
    public void setSpeedModifier(double speedModifier) {
        this.speedModifier = speedModifier;
    }

    /**
     * Get distance to current waypoint.
     *
     * @return Distance to current waypoint, or -1 if complete
     */
    public double getDistanceToCurrentWaypoint() {
        BlockPos target = getCurrentWaypoint();
        if (target == null) {
            return -1;
        }

        return entity.distanceToSqr(
            target.getX() + 0.5,
            target.getY(),
            target.getZ() + 0.5
        );
    }
}
