/** Deterministic snapshot interpolation with deliberately bounded prediction. */

import type {
  BufferedPoseSample,
  EntityPoseSnapshot,
  InterpolationOptions,
  PoseVelocity,
  SpatialPose,
} from "./types.ts";

interface ResolvedOptions {
  interpolationDelayMs: number;
  maximumExtrapolationMs: number;
  capacity: number;
}

function clamp(value: number, min: number, max: number): number {
  return Math.max(min, Math.min(max, value));
}

function options(value: InterpolationOptions): ResolvedOptions {
  return {
    interpolationDelayMs: clamp(value.interpolationDelayMs ?? 100, 0, 2_000),
    maximumExtrapolationMs: clamp(value.maximumExtrapolationMs ?? 150, 0, 1_000),
    capacity: Math.floor(clamp(value.capacity ?? 32, 2, 256)),
  };
}

function angle(from: number, to: number, t: number): number {
  const delta = ((to - from + 540) % 360) - 180;
  return from + delta * t;
}

function compatible(a: SpatialPose, b: SpatialPose): boolean {
  if (a.space !== b.space) return false;
  if (a.space === "geographic") return true;
  return b.space === "local" && JSON.stringify(a.cell) === JSON.stringify(b.cell);
}

function mixVelocity(a: PoseVelocity, b: PoseVelocity, t: number): PoseVelocity {
  return {
    xMps: a.xMps + (b.xMps - a.xMps) * t,
    yMps: a.yMps + (b.yMps - a.yMps) * t,
    zMps: a.zMps + (b.zMps - a.zMps) * t,
    yawDegPerSec: a.yawDegPerSec + (b.yawDegPerSec - a.yawDegPerSec) * t,
  };
}

function mixPose(a: SpatialPose, b: SpatialPose, t: number): SpatialPose {
  if (!compatible(a, b)) return { ...b };
  if (a.space === "geographic" && b.space === "geographic") {
    return {
      space: "geographic",
      lat: a.lat + (b.lat - a.lat) * t,
      lng: a.lng + (b.lng - a.lng) * t,
      altitudeM: a.altitudeM + (b.altitudeM - a.altitudeM) * t,
      headingDeg: angle(a.headingDeg, b.headingDeg, t),
      pitchDeg: a.pitchDeg + (b.pitchDeg - a.pitchDeg) * t,
    };
  }
  if (a.space === "local" && b.space === "local") {
    return {
      space: "local",
      cell: { ...b.cell },
      xM: a.xM + (b.xM - a.xM) * t,
      yM: a.yM + (b.yM - a.yM) * t,
      zM: a.zM + (b.zM - a.zM) * t,
      headingDeg: angle(a.headingDeg, b.headingDeg, t),
      pitchDeg: a.pitchDeg + (b.pitchDeg - a.pitchDeg) * t,
    };
  }
  return { ...b };
}

function predictPose(pose: SpatialPose, velocity: PoseVelocity, seconds: number): SpatialPose {
  if (pose.space === "local") {
    return {
      ...pose,
      cell: { ...pose.cell },
      xM: pose.xM + velocity.xMps * seconds,
      yM: pose.yM + velocity.yMps * seconds,
      zM: pose.zM + velocity.zMps * seconds,
      headingDeg: pose.headingDeg + velocity.yawDegPerSec * seconds,
    };
  }
  // Geographic velocity uses east/up/north axes matching local x/y/z.
  const earth = 6_371_000;
  const latitudeRadians = pose.lat * Math.PI / 180;
  return {
    ...pose,
    lat: pose.lat + (velocity.zMps * seconds / earth) * 180 / Math.PI,
    lng: pose.lng + (velocity.xMps * seconds / (earth * Math.max(0.01, Math.cos(latitudeRadians)))) * 180 / Math.PI,
    altitudeM: pose.altitudeM + velocity.yMps * seconds,
    headingDeg: pose.headingDeg + velocity.yawDegPerSec * seconds,
  };
}

function id(snapshot: EntityPoseSnapshot): string {
  if (snapshot.entity === "actor") return `actor:${snapshot.actorId}`;
  if (snapshot.entity === "vehicle") return `vehicle:${snapshot.vehicleId}`;
  return `aircraft:${snapshot.aircraftId}`;
}

export class PoseInterpolationBuffer {
  private readonly settings: ResolvedOptions;
  private readonly snapshots: EntityPoseSnapshot[] = [];
  private entityId: string | null = null;

  constructor(optionsValue: InterpolationOptions = {}) {
    this.settings = options(optionsValue);
  }

  clear(): void {
    this.snapshots.length = 0;
    this.entityId = null;
  }

  size(): number {
    return this.snapshots.length;
  }

  /** Reject duplicates, stale sequence numbers, and accidental entity mixing. */
  push(snapshot: EntityPoseSnapshot): boolean {
    const incomingId = id(snapshot);
    if (this.entityId !== null && incomingId !== this.entityId) return false;
    const latest = this.snapshots.at(-1);
    if (latest && (snapshot.sequence <= latest.sequence || snapshot.timestampMs <= latest.timestampMs)) {
      return false;
    }
    this.entityId = incomingId;
    this.snapshots.push(snapshot);
    if (this.snapshots.length > this.settings.capacity) this.snapshots.shift();
    return true;
  }

  /** Sample server time minus interpolation delay; never predicts past the configured cap. */
  sample(serverNowMs: number): BufferedPoseSample | null {
    if (!Number.isFinite(serverNowMs) || this.snapshots.length === 0) return null;
    const target = serverNowMs - this.settings.interpolationDelayMs;
    const first = this.snapshots[0];
    const latest = this.snapshots.at(-1);
    if (!first || !latest) return null;
    if (target <= first.timestampMs) return this.from(first, "held", first.timestampMs);

    for (let index = 1; index < this.snapshots.length; index += 1) {
      const after = this.snapshots[index];
      const before = this.snapshots[index - 1];
      if (!after || !before || target > after.timestampMs) continue;
      if (target === after.timestampMs) return this.from(after, "exact", target);
      if (!compatible(before.pose, after.pose)) return this.from(after, "held", target);
      const t = (target - before.timestampMs) / (after.timestampMs - before.timestampMs);
      return {
        mode: "interpolated",
        pose: mixPose(before.pose, after.pose, t),
        velocity: mixVelocity(before.velocity, after.velocity, t),
        timestampMs: target,
        sourceSequence: after.sequence,
      };
    }

    const extrapolationMs = clamp(target - latest.timestampMs, 0, this.settings.maximumExtrapolationMs);
    if (extrapolationMs === 0) return this.from(latest, "exact", latest.timestampMs);
    return {
      mode: target - latest.timestampMs > this.settings.maximumExtrapolationMs ? "held" : "extrapolated",
      pose: predictPose(latest.pose, latest.velocity, extrapolationMs / 1_000),
      velocity: { ...latest.velocity },
      timestampMs: latest.timestampMs + extrapolationMs,
      sourceSequence: latest.sequence,
    };
  }

  private from(snapshot: EntityPoseSnapshot, mode: "exact" | "held", timestampMs: number): BufferedPoseSample {
    return {
      mode,
      pose: snapshot.pose.space === "local"
        ? { ...snapshot.pose, cell: { ...snapshot.pose.cell } }
        : { ...snapshot.pose },
      velocity: { ...snapshot.velocity },
      timestampMs,
      sourceSequence: snapshot.sequence,
    };
  }
}
