diff --git a/src/helio/helio_step.f90 b/src/helio/helio_step.f90
index 9882be084..6c8da9d54 100644
--- a/src/helio/helio_step.f90
+++ b/src/helio/helio_step.f90
@@ -15,21 +15,12 @@ module subroutine helio_step_system(self, param, t, dt)
       real(DP),                   intent(in)    :: t      !! Simulation time
       real(DP),                   intent(in)    :: dt     !! Current stepsize
 
-      select type(system => self)
-      class is (helio_nbody_system)
-         select type(cb => self%cb)
-         class is (helio_cb)
-            select type(pl => self%pl)
-            class is (helio_pl)
-               select type(tp => self%tp)
-               class is (helio_tp)
-                  call pl%set_rhill(cb)
-                  call pl%step(system, param, t, dt)
-                  call tp%step(system, param, t, dt)
-               end select
-            end select
-         end select
-      end select
+      associate(system => self, cb => self%cb, pl => self%pl, tp => self%tp)
+         call pl%set_rhill(cb)
+         tp%lfirst = pl%lfirst
+         call pl%step(system, param, t, dt)
+         call tp%step(system, param, t, dt)
+      end associate
       return
    end subroutine helio_step_system 
 
@@ -50,16 +41,14 @@ module subroutine helio_step_pl(self, system, param, t, dt)
       ! Internals 
       integer(I4B)     :: i
       real(DP)         :: dth, msys
-      !real(DP), dimension(NDIM) :: ptbeg, ptend !! TODO: Incorporate these into the tp structure
-      logical, save :: lfirst = .true.
  
       select type(system)
       class is (helio_nbody_system)
          associate(pl => self, cb => system%cb, ptb => system%ptb, pte => system%pte)
             dth = 0.5_DP * dt
-            if (lfirst) then
+            if (pl%lfirst) then
                call pl%vh2vb(cb)
-               lfirst = .false.
+               pl%lfirst = .false.
             end if
             call pl%lindrift(system, dth, ptb)
             call pl%get_accel(system, param, t)
@@ -94,16 +83,15 @@ module subroutine helio_step_tp(self, system, param, t, dt)
       real(DP),                     intent(in)    :: t      !! Current simulation time
       real(DP),                     intent(in)    :: dt     !! Stepsiz
       ! Internals
-      logical, save  :: lfirst = .true. !! Flag to indicate that this is the first call
       real(DP) :: dth   !! Half step size
    
       select type(system)
       class is (helio_nbody_system)
          associate(tp => self, cb => system%cb, pl => system%pl, xbeg => system%xbeg, xend => system%xend, ptb => system%ptb, pte => system%pte)
             dth = 0.5_DP * dt
-            if (lfirst) then
+            if (tp%lfirst) then
                call tp%vh2vb(vbcb = -ptb)
-               lfirst = .false.
+               tp%lfirst = .false.
             end if
             call tp%lindrift(system, dth, ptb)
             call tp%get_accel(system, param, t, xbeg)
diff --git a/src/main/swiftest_driver.f90 b/src/main/swiftest_driver.f90
index bce2fd38c..4ed7cf3fe 100644
--- a/src/main/swiftest_driver.f90
+++ b/src/main/swiftest_driver.f90
@@ -14,7 +14,6 @@ program swiftest_driver
    integer(I4B)                               :: integrator       !! Integrator type code (see swiftest_globals for symbolic names)
    character(len=:),allocatable               :: param_file_name !! Name of the file containing user-defined parameters
    integer(I4B)                               :: ierr             !! I/O error code 
-   logical                                    :: lfirst           !! Flag indicating that this is the first time through the main loop
    integer(I8B)                               :: iloop            !! Loop counter
    integer(I8B)                               :: idump            !! Dump cadence counter
    integer(I8B)                               :: iout             !! Output cadence counter
@@ -42,7 +41,6 @@ program swiftest_driver
              istep_out  => param%istep_out, &
              istep_dump => param%istep_dump)  
       call nbody_system%initialize(param)
-      lfirst = .true.
       t = t0
       iloop = 0
       iout = istep_out
diff --git a/src/setup/setup.f90 b/src/setup/setup.f90
index e1603b966..a50f57254 100644
--- a/src/setup/setup.f90
+++ b/src/setup/setup.f90
@@ -69,6 +69,7 @@ module subroutine setup_body(self,n)
 
       self%nbody = n
       if (n <= 0) return
+      self%lfirst = .true.
 
       !write(*,*) 'Allocating the basic Swiftest particle'
       allocate(self%name(n))