/**
 * editRobot.js
 * Hilfsfunktionen zum Bearbeiten von robot_xxx.json.
 * Alle Schreibvorgänge machen ein Backup-Kommentar in der Datei (nein, aber
 * atomisches Write per Temp-Datei ist hier nicht nötig – die Datei wird direkt
 * überschrieben; bei Bedarf Backup-Strategie ergänzen).
 */
import fsPromises from 'fs/promises';

// ── I/O ───────────────────────────────────────────────────────────────────────

async function readRobot(robotPath) {
  return JSON.parse(await fsPromises.readFile(robotPath, 'utf8'));
}

async function writeRobot(robotPath, data) {
  await fsPromises.writeFile(robotPath, JSON.stringify(data, null, 2), 'utf8');
}

// ── Aktion 1: Marker nach Z-Bereich zuordnen ─────────────────────────────────

/**
 * Weist allen Markern, deren z-Position (mm) zwischen zMin und zMax liegt,
 * das angegebene Set und/oder den angegebenen Link zu.
 *
 * - set          (optional): Setzt den set-Wert des Markers.
 * - link         (optional): Verschiebt den Marker in diesen Link (wird ggf. angelegt).
 * - extraMarkers (optional): Triangulierte Marker aus aruco_marker_poses.json –
 *                            werden als neue Einträge in robot.json hinzugefügt,
 *                            wenn sie noch nicht vorhanden sind.
 *
 * Gibt { numChanged, changes[] } zurück.
 * changes[]: { markerId, action, oldLink, newLink, oldSet, newSet }
 *   action: 'updated' | 'added'
 */
export async function assignByZRange(robotPath, { zMin, zMax, set, link, extraMarkers = [] }) {
  const robot = await readRobot(robotPath);
  const links = robot.links ?? {};
  const changes = [];

  const zLo = Number(zMin);
  const zHi = Number(zMax);

  // ── Teil 1: Bestehende robot.json-Marker aktualisieren / verschieben ──────────
  const snapshot = [];
  for (const [linkName, linkData] of Object.entries(links)) {
    for (const marker of (linkData.markers ?? [])) {
      if (Array.isArray(marker.position)) snapshot.push({ id: marker.id, currentLink: linkName });
    }
  }

  for (const { id, currentLink } of snapshot) {
    const srcLinkData = links[currentLink];
    const idx = (srcLinkData?.markers ?? []).findIndex(m => Number(m.id) === id);
    if (idx === -1) continue;

    const marker = srcLinkData.markers[idx];
    const z = Number(marker.position[2]);
    if (z < zLo || z > zHi) continue;

    const change = {
      action:  'updated',
      markerId: marker.id,
      oldLink:  currentLink,
      oldSet:   marker.set ?? '',
      newLink:  currentLink,
      newSet:   marker.set ?? '',
    };

    if (set !== undefined && set !== '') { marker.set = set; change.newSet = set; }

    if (link && link !== currentLink) {
      srcLinkData.markers.splice(idx, 1);
      if (!links[link])         links[link]         = { markers: [] };
      if (!links[link].markers) links[link].markers = [];
      links[link].markers.push(marker);
      change.newLink = link;
    }

    changes.push(change);
  }

  // ── Teil 2: Neue Marker aus 3b-Triangulation einfügen (noch nicht in robot.json) ──
  // Diese haben keine Position in robot.json – wir übernehmen die gemessene Position.
  if (link && extraMarkers.length > 0) {
    const knownIds = new Set();
    for (const ld of Object.values(links)) {
      for (const m of (ld.markers ?? [])) knownIds.add(Number(m.id));
    }

    for (const em of extraMarkers) {
      const emId  = Number(em.marker_id);
      const emPos = em.position_mm;           // [x_mm, y_mm, z_mm] robot-Koordinaten
      if (knownIds.has(emId)) continue;       // bereits in robot.json (evtl. gerade hinzugefügt)
      if (!Array.isArray(emPos) || emPos.length < 3) continue;

      const z = Number(emPos[2]);
      if (z < zLo || z > zHi) continue;

      const newMarker = {
        id:       emId,
        position: emPos.map(v => Math.round(Number(v) * 100) / 100),  // 2 Dezimalstellen
      };
      if (set) newMarker.set = set;

      if (!links[link])         links[link]         = { markers: [] };
      if (!links[link].markers) links[link].markers = [];
      links[link].markers.push(newMarker);
      knownIds.add(emId);

      changes.push({
        action:   'added',
        markerId: emId,
        oldLink:  null,
        oldSet:   '',
        newLink:  link,
        newSet:   set ?? '',
      });
    }
  }

  if (changes.length > 0) {
    robot.links = links;
    await writeRobot(robotPath, robot);
  }

  return { numChanged: changes.length, changes };
}

// ── Aktion 2: Set oder Link-Zuordnung entfernen ───────────────────────────────

/**
 * Entfernt die Set- oder Link-Zuordnung eines Markers.
 *
 * removeFrom: 'set'  → löscht nur den set-Wert (Marker bleibt im Link)
 * removeFrom: 'link' → entfernt Marker komplett aus dem Link
 *                      (nur wenn set bereits leer/nicht gesetzt)
 *
 * Gibt { changed, markerId, action, link, [oldSet], [error] } zurück.
 */
export async function removeMarkerAssignment(robotPath, { markerId, removeFrom }) {
  const id = Number(markerId);
  const robot = await readRobot(robotPath);
  const links = robot.links ?? {};

  for (const [linkName, linkData] of Object.entries(links)) {
    const markers = linkData.markers ?? [];
    const idx = markers.findIndex(m => Number(m.id) === id);
    if (idx === -1) continue;

    const marker = markers[idx];

    if (removeFrom === 'set') {
      const oldSet = marker.set ?? '';
      delete marker.set;
      await writeRobot(robotPath, robot);
      return { changed: true, markerId: id, action: 'set-removed', link: linkName, oldSet };
    }

    if (removeFrom === 'link') {
      if (marker.set) {
        return {
          changed: false,
          markerId: id,
          error: `Marker ${id} hat noch Set "${marker.set}". Bitte zuerst Set entfernen.`,
        };
      }
      markers.splice(idx, 1);
      await writeRobot(robotPath, robot);
      return { changed: true, markerId: id, action: 'link-removed', link: linkName };
    }

    return { changed: false, error: `Unbekannte Aktion: "${removeFrom}"` };
  }

  return { changed: false, error: `Marker-ID ${id} nicht gefunden.` };
}