/**
 * 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.` };
}

// ── Aktion 3: Sets untereinander justieren (2D+Z Kabsch) ─────────────────────

/**
 * Optimale 2D-Rotation (um Z-Achse) + 3D-Translation, die model→measured minimiert.
 * Gibt { tx, ty, tz, theta, thetaDeg } zurück.
 * modelPts / measuredPts: Arrays von [x, y, z] in mm.
 */
function computeRigid2DZ(modelPts, measuredPts) {
  const n = modelPts.length;
  if (n === 0) return { tx: 0, ty: 0, tz: 0, theta: 0, thetaDeg: 0 };

  // Schwerpunkte
  let mCx = 0, mCy = 0, mCz = 0, pCx = 0, pCy = 0, pCz = 0;
  for (let i = 0; i < n; i++) {
    mCx += modelPts[i][0];   mCy += modelPts[i][1];   mCz += modelPts[i][2];
    pCx += measuredPts[i][0]; pCy += measuredPts[i][1]; pCz += measuredPts[i][2];
  }
  mCx /= n; mCy /= n; mCz /= n;
  pCx /= n; pCy /= n; pCz /= n;

  // 2D-Kreuzkovarianz für Kabsch-Rotation in XY-Ebene
  let H00 = 0, H01 = 0, H10 = 0, H11 = 0;
  for (let i = 0; i < n; i++) {
    const ax = modelPts[i][0] - mCx,   ay = modelPts[i][1] - mCy;
    const bx = measuredPts[i][0] - pCx, by = measuredPts[i][1] - pCy;
    H00 += ax * bx; H01 += ax * by;
    H10 += ay * bx; H11 += ay * by;
  }

  // Optimaler Drehwinkel (2D-Sonderfall von Kabsch / SVD → atan2)
  const theta = Math.atan2(H10 - H01, H00 + H11);
  const cos   = Math.cos(theta);
  const sin   = Math.sin(theta);

  // Translation: gemessener Schwerpunkt − R × Modell-Schwerpunkt
  const tx = pCx - (cos * mCx - sin * mCy);
  const ty = pCy - (sin * mCx + cos * mCy);
  const tz = pCz - mCz;

  return { tx, ty, tz, theta, thetaDeg: theta * 180 / Math.PI };
}

function applyRigid2DZ([x, y, z], { tx, ty, tz, theta }) {
  const cos = Math.cos(theta);
  const sin = Math.sin(theta);
  return [
    Math.round((cos * x - sin * y + tx) * 100) / 100,
    Math.round((sin * x + cos * y + ty) * 100) / 100,
    Math.round((z + tz) * 100) / 100,
  ];
}

/**
 * Richtet alle Marker des Sets `setToMove` rigid an ihren triangulieren 3b-Positionen aus.
 * Das andere Set bleibt unberührt – die Transformation wird aus den Paaren
 * (Modellposition, Messposition) der matching Marker berechnet.
 *
 * setToMove:    Name des zu verschiebenden Sets (z.B. "rail")
 * extraMarkers: Marker aus aruco_marker_poses.json (mit marker_id, position_mm)
 *
 * Gibt { numChanged, numMatchingPts, transform: {tx, ty, tz, thetaDeg} } zurück,
 * oder { error } bei Fehler.
 */
export async function alignSetToMeasured(robotPath, { setToMove, extraMarkers = [] }) {
  const robot = await readRobot(robotPath);
  const links = robot.links ?? {};

  // Alle Marker des zu verschiebenden Sets sammeln
  const setMarkers = [];
  for (const [linkName, linkData] of Object.entries(links)) {
    for (const marker of (linkData.markers ?? [])) {
      if (marker.set === setToMove) {
        setMarkers.push({ marker, linkName });
      }
    }
  }

  if (setMarkers.length === 0) {
    return { error: `Set "${setToMove}" nicht gefunden oder leer.` };
  }

  // Gemessene Positionen aus 3b indizieren
  const measuredMap = new Map();
  for (const em of extraMarkers) {
    if (Array.isArray(em.position_mm) && em.position_mm.length >= 3) {
      measuredMap.set(Number(em.marker_id), em.position_mm.map(Number));
    }
  }

  // Matching-Paare für Kabsch (nur Marker, die sowohl Modellposition als auch Messung haben)
  const modelPts = [], measuredPts = [];
  for (const { marker } of setMarkers) {
    if (!Array.isArray(marker.position) || marker.position.length < 3) continue;
    const mpos = measuredMap.get(Number(marker.id));
    if (!mpos) continue;
    modelPts.push(marker.position.map(Number));
    measuredPts.push(mpos);
  }

  if (modelPts.length < 2) {
    return {
      error: `Zu wenig Messpunkte für Set "${setToMove}" (${modelPts.length} von ${setMarkers.length} Markern gemessen). ` +
             `Bitte Board-Run mit ≥2 Kameras durchführen.`,
    };
  }

  const transform = computeRigid2DZ(modelPts, measuredPts);

  // Transformation auf ALLE Marker des Sets anwenden (auch nicht gemessene)
  let numChanged = 0;
  for (const { marker } of setMarkers) {
    if (!Array.isArray(marker.position) || marker.position.length < 3) continue;
    marker.position = applyRigid2DZ(marker.position.map(Number), transform);
    numChanged++;
  }

  robot.links = links;
  await writeRobot(robotPath, robot);

  return {
    numChanged,
    numMatchingPts: modelPts.length,
    transform: {
      tx:       Math.round(transform.tx       * 100) / 100,
      ty:       Math.round(transform.ty       * 100) / 100,
      tz:       Math.round(transform.tz       * 100) / 100,
      thetaDeg: Math.round(transform.thetaDeg * 100) / 100,
    },
  };
}

// ── Aktion 4: Einzelnen Marker per ID hinzufügen / aktualisieren ──────────────

/**
 * Fügt einen Marker per ID zu Set und Link hinzu oder aktualisiert einen bestehenden.
 *
 * - Existiert der Marker bereits in robot.json: set und/oder link werden aktualisiert.
 * - Existiert er noch nicht: Position wird aus extraMarkers (3b-Output) geholt und
 *   der Marker wird dem angegebenen Link hinzugefügt.
 *
 * Gibt { changed, change } oder { changed: false, error } zurück.
 */
export async function assignMarkerId(robotPath, { markerId, set, link, extraMarkers = [] }) {
  const id = Number(markerId);
  if (!Number.isFinite(id) || id < 0) return { changed: false, error: 'Ungültige Marker-ID.' };

  const robot = await readRobot(robotPath);
  const links = robot.links ?? {};

  // Marker in robot.json suchen
  let foundMarker = null, foundLink = null;
  outer: for (const [linkName, linkData] of Object.entries(links)) {
    for (const m of (linkData.markers ?? [])) {
      if (Number(m.id) === id) { foundMarker = m; foundLink = linkName; break outer; }
    }
  }

  if (foundMarker) {
    // Bestehenden Marker: Set und ggf. Link aktualisieren
    const change = {
      action:  'updated',
      markerId: id,
      oldLink:  foundLink,
      oldSet:   foundMarker.set ?? '',
      newLink:  foundLink,
      newSet:   foundMarker.set ?? '',
    };

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

    if (link && link !== foundLink) {
      // In neuen Link verschieben
      links[foundLink].markers = links[foundLink].markers.filter(m => Number(m.id) !== id);
      if (!links[link])         links[link]         = { markers: [] };
      if (!links[link].markers) links[link].markers = [];
      links[link].markers.push(foundMarker);
      change.newLink = link;
    }

    robot.links = links;
    await writeRobot(robotPath, robot);
    return { changed: true, change };
  }

  // Neuer Marker: Position aus 3b-Output holen
  const em = extraMarkers.find(m => Number(m.marker_id) === id);
  if (!em) {
    return {
      changed: false,
      error: `Marker ${id} ist nicht in robot.json und nicht im letzten 3b-Run vorhanden.`,
    };
  }
  if (!link) {
    return { changed: false, error: 'Link muss angegeben werden, um einen neuen Marker hinzuzufügen.' };
  }

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

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

  robot.links = links;
  await writeRobot(robotPath, robot);
  return {
    changed: true,
    change: { action: 'added', markerId: id, oldLink: null, oldSet: '', newLink: link, newSet: set ?? '' },
  };
}

// ── Aktion 5: X-Achse übernehmen ─────────────────────────────────────────────

/**
 * Rotiert alle Marker-Positionen in robot.json so, dass die übergebene Richtung
 * zur neuen X-Achse [1,0,0] wird.  Rotation um den Schwerpunkt aller A0-Marker
 * (Origin bleibt erhalten).
 *
 * direction: [vx, vy, vz] – gemessene Ist-X-Richtung in Roboter-Koordinaten
 *
 * Algorithmus:
 *   1. Normalisiere direction, ggf. Vorzeichen so dass vx > 0
 *   2. Baue Orthonormalbasis: x_new = v,
 *                              z_new = Gram-Schmidt([0,0,1] gegen v),
 *                              y_new = cross(z_new, x_new)
 *   3. Rotationsmatrix R (old→new): Zeilen = [x_new, y_new, z_new]
 *   4. p_new = origin + R * (p_old − origin)
 */
export async function adoptXAxis(robotPath, { direction }) {
  const [vx, vy, vz] = direction.map(Number);
  const len = Math.sqrt(vx * vx + vy * vy + vz * vz);
  if (len < 1e-9) throw new Error('Richtungsvektor zu klein (fast Null-Vektor).');

  // Normalisieren, immer positive X-Komponente
  let nx = vx / len, ny = vy / len, nz = vz / len;
  if (nx < 0) { nx = -nx; ny = -ny; nz = -nz; }

  // ── Orthonormalbasis ──────────────────────────────────────────────────────
  // Z_new: Gram-Schmidt von [0,0,1] gegen x_new
  const dotZ = nz;  // dot([0,0,1], [nx,ny,nz])
  let zx = -dotZ * nx,  zy = -dotZ * ny,  zz = 1 - dotZ * nz;
  let zlen = Math.sqrt(zx * zx + zy * zy + zz * zz);
  if (zlen < 1e-9) {
    // Sonderfall: x_new fast parallel zu Z – Fallback auf [0,1,0]
    const dotY = ny;
    zx = -dotY * nx;  zy = 1 - dotY * ny;  zz = -dotY * nz;
    zlen = Math.sqrt(zx * zx + zy * zy + zz * zz);
  }
  zx /= zlen;  zy /= zlen;  zz /= zlen;

  // Y_new = cross(z_new, x_new)  [rechte-Hand-Regel: ẑ × x̂ = ŷ]
  const yx = zy * nz - zz * ny;
  const yy = zz * nx - zx * nz;
  const yz = zx * ny - zy * nx;

  // Rotationsfunktion: p_rot = R * p  (R hat Zeilen = neue Achsen in alten Koordinaten)
  function rotVec(px, py, pz) {
    return [
      nx * px + ny * py + nz * pz,   // neue X-Komponente
      yx * px + yy * py + yz * pz,   // neue Y-Komponente
      zx * px + zy * py + zz * pz,   // neue Z-Komponente
    ];
  }

  const robot = await readRobot(robotPath);
  const links = robot.links ?? {};

  // ── Ursprung: Schwerpunkt aller A0-Marker ────────────────────────────────
  const a0Pos = [];
  for (const ld of Object.values(links)) {
    for (const m of (ld.markers ?? [])) {
      if (m.set === 'A0' && Array.isArray(m.position) && m.position.length >= 3) {
        a0Pos.push(m.position.map(Number));
      }
    }
  }
  let ox = 0, oy = 0, oz = 0;
  if (a0Pos.length > 0) {
    for (const [px, py, pz] of a0Pos) { ox += px;  oy += py;  oz += pz; }
    ox /= a0Pos.length;  oy /= a0Pos.length;  oz /= a0Pos.length;
  }

  // ── Alle Marker rotieren ──────────────────────────────────────────────────
  let numChanged = 0;
  for (const ld of Object.values(links)) {
    for (const m of (ld.markers ?? [])) {
      if (!Array.isArray(m.position) || m.position.length < 3) continue;
      const [px, py, pz] = m.position.map(Number);
      const [rx, ry, rz] = rotVec(px - ox, py - oy, pz - oz);
      m.position = [
        Math.round((ox + rx) * 100) / 100,
        Math.round((oy + ry) * 100) / 100,
        Math.round((oz + rz) * 100) / 100,
      ];
      numChanged++;
    }
  }

  robot.links = links;
  await writeRobot(robotPath, robot);

  return {
    numChanged,
    origin:     [ox, oy, oz].map(v => Math.round(v * 10) / 10),
    newXAxis:   [nx, ny, nz].map(v => Math.round(v * 10000) / 10000),
    angleXYdeg: Math.round(Math.atan2(ny, nx) * 18000 / Math.PI) / 100,
    angleXZdeg: Math.round(Math.atan2(nz, nx) * 18000 / Math.PI) / 100,
  };
}

// ── Aktion 6: Fixe Marker einem Link zuordnen ─────────────────────────────────

/**
 * Ordnet Marker, die sich bei einer Gelenk-Rotation kaum bewegen, dem angegebenen
 * Link zu.  Typischer Anwendungsfall: Marker auf dem Basis-Körper werden als
 * "Base"-Marker in robot.json registriert.
 *
 * - markerIds:          Array von Marker-IDs (number[])
 * - targetLink:         Ziel-Link-Name (z. B. 'Base')
 * - measuredPositions:  Array von { id, position_mm:[x,y,z] } aus Pos A
 *
 * Gibt { numAdded, numAlreadyPresent, changes[] } zurück.
 * Bestehende Einträge werden NICHT verschoben (überschreiben wäre destruktiv).
 */
export async function assignFixedMarkersToLink(robotPath, { markerIds, targetLink, measuredPositions = [] }) {
  if (!targetLink) throw new Error('targetLink muss angegeben werden.');

  const robot = await readRobot(robotPath);
  const links = robot.links ?? {};

  // Index: markerId → aktueller Link
  const existingMap = new Map();
  for (const [linkName, linkData] of Object.entries(links)) {
    for (const m of (linkData.markers ?? [])) {
      existingMap.set(Number(m.id), linkName);
    }
  }

  // Pos-A-Positionen indizieren
  const posMap = new Map();
  for (const p of measuredPositions) {
    posMap.set(Number(p.id), p.position_mm);
  }

  if (!links[targetLink])         links[targetLink]         = { markers: [] };
  if (!links[targetLink].markers) links[targetLink].markers = [];

  const changes = [];
  let numAdded = 0, numAlreadyPresent = 0;

  for (const rawId of markerIds) {
    const id = Number(rawId);
    if (existingMap.has(id)) {
      numAlreadyPresent++;
      changes.push({ action: 'already-present', markerId: id, existingLink: existingMap.get(id) });
      continue;
    }

    const pos = posMap.get(id);
    if (!pos || pos.length < 3) {
      changes.push({ action: 'skipped-no-position', markerId: id });
      continue;
    }

    links[targetLink].markers.push({
      id,
      position: pos.map(v => Math.round(Number(v) * 10) / 10),  // 1 Dezimalstelle (mm)
      positionSource: 'calibration-fixed-detection',
    });
    numAdded++;
    changes.push({ action: 'added', markerId: id, targetLink });
  }

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

  return { numAdded, numAlreadyPresent, changes };
}

// ── Aktion 7: Joint-Origin Y/Z aus Drehachse übernehmen ──────────────────────

/**
 * Setzt die Y- und Z-Koordinaten des jointToParent.origin eines Links.
 *
 * Der Ursprung der Drehachse (axisPoint_mm) liegt im Board-Koordinatensystem.
 * Y und Z sind davon direkt auf den Joint-Origin übertragbar (X wird vom
 * Slider bestimmt und bleibt unverändert).
 *
 * - linkName:    Name des Links, dessen Joint aktualisiert wird (z. B. 'Arm1')
 * - y:           Neuer Y-Wert des Joint-Origins (mm)
 * - z:           Neuer Z-Wert des Joint-Origins (mm)
 *
 * Gibt { changed, linkName, oldOrigin, newOrigin } zurück.
 */
export async function setJointOriginYZ(robotPath, { linkName, y, z }) {
  if (!linkName) throw new Error('linkName muss angegeben werden.');

  const robot = await readRobot(robotPath);
  const link  = (robot.links ?? {})[linkName];
  if (!link) return { changed: false, error: `Link '${linkName}' nicht in robot.json gefunden.` };

  const joint = link.jointToParent;
  if (!joint) return { changed: false, error: `Link '${linkName}' hat kein jointToParent.` };

  const oldOrigin = Array.isArray(joint.origin) ? [...joint.origin] : [null, null, null];

  if (!Array.isArray(joint.origin) || joint.origin.length < 3) {
    joint.origin = [oldOrigin[0] ?? 0, 0, 0];
  }

  joint.origin[1] = Math.round(Number(y) * 10) / 10;   // Y (1 Dezimalstelle)
  joint.origin[2] = Math.round(Number(z) * 10) / 10;   // Z

  // Quelle dokumentieren
  if (!joint.originSource) joint.originSource = [null, null, null];
  while (joint.originSource.length < 3) joint.originSource.push(null);
  joint.originSource[1] = 'calibration-yaxis';
  joint.originSource[2] = 'calibration-yaxis';

  await writeRobot(robotPath, robot);

  return {
    changed:    true,
    linkName,
    oldOrigin,
    newOrigin:  [...joint.origin],
  };
}