// Shared distance-based trajectories and local traffic clearance, in canvas pixels.
export function measure(points) {
  const lengths = [0];
  for (let i = 1; i < points.length; i++) lengths.push(lengths[i - 1] + Math.hypot(points[i].x - points[i - 1].x, points[i].y - points[i - 1].y));
  return { points, lengths, length: lengths.at(-1) };
}
export function at(path, distance) {
  const d = Math.max(0, Math.min(path.length, distance));
  let i = path.lengths.findIndex((v, index) => index && v >= d);
  if (i < 1) i = path.points.length - 1;
  const a = path.points[i - 1], b = path.points[i], len = path.lengths[i] - path.lengths[i - 1];
  const t = len ? (d - path.lengths[i - 1]) / len : 0;
  return { x: a.x + (b.x - a.x) * t, y: a.y + (b.y - a.y) * t, angle: Math.atan2(b.y - a.y, b.x - a.x) };
}
export function pointSegmentDistance(p, a, b) {
  const dx = b.x - a.x, dy = b.y - a.y, length = dx * dx + dy * dy;
  const t = length ? Math.max(0, Math.min(1, ((p.x-a.x)*dx+(p.y-a.y)*dy)/length)) : 0;
  return Math.hypot(p.x-a.x-dx*t,p.y-a.y-dy*t);
}
export function hasPriority(a, b) {
  if (a.yieldingTo === b) return false;
  if (b.yieldingTo === a) return true;
  return a.order < b.order;
}
// A car reserves road conflicts only after departure. Parked cars are physical
// obstacles, not reservations for every road they may eventually travel through.
export function activeCorridors(traffic, actor) {
  return traffic.filter(other => other !== actor
    && ['board', 'waiting', 'joining'].includes(other.stage)
    && other.distance < other.boardLength && hasPriority(other, actor));
}
// Only the untraveled part of a priority vehicle's route reserves a conflict area.
// Other cars travel up to that area, wait there, and resume as it clears.
export function advance(path, current, target, obstacles, corridors, clearance) {
  let distance = current;
  while (distance < target - 1e-6) {
    const next = Math.min(target, distance + 1.5), p = at(path, next);
    if (obstacles.some(o => Math.hypot(p.x-o.x,p.y-o.y) < clearance)) break;
    let conflict = false;
    for (const corridor of corridors) {
      let a = at(corridor.path, corridor.distance);
      for (let i = 1; i < corridor.path.points.length; i++) {
        if (corridor.path.lengths[i] <= corridor.distance) continue;
        if (corridor.path.lengths[i-1] > corridor.boardLength + clearance) break;
        const b = corridor.path.points[i];
        if (pointSegmentDistance(p,a,b) < clearance) { conflict = true; break; }
        a = b;
      }
      if (conflict) break;
    }
    if (conflict) break;
    distance = next;
  }
  return distance;
}

// Stable passage priority respects the parked-car dependency graph, avoiding
// circular reservations when several intersecting cars are clicked together.
export function passageOrder(board, cars, trace) {
  let remaining = [...cars], rank = 0;
  const order = new Map();
  while (remaining.length) {
    const free = remaining.filter(car => trace(board, car, remaining).status === 'exit');
    if (!free.length) { remaining.forEach(car => order.set(car.id, rank++)); break; }
    const ids = new Set(free.map(car => car.id));
    free.forEach(car => order.set(car.id, rank++));
    remaining = remaining.filter(car => !ids.has(car.id));
  }
  return order;
}

// Back out of a mutual standstill, using the same swept body clearance as forward travel.
// The yielding car keeps its priority override until the other has left the board.
export function resolveGridlock(traffic, fixedObstacles, positionOf, clearance, step) {
  const reversed = new Set();
  const internal = traffic.filter(t => t.stage === 'board');
  for (const actor of internal) {
    if (actor.yieldingTo && !internal.includes(actor.yieldingTo)) {
      actor.yieldingTo = null; actor.retreating = false;
    }
  }
  const retreat = actor => {
    const obstacles = [...fixedObstacles, ...traffic.filter(o => o !== actor && o.stage !== 'done').map(positionOf)];
    let distance = actor.distance;
    const target = Math.max(0, distance - step);
    while (distance > target + 1e-6) {
      const next = Math.max(target, distance - 1.5), p = at(actor.path, next);
      if (obstacles.some(o => Math.hypot(p.x - o.x, p.y - o.y) < clearance)) break;
      distance = next;
    }
    return distance;
  };
  for (let i = 0; i < internal.length; i++) for (let j = i + 1; j < internal.length; j++) {
    const a = internal[i], b = internal[j];
    if (a.yieldingTo || b.yieldingTo || !a.stopped || !b.stopped || Math.min(a.blockedFor || 0, b.blockedFor || 0) < .5) continue;
    const p = positionOf(a), q = positionOf(b);
    if (Math.hypot(p.x - q.x, p.y - q.y) > clearance * 1.6) continue;
    const candidates = hasPriority(a, b) ? [b, a] : [a, b];
    const loser = candidates.find(t => retreat(t) < t.distance - .01);
    if (loser) { loser.yieldingTo = loser === a ? b : a; loser.retreating = true; }
  }
  for (const actor of internal) {
    if (!actor.retreating || !actor.yieldingTo) continue;
    // A short probe tests whether the whole body has cleared the winning route.
    const p = positionOf(actor), probe = measure([p, { x: p.x + .01, y: p.y }]);
    if (advance(probe, 0, probe.length, [], [actor.yieldingTo], clearance + 3) >= probe.length) {
      actor.retreating = false;
      continue;
    }
    const next = retreat(actor);
    actor.distance = next; actor.stopped = true; reversed.add(actor);
    if (next <= 0) actor.retreating = false;
  }
  return reversed;
}