appRobotHoming/public/calculateAngles.js

/**
 * calculateAngles module
 *
 * Browser + Server + Jest (CJS) kompatibel
 */


function getAnalysisLogEl() {
  if (typeof document === "undefined") return null;
  return document.getElementById("analysis-log");
}

function appendToAnalysis(line) {
  const el = getAnalysisLogEl();
  if (!el) return;

  const now = new Date().toISOString();
  el.value += `[${now}] ${line}\n`;
  el.scrollTop = el.scrollHeight;
}


function calculateXPos(listIdAndX, jsonRobot){
  appendToAnalysis("Calculate XPos");

  const partsMovingFixedX = new Set(['Base', 'Arm1', 'Joint1']);
  const markersMovingFixedX = jsonRobot.Marker.filter(m => partsMovingFixedX.has(m.on));
  // Join: Robot-Marker ↔ Found-Marker

  const markersListeWithRobotInfo = jsonRobot.Marker
  .filter(m => partsMovingFixedX.has(m.on))   // nur relevante Teile
  .map(m => {
    const found = listIdAndX.get(m.id);
    if (!found) return null;

    return {
      id: m.id,
      relPos: m.relPos,   // oder m.relPos, je nach Struktur
      position: found,
      Px: found[0] - m.relPos[0]
    };
  })
  .filter(Boolean); // nulls entfernen

  const pxValues = markersListeWithRobotInfo.map(m => m.Px);
  const meanPx =  pxValues.reduce((sum, x) => sum + x, 0) / pxValues.length;

  const variancePx =  pxValues.reduce((sum, x) => sum + Math.pow(x - meanPx, 2), 0) / pxValues.length;
  const stdDevPx = Math.sqrt(variancePx);

  return { meanPx, stdDevPx };
}


function optimizeRobot(listFoundMarkers, jsonRobot) {
  
  const map = new Map();

  for (const foundMarkers of listFoundMarkers) {
    var x_222_226 = null;
    var x_222_226_count = 0;
    
    for (const mark of foundMarkers.markers.filter(m => m.id === 226).map(f => [f.id, f.position_mm])) {     
      x_222_226 = mark[1][0];
      x_222_226_count++;
    }
    if (x_222_226_count > 0) {
      x_222_226 = x_222_226 / x_222_226_count;
    }
    else{
      continue; // Wenn weder 222 noch 226 gefunden wurden, überspringen
    }

    for (const mark of foundMarkers.markers.map(f => [f.id, f.position_mm ])){
      const id = mark[0];
      const dx_222_226 = mark[1][0] - x_222_226;
      if (!map.has(id)) {
        map.set(id, []); // Initialisiere mit  x_222_226, damit wir später die Abweichung berechnen können
      }
      map.get(id).push(dx_222_226);
    }
  }

  const result = Array.from(map, ([id, mm]) => ({ id, mm })).filter(m => [198,200,204,229,243].includes(m.id));

  
  const withStats = result.map(entry => {
    const { mm } = entry;
    const n = mm.length;

    if (n === 0) {
      return {
        ...entry,
        n: 0,
        average: null,
        deviation: null
      };
    }

    const average = mm.reduce((a, b) => a + b, 0) / n;
    const deviation = Math.sqrt(mm.reduce((sum, x) => sum + Math.pow(x - average, 2), 0) / n);

    return {
      ...entry,
      n,
      average,
      deviation
    };
  });

  return withStats;
}
function calculateRotationAngle(listIdAndX, jsonRobot, jointName, method = "tan") {
  // Achse finden
  const jointInfo = jsonRobot.Joints[jointName];
  if(!jointInfo){return null, null; }    
  if(jointInfo.type !== 'revolute'){ return null, null; }
  if(!(jointInfo.origin)){ return null, null; }
  if(!(jointInfo.axis)){ return null, null; }
  if(!(jointInfo.child)){ return null, null; }

  var a, b;
  if(jointInfo.axis == [1,0,0]){
    // Auf welche Elemente (x,y,z) zugegriffen wird. 
    // bei Rotation um a wird mit y=1 und z=2 gearbeitet.
    a = 2;
    b = 1;
  }
  else{
    // Default: Rotationum X Achse
    a =  2;
    b =  1;
  }
  
  const jointA = jointInfo.origin[a];
  const jointB = jointInfo.origin[b];
  
  markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
  if(markerUsed.length === 0){ return null, null; }

  
const markerFound = markerUsed
  .map(m => [m.id, listIdAndX.get(m.id)])
  .filter(v => v !== undefined && v[1] !== undefined); // Nur Marker, die gefunden wurden

  var angles = [];

  for(const pos of markerFound) {
    const id = pos[0];
    const mRobot = jsonRobot.Marker.filter(m => m.id === id)[0];

    // Arbeiten mit x,y und Tan
    const angleZero = Math.atan2(mRobot.relPos[b], mRobot.relPos[a]) * (180 / Math.PI);
    if(method === "tan"){
      const da = pos[1][a] - jointA;
      const db = pos[1][b] - jointB;
      const angleOne = Math.atan2(db, da) * (180 / Math.PI);
      const deltaAngleTan = angleOne - angleZero;
      angles.push({ deltaAngleTan });
    }
    else{
      const hypotenuse = Math.sqrt(mRobot.relPos[a]**2 + mRobot.relPos[b]**2);
      
      // Arbetein mit sin und hypotenuse
      if(method === "sin"){
        const angleOneSin = Math.asin(db / hypotenuse) * (180 / Math.PI);
        const deltaAngleSin = angleOneSin - angleZero;
        angles.push({ deltaAngleSin });
      }
      // Arbeiten mit cos und hypotenuse
      else{
        const angleOneCos = Math.acos(db / hypotenuse) * (180 / Math.PI);
        const deltaAngleCos = -(angleOneCos - angleZero);
        angles.push({ deltaAngleCos });
      }
    }
  }

  
  const n = angles.length;
  if(n === 0){ return null, null; }

  const average = angles.reduce((a, b) => a + b, 0) / n;
  const deviation = Math.sqrt(angles.reduce((sum, x) => sum + Math.pow(x - average, 2), 0) / n);


  return average, deviation;
}

async function calculate(foundMarkers, jsonRobot) {

  const foundById = new Map(foundMarkers.markers.map(f => [f.id, f.position_mm ]));
  const { meanPx: x, stdDevPx: varx } = calculateXPos(foundById, jsonRobot);

  const { meanY: y, stdDevY: vary } = calculateRotationAngle(foundById, jsonRobot, "jointB", "tan");

  return {
    meta: {
      module: 'calculateAngles',
      timestamp: new Date().toISOString()
    },
    inputs: {
      markers: foundMarkers ?? null,
      robot: jsonRobot ?? null
    },
    status: 'ok',
    result: {
      x: x,
      varx: varx 
    }
  };
}


module.exports = { calculate, optimizeRobot };