/**
 * homingXEstimate.cjs
 * ===================
 * Reine Geometrie-Logik zur Schätzung der Slider-X-Position (kein I/O, kein fs).
 *
 * Bewusst CommonJS (`.cjs`): so kann sowohl der ESM-Server
 * (`server/homingOrchestrator.js` via Node-Interop `import { … } from './…cjs'`)
 * als auch Jest (`require('../server/homingXEstimate.cjs')`) dieselbe Logik nutzen.
 * Folgt damit dem Repo-Muster „pure Logik herauslösen" (vgl. `public/yAxisCompute.js`),
 * nur als `.cjs`, weil der Importeur ein ESM-Modul unter `"type":"module"` ist.
 */

/**
 * Modell-Welt-x eines Markers bei slider=0, durch Aufsummieren der origin.x
 * entlang der Kette Link→…→Base.
 *
 * Das Ergebnis ist winkel-unabhängig (und damit exakt) genau dann, wenn alle
 * revolute-Gelenke der Kette um die x-Achse drehen — Rotation um x erhält die
 * x-Koordinate. Sobald ein revolute-Gelenk um eine andere Achse dreht, hängt
 * das Welt-x vom (hier unbekannten) Winkel ab → xSafe=false.
 *
 * @param {object}   links     robot.json `links`
 * @param {string}   linkName  Link des Markers
 * @param {number[]} localPos  Marker-Position im lokalen Link-Frame [x,y,z]
 * @returns {{ worldX: number, xSafe: boolean }}
 */
function modelWorldXAtSliderZero(links, linkName, localPos) {
  let xOffset = 0;
  let xSafe   = true;
  let cur     = linkName;
  const seen  = new Set();

  while (cur && links?.[cur]?.jointToParent && !seen.has(cur)) {
    seen.add(cur);
    const jtp = links[cur].jointToParent;
    xOffset += jtp.origin?.[0] ?? 0;

    const axis    = jtp.axis ?? [0, 0, 0];
    const isXAxis = Math.abs(axis[0]) === 1 && axis[1] === 0 && axis[2] === 0;
    if (jtp.type === 'revolute' && !isXAxis) xSafe = false;

    cur = links[cur].parent;
  }
  return { worldX: xOffset + (localPos?.[0] ?? 0), xSafe };
}

/**
 * Schätzt die Slider-X-Position aus bereits geparsten Daten.
 *
 * Für jeden beobachteten Arm-Marker wird der implizierte Slider-Wert berechnet:
 *   slider_i = beobachtetes_world_x  −  Modell_world_x(slider=0)
 * und über alle x-zuverlässigen Marker gemittelt. So wird der kinematische
 * Gelenk-Offset (z.B. Arm1.origin.x = 110 mm) korrekt herausgerechnet.
 *
 * Übersprungen werden: Board-Marker, Marker ohne Modell-Eintrag (unbekannte ID),
 * Marker nicht-x-zuverlässiger Ketten und Marker ohne beobachtetes x.
 *
 * Fallback (keine zuverlässigen Marker, z.B. ohne robot.json): roher Mittelwert
 * der world-x aller Nicht-Board-Marker – nur Notlösung.
 *
 * @param {{ markers?: Array<{ marker_id:number, link?:string, position_mm?:number[] }> }} arucoData
 * @param {object} links  robot.json `links` (oder {} falls nicht vorhanden)
 * @returns {number} x_mm
 */
function estimateXFromParsed(arucoData, links) {
  const markers = arucoData?.markers ?? [];
  const samples = [];

  for (const obs of markers) {
    if (!obs.link || obs.link === 'Board') continue;
    const modelMarker = links?.[obs.link]?.markers?.find(m => m.id === obs.marker_id);
    if (!modelMarker?.position) continue;
    const { worldX, xSafe } = modelWorldXAtSliderZero(links, obs.link, modelMarker.position);
    if (!xSafe) continue;
    const obsX = obs.position_mm?.[0];
    if (obsX == null) continue;
    samples.push(obsX - worldX);
  }

  if (samples.length > 0) {
    return samples.reduce((a, b) => a + b, 0) / samples.length;
  }

  // ── Fallback: alter, geometrisch ungenauer Mittelwert ──
  const armMarkers = markers.filter(m => m.link && m.link !== 'Board');
  if (armMarkers.length === 0) return 0.0;
  return armMarkers.reduce((s, m) => s + (m.position_mm?.[0] ?? 0), 0) / armMarkers.length;
}

module.exports = { modelWorldXAtSliderZero, estimateXFromParsed };