const IDENTITY_TRANSFORM = Object.freeze([
  1, 0, 0, 0,
  0, 1, 0, 0,
  0, 0, 1, 0,
  0, 0, 0, 1
]);
const HEX_COLOR_PATTERN = /^#(?:[0-9a-fA-F]{3}){1,2}$/;

function srgbToLinear(value) {
  return value <= 0.04045
    ? value / 12.92
    : ((value + 0.055) / 1.055) ** 2.4;
}

function parseHexColorToLinearRgb(value) {
  const normalized = String(value || "").trim();
  if (!HEX_COLOR_PATTERN.test(normalized)) {
    return null;
  }
  const expanded = normalized.length === 4
    ? `#${normalized[1]}${normalized[1]}${normalized[2]}${normalized[2]}${normalized[3]}${normalized[3]}`
    : normalized;
  return [
    srgbToLinear(Number.parseInt(expanded.slice(1, 3), 16) / 255),
    srgbToLinear(Number.parseInt(expanded.slice(3, 5), 16) / 255),
    srgbToLinear(Number.parseInt(expanded.slice(5, 7), 16) / 255)
  ];
}

function toTransformArray(value, fallback = IDENTITY_TRANSFORM) {
  if (!Array.isArray(value) || value.length !== 16) {
    return [...fallback];
  }
  return value.map((component, index) => Number.isFinite(Number(component)) ? Number(component) : fallback[index]);
}

function toVector3(value, fallback = [0, 0, 1]) {
  if (!Array.isArray(value) || value.length < 3) {
    return [...fallback];
  }
  return [
    Number.isFinite(Number(value[0])) ? Number(value[0]) : fallback[0],
    Number.isFinite(Number(value[1])) ? Number(value[1]) : fallback[1],
    Number.isFinite(Number(value[2])) ? Number(value[2]) : fallback[2]
  ];
}

function normalizeVector(vector) {
  const [x, y, z] = toVector3(vector, [0, 0, 1]);
  const length = Math.hypot(x, y, z);
  if (length <= 1e-9) {
    return [0, 0, 1];
  }
  return [x / length, y / length, z / length];
}

export function multiplyTransforms(left, right) {
  const a = toTransformArray(left);
  const b = toTransformArray(right);
  const product = new Array(16).fill(0);
  for (let row = 0; row < 4; row += 1) {
    for (let column = 0; column < 4; column += 1) {
      let total = 0;
      for (let offset = 0; offset < 4; offset += 1) {
        total += a[(row * 4) + offset] * b[(offset * 4) + column];
      }
      product[(row * 4) + column] = total;
    }
  }
  return product;
}

export function axisAngleTransform(axis, angleRad) {
  const [x, y, z] = normalizeVector(axis);
  const cosine = Math.cos(angleRad);
  const sine = Math.sin(angleRad);
  const oneMinusCosine = 1 - cosine;
  return [
    cosine + (x * x * oneMinusCosine), (x * y * oneMinusCosine) - (z * sine), (x * z * oneMinusCosine) + (y * sine), 0,
    (y * x * oneMinusCosine) + (z * sine), cosine + (y * y * oneMinusCosine), (y * z * oneMinusCosine) - (x * sine), 0,
    (z * x * oneMinusCosine) - (y * sine), (z * y * oneMinusCosine) + (x * sine), cosine + (z * z * oneMinusCosine), 0,
    0, 0, 0, 1
  ];
}

export function transformPoint(transform, point) {
  const matrix = toTransformArray(transform);
  const [x, y, z] = toVector3(point, [0, 0, 0]);
  return [
    (matrix[0] * x) + (matrix[1] * y) + (matrix[2] * z) + matrix[3],
    (matrix[4] * x) + (matrix[5] * y) + (matrix[6] * z) + matrix[7],
    (matrix[8] * x) + (matrix[9] * y) + (matrix[10] * z) + matrix[11]
  ];
}

export function transformBounds(bounds, transform) {
  const min = Array.isArray(bounds?.min) ? bounds.min : [0, 0, 0];
  const max = Array.isArray(bounds?.max) ? bounds.max : [0, 0, 0];
  const corners = [
    [min[0], min[1], min[2]],
    [min[0], min[1], max[2]],
    [min[0], max[1], min[2]],
    [min[0], max[1], max[2]],
    [max[0], min[1], min[2]],
    [max[0], min[1], max[2]],
    [max[0], max[1], min[2]],
    [max[0], max[1], max[2]]
  ];
  const transformed = corners.map((corner) => transformPoint(transform, corner));
  const xs = transformed.map((point) => point[0]);
  const ys = transformed.map((point) => point[1]);
  const zs = transformed.map((point) => point[2]);
  return {
    min: [Math.min(...xs), Math.min(...ys), Math.min(...zs)],
    max: [Math.max(...xs), Math.max(...ys), Math.max(...zs)]
  };
}

export function mergeBounds(boundsList) {
  const normalized = (Array.isArray(boundsList) ? boundsList : []).filter(Boolean);
  if (!normalized.length) {
    return {
      min: [0, 0, 0],
      max: [0, 0, 0]
    };
  }
  const xs = normalized.flatMap((bounds) => [bounds.min[0], bounds.max[0]]);
  const ys = normalized.flatMap((bounds) => [bounds.min[1], bounds.max[1]]);
  const zs = normalized.flatMap((bounds) => [bounds.min[2], bounds.max[2]]);
  return {
    min: [Math.min(...xs), Math.min(...ys), Math.min(...zs)],
    max: [Math.max(...xs), Math.max(...ys), Math.max(...zs)]
  };
}

export function buildDefaultUrdfJointValues(urdfData) {
  const joints = Array.isArray(urdfData?.joints) ? urdfData.joints : [];
  return Object.fromEntries(
    joints
      .filter((joint) => String(joint?.type || "") !== "fixed" && !joint?.mimic)
      .map((joint) => [String(joint?.name || ""), Number(joint?.defaultValueDeg) || 0])
      .filter(([name]) => name)
  );
}

export function clampJointValueDeg(joint, valueDeg) {
  const defaultValue = Number(joint?.defaultValueDeg) || 0;
  const jointType = String(joint?.type || "");
  if (jointType === "fixed") {
    return defaultValue;
  }
  const numericValue = Number.isFinite(Number(valueDeg)) ? Number(valueDeg) : defaultValue;
  if (jointType === "continuous") {
    return numericValue;
  }
  const minValue = Number.isFinite(Number(joint?.minValueDeg)) ? Number(joint.minValueDeg) : numericValue;
  const maxValue = Number.isFinite(Number(joint?.maxValueDeg)) ? Number(joint.maxValueDeg) : numericValue;
  return Math.min(Math.max(numericValue, minValue), Math.max(minValue, maxValue));
}

function isAngularJoint(joint) {
  const jointType = String(joint?.type || "fixed");
  return jointType === "continuous" || jointType === "revolute";
}

function jointValueToNative(joint, value) {
  const clampedValue = clampJointValueDeg(joint, value);
  return isAngularJoint(joint) ? (clampedValue * Math.PI) / 180 : clampedValue;
}

function nativeToJointValue(joint, value) {
  const numericValue = Number.isFinite(Number(value)) ? Number(value) : 0;
  return isAngularJoint(joint) ? (numericValue * 180) / Math.PI : numericValue;
}

function translationAlongAxisTransform(axis, distance) {
  const [x, y, z] = normalizeVector(axis);
  const safeDistance = Number.isFinite(Number(distance)) ? Number(distance) : 0;
  return [
    1, 0, 0, x * safeDistance,
    0, 1, 0, y * safeDistance,
    0, 0, 1, z * safeDistance,
    0, 0, 0, 1
  ];
}

export function posedJointLocalTransform(joint, valueDeg) {
  const jointType = String(joint?.type || "fixed");
  const originTransform = toTransformArray(joint?.originTransform);
  if (jointType === "fixed") {
    return originTransform;
  }
  const axis = toVector3(joint?.axis ?? joint?.axisInJointFrame ?? joint?.axisInParentFrame, [0, 0, 1]);
  if (jointType === "prismatic") {
    return multiplyTransforms(originTransform, translationAlongAxisTransform(axis, clampJointValueDeg(joint, valueDeg)));
  }
  const angleRad = (clampJointValueDeg(joint, valueDeg) * Math.PI) / 180;
  // URDF axes are defined in the joint frame, so the static origin rotation
  // must be applied before the animated axis-angle motion.
  return multiplyTransforms(originTransform, axisAngleTransform(axis, angleRad));
}

function resolveJointValue(joint, jointByName, jointValuesByName, resolving = new Set()) {
  const jointName = String(joint?.name || "");
  if (!joint?.mimic) {
    return clampJointValueDeg(joint, jointValuesByName?.[jointName]);
  }
  if (resolving.has(jointName)) {
    return clampJointValueDeg(joint, joint?.defaultValueDeg);
  }
  resolving.add(jointName);
  const mimic = joint.mimic;
  const masterJoint = jointByName.get(String(mimic.joint || ""));
  const masterValue = masterJoint
    ? resolveJointValue(masterJoint, jointByName, jointValuesByName, resolving)
    : Number(jointValuesByName?.[mimic.joint]) || 0;
  resolving.delete(jointName);

  const masterNativeValue = masterJoint ? jointValueToNative(masterJoint, masterValue) : masterValue;
  const multiplier = Number.isFinite(Number(mimic.multiplier)) ? Number(mimic.multiplier) : 1;
  const offset = Number.isFinite(Number(mimic.offset)) ? Number(mimic.offset) : 0;
  return clampJointValueDeg(joint, nativeToJointValue(joint, (multiplier * masterNativeValue) + offset));
}

export function solveUrdfLinkWorldTransforms(urdfData, jointValuesByName = {}) {
  const rootLink = String(urdfData?.rootLink || "");
  const rootWorldTransform = toTransformArray(urdfData?.rootWorldTransform);
  const joints = Array.isArray(urdfData?.joints) ? urdfData.joints : [];
  const jointByName = new Map(joints.map((joint) => [String(joint?.name || ""), joint]).filter(([name]) => name));
  const linkTransforms = new Map();
  if (!rootLink) {
    return linkTransforms;
  }
  const jointsByParent = new Map();
  for (const joint of joints) {
    const parentLink = String(joint?.parentLink || "");
    if (!parentLink) {
      continue;
    }
    const current = jointsByParent.get(parentLink) || [];
    current.push(joint);
    jointsByParent.set(parentLink, current);
  }

  const visit = (linkName, worldTransform) => {
    linkTransforms.set(linkName, worldTransform);
    const childJoints = jointsByParent.get(linkName) || [];
    for (const joint of childJoints) {
      const childLink = String(joint?.childLink || "");
      if (!childLink) {
        continue;
      }
      const jointValueDeg = resolveJointValue(joint, jointByName, jointValuesByName);
      const childWorldTransform = multiplyTransforms(worldTransform, posedJointLocalTransform(joint, jointValueDeg));
      visit(childLink, childWorldTransform);
    }
  };

  visit(rootLink, rootWorldTransform);
  return linkTransforms;
}

function resolveVisualMesh(meshesByUrl, meshUrl, partFileRef) {
  const lookupKey = meshUrl || partFileRef;
  if (!lookupKey) {
    return null;
  }
  if (meshesByUrl instanceof Map) {
    return meshesByUrl.get(lookupKey) || null;
  }
  if (meshesByUrl && typeof meshesByUrl === "object") {
    return meshesByUrl[lookupKey] || null;
  }
  return null;
}

function resolveUrdfVisuals(urdfData, meshesByUrl) {
  const links = Array.isArray(urdfData?.links) ? urdfData.links : [];
  const resolvedVisuals = [];
  for (const link of links) {
    const linkName = String(link?.name || "");
    const visuals = Array.isArray(link?.visuals) ? link.visuals : [];
    for (const visual of visuals) {
      const meshUrl = String(visual?.meshUrl || "");
      const partFileRef = String(visual?.partFileRef || "");
      const partMesh = resolveVisualMesh(meshesByUrl, meshUrl, partFileRef);
      if (!partMesh) {
        continue;
      }
      resolvedVisuals.push({
        linkName,
        meshUrl,
        partFileRef,
        visual,
        partMesh
      });
    }
  }
  return resolvedVisuals;
}

export function buildUrdfMeshGeometry(urdfData, meshesByUrl) {
  const resolvedVisuals = resolveUrdfVisuals(urdfData, meshesByUrl);
  let totalVertexCount = 0;
  let totalIndexCount = 0;
  let hasSourceColors = false;
  for (const resolvedVisual of resolvedVisuals) {
    const partMesh = resolvedVisual.partMesh;
    totalVertexCount += Math.floor((partMesh.vertices?.length || 0) / 3);
    totalIndexCount += partMesh.indices?.length || 0;
    const visualColor = String(resolvedVisual.visual?.color || "").trim();
    hasSourceColors ||= urdfVisualHasDisplayColors(visualColor, partMesh);
  }

  const vertices = new Float32Array(totalVertexCount * 3);
  const normals = new Float32Array(totalVertexCount * 3);
  const indices = new Uint32Array(totalIndexCount);
  const colors = hasSourceColors ? new Float32Array(totalVertexCount * 3).fill(1) : new Float32Array(0);
  const parts = [];
  let vertexOffset = 0;
  let indexOffset = 0;

  for (const resolvedVisual of resolvedVisuals) {
    const { linkName, meshUrl, partFileRef, partMesh, visual } = resolvedVisual;
    const sourceVertices = partMesh.vertices || new Float32Array(0);
    const sourceNormals = partMesh.normals || new Float32Array(0);
    const sourceColors = partMesh.colors || new Float32Array(0);
    const sourceIndices = partMesh.indices || new Uint32Array(0);
    const partVertexOffset = vertexOffset;
    const partTriangleOffset = Math.floor(indexOffset / 3);
    const vertexCount = Math.floor(sourceVertices.length / 3);
    const triangleCount = Math.floor(sourceIndices.length / 3);
    const visualColor = String(visual?.color || "").trim();
    const visualRgb = parseHexColorToLinearRgb(visualColor);
    const partHasSourceColors = !!visualRgb || urdfMeshHasSourceColors(partMesh);

    vertices.set(sourceVertices, partVertexOffset * 3);
    if (sourceNormals.length === sourceVertices.length) {
      normals.set(sourceNormals, partVertexOffset * 3);
    }
    if (hasSourceColors && partHasSourceColors) {
      if (visualRgb) {
        for (let colorIndex = 0; colorIndex < vertexCount; colorIndex += 1) {
          const offset = (partVertexOffset + colorIndex) * 3;
          colors[offset] = visualRgb[0];
          colors[offset + 1] = visualRgb[1];
          colors[offset + 2] = visualRgb[2];
        }
      } else if (sourceColors.length === sourceVertices.length) {
        colors.set(sourceColors, partVertexOffset * 3);
      }
    }
    for (let index = 0; index < sourceIndices.length; index += 1) {
      indices[indexOffset + index] = sourceIndices[index] + partVertexOffset;
    }

    const partLabel = String(visual?.label || visual?.instanceId || visual?.id || meshUrl || partFileRef).trim();
    parts.push({
      id: String(visual?.id || `${linkName}:${meshUrl || partFileRef}`),
      name: partLabel,
      label: partLabel,
      color: visualColor,
      meshUrl,
      partFileRef,
      linkName,
      localTransform: toTransformArray(visual?.localTransform),
      sourceBounds: partMesh.bounds,
      bounds: partMesh.bounds,
      transform: [...IDENTITY_TRANSFORM],
      hasSourceColors: partHasSourceColors,
      vertexOffset: partVertexOffset,
      vertexCount,
      triangleOffset: partTriangleOffset,
      triangleCount,
      edgeIndexOffset: 0,
      edgeIndexCount: 0
    });

    vertexOffset += vertexCount;
    indexOffset += sourceIndices.length;
  }

  return {
    vertices,
    indices,
    normals,
    colors,
    edge_indices: new Uint32Array(0),
    bounds: mergeBounds(parts.map((part) => part.bounds)),
    parts,
    has_source_colors: hasSourceColors
  };
}

function urdfMeshHasSourceColors(partMesh) {
  return !!partMesh?.has_source_colors &&
    partMesh.colors?.length > 0 &&
    partMesh.colors.length === partMesh.vertices?.length;
}

function urdfVisualHasDisplayColors(visualColor, partMesh) {
  return !!parseHexColorToLinearRgb(visualColor) || (!visualColor && urdfMeshHasSourceColors(partMesh));
}

export function poseUrdfMeshData(urdfData, meshData, jointValuesByName = {}) {
  const linkWorldTransforms = solveUrdfLinkWorldTransforms(urdfData, jointValuesByName);
  const sourceParts = Array.isArray(meshData?.parts) ? meshData.parts : [];
  const posedParts = sourceParts.map((part) => {
    const linkWorldTransform = linkWorldTransforms.get(String(part?.linkName || "")) || [...IDENTITY_TRANSFORM];
    const localTransform = toTransformArray(part?.localTransform);
    const worldTransform = multiplyTransforms(linkWorldTransform, localTransform);
    const sourceBounds = part?.sourceBounds || part?.bounds;
    return {
      ...part,
      transform: worldTransform,
      bounds: transformBounds(sourceBounds, worldTransform)
    };
  });

  return {
    meshData: {
      ...meshData,
      bounds: mergeBounds(posedParts.map((part) => part.bounds)),
      parts: posedParts
    },
    linkWorldTransforms
  };
}

export function buildUrdfMeshData(urdfData, meshesByUrl, jointValuesByName = {}) {
  return poseUrdfMeshData(
    urdfData,
    buildUrdfMeshGeometry(urdfData, meshesByUrl),
    jointValuesByName
  );
}