text: package us.ihmc.simulationConstructionSetTools.robotController;
  
  import us.ihmc.euclid.transform.RigidBodyTransform;
  import us.ihmc.yoVariables.registry.YoRegistry;
  import us.ihmc.robotics.robotController.RobotControllerExecutor;
  import us.ihmc.simulationconstructionset.Robot;
  
  class SingleThreadedRobotControllerExecutor implements RobotControllerExecutor
  {
     private final long ticksPerSimulationTick;
     private final MultiThreadedRobotControlElement robotControlElement;
     private final boolean skipFirstControlCycle;
     private final Robot simulatedRobot;
     
     SingleThreadedRobotControllerExecutor(Robot simulatedRobot, MultiThreadedRobotControlElement robotControlElement, int ticksPerSimulationTick, boolean skipFirstControlCycle, YoRegistry parentRegistry)
     {
        this.ticksPerSimulationTick = ticksPerSimulationTick;
        this.robotControlElement = robotControlElement;
        this.skipFirstControlCycle = skipFirstControlCycle;
        this.simulatedRobot = simulatedRobot;
        
        parentRegistry.addChild(robotControlElement.getYoVariableRegistry());
     }
  
     @Override
     public void waitAndWriteData(long tick)
     {
        if (tick % ticksPerSimulationTick == 0 && !(tick == 0 && skipFirstControlCycle))
        {
           robotControlElement.write(System.nanoTime());
           if (simulatedRobot != null)
           {
              RigidBodyTransform transformToWorld = new RigidBodyTransform();
              simulatedRobot.getRootJoints().get(0).getTransformToWorld(transformToWorld);
              if (robotControlElement.getYoGraphicsListRegistry() != null)
              {
                 robotControlElement.getYoGraphicsListRegistry().setSimulationTransformToWorld(transformToWorld);
              }
           }
        }
     }
  
     @Override
     public void readData(long tick)
     {
        if (tick % ticksPerSimulationTick == 0 && !(tick == 0 && skipFirstControlCycle))
        {
           robotControlElement.read(System.nanoTime());
        }
     }
  
     @Override
     public void executeForSimulationTick(long tick)
     {
        if (tick % ticksPerSimulationTick == 0 && !(tick == 0 && skipFirstControlCycle))
        {
           robotControlElement.run();
        }
     }
  
     @Override
     public void initialize()
     {
        robotControlElement.initialize();
     }
     
     @Override
     public long getTicksPerSimulationTick()
     {
        return ticksPerSimulationTick;
     }
  
     @Override
     public void stop()
     {
        // Nothing to do here.
     }
  
     @Override
     public void updateYoGraphicsListRegistry()
     {
     }
     
  
  }
  