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



function v_multiplication(vec, scalar){
  if(vec.length == 3){
    return [vec[0]*scalar, vec[1]*scalar, vec[2]*scalar]
  }
}

function v_dot(v1, v2) {
  return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
}

function v_length(v) {
  return Math.sqrt(v_dot(v, v));
}

function v_minus(v1, v2){
  if(v1.length == 3 && v2.length==3)
    return [v1[0] - v2[0], v1[1] - v2[1], v1[2] - v2[2]]
  else
    throw "Vektor Length is wrong";
}

function v_angle(v1, v2) {
  const cosTheta = v_dot(v1, v2) / (v_length(v1) * v_length(v2));
  
  // kleine numerische Fehler abfangen
  const clamped = Math.max(-1, Math.min(1, cosTheta));
  
  return Math.acos(clamped); // Winkel in Radiant
}

function v_getProjectionToPlane(vec, normal){
    return v_minus(vec,  v_multiplication(normal, v_dot(vec, normal)) )
}



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("xPos");

  const partsMovingFixedX = new Set(['Base', 'Arm1', 'Joint1']);
  const markersMovingFixedX = jsonRobot.Marker.filter(m => partsMovingFixedX.has(m.on));

  appendToAnalysis(`xPos found ${markersMovingFixedX.length} Markers`);
  // 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);

  
  appendToAnalysis(`xPos > ${meanPx.toFixed(2)} ± ${stdDevPx.toFixed(2)}`)
  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,
        result: "X",
        status: "ok"
      };
    }

    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,
      result: "X",
      status: "ok"
    };
  });
  withStats.status = "ok";
  withStats.result = "X";

  return withStats;
}

function calculateAngle_byRelativePositionOfMarker(listRecoginize, jsonRobot, jointName){

  // Achse finden
  const jointInfo = jsonRobot.Joints[jointName];
  if(!jointInfo){return null, null; }    
  if(jointInfo.type !== 'revolute'){ return null, null; }

  if(!(jointInfo.axis)){ return null, null; }
  if(!(jointInfo.child)){ return null, null; }

  const achsisName = (jointName == "jointB") ? "y" : (jointName == "jointC") ? "z" : "a" 
  
  appendToAnalysis(`${achsisName}Ang - RelativePosition Started with  n=[${jointInfo.axis}]`)

  markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
  if(markerUsed.length < 2 ){ 
    appendToAnalysis(`${achsisName}Ang - RelativePosition no Marker-Pairs on Robot`)
    return null, null; 
  }

  const markerFound = markerUsed
  .map(m => [m.id, listRecoginize.get(m.id)])
  .filter(v => v !== undefined && v[1] !== undefined);

  if(markerFound.length < 2 ){ 
    appendToAnalysis(`${achsisName}Ang - RelativePosition no Marker-Pairs found in Fotos`)
    return null, null; 
  }

 
  const pairs = markerFound.flatMap((a, i) =>
    markerFound.slice(i + 1).map(b => [a[0], b[0]])
  );


  if(pairs == []){ appendToAnalysis(`${achsisName}Ang - Double not found`); return; }

  const n = v_multiplication(jointInfo.axis, 1/(Math.sqrt(v_dot(jointInfo.axis,jointInfo.axis))))

  const markerMap = Object.fromEntries(
    jsonRobot.Marker.map(m => [m.id, m])
  );

  const pairsWithAngles = pairs.map(([id0, id1]) => {

    // Point in real World
    const m0p = v_getProjectionToPlane(listRecoginize.get(id0), n);
    const m1p = v_getProjectionToPlane(listRecoginize.get(id1), n);

    // Point in Robot Model
    const f0p = v_getProjectionToPlane(markerMap[id0].relPos, n);
    const f1p = v_getProjectionToPlane(markerMap[id1].relPos, n);

    const angleMarker = v_angle(m0p, m1p) * 180 / Math.PI;
    const angleFound  = v_angle(f0p, f1p) * 180 / Math.PI;

    const lengthProjM = v_length(v_minus(m0p, m1p));
    const lengthProjF = v_length(v_minus(f0p, f1p));

    return {
      pair: [id0, id1],
      angleMarker,
      angleFound,
      angleRotation: angleMarker - angleFound,
      lengthProjM,
      lengthProjF
    };
  });


  
  const formatted = JSON.stringify(pairsWithAngles, (key, value) => {
    if (typeof value === "number") {
      return Number(value.toFixed(3));
    }
    return value;
  });

  appendToAnalysis(`${achsisName}Ang - Pairs with Angles: ${formatted}`);
}

function calculateAngle_byPosAndAxis(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;
  }
  if(JSON.stringify(jointInfo.axis) === JSON.stringify([0, 1, 0])){
    a = 2;
    b = 0;
  }
  else{
    // Default: Rotationum X Achse
    a =  2;
    b =  1;
  }
  
  const achsisName = (jointName == "jointB") ? "y" : (jointName == "jointC") ? "z" : "a" 
  appendToAnalysis(`${achsisName}Ang - Started with ${method} on dir [${a}, ${b}]`)

  const jointA = jointInfo.origin[a];
  const jointB = jointInfo.origin[b];

  appendToAnalysis(`${achsisName}Ang - Axis: (${jointA.toFixed(2)}, ${jointB.toFixed(2)})`);
  
  markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
  if(markerUsed.length === 0){ return {average: null, deviation: null}; }

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

  appendToAnalysis(`${achsisName}Ang found ${markerFound.length} markers`);

  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);

      appendToAnalysis(`${achsisName}Ang    tan: ${mRobot.id} Pos=(${pos[1][a].toFixed(2)}, ${pos[1][b].toFixed(2)}) Δ=(${da.toFixed(2)},${db.toFixed(2)}) α°=${angleZero.toFixed(2)}  ${achsisName}=${deltaAngleTan.toFixed(2)}`);
    }
    else{
      const hypotenuse = Math.sqrt(mRobot.relPos[a]**2 + mRobot.relPos[b]**2);
      
      // Arbetein mit sin und hypotenuse
      if(method === "sin"){
        const db = pos[1][b] - jointB;
        var angleOneSin;
        if(Math.abs(db) > hypotenuse && db < 1.3 * hypotenuse){angleOneSin = -180}
        else if(Math.abs(db) < hypotenuse && -1*Math.abs(db) > -hypotenuse){
          angleOneSin = Math.asin(db / hypotenuse) * (180 / Math.PI);
        }
        else if(Math.abs(db) < -1*hypotenuse && Math.abs(db) > 1.3*Math.abs(db)){angleOneSin = 180}
        else angleOneSin = NaN;

        const deltaAngleSin = angleOneSin - angleZero;
        angles.push(deltaAngleSin);

        appendToAnalysis(`${achsisName}Ang    sin: ${mRobot.id} Pos=(${pos[1][a].toFixed(2)}, ${pos[1][b].toFixed(2)}) Δ=${db.toFixed(2)} hyp=${hypotenuse.toFixed(2)} α°=${angleOneSin.toFixed(2)}  ${achsisName}=${deltaAngleSin.toFixed(2)}`);
      }
      // Arbeiten mit cos und hypotenuse
      else{
        const db = pos[1][b] - jointB;
        const angleOneCos = Math.acos(db / hypotenuse) * (180 / Math.PI);
        const deltaAngleCos = -(angleOneCos - angleZero);
        angles.push(deltaAngleCos);

        appendToAnalysis(`${achsisName}Ang    cos: ${mRobot.id} Pos=(${pos[1][a].toFixed(2)}, ${pos[1][b].toFixed(2)}) Δ=${db.toFixed(2)} hyp=${hypotenuse.toFixed(2)} α°=${angleOneCos.toFixed(2)}  ${achsisName}=${deltaAngleCos.toFixed(2)}`);
      }
    }
  }
  
  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);

  appendToAnalysis(`${achsisName}Ang ${achsisName}=${average.toFixed(2)} ± ${deviation.toFixed(2)}`)
  return {average, deviation};
}

async function calculate(foundMarkers, jsonRobot) {

  if(foundMarkers == undefined || jsonRobot == undefined){console.warn("calculateAngles mit falschen Parametern aufgerufen.");}

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

  jsonRobot.recognized.x = x;

  const { average: y, deviation: vary } = calculateAngle_byPosAndAxis(foundById, jsonRobot, "jointB", "tan");
  
  calculateAngle_byRelativePositionOfMarker(foundById, jsonRobot, "jointB");
  
  jsonRobot.recognized.y = y;
  // ToDo !  callibration
  if(jsonRobot.Joints["jointD"] !== undefined && jsonRobot.ElementLength !== undefined){
      jsonRobot.Joints["jointD"].origin[0] = x;
      jsonRobot.Joints["jointD"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180) + jsonRobot.Joints["jointB"].origin[1];
      jsonRobot.Joints["jointD"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180) + jsonRobot.Joints["jointB"].origin[2];

      const { average: a, deviation: vara } = calculateAngle_byPosAndAxis(foundById, jsonRobot, "jointD", "sin");
  }
  if(jsonRobot.Joints["jointC"] !== undefined && jsonRobot.ElementLength !== undefined){
      jsonRobot.Joints["jointC"].origin[0] = x;
      jsonRobot.Joints["jointC"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180)+ jsonRobot.Joints["jointB"].origin[1]
      jsonRobot.Joints["jointC"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180)+ jsonRobot.Joints["jointB"].origin[2]
  }
  

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


// export { calculate, optimizeRobot };

if (typeof window !== "undefined") {
  window.calculate = calculate;
  window.optimizeRobot = optimizeRobot;
}

if (typeof module !== "undefined") {
  module.exports = {
    calculate,
    optimizeRobot
  };
}