<template>
  <div class="elfin-viewer">
    <div ref="viewerContainer" class="viewer-container">
      <!-- Three.js 渲染 -->
    </div>
    
    <div v-if="isLoading" class="loading-overlay">
      <div class="spinner"></div>
      <p>正在加载机器人模型...</p>
      <p class="loading-detail">{{ loadingMessage }}</p>
    </div>

    <div class="control-panel" v-if="!isLoading && jointControls.length > 0">
      <h4>关节控制</h4>
      <div class="joint-list">
        <div v-for="joint in jointControls" :key="joint.name" class="joint-item">
          <label :for="joint.name">{{ joint.displayName }}</label>
          <input
            type="range"
            :id="joint.name"
            :min="joint.min"
            :max="joint.max"
            :step="0.01"
            v-model.number="joint.value"
            @input="updateJoint(joint.name, joint.value)"
          />
          <span class="angle">{{ (joint.value * 180 / Math.PI).toFixed(1) }}°</span>
        </div>
      </div>
      <button @click="resetJoints" class="reset-btn">重置关节</button>
    </div>
  </div>
</template>

<script setup>
import { ref, onMounted, onBeforeUnmount, nextTick, reactive } from 'vue';
import * as THREE from 'three';
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';

const props = defineProps({
  basePath: { type: String, default: '/inspection/elfin10' },
  modelColor: { type: String, default: '#cbcbcb' },
  showControls: { type: Boolean, default: true }
});

const viewerContainer = ref(null);
const isLoading = ref(true);
const loadingMessage = ref('初始化...');

let scene, camera, renderer, controls;
let robotGroup = null;
const jointMap = new Map();
const animationId = ref(null);
const jointControls = reactive([]);

// ============ XACRO 解析器 ============
class XacroParser {
  constructor() {
    this.variables = new Map();
    this.macros = new Map();
    this.includes = [];
    this.robotXml = null;
  }
  parse(xacroContent) {
    const parser = new DOMParser();
    const xmlDoc = parser.parseFromString(xacroContent, 'text/xml');
    const properties = xmlDoc.getElementsByTagName('xacro:property');
    for (const prop of properties) {
      const name = prop.getAttribute('name');
      const value = prop.getAttribute('value');
      if (name && value) this.variables.set(name, this.evaluateExpression(value));
    }
    const includes = xmlDoc.getElementsByTagName('xacro:include');
    for (const inc of includes) {
      const filename = inc.getAttribute('filename');
      if (filename) this.includes.push(filename);
    }
    const macros = xmlDoc.getElementsByTagName('xacro:macro');
    for (const macro of macros) {
      const name = macro.getAttribute('name');
      const params = macro.getAttribute('params') || '';
      if (name) {
        this.macros.set(name, {
          params: params.split(/\s+/).filter(p => p),
          content: macro.innerHTML
        });
      }
    }
    this.robotXml = this.processRobotXml(xmlDoc);
    return this.robotXml;
  }
  processRobotXml(xmlDoc) {
    const doc = xmlDoc.cloneNode(true);
    const allElements = Array.from(doc.getElementsByTagName('*'));
    for (const elem of allElements) {
      if (elem.tagName.toLowerCase().startsWith('xacro:')) {
        elem.parentNode?.removeChild(elem);
      }
    }
    const remaining = Array.from(doc.getElementsByTagName('*'));
    for (const elem of remaining) {
      for (const attr of elem.attributes) {
        if (attr.value?.includes('${')) attr.value = this.replaceVariables(attr.value);
      }
      if (elem.textContent?.includes('${')) elem.textContent = this.replaceVariables(elem.textContent);
    }
    return doc;
  }
  replaceVariables(text) {
    return text.replace(/\$\{([^}]+)\}/g, (match, expr) => this.evaluateExpression(expr));
  }
  evaluateExpression(expr) {
    expr = expr.trim();
    if (!expr) return '0';
    try {
      for (const [key, value] of this.variables) {
        if (key.length === 0) continue;
        const regex = new RegExp('\\b' + this.escapeRegex(key) + '\\b', 'g');
        expr = expr.replace(regex, '(' + value + ')');
      }
      expr = expr.replace(/\bPI\b/g, '(' + Math.PI + ')');
      expr = expr.replace(/\bpi\b/g, '(' + Math.PI + ')');
      const sanitized = expr.replace(/\s+/g, '');
      if (/^[\d+\-*/().eE]+$/.test(sanitized)) {
        const result = Function('"use strict"; return (' + sanitized + ')')();
        if (typeof result === 'number' && isFinite(result)) return result.toString();
      }
      console.warn('表达式求值失败，返回原值:', expr);
      return expr;
    } catch (e) {
      console.warn('表达式求值异常:', expr, e);
      return expr;
    }
  }
  escapeRegex(str) {
    return str.replace(/[.*+?^${}()|[\]\\]/g, '\\$&');
  }
  getRobotXml() { return this.robotXml; }
  getIncludes() { return this.includes; }
}

// ============ 工具函数 ============
const cleanMeshPath = (path) => {
  if (!path) return '';
  path = path.replace(/^file:\/\//, '');
  path = path.replace(/^package:\/\/[^/]+\//, '');
  const findMatch = path.match(/\$\(find\s+([^)]+)\)/);
  if (findMatch) path = path.replace(/\$\(find\s+[^)]+\)/, '');
  path = path.replace(/\\/g, '/').replace(/^\/+/, '');
  return path;
};

const parseXYZ = (str) => {
  if (!str) return { x: 0, y: 0, z: 0 };
  const [x, y, z] = str.trim().split(/\s+/).map(Number);
  return { x: x || 0, y: y || 0, z: z || 0 };
};

const parseRPY = (str) => {
  if (!str) return { roll: 0, pitch: 0, yaw: 0 };
  const [r, p, y] = str.trim().split(/\s+/).map(Number);
  return { roll: r || 0, pitch: p || 0, yaw: y || 0 };
};

// ★ 正确转换 RPY → 四元数（URDF 固定轴 XYZ = Three.js 局部轴 ZYX）
const rpyToQuaternion = (rpy) => {
  const euler = new THREE.Euler(rpy.roll, rpy.pitch, rpy.yaw, 'ZYX');
  return new THREE.Quaternion().setFromEuler(euler);
};

// ============ STL 加载 ============
const loadSTLWithRetry = (url, group, visualPos, visualQuat) => {
  return new Promise((resolve) => {
    const loader = new STLLoader();
    const loadMesh = (attemptUrl) => {
      loader.load(
        attemptUrl,
        (geometry) => {
          geometry.computeVertexNormals();
          const material = new THREE.MeshStandardMaterial({
            color: props.modelColor,
            roughness: 0.4,
            metalness: 0.3,
          });
          const mesh = new THREE.Mesh(geometry, material);
          mesh.position.set(visualPos.x, visualPos.y, visualPos.z);
          mesh.quaternion.copy(visualQuat);          // 使用四元数
          mesh.castShadow = true;
          mesh.receiveShadow = true;
          while (group.children.length > 0) {
            const child = group.children[0];
            if (child.isMesh) {
              child.geometry?.dispose();
              child.material?.dispose();
            }
            group.remove(child);
          }
          group.add(mesh);
          resolve(mesh);
        },
        undefined,
        (error) => {
          console.warn(`加载STL失败: ${attemptUrl}`, error);
          if (!attemptUrl.startsWith('/') && !attemptUrl.startsWith('http')) {
            loadMesh('/' + attemptUrl);
          } else {
            resolve(null);
          }
        }
      );
    };
    loadMesh(url);
  });
};

// ============ URDF 解析与构建（严格修复） ============
const parseAndBuildRobot = async (urdfDoc) => {
  const links = urdfDoc.getElementsByTagName('link');
  const jointsRaw = urdfDoc.getElementsByTagName('joint');
  const joints = sortJointsTopologically(jointsRaw, links);
  const linkGroupMap = new Map();
  const mainGroup = new THREE.Group();
  mainGroup.name = 'robot';

  loadingMessage.value = '创建链接结构...';
  for (const link of links) {
    const name = link.getAttribute('name');
    const group = new THREE.Group();
    group.name = name;
    const visual = link.getElementsByTagName('visual')[0];
    if (visual) {
      const origin = visual.getElementsByTagName('origin')[0];
      const geometry = visual.getElementsByTagName('geometry')[0];
      const meshTag = geometry?.getElementsByTagName('mesh')[0];
      if (meshTag) {
        let meshPath = cleanMeshPath(meshTag.getAttribute('filename'));
        let visualPos = { x: 0, y: 0, z: 0 };
        let visualQuat = new THREE.Quaternion();
        if (origin) {
          const rawXyz = parseXYZ(origin.getAttribute('xyz') || '0 0 0');
          const rawRpy = parseRPY(origin.getAttribute('rpy') || '0 0 0');
          visualPos = rawXyz;
          visualQuat = rpyToQuaternion(rawRpy);      // ★ 使用正确顺序
        }
        group.userData = {
          meshPath: `${props.basePath}${meshPath}`,
          visualPos,
          visualQuat
        };
      }
    }
    linkGroupMap.set(name, group);
    mainGroup.add(group);
  }

  loadingMessage.value = '加载3D模型文件...';
  const loadPromises = [];
  for (const [name, group] of linkGroupMap) {
    if (group.userData.meshPath) {
      loadPromises.push(
        loadSTLWithRetry(group.userData.meshPath, group, group.userData.visualPos, group.userData.visualQuat)
      );
    }
  }
  await Promise.allSettled(loadPromises);

  loadingMessage.value = '构建关节结构...';
  const jointDataList = [];
  for (const joint of joints) {
    const type = joint.getAttribute('type');
    const name = joint.getAttribute('name');
    const parent = joint.getElementsByTagName('parent')[0]?.getAttribute('link');
    const child = joint.getElementsByTagName('child')[0]?.getAttribute('link');
    const origin = joint.getElementsByTagName('origin')[0];
    let xyz = { x: 0, y: 0, z: 0 };
    let rpy = { roll: 0, pitch: 0, yaw: 0 };
    if (origin) {
      xyz = parseXYZ(origin.getAttribute('xyz') || '0 0 0');
      rpy = parseRPY(origin.getAttribute('rpy') || '0 0 0');
    }
    const axis = joint.getElementsByTagName('axis')[0];
    const axisVec = parseXYZ(axis?.getAttribute('xyz') || '0 0 1');
    const limit = joint.getElementsByTagName('limit')[0];
    const min = limit ? parseFloat(limit.getAttribute('lower')) : -Math.PI;
    const max = limit ? parseFloat(limit.getAttribute('upper')) : Math.PI;
    jointDataList.push({
      name, type, parent, child, xyz, rpy,
      axis: new THREE.Vector3(axisVec.x, axisVec.y, axisVec.z),
      min, max
    });
  }

  // 构建层级（Euler 顺序 ZYX）
  for (const jointData of jointDataList) {
    const { name, type, parent, child, xyz, rpy, axis, min, max } = jointData;
    const parentGroup = linkGroupMap.get(parent);
    const childGroup = linkGroupMap.get(child);
    if (!parentGroup || !childGroup) {
      console.warn(`关节 ${name} 的父或子链接不存在`);
      continue;
    }

    if (mainGroup.children.includes(childGroup)) mainGroup.remove(childGroup);
    if (parentGroup.children.includes(childGroup)) parentGroup.remove(childGroup);

    if (type === 'revolute' || type === 'continuous') {
      const varGroup = new THREE.Group();
      varGroup.name = name + '_var';

      const originGroup = new THREE.Group();
      originGroup.name = name + '_origin';
      originGroup.position.set(xyz.x, xyz.y, xyz.z);
      // ★ 使用正确顺序
      const euler = new THREE.Euler(rpy.roll, rpy.pitch, rpy.yaw, 'ZYX');
      originGroup.quaternion.setFromEuler(euler);

      originGroup.add(childGroup);
      varGroup.add(originGroup);
      jointMap.set(name, {
        name,
        group: varGroup,
        axis: axis.clone().normalize(),      // 仍为 Z-up 轴，稍后在 loadRobot 中统一变换
        min, max,
        value: 0,
        displayName: name.replace(/_/g, ' ').replace('elfin ', '')
      });
      parentGroup.add(varGroup);
    } else {
      // 固定关节
      childGroup.position.set(xyz.x, xyz.y, xyz.z);
      const euler = new THREE.Euler(rpy.roll, rpy.pitch, rpy.yaw, 'ZYX');
      childGroup.quaternion.setFromEuler(euler);
      parentGroup.add(childGroup);
    }
  }

  return mainGroup;
};

// ============ 拓扑排序 ============
const sortJointsTopologically = (joints, links) => {
  const childToJoint = new Map();
  const allLinkNames = new Set();
  for (const link of links) allLinkNames.add(link.getAttribute('name'));
  const jointArray = Array.from(joints);
  for (const joint of jointArray) {
    const child = joint.getElementsByTagName('child')[0]?.getAttribute('link');
    if (child) childToJoint.set(child, joint);
  }
  const childLinks = new Set(childToJoint.keys());
  const rootLinks = [...allLinkNames].filter(name => !childLinks.has(name));
  const sorted = [];
  const queue = [...rootLinks];
  const visited = new Set();
  while (queue.length > 0) {
    const linkName = queue.shift();
    for (const joint of jointArray) {
      const parent = joint.getElementsByTagName('parent')[0]?.getAttribute('link');
      const child = joint.getElementsByTagName('child')[0]?.getAttribute('link');
      if (parent === linkName && !visited.has(joint.getAttribute('name'))) {
        visited.add(joint.getAttribute('name'));
        sorted.push(joint);
        if (child) queue.push(child);
      }
    }
  }
  for (const joint of jointArray) {
    if (!visited.has(joint.getAttribute('name'))) sorted.push(joint);
  }
  return sorted;
};

// ============ 场景初始化 ============
const initScene = () => {
  const container = viewerContainer.value;
  const width = container.clientWidth || 800;
  const height = container.clientHeight || 600;
  scene = new THREE.Scene();
  scene.background = new THREE.Color(0x1a1a2e);
  camera = new THREE.PerspectiveCamera(45, width / height, 0.1, 1000);
  camera.position.set(2, 2, 3);
  renderer = new THREE.WebGLRenderer({ antialias: true, alpha: true });
  renderer.setSize(width, height);
  renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
  renderer.shadowMap.enabled = true;
  renderer.shadowMap.type = THREE.PCFSoftShadowMap;
  renderer.toneMapping = THREE.ACESFilmicToneMapping;
  renderer.toneMappingExposure = 1.2;
  container.appendChild(renderer.domElement);
  controls = new OrbitControls(camera, renderer.domElement);
  controls.enableDamping = true;
  controls.dampingFactor = 0.1;
  controls.target.set(0, 0.5, 0);
  controls.update();

  const ambient = new THREE.AmbientLight(0x404060, 0.6);
  scene.add(ambient);
  const main = new THREE.DirectionalLight(0xffffff, 1.0);
  main.position.set(5, 10, 7);
  main.castShadow = true;
  scene.add(main);
  const fill = new THREE.DirectionalLight(0x8888ff, 0.4);
  fill.position.set(-5, 0, 5);
  scene.add(fill);
  const rim = new THREE.DirectionalLight(0xffffff, 0.3);
  rim.position.set(0, -5, 5);
  scene.add(rim);
  const grid = new THREE.GridHelper(3, 20, 0x444466, 0x333355);
  grid.position.y = -0.5;
  scene.add(grid);
};

// ============ 关节控制（无需修改） ============
const updateJoint = (jointName, angle) => {
  const joint = jointMap.get(jointName);
  if (!joint) return;
  const clampedAngle = Math.max(joint.min, Math.min(joint.max, angle));
  joint.value = clampedAngle;

  joint.group.quaternion.identity();
  const axis = joint.axis.clone().normalize();
  joint.group.quaternion.setFromAxisAngle(axis, clampedAngle); // 正负根据实际情况调整

  const control = jointControls.find(j => j.name === jointName);
  if (control) control.value = clampedAngle;
};

const resetJoints = () => {
  for (const joint of jointControls) updateJoint(joint.name, 0);
};

// ============ 加载机器人（轴变换加入） ============
const loadRobot = async () => {
  try {
    isLoading.value = true;
    loadingMessage.value = '加载XACRO文件...';
    const xacroUrl = `${props.basePath}elfin10.urdf.xacro`;
    const response = await fetch(xacroUrl);
    if (!response.ok) throw new Error(`HTTP ${response.status}`);
    const xacroContent = await response.text();
    loadingMessage.value = '解析XACRO...';
    const parser = new XacroParser();
    const robotDoc = parser.parse(xacroContent);
    loadingMessage.value = '构建机器人模型...';
    robotGroup = await parseAndBuildRobot(robotDoc);
    if (robotGroup) {
      scene.add(robotGroup);
      // ★ 全局旋转：Z-up → Y-up
      robotGroup.rotation.x = -Math.PI / 2;

      // ★ 关键：将所有关节轴从 Z-up 坐标系变换到 Y-up 坐标系
      const globalQuat = new THREE.Quaternion().setFromEuler(
        new THREE.Euler(-Math.PI / 2, 0, 0, 'XYZ')
      );
      const globalQuat1 = new THREE.Quaternion().setFromEuler(
        new THREE.Euler(-Math.PI / 2, 0, 0, 'ZXY')
      );
      for (const joint of jointMap.values()) {
        if (['joint2', 'joint6'].includes(joint.displayName)) {
            joint.axis.applyQuaternion(globalQuat).normalize();
        }
        if (['joint3', 'joint4', 'joint5'].includes(joint.displayName)) {
            joint.axis.applyQuaternion(globalQuat1).normalize();
        }
      }

      jointControls.length = 0;
      for (const [name, joint] of jointMap) {
        jointControls.push({
          name,
          displayName: joint.displayName,
          value: 0,
          min: joint.min,
          max: joint.max
        });
      }
      fitCamera();
    }
    isLoading.value = false;
    loadingMessage.value = '加载完成';
  } catch (error) {
    console.error('加载机器人失败:', error);
    isLoading.value = false;
    loadingMessage.value = `加载失败: ${error.message}`;
  }
};

const fitCamera = () => {
  if (!robotGroup) return;
  const box = new THREE.Box3().setFromObject(robotGroup);
  const center = box.getCenter(new THREE.Vector3());
  const size = box.getSize(new THREE.Vector3());
  const maxDim = Math.max(size.x, size.y, size.z);
  if (maxDim === 0) return;
  const distance = maxDim / (2 * Math.tan((camera.fov * Math.PI) / 360));
  const direction = new THREE.Vector3(1, 1, 1).normalize();
  camera.position.copy(center).add(direction.multiplyScalar(distance * 1.5));
  controls.target.copy(center);
  controls.update();
};

// ============ 动画循环 ============
const animate = () => {
  animationId.value = requestAnimationFrame(animate);
  controls?.update();
  renderer?.render(scene, camera);
};

// ============ 生命周期 ============
onMounted(async () => {
  await nextTick();
  initScene();
  await loadRobot();
  animate();
  window.addEventListener('resize', onResize);
});

onBeforeUnmount(() => {
  if (animationId.value) cancelAnimationFrame(animationId.value);
  window.removeEventListener('resize', onResize);
  renderer?.dispose();
  controls?.dispose();
});

const onResize = () => {
  const container = viewerContainer.value;
  if (!container || !camera || !renderer) return;
  const width = container.clientWidth;
  const height = container.clientHeight;
  camera.aspect = width / height;
  camera.updateProjectionMatrix();
  renderer.setSize(width, height);
};

defineExpose({ updateJoint, resetJoints, jointControls, jointMap });
</script>

<style scoped>
.elfin-viewer {
  width: 100%;
  height: 100%;
  position: relative;
  overflow: hidden;
  background: #0d0d1a;
}
.viewer-container {
  width: 100%;
  height: 100%;
}
.viewer-container canvas {
  display: block;
  width: 100% !important;
  height: 100% !important;
}
.control-panel {
  position: absolute;
  top: 20px;
  right: 20px;
  background: rgba(20, 20, 40, 0.85);
  padding: 15px;
  border-radius: 8px;
  color: #ccc;
  max-height: 80%;
  overflow-y: auto;
}
.joint-item {
  display: flex;
  align-items: center;
  gap: 8px;
  margin-bottom: 10px;
}
.joint-item label {
  width: 120px;
  font-size: 0.9em;
}
.joint-item input[type=range] {
  flex: 1;
}
.angle {
  width: 50px;
  text-align: right;
}
.reset-btn {
  margin-top: 10px;
  width: 100%;
}
</style>