gt:         for (KeyPosition kp : p)
context: /*******************************************************************************
   * Copyright (C) 2009-2020 Human Media Interaction, University of Twente, the Netherlands
   *
   * This file is part of the Articulated Social Agents Platform BML realizer (ASAPRealizer).
   *
   * ASAPRealizer is free software: you can redistribute it and/or modify
   * it under the terms of the GNU Lesser General Public License (LGPL) as published by
   * the Free Software Foundation, either version 3 of the License, or
   * (at your option) any later version.
   *
   * ASAPRealizer is distributed in the hope that it will be useful,
   * but WITHOUT ANY WARRANTY; without even the implied warranty of
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   * GNU General Public License for more details.
   *
   * You should have received a copy of the GNU Lesser General Public License
   * along with ASAPRealizer.  If not, see http://www.gnu.org/licenses/.
   ******************************************************************************/
  package asap.animationengine.procanimation;
  
  import hmi.animation.Hanim;
  import hmi.animation.VJoint;
  import hmi.animation.VJointUtils;
  import hmi.math.Vec3f;
  import hmi.util.Resources;
  import hmi.util.StringUtil;
  
  import java.io.IOException;
  import java.util.ArrayList;
  import java.util.List;
  import java.util.Set;
  
  import lombok.extern.slf4j.Slf4j;
  import saiba.bml.BMLGestureSync;
  import asap.animationengine.AnimationPlayer;
  import asap.animationengine.MovementTimingUtils;
  import asap.animationengine.motionunit.AnimationUnit;
  import asap.animationengine.motionunit.MUSetupException;
  import asap.animationengine.transitions.SlerpTransitionToPoseMU;
  import asap.motionunit.MUPlayException;
  import asap.motionunit.MotionUnit;
  import asap.realizer.feedback.FeedbackManager;
  import asap.realizer.pegboard.BMLBlockPeg;
  import asap.realizer.pegboard.PegBoard;
  import asap.realizer.planunit.InvalidParameterException;
  import asap.realizer.planunit.KeyPosition;
  import asap.realizer.planunit.KeyPositionManager;
  import asap.realizer.planunit.KeyPositionManagerImpl;
  import asap.realizer.planunit.ParameterException;
  import asap.realizer.planunit.Priority;
  
  import com.google.common.collect.ImmutableSet;
  
  /**
   * Wraps a ProcAnimationMU inbetween a slerp transitions, and an automatically generated (by the restpose)
   * relax controller
   * The ProcAnimation is played between its strokeStart and strokeEnd
   * @author Herwin
   */
  @Slf4j
  public class ProcAnimationGestureMU implements GestureUnit
  {
      private ProcAnimationMU gestureUnit = new ProcAnimationMU();
      private KeyPositionManager keyPositionManager = new KeyPositionManagerImpl();
      private SlerpTransitionToPoseMU prepUnit;
      private AnimationUnit relaxUnit;
      private VJoint vStart;
      private VJoint vNext;
  
      // copy of the procanimation, used to figure out the stroke start positions
      private ProcAnimationMU copyProc;
      private VJoint copyJoint;
  
      private Resources resource;
      private double preStrokeHoldDuration = 0;
  
      public double getStrokeDuration()
      {
          return gestureUnit.getPrefDuration();
      }
  
      public double getPreStrokeHoldDuration()
      {
          return preStrokeHoldDuration;
      }
  
      public double getPostStrokeHoldDuration()
      {
          return postStrokeHoldDuration;
      }
  
      private double postStrokeHoldDuration = 0;
      private int priority = Priority.GESTURE;
      private AnimationPlayer aniPlayer;
  
      private float[] getPoseInStroke(String sync)
      {
          int i = 0;
          float[] pose = new float[gestureUnit.getControlledJoints().size() * 4];
  
          i = 0;
  
          copyProc.play(copyProc.getKeyPosition(sync).time + (0.01 / getStrokeDuration()));
          for (VJoint v : gestureUnit.getControlledJoints())
          {
              VJoint v2 = copyJoint.getPartBySid(v.getSid());
              v2.getRotation(pose, i);
              i += 4;
          }
          return pose;
      }
  
      public double getInterruptionDuration()
      {
          return aniPlayer.getRestPose().getTransitionToRestDuration(aniPlayer.getVCurr(),
                  VJointUtils.transformToSidSet(gestureUnit.getControlledJoints()));
      }
  
      public double getRetractionDuration()
      {
          copyProc.play(copyProc.getKeyPosition(BMLGestureSync.STROKE_END.getId()).time - (0.01 / getStrokeDuration()));
          return aniPlayer.getRestPose().getTransitionToRestDuration(copyJoint,
                  VJointUtils.transformToSidSet(gestureUnit.getControlledJoints()));
  
      }
  
      public double getPreparationDuration()
      {
          copyProc.play(copyProc.getKeyPosition(BMLGestureSync.STROKE_START.getId()).time + (0.01 / getStrokeDuration()));
  
          if (log.isDebugEnabled())
          {
              float vecDst[] = new float[3];
              float vecSrc[] = new float[3];
              copyJoint.getPart(Hanim.l_wrist).getPathTranslation(copyJoint.getPart(Hanim.l_shoulder).getParent(), vecDst);
              aniPlayer.getVCurrPartBySid(Hanim.l_wrist).getPathTranslation(aniPlayer.getVCurrPartBySid(Hanim.l_shoulder), vecSrc);
              log.debug("copyJoint lwrist pos at strokeStart:{}\n current lwrist pos:{}", Vec3f.toString(vecDst), Vec3f.toString(vecSrc));
          }
  
          double duration = MovementTimingUtils.getFittsMaximumLimbMovementDuration(aniPlayer.getVCurr(), copyJoint,
                  VJointUtils.transformToSidSet(gestureUnit.getControlledJoints()));
          if (duration > 0) return duration;
          return 1;
  
          // return aniPlayer.getRestPose().getTransitionToRestDuration(vCopy, VJointUtils.transformToSidSet(gestureUnit.getControlledJoints()));
      }
  
      public double getPreparationFromRestDuration()
      {
          copyProc.play(getKeyPosition(BMLGestureSync.STROKE_START.getId()).time + (0.01 / getStrokeDuration()));
          return aniPlayer.getRestPose().getTransitionToRestDuration(copyJoint,
                  VJointUtils.transformToSidSet(gestureUnit.getControlledJoints()));
      }
  
      public int getPriority()
      {
          return priority;
      }
  
      private boolean inGesturePhase(String syncId)
      {
          return !(syncId.equals(BMLGestureSync.START.getId()) || syncId.equals(BMLGestureSync.END.getId())
                  || syncId.equals(BMLGestureSync.READY.getId()) || syncId.equals(BMLGestureSync.RELAX.getId()));
      }
  
      @Override
      public void addKeyPosition(KeyPosition kp)
      {
          keyPositionManager.addKeyPosition(kp);
          if (inGesturePhase(kp.id))
          {
              gestureUnit.addKeyPosition(kp);
          }
      }
  
      public void setGestureUnit(ProcAnimationMU pa) throws MUSetupException
      {
          cleanup(gestureUnit);
          cleanup(copyProc);
          gestureUnit = pa;
          for (KeyPosition kp : gestureUnit.getKeyPositions())
          {
              if (inGesturePhase(kp.id))
              {
                  keyPositionManager.addKeyPosition(kp);
              }
          }
          setPreStrokeHoldDuration();
          setPostStrokeHoldDuration();
          gestureUnit.setup(vNext);
          if (vNext != null)
          {
              copyJoint = vNext.copyTree("copy-");
          }
          copyProc = gestureUnit.copy(copyJoint);
      }
  
      public void setAnimationPlayer(AnimationPlayer ap) throws MUSetupException
      {
          vStart = ap.getVCurr();
          vNext = ap.getVNext();
          gestureUnit.setup(ap);
          copyJoint = vNext.copyTree("copy-");
          copyProc = gestureUnit.copy(copyJoint);
          aniPlayer = ap;
      }
  
      public void setVNext(VJoint vj)
      {
          vNext = vj;
      }
  
      @Override
      public List<KeyPosition> getKeyPositions()
      {
          return keyPositionManager.getKeyPositions();
      }
  
      @Override
      public void setKeyPositions(List<KeyPosition> p)
      {
          keyPositionManager.setKeyPositions(p);
  