GhiVideo_v4/client/src/utils/djmdTrack.ts

/**
 * DJI 영상 내장 텔레메트리(djmd 트랙) — 프레임 단위 정밀 위치/고도 추출
 *
 * DJI MP4 에는 비디오 프레임마다 djmd 데이터 샘플(protobuf)이 기록된다:
 *   6.3 = 프레임 카운터, 6.4/6.5 = 위도/경도(라디안, double), 6.6 = 타원체고(mm)
 * 이 위치는 해당 '영상 프레임'에 하드웨어 수준으로 동기되어 있어, 외부 비행로그(CSV,
 * 10Hz + 시각 정렬 가정)의 시간 동기 오차가 원천적으로 없다 — 오르막/내리막/가감속에서
 * POI 정합("풀 붙임")의 근본 해법. 자세(yaw/pitch/roll)는 djmd 에 없어 CSV 를 유지한다.
 *
 * 구현: MP4 박스(moov→trak→stbl)를 파싱해 djmd 트랙의 샘플 오프셋 테이블을 얻고,
 * 전체 파일 스캔 없이 해당 바이트 구간만 골라 읽는다(기본 6프레임 간격 샘플링 ≈ 10Hz —
 * 동기 정확도는 프레임 단위, 밀도는 CSV 동급).
 */

import type { DroneFrame } from '../types/geo';

interface DjmdSample {
  frame: number;
  lat: number;   // deg
  lon: number;   // deg
  altM: number;  // 타원체고 (m)
}

// ── 저수준 리더 ───────────────────────────────────────────────────────

async function readAt(file: File, off: number, len: number): Promise<DataView> {
  const buf = await file.slice(off, Math.min(off + len, file.size)).arrayBuffer();
  return new DataView(buf);
}

function str4(dv: DataView, off: number): string {
  return String.fromCharCode(dv.getUint8(off), dv.getUint8(off + 1), dv.getUint8(off + 2), dv.getUint8(off + 3));
}

/** 파일 최상위에서 type 박스 탐색 (mdat 등 대형 박스는 크기로 건너뜀). */
async function findTopBox(file: File, type: string): Promise<{ off: number; size: number; hdr: number } | null> {
  let off = 0;
  while (off + 8 <= file.size) {
    const h = await readAt(file, off, 16);
    if (h.byteLength < 8) return null;
    let size = h.getUint32(0);
    const t = str4(h, 4);
    let hdr = 8;
    if (size === 1) {
      if (h.byteLength < 16) return null;
      size = Number(h.getBigUint64(8));
      hdr = 16;
    } else if (size === 0) size = file.size - off;
    if (t === type) return { off, size, hdr };
    if (size < 8) return null;
    off += size;
  }
  return null;
}

/** DataView 내 자식 박스 순회. */
function* boxes(dv: DataView, start: number, end: number): Generator<{ type: string; start: number; end: number }> {
  let off = start;
  while (off + 8 <= end) {
    let size = dv.getUint32(off);
    const type = str4(dv, off + 4);
    let hdr = 8;
    if (size === 1) {
      size = Number(dv.getBigUint64(off + 8));
      hdr = 16;
    } else if (size === 0) size = end - off;
    if (size < hdr) return;
    yield { type, start: off + hdr, end: off + Math.min(size, end - off) };
    off += size;
  }
}

function findChild(dv: DataView, start: number, end: number, type: string): { start: number; end: number } | null {
  for (const b of boxes(dv, start, end)) if (b.type === type) return { start: b.start, end: b.end };
  return null;
}

// ── protobuf 미니 파서 (cameraMeta 와 동일 방식) ──────────────────────

function readVarint(b: Uint8Array, i: number): [number, number] | null {
  let v = 0, s = 0;
  while (i < b.length && (b[i] & 0x80)) {
    v += (b[i] & 0x7f) * 2 ** s;
    s += 7; i++;
    if (s > 42) return null;
  }
  if (i >= b.length) return null;
  v += (b[i] & 0x7f) * 2 ** s;
  return [v, i + 1];
}

/** djmd 샘플에서 6.3(frame)/6.4(lat rad)/6.5(lon rad)/6.6(alt mm) 추출. */
function parseSample(b: Uint8Array): DjmdSample | null {
  const dv = new DataView(b.buffer, b.byteOffset, b.byteLength);
  let frame: number | undefined, lat: number | undefined, lon: number | undefined, alt: number | undefined;
  const walk = (start: number, end: number, depth: number, parentTag: number): void => {
    let i = start;
    while (i < end) {
      const kv = readVarint(b, i);
      if (!kv) return;
      const [key, ni] = kv;
      i = ni;
      const tag = key >> 3, wire = key & 7;
      if (tag === 0 || tag > 60) return;
      if (wire === 0) {
        const r = readVarint(b, i); if (!r) return;
        if (depth === 1 && parentTag === 6) {
          if (tag === 3) frame = r[0];
          else if (tag === 6) alt = r[0] / 1000;
        }
        i = r[1];
      } else if (wire === 5) { i += 4; }
      else if (wire === 1) {
        if (i + 8 > end) return;
        if (depth === 1 && parentTag === 6) {
          const val = dv.getFloat64(i, true) * 180 / Math.PI;
          if (tag === 4) lat = val;
          else if (tag === 5) lon = val;
        }
        i += 8;
      } else if (wire === 2) {
        const r = readVarint(b, i); if (!r) return;
        const [len, p] = r;
        if (p + len > end) return;
        if (depth === 0 && tag === 6) walk(p, p + len, 1, 6);
        i = p + len;
      } else return;
    }
  };
  walk(0, b.length, 0, 0);
  if (frame === undefined || lat === undefined || lon === undefined) return null;
  if (!(lat > -90 && lat < 90 && lon > -180 && lon < 180)) return null;
  return { frame, lat, lon, altM: alt ?? 0 };
}

// ── djmd 샘플 테이블 파싱 + 선별 읽기 ─────────────────────────────────

/**
 * 영상 파일의 djmd 트랙에서 프레임별 위치 샘플을 읽는다 (step 프레임 간격).
 * DJI 영상이 아니거나 트랙이 없으면 null.
 */
export async function parseDjmdPositions(file: File, step = 6): Promise<DjmdSample[] | null> {
  const moov = await findTopBox(file, 'moov');
  if (!moov || moov.size > 64 * 1024 * 1024) return null;
  const mv = await readAt(file, moov.off + moov.hdr, moov.size - moov.hdr);
  const end = mv.byteLength;

  // djmd stsd 를 가진 trak 의 stbl 탐색
  let stbl: { start: number; end: number } | null = null;
  for (const trak of boxes(mv, 0, end)) {
    if (trak.type !== 'trak') continue;
    const mdia = findChild(mv, trak.start, trak.end, 'mdia');
    if (!mdia) continue;
    const minf = findChild(mv, mdia.start, mdia.end, 'minf');
    if (!minf) continue;
    const st = findChild(mv, minf.start, minf.end, 'stbl');
    if (!st) continue;
    const stsd = findChild(mv, st.start, st.end, 'stsd');
    if (!stsd) continue;
    // stsd: ver/flags(4) count(4) → entry: size(4) fourcc(4)
    if (stsd.end - stsd.start >= 16 && str4(mv, stsd.start + 12) === 'djmd') { stbl = st; break; }
  }
  if (!stbl) return null;

  const stsz = findChild(mv, stbl.start, stbl.end, 'stsz');
  const stsc = findChild(mv, stbl.start, stbl.end, 'stsc');
  const stco = findChild(mv, stbl.start, stbl.end, 'stco');
  const co64 = findChild(mv, stbl.start, stbl.end, 'co64');
  if (!stsz || !stsc || (!stco && !co64)) return null;

  // stsz — 샘플 크기
  const fixedSize = mv.getUint32(stsz.start + 4);
  const sampleCount = mv.getUint32(stsz.start + 8);
  if (!sampleCount) return null;
  const sizeOf = (i: number): number => (fixedSize ? fixedSize : mv.getUint32(stsz.start + 12 + i * 4));

  // stco/co64 — 청크 오프셋
  const chunkCount = mv.getUint32((co64 ?? stco!).start + 4);
  const chunkOff = (c: number): number =>
    co64 ? Number(mv.getBigUint64(co64.start + 8 + c * 8)) : mv.getUint32(stco!.start + 8 + c * 4);

  // stsc — 청크당 샘플 수 (구간 압축) 전개
  const stscCount = mv.getUint32(stsc.start + 4);
  const stscEntry = (i: number): [number, number] => [
    mv.getUint32(stsc.start + 8 + i * 12),       // first_chunk (1-base)
    mv.getUint32(stsc.start + 8 + i * 12 + 4),   // samples_per_chunk
  ];

  // 샘플 인덱스 → 파일 오프셋 테이블 (step 간격 대상만)
  const targets: { idx: number; off: number; size: number }[] = [];
  let sampleIdx = 0, entry = 0;
  for (let c = 0; c < chunkCount && sampleIdx < sampleCount; c++) {
    while (entry + 1 < stscCount && stscEntry(entry + 1)[0] <= c + 1) entry++;
    const spc = stscEntry(entry)[1];
    let off = chunkOff(c);
    for (let s = 0; s < spc && sampleIdx < sampleCount; s++, sampleIdx++) {
      const sz = sizeOf(sampleIdx);
      if (sampleIdx % step === 0) targets.push({ idx: sampleIdx, off, size: sz });
      off += sz;
    }
  }
  if (targets.length < 5) return null;

  // 배치 읽기(동시 64) + 파싱
  const out: DjmdSample[] = [];
  const BATCH = 64;
  for (let i = 0; i < targets.length; i += BATCH) {
    const batch = targets.slice(i, i + BATCH);
    const parsed = await Promise.all(batch.map(async (t) => {
      const dv = await readAt(file, t.off, t.size);
      return parseSample(new Uint8Array(dv.buffer));
    }));
    for (const p of parsed) if (p) out.push(p);
  }
  out.sort((a, b) => a.frame - b.frame);
  return out.length >= 5 ? out : null;
}

// ── DroneFrame 정밀 위치 병합 ─────────────────────────────────────────

/**
 * CSV 기반 프레임의 위치/고도를 djmd(영상 내장, 프레임 동기) 값으로 교체하고,
 * **자세(yaw/pitch/roll)도 CSV↔영상 시간 오프셋을 자동 추정해 재정렬**한다.
 *
 * 시간 오프셋 추정: CSV 위치(시각 정렬 가정)와 djmd 위치(프레임 동기 실측)를
 * 교차상관해 평균 거리가 최소가 되는 dt 를 찾는다. 이 dt 로 자세를 재샘플하면
 * 옆으로 지나치는 지물(상대 방위가 빠르게 변함)의 라벨 미끄러짐이 제거된다.
 *
 * 고도 datum: djmd 는 타원체고 —
 *  - useEllipsoidal(타원체고 데이터셋): 그대로 사용
 *  - 정표고 데이터셋: 첫 샘플과 CSV 첫 프레임의 차(지오이드고)를 빼서 정표고로 변환
 */
export async function refineFramesWithDjmd(
  videoFile: File,
  frames: DroneFrame[],
  useEllipsoidal: boolean,
): Promise<{ frames: DroneFrame[]; dtSec: number } | null> {
  if (!frames.length) return null;
  const samples = await parseDjmdPositions(videoFile);
  if (!samples) return null;
  const FPS = 59.94; // 프레임 번호 ↔ 시간 환산 (DJI_VIRTUAL_FPS 와 동일 기준)

  // 정표고 데이터셋: 지오이드고 = djmd 타원체고 − CSV 고도 (같은 프레임 부근)
  let altShift = 0;
  if (!useEllipsoidal) {
    const s0 = samples[0];
    let nearest = frames[0];
    for (const f of frames) if (Math.abs(f.frame - s0.frame) < Math.abs(nearest.frame - s0.frame)) nearest = f;
    altShift = s0.altM - nearest.altitude;
    if (!isFinite(altShift) || Math.abs(altShift) > 80) return null; // 비정상 → 사용 안 함
  }

  // 프레임 번호 기준 선형 보간 (djmd 위치)
  const at = (frame: number): { lat: number; lon: number; altM: number } => {
    let lo = 0, hi = samples.length - 1;
    if (frame <= samples[0].frame) return samples[0];
    if (frame >= samples[hi].frame) return samples[hi];
    while (hi - lo > 1) { const m = (lo + hi) >> 1; if (samples[m].frame <= frame) lo = m; else hi = m; }
    const a = samples[lo], b = samples[hi];
    const t = b.frame > a.frame ? (frame - a.frame) / (b.frame - a.frame) : 0;
    return {
      lat: a.lat + (b.lat - a.lat) * t,
      lon: a.lon + (b.lon - a.lon) * t,
      altM: a.altM + (b.altM - a.altM) * t,
    };
  };

  // ── CSV 시각(t=frame/FPS) 기준 보간 (위치+자세) — 시간 오프셋 추정·자세 재샘플용 ──
  const ct = frames.map((f) => f.frame / FPS);
  const cosLat = Math.cos((frames[0].lat * Math.PI) / 180);
  const csvAt = (t: number): DroneFrame => {
    const n = ct.length;
    if (t <= ct[0]) return frames[0];
    if (t >= ct[n - 1]) return frames[n - 1];
    let lo = 0, hi = n - 1;
    while (hi - lo > 1) { const m = (lo + hi) >> 1; if (ct[m] <= t) lo = m; else hi = m; }
    const a = frames[lo], b = frames[hi];
    const r = ct[hi] > ct[lo] ? (t - ct[lo]) / (ct[hi] - ct[lo]) : 0;
    let dy = b.yaw - a.yaw; dy = ((dy + 540) % 360) - 180; // 최단각
    return {
      ...a,
      lat: a.lat + (b.lat - a.lat) * r,
      lon: a.lon + (b.lon - a.lon) * r,
      yaw: a.yaw + dy * r,
      pitch: a.pitch + (b.pitch - a.pitch) * r,
      roll: a.roll + (b.roll - a.roll) * r,
    };
  };

  // CSV↔영상 시간 오프셋(dt) 추정 — 위치 교차상관 (조밀 50포인트, 조사 ±1.5s → 0.01s 정밀)
  const stride = Math.max(1, Math.floor(samples.length / 50));
  const probes = samples.filter((_, i) => i % stride === 0);
  const cost = (dt: number): number => {
    let s = 0, n = 0;
    for (const p of probes) {
      const c = csvAt(p.frame / FPS + dt);
      s += Math.hypot((p.lat - c.lat) * 111000, (p.lon - c.lon) * 111000 * cosLat);
      n++;
    }
    return n ? s / n : Infinity;
  };
  let bestDt = 0, bestC = cost(0);
  for (let dt = -1.5; dt <= 1.5001; dt += 0.05) { const c = cost(dt); if (c < bestC) { bestC = c; bestDt = dt; } }
  for (let dt = bestDt - 0.05; dt <= bestDt + 0.0501; dt += 0.01) { const c = cost(dt); if (c < bestC) { bestC = c; bestDt = dt; } }
  if (Math.abs(bestDt) > 0.005) {
    console.log(`[djmd] CSV↔영상 시간 오프셋 ${bestDt.toFixed(2)}s 추정 (잔차 ${bestC.toFixed(1)}m) — 자세 재정렬 적용`);
  }

  const refined = frames.map((f) => {
    const p = at(f.frame);                       // 위치/고도: djmd (프레임 동기)
    const a = csvAt(f.frame / FPS + bestDt);     // 자세: CSV 를 추정 오프셋으로 재시각화
    return {
      ...f,
      lat: p.lat, lon: p.lon, altitude: p.altM - altShift,
      yaw: a.yaw, pitch: a.pitch, roll: a.roll,
    };
  });
  // dtSec: CSV↔영상 시간 오프셋 — 영상시간 = CSV시간 − dtSec.
  // 측점 검색(CSV 축 인덱스)이 영상 시각으로 변환할 때 사용(0.33s ≈ 2m 오차 제거).
  return { frames: refined, dtSec: bestDt };
}