/* =============================================================================
 * map_guide_route.ts(logic)
 * 예정노선 가이드 추천선 셈(PLAN 636-1 · 요구 546) — 기준지점을 차례로 거치는 참고용 선.
 *   화면 가이드 전용 · 파이썬 짝 없음 · 저장 안 함(설계 값 아님 · 선례 `D03_PlanAnalysis_Fan.ts:8`).
 *   2 m 격자 A*: 걸음 = 반지름 걸음/칸 ± 0.5 격자점(20 m = 56 방향) · |Δz| > 상한 × 걸음 길이 막음 ·
 *   비용 = 길이 × (1 + 급경사 × 급경사 표본 비율 + 기울기 × (g/상한)²) + 꺾임 × 걸음 × (꺾임각/π)² ·
 *   어림 = 직선 거리 × 가중(가중 A*) · 구간마다 상자 = 두 점 + 여유(못 닿으면 두 배 한 번) · 펼침 상한.
 *   첫 걸음 = 정확한 시작점에서 · 끝 걸음도 기울기 검사 · 꼭짓점 그대로(20 m 다시 나누기 금지).
 *   1 m 격자 = 2 칸 솎아 2 m · 급경사 = 격자 높이에서 바로 셈 · 못 닿음 = 가장 가까이 간 데까지.
 *   근거 = `docs/raw/요구_1101/_분석_546-C_추천노선_라이다.md` C1 · DOM · fetch · three 없음.
 * ========================================================================== */

import {
  heightAt as gridHeightAt,
  type DtmGrid,
} from "../../../common_util/common_util_surface_sampler";

export type Point2 = [number, number];
export type Point3 = [number, number, number];

/** 높이 원천 — heightAt(NaN = 밖 · 빈칸) · 원천 격자 칸(m) · 격자 원점(칸 꼭짓점 맞춤). */
export interface GuideTerrain {
  heightAt: (x: number, y: number) => number;
  /** 원천 격자 칸 m(기본 2) — 2 m 보다 잘면 솎아 2 m 안팎으로. */
  cell?: number;
  origin?: Point2;
}

export interface GuideWeights {
  /** 급경사 표본 비율 벌점(기본 3) */
  steep: number;
  /** (g/상한)² 벌점(기본 0.3) */
  grade: number;
  /** 꺾임 벌점(기본 1) */
  turn: number;
  /** 어림 가중(기본 3 · 1 = 정확 A*) */
  heuristic: number;
}

export interface GuideOptions {
  /** 걸음 m(기본 20 · 그리기 「길이·기울기 고정」 칸) */
  step?: number;
  /** 기울기 상한 비율(기본 0.14) */
  maxGrade?: number;
  /** 급경사 문턱 %(기본 70 = 35° · 급경사 겹 둘째 경계) */
  steepPct?: number;
  weights?: Partial<GuideWeights>;
  /** 구간마다 펼침 상한(기본 30 만 ≈ 0.4 s · 못 닿음 ≤ 0.5 s) */
  maxExpand?: number;
  /** 구간마다 시간 상한 ms(기본 450 · 못 닿음 ≤ 0.5 s · 느린 기계 막음) */
  maxMs?: number;
}

export interface GuideSegment {
  /** 꼭짓점 [x, y, z] — 닿음이면 끝 = 다음 지점 · 아니면 가장 가까이 간 데까지 */
  vertices: Point3[];
  reached: boolean;
  /** 평면 길이 m */
  length: number;
  /** 걸음 가운데 가장 큰 |기울기| 비율 */
  maxGrade: number;
  /** 급경사(문턱 이상) 지나는 길이 비율 0~1 */
  steepRatio: number;
  expanded: number;
}

export interface GuideResult {
  segments: GuideSegment[];
  ms: number;
}

export const GUIDE_DEFAULTS = {
  step: 20,
  maxGrade: 0.14,
  steepPct: 70,
  weights: { steep: 3, grade: 0.3, turn: 1, heuristic: 3 } as GuideWeights,
  maxExpand: 300_000,
  maxMs: 450,
};

const MARGIN_MIN = 200;
const MARGIN_MAX = 800;
const STEEP_SAMPLES = [0.25, 0.5, 0.75, 1];

/** 서버 지형 격자(`parseDtmGrid`) → 가이드 높이 원천(원천 칸 · 원점 그대로). */
export function guideTerrainFromGrid(grid: DtmGrid): GuideTerrain {
  const n = grid.x.length;
  const cell = n > 1 ? (grid.x[n - 1] - grid.x[0]) / (n - 1) : 2;
  return {
    heightAt: (x, y) => gridHeightAt(grid, x, y),
    cell,
    origin: [grid.x[0], grid.y[0]],
  };
}

/** 지점 차례로 잇는 가이드 선 — 지점 사이 구간마다 A*. */
export function guideRoute(
  points: Point2[],
  terrain: GuideTerrain,
  options: GuideOptions = {},
): GuideResult {
  const started = performance.now();
  const segments: GuideSegment[] = [];
  for (let index = 0; index + 1 < points.length; index++) {
    segments.push(guideSegment(points[index], points[index + 1], terrain, options));
  }
  return { segments, ms: performance.now() - started };
}

/** 두 점 사이 가이드 선 하나. */
export function guideSegment(
  start: Point2,
  goal: Point2,
  terrain: GuideTerrain,
  options: GuideOptions = {},
): GuideSegment {
  const step = options.step ?? GUIDE_DEFAULTS.step;
  const gmax = options.maxGrade ?? GUIDE_DEFAULTS.maxGrade;
  const weights = { ...GUIDE_DEFAULTS.weights, ...options.weights };
  const steepGrade = (options.steepPct ?? GUIDE_DEFAULTS.steepPct) / 100;
  const source = terrain.cell ?? 2;
  const cell = source * Math.max(1, Math.round(2 / source));
  const origin = terrain.origin ?? [0, 0];
  const za = terrain.heightAt(start[0], start[1]);
  const zb = terrain.heightAt(goal[0], goal[1]);
  if (!Number.isFinite(za) || !Number.isFinite(zb)) {
    return measure([[start[0], start[1], za]], false, 0, null);
  }
  const span = Math.hypot(goal[0] - start[0], goal[1] - start[1]);
  const margin = Math.min(MARGIN_MAX, Math.max(MARGIN_MIN, span / 2));
  const limit = options.maxExpand ?? GUIDE_DEFAULTS.maxExpand;
  let spent = 0; // 두 상자 합친 펼침 수
  let late = false;
  const deadline = performance.now() + (options.maxMs ?? GUIDE_DEFAULTS.maxMs);
  let edge = false; // 상자 끝에 닿음 = 넓히면 길이 있을 수 있음
  let found = search(margin);
  if (!found.reached && edge && spent < limit && !late) found = search(margin * 2);
  found.expanded = spent;
  return found;

  function search(pad: number): GuideSegment {
    const i0 = Math.floor((Math.min(start[0], goal[0]) - pad - origin[0]) / cell);
    const j0 = Math.floor((Math.min(start[1], goal[1]) - pad - origin[1]) / cell);
    const w = Math.ceil((Math.max(start[0], goal[0]) + pad - origin[0]) / cell) - i0 + 1;
    const h = Math.ceil((Math.max(start[1], goal[1]) + pad - origin[1]) / cell) - j0 + 1;
    const n = w * h;
    const START = n;
    const GOAL = n + 1;
    const zs = new Float32Array(n).fill(Infinity); // Infinity = 아직 안 읽음 · NaN = 밖
    const steepState = new Uint8Array(n); // 0 모름 · 1 급경사 · 2 아님
    const g = new Float64Array(n + 2).fill(Infinity);
    const parent = new Int32Array(n + 2).fill(-1);
    const dir = new Float32Array(n + 2).fill(NaN);
    const closed = new Uint8Array(n + 2);
    const heap = new MinHeap();
    const nx = (id: number) => origin[0] + (i0 + (id % w)) * cell;
    const ny = (id: number) => origin[1] + (j0 + Math.floor(id / w)) * cell;
    const zAt = (id: number): number => {
      let value = zs[id];
      if (value === Infinity) {
        value = terrain.heightAt(nx(id), ny(id));
        zs[id] = Number.isFinite(value) ? value : NaN;
      }
      return value;
    };
    const steepSq = (2 * cell * steepGrade) ** 2;
    const steepAt = (id: number): boolean => {
      if (steepState[id] === 0) {
        const i = id % w;
        const j = (id - i) / w;
        if (i < 1 || j < 1 || i >= w - 1 || j >= h - 1) steepState[id] = 2;
        else {
          const gx = zAt(id + 1) - zAt(id - 1);
          const gy = zAt(id + w) - zAt(id - w);
          steepState[id] = gx * gx + gy * gy >= steepSq ? 1 : 2; // NaN = 아님
        }
      }
      return steepState[id] === 1;
    };
    const radius = step / cell;
    // 걸음 표 — 격자 차 · 길이 · 방위 · 급경사 표본 차(1/4 · 1/2 · 3/4 · 끝)
    const offI: number[] = [];
    const offJ: number[] = [];
    const offLen: number[] = [];
    const offDir: number[] = [];
    const sampleD: number[] = []; // 칸 번호 차
    const reach = Math.ceil(radius + 0.5);
    for (let dj = -reach; dj <= reach; dj++) {
      for (let di = -reach; di <= reach; di++) {
        if (Math.abs(Math.hypot(di, dj) - radius) > 0.5) continue;
        offI.push(di);
        offJ.push(dj);
        offLen.push(Math.hypot(di, dj) * cell);
        offDir.push(Math.atan2(dj, di));
        for (const t of STEEP_SAMPLES) {
          sampleD.push(Math.round(dj * t) * w + Math.round(di * t));
        }
      }
    }
    const steepShare = weights.steep / STEEP_SAMPLES.length;
    const turnWeight = (weights.turn * step) / (Math.PI * Math.PI);
    // 걸음 비용 — 막히면 Infinity
    const cost = (
      ax: number,
      ay: number,
      az: number,
      bx: number,
      by: number,
      bz: number,
      inDir: number,
    ): number => {
      const len = Math.hypot(bx - ax, by - ay);
      const dz = Math.abs(bz - az);
      if (!(dz <= gmax * len)) return Infinity;
      let hits = 0;
      for (const t of STEEP_SAMPLES) {
        const si = Math.round((ax + (bx - ax) * t - origin[0]) / cell) - i0;
        const sj = Math.round((ay + (by - ay) * t - origin[1]) / cell) - j0;
        if (si >= 0 && sj >= 0 && si < w && sj < h && steepAt(sj * w + si)) hits++;
      }
      const grade = len > 0 ? dz / len / gmax : 0;
      let value = len * (1 + steepShare * hits + weights.grade * grade * grade);
      if (Number.isFinite(inDir) && len > 0) {
        let turn = Math.abs(Math.atan2(by - ay, bx - ax) - inDir);
        if (turn > Math.PI) turn = 2 * Math.PI - turn;
        value += turnWeight * turn * turn;
      }
      return value;
    };
    const relax = (
      from: number,
      to: number,
      c: number,
      ax: number,
      ay: number,
      bx: number,
      by: number,
    ) => {
      if (!Number.isFinite(c) || g[from] + c >= g[to]) return;
      g[to] = g[from] + c;
      parent[to] = from;
      dir[to] = Math.atan2(by - ay, bx - ax);
      heap.push(g[to] + weights.heuristic * Math.hypot(goal[0] - bx, goal[1] - by), to);
    };
    // 끝 걸음 — 한 걸음 안이면 정확한 끝점으로(기울기 검사)
    const tryGoal = (from: number, ax: number, ay: number, az: number) => {
      if (Math.hypot(goal[0] - ax, goal[1] - ay) <= step + 0.5 * cell) {
        relax(
          from,
          GOAL,
          cost(ax, ay, az, goal[0], goal[1], zb, dir[from]),
          ax,
          ay,
          goal[0],
          goal[1],
        );
      }
    };
    g[START] = 0;
    tryGoal(START, start[0], start[1], za);
    // 첫 걸음 — 정확한 시작점에서 반지름 걸음 고리
    const ci = Math.round((start[0] - origin[0]) / cell) - i0;
    const cj = Math.round((start[1] - origin[1]) / cell) - j0;
    for (let j = cj - reach - 1; j <= cj + reach + 1; j++) {
      for (let i = ci - reach - 1; i <= ci + reach + 1; i++) {
        if (i < 0 || j < 0 || i >= w || j >= h) continue;
        const id = j * w + i;
        const bx = nx(id);
        const by = ny(id);
        if (Math.abs(Math.hypot(bx - start[0], by - start[1]) - step) > 0.5 * cell) continue;
        const bz = zAt(id);
        if (Number.isNaN(bz)) continue;
        relax(START, id, cost(start[0], start[1], za, bx, by, bz, NaN), start[0], start[1], bx, by);
      }
    }
    let best = START;
    let bestDist = span;
    while (heap.size > 0 && spent < limit && !late) {
      const u = heap.pop();
      if (closed[u]) continue;
      closed[u] = 1;
      if (u === GOAL) return finish(GOAL, true);
      spent++;
      if ((spent & 1023) === 0 && performance.now() > deadline) late = true;
      const ui = u % w;
      const uj = Math.floor(u / w);
      const ux = nx(u);
      const uy = ny(u);
      const uz = zs[u];
      const left = Math.hypot(goal[0] - ux, goal[1] - uy);
      if (left < bestDist) {
        bestDist = left;
        best = u;
      }
      tryGoal(u, ux, uy, uz);
      const gu = g[u];
      const du = dir[u];
      for (let k = 0; k < offI.length; k++) {
        const vi = ui + offI[k];
        const vj = uj + offJ[k];
        if (vi < 0 || vj < 0 || vi >= w || vj >= h) {
          edge = true;
          continue;
        }
        const v = vj * w + vi;
        if (closed[v]) continue;
        let vz = zs[v];
        if (vz === Infinity) vz = zAt(v);
        if (vz !== vz) continue; // NaN = 밖
        const len = offLen[k];
        const dz = Math.abs(vz - uz);
        if (dz > gmax * len) continue;
        let hits = 0;
        for (let s = 4 * k; s < 4 * k + 4; s++) {
          const at = u + sampleD[s];
          const state = steepState[at];
          if (state === 1 || (state === 0 && steepAt(at))) hits++;
        }
        const grade = dz / len / gmax;
        let c = len * (1 + steepShare * hits + weights.grade * grade * grade);
        if (du === du) {
          let turn = Math.abs(offDir[k] - du);
          if (turn > Math.PI) turn = 2 * Math.PI - turn;
          c += turnWeight * turn * turn;
        }
        const next = gu + c;
        if (next >= g[v]) continue;
        g[v] = next;
        parent[v] = u;
        dir[v] = offDir[k];
        const ex = goal[0] - (ux + offI[k] * cell);
        const ey = goal[1] - (uy + offJ[k] * cell);
        heap.push(next + weights.heuristic * Math.sqrt(ex * ex + ey * ey), v);
      }
    }
    return finish(best, false);

    function finish(last: number, reached: boolean): GuideSegment {
      const chain: Point3[] = [];
      for (let id = last; id !== -1; id = parent[id]) {
        if (id === START) chain.push([start[0], start[1], za]);
        else if (id === GOAL) chain.push([goal[0], goal[1], zb]);
        else chain.push([nx(id), ny(id), zs[id]]);
      }
      chain.reverse();
      return measure(chain, reached, 0, (x, y) => {
        const si = Math.round((x - origin[0]) / cell) - i0;
        const sj = Math.round((y - origin[1]) / cell) - j0;
        return si >= 0 && sj >= 0 && si < w && sj < h && steepAt(sj * w + si);
      });
    }
  }

  // 길이 · 최대 기울기 · 급경사 지남 비율(걸음마다 칸 간격 표본)
  function measure(
    vertices: Point3[],
    reached: boolean,
    expanded: number,
    steepAt: ((x: number, y: number) => boolean) | null,
  ): GuideSegment {
    let length = 0;
    let maxGrade = 0;
    let steepLength = 0;
    for (let k = 1; k < vertices.length; k++) {
      const [ax, ay, az] = vertices[k - 1];
      const [bx, by, bz] = vertices[k];
      const len = Math.hypot(bx - ax, by - ay);
      if (len <= 0) continue;
      length += len;
      maxGrade = Math.max(maxGrade, Math.abs(bz - az) / len);
      if (!steepAt) continue;
      const parts = Math.max(1, Math.ceil(len / cell));
      for (let p = 0; p < parts; p++) {
        const t = (p + 0.5) / parts;
        if (steepAt(ax + (bx - ax) * t, ay + (by - ay) * t)) steepLength += len / parts;
      }
    }
    return {
      vertices,
      reached,
      length,
      maxGrade,
      steepRatio: length > 0 ? steepLength / length : 0,
      expanded,
    };
  }
}

/** 이진 힙(값 작은 것 먼저) — 타입 배열 · 늘어남. */
class MinHeap {
  private keys = new Float64Array(1024);
  private ids = new Int32Array(1024);
  size = 0;

  push(key: number, id: number): void {
    if (this.size === this.keys.length) {
      const keys = new Float64Array(this.size * 2);
      keys.set(this.keys);
      const ids = new Int32Array(this.size * 2);
      ids.set(this.ids);
      this.keys = keys;
      this.ids = ids;
    }
    let at = this.size++;
    while (at > 0) {
      const up = (at - 1) >> 1;
      if (this.keys[up] <= key) break;
      this.keys[at] = this.keys[up];
      this.ids[at] = this.ids[up];
      at = up;
    }
    this.keys[at] = key;
    this.ids[at] = id;
  }

  pop(): number {
    const top = this.ids[0];
    const key = this.keys[--this.size];
    const id = this.ids[this.size];
    let at = 0;
    for (;;) {
      let child = 2 * at + 1;
      if (child >= this.size) break;
      if (child + 1 < this.size && this.keys[child + 1] < this.keys[child]) child++;
      if (this.keys[child] >= key) break;
      this.keys[at] = this.keys[child];
      this.ids[at] = this.ids[child];
      at = child;
    }
    this.keys[at] = key;
    this.ids[at] = id;
    return top;
  }
}