/**
   * Creates a KeyFrameInterpolator, with {@code frame} as associated {@link #frame()}.
   *
   * <p>The {@link #frame()} can be set or changed using {@link #setFrame(Frame)}.
   *
   * <p>{@link #interpolationTime()}, {@link #interpolationSpeed()} and {@link
   * #interpolationPeriod()} are set to their default values.
   */
  public KeyFrameInterpolator(AbstractScene scn, Frame frame) {
    gScene = scn;
    keyFrameList = new ArrayList<KeyFrame>();
    path = new ArrayList<Frame>();
    mainFrame = null;
    period = 40;
    interpolationTm = 0.0f;
    interpolationSpd = 1.0f;
    interpolationStrt = false;
    lpInterpolation = false;
    pathIsValid = false;
    valuesAreValid = true;
    currentFrmValid = false;
    setFrame(frame);

    currentFrame0 = keyFrameList.listIterator();
    currentFrame1 = keyFrameList.listIterator();
    currentFrame2 = keyFrameList.listIterator();
    currentFrame3 = keyFrameList.listIterator();

    interpolationTimerTask =
        new TimingTask() {
          public void execute() {
            update();
          }
        };
    gScene.registerTimingTask(interpolationTimerTask);
  }
  /**
   * Appends a new keyFrame to the path.
   *
   * <p>Same as {@link #addKeyFrame(GenericFrame, float)}, except that the {@link
   * #keyFrameTime(int)} is set to the previous {@link #keyFrameTime(int)} plus one second (or 0.0
   * if there is no previous keyFrame).
   */
  public void addKeyFrame(GenericFrame frame) {
    float time;

    if (keyFrameList.isEmpty()) time = 0.0f;
    else time = keyFrameList.get(keyFrameList.size() - 1).time() + 1.0f;

    addKeyFrame(frame, time);
  }
 /**
  * Remove KeyFrame according to {@code index} in the list and {@link #stopInterpolation()} if
  * {@link #interpolationStarted()}. If {@code index < 0 || index >= keyFr.size()} the call is
  * silently ignored.
  */
 public void removeKeyFrame(int index) {
   if (index < 0 || index >= keyFrameList.size()) return;
   valuesAreValid = false;
   pathIsValid = false;
   currentFrmValid = false;
   if (interpolationStarted()) stopInterpolation();
   KeyFrame kf = keyFrameList.remove(index);
   for (Agent agent : gScene.inputHandler().agents()) agent.removeGrabber(kf.frm);
   setInterpolationTime(firstTime());
 }
 protected void updateSplineCache() {
   Vec deltaP =
       Vec.subtract(
           keyFrameList.get(currentFrame2.nextIndex()).position(),
           keyFrameList.get(currentFrame1.nextIndex()).position());
   pv1 =
       Vec.add(
           Vec.multiply(deltaP, 3.0f),
           Vec.multiply(keyFrameList.get(currentFrame1.nextIndex()).tgP(), (-2.0f)));
   pv1 = Vec.subtract(pv1, keyFrameList.get(currentFrame2.nextIndex()).tgP());
   pv2 = Vec.add(Vec.multiply(deltaP, (-2.0f)), keyFrameList.get(currentFrame1.nextIndex()).tgP());
   pv2 = Vec.add(pv2, keyFrameList.get(currentFrame2.nextIndex()).tgP());
   splineCacheIsValid = true;
 }
  protected void updateModifiedFrameValues() {
    KeyFrame kf;
    KeyFrame prev = keyFrameList.get(0);
    kf = keyFrameList.get(0);

    int index = 1;
    while (kf != null) {
      KeyFrame next = (index < keyFrameList.size()) ? keyFrameList.get(index) : null;
      index++;
      if (next != null) kf.computeTangent(prev, next);
      else kf.computeTangent(prev, kf);
      prev = kf;
      kf = next;
    }
    valuesAreValid = true;
  }
 /** Removes all keyFrames from the path. The {@link #numberOfKeyFrames()} is set to 0. */
 public void deletePath() {
   stopInterpolation();
   keyFrameList.clear();
   pathIsValid = false;
   valuesAreValid = false;
   currentFrmValid = false;
 }
 /**
  * Returns the Frame associated with the keyFrame at index {@code index}.
  *
  * <p>See also {@link #keyFrameTime(int)}. {@code index} has to be in the range 0..{@link
  * #numberOfKeyFrames()}-1.
  *
  * <p><b>Note:</b> If this keyFrame was defined using a reference to a Frame (see {@link
  * #addKeyFrame(GenericFrame, float)} the current referenced Frame state is returned.
  */
 public GenericFrame keyFrame(int index) {
   /**
    * AbstractKeyFrame kf = keyFr.get(index); return new Frame(kf.orientation(), kf.position(),
    * kf.magnitude());
    */
   return keyFrameList.get(index).frame();
 }
  protected KeyFrameInterpolator(KeyFrameInterpolator otherKFI) {
    this.gScene = otherKFI.gScene;
    this.path = new ArrayList<Frame>();
    ListIterator<Frame> frameIt = otherKFI.path.listIterator();
    while (frameIt.hasNext()) {
      this.path.add(frameIt.next().get());
    }

    this.setFrame(otherKFI.frame());

    this.period = otherKFI.period;
    this.interpolationTm = otherKFI.interpolationTm;
    this.interpolationSpd = otherKFI.interpolationSpd;
    this.interpolationStrt = otherKFI.interpolationStrt;
    this.lpInterpolation = otherKFI.lpInterpolation;
    this.pathIsValid = otherKFI.pathIsValid;
    this.valuesAreValid = otherKFI.valuesAreValid;
    this.currentFrmValid = otherKFI.currentFrmValid;

    this.keyFrameList = new ArrayList<KeyFrame>();

    for (KeyFrame element : otherKFI.keyFrameList) {
      KeyFrame kf = (KeyFrame) element.get();
      this.keyFrameList.add(kf);
    }

    this.currentFrame0 = keyFrameList.listIterator(otherKFI.currentFrame0.nextIndex());
    this.currentFrame1 = keyFrameList.listIterator(otherKFI.currentFrame1.nextIndex());
    this.currentFrame2 = keyFrameList.listIterator(otherKFI.currentFrame2.nextIndex());
    this.currentFrame3 = keyFrameList.listIterator(otherKFI.currentFrame3.nextIndex());

    this.interpolationTimerTask =
        new TimingTask() {
          public void execute() {
            update();
          }
        };
    gScene.registerTimingTask(interpolationTimerTask);

    this.invalidateValues();
  }
  /**
   * Starts the interpolation process.
   *
   * <p>A timer is started with an {@link #interpolationPeriod()} period that updates the {@link
   * #frame()}'s position, orientation and magnitude. {@link #interpolationStarted()} will return
   * {@code true} until {@link #stopInterpolation()} or {@link #toggleInterpolation()} is called.
   *
   * <p>If {@code period} is positive, it is set as the new {@link #interpolationPeriod()}. The
   * previous {@link #interpolationPeriod()} is used otherwise (default).
   *
   * <p>If {@link #interpolationTime()} is larger than {@link #lastTime()}, {@link
   * #interpolationTime()} is reset to {@link #firstTime()} before interpolation starts (and
   * inversely for negative {@link #interpolationSpeed()}.
   *
   * <p>Use {@link #setInterpolationTime(float)} before calling this method to change the starting
   * {@link #interpolationTime()}.
   *
   * <p><b>Attention:</b> The keyFrames must be defined (see {@link #addKeyFrame(GenericFrame,
   * float)}) before you startInterpolation(), or else the interpolation will naturally immediately
   * stop.
   */
  public void startInterpolation(int myPeriod) {
    if (myPeriod >= 0) setInterpolationPeriod(myPeriod);

    if (!keyFrameList.isEmpty()) {
      if ((interpolationSpeed() > 0.0)
          && (interpolationTime() >= keyFrameList.get(keyFrameList.size() - 1).time()))
        setInterpolationTime(keyFrameList.get(0).time());
      if ((interpolationSpeed() < 0.0) && (interpolationTime() <= keyFrameList.get(0).time()))
        setInterpolationTime(keyFrameList.get(keyFrameList.size() - 1).time());
      if (keyFrameList.size() > 1) interpolationTimerTask.run(interpolationPeriod());
      interpolationStrt = true;
      update();
    }
  }
  protected void updateCurrentKeyFrameForTime(float time) {
    // Assertion: times are sorted in monotone order.
    // Assertion: keyFrame_ is not empty

    // TODO: Special case for loops when closed path is implemented !!
    if (!currentFrmValid)
      // Recompute everything from scratch
      currentFrame1 = keyFrameList.listIterator();

    // currentFrame_[1]->peekNext() <---> keyFr.get(currentFrame1.nextIndex());
    while (keyFrameList.get(currentFrame1.nextIndex()).time() > time) {
      currentFrmValid = false;
      if (!currentFrame1.hasPrevious()) break;
      currentFrame1.previous();
    }

    if (!currentFrmValid) currentFrame2 = keyFrameList.listIterator(currentFrame1.nextIndex());

    while (keyFrameList.get(currentFrame2.nextIndex()).time() < time) {
      currentFrmValid = false;

      if (!currentFrame2.hasNext()) break;

      currentFrame2.next();
    }

    if (!currentFrmValid) {
      currentFrame1 = keyFrameList.listIterator(currentFrame2.nextIndex());

      if ((currentFrame1.hasPrevious())
          && (time < keyFrameList.get(currentFrame2.nextIndex()).time())) currentFrame1.previous();

      currentFrame0 = keyFrameList.listIterator(currentFrame1.nextIndex());

      if (currentFrame0.hasPrevious()) currentFrame0.previous();

      currentFrame3 = keyFrameList.listIterator(currentFrame2.nextIndex());

      if (currentFrame3.hasNext()) currentFrame3.next();

      currentFrmValid = true;
      splineCacheIsValid = false;
    }
  }
  /**
   * Appends a new keyFrame to the path, with its associated {@code time} (in seconds).
   *
   * <p>When {@code setRef} is {@code false} the keyFrame is added by value, meaning that the path
   * will use the current {@code frame} state.
   *
   * <p>When {@code setRef} is {@code true} the keyFrame is given as a reference to a Frame, which
   * will be connected to the KeyFrameInterpolator: when {@code frame} is modified, the
   * KeyFrameInterpolator path is updated accordingly. This allows for dynamic paths, where keyFrame
   * can be edited, even during the interpolation. {@code null} frame references are silently
   * ignored. The {@link #keyFrameTime(int)} has to be monotonously increasing over keyFrames.
   */
  public void addKeyFrame(GenericFrame frame, float time) {
    if (frame == null) return;

    if (keyFrameList.isEmpty()) interpolationTm = time;

    if ((!keyFrameList.isEmpty()) && (keyFrameList.get(keyFrameList.size() - 1).time() > time))
      System.out.println("Error in KeyFrameInterpolator.addKeyFrame: time is not monotone");
    else {
      if (gScene.is3D()) keyFrameList.add(new KeyFrame3D(frame, time));
      else keyFrameList.add(new KeyFrame2D(frame, time));
    }

    valuesAreValid = false;
    pathIsValid = false;
    currentFrmValid = false;
    resetInterpolation();
  }
  /**
   * Interpolate {@link #frame()} at time {@code time} (expressed in seconds). {@link
   * #interpolationTime()} is set to {@code time} and {@link #frame()} is set accordingly.
   *
   * <p>If you simply want to change {@link #interpolationTime()} but not the {@link #frame()}
   * state, use {@link #setInterpolationTime(float)} instead.
   */
  public void interpolateAtTime(float time) {
    this.checkValidity();
    setInterpolationTime(time);

    if ((keyFrameList.isEmpty()) || (frame() == null)) return;

    if (!valuesAreValid) updateModifiedFrameValues();

    updateCurrentKeyFrameForTime(time);

    if (!splineCacheIsValid) updateSplineCache();

    float alpha;
    float dt =
        keyFrameList.get(currentFrame2.nextIndex()).time()
            - keyFrameList.get(currentFrame1.nextIndex()).time();
    if (Util.zero(dt)) alpha = 0.0f;
    else alpha = (time - keyFrameList.get(currentFrame1.nextIndex()).time()) / dt;

    Vec pos =
        Vec.add(
            keyFrameList.get(currentFrame1.nextIndex()).position(),
            Vec.multiply(
                Vec.add(
                    keyFrameList.get(currentFrame1.nextIndex()).tgP(),
                    Vec.multiply(Vec.add(pv1, Vec.multiply(pv2, alpha)), alpha)),
                alpha));

    float mag =
        Util.lerp(
            keyFrameList.get(currentFrame1.nextIndex()).magnitude(),
            keyFrameList.get(currentFrame2.nextIndex()).magnitude(),
            alpha);

    Rotation q;
    if (gScene.is3D()) {
      q =
          Quat.squad(
              (Quat) keyFrameList.get(currentFrame1.nextIndex()).orientation(),
              ((KeyFrame3D) keyFrameList.get(currentFrame1.nextIndex())).tgQ(),
              ((KeyFrame3D) keyFrameList.get(currentFrame2.nextIndex())).tgQ(),
              (Quat) keyFrameList.get(currentFrame2.nextIndex()).orientation(),
              alpha);
    } else {
      q =
          new Rot(
              Util.lerp(
                  keyFrameList.get(currentFrame1.nextIndex()).orientation().angle(),
                  keyFrameList.get(currentFrame2.nextIndex()).orientation().angle(),
                  (alpha)));
    }

    frame().setPositionWithConstraint(pos);
    frame().setRotationWithConstraint(q);
    frame().setMagnitude(mag);
  }
 /**
  * Returns the number of keyFrames used by the interpolation. Use {@link
  * #addKeyFrame(GenericFrame)} to add new keyFrames.
  */
 public int numberOfKeyFrames() {
   return keyFrameList.size();
 }
 /**
  * Returns the time corresponding to the last keyFrame, expressed in seconds.
  *
  * <p>
  *
  * @see #firstTime()
  * @see #duration()
  * @see #keyFrameTime(int)
  */
 public float lastTime() {
   if (keyFrameList.isEmpty()) return 0.0f;
   else return keyFrameList.get(keyFrameList.size() - 1).time();
 }
 /**
  * Returns the time corresponding to the first keyFrame, expressed in seconds.
  *
  * <p>Returns 0.0 if the path is empty.
  *
  * @see #lastTime()
  * @see #duration()
  * @see #keyFrameTime(int)
  */
 public float firstTime() {
   if (keyFrameList.isEmpty()) return 0.0f;
   else return keyFrameList.get(0).time();
 }
 /**
  * Returns the time corresponding to the {@code index} keyFrame. index has to be in the range 0..
  * {@link #numberOfKeyFrames()}-1.
  *
  * @see #keyFrame(int)
  */
 public float keyFrameTime(int index) {
   return keyFrameList.get(index).time();
 }
  /** Intenal use. Call {@link #checkValidity()} and if path is not valid recomputes it. */
  protected void updatePath() {
    checkValidity();
    if (!pathIsValid) {
      path.clear();
      int nbSteps = 30;

      if (keyFrameList.isEmpty()) return;

      if (!valuesAreValid) updateModifiedFrameValues();

      if (keyFrameList.get(0) == keyFrameList.get(keyFrameList.size() - 1))
        // TODO experimenting really
        path.add(
            new Frame(
                keyFrameList.get(0).position(),
                keyFrameList.get(0).orientation(),
                keyFrameList.get(0).magnitude()));
      else {
        KeyFrame[] kf = new KeyFrame[4];
        kf[0] = keyFrameList.get(0);
        kf[1] = kf[0];

        int index = 1;
        kf[2] = (index < keyFrameList.size()) ? keyFrameList.get(index) : null;
        index++;
        kf[3] = (index < keyFrameList.size()) ? keyFrameList.get(index) : null;

        while (kf[2] != null) {
          Vec pdiff = Vec.subtract(kf[2].position(), kf[1].position());
          Vec pvec1 = Vec.add(Vec.multiply(pdiff, 3.0f), Vec.multiply(kf[1].tgP(), (-2.0f)));
          pvec1 = Vec.subtract(pvec1, kf[2].tgP());
          Vec pvec2 = Vec.add(Vec.multiply(pdiff, (-2.0f)), kf[1].tgP());
          pvec2 = Vec.add(pvec2, kf[2].tgP());

          for (int step = 0; step < nbSteps; ++step) {
            Frame frame = new Frame();
            float alpha = step / (float) nbSteps;
            frame.setPosition(
                Vec.add(
                    kf[1].position(),
                    Vec.multiply(
                        Vec.add(
                            kf[1].tgP(),
                            Vec.multiply(Vec.add(pvec1, Vec.multiply(pvec2, alpha)), alpha)),
                        alpha)));
            if (gScene.is3D()) {
              frame.setOrientation(
                  Quat.squad(
                      (Quat) kf[1].orientation(),
                      ((KeyFrame3D) kf[1]).tgQ(),
                      ((KeyFrame3D) kf[2]).tgQ(),
                      (Quat) kf[2].orientation(),
                      alpha));
            } else {
              // linear interpolation
              float start = kf[1].orientation().angle();
              float stop = kf[2].orientation().angle();
              frame.setOrientation(new Rot(start + (stop - start) * alpha));
            }
            frame.setMagnitude(Util.lerp(kf[1].magnitude(), kf[2].magnitude(), alpha));
            path.add(frame.get());
          }

          // Shift
          kf[0] = kf[1];
          kf[1] = kf[2];
          kf[2] = kf[3];

          index++;
          kf[3] = (index < keyFrameList.size()) ? keyFrameList.get(index) : null;
        }
        // Add last KeyFrame
        path.add(new Frame(kf[1].position(), kf[1].orientation(), kf[1].magnitude()));
      }
      pathIsValid = true;
    }
  }
  /**
   * Updates {@link #frame()} state according to current {@link #interpolationTime()}. Then adds
   * {@link #interpolationPeriod()}* {@link #interpolationSpeed()} to {@link #interpolationTime()}.
   *
   * <p>This internal method is called by a timer when {@link #interpolationStarted()}. It can be
   * used for debugging purpose. {@link #stopInterpolation()} is called when {@link
   * #interpolationTime()} reaches {@link #firstTime()} or {@link #lastTime()}, unless {@link
   * #loopInterpolation()} is {@code true}.
   */
  protected void update() {
    interpolateAtTime(interpolationTime());

    interpolationTm += interpolationSpeed() * interpolationPeriod() / 1000.0f;

    if (interpolationTime() > keyFrameList.get(keyFrameList.size() - 1).time()) {
      if (loopInterpolation())
        setInterpolationTime(
            keyFrameList.get(0).time()
                + interpolationTm
                - keyFrameList.get(keyFrameList.size() - 1).time());
      else {
        // Make sure last KeyFrame is reached and displayed
        interpolateAtTime(keyFrameList.get(keyFrameList.size() - 1).time());
        stopInterpolation();
      }
    } else if (interpolationTime() < keyFrameList.get(0).time()) {
      if (loopInterpolation())
        setInterpolationTime(
            keyFrameList.get(keyFrameList.size() - 1).time()
                - keyFrameList.get(0).time()
                + interpolationTm);
      else {
        // Make sure first KeyFrame is reached and displayed
        interpolateAtTime(keyFrameList.get(0).time());
        stopInterpolation();
      }
    }
  }