healer-man/src/game/partyPathing.ts

import type { MutablePartyWorldPosition, PartyWorldPosition } from "./partyRuntimeRegistry";
import {
  findPartyNavigationPath,
  type PartyNavigationGraph,
  type PartyNavigationPosition,
} from "./partyNavigation";

export interface PartyRouteState {
  goalKey: string | null;
  destination: PartyNavigationPosition | null;
  waypoints: readonly PartyNavigationPosition[];
  cursor: number;
  plannedAt: number;
  blocked: boolean;
}

export interface PartyRouteOptions {
  readonly maxSnapDistance?: number;
  readonly maxOriginSnapDistance?: number;
  readonly maxDestinationSnapDistance?: number;
  readonly localFinishDistance?: number;
  readonly waypointReachedDistance?: number;
  readonly replanDistance?: number;
  readonly replanIntervalMs?: number;
  readonly blockedRetryMs?: number;
  readonly simplifyTolerance?: number;
  /** Required for every off-surface connector longer than the reached radius. */
  readonly directSegmentAllowed?: (
    start: PartyWorldPosition,
    end: PartyWorldPosition,
  ) => boolean;
  /** Agent-radius validation for each segment that lies on the route surface. */
  readonly routeSegmentAllowed?: (
    start: PartyWorldPosition,
    end: PartyWorldPosition,
  ) => boolean;
  /** Combat may finish on the retained surface only when range AND sight allow a hit. */
  readonly destinationReached?: (surface: PartyWorldPosition, destination: PartyWorldPosition) => boolean;
}

const DEFAULT_MAX_SNAP_DISTANCE = 3;
const DEFAULT_LOCAL_FINISH_DISTANCE = 3;
const DEFAULT_WAYPOINT_REACHED_DISTANCE = 0.04;
const DEFAULT_REPLAN_DISTANCE = 2.25;
const DEFAULT_REPLAN_INTERVAL_MS = 650;
const DEFAULT_BLOCKED_RETRY_MS = 1_500;

function positiveOption(value: number | undefined, fallback: number): number {
  return Number.isFinite(value) ? Math.max(0, value!) : fallback;
}

function fullDistance(left: PartyWorldPosition, right: PartyWorldPosition): number {
  return Math.hypot(right[0] - left[0], right[1] - left[1], right[2] - left[2]);
}

function copyPosition(position: PartyWorldPosition): PartyNavigationPosition {
  return [position[0], position[1], position[2]];
}

export function emptyPartyRouteState(): PartyRouteState {
  return {
    goalKey: null,
    destination: null,
    waypoints: [],
    cursor: 0,
    plannedAt: Number.NEGATIVE_INFINITY,
    blocked: false,
  };
}

/** Plans only over approved graph edges and a short final encounter approach. */
export function planPartyRoute(
  graph: PartyNavigationGraph | null,
  goalKey: string,
  origin: PartyWorldPosition,
  destination: PartyWorldPosition,
  now: number,
  options: PartyRouteOptions = {},
): PartyRouteState {
  const maxSnapDistance = positiveOption(options.maxSnapDistance, DEFAULT_MAX_SNAP_DISTANCE);
  const maxOriginSnapDistance = positiveOption(options.maxOriginSnapDistance, maxSnapDistance);
  const maxDestinationSnapDistance = positiveOption(options.maxDestinationSnapDistance, maxSnapDistance);
  const localFinishDistance = positiveOption(options.localFinishDistance, DEFAULT_LOCAL_FINISH_DISTANCE);
  const reachedDistance = positiveOption(
    options.waypointReachedDistance,
    DEFAULT_WAYPOINT_REACHED_DISTANCE,
  );
  const blocked = (waypoints: readonly PartyNavigationPosition[] = []): PartyRouteState => ({
    goalKey,
    destination: copyPosition(destination),
    waypoints,
    cursor: 0,
    plannedAt: now,
    blocked: true,
  });
  if (!graph) return blocked();

  const path = findPartyNavigationPath(graph, origin, destination, {
    maxSnapDistance,
    maxStartSnapDistance: maxOriginSnapDistance,
    maxEndSnapDistance: maxDestinationSnapDistance,
    simplifyTolerance: options.simplifyTolerance ?? 0.01,
    routeSegmentAllowed: options.routeSegmentAllowed,
  });
  if (!path?.length) return blocked();

  const waypoints = [...path];
  if (options.routeSegmentAllowed) {
    for (let index = 1; index < waypoints.length; index += 1) {
      if (!options.routeSegmentAllowed(waypoints[index - 1], waypoints[index])) {
        return blocked(waypoints);
      }
    }
  }
  const startDistance = fullDistance(origin, waypoints[0]);
  if (
    startDistance > reachedDistance * 0.5
    && !options.directSegmentAllowed?.(origin, waypoints[0])
  ) return blocked(waypoints);
  const finishDistance = fullDistance(waypoints[waypoints.length - 1], destination);
  if (finishDistance > localFinishDistance) return blocked(waypoints);
  if (finishDistance > reachedDistance * 0.5 && !options.destinationReached?.(waypoints[waypoints.length - 1], destination)) {
    if (!options.directSegmentAllowed?.(waypoints[waypoints.length - 1], destination)) {
      return blocked(waypoints);
    }
    waypoints.push(copyPosition(destination));
  }
  return {
    goalKey,
    destination: copyPosition(destination),
    waypoints,
    cursor: 0,
    plannedAt: now,
    blocked: false,
  };
}

export function partyRouteNeedsPlanning(
  route: PartyRouteState,
  goalKey: string,
  current: PartyWorldPosition,
  destination: PartyWorldPosition,
  now: number,
  options: PartyRouteOptions = {},
): boolean {
  if (route.goalKey !== goalKey || !route.destination) return true;
  const elapsed = now - route.plannedAt;
  const blockedRetryMs = positiveOption(options.blockedRetryMs, DEFAULT_BLOCKED_RETRY_MS);
  if (route.blocked) return elapsed >= blockedRetryMs;
  const replanDistance = positiveOption(options.replanDistance, DEFAULT_REPLAN_DISTANCE);
  const replanIntervalMs = positiveOption(options.replanIntervalMs, DEFAULT_REPLAN_INTERVAL_MS);
  if (
    fullDistance(route.destination, destination) >= replanDistance
    && elapsed >= replanIntervalMs
  ) return true;
  const reachedDistance = positiveOption(
    options.waypointReachedDistance,
    DEFAULT_WAYPOINT_REACHED_DISTANCE,
  );
  return route.cursor >= route.waypoints.length
    && fullDistance(current, destination) > reachedDistance
    && elapsed >= replanIntervalMs;
}

/** Returns false for a blocked/exhausted route; callers must hold, never go direct. */
export function nextPartyRouteWaypoint(
  route: PartyRouteState,
  current: PartyWorldPosition,
  out: MutablePartyWorldPosition,
  options: PartyRouteOptions = {},
): boolean {
  const reachedDistance = positiveOption(
    options.waypointReachedDistance,
    DEFAULT_WAYPOINT_REACHED_DISTANCE,
  );
  if (
    route.cursor < route.waypoints.length
    && fullDistance(current, route.waypoints[route.cursor]) <= reachedDistance
  ) route.cursor += 1;
  if (route.blocked || route.cursor >= route.waypoints.length) return false;
  const waypoint = route.waypoints[route.cursor];
  out[0] = waypoint[0];
  out[1] = waypoint[1];
  out[2] = waypoint[2];
  return true;
}

/**
 * A route can be blocked by a later segment or its final connector while its
 * opening waypoints remain safe. Advance only to the first directly validated
 * prefix point, then let the blocked-route retry plan again from there.
 */
export function nextPartyRouteRecoveryWaypoint(
  route: PartyRouteState,
  current: PartyWorldPosition,
  out: MutablePartyWorldPosition,
  segmentAllowed: (start: PartyWorldPosition, end: PartyWorldPosition) => boolean,
  reachedDistance = DEFAULT_WAYPOINT_REACHED_DISTANCE,
): boolean {
  if (!route.blocked) return false;
  const reached = Math.max(0, Number.isFinite(reachedDistance) ? reachedDistance : 0);
  let nearestIndex = -1;
  let nearestDistance = Number.POSITIVE_INFINITY;
  route.waypoints.forEach((waypoint, index) => {
    const candidateDistance = fullDistance(current, waypoint);
    if (candidateDistance < nearestDistance) {
      nearestIndex = index;
      nearestDistance = candidateDistance;
    }
  });
  const firstCandidate = nearestDistance <= reached ? nearestIndex + 1 : nearestIndex;
  for (let index = Math.max(0, firstCandidate); index < route.waypoints.length; index += 1) {
    const waypoint = route.waypoints[index];
    if (!segmentAllowed(current, waypoint)) return false;
    out[0] = waypoint[0];
    out[1] = waypoint[1];
    out[2] = waypoint[2];
    return true;
  }
  return false;
}