diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc
index f34a21cafc..2840b3f0ea 100644
--- a/src/SimulationRunner.cc
+++ b/src/SimulationRunner.cc
@@ -456,10 +456,8 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system,
       std::optional<Entity> _entity,
       std::optional<std::shared_ptr<const sdf::Element>> _sdf)
 {
-  auto entity = _entity.has_value() ? _entity.value()
-      : worldEntity(this->entityCompMgr);
-  auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element();
-
+  auto entity = _entity.value_or(worldEntity(this->entityCompMgr));
+  auto sdf = _sdf.value_or(this->sdfWorld->Element());
   this->systemMgr->AddSystem(_system, entity, sdf);
 }
 
@@ -469,10 +467,8 @@ void SimulationRunner::AddSystem(
       std::optional<Entity> _entity,
       std::optional<std::shared_ptr<const sdf::Element>> _sdf)
 {
-  auto entity = _entity.has_value() ? _entity.value()
-      : worldEntity(this->entityCompMgr);
-  auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element();
-
+  auto entity = _entity.value_or(worldEntity(this->entityCompMgr));
+  auto sdf = _sdf.value_or(this->sdfWorld->Element());
   this->systemMgr->AddSystem(_system, entity, sdf);
 }
 
diff --git a/src/SystemManager.cc b/src/SystemManager.cc
index c159a2515d..ba716f99a9 100644
--- a/src/SystemManager.cc
+++ b/src/SystemManager.cc
@@ -54,28 +54,26 @@ void SystemManager::LoadPlugin(const Entity _entity,
 //////////////////////////////////////////////////
 size_t SystemManager::TotalCount() const
 {
-  std::lock_guard<std::mutex> lock(this->systemsMutex);
-  return this->systems.size() + this->pendingSystems.size();
+  return this->ActiveCount() + this->PendingCount();
 }
 
 //////////////////////////////////////////////////
 size_t SystemManager::ActiveCount() const
 {
-  std::lock_guard<std::mutex> lock(this->systemsMutex);
   return this->systems.size();
 }
 
 //////////////////////////////////////////////////
 size_t SystemManager::PendingCount() const
 {
-  std::lock_guard<std::mutex> lock(this->systemsMutex);
+  std::lock_guard<std::mutex> lock(this->pendingSystemsMutex);
   return this->pendingSystems.size();
 }
 
 //////////////////////////////////////////////////
 size_t SystemManager::ActivatePendingSystems()
 {
-  std::lock_guard<std::mutex> lock(this->systemsMutex);
+  std::lock_guard<std::mutex> lock(this->pendingSystemsMutex);
 
   auto count = this->pendingSystems.size();
 
@@ -132,7 +130,7 @@ void SystemManager::AddSystemImpl(
   }
 
   // Update callbacks will be handled later, add to queue
-  std::lock_guard<std::mutex> lock(this->systemsMutex);
+  std::lock_guard<std::mutex> lock(this->pendingSystemsMutex);
   this->pendingSystems.push_back(_system);
 }
 
diff --git a/src/SystemManager.hh b/src/SystemManager.hh
index 8b882e50b6..a1813b1f14 100644
--- a/src/SystemManager.hh
+++ b/src/SystemManager.hh
@@ -120,11 +120,8 @@ namespace ignition
       /// \brief Pending systems to be added to systems.
       private: std::vector<SystemInternal> pendingSystems;
 
-      /// \brief Mark if a pending system has been configured
-      private: std::vector<bool> pendingSystemsConfigured;
-
       /// \brief Mutex to protect pendingSystems
-      private: mutable std::mutex systemsMutex;
+      private: mutable std::mutex pendingSystemsMutex;
 
       /// \brief Systems implementing Configure
       private: std::vector<ISystemConfigure *> systemsConfigure;
diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc
index 0aeb55f331..4fc7288580 100644
--- a/src/SystemManager_TEST.cc
+++ b/src/SystemManager_TEST.cc
@@ -28,7 +28,7 @@
 using namespace ignition::gazebo;
 
 /////////////////////////////////////////////////
-class System_WithConfigure:
+class SystemWithConfigure:
   public System,
   public ISystemConfigure
 {
@@ -43,7 +43,7 @@ class System_WithConfigure:
 };
 
 /////////////////////////////////////////////////
-class System_WithUpdates:
+class SystemWithUpdates:
   public System,
   public ISystemPreUpdate,
   public ISystemUpdate,
@@ -79,7 +79,7 @@ TEST(SystemManager, Constructor)
 }
 
 /////////////////////////////////////////////////
-TEST(SystemManager, AddSystem_NoEcm)
+TEST(SystemManager, AddSystemNoEcm)
 {
   auto loader = std::make_shared<SystemLoader>();
   SystemManager systemMgr(loader);
@@ -92,7 +92,7 @@ TEST(SystemManager, AddSystem_NoEcm)
   EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
   EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
 
-  auto configSystem = std::make_shared<System_WithConfigure>();
+  auto configSystem = std::make_shared<SystemWithConfigure>();
   systemMgr.AddSystem(configSystem, kNullEntity, nullptr);
 
   // SystemManager without an ECM/EventmManager will mean no config occurs
@@ -115,7 +115,7 @@ TEST(SystemManager, AddSystem_NoEcm)
   EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
   EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
 
-  auto updateSystem = std::make_shared<System_WithUpdates>();
+  auto updateSystem = std::make_shared<SystemWithUpdates>();
   systemMgr.AddSystem(updateSystem, kNullEntity, nullptr);
   EXPECT_EQ(1u, systemMgr.ActiveCount());
   EXPECT_EQ(1u, systemMgr.PendingCount());
@@ -136,7 +136,7 @@ TEST(SystemManager, AddSystem_NoEcm)
 }
 
 /////////////////////////////////////////////////
-TEST(SystemManager, AddSystem_Ecm)
+TEST(SystemManager, AddSystemEcm)
 {
   auto loader = std::make_shared<SystemLoader>();
 
@@ -153,7 +153,7 @@ TEST(SystemManager, AddSystem_Ecm)
   EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
   EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
 
-  auto configSystem = std::make_shared<System_WithConfigure>();
+  auto configSystem = std::make_shared<SystemWithConfigure>();
   systemMgr.AddSystem(configSystem, kNullEntity, nullptr);
 
   // Configure called during AddSystem
@@ -176,7 +176,7 @@ TEST(SystemManager, AddSystem_Ecm)
   EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
   EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
 
-  auto updateSystem = std::make_shared<System_WithUpdates>();
+  auto updateSystem = std::make_shared<SystemWithUpdates>();
   systemMgr.AddSystem(updateSystem, kNullEntity, nullptr);
   EXPECT_EQ(1u, systemMgr.ActiveCount());
   EXPECT_EQ(1u, systemMgr.PendingCount());
diff --git a/src/WorldControl.hh b/src/WorldControl.hh
index bb0005e093..9016e8c8f4 100644
--- a/src/WorldControl.hh
+++ b/src/WorldControl.hh
@@ -17,6 +17,11 @@
 #ifndef IGNITION_GAZEBO_WORLDCONTROL_HH_
 #define IGNITION_GAZEBO_WORLDCONTROL_HH_
 
+#include <chrono>
+#include <cstdint>
+
+#include "ignition/gazebo/config.hh"
+
 namespace ignition
 {
   namespace gazebo