import { INF, MAP_H, MAP_W, MinHeap, indexOf, inside } from "./mapUtils.js";

export function buildCoarseCostGraph({ sea, costField }, options = {}) {
  const scale = options.scale ?? 4;
  const cw = Math.ceil(MAP_W / scale);
  const ch = Math.ceil(MAP_H / scale);
  const size = cw * ch;
  const cost = new Float32Array(size);
  const passable = new Uint8Array(size);
  cost.fill(INF);

  for (let cy = 0; cy < ch; cy++) {
    for (let cx = 0; cx < cw; cx++) {
      let best = INF;
      let sum = 0;
      let n = 0;
      for (let dy = 0; dy < scale; dy++) {
        for (let dx = 0; dx < scale; dx++) {
          const x = cx * scale + dx;
          const y = cy * scale + dy;
          if (!inside(x, y)) continue;
          const i = indexOf(x, y);
          if (sea?.[i] || costField?.[i] >= INF) continue;
          const v = costField[i];
          best = Math.min(best, v);
          sum += v;
          n++;
        }
      }
      const ci = cy * cw + cx;
      if (n > 0) {
        passable[ci] = 1;
        cost[ci] = best * 0.55 + (sum / n) * 0.45;
      }
    }
  }

  return { scale, cw, ch, size, cost, passable };
}

export function routeCoarsePath(start, goal, graph, options = {}) {
  if (!start || !goal || !graph) return [];
  const sx = Math.floor(start.x / graph.scale);
  const sy = Math.floor(start.y / graph.scale);
  const gx = Math.floor(goal.x / graph.scale);
  const gy = Math.floor(goal.y / graph.scale);
  if (sx < 0 || sy < 0 || sx >= graph.cw || sy >= graph.ch || gx < 0 || gy < 0 || gx >= graph.cw || gy >= graph.ch) return [];
  const startCi = sy * graph.cw + sx;
  const goalCi = gy * graph.cw + gx;
  if (!graph.passable[startCi] || !graph.passable[goalCi]) return [];

  const score = new Float32Array(graph.size);
  const cameFrom = new Int32Array(graph.size);
  const closed = new Uint8Array(graph.size);
  score.fill(INF);
  cameFrom.fill(-1);
  score[startCi] = 0;
  const heap = new MinHeap();
  heap.push({ i: startCi, f: Math.hypot(sx - gx, sy - gy) });
  const maxExpanded = options.maxExpanded ?? graph.size;
  let expanded = 0;
  let found = -1;

  while (heap.length && expanded++ < maxExpanded) {
    const cur = heap.pop();
    if (!cur || closed[cur.i]) continue;
    closed[cur.i] = 1;
    if (cur.i === goalCi) {
      found = cur.i;
      break;
    }
    const cx = cur.i % graph.cw;
    const cy = Math.floor(cur.i / graph.cw);
    for (let dy = -1; dy <= 1; dy++) {
      for (let dx = -1; dx <= 1; dx++) {
        if (!dx && !dy) continue;
        const nx = cx + dx;
        const ny = cy + dy;
        if (nx < 0 || ny < 0 || nx >= graph.cw || ny >= graph.ch) continue;
        const ni = ny * graph.cw + nx;
        if (closed[ni] || !graph.passable[ni]) continue;
        const nd = score[cur.i] + graph.cost[ni] * Math.hypot(dx, dy);
        if (nd < score[ni]) {
          score[ni] = nd;
          cameFrom[ni] = cur.i;
          heap.push({ i: ni, f: nd + Math.hypot(nx - gx, ny - gy) * (options.heuristicWeight ?? 0.85) });
        }
      }
    }
  }
  if (found < 0) return [];

  const path = [];
  for (let p = found; p >= 0; p = cameFrom[p]) {
    const cx = p % graph.cw;
    const cy = Math.floor(p / graph.cw);
    path.push([
      Math.min(MAP_W - 1, Math.round(cx * graph.scale + graph.scale * 0.5)),
      Math.min(MAP_H - 1, Math.round(cy * graph.scale + graph.scale * 0.5)),
    ]);
    if (p === startCi) break;
  }
  return path.reverse();
}

export function refineCoarsePath(path, costField, options = {}) {
  if (!path || path.length < 2) return [];
  const sea = options.sea || null;
  const radius = options.snapRadius ?? 1;
  const out = [];
  let lastKey = "";
  for (let k = 0; k < path.length - 1; k++) {
    const a = path[k];
    const b = path[k + 1];
    const steps = Math.max(1, Math.ceil(Math.hypot(b[0] - a[0], b[1] - a[1])));
    for (let s = k === 0 ? 0 : 1; s <= steps; s++) {
      const t = s / steps;
      const tx = Math.round(a[0] + (b[0] - a[0]) * t);
      const ty = Math.round(a[1] + (b[1] - a[1]) * t);
      let best = null;
      let bestScore = INF;
      for (let dy = -radius; dy <= radius; dy++) {
        for (let dx = -radius; dx <= radius; dx++) {
          const x = tx + dx;
          const y = ty + dy;
          if (!inside(x, y)) continue;
          const i = indexOf(x, y);
          if (sea?.[i] || costField?.[i] >= INF) continue;
          const score = costField[i] + Math.hypot(dx, dy) * 0.22;
          if (score < bestScore) {
            bestScore = score;
            best = [x, y];
          }
        }
      }
      if (!best) return [];
      const key = `${best[0]},${best[1]}`;
      if (key !== lastKey) {
        out.push(best);
        lastKey = key;
      }
    }
  }
  return out.length >= 2 ? out : [];
}