import type { BeaconReading, PositionEstimate } from './positioning';
import { getBeaconKey, type MappedBeacon } from './beaconMap';
import { rssiToDistance } from './positionEstimator';

// #region agent log
let lastEstimatorLog = 0;
function agentLog(message: string, data: Record<string, unknown>, hypothesisId: string): void {
  const now = Date.now();
  if (now - lastEstimatorLog < 1000) return;
  lastEstimatorLog = now;
  fetch('https://worst-copier-smitten.ngrok-free.dev/ingest/3feb8fa8-d5b1-4f19-8791-ddc034752f2c', {
    method: 'POST',
    headers: {
      'Content-Type': 'application/json',
      'X-Debug-Session-Id': '41be73',
      'ngrok-skip-browser-warning': 'true',
    },
    body: JSON.stringify({
      sessionId: '41be73',
      runId: 'post-fix',
      location: 'hallwayEstimator.ts:offsetAlongCorridor',
      message,
      data,
      timestamp: now,
      hypothesisId,
    }),
  }).catch(() => {});
}
// #endregion

/**
 * Hallway-line position estimator for sparse beacon deployments.
 *
 * Instead of a 2D weighted centroid (which can pull the estimate off-corridor),
 * this function uses only the 1–2 strongest beacons and interpolates along
 * the line segment between them. A single beacon is placed along the corridor
 * axis at the RSSI-estimated distance instead of snapping to the pin.
 */
export function estimateHallwayPosition(
  readings: BeaconReading[],
  beaconMap: Map<string, MappedBeacon>,
  previous: { x: number; z: number } | null = null,
  metresPerUnit = 1,
): PositionEstimate | null {
  const matched: { reading: BeaconReading; mapped: MappedBeacon }[] = [];

  for (const reading of readings) {
    const key = getBeaconKey(reading.uuid, reading.major, reading.minor);
    const mapped = beaconMap.get(key);
    if (mapped) {
      matched.push({ reading, mapped });
    }
  }

  if (matched.length === 0) {
    return null;
  }

  matched.sort((a, b) => b.reading.rssi - a.reading.rssi);
  const top = matched.slice(0, 2);

  if (top.length === 1) {
    const { mapped, reading } = top[0];
    const alongCorridor = offsetAlongCorridor(mapped, reading, previous, beaconMap, metresPerUnit);
    return {
      x: alongCorridor.x,
      z: alongCorridor.z,
      floorId: mapped.floorId,
      accuracy: 'low',
      timestamp: Date.now(),
    };
  }

  const [a, b] = top;

  const dA = Math.max(
    a.reading.distanceMeters ?? rssiToDistance(a.reading.rssi, a.mapped.txPower, a.mapped.pathLossN),
    0.1,
  );
  const dB = Math.max(
    b.reading.distanceMeters ?? rssiToDistance(b.reading.rssi, b.mapped.txPower, b.mapped.pathLossN),
    0.1,
  );

  const t = dA / (dA + dB);

  return {
    x: a.mapped.x + t * (b.mapped.x - a.mapped.x),
    z: a.mapped.z + t * (b.mapped.z - a.mapped.z),
    floorId: a.mapped.floorId,
    accuracy: 'medium',
    timestamp: Date.now(),
  };
}

/**
 * Place a 1-beacon estimate on the corridor axis at the RSSI range, not on the pin.
 *
 * `rssiToDistance` returns metres while the scene is a normalised 10-unit space,
 * so the range must be divided by `metresPerUnit` before it is used as a model
 * offset. The offset is also capped at half the spacing to the nearest beacon:
 * past that midpoint the neighbour would be the stronger beacon, so a larger
 * offset is never physically consistent with this beacon being the top match.
 */
function offsetAlongCorridor(
  mapped: MappedBeacon,
  reading: BeaconReading,
  previous: { x: number; z: number } | null,
  beaconMap: Map<string, MappedBeacon>,
  metresPerUnit: number,
): { x: number; z: number } {
  const rangeMetres = Math.max(
    reading.distanceMeters ?? rssiToDistance(reading.rssi, mapped.txPower, mapped.pathLossN),
    0.25,
  );
  const rangeUnits = rangeMetres / Math.max(metresPerUnit, 0.01);

  let axisX = 0;
  let axisZ = 0;
  let best = Infinity;
  for (const other of beaconMap.values()) {
    const dx = other.x - mapped.x;
    const dz = other.z - mapped.z;
    const mag2 = dx * dx + dz * dz;
    if (mag2 < 0.05 || mag2 >= best) continue;
    best = mag2;
    axisX = dx;
    axisZ = dz;
  }

  const axisMag = Math.hypot(axisX, axisZ);
  if (axisMag < 0.01) {
    if (previous) return previous;
    return { x: mapped.x, z: mapped.z };
  }

  const offset = Math.min(rangeUnits, axisMag / 2);
  const ux = axisX / axisMag;
  const uz = axisZ / axisMag;
  const pPos = { x: mapped.x + ux * offset, z: mapped.z + uz * offset };
  const pNeg = { x: mapped.x - ux * offset, z: mapped.z - uz * offset };

  // #region agent log
  agentLog('single beacon corridor offset', {
    rssi: reading.rssi,
    txPower: mapped.txPower,
    pathLossN: mapped.pathLossN,
    rangeMetres,
    rangeUnits,
    axisMag,
    halfSpacingUnits: axisMag / 2,
    halfSpacingMetres: (axisMag / 2) * metresPerUnit,
    offset,
    clampedToHalfSpacing: rangeUnits >= axisMag / 2,
    metresPerUnit,
    beaconX: mapped.x,
    beaconZ: mapped.z,
    pPosX: pPos.x,
    pNegX: pNeg.x,
    hasPrevious: previous !== null,
  }, 'F');
  // #endregion

  if (!previous) {
    return { x: mapped.x, z: mapped.z };
  }

  const dPos = Math.hypot(previous.x - pPos.x, previous.z - pPos.z);
  const dNeg = Math.hypot(previous.x - pNeg.x, previous.z - pNeg.z);
  return dPos <= dNeg ? pPos : pNeg;
}
