


(function () {
  'use strict';

  var RAD2DEG = 180 / Math.PI;

  
  
  var SINGLE_AXIS = { X: true, Y: true, Z: true };
  
  
  var THROTTLE_MS = 3;

  
  
  
  
  
  
  
  var PLANE_PAD_GROW = 1.4;

  
  
  
  
  
  
  
  var GizmoArbiter = {
    _gizmos: [],
    _claimants: [],
    _wired: false,
    _raycaster: null,

    register: function (gizmo) {
      this._gizmos.push(gizmo);
      if (!this._wired) this._wire(gizmo.scene);
    },
    unregister: function (gizmo) {
      var i = this._gizmos.indexOf(gizmo);
      if (i >= 0) this._gizmos.splice(i, 1);
    },

    












    addClaimant: function (fn, onArbitrated) {
      this._claimants.push({ quote: fn, notify: onArbitrated || null });
    },
    removeClaimant: function (fn) {
      for (var i = 0; i < this._claimants.length; i++) {
        if (this._claimants[i].quote === fn) { this._claimants.splice(i, 1); return; }
      }
    },
    

    anyDragging: function () {
      return this._gizmos.some(function (g) {
        return g.controls.some(function (c) { return c.dragging; });
      });
    },
    _wire: function (vizScene) {
      this._wired = true;
      this._raycaster = new THREE.Raycaster();
      var dom = vizScene.renderer.domElement;
      var self = this;
      var arbitrate = function (e) { self._arbitrate(vizScene, e); };
      dom.addEventListener('pointermove', arbitrate, true);
      dom.addEventListener('pointerdown', arbitrate, true);
      dom.addEventListener('pointerup', this._restore.bind(this), true);
    },
    



    _arbitrate: function (vizScene, event) {
      if (this.anyDragging()) return;   

      var candidates = [];   
      this._gizmos.forEach(function (g) {
        if (!g.enabled) return;
        
        if (g.handlesFollowAxes && !g.axesVisible) return;
        g.controls.forEach(function (tc) { tc.enabled = true; candidates.push(tc); });
      });
      if (!candidates.length && !this._claimants.length) return;

      var rect = vizScene.renderer.domElement.getBoundingClientRect();
      var ndc = new THREE.Vector2(
        ((event.clientX - rect.left) / rect.width) * 2 - 1,
        -((event.clientY - rect.top) / rect.height) * 2 + 1
      );
      this._raycaster.setFromCamera(ndc, vizScene.camera);
      var raycaster = this._raycaster;

      
      var claimDist = Infinity, claimWinner = null;
      this._claimants.forEach(function (c) {
        var d = c.quote(raycaster);
        if (d != null && d < claimDist) { claimDist = d; claimWinner = c; }
      });

      
      var winner = null, bestDist = Infinity;
      candidates.forEach(function (tc) {
        if (!tc._gizmo) return;
        var pickerGroup = tc._gizmo.picker[tc.mode];   
        if (!pickerGroup) return;
        var hits = raycaster.intersectObjects(pickerGroup.children, true);
        if (hits.length && hits[0].distance < bestDist) {
          bestDist = hits[0].distance;
          winner = tc;
        }
      });

      

      var claimTakesIt = claimWinner !== null && claimDist < bestDist;
      this._claimants.forEach(function (c) {
        if (c.notify) c.notify(claimTakesIt && c === claimWinner);
      });

      if (!candidates.length) return;
      
      if (claimTakesIt) {
        candidates.forEach(function (tc) { tc.enabled = false; tc.axis = null; });
        return;
      }
      if (!winner) return;   
      candidates.forEach(function (tc) {
        if (tc !== winner) { tc.enabled = false; tc.axis = null; }   
      });
    },
    
    _restore: function () {
      this._gizmos.forEach(function (g) {
        g.controls.forEach(function (tc) { tc.enabled = g.enabled; });
      });
    }
  };

  function Gizmo(vizScene, robotDesc, opts) {
    opts = opts || {};
    this.scene = vizScene;
    this.desc = robotDesc;
    this.getSeed = opts.getSeed;         
    this.onSolution = opts.onSolution;   
    this.enabled = false;                
    this.axesVisible = true;             
    
    this.handlesFollowAxes = !!opts.handlesFollowAxes;
    




    this._dragMode = null;               
    




    this.skipContinuity = false;
    this.lastSolution = null;            
    this.lastTcp = null;                 
    




    this.toolOffsetM = 0;
    this.lastValidPos = new THREE.Vector3();    
    this.lastValidQuat = new THREE.Quaternion();
    this.blocked = false;                       
    
    
    this.endedBlocked = false;

    
    
    
    
    this.target = new THREE.Group();
    vizScene.scene.add(this.target);
    this._label = null;             
    this._labelText = null;
    this.labelVisible = true;       
    this._originSphere = null;      

    
    
    this._lastSent = 0;
    this.controls = [this._makeControl('translate'), this._makeControl('rotate')];

    
    
    GizmoArbiter.register(this);
  }

  
  Gizmo.prototype.destroy = function () {
    GizmoArbiter.unregister(this);
    this._disposeLabel();
    this._disposeOriginSphere();
    var vizScene = this.scene;
    for (var i = 0; i < this.controls.length; i++) {
      var tc = this.controls[i];
      if (tc.dragging) vizScene.controls.enabled = true;   
      tc.detach();
      tc.dispose();
      vizScene.scene.remove(tc);
    }
    this.controls = [];
    vizScene.scene.remove(this.target);
    this.target = null;
  };

  












  function growPlanePads(tc) {
    if (!tc._gizmo) return;
    [tc._gizmo.gizmo.translate, tc._gizmo.picker.translate].forEach(function (grp) {
      if (!grp) return;
      grp.children.forEach(function (h) {
        if (h.name !== 'XY' && h.name !== 'YZ' && h.name !== 'XZ') return;
        if (!h.geometry || h.geometry.__padGrown) return;   
        h.geometry.scale(PLANE_PAD_GROW, PLANE_PAD_GROW, PLANE_PAD_GROW);
        h.geometry.__padGrown = true;
      });
    });
  }

  
  Gizmo.prototype._makeControl = function (mode) {
    var self = this;
    var vizScene = this.scene;
    var tc = new THREE.TransformControls(vizScene.camera, vizScene.renderer.domElement);
    tc.setMode(mode);
    tc.setSpace('local');   
    tc.setSize(mode === 'rotate' ? 1.0 : 0.8);   
    tc.attach(this.target);
    if (mode === 'translate') growPlanePads(tc);   
    tc.visible = true;      
    tc.enabled = false;     
    vizScene.scene.add(tc);

    
    
    var origUMW = tc.updateMatrixWorld.bind(tc);
    tc.updateMatrixWorld = function (force) {
      origUMW(force);
      if (!tc._gizmo) return;
      





      var helper = tc._gizmo.helper && tc._gizmo.helper[mode];
      if (helper) {
        for (var k = 0; k < helper.children.length; k++) helper.children[k].visible = false;
      }
      [tc._gizmo.gizmo[mode], tc._gizmo.picker[mode]].forEach(function (grp) {
        if (!grp) return;
        for (var i = 0; i < grp.children.length; i++) {
          var h = grp.children[i];
          var isAxis = mode === 'translate' && (h.name === 'X' || h.name === 'Y' || h.name === 'Z');
          
          
          var showHandle = isAxis ? self.axesVisible
            : (self.enabled && (!self.handlesFollowAxes || self.axesVisible));
          h.visible = h.visible && showHandle;
        }
      });
    };

    tc.addEventListener('dragging-changed', function (e) {
      vizScene.controls.enabled = !e.value;       
      if (e.value) {
        
        
        
        self._dragMode = mode;
        self._dragAxis = tc.axis;
        self.endedBlocked = false;               
      } else {
        






        self.endedBlocked = self.blocked;
        self._requestIK(true);                   
        self._dragMode = null;                   
        self._dragAxis = null;
      }
    });
    tc.addEventListener('objectChange', function () {
      
      var dpos = self.target.position.clone();
      var dquat = self.target.quaternion.clone();
      if (self.blocked) {
        self.target.position.copy(self.lastValidPos);
        self.target.quaternion.copy(self.lastValidQuat);
      }
      var now = performance.now();
      if (now - self._lastSent > THROTTLE_MS) { self._lastSent = now; self._solveFor(dpos, dquat); }
    });
    return tc;
  };

  

  Gizmo.prototype._toolVec = function (quat) {
    return new THREE.Vector3(0, 0, this.toolOffsetM).applyQuaternion(quat);
  };

  



  Gizmo.prototype.setToolOffset = function (m) {
    this.toolOffsetM = (typeof m === 'number' && isFinite(m)) ? m : 0;
  };

  
  Gizmo.prototype.syncToPose = function (robot) {
    robot.root.updateMatrixWorld(true);
    var link6 = robot._linkGroups['link6'];
    link6.getWorldPosition(this.target.position);
    link6.getWorldQuaternion(this.target.quaternion);
    if (this.toolOffsetM) this.target.position.add(this._toolVec(this.target.quaternion));
  };

  


  Gizmo.prototype.syncToTcp = function (tcp) {
    if (!tcp) return;
    this.target.position.set(tcp.xyz_mm[0] / 1000, tcp.xyz_mm[1] / 1000, tcp.xyz_mm[2] / 1000);
    var e = new THREE.Euler(
      tcp.euler_xyz_deg[0] / RAD2DEG, tcp.euler_xyz_deg[1] / RAD2DEG, tcp.euler_xyz_deg[2] / RAD2DEG, 'ZYX'
    );
    this.target.quaternion.setFromEuler(e);
    if (this.toolOffsetM) this.target.position.add(this._toolVec(this.target.quaternion));
  };

  


  Gizmo.prototype.flangeTcp = function () {
    if (!this.target) return null;
    var p = this.target.position.clone();
    if (this.toolOffsetM) p.sub(this._toolVec(this.target.quaternion));
    var e = new THREE.Euler().setFromQuaternion(this.target.quaternion, 'ZYX');
    return { xyz_mm: [p.x * 1000, p.y * 1000, p.z * 1000],
             euler_xyz_deg: [e.x * RAD2DEG, e.y * RAD2DEG, e.z * RAD2DEG] };
  };

  
  Gizmo.prototype.setAxesVisible = function (on) { this.axesVisible = !!on; };

  

  Gizmo.prototype.setLabelVisible = function (on) {
    this.labelVisible = !!on;
    if (this._label) {
      this._label.visible = this.labelVisible;
      this.updateLabel();      
    }
    if (this._originSphere) {
      this._originSphere.visible = this.labelVisible;   
    }
  };

  

  




  var ORIGIN_SPHERE_R_AT_1M = 0.010;   

  Gizmo.prototype.setOriginSphere = function (on) {
    if (on) {
      if (!this._originSphere && this.target) {
        var mat = new THREE.MeshBasicMaterial({
          color: 0xffd97a, transparent: true,
          depthTest: false, depthWrite: false,
        });
        var mesh = new THREE.Mesh(new THREE.SphereGeometry(1, 16, 12), mat);
        mesh.renderOrder = 999;             
        mesh.visible = this.labelVisible;   
        this.target.add(mesh);              
        this._originSphere = mesh;
        this.updateLabel();                 
      }
    } else {
      this._disposeOriginSphere();
    }
  };

  














  var LABEL_H_AT_1M = 0.016;      
  var LABEL_R_AT_1M = 0.022;      
  var LABEL_FONT_PX = 64;         

  





  Gizmo.prototype.setLabel = function (text) {
    text = (text == null) ? '' : String(text);
    if (text === this._labelText) return;      
    this._labelText = text;
    this._disposeLabel();
    if (!text || !this.target) return;

    var dpr = Math.min(window.devicePixelRatio || 1, 2);
    var pad = Math.round(LABEL_FONT_PX * 0.25);
    var measure = document.createElement('canvas').getContext('2d');
    measure.font = 'bold ' + LABEL_FONT_PX + 'px sans-serif';
    var textW = Math.ceil(measure.measureText(text).width);

    var cw = textW + pad * 2, ch = Math.round(LABEL_FONT_PX * 1.4);
    var cv = document.createElement('canvas');
    cv.width = Math.round(cw * dpr);
    cv.height = Math.round(ch * dpr);
    var ctx = cv.getContext('2d');
    ctx.scale(dpr, dpr);
    
    ctx.font = 'bold ' + LABEL_FONT_PX + 'px sans-serif';
    ctx.textAlign = 'center';
    ctx.textBaseline = 'middle';
    ctx.lineWidth = Math.round(LABEL_FONT_PX * 0.18);
    ctx.strokeStyle = 'rgba(16,18,20,.85)';
    ctx.strokeText(text, cw / 2, ch / 2);
    ctx.fillStyle = '#ffe08a';
    ctx.fillText(text, cw / 2, ch / 2);

    var tex = new THREE.CanvasTexture(cv);
    tex.minFilter = THREE.LinearFilter;        
    tex.generateMipmaps = false;
    var mat = new THREE.SpriteMaterial({
      map: tex, transparent: true, depthTest: false, depthWrite: false,
    });
    var sp = new THREE.Sprite(mat);
    sp.center.set(0, 0.5);                     
    sp.renderOrder = 999;                      
    sp.visible = this.labelVisible;
    this._labelAspect = cw / ch;               
    this.scene.scene.add(sp);
    this._label = sp;
    this.updateLabel();                        
  };

  

  Gizmo.prototype.updateLabel = function () {
    var sp = this._label;
    if (!this.target) return;
    var cam = this.scene.camera;
    this.target.getWorldPosition(_labelPos);
    this.target.getWorldQuaternion(_labelQuat);
    var dist = _labelPos.distanceTo(cam.position) || 1;
    if (this._originSphere && this._originSphere.visible) {
      this._originSphere.scale.setScalar(ORIGIN_SPHERE_R_AT_1M * dist);
    }
    if (!sp || !sp.visible) return;
    var h = LABEL_H_AT_1M * dist;
    sp.scale.set(h * this._labelAspect, h, 1);
    
    _labelDir.copy(LABEL_DIR_LOCAL).applyQuaternion(_labelQuat)
             .multiplyScalar(LABEL_R_AT_1M * dist);
    sp.position.copy(_labelPos).add(_labelDir);
  };

  
  var _labelPos = new THREE.Vector3();
  var _labelDir = new THREE.Vector3();
  var _labelQuat = new THREE.Quaternion();
  var LABEL_DIR_LOCAL = new THREE.Vector3(1, 1, 0).normalize();   

  Gizmo.prototype._disposeLabel = function () {
    if (!this._label) return;
    this.scene.scene.remove(this._label);
    if (this._label.material) {
      if (this._label.material.map) this._label.material.map.dispose();
      this._label.material.dispose();
    }
    this._label = null;
  };
  Gizmo.prototype._disposeOriginSphere = function () {
    if (!this._originSphere) return;
    if (this.target) this.target.remove(this._originSphere);
    if (this._originSphere.geometry) this._originSphere.geometry.dispose();
    if (this._originSphere.material) this._originSphere.material.dispose();
    this._originSphere = null;
  };
  Gizmo.prototype.getAxesVisible = function () { return this.axesVisible; };

  
  Gizmo.prototype.followTwin = function (robot) {
    if (this.enabled || !robot) return;
    this.syncToPose(robot);
  };

  Gizmo.prototype.setEnabled = function (on) {
    
    this.enabled = on;
    for (var i = 0; i < this.controls.length; i++) {
      this.controls[i].enabled = on;   
    }
    if (on) {
      
      
      
      this.lastSolution = null;
      this.blocked = false;
      this._everSolved = false;
      this.lastValidPos.copy(this.target.position);      
      this.lastValidQuat.copy(this.target.quaternion);
      this._requestIK(true);
    }
  };

  Gizmo.prototype._requestIK = function (checkCollision) {
    this._solveFor(this.target.position, this.target.quaternion, checkCollision);
  };

  


  Gizmo.prototype._solveFor = function (pos, quat, checkCollision) {
    if (!this.enabled || !VizChannel.bridge) return;
    var e = new THREE.Euler().setFromQuaternion(quat, 'ZYX');
    var self = this;
    var reqPos = pos.clone();
    var reqQuat = quat.clone();
    


    var solvePos = reqPos.clone();
    if (this.toolOffsetM) solvePos.sub(this._toolVec(reqQuat));
    




    var fromDrag = !!this._dragMode;
    var reqId = VizChannel.request(function (payload) {
      if (payload.ok) {
        self.lastSolution = payload.joints_deg;
        self.lastTcp = payload.tcp;
        self._everSolved = true;
        self.lastValidPos.copy(reqPos);
        self.lastValidQuat.copy(reqQuat);
        if (self.blocked) {                    
          self.target.position.copy(reqPos);
          self.target.quaternion.copy(reqQuat);
        }
        self.blocked = false;
      } else if (payload.error === 'self_collision') {
        
        
        
        self.lastSolution = null;
        self.blocked = false;
        self.lastValidPos.copy(reqPos);
        self.lastValidQuat.copy(reqQuat);
      } else {
        
        
        
        self.lastSolution = null;              
        self.blocked = true;                   
        self.target.position.copy(self.lastValidPos);
        self.target.quaternion.copy(self.lastValidQuat);
      }
      
      
      if (self.onSolution) self.onSolution(payload, fromDrag);
    });
    VizChannel.bridge.solve_ik(JSON.stringify({
      req_id: reqId,
      xyz_mm: [solvePos.x * 1000, solvePos.y * 1000, solvePos.z * 1000],
      
      euler_xyz_deg: [e.x * RAD2DEG, e.y * RAD2DEG, e.z * RAD2DEG],
      seed_joints_deg: this.getSeed(),
      check_collision: !!checkCollision,
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      check_continuity: !this.skipContinuity &&
                        this._dragMode === 'translate' &&
                        SINGLE_AXIS[this._dragAxis] === true
    }));
  };

  window.Gizmo = Gizmo;
  Gizmo.Arbiter = GizmoArbiter;   
})();
