From b78c88716b95b4ce8c1c79236a38def3a9d48d7f Mon Sep 17 00:00:00 2001
From: David A Minton <daminton@purdue.edu>
Date: Mon, 12 Jul 2021 11:26:28 -0400
Subject: [PATCH] Restructured GR routines to prepare for testing in
 Helio/SyMBA. Built additional templates for SyMBA.

---
 src/gr/gr.f90                       | 172 ++++++++++++++++++++---
 src/modules/swiftest.f90            |   3 -
 src/modules/swiftest_classes.f90    |  36 +++++
 src/modules/symba_classes.f90       |   1 -
 src/rmvs/rmvs_setup.f90             |   1 -
 src/setup/setup.f90                 |  17 ++-
 src/symba/symba_discard.f90         |  25 ++++
 src/symba/symba_encounter_check.f90 |  30 ++++
 src/symba/symba_io.f90              |   7 +
 src/symba/symba_setup.f90           |  14 +-
 src/symba/symba_step.f90            |  10 ++
 src/util/util_spill_and_fill.f90    |  70 +++++-----
 src/whm/whm_gr.f90                  | 205 ++++++----------------------
 src/whm/whm_setup.f90               |  15 +-
 14 files changed, 366 insertions(+), 240 deletions(-)
 create mode 100644 src/symba/symba_discard.f90
 create mode 100644 src/symba/symba_encounter_check.f90

diff --git a/src/gr/gr.f90 b/src/gr/gr.f90
index a778e3db2..2cb200efa 100644
--- a/src/gr/gr.f90
+++ b/src/gr/gr.f90
@@ -1,43 +1,39 @@
 submodule(swiftest_classes) s_gr
    use swiftest
 contains
-   subroutine gr_getaccb_ns_body(self, cb, param, agr, agr0) 
-
+   module pure subroutine gr_getaccb_ns_body(self, system, param) 
       !! author: David A. Minton
       !!
-      !! Add relativistic correction acceleration for non-symplectic integrators
-      !!    Based on Quinn, Tremaine, & Duncan 1998
+      !! Add relativistic correction acceleration for non-symplectic integrators.
+      !! Based on Quinn et al. (1991) eq. 5
+      !!
+      !! Reference:  
+      !!    Quinn, T.R., Tremaine, S., Duncan, M., 1991. A three million year integration of the earth’s orbit. 
+      !!       AJ 101, 2287–2305. https://doi.org/10.1086/115850
       !!
       !! Adapted from David A. Minton's Swifter routine routine gr_getaccb_ns.f90
       implicit none
       ! Arguments
-      class(swiftest_body),       intent(inout) :: self
-      class(swiftest_cb),         intent(inout) :: cb
-      class(swiftest_parameters), intent(in)    :: param
-      real(DP), dimension(:, :),  intent(inout) :: agr
-      real(DP), dimension(NDIM),  intent(out)   :: agr0
+      class(swiftest_body),         intent(inout) :: self   !! Swiftest generic body object
+      class(swiftest_nbody_system), intent(inout) :: system !! Swiftest nbody system object
+      class(swiftest_parameters),   intent(in)    :: param  !! Current run configuration parameters 
       ! Internals
-      real(DP), dimension(NDIM) :: xh, vh
       real(DP)                  :: rmag, rdotv, vmag2
       integer(I4B)              :: i
 
-      associate(n => self%nbody, msun => cb%Gmass, vbsun => cb%vb, xbsun => cb%xb, mu => self%mu, c2 => param%inv_c2, &
-                xb => self%xb, vb => self%vb)
+      associate(n => self%nbody, cb => system%cb,  inv_c2 => param%inv_c2)
          if (n == 0) return
          do i = 1, n
-            xh(:) = xb(:, i) - xbsun(:)
-            vh(:) = vb(:, i) - vbsun(:)
-            rmag = norm2(xh(:))
-            vmag2 = dot_product(vh(:), vh(:))
-            rdotv = dot_product(xh(:), vh(:))
-            agr(:, i) =  mu * c2 / rmag**3 * ((4 * mu(i) / rmag - vmag2) * xh(:) + 4 * rdotv * vh(:))
+            rmag = norm2(self%xh(:,i))
+            vmag2 = dot_product(self%vh(:,i), self%vh(:,i))
+            rdotv = dot_product(self%xh(:,i), self%vh(:,i))
+            self%agr(:, i) = self%mu * inv_c2 / rmag**3 * ((4 * self%mu(i) / rmag - vmag2) * self%xh(:,i) + 4 * rdotv * self%vh(:,i))
          end do
 
-         agr0 =  0.0_DP
          select type(self)
          class is (swiftest_pl)
             do i = 1, NDIM
-               agr0(i) = -sum(self%Gmass(1:n) * agr(1:n, i) / msun)
+               cb%agr(i) = -sum(self%Gmass(1:n) * self%agr(1:n, i) / cb%Gmass)
             end do
          end select
 
@@ -45,6 +41,140 @@ subroutine gr_getaccb_ns_body(self, cb, param, agr, agr0)
 
       return
 
-      end subroutine gr_getaccb_ns_body
+   end subroutine gr_getaccb_ns_body
+
+   module pure subroutine gr_p4_pos_kick(param, x, v, dt)
+      !! author: David A. Minton
+      !!
+      !! Position kick due to p**4 term in the post-Newtonian correction
+      !!    Based on Saha & Tremaine (1994) Eq. 28
+      !!
+      !! Reference:
+      !!    Saha, P., Tremaine, S., 1994. Long-term planetary integration with individual time steps. 
+      !!       AJ 108, 1962–1969. https://doi.org/10.1086/117210
+      !!
+      !! Adapted from David A. Minton's Swifter routine gr_whm_p4.f90  
+      implicit none
+      ! Arguments
+      class(swiftest_parameters), intent(in)    :: param !! Current run configuration parameters 
+      real(DP), dimension(:),     intent(inout) :: x     !! Position vector
+      real(DP), dimension(:),     intent(in)    :: v     !! Velocity vector
+      real(DP),                   intent(in)    :: dt    !! Step size
+      ! Internals
+      real(DP), dimension(NDIM)             :: dr
+      real(DP)                              :: vmag2
+
+      vmag2 = dot_product(v(:), v(:)) 
+      dr(:) = -2 * param%inv_c2 * vmag2 * v(:)
+      x(:) = x(:) + dr(:) * dt
+
+      return
+   end subroutine gr_p4_pos_kick
+
+   module pure subroutine gr_pseudovel2vel(param, mu, xh, pv, vh) 
+      !! author: David A. Minton
+      !!
+      !! Converts the relativistic pseudovelocity back into a veliocentric velocity
+      !!    Based on Saha & Tremaine (1994) Eq. 32
+      !! 
+      !! Reference:
+      !!    Saha, P., Tremaine, S., 1994. Long-term planetary integration with individual time steps. 
+      !!       AJ 108, 1962–1969. https://doi.org/10.1086/117210
+      !!
+      !! Adapted from David A. Minton's Swifter routine gr_pseudovel2vel.f90 
+      implicit none
+      ! Arguments
+      class(swiftest_parameters), intent(in)  :: param !! Current run configuration parameters 
+      real(DP),                   intent(in)  :: mu    !! G * (Mcb + m), G = gravitational constant, Mcb = mass of central body, m = mass of body
+      real(DP), dimension(:),     intent(in)  :: xh    !! Heliocentric position vector 
+      real(DP), dimension(:),     intent(in)  :: pv    !! Pseudovelocity velocity vector - see Saha & Tremain (1994), eq. (32)
+      real(DP), dimension(:),     intent(out) :: vh    !! Heliocentric velocity vector 
+      ! Internals
+      real(DP) :: vmag2, rmag, grterm
+   
+      associate(c2 => param%inv_c2)
+         vmag2 = dot_product(pv(:), pv(:))
+         rmag  = norm2(xh(:))
+         grterm = 1.0_DP - c2 * (0.5_DP * vmag2 + 3 * mu / rmag)
+         vh(:) = pv(:) * grterm
+      end associate
+      return
+   end subroutine gr_pseudovel2vel
+
+   module pure subroutine gr_vel2pseudovel(param, mu, xh, vh, pv)
+      !! author: David A. Minton
+      !!
+      !! Converts the heliocentric velocity into a pseudovelocity with relativistic corrections. 
+      !! Uses Newton-Raphson method with direct inversion of the Jacobian (yeah, it's slow, but 
+      !! this is only done once per run).
+      !!
+      !! Reference:
+      !!    Saha, P., Tremaine, S., 1994. Long-term planetary integration with individual time steps. 
+      !!       AJ 108, 1962–1969. https://doi.org/10.1086/117210
+      !!
+      !! Adapted from David A. Minton's Swifter routine gr_vel2pseudovel.f90
+      implicit none
+      ! Arguments
+      class(swiftest_parameters), intent(in)  :: param !! Current run configuration parameters 
+      real(DP),                   intent(in)  :: mu    !! G * (Mcb + m), G = gravitational constant, Mcb = mass of central body, m = mass of body
+      real(DP), dimension(:),     intent(in)  :: xh    !! Heliocentric position vector 
+      real(DP), dimension(:),     intent(in)  :: vh    !! Heliocentric velocity vector 
+      real(DP), dimension(:),     intent(out) :: pv    !! Pseudovelocity vector - see Saha & Tremain (1994), eq. (32)
+      ! Internals
+      real(DP)                       :: v2, G, pv2, rterm, det
+      real(DP), dimension(NDIM,NDIM) :: J,Jinv
+      real(DP), dimension(NDIM)      :: F
+      integer(I4B)                   :: n,i,k
+      integer(I4B), parameter        :: MAXITER = 50
+      real(DP),parameter             :: TOL = 1.0e-12_DP
+
+      associate (c2 => param%inv_c2)
+         pv(1:NDIM) = vh(1:NDIM) ! Initial guess
+         rterm = 3 * mu / norm2(xh(:))
+         v2 = dot_product(vh(:), vh(:))
+         do n = 1, MAXITER
+            pv2 = dot_product(pv(:), pv(:))
+            G = 1.0_DP - c2 * (0.5_DP * pv2 + rterm)
+            F(:) = pv(:) * G - vh(:)
+            if (abs(sum(F) / v2 ) < TOL) exit ! Root found
+   
+            ! Calculate the Jacobian
+            do k = 1, NDIM
+               do i = 1, NDIM
+                  if (i == k) then
+                     J(i,k) = G - c2 * pv(k)
+                  else
+                     J(i,k) = - c2 * pv(k)
+                  end if
+               end do
+            end do
+   
+            ! Inverse of the Jacobian
+            det = J(1,1) * (J(3,3) * J(2,2) - J(3,2) * J(2,3))
+            det = det - J(2,1) * (J(3,3) * J(1,2)-J(3,2) * J(1,3))
+            det = det + J(3,1) * (J(2,3) * J(1,2)-J(2,2) * J(1,3))
+   
+            Jinv(1,1) =   J(3,3) * J(2,2) - J(3,2) * J(2,3)
+            Jinv(1,2) = -(J(3,3) * J(1,2) - J(3,2) * J(1,3))
+            Jinv(1,3) =   J(2,3) * J(1,2) - J(2,2) * J(1,3)
+   
+            Jinv(2,1) = -(J(3,3) * J(2,1) - J(3,1) * J(2,3))
+            Jinv(2,2) =   J(3,3) * J(1,1) - J(3,1) * J(1,3)
+            Jinv(2,3) = -(J(2,3) * J(1,1) - J(2,1) * J(1,3))
+   
+            Jinv(3,1) =   J(3,2) * J(2,1) - J(3,1) * J(2,2)
+            Jinv(3,2) = -(J(3,2) * J(1,1) - J(3,1) * J(1,2))
+            Jinv(3,3) =   J(2,2) * J(1,1) - J(2,1) * J(1,2)
+   
+            Jinv = Jinv * det
+   
+            do i = 1, NDIM
+               pv(i) = pv(i) - dot_product(Jinv(i,:) ,F(:))
+            end do
+         end do 
+   
+      end associate
+      return
+   end subroutine gr_vel2pseudovel
 
 end submodule s_gr
\ No newline at end of file
diff --git a/src/modules/swiftest.f90 b/src/modules/swiftest.f90
index 2b8749e2f..dcc8dc875 100644
--- a/src/modules/swiftest.f90
+++ b/src/modules/swiftest.f90
@@ -14,9 +14,6 @@ module swiftest
    use module_nrutil
    !use advisor_annotate
    !$ use omp_lib
-
-
-
    implicit none
    public
 
diff --git a/src/modules/swiftest_classes.f90 b/src/modules/swiftest_classes.f90
index 229e283d8..0e8a0a3f5 100644
--- a/src/modules/swiftest_classes.f90
+++ b/src/modules/swiftest_classes.f90
@@ -9,6 +9,7 @@ module swiftest_classes
    public :: discard_pl, discard_system, discard_tp 
    public :: drift_one
    public :: eucl_dist_index_plpl, eucl_dist_index_pltp, eucl_irij3_plpl
+   public :: gr_getaccb_ns_body, gr_p4_pos_kick, gr_pseudovel2vel, gr_vel2pseudovel
    public :: io_dump_param, io_dump_swiftest, io_dump_system, io_get_args, io_get_token, io_param_reader, io_param_writer, io_read_body_in, &
              io_read_cb_in, io_read_param_in, io_read_frame_body, io_read_frame_cb, io_read_frame_system, io_read_initialize_system, &
              io_write_discard, io_write_encounter, io_write_frame_body, io_write_frame_cb, io_write_frame_system
@@ -111,6 +112,7 @@ module swiftest_classes
       real(DP), dimension(NDIM) :: aoblend = 0.0_DP !! Barycentric acceleration due to central body oblatenes at end of step
       real(DP), dimension(NDIM) :: xb      = 0.0_DP !! Barycentric position (units DU)
       real(DP), dimension(NDIM) :: vb      = 0.0_DP !! Barycentric velocity (units DU / TU)
+      real(DP), dimension(NDIM) :: agr     = 0.0_DP !! Acceleration due to post-Newtonian correction
    contains
       private
       procedure, public         :: initialize  => io_read_cb_in        !! I/O routine for reading in central body data
@@ -137,6 +139,7 @@ module swiftest_classes
       real(DP),              dimension(:,:), allocatable :: vb              !! Barycentric velocity
       real(DP),              dimension(:,:), allocatable :: ah              !! Total heliocentric acceleration
       real(DP),              dimension(:,:), allocatable :: aobl            !! Barycentric accelerations of bodies due to central body oblatenes
+      real(DP),              dimension(:,:), allocatable :: agr             !! Acceleration due to post-Newtonian correction
       real(DP),              dimension(:),   allocatable :: ir3h            !! Inverse heliocentric radius term (1/rh**3)
       real(DP),              dimension(:),   allocatable :: a               !! Semimajor axis (pericentric distance for a parabolic orbit)
       real(DP),              dimension(:),   allocatable :: e               !! Eccentricity
@@ -380,6 +383,39 @@ module subroutine eucl_irij3_plpl(self)
          class(swiftest_pl), intent(inout) :: self !! Swiftest massive body object
       end subroutine eucl_irij3_plpl
 
+      module pure subroutine gr_getaccb_ns_body(self, system, param)
+         implicit none
+         class(swiftest_body),         intent(inout) :: self   !! Swiftest generic body object
+         class(swiftest_nbody_system), intent(inout) :: system !! Swiftest nbody system object
+         class(swiftest_parameters),   intent(in)    :: param  !! Current run configuration parameters 
+      end subroutine gr_getaccb_ns_body
+
+      module pure subroutine gr_p4_pos_kick(param, x, v, dt)
+         implicit none
+         class(swiftest_parameters), intent(in)    :: param !! Current run configuration parameters 
+         real(DP), dimension(:),     intent(inout) :: x     !! Position vector
+         real(DP), dimension(:),     intent(in)    :: v     !! Velocity vector
+         real(DP),                   intent(in)    :: dt    !! Step size
+      end subroutine gr_p4_pos_kick
+
+      module pure subroutine gr_pseudovel2vel(param, mu, xh, pv, vh) 
+         implicit none
+         class(swiftest_parameters), intent(in)  :: param !! Current run configuration parameters 
+         real(DP),                   intent(in)  :: mu    !! G * (Mcb + m), G = gravitational constant, Mcb = mass of central body, m = mass of body
+         real(DP), dimension(:),     intent(in)  :: xh    !! Heliocentric position vector 
+         real(DP), dimension(:),     intent(in)  :: pv    !! Pseudovelocity velocity vector - see Saha & Tremain (1994), eq. (32)
+         real(DP), dimension(:),     intent(out) :: vh    !! Heliocentric velocity vector 
+      end subroutine gr_pseudovel2vel
+
+      module pure subroutine gr_vel2pseudovel(param, mu, xh, vh, pv)
+         implicit none
+         class(swiftest_parameters), intent(in)  :: param !! Current run configuration parameters 
+         real(DP),                   intent(in)  :: mu    !! G * (Mcb + m), G = gravitational constant, Mcb = mass of central body, m = mass of body
+         real(DP), dimension(:),     intent(in)  :: xh    !! Heliocentric position vector 
+         real(DP), dimension(:),     intent(in)  :: vh    !! Heliocentric velocity vector 
+         real(DP), dimension(:),     intent(out) :: pv    !! Pseudovelocity vector - see Saha & Tremain (1994), eq. (32)
+      end subroutine gr_vel2pseudovel
+
       module subroutine io_dump_param(self, param_file_name)
          implicit none
          class(swiftest_parameters),intent(in)    :: self            !! Output collection of parameters
diff --git a/src/modules/symba_classes.f90 b/src/modules/symba_classes.f90
index 0faea9561..26ca7df6e 100644
--- a/src/modules/symba_classes.f90
+++ b/src/modules/symba_classes.f90
@@ -271,7 +271,6 @@ module subroutine symba_io_read_frame_pl(self, iu, param, form, ierr)
          integer(I4B),               intent(out)   :: ierr    !! Error code
       end subroutine symba_io_read_frame_pl
 
-
       module subroutine symba_io_read_pl_in(self, param) 
          use swiftest_classes, only : swiftest_parameters
          implicit none
diff --git a/src/rmvs/rmvs_setup.f90 b/src/rmvs/rmvs_setup.f90
index f26d6e42d..83dd51106 100644
--- a/src/rmvs/rmvs_setup.f90
+++ b/src/rmvs/rmvs_setup.f90
@@ -24,7 +24,6 @@ module subroutine rmvs_setup_pl(self,n)
          if (.not.pl%lplanetocentric) then
             allocate(pl%nenc(n))
             pl%nenc(:) = 0
-
             ! Set up inner and outer planet interpolation vector storage containers
             do i = 0, NTENC
                allocate(pl%outer(i)%x(NDIM, n))
diff --git a/src/setup/setup.f90 b/src/setup/setup.f90
index a64f64a15..402ef62a4 100644
--- a/src/setup/setup.f90
+++ b/src/setup/setup.f90
@@ -46,7 +46,19 @@ module subroutine setup_construct_system(system, param)
             allocate(rmvs_tp :: system%tp_discards)
          end select
       case (SYMBA)
-         write(*,*) 'SyMBA integrator not yet enabled'
+         allocate(symba_nbody_system :: system)
+         select type(system)
+         class is (symba_nbody_system)
+            allocate(symba_cb :: system%cb)
+            allocate(symba_pl :: system%pl)
+            allocate(symba_tp :: system%tp)
+            allocate(symba_pl :: system%pl_discards)
+            allocate(symba_tp :: system%tp_discards)
+            allocate(symba_pl :: system%mergeadd_list)
+            allocate(symba_pl :: system%mergesub_list)
+            allocate(symba_plplenc :: system%plplenc_list)
+            allocate(symba_pltpenc :: system%pltpenc_list)
+         end select
       case (RINGMOONS)
          write(*,*) 'RINGMOONS-SyMBA integrator not yet enabled'
       case default
@@ -73,6 +85,7 @@ module subroutine setup_body(self,n)
 
       !write(*,*) 'Allocating the basic Swiftest particle'
       allocate(self%id(n))
+      allocate(self%name(n))
       allocate(self%status(n))
       allocate(self%ldiscard(n))
       allocate(self%xh(NDIM, n))
@@ -81,6 +94,7 @@ module subroutine setup_body(self,n)
       allocate(self%vb(NDIM, n))
       allocate(self%ah(NDIM, n))
       allocate(self%aobl(NDIM, n))
+      allocate(self%agr(NDIM, n))
       allocate(self%ir3h(n))
       allocate(self%a(n))
       allocate(self%e(n))
@@ -91,6 +105,7 @@ module subroutine setup_body(self,n)
       allocate(self%mu(n))
 
       self%id(:)   = 0
+      self%name(:) = "UNNAMED"
       self%status(:) = INACTIVE
       self%ldiscard(:) = .false.
       self%xh(:,:)   = 0.0_DP
diff --git a/src/symba/symba_discard.f90 b/src/symba/symba_discard.f90
new file mode 100644
index 000000000..8bafdb2b5
--- /dev/null
+++ b/src/symba/symba_discard.f90
@@ -0,0 +1,25 @@
+submodule (symba_classes) s_symba_discard
+   use swiftest
+contains
+
+   module subroutine symba_discard_pl(self, system, param)
+      implicit none
+      ! Arguments
+      class(symba_pl),              intent(inout) :: self   !! SyMBA test particle object
+      class(swiftest_nbody_system), intent(inout) :: system !! Swiftest nbody system object
+      class(swiftest_parameters),   intent(in)    :: param  !! Current run configuration parameters 
+      
+      return
+   end subroutine symba_discard_pl
+
+   module subroutine symba_discard_tp(self, system, param)
+      implicit none
+      ! Arguments
+      class(symba_tp),              intent(inout) :: self   !! SyMBA test particle object
+      class(swiftest_nbody_system), intent(inout) :: system !! Swiftest nbody system object
+      class(swiftest_parameters),   intent(in)    :: param  !! Current run configuration parameters 
+
+      return
+   end subroutine symba_discard_tp
+
+end submodule s_symba_discard
\ No newline at end of file
diff --git a/src/symba/symba_encounter_check.f90 b/src/symba/symba_encounter_check.f90
new file mode 100644
index 000000000..ce4b53dff
--- /dev/null
+++ b/src/symba/symba_encounter_check.f90
@@ -0,0 +1,30 @@
+submodule (symba_classes) s_symba_encounter_check
+   use swiftest
+contains
+   module function symba_encounter_check_pl(self, system, dt) result(lencounter)
+      implicit none
+      ! Arguments
+      class(symba_pl),           intent(inout) :: self       !! SyMBA test particle object  
+      class(symba_nbody_system), intent(inout) :: system     !! SyMBA nbody system object
+      real(DP),                  intent(in)    :: dt         !! step size
+      ! Result
+      logical                                  :: lencounter !! Returns true if there is at least one close encounter      
+
+      lencounter = .false.
+      return
+   end function symba_encounter_check_pl
+
+   module function symba_encounter_check_tp(self, system, dt) result(lencounter)
+      implicit none
+      ! Arguments
+      class(symba_tp),           intent(inout) :: self       !! SyMBA test particle object  
+      class(symba_nbody_system), intent(inout) :: system     !! SyMBA nbody system object
+      real(DP),                  intent(in)    :: dt         !! step size
+      ! Result
+      logical                                  :: lencounter !! Returns true if there is at least one close encounter      
+
+      lencounter = .false.
+      return
+   end function symba_encounter_check_tp
+
+end submodule s_symba_encounter_check
\ No newline at end of file
diff --git a/src/symba/symba_io.f90 b/src/symba/symba_io.f90
index 6657582e0..8fd9bc6bc 100644
--- a/src/symba/symba_io.f90
+++ b/src/symba/symba_io.f90
@@ -439,6 +439,13 @@ module subroutine symba_io_write_frame_cb(self, iu, param)
       end select
    end subroutine symba_io_write_frame_cb
 
+   module subroutine symba_io_write_frame_info(self, iu, param)
+      implicit none
+      class(symba_particle_info), intent(in)    :: self  !! SyMBA particle info object
+      integer(I4B),               intent(inout) :: iu      !! Unit number for the output file to write frame to
+      class(swiftest_parameters), intent(in)    :: param   !! Current run configuration parameters 
+   end subroutine symba_io_write_frame_info 
+
    module subroutine symba_io_write_frame_pl(self, iu, param)
       !! author: David A. Minton
       !!
diff --git a/src/symba/symba_setup.f90 b/src/symba/symba_setup.f90
index 9c311f2d1..6449013dd 100644
--- a/src/symba/symba_setup.f90
+++ b/src/symba/symba_setup.f90
@@ -15,10 +15,9 @@ module subroutine symba_setup_pl(self,n)
       integer(I4B)                   :: i,j
 
       !> Call allocation method for parent class
-      associate(pl => self)
-         call helio_setup_pl(pl, n) 
-         if (n <= 0) return
-      end associate
+      !call helio_setup_pl(self, n) 
+      call setup_pl(self, n) 
+      if (n <= 0) return
       return
    end subroutine symba_setup_pl 
 
@@ -35,10 +34,8 @@ module subroutine symba_setup_system(self, param)
       integer(I4B) :: i, j
 
       ! Call parent method
-      call helio_setup_system(self, param)
+      call whm_setup_system(self, param)
 
-      ! Set up the pl-tp planetocentric encounter structures for pl and cb. The planetocentric tp structures are 
-      ! generated as necessary during close encounter steps.
       select type(pl => self%pl)
       class is(symba_pl)
          select type(cb => self%cb)
@@ -65,7 +62,8 @@ module subroutine symba_setup_tp(self,n)
       integer,         intent(in)    :: n    !! Number of test particles to allocate
 
       !> Call allocation method for parent class
-      call helio_setup_tp(self, n) 
+      !call helio_setup_tp(self, n) 
+      call setup_tp(self, n) 
       if (n <= 0) return
       return
    end subroutine symba_setup_tp
diff --git a/src/symba/symba_step.f90 b/src/symba/symba_step.f90
index 10e8938db..b04caa74e 100644
--- a/src/symba/symba_step.f90
+++ b/src/symba/symba_step.f90
@@ -34,4 +34,14 @@ module subroutine symba_step_system(self, param, t, dt)
       return
 
    end subroutine symba_step_system
+
+   module subroutine symba_step_interp_system(self, param, t, dt)
+      implicit none
+      class(symba_nbody_system),  intent(inout) :: self    !! SyMBA nbody system object
+      class(swiftest_parameters), intent(inout) :: param  !! Current run configuration parameters 
+      real(DP),                   intent(in)    :: t      !! Simulation time
+      real(DP),                   intent(in)    :: dt     !! Current stepsize
+
+      return
+   end subroutine symba_step_interp_system
 end submodule s_symba_step
diff --git a/src/util/util_spill_and_fill.f90 b/src/util/util_spill_and_fill.f90
index 4513e5b24..1a51c06c5 100644
--- a/src/util/util_spill_and_fill.f90
+++ b/src/util/util_spill_and_fill.f90
@@ -17,39 +17,43 @@ module subroutine util_spill_body(self, discards, lspill_list)
       ! For each component, pack the discarded bodies into the discard object and do the inverse with the keeps
       !> Spill all the common components
       associate(keeps => self)
-         discards%id(:)     = pack(keeps%id(:),          lspill_list(:))
-         discards%status(:)   = pack(keeps%status(:),        lspill_list(:))
-         discards%a(:)        = pack(keeps%a(:),             lspill_list(:))
-         discards%e(:)        = pack(keeps%e(:),             lspill_list(:))
-         discards%capom(:)    = pack(keeps%capom(:),         lspill_list(:))
-         discards%omega(:)    = pack(keeps%omega(:),         lspill_list(:))
-         discards%capm(:)     = pack(keeps%capm(:),          lspill_list(:))
-         discards%mu(:)       = pack(keeps%mu(:),            lspill_list(:))
+         discards%id(:)       = pack(keeps%id(:),     lspill_list(:))
+         discards%name(:)     = pack(keeps%name(:),   lspill_list(:))
+         discards%status(:)   = pack(keeps%status(:), lspill_list(:))
+         discards%a(:)        = pack(keeps%a(:),      lspill_list(:))
+         discards%e(:)        = pack(keeps%e(:),      lspill_list(:))
+         discards%capom(:)    = pack(keeps%capom(:),  lspill_list(:))
+         discards%omega(:)    = pack(keeps%omega(:),  lspill_list(:))
+         discards%capm(:)     = pack(keeps%capm(:),   lspill_list(:))
+         discards%mu(:)       = pack(keeps%mu(:),     lspill_list(:))
          do i = 1, NDIM
-            discards%xh(i, :) = pack(keeps%xh(i, :),          lspill_list(:))
-            discards%vh(i, :) = pack(keeps%vh(i, :),          lspill_list(:))
-            discards%xb(i, :) = pack(keeps%xb(i, :),          lspill_list(:))
-            discards%vb(i, :) = pack(keeps%vb(i, :),          lspill_list(:))
-            discards%ah(i, :) = pack(keeps%ah(i, :),          lspill_list(:))
-            discards%aobl(i, :) = pack(keeps%aobl(i, :),          lspill_list(:))
+            discards%xh(i, :) = pack(keeps%xh(i, :),     lspill_list(:))
+            discards%vh(i, :) = pack(keeps%vh(i, :),     lspill_list(:))
+            discards%xb(i, :) = pack(keeps%xb(i, :),     lspill_list(:))
+            discards%vb(i, :) = pack(keeps%vb(i, :),     lspill_list(:))
+            discards%ah(i, :) = pack(keeps%ah(i, :),     lspill_list(:))
+            discards%aobl(i, :) = pack(keeps%aobl(i, :), lspill_list(:))
+            discards%agr(i, :)   = pack(keeps%agr(i, :), lspill_list(:))
          end do
          if (count(.not.lspill_list(:))  > 0) then 
-            keeps%id(:)       = pack(keeps%id(:),     .not. lspill_list(:))
-            keeps%status(:)     = pack(keeps%status(:),   .not. lspill_list(:))
-            keeps%a(:)          = pack(keeps%a(:),        .not. lspill_list(:))
-            keeps%e(:)          = pack(keeps%e(:),        .not. lspill_list(:))
-            keeps%inc(:)        = pack(keeps%inc(:),      .not. lspill_list(:))
-            keeps%capom(:)      = pack(keeps%capom(:),    .not. lspill_list(:))
-            keeps%omega(:)      = pack(keeps%omega(:),    .not. lspill_list(:))
-            keeps%capm(:)       = pack(keeps%capm(:),     .not. lspill_list(:))
-            keeps%mu(:)         = pack(keeps%mu(:),       .not. lspill_list(:))
+            keeps%id(:)         = pack(keeps%id(:),     .not. lspill_list(:))
+            keeps%name(:)       = pack(keeps%name(:),   .not. lspill_list(:))
+            keeps%status(:)     = pack(keeps%status(:), .not. lspill_list(:))
+            keeps%a(:)          = pack(keeps%a(:),      .not. lspill_list(:))
+            keeps%e(:)          = pack(keeps%e(:),      .not. lspill_list(:))
+            keeps%inc(:)        = pack(keeps%inc(:),    .not. lspill_list(:))
+            keeps%capom(:)      = pack(keeps%capom(:),  .not. lspill_list(:))
+            keeps%omega(:)      = pack(keeps%omega(:),  .not. lspill_list(:))
+            keeps%capm(:)       = pack(keeps%capm(:),   .not. lspill_list(:))
+            keeps%mu(:)         = pack(keeps%mu(:),     .not. lspill_list(:))
             do i = 1, NDIM
-               keeps%xh(i, :)    = pack(keeps%xh(i, :),    .not. lspill_list(:))
-               keeps%vh(i, :)    = pack(keeps%vh(i, :),    .not. lspill_list(:))
-               keeps%xb(i, :)    = pack(keeps%xb(i, :),    .not. lspill_list(:))
-               keeps%vb(i, :)    = pack(keeps%vb(i, :),    .not. lspill_list(:))
-               keeps%ah(i, :)    = pack(keeps%ah(i, :),    .not. lspill_list(:))
-               keeps%aobl(i, :)    = pack(keeps%aobl(i, :),    .not. lspill_list(:))
+               keeps%xh(i, :)    = pack(keeps%xh(i, :),   .not. lspill_list(:))
+               keeps%vh(i, :)    = pack(keeps%vh(i, :),   .not. lspill_list(:))
+               keeps%xb(i, :)    = pack(keeps%xb(i, :),   .not. lspill_list(:))
+               keeps%vb(i, :)    = pack(keeps%vb(i, :),   .not. lspill_list(:))
+               keeps%ah(i, :)    = pack(keeps%ah(i, :),   .not. lspill_list(:))
+               keeps%aobl(i, :)  = pack(keeps%aobl(i, :), .not. lspill_list(:))
+               keeps%agr(i, :)   = pack(keeps%agr(i, :),  .not. lspill_list(:))
             end do
          end if
          ! This is the base class, so will be the last to be called in the cascade. 
@@ -82,11 +86,13 @@ module subroutine util_fill_body(self, inserts, lfill_list)
 
       ! For each component, pack the discarded bodies into the discard object and do the inverse with the keeps
       !> Spill all the common components
-      associate(keeps => self, insname => inserts%id, keepname => self%id)
-
+      associate(keeps => self)
          keeps%id(:)     = unpack(keeps%id(:), .not.lfill_list(:), keeps%id(:))
          keeps%id(:)     = unpack(inserts%id(:), lfill_list(:), keeps%id(:))
 
+         keeps%name(:)     = unpack(keeps%name(:), .not.lfill_list(:), keeps%name(:))
+         keeps%name(:)     = unpack(inserts%name(:), lfill_list(:), keeps%name(:))
+
          keeps%status(:)   = unpack(keeps%status(:), .not.lfill_list(:), keeps%status(:))
          keeps%status(:)   = unpack(inserts%status(:), lfill_list(:), keeps%status(:))
 
@@ -112,6 +118,8 @@ module subroutine util_fill_body(self, inserts, lfill_list)
             keeps%aobl(i, :)  = unpack(keeps%aobl(i, :), .not.lfill_list(:), keeps%aobl(i, :))
             keeps%aobl(i, :)  = unpack(inserts%aobl(i, :), lfill_list(:), keeps%aobl(i, :))
 
+            keeps%agr(i, :)  = unpack(keeps%agr(i, :), .not.lfill_list(:), keeps%agr(i, :))
+            keeps%agr(i, :)  = unpack(inserts%agr(i, :), lfill_list(:), keeps%agr(i, :))
          end do
          
          keeps%a(:)     = unpack(keeps%a(:),    .not.lfill_list(:), keeps%a(:))
diff --git a/src/whm/whm_gr.f90 b/src/whm/whm_gr.f90
index 363ce5ad4..88018057e 100644
--- a/src/whm/whm_gr.f90
+++ b/src/whm/whm_gr.f90
@@ -78,10 +78,10 @@ module pure subroutine whm_gr_p4_pl(self, param, dt)
       ! Internals
       integer(I4B)                                 :: i
 
-      associate(n => self%nbody, xj => self%xj, vj => self%vj, status => self%status, c2 => param%inv_c2)
-         if (n == 0) return
-         do i = 1,n
-            call p4_func(xj(:, i), vj(:, i), dt, c2)
+      associate(pl => self, npl => self%nbody)
+         if (npl == 0) return
+         do i = 1, npl
+            call gr_p4_pos_kick(param, pl%xj(:, i), pl%vj(:, i), dt)
          end do
       end associate
  
@@ -97,17 +97,16 @@ module pure subroutine whm_gr_p4_tp(self, param, dt)
       !! Adapted from David A. Minton's Swifter routine routine gr_whm_p4.f90
       implicit none
       ! Arguments
-      class(whm_tp),                 intent(inout) :: self   !! Swiftest particle object
+      class(whm_tp),              intent(inout) :: self  !! Swiftest particle object
       class(swiftest_parameters), intent(in)    :: param !! Current run configuration parameters of on parameters 
-      real(DP),                      intent(in)    :: dt     !! Step size
+      real(DP),                   intent(in)    :: dt    !! Step size
       ! Internals
-      integer(I4B)                                 :: i
+      integer(I4B)                              :: i
 
-      associate(n => self%nbody, xh => self%xh, vh => self%vh, status => self%status, c2 => param%inv_c2)
-         if (n == 0) return
-         !do concurrent (i = 1:n, status(i) == ACTIVE)
-         do i = 1, n
-            call p4_func(xh(:, i), vh(:, i), dt, c2)
+      associate(tp => self, ntp => self%nbody)
+         if (ntp == 0) return
+         do i = 1, ntp
+            call gr_p4_pos_kick(param, tp%xh(:, i), tp%vh(:, i), dt)
          end do
       end associate
  
@@ -121,21 +120,19 @@ module pure subroutine whm_gr_pv2vh_pl(self, param)
       !! in a WHM object
       implicit none
       ! Arguments
-      class(whm_pl),                 intent(inout) :: self   !! Swiftest particle object
+      class(whm_pl),              intent(inout) :: self  !! Swiftest particle object
       class(swiftest_parameters), intent(in)    :: param !! Current run configuration parameters of on parameters 
       ! Internals
-      integer(I4B)                                 :: i
-      real(DP), dimension(:,:), allocatable        :: vh !! Temporary holder of pseudovelocity for in-place conversion
+      integer(I4B)                              :: i
+      real(DP), dimension(:,:), allocatable     :: vh    !! Temporary holder of pseudovelocity for in-place conversion
       
-      associate(n => self%nbody, xh => self%xh, pv => self%vh, status => self%status, mu => self%muj)
-         if (n == 0) return
-         allocate(vh, mold = pv)
-         !do concurrent(i = 1:n, status(i) == ACTIVE)
-         do i = 1, n
-            call gr_pseudovel2vel(param, mu(i), xh(:, i), pv(:, i), vh(:, i))
-            pv(:, i) = vh(:, i)
+      associate(pl => self, npl => self%nbody)
+         if (npl == 0) return
+         allocate(vh, mold = pl%vh)
+         do i = 1, npl
+            call gr_pseudovel2vel(param, pl%mu(i), pl%xh(:, i), pl%vh(:, i), vh(:, i))
          end do
-         deallocate(vh)
+         call move_alloc(vh, pl%vh)
       end associate
       return
    end subroutine whm_gr_pv2vh_pl
@@ -147,21 +144,19 @@ module pure subroutine whm_gr_pv2vh_tp(self, param)
       !! in a WHM object
       implicit none
       ! Arguments
-      class(whm_tp),                 intent(inout) :: self   !! Swiftest particle object
+      class(whm_tp),              intent(inout) :: self   !! Swiftest particle object
       class(swiftest_parameters), intent(in)    :: param !! Current run configuration parameters of on parameters 
       ! Internals
       integer(I4B)                                 :: i
       real(DP), dimension(:,:), allocatable        :: vh !! Temporary holder of pseudovelocity for in-place conversion
       
-      associate(n => self%nbody, xh => self%xh, pv => self%vh, status => self%status, mu => self%mu)
-         if (n == 0) return
-         allocate(vh, mold = pv)
-         !do concurrent(i = 1:n, status(i) == ACTIVE)
-         do i = 1, n
-            call gr_pseudovel2vel(param, mu(i), xh(:, i), pv(:, i), vh(:, i))
-            pv(:, i) = vh(:, i)
+      associate(tp => self, ntp => self%nbody)
+         if (ntp == 0) return
+         allocate(vh, mold = tp%vh)
+         do i = 1, ntp
+            call gr_pseudovel2vel(param, tp%mu(i), tp%xh(:, i), tp%vh(:, i), vh(:, i))
          end do
-         deallocate(vh)
+         call move_alloc(vh, tp%vh)
       end associate
       return
    end subroutine whm_gr_pv2vh_tp
@@ -173,21 +168,19 @@ module pure subroutine whm_gr_vh2pv_pl(self, param)
       !! in a WHM object
       implicit none
       ! Arguments
-      class(whm_pl),                 intent(inout) :: self   !! Swiftest particle object
+      class(whm_pl),              intent(inout) :: self   !! Swiftest particle object
       class(swiftest_parameters), intent(in)    :: param !! Current run configuration parameters of on parameters 
       ! Internals
       integer(I4B)                                 :: i
       real(DP), dimension(:,:), allocatable        :: pv !! Temporary holder of pseudovelocity for in-place conversion
       
-      associate(n => self%nbody, xh => self%xh, vh => self%vh, status => self%status, mu => self%muj)
-         if (n == 0) return
-         allocate(pv, mold = vh)
-         !do concurrent(i = 1:n, status(i) == ACTIVE)
-         do i = 1, n
-            call gr_vel2pseudovel(param, mu(i), xh(:, i), vh(:, i), pv(:, i))
-            vh(:, i) = pv(:, i)
+      associate(pl => self, npl => self%nbody)
+         if (npl == 0) return
+         allocate(pv, mold = pl%vh)
+         do i = 1, npl
+            call gr_vel2pseudovel(param, pl%mu(i), pl%xh(:, i), pl%vh(:, i), pv(:, i))
          end do
-         deallocate(pv)
+         call move_alloc(pv, pl%vh)
       end associate
       return
    end subroutine whm_gr_vh2pv_pl
@@ -199,137 +192,21 @@ module pure subroutine whm_gr_vh2pv_tp(self, param)
       !! in a WHM object
       implicit none
       ! Arguments
-      class(whm_tp),                 intent(inout) :: self   !! Swiftest particle object
+      class(whm_tp),              intent(inout) :: self   !! Swiftest particle object
       class(swiftest_parameters), intent(in)    :: param !! Current run configuration parameters of on parameters 
       ! Internals
       integer(I4B)                                 :: i
       real(DP), dimension(:,:), allocatable        :: pv !! Temporary holder of pseudovelocity for in-place conversion
       
-      associate(n => self%nbody, xh => self%xh, vh => self%vh, status => self%status, mu => self%mu)
-         if (n == 0) return
-         allocate(pv, mold = vh)
-         !do concurrent(i = 1:n, status(i) == ACTIVE)
-         do i = 1, n
-            call gr_vel2pseudovel(param, mu(i), xh(:, i), vh(:, i), pv(:, i))
-            vh(:, i) = pv(:, i)
+      associate(tp => self, ntp => self%nbody)
+         if (ntp == 0) return
+         allocate(pv, mold = tp%vh)
+         do i = 1, ntp
+            call gr_vel2pseudovel(param, tp%mu(i), tp%xh(:, i), tp%vh(:, i), pv(:, i))
          end do
-         deallocate(pv)
+         call move_alloc(pv, tp%vh)
       end associate
       return
    end subroutine whm_gr_vh2pv_tp
 
-   pure subroutine gr_vel2pseudovel(param, mu, xh, vh, pv)
-      !! author: David A. Minton
-      !!
-      !! Converts the heliocentric velocity into a pseudovelocity with relativistic corrections. 
-      !! Uses Newton-Raphson method with direct inversion of the Jacobian (yeah, it's slow, but 
-      !! this is only done once per run).
-      !!
-      !! Adapted from David A. Minton's Swifter routine gr_vel2pseudovel.f90
-      implicit none
-
-      class(swiftest_parameters), intent(in)  :: param !! Current run configuration parameters 
-      real(DP),                      intent(in)  :: mu     !! G * (Mcb + m), G = gravitational constant, Mcb = mass of central body, m = mass of body
-      real(DP), dimension(:),        intent(in)  :: xh     !! Heliocentric position vector 
-      real(DP), dimension(:),        intent(in)  :: vh     !! Heliocentric velocity vector 
-      real(DP), dimension(:),        intent(out) :: pv     !! Pseudovelocity vector - see Saha & Tremain (1994), eq. (32)
-
-      real(DP)                       :: v2, G, pv2, rterm, det
-      real(DP), dimension(NDIM,NDIM) :: J,Jinv
-      real(DP), dimension(NDIM)      :: F
-      integer(I4B)                   :: n,i,k
-      integer(I4B), parameter        :: MAXITER = 50
-      real(DP),parameter             :: TOL = 1.0e-12_DP
-
-      associate (c2 => param%inv_c2)
-         pv(1:NDIM) = vh(1:NDIM) ! Initial guess
-         rterm = 3 * mu / norm2(xh(:))
-         v2 = dot_product(vh(:), vh(:))
-         do n = 1, MAXITER
-            pv2 = dot_product(pv(:), pv(:))
-            G = 1.0_DP - c2 * (0.5_DP * pv2 + rterm)
-            F(:) = pv(:) * G - vh(:)
-            if (abs(sum(F) / v2 ) < TOL) exit ! Root found
-   
-            ! Calculate the Jacobian
-            !do concurrent (k = 1:NDIM)
-            !   do concurrent (i = 1:NDIM)
-            do k = 1, NDIM
-               do i = 1, NDIM
-                  if (i == k) then
-                     J(i,k) = G - c2 * pv(k)
-                  else
-                     J(i,k) = - c2 * pv(k)
-                  end if
-               end do
-            end do
-   
-            ! Inverse of the Jacobian
-            det = J(1,1) * (J(3,3) * J(2,2) - J(3,2) * J(2,3))
-            det = det - J(2,1) * (J(3,3) * J(1,2)-J(3,2) * J(1,3))
-            det = det + J(3,1) * (J(2,3) * J(1,2)-J(2,2) * J(1,3))
-   
-            Jinv(1,1) =   J(3,3) * J(2,2) - J(3,2) * J(2,3)
-            Jinv(1,2) = -(J(3,3) * J(1,2) - J(3,2) * J(1,3))
-            Jinv(1,3) =   J(2,3) * J(1,2) - J(2,2) * J(1,3)
-   
-            Jinv(2,1) = -(J(3,3) * J(2,1) - J(3,1) * J(2,3))
-            Jinv(2,2) =   J(3,3) * J(1,1) - J(3,1) * J(1,3)
-            Jinv(2,3) = -(J(2,3) * J(1,1) - J(2,1) * J(1,3))
-   
-            Jinv(3,1) =   J(3,2) * J(2,1) - J(3,1) * J(2,2)
-            Jinv(3,2) = -(J(3,2) * J(1,1) - J(3,1) * J(1,2))
-            Jinv(3,3) =   J(2,2) * J(1,1) - J(2,1) * J(1,2)
-   
-            Jinv = Jinv * det
-   
-            do i = 1, NDIM
-               pv(i) = pv(i) - dot_product(Jinv(i,:) ,F(:))
-            end do
-         end do 
-   
-      end associate
-      return
-   end subroutine gr_vel2pseudovel
-
-   pure subroutine gr_pseudovel2vel(param, mu, xh, pv, vh) 
-      !! author: David A. Minton
-      !!
-      !! Converts the relativistic pseudovelocity back into a veliocentric velocity
-      !!    Based on Saha & Tremaine (1994) Eq. 32
-      !!
-      !! Adapted from David A. Minton's Swifter routine gr_pseudovel2vel.f90 
-      implicit none
-      class(swiftest_parameters), intent(in)  :: param !! Current run configuration parameters 
-      real(DP),                      intent(in)  :: mu     !! G * (Mcb + m), G = gravitational constant, Mcb = mass of central body, m = mass of body
-      real(DP), dimension(:),        intent(in)  :: xh     !! Heliocentric position vector 
-      real(DP), dimension(:),        intent(in)  :: pv     !! Pseudovelocity velocity vector - see Saha & Tremain (1994), eq. (32)
-      real(DP), dimension(:),        intent(out) :: vh     !! Heliocentric velocity vector 
-
-      real(DP) :: vmag2, rmag, grterm
-   
-      associate(c2 => param%inv_c2)
-         vmag2 = dot_product(pv(:), pv(:))
-         rmag  = norm2(xh(:))
-         grterm = 1.0_DP - c2 * (0.5_DP * vmag2 + 3 * mu / rmag)
-         vh(:) = pv(:) * grterm
-      end associate
-      return
-   end subroutine gr_pseudovel2vel
-
-   pure subroutine p4_func(x, v, dt, c2)
-      implicit none
-      real(DP), dimension(:), intent(inout) :: x
-      real(DP), dimension(:), intent(in)    :: v
-      real(DP),               intent(in)    :: dt, c2 
-      real(DP), dimension(NDIM)             :: dr
-      real(DP)                              :: vmag2
-
-      vmag2 = dot_product(v(:), v(:)) 
-      dr(:) = -2 * c2 * vmag2 * v(:)
-      x(:) = x(:) + dr(:) * dt
-
-      return
-   end subroutine p4_func  
-
 end submodule s_whm_gr
\ No newline at end of file
diff --git a/src/whm/whm_setup.f90 b/src/whm/whm_setup.f90
index 9598b61ea..b0b25dc55 100644
--- a/src/whm/whm_setup.f90
+++ b/src/whm/whm_setup.f90
@@ -84,21 +84,16 @@ module subroutine whm_setup_system(self, param)
       call io_read_initialize_system(self, param)
       ! Make sure that the discard list gets allocated initially
       call self%tp_discards%setup(self%tp%nbody)
-
-      if (self%pl%nbody > 0) then
+      call self%pl%set_mu(self%cb)
+      call self%tp%set_mu(self%cb)
+      if (param%lgr) then
          select type(pl => self%pl)
          class is (whm_pl)
-            call pl%set_mu(self%cb)
-            if (param%lgr) call pl%vh2pv(param)
-            !call pl%eucl_index()
+            call pl%vh2pv(param)
          end select
-      end if
-
-      if (self%tp%nbody > 0) then
          select type(tp => self%tp)
          class is (whm_tp)
-            call tp%set_mu(self%cb)
-            if (param%lgr) call tp%vh2pv(param)
+            call tp%vh2pv(param)
          end select
       end if