package us.ihmc.avatar.posePlayback;

import us.ihmc.yoVariables.registry.YoRegistry;
import us.ihmc.simulationconstructionset.SimulationConstructionSet;
import us.ihmc.commons.thread.ThreadTools;
import us.ihmc.simulationConstructionSetTools.util.HumanoidFloatingRootJointRobot;

public class PoseInterpolatorPlaybacker
{
   public static void playASequence(HumanoidFloatingRootJointRobot sdfRobot, PlaybackPoseSequence sequence, boolean showGui, PoseCheckerCallback poseCheckerCallback)
   {
      YoRegistry registry = new YoRegistry("PoseInterpolatorPlaybacker");
      PlaybackPoseInterpolator interpolator = new PlaybackPoseInterpolator(registry);

      double simulateDT = 0.005;
      
      SimulationConstructionSet scs = null;
      scs = new SimulationConstructionSet(sdfRobot);
      if (showGui)
      {
         int recordFrequency = 1;
         scs.setDT(simulateDT, recordFrequency);
         scs.addYoRegistry(registry);
         scs.startOnAThread();
      }

      double startTime = 1.7;
      double time = startTime;

      interpolator.startSequencePlayback(sequence, startTime);


      PlaybackPose previousPose = null;
      while (!interpolator.isDone())
      {
         time = time + simulateDT;
         scs.setTime(time);

         PlaybackPose pose = interpolator.getPose(time);

//         System.out.println(pose);

         if (showGui)
         {
            pose.setRobotAtPose(sdfRobot);
            scs.tickAndUpdate();
         }

         poseCheckerCallback.checkPose(pose, previousPose);
         previousPose = pose;
      }

      if (showGui)
      {
         ThreadTools.sleepForever();
      }
   }
}
