




(function () {
  'use strict';

  function Sim(opts) {
    opts = opts || {};
    this.onJoints = opts.onJoints || function () {};       
    this.onProgress = opts.onProgress || function () {};   
    this.onEvent = opts.onEvent || function () {};         
    this.onLoop = opts.onLoop || function () {};           
    this.onWaypoint = opts.onWaypoint || function () {};   
    this.onDone = opts.onDone || function () {};
    this.segments = [];
    this.playing = false;
    this.loop = false;
    this._segIdx = 0;
    this._segStart = 0;
    this._cur = null;     
    this._cursor = 0;     
  }

  


  function trapezoid(k, r) {
    if (!(r > 0)) return k;
    if (r > 0.5) r = 0.5;          
    var v = 1 / (1 - r);           
    if (k <= r) return v * k * k / (2 * r);
    if (k >= 1 - r) { var d = 1 - k; return 1 - v * d * d / (2 * r); }
    return v * (k - r / 2);
  }

  


  Sim.prototype.start = function (segments, fromJoints, loop, wrapDurationS, wrapRampS,
                                  opts) {
    opts = opts || {};
    this.segments = segments || [];
    this._cur = (fromJoints || []).slice();
    this._segIdx = 0;
    this._segStart = 0;     
    this._started = false;  
    this._enteredIdx = -1;  
    this._cursor = 0;
    this.cycles = 0;        
    this.loop = !!loop;
    this.wrapDurationS = (typeof wrapDurationS === 'number') ? wrapDurationS : null;
    this.wrapRampS = wrapRampS || null;
    
    this.syncMachine = !!opts.syncMachine;
    this.modelStep = -1;    
    this.machineStep = -1;  
    this._curWp = null;     
    this._holding = false;  
    this.playing = this.segments.length > 0;
    if (this.playing) this.onJoints(this._cur.slice());
  };

  Sim.prototype.stop = function () {
    this.playing = false;
    this.loop = false;
    this._holding = false;
  };

  


  Sim.prototype.setMachineStep = function (n) {
    this.machineStep = n;
  };

  

  var MAX_SEG_PER_FRAME = 64;

  Sim.prototype.update = function (now) {
    if (!this.playing) return;
    if (!this._started) { this._started = true; this._segStart = now; }
    for (var guard = 0; guard < MAX_SEG_PER_FRAME; guard++) {
      if (this._segIdx >= this.segments.length) {
        if (this.loop) {
          
          
          var first = this.segments[0];
          if (first && first.type === 'joint') {
            first.from = this._cur.slice();
            if (this.wrapDurationS !== null) first.duration_s = this.wrapDurationS;
            if (this.wrapRampS) first.ramp_s = this.wrapRampS;   
          }
          this._segIdx = 0;
          this._enteredIdx = -1;
          this._cursor = 0;
          this.cycles++;
          this.onLoop(this.cycles);
          continue;      
        }
        this.playing = false;
        this.onDone();
        return;
      }
      var seg = this.segments[this._segIdx];
      if (this._enteredIdx !== this._segIdx) {
        
        if (seg.index !== this._curWp) {
          if (!this._enterWaypoint(seg, now)) return;   
        }
        this._enteredIdx = this._segIdx;
        
        
        this.onProgress(seg.index, seg.type);
        
        
        if (seg.type === 'vacuum' || seg.type === 'servo') this.onEvent(seg);
      }
      
      
      
      
      
      var hasDur = (typeof seg.duration_s === 'number');
      var durMs = hasDur ? seg.duration_s * 1000 : 0;
      if (!hasDur && (seg.type === 'joint' || seg.type === 'path')) durMs = 300;
      var k = durMs > 0 ? (now - this._segStart) / durMs : 1;
      
      
      if (this.syncMachine && this.machineStep - this.modelStep >= 2) k = 1;
      if (k >= 1) {
        
        
        
        this._finishSeg(seg);
        this._segStart += durMs;
        this._segIdx++;
        this._cursor = 0;
        continue;
      }
      this._renderSeg(seg, k);
      return;
    }
  };

  









  Sim.prototype._enterWaypoint = function (seg, now) {
    var step = this.modelStep + 1;
    if (this.syncMachine && step > this.machineStep) {
      this._holding = true;
      return false;
    }
    if (this._holding) {
      
      
      this._holding = false;
      this._segStart = now;
    }
    this.modelStep = step;
    this._curWp = seg.index;
    this.onWaypoint(step, seg.index);
    return true;
  };

  
  Sim.prototype._renderSeg = function (seg, k) {
    if (seg.type === 'joint') {
      
      
      
      var ramp = seg.ramp_s;
      var durS = seg.duration_s || 0;
      for (var i = 0; i < 6; i++) {
        var s = (ramp && durS > 0) ? trapezoid(k, (ramp[i] || 0) / durS) : k;
        this._cur[i] = seg.from[i] + (seg.to[i] - seg.from[i]) * s;
      }
      this.onJoints(this._cur.slice());
    } else if (seg.type === 'path') {
      var n = seg.points.length;
      var cum = seg.cum_t;
      var i0, i1, frac;
      if (n < 2) {
        i0 = i1 = 0; frac = 0;
      } else if (cum && cum.length === n && cum[n - 1] > 0) {
        
        var tS = k * cum[n - 1];
        var c = Math.min(this._cursor, n - 2);
        while (c < n - 2 && cum[c + 1] <= tS) c++;
        while (c > 0 && cum[c] > tS) c--;
        this._cursor = c;
        i0 = c; i1 = c + 1;
        var span = cum[i1] - cum[i0];
        frac = span > 0 ? (tS - cum[i0]) / span : 1;
        frac = frac < 0 ? 0 : (frac > 1 ? 1 : frac);
      } else {
        
        var f = k * (n - 1);
        i0 = Math.floor(f); i1 = Math.min(i0 + 1, n - 1); frac = f - i0;
      }
      for (var j = 0; j < 6; j++) {
        this._cur[j] = seg.points[i0][j] + (seg.points[i1][j] - seg.points[i0][j]) * frac;
      }
      this.onJoints(this._cur.slice());
    }
    
  };

  
  Sim.prototype._finishSeg = function (seg) {
    if (!seg) return;
    if (seg.type === 'joint') this._cur = seg.to.slice();
    else if (seg.type === 'path') this._cur = seg.points[seg.points.length - 1].slice();
    else return;
    this.onJoints(this._cur.slice());
  };

  window.Sim = Sim;
})();
