








(function () {
  'use strict';

  var DEG2RAD = Math.PI / 180;

  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  
  var MOUNT_XYZ_FALLBACK = [0, 0, 0.005];
  var MOUNT_RPY_FALLBACK = [0, 0, 0];

  var COL_ACTIVE = 0x4da3ff;   
  var COL_COLLIDE = 0xff9500;  
  var COL_VACUUM = 0x2ecc71;   

  function Gripper(desc, opts) {
    opts = opts || {};
    this.desc = desc;
    this.scene = opts.vizScene || null;
    this.mountXyz = desc.mount_xyz || MOUNT_XYZ_FALLBACK;
    this.mountRpy = desc.mount_rpy || MOUNT_RPY_FALLBACK;
    this.root = new THREE.Group();
    
    
    this.prefix = opts.prefix || 'gripper:';
    this.root.name = this.prefix + desc.name;
    this.root.position.set(this.mountXyz[0], this.mountXyz[1], this.mountXyz[2]);
    this.root.quaternion.setFromEuler(
      new THREE.Euler(this.mountRpy[0], this.mountRpy[1], this.mountRpy[2], 'ZYX'));

    this._linkGroups = {};
    this._joints = {};        
    this._meshesPending = 0;
    this._onReady = opts.onReady || null;
    this._bracketMaterials = [];   
    this._tintOn = false;          
    this.angleDeg = 0;

    this._build();
    









    this.setAngleDeg(opts.angleDeg || 0);
  }

  Gripper.prototype._material = function (rgba) {
    return new THREE.MeshStandardMaterial({
      color: new THREE.Color(rgba[0], rgba[1], rgba[2]),
      roughness: 0.55, metalness: 0.35
    });
  };

  





  Gripper.prototype._buildMaterials = function (l, geometry) {
    var single = this._material(l.color);
    var parts = l.parts;
    if (!parts || !parts.length) return single;

    var triCount = geometry.attributes.position.count / 3;
    var sane = parts.every(function (p) {
      return p.start >= 0 && p.count > 0 && (p.start + p.count) <= triCount;
    });
    if (!sane) {
      console.warn('gripper: mesh parts out of range for ' + l.name +
                   ' (tris=' + triCount + '), bracket toggle disabled');
      return single;
    }

    geometry.clearGroups();
    var mats = [];
    var self = this;
    parts.forEach(function (p, i) {
      geometry.addGroup(p.start * 3, p.count * 3, i);
      var m = self._material(l.color);
      if (p.role === 'bracket') self._bracketMaterials.push(m);
      mats.push(m);
    });
    return mats;
  };

  Gripper.prototype._build = function () {
    var self = this;
    var desc = this.desc;

    desc.links.forEach(function (l) {
      var g = new THREE.Group();
      g.name = l.name;
      self._linkGroups[l.name] = g;
    });

    
    var children = {};
    desc.joints.forEach(function (j) { children[j.child] = true; });
    desc.links.forEach(function (l) {
      if (!children[l.name]) self.root.add(self._linkGroups[l.name]);
    });

    desc.joints.forEach(function (j) {
      
      
      var jointGroup = new THREE.Group();
      jointGroup.name = 'joint:' + j.name;
      jointGroup.position.set(j.origin.xyz[0], j.origin.xyz[1], j.origin.xyz[2]);
      jointGroup.quaternion.setFromEuler(
        new THREE.Euler(j.origin.rpy[0], j.origin.rpy[1], j.origin.rpy[2], 'ZYX'));
      self._linkGroups[j.parent].add(jointGroup);
      var child = self._linkGroups[j.child];
      jointGroup.add(child);
      if (j.type !== 'fixed') {          
        self._joints[j.name] = {
          node: child,
          axis: new THREE.Vector3(j.axis[0], j.axis[1], j.axis[2]).normalize(),
          entry: j
        };
      }
      if (j.child === 'camera_link') self._cameraGroup = child;
    });

    var loader = new THREE.STLLoader();
    desc.links.forEach(function (l) {
      if (!l.mesh) return;
      self._meshesPending++;
      loader.load(l.mesh, function (geometry) {
        geometry.computeVertexNormals();
        var mesh = new THREE.Mesh(geometry, self._buildMaterials(l, geometry));
        mesh.position.set(l.visual_origin.xyz[0], l.visual_origin.xyz[1],
                          l.visual_origin.xyz[2]);
        mesh.quaternion.setFromEuler(new THREE.Euler(
          l.visual_origin.rpy[0], l.visual_origin.rpy[1], l.visual_origin.rpy[2], 'ZYX'));
        var s = l.mesh_scale || [1, 1, 1];
        mesh.scale.set(s[0], s[1], s[2]);   
        self._linkGroups[l.name].add(mesh);
        
        
        
        
        self._repaint();
        self._meshesPending--;
        if (self._meshesPending === 0 && self._onReady) self._onReady();
      }, undefined, function (err) {
        console.error('gripper STL load failed: ' + l.mesh + ' ' + err);
        self._meshesPending--;
      });
    });
  };

  



  Gripper.prototype._valueRad = function (name, driveRad, memo) {
    if (Object.prototype.hasOwnProperty.call(memo, name)) return memo[name];
    memo[name] = 0;                       
    var j = this._joints[name];
    if (!j) return 0;
    var v;
    if (!j.entry.mimic) {
      
      v = (name === this.desc.drive_joint) ? driveRad : 0;
    } else {
      var m = j.entry.mimic;
      v = m.multiplier * this._valueRad(m.joint, driveRad, memo) + m.offset;
    }
    memo[name] = v;
    return v;
  };

  
  Gripper.prototype.setAngleDeg = function (deg) {
    var lim = this.desc.limit_deg || [0, 110];
    deg = Math.max(lim[0], Math.min(lim[1], deg));
    this.angleDeg = deg;
    var driveRad = deg * DEG2RAD;
    var memo = {};
    for (var name in this._joints) {
      if (!Object.prototype.hasOwnProperty.call(this._joints, name)) continue;
      var j = this._joints[name];
      j.node.quaternion.setFromAxisAngle(j.axis, this._valueRad(name, driveRad, memo));
    }
  };

  
  Gripper.prototype.attachTo = function (parentGroup) {
    if (parentGroup) parentGroup.add(this.root);
  };

  

  Gripper.prototype.setMountYawDeg = function (deg) {
    this.mountYawDeg = deg || 0;
    this.root.quaternion.setFromEuler(new THREE.Euler(
      this.mountRpy[0], this.mountRpy[1],
      this.mountRpy[2] + this.mountYawDeg * DEG2RAD, 'ZYX'));
  };

  Gripper.prototype.setVisible = function (on) { this.root.visible = !!on; };
  Gripper.prototype.isVisible = function () { return this.root.visible; };

  


  Gripper.prototype.setCameraVisible = function (on) {
    if (this._cameraGroup) this._cameraGroup.visible = !!on;
    this._bracketMaterials.forEach(function (m) { m.visible = !!on; });
  };
  Gripper.prototype.isCameraVisible = function () {
    return !!(this._cameraGroup && this._cameraGroup.visible);
  };

  






  Gripper.prototype.setActiveTint = function (on) {
    this._tintOn = !!on;           
    this._repaint();
  };

  

  Gripper.prototype.setVacuumTint = function (on) {
    this._vacuumOn = !!on;
    this._repaint();
  };

  

  Gripper.prototype.setLinkTint = function (names) {
    this._colliding = {};
    (names || []).forEach(function (n) { this._colliding[n] = true; }, this);
    this._repaint();
  };

  



  Gripper.prototype._repaint = function () {
    var vacuumOn = this._vacuumOn;
    var tintOn = this._tintOn;
    var colliding = this._colliding || {};
    var self = this;
    
    
    function linkNameOf(o) {         
      for (var p = o; p; p = p.parent) {
        if (self._linkGroups[p.name] === p) return p.name;
      }
      return null;
    }
    this.root.traverse(function (o) {
      if (!o.isMesh || !o.material) return;
      var hit = !!colliding[linkNameOf(o)];
      var mats = Array.isArray(o.material) ? o.material : [o.material];
      mats.forEach(function (m) {
        if (!m.color) return;
        if (!m.__origColor) m.__origColor = m.color.clone();
        if (hit) m.color.setHex(COL_COLLIDE);
        else if (vacuumOn) m.color.setHex(COL_VACUUM);
        else if (tintOn) m.color.setHex(COL_ACTIVE);
        else m.color.copy(m.__origColor);
      });
    });
  };

  



  Gripper.prototype.fingerGapM = function () {
    var a = this._linkGroups['link_finger1'];
    var b = this._linkGroups['link_finger2'];
    if (!a || !b) return null;
    this.root.updateMatrixWorld(true);
    var boxA = new THREE.Box3().setFromObject(a);
    var boxB = new THREE.Box3().setFromObject(b);
    if (boxA.isEmpty() || boxB.isEmpty()) return null;   
    var ca = boxA.getCenter(new THREE.Vector3());
    var cb = boxB.getCenter(new THREE.Vector3());
    var d = new THREE.Vector3().subVectors(cb, ca);
    var dist = d.length();
    if (dist < 1e-9) return 0;
    d.divideScalar(dist);
    var ha = boxA.getSize(new THREE.Vector3()).multiplyScalar(0.5);
    var hb = boxB.getSize(new THREE.Vector3()).multiplyScalar(0.5);
    
    function halfAlong(h, dir) {
      return Math.abs(h.x * dir.x) + Math.abs(h.y * dir.y) + Math.abs(h.z * dir.z);
    }
    return dist - halfAlong(ha, d) - halfAlong(hb, d);
  };

  Gripper.prototype.dispose = function () {
    if (this.root.parent) this.root.parent.remove(this.root);
    this.root.traverse(function (o) {
      if (o.isMesh) {
        if (o.geometry) o.geometry.dispose();
        if (o.material) o.material.dispose();
      }
    });
  };

  window.Gripper = Gripper;
})();
