public abstract class DRCPushRecoveryMultiStepTest implements MultiRobotTestInterface {
  private static final SimulationTestingParameters simulationTestingParameters =
      SimulationTestingParameters.createFromEnvironmentVariables();

  private DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack;

  private static final boolean VISUALIZE_FORCE = false;

  private static final double PUSH_DELAY = 0.5;

  protected PushRobotController pushRobotController;

  protected BlockingSimulationRunner blockingSimulationRunner;

  protected double forceMagnitude;

  protected double forceDuration;

  protected SideDependentList<StateTransitionCondition> doubleSupportStartConditions =
      new SideDependentList<>();

  StateTransitionCondition pushCondition = doubleSupportStartConditions.get(RobotSide.LEFT);

  @Before
  public void showMemoryUsageBeforeTest() {
    MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
        getClass().getSimpleName() + " before test.");
  }

  @After
  public void destroySimulationAndRecycleMemory() {
    if (simulationTestingParameters.getKeepSCSUp()) {
      ThreadTools.sleepForever();
    }

    // Do this here in case a test fails. That way the memory will be recycled.
    if (drcFlatGroundWalkingTrack != null) {
      drcFlatGroundWalkingTrack.destroySimulation();
      drcFlatGroundWalkingTrack = null;
    }

    if (blockingSimulationRunner != null) {
      blockingSimulationRunner.destroySimulation();
      blockingSimulationRunner = null;
    }

    pushRobotController = null;
    doubleSupportStartConditions = null;
    pushCondition = null;
    MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
        getClass().getSimpleName() + " after test.");
  }

  @Ignore("Needs to be improved")
  @ContinuousIntegrationTest(estimatedDuration = 67.1)
  @Test(timeout = 340000)
  public void testMultiStepForwardAndContinueWalking()
      throws SimulationExceededMaximumTimeException, InterruptedException,
          ControllerFailureException {
    BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
    setupTest(getRobotModel());
    SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
    setForwardPushParameters();
    Vector3d forceDirection = new Vector3d(1.0, 0.0, 0.0);
    blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0);
    BooleanYoVariable walk =
        (BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");

    // disable walking
    walk.set(false);
    blockingSimulationRunner.simulateAndBlock(4.0);

    // push the robot
    pushRobotController.applyForceDelayed(
        pushCondition, PUSH_DELAY, forceDirection, forceMagnitude, forceDuration);

    // simulate for a little while longer
    blockingSimulationRunner.simulateAndBlock(forceDuration + 6.0);

    // re-enable walking
    walk.set(true);
    blockingSimulationRunner.simulateAndBlock(6.0);

    Point3d center = new Point3d(0.0, 0.0, 0.8882009563211146);
    Vector3d plusMinusVector =
        new Vector3d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5);
    BoundingBox3d boundingBox =
        BoundingBox3d.createUsingCenterAndPlusMinusVector(center, plusMinusVector);
    DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(
        boundingBox, drcFlatGroundWalkingTrack.getDrcSimulation().getRobot());

    createVideo(scs);
    BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
  }

  @ContinuousIntegrationTest(estimatedDuration = 53.2)
  @Test(timeout = 270000)
  public void testMultiStepBackwardAndContinueWalking()
      throws SimulationExceededMaximumTimeException, InterruptedException,
          ControllerFailureException {
    BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
    setupTest(getRobotModel());
    SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
    setBackwardPushParameters();
    Vector3d forceDirection = new Vector3d(1.0, 0.0, 0.0);
    blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0);
    BooleanYoVariable walk =
        (BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");

    // disable walking
    walk.set(false);
    blockingSimulationRunner.simulateAndBlock(1.0);

    // push the robot
    pushRobotController.applyForceDelayed(
        pushCondition, PUSH_DELAY, forceDirection, forceMagnitude, forceDuration);

    // simulate for a little while longer
    blockingSimulationRunner.simulateAndBlock(forceDuration + 5.0);

    // re-enable walking
    walk.set(true);
    blockingSimulationRunner.simulateAndBlock(6.0);

    Point3d center = new Point3d(0.0, 0.0, 0.8882009563211146);
    Vector3d plusMinusVector =
        new Vector3d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5);
    BoundingBox3d boundingBox =
        BoundingBox3d.createUsingCenterAndPlusMinusVector(center, plusMinusVector);
    DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(
        boundingBox, drcFlatGroundWalkingTrack.getDrcSimulation().getRobot());

    createVideo(scs);
    BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
  }

  protected void setupTest(DRCRobotModel robotModel)
      throws SimulationExceededMaximumTimeException, InterruptedException {
    boolean runMultiThreaded = false;
    setupTrack(runMultiThreaded, robotModel);
    FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
    pushRobotController =
        new PushRobotController(
            drcFlatGroundWalkingTrack.getDrcSimulation().getRobot(), fullRobotModel);

    if (VISUALIZE_FORCE) {
      drcFlatGroundWalkingTrack
          .getSimulationConstructionSet()
          .addYoGraphic(pushRobotController.getForceVisualizer());
    }

    SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
    BooleanYoVariable enable =
        (BooleanYoVariable) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery");

    // enable push recovery
    enable.set(true);

    for (RobotSide robotSide : RobotSide.values) {
      String prefix = fullRobotModel.getFoot(robotSide).getName();
      scs.getVariable(prefix + "FootControlModule", prefix + "State");
      @SuppressWarnings("unchecked")
      final EnumYoVariable<WalkingStateEnum> walkingState =
          (EnumYoVariable<WalkingStateEnum>)
              scs.getVariable("WalkingHighLevelHumanoidController", "walkingState");
      doubleSupportStartConditions.put(
          robotSide, new DoubleSupportStartCondition(walkingState, robotSide));
    }
  }

  private void setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel) {
    DRCGuiInitialSetup guiInitialSetup =
        new DRCGuiInitialSetup(true, false, simulationTestingParameters);
    GroundProfile3D groundProfile = new FlatGroundProfile();
    DRCSCSInitialSetup scsInitialSetup =
        new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT());

    // scsInitialSetup.setInitializeEstimatorToActual(true);
    scsInitialSetup.setDrawGroundProfile(true);
    scsInitialSetup.setRunMultiThreaded(runMultiThreaded);
    DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup =
        robotModel.getDefaultRobotInitialSetup(0.0, 0.0);
    drcFlatGroundWalkingTrack =
        new DRCFlatGroundWalkingTrack(
            robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false, robotModel);
    drcFlatGroundWalkingTrack.getDrcSimulation();
  }

  private class DoubleSupportStartCondition implements StateTransitionCondition {
    private final EnumYoVariable<WalkingStateEnum> walkingState;

    private final RobotSide side;

    public DoubleSupportStartCondition(
        EnumYoVariable<WalkingStateEnum> walkingState, RobotSide side) {
      this.walkingState = walkingState;
      this.side = side;
    }

    @Override
    public boolean checkCondition() {
      if (side == RobotSide.LEFT) {
        return (walkingState.getEnumValue() == WalkingStateEnum.TO_STANDING)
            || (walkingState.getEnumValue() == WalkingStateEnum.TO_WALKING_LEFT_SUPPORT);
      } else {
        return (walkingState.getEnumValue() == WalkingStateEnum.TO_STANDING)
            || (walkingState.getEnumValue() == WalkingStateEnum.TO_WALKING_RIGHT_SUPPORT);
      }
    }
  }

  private void createVideo(SimulationConstructionSet scs) {
    if (simulationTestingParameters.getCreateSCSVideos()) {
      BambooTools.createVideoAndDataWithDateTimeClassMethodAndShareOnSharedDriveIfAvailable(
          getSimpleRobotName(), scs, 1);
    }
  }

  protected abstract void setForwardPushParameters();

  protected abstract void setBackwardPushParameters();
}
public abstract class DRCWalkToGoalBehaviorTest implements MultiRobotTestInterface {
  private static final SimulationTestingParameters simulationTestingParameters =
      SimulationTestingParameters.createFromEnvironmentVariables();

  private DRCSimulationTestHelper drcSimulationTestHelper;

  @Before
  public void showMemoryUsageBeforeTest() {
    MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
        getClass().getSimpleName() + " before test.");
  }

  @After
  public void destroySimulationAndRecycleMemory() {
    if (simulationTestingParameters.getKeepSCSUp()) {
      ThreadTools.sleepForever();
    }

    // Do this here in case a test fails. That way the memory will be recycled.
    if (drcSimulationTestHelper != null) {
      drcSimulationTestHelper.destroySimulation();
      drcSimulationTestHelper = null;
    }

    if (behaviorCommunicatorClient != null) {
      behaviorCommunicatorClient.close();
    }

    if (behaviorCommunicatorServer != null) {
      behaviorCommunicatorServer.close();
    }

    MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
        getClass().getSimpleName() + " after test.");
  }

  private static final boolean DEBUG = true;

  private final double ASSUMED_WALKING_SPEED_mPerSec = 0.2;

  private final double POSITION_THRESHOLD = 0.1;
  private final double ORIENTATION_THRESHOLD = 0.05;
  private final double EXTRA_SIM_TIME_FOR_SETTLING = 1.0;

  private final DRCDemo01NavigationEnvironment testEnvironment =
      new DRCDemo01NavigationEnvironment();

  private DoubleYoVariable yoTime;

  private ForceSensorDataHolder forceSensorDataHolder;
  private HumanoidRobotDataReceiver robotDataReceiver;

  private BehaviorCommunicationBridge communicationBridge;

  private HumanoidFloatingRootJointRobot robot;
  private FullHumanoidRobotModel fullRobotModel;

  private PacketCommunicator behaviorCommunicatorServer;

  private PacketCommunicator behaviorCommunicatorClient;

  @Before
  public void setUp() {
    MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
        getClass().getSimpleName() + " before test.");

    behaviorCommunicatorServer =
        PacketCommunicator.createIntraprocessPacketCommunicator(
            NetworkPorts.BEHAVIOUR_MODULE_PORT, new IHMCCommunicationKryoNetClassList());
    behaviorCommunicatorClient =
        PacketCommunicator.createIntraprocessPacketCommunicator(
            NetworkPorts.BEHAVIOUR_MODULE_PORT, new IHMCCommunicationKryoNetClassList());
    try {
      behaviorCommunicatorClient.connect();
      behaviorCommunicatorServer.connect();
    } catch (IOException e) {
      throw new RuntimeException(e);
    }
    drcSimulationTestHelper =
        new DRCSimulationTestHelper(
            testEnvironment,
            getSimpleRobotName(),
            DRCObstacleCourseStartingLocation.DEFAULT,
            simulationTestingParameters,
            getRobotModel());

    Robot robotToTest = drcSimulationTestHelper.getRobot();
    yoTime = robotToTest.getYoTime();

    robot = drcSimulationTestHelper.getRobot();
    fullRobotModel = getRobotModel().createFullRobotModel();

    forceSensorDataHolder =
        new ForceSensorDataHolder(Arrays.asList(fullRobotModel.getForceSensorDefinitions()));
    robotDataReceiver = new HumanoidRobotDataReceiver(fullRobotModel, forceSensorDataHolder);

    drcSimulationTestHelper.attachListener(RobotConfigurationData.class, robotDataReceiver);

    PacketRouter<PacketDestination> networkProcessor = new PacketRouter<>(PacketDestination.class);
    networkProcessor.attachPacketCommunicator(
        PacketDestination.CONTROLLER, drcSimulationTestHelper.getControllerCommunicator());
    networkProcessor.attachPacketCommunicator(
        PacketDestination.BEHAVIOR_MODULE, behaviorCommunicatorClient);

    communicationBridge =
        new BehaviorCommunicationBridge(
            behaviorCommunicatorServer, robotToTest.getRobotsYoVariableRegistry());
  }

  @ContinuousIntegrationTest(
      estimatedDuration = 50.0,
      categoriesOverride = IntegrationCategory.EXCLUDE)
  @Test(timeout = 300000)
  public void testWalkForwardsX() throws SimulationExceededMaximumTimeException {
    BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
    boolean success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0);
    assertTrue(success);

    double walkDistance = RandomTools.generateRandomDouble(new Random(), 1.0, 2.0);
    Vector2d walkDirection = new Vector2d(1, 0);
    double trajectoryTime = walkDistance / ASSUMED_WALKING_SPEED_mPerSec;

    WalkToGoalBehavior walkToGoalBehavior =
        testWalkToGoalBehavior(walkDistance, walkDirection, trajectoryTime);

    assertTrue(walkToGoalBehavior.isDone());

    BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
  }

  private WalkToGoalBehavior testWalkToGoalBehavior(
      double walkDistance, Vector2d walkDirection, double trajectoryTime)
      throws SimulationExceededMaximumTimeException {
    FramePose2d initialMidFeetPose =
        getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot);

    FramePose2d desiredMidFeetPose =
        offsetMidFeetPose2d(initialMidFeetPose, walkDistance, walkDirection);

    final WalkToGoalBehavior walkToGoalBehavior =
        new WalkToGoalBehavior(
            communicationBridge,
            fullRobotModel,
            yoTime,
            getRobotModel().getPhysicalProperties().getAnkleHeight());
    communicationBridge.attachGlobalListener(
        walkToGoalBehavior.getControllerGlobalPacketConsumer());
    walkToGoalBehavior.initialize();

    WalkToGoalBehaviorPacket requestedGoal =
        new WalkToGoalBehaviorPacket(
            desiredMidFeetPose.getX(),
            desiredMidFeetPose.getY(),
            desiredMidFeetPose.getYaw(),
            RobotSide.RIGHT);
    walkToGoalBehavior.consumeObjectFromNetworkProcessor(requestedGoal);

    WalkToGoalBehaviorPacket walkToGoalFindPathPacket =
        new WalkToGoalBehaviorPacket(WalkToGoalAction.FIND_PATH);
    walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalFindPathPacket);

    WalkToGoalBehaviorPacket walkToGoalExecutePacket =
        new WalkToGoalBehaviorPacket(WalkToGoalAction.EXECUTE);
    walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalExecutePacket);
    walkToGoalBehavior.doControl();
    assertTrue(walkToGoalBehavior.hasInputBeenSet());

    boolean success = executeBehavior(walkToGoalBehavior, trajectoryTime);
    assertTrue(success);

    FramePose2d finalMidFeetPose =
        getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot);
    //      FramePose2d finalMidFeetPose = getCurrentMidFeetPose2d(referenceFrames);

    PrintTools.debug(this, " initial Midfeet Pose :\n" + initialMidFeetPose + "\n");
    assertPosesAreWithinThresholds(desiredMidFeetPose, finalMidFeetPose);

    return walkToGoalBehavior;
  }

  private FramePose2d offsetMidFeetPose2d(
      FramePose2d initialPose, double walkDistance, Vector2d walkDirection) {
    FramePose2d ret = new FramePose2d(initialPose);

    walkDirection.normalize();
    ret.setX(initialPose.getX() + walkDistance * walkDirection.getX());
    ret.setY(initialPose.getY() + walkDistance * walkDirection.getY());

    return ret;
  }

  private boolean executeBehavior(final AbstractBehavior behavior, double trajectoryTime)
      throws SimulationExceededMaximumTimeException {
    final double simulationRunTime = trajectoryTime + EXTRA_SIM_TIME_FOR_SETTLING;

    if (DEBUG) {
      System.out.println("\n");
      PrintTools.debug(
          this, "starting behavior: " + behavior.getName() + "   t = " + yoTime.getDoubleValue());
    }

    Thread behaviorThread =
        new Thread() {
          public void run() {
            {
              double startTime = Double.NaN;
              boolean simStillRunning = true;
              boolean initalized = false;

              while (simStillRunning) {
                if (!initalized) {
                  startTime = yoTime.getDoubleValue();
                  initalized = true;
                }

                double timeSpentSimulating = yoTime.getDoubleValue() - startTime;
                simStillRunning = timeSpentSimulating < simulationRunTime;

                robotDataReceiver.updateRobotModel();
                behavior.doControl();
              }
            }
          }
        };

    behaviorThread.start();

    boolean ret = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(simulationRunTime);

    PrintTools.debug(
        this, "done with behavior: " + behavior.getName() + "   t = " + yoTime.getDoubleValue());

    return ret;
  }

  private FramePose2d getCurrentMidFeetPose2d(HumanoidReferenceFrames referenceFrames) {
    robotDataReceiver.updateRobotModel();
    referenceFrames.updateFrames();
    ReferenceFrame midFeetFrame = referenceFrames.getMidFeetZUpFrame();

    FramePose midFeetPose = new FramePose();
    midFeetPose.setToZero(midFeetFrame);
    midFeetPose.changeFrame(ReferenceFrame.getWorldFrame());

    FramePose2d ret = new FramePose2d();
    ret.setPoseIncludingFrame(
        midFeetPose.getReferenceFrame(),
        midFeetPose.getX(),
        midFeetPose.getY(),
        midFeetPose.getYaw());

    return ret;
  }

  private FramePose2d getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(
      HumanoidFloatingRootJointRobot robot) {
    FramePose midFeetPose = getRobotMidFeetPose(robot);

    FramePose2d ret = new FramePose2d();
    ret.setPoseIncludingFrame(
        ReferenceFrame.getWorldFrame(),
        midFeetPose.getX(),
        midFeetPose.getY(),
        midFeetPose.getYaw());

    return ret;
  }

  private FramePose getRobotMidFeetPose(HumanoidFloatingRootJointRobot robot) {
    FramePose leftFootPose = getRobotFootPose(robot, RobotSide.LEFT);
    FramePose rightFootPose = getRobotFootPose(robot, RobotSide.RIGHT);

    FramePose ret = new FramePose();
    ret.interpolate(leftFootPose, rightFootPose, 0.5);

    return ret;
  }

  private FramePose getRobotFootPose(HumanoidFloatingRootJointRobot robot, RobotSide robotSide) {
    List<GroundContactPoint> gcPoints = robot.getFootGroundContactPoints(robotSide);
    Joint ankleJoint = gcPoints.get(0).getParentJoint();
    RigidBodyTransform ankleTransformToWorld = new RigidBodyTransform();
    ankleJoint.getTransformToWorld(ankleTransformToWorld);

    FramePose ret = new FramePose();
    ret.setPose(ankleTransformToWorld);

    return ret;
  }

  private void assertPosesAreWithinThresholds(FramePose2d framePose1, FramePose2d framePose2) {
    double positionDistance = framePose1.getPositionDistance(framePose2);
    double orientationDistance = framePose1.getOrientationDistance(framePose2);

    if (DEBUG) {
      PrintTools.debug(this, " desired Midfeet Pose :\n" + framePose1 + "\n");
      PrintTools.debug(this, " actual Midfeet Pose :\n" + framePose2 + "\n");

      PrintTools.debug(this, " positionDistance = " + positionDistance);
      PrintTools.debug(this, " orientationDistance = " + orientationDistance);
    }

    assertEquals(0.0, positionDistance, POSITION_THRESHOLD);
    assertEquals(0.0, orientationDistance, ORIENTATION_THRESHOLD);
  }
}