module ssd

  use constants_mod, only: wp, sin_eps, cos_eps, gm_sun, two_pi, deg_per_rad, &
       sec_per_rad, au, pi, v_light, earth_moon_ratio, sec_per_day, rad_per_deg
  use open_eph_mod, only: open_eph_def, open_eph
  use intpar_mod, only: intpar_def
  use eop_mod, only: eop_def
  use obscode_mod, only: obscode_msg, obscode_ini, obscode_index, sites, &
       observatory_type
  use stn_pos_mod, only: stn_pos_msg, stn_pos_ini, stn_pos
  use eop_mod, only: eoplbl, eoptim
  use get_unit_mod, only: get_unit
  use matvec_util_mod, only: vsmat2mat, vsmat_mat_mul

  implicit none
  save
  private

  ! Public procedures
  public :: ssd_init, ssd_compute_ephemerides, ssd_propagate_elements, &
       compute_src
  ! Public variables
  public :: many_ephem_msg

  
  !====== BEGIN DECLARATIONS ===================================================

  ! Status message
  character (len=128) :: many_ephem_msg

  ! Program Version and Usage Strings:
  character (len=*), parameter :: prog_name = "ps_eph"
  character (len=*), parameter :: version = "1.0"
  character (len=*), parameter :: prog_name_version = prog_name//" "//version

  ! Misc.
  !integer :: k, inp_unit
  character (len=64) :: object
  real (kind=wp) :: eph_start, eph_stop
  real (kind=wp) :: t_ref = 2400000.5_wp ! Time origin in JED, MJD 0.0
  logical :: j2000




  !====== BEGIN EXECUTABLE CODE ================================================

contains

  subroutine ssd_init
    integer :: errcod

    !------ MISC INITIALIZATIONS -----------------------------------------------
    ! Initialize the system and STDERR device, verify execution authorization.
    call sysini
    call errini
    !!!print *, 'SSD_INIT'

    ! Write header line.
    !        call hdrlin(prog_name_version)

    !------ VARLIST: DEFINE NAMES AND LOAD OPTIONS -----------------------------

    ! Used elsewhere, but not vldef'ed
    call vldef('ETMFILE:c')

    ! Define varlist input variables used by other routines.
    call icsdef
    call open_eph_def
    call eop_def
    call intpar_def

    !------ INITIALIZATIONS ----------------------------------------------------
    ! Open the ephemeris files and initialize the propagator
    call vladd( 'SBPERT= T', 0, errcod)
    call open_eph(object, eph_start, eph_stop, t_ref, .true., j2000=j2000)

    ! Load observatory codes into memory
    call obscode_ini(errcod)
    if (errcod /= 0) call errabt(prog_name_version, obscode_msg)

    ! Load EOP data into memory
    call stn_pos_ini(j2000, t_ref, errcod)
    if (errcod /= 0) call errabt(prog_name_version, trim(stn_pos_msg))

  end subroutine ssd_init


  subroutine ssd_compute_ephemerides(epoch_mjd, elements, src, obs_code, &
       g_param, eph_times_ut, cov_int, ra, dec, err_ra, err_dec, smaa, smia, &
       pa, v_minus_h, errcod)
    real (kind=wp), intent(in)  :: epoch_mjd
    real (kind=wp), intent(in)  :: elements(:)
    real (kind=wp), intent(in)  :: src(:)
    character (len=3), intent(in):: obs_code
    real (kind=wp), intent(in)  :: g_param
    real (kind=wp), intent(in)  :: eph_times_ut(:)
    integer,        intent(in)  :: cov_int
    real (kind=wp), intent(out) :: ra(:)
    real (kind=wp), intent(out) :: dec(:)
    real (kind=wp), intent(out) :: err_ra(:)
    real (kind=wp), intent(out) :: err_dec(:)
    real (kind=wp), intent(out) :: smaa(:)
    real (kind=wp), intent(out) :: smia(:)
    real (kind=wp), intent(out) :: pa(:)
    real (kind=wp), intent(out) :: v_minus_h(:)
    integer,        intent(out) :: errcod

!===============================================================================
! many_eph computes the ephemeris positions for a sorted array of times. 
! This is an example template for use by MOPS coarse ephem.
!
! Input:
!    epoch_mjd - the epoch of the initial_elements (JD)
!    x_ini - Initial cartesian state and partials wrt initial conditions.
!            (The partials could be wrt elements or state.)
!    eph_times - desired times IN REVERSE CHRON. ORDER!!!!!
! Output:
!    eph_pos - Ephemeris positions
!    errcod     - error code. Zero indicates success.
!
!-------------------------------------------------------------------------------
! Written       Sep 27, 2007      Steve Chesley
!$==============================================================================

    ! Utility variables.
    real (kind=wp), allocatable :: x_ini(:,:), x(:,:), time_et(:), x_rec(:,:)
    real (kind=wp) :: x_earth(6), rho_temp(3), t_int
    real (kind=wp) :: owlt, owlt_sav, max_ltime, t_obj, dt, max_owlt
    real (kind=wp) :: coords_xyz(3)
    real (kind=wp), parameter :: lt_eps = 1.0e-12_wp
    ! Temp variables
    real (kind=wp) ::  x_rec_ssb(6), x_earth_ssb(6), et_minus_ut
    integer :: error, i_epoch, i_obs,  n_clapp, n_col, i, n_eph
    character (len=256) :: temp_str=" "
    logical :: single_step, forward, success, finished, cov_flag
    integer :: i_start, i_stop, i_step
    real (kind=wp) :: t_ref = 2400000.5
    real (kind=wp) :: radec_part(2,4), radec_partial(2,6), w(2,6)
    real (kind=wp) :: dist_prod, cos_beta, s, phi1, phi2
    real (kind=wp) :: r_sun(3), r_obs(3)
    real (kind=wp) :: tp0, temp_elem(6), x_temp(6), r1, r2
    real (kind=wp), allocatable :: ra_dec_cov(:,:,:)

    real (kind=wp) :: elems(6)

    ! Uncertainty stuff.
    real (kind=wp) :: eigval(2), eigvec(2, 2), tmp(2), radec_cov(3)

    
    ! Save the elements array and convert the times to JD.
    ! We need to do it because elements is declared intent(in)...
    elems = elements
    elems(3) = elements(3) - epoch_mjd

    ! Init some of the output vars
    errcod = 0
    many_ephem_msg = ' '
    err_ra = -1.0d0
    err_dec = -1.0d0
    smaa = -1.0d0
    smia = -1.0d0
    pa = -1.0d0
    cov_flag = cov_int /= 0
    n_col=1
    if (cov_flag) n_col=7
    n_eph = size(eph_times_ut)


    ! Allocate arrays
    allocate(x_ini(6,n_col), x(6,n_col), time_et(n_eph), x_rec(n_eph,3), &
         ra_dec_cov(n_eph, 2, 2), stat=error)
    if (error /= 0) then
       errcod = 4
       write (many_ephem_msg, *) "many_ephem error (", errcod, "): ", &
            "Memory allocation error."
       return
    endif

    ! Get station position in body-fixed frame.
    coords_xyz = sites(obscode_index(obs_code)) % coords_xyz

    ! Get station positions and ET times for the UTC ephem times.
    do i=1, n_eph
       call stn_pos(eph_times_ut(i), coords_xyz, .false., .true., .true., &
            time_et(i), et_minus_ut, x_rec_ssb, x_earth_ssb, &
            x_rec(i,:), error)
       if (error > 0) then
          errcod = 12
          write (many_ephem_msg, *) "many_ephem error (", errcod, "): ", &
               trim(stn_pos_msg)
          return
       endif
    enddo

    ! Covert to cartesian
    call cetpv(elems, gm_sun, cos_eps, sin_eps, cov_flag, x_ini, error)
    if (error /= 0) then
       call cetmsg(errcod, temp_str)
       errcod = 3
       write (many_ephem_msg, *) "many_ephem error (", errcod, "): ", &
            trim(temp_str)
       return
    endif

    ! Compute the light time to geocenter of a photon leaving object at epoch.
    ! This could be improved to include LTCORR, and topocentric dist of each obs.
    call earth(epoch_mjd, t_ref, x_earth)
    rho_temp = x_ini(:3,1) - x_earth(:3)
    owlt = sqrt(dot_product(rho_temp,rho_temp))/v_light
    do
      call earth(epoch_mjd + owlt, t_ref, x_earth)
      rho_temp = x_ini(:3,1) - x_earth(:3)
      owlt_sav = owlt
      owlt = sqrt(dot_product(rho_temp,rho_temp))/v_light
      if (abs(owlt - owlt_sav) < lt_eps) exit
    enddo

    ! Compute max_owlt
    ! When going forward in time use the upper 
    ! limit on the light time (aphelion + 1 AU)/vlight for elliptic orbits, 
    ! and for parabolic/hyperbolic orbits assume that all observations 
    ! are within one day light time, or about 173 AU.
    if (elems(1) < 0.999_wp) then

      ! Use aphelion + 1 for elliptical orbits
      max_owlt = (1._wp + elems(2) * &
         (1.0_wp + elems(1)) / (1.0_wp - elems(1))) / v_light

    else

      ! Use maximum of distances at beginning and end of arc, plus 1
      temp_elem = elems
      tp0 = temp_elem(3)
      temp_elem(3) = tp0 + (epoch_mjd - t_ref) - time_et(1)
      call cetpv(temp_elem, gm_sun, cos_eps, sin_eps, .false., x_temp, &
         errcod)
      if (errcod /= 0) then
        call cetmsg(errcod, temp_str)
        errcod = 11
        write (many_ephem_msg, *) "od_solve error (", errcod, "): ", &
           trim(temp_str)
        return
      endif
      r2 = sqrt(dot_product(x_temp(1:3), x_temp(1:3)))
      temp_elem(3) = tp0 + (epoch_mjd - t_ref) - time_et(n_eph)
      call cetpv(temp_elem, gm_sun, cos_eps, sin_eps, .false., x_temp, &
         errcod)
      if (errcod /= 0) then
        call cetmsg(errcod, temp_str)
        errcod = 12
        write (many_ephem_msg, *) "od_solve error (", errcod, "): ", &
           trim(temp_str)
        return
      endif
      r1 = sqrt(dot_product(x_temp(1:3), x_temp(1:3)))
      max_owlt = (max(r1, r2)+1)/v_light

    endif

    ! Find index of first obs before the photon got to geocenter.
    ! Remember that the obs are in reverse chronological order.
    do i_epoch = 1, n_eph + 1

      ! All obs are after epoch
      if (i_epoch > n_eph) exit 

      ! Compute how early or late the light reaches the Earth for each observation.
      dt = time_et(i_epoch) - (epoch_mjd + owlt)

      ! Handle cases too close to use geocenter, about 4 earth radii at v_light
      if (abs(dt) < 1.e-6_wp) then

        ! Compute time at topocenter for a photon emitted at epoch and 
        ! compare to receive time for observation.
        rho_temp = x_ini(:3,1) - x_rec(i_epoch,:)
        owlt_sav = sqrt(dot_product(rho_temp,rho_temp))/v_light
        dt = time_et(i_epoch) - (epoch_mjd + owlt_sav) 
      endif

      ! First one found before epoch
      if (dt < 0.0_wp) exit 
    enddo

    !============= OBSERVATION PROCESSING ======================================
    ! Make two loops to go first back then forward.
    ! Go backward first, unless all obs are after epoch.
    forward = i_epoch > n_eph
    finished = .false.
    
    do

       ! Set parameters to go forward or back as necessary
       ! (Recall obs are stored in reverse chronological order.)
       if (finished) then
          exit
       elseif (.not.forward .and. i_epoch <= n_eph) then

          ! Set indices to integrate backward.
          i_start = i_epoch
          i_stop = n_eph
          i_step = 1
      
          ! This is the amount of time that the integrator must stop before 
          ! reaching the observation time. It is zero when going backwards.
          max_ltime = 0.0_wp
          
          ! Go back to first observation
          x = x_ini
          call intdhd('Integrating back in time:')
          call intini(epoch_mjd, -9.9e9_wp, t_ref, x, n_col, .false., .false., .false.)
          single_step = .false.
          call intgrt(time_et(i_start), single_step, t_int, x, n_clapp)
          single_step = .true.
      
          ! If epoch is between emit time and observe time then avoid trying 
          ! to interpolate at a time outside of the allowed interval.
          ! Set DT so that interpolation falls between emit and epoch.
          if (time_et(i_start) > epoch_mjd) call objsdt(time_et(i_start) - epoch_mjd)

       elseif (forward .and. i_epoch /= 1) then

          ! Set indices to integrate forward.
          i_start = i_epoch - 1
          i_stop = 1
          i_step = -1
          
          ! Set One-Way Light Time for integrator to stop before passing emit time
          max_ltime = max_owlt
          
          ! Go forward to first observation time
          x = x_ini
          call intdhd('Integrating forward in time:')
          call intini(epoch_mjd, 9.9e9_wp, t_ref, x, n_col, &
               &.false., .false., .false.)
          single_step = .false.
          call intgrt(time_et(i_start)-max_ltime, single_step, t_int, x, n_clapp)
          single_step = .true.
          
          ! Reset value of DT. Going forward
          call objsdt(0.0_wp)

       endif
   
       ! ------------- Observation Loop ----------------------------------------
       do i_obs = i_start, i_stop, i_step

          ! Get state at time light left object
          do
             call objsta(forward, time_et(i_obs), t_int, x(1:3,1), &
                  x_rec(i_obs,1:3), t_obj, x(:,1), rho_temp, success)
             if (success) then
                exit
             else
                call intgrt(time_et(i_obs)-max_ltime, single_step, t_int, x, &
                     &n_clapp)
                single_step = .true.
             endif
          enddo
          call intint(t_obj, n_col, x)
          single_step = .false.
          
          ! Compute the predicted magnitude less H
          v_minus_h(i_obs) = g_param 
          !r_sun = - x(1,:3)
          !r_obs = r_sun + x_rec(i_obs,:)  ! cart_topo
          r_sun = x(:3,1)
          r_obs = r_sun - x_rec(i_obs,:)  ! cart_topo
          dist_prod = sqrt(dot_product(r_sun, r_sun) * dot_product(r_obs, r_obs))
          cos_beta = dot_product(r_sun, r_obs) / dist_prod
          s = sqrt((1.0_wp - cos_beta) / (1.0_wp + cos_beta)) ! tan(phase/2)
          phi1 = exp(-3.33_wp * s**0.63_wp)
          phi2 = exp(-1.87_wp * s**1.22_wp)
          v_minus_h(i_obs) = 5.0_wp * log10(dist_prod) - &
               2.5_wp * log10((1.0_wp-g_param)*phi1 + g_param*phi2)
         

          ! Compute predicted obs, residuals and partials wrt to current state.
          call radec(x(1:3,1), x_rec(i_obs,1:3), time_et(i_obs), cov_flag, &
               ra(i_obs), dec(i_obs), radec_part(1,:), radec_part(2,:))
          if (ra(i_obs) < 0.0_wp) ra(i_obs) = ra(i_obs) + two_pi

          ! Map the ra and dec partials from current state to initial 
          ! elements. If there are no estimated nondynamical parameters or no
          ! considered dynamical parameters then the multiplication can be 
          ! done in one block. Otherwise we map the estimated and considered
          ! parameters separately.
          if (cov_flag) then

             ! Compute partials wrt to initial elements, a 2 x 6 matrix
             radec_partial = matmul(radec_part(:,:3), x(:3,2:7))
             
             ! d(RA,DEC)/d(E_0) * Elements Sqrt Cov = (RA,DEC) Sqrt Cov
             call vsmat_mat_mul(src, radec_partial, 'PRE', w, errcod)
             if (error > 0) then
                errcod = 121
                write (many_ephem_msg, *) "many_ephem error (", &
                     errcod, "): ", "Error in vsmat_mat_mul."
                return
             endif
             ! Square it up to get full covariance
             ra_dec_cov(i_obs, :, :) = matmul(w,transpose(w)) 
             
             !!!!print *, 'ra_dec_cov', ra_dec_cov(i_obs,:,:)
             
             ! RA, Dec uncertainty (just for the fun of it).
             ! Multiply it by 3 to get the 3-sigma uncertainty (if you want).
             err_ra(i_obs) = sqrt(ra_dec_cov(i_obs, 1,1)) * & 
                  cos(dec(i_obs)) * sec_per_rad * 1.
             err_dec(i_obs) = sqrt(ra_dec_cov(i_obs, 2,2)) * sec_per_rad * 1.
             
             ! Pre and post ultiply the ra_dec_cov by ((cos(Dec), 0), (0, 1))
             ! in order to get the semi axis and position angle of the sky
             ! plane error ellipse. What we want are the eigenvalue and
             ! eigenvectors of the resulting 2x2. Also convert the semi axis
             ! to arcsecs.
             radec_cov(1) = ra_dec_cov(i_obs, 1, 1) * cos(dec(i_obs)) * &
                  cos(dec(i_obs))
             radec_cov(2) = ra_dec_cov(i_obs, 1, 2) * cos(dec(i_obs))
             radec_cov(3) = ra_dec_cov(i_obs, 2, 2)
             call eigsym(radec_cov, 2, 2, tmp, eigval, eigvec, errcod)
             
             ! The sqrt of each eigenvalue gives us the semin axis of the
             ! allipse. Again, one can multiply by 3 to get the 3-sigma values
             ! instead of 1-sigma.
             if(eigval(2) .gt. 0.0d0) then
                smaa(i_obs) = sqrt(eigval(2)) * sec_per_rad * 1.
             else
                ! Ops! we got a negative eigenvalue!!!!!! No good!
                errcod = 122
                write (many_ephem_msg, *) &
                     "many_ephem error (", errcod, "): ", &
                     "Negative 2nd eigenvalue."
                return
             end if
             if(eigval(1) .gt. 0.0d0) then
                smia(i_obs) = sqrt(eigval(1)) * sec_per_rad * 1.
             else
                ! Ops! we got a negative eigenvalue!!!!!! No good!
                errcod = 123
                write (many_ephem_msg, *) &
                     "many_ephem error (", errcod, "): ", &
                     "Negative 1st eigenvalue."
                return
             end if

             ! Now the PA. It is given by the two eigenvectors.
             pa(i_obs) = ATAN2(eigvec(2, 2), eigvec(1, 2)) * deg_per_rad

             ! We want the sense of the PA to be zero pointed N, and PI/2 when
             ! pointed to the east.  ATAN2() orients zero to the east, then
             ! anticlockwise.
             pa(i_obs) = 90 - pa(i_obs)
             if (pa(i_obs) .gt. 180.0d0) then
                pa(i_obs) = pa(i_obs) - 360.0d0
             else if (pa(i_obs) .lt. -180.0d0) then
                pa(i_obs) = pa(i_obs) + 360.0d0
             !if(pa(i_obs) .gt. 90.0d0) then
             !   pa(i_obs) = pa(i_obs) - 180.0d0
             !else if(pa(i_obs) .lt. -90.0d0) then
             !   pa(i_obs) = pa(i_obs) + 180.0d0

             end if
          endif
       enddo

       ! Clear integrator
       call intfin

       ! Stop after running forward, or after running backward when 
       ! epoch is after all obs.
       finished = forward .or. i_epoch == 1

       ! If we are not done then we must go forward still
       forward = .true.
    enddo

    deallocate(x_ini, x, time_et, x_rec, ra_dec_cov, stat=error)
    if (error /= 0) then
       errcod = 4
       write (many_ephem_msg, *) "many_ephem error (", errcod, "): ", &
            "Memory deallocation error."
       return
    endif
  end subroutine ssd_compute_ephemerides


  subroutine ssd_propagate_elements(t_epoch, in_elems, out_elems,&
        t_eph_ut, errcod)
    real (kind=wp), intent(in) :: t_epoch
    real (kind=wp), intent(in) :: in_elems(6)
    real (kind=wp), intent(out) :: out_elems(6)
    character (len=3) :: obscod
    real (kind=wp), intent(in) :: t_eph_ut
    integer, intent(out) :: errcod

    real (kind=wp) :: site_coords(3)
    ! x_rec_ssb station pos+vel wrt SSB, x_earth center pos+vel wrt SSB, 
    ! x_rec station position wrt Sun
    ! x pos+vel of obj wrt SSB
    real (kind=wp) :: elements(6)
    real (kind=wp) :: t_eph_et, et_minus_ut, x_rec_ssb(6)
    real (kind=wp) :: x_earth_ssb(6), x_rec(3)
    real (kind=wp) :: x(6, 7)
    logical :: forward
    real (kind=wp) :: fore_max_owlt, back_max_owlt
    real (kind=wp) :: fore_t_stop, back_t_stop
    real (kind=wp) :: t_stop, max_owlt

    !------ HOUSEKEEPING -------------------------------------------------------
    real (kind=wp) :: min_et


    ! Internally, tp is measured wrt epoch.
    elements(:) = in_elems(:)
    elements(3) = in_elems(3) - t_epoch

    ! Obtain body-fixed geocentric pos. of observatory.
    obscod = '500'          ! geocenter
    site_coords = sites(obscode_index(obscod)) % coords_xyz 

    ! Convert Cometary Elements to Cartesian State in J2000 Equatorial Frame.
    ! The array x will contain the Cartesia state (position, velocity etc).
    call cetpv(elements, gm_sun, cos_eps, sin_eps, .true., x, errcod)
    if (errcod /= 0) then
       ! error in conversion to J2000.
       errcod = 5
       return
    endif

    ! Ensure solution epoch is not off the ephemeris.
    if (t_epoch + t_ref < eph_start .or. t_epoch + t_ref > eph_stop) then
       ! Orbit epoch is off the ephemerides range.
       errcod = 3
       return
    endif
    
    ! Precompute the amount of time that the integrator must stop before 
    ! reaching the observation time in order to still be able interpolate at the
    ! emit time. We have two different values depending whether we are 
    ! integrating forward or backward in time with respect to the orbit epoch.
    ! Forward integration values: when going forward in time use the upper 
    ! limit on the light time (aphelion + 1 AU)/vlight for elliptic orbits, and
    ! for parabolic/hyperbolic orbits assume that all observations are within 
    ! one day light time, or about 173 AU.
    if (elements(1) < 1.0_wp) then
       fore_max_owlt = (1._wp + elements(2) * &
            (1.0_wp + elements(1)) / (1.0_wp - elements(1))) / v_light
    else
       fore_max_owlt = 1.0_wp
    endif
    fore_t_stop = 9.e99_wp
    ! Backward integration: when going backward use zero.
    back_max_owlt = 0._wp
    back_t_stop = -9.e99_wp
    

    
    !----- Time dependent code starts here. ------------------------------------
    
    ! Compute ET times for the extremes (there is probably an easier way).
    call stn_pos(t_eph_ut, site_coords, .false., .true., .true., &
            min_et, et_minus_ut, x_rec_ssb, x_earth_ssb, x_rec, errcod)
    
    ! Ensure ephemeris time is not off the ephemeris.
    if (t_epoch + t_ref < eph_start .or. t_epoch + t_ref > eph_stop) then
       ! Ephemerides time is off the ephemerides range.
       errcod = 4
       return
    endif

    ! We have three cases to consider here: 
    !  1. All ephem times and in the future wrt t_epoch.
    !  2. All ephem times are in the past wrt t_epoch.
    !  3. Some ephem times are in the future, some in the past of t_epoch.
    ! In cases 1. and 2. we only init the integrator once. For case 3. we have 
    ! to init it twive: once when we start going backwards, and the again when
    ! we start going forward.
    ! Which direction are we going?
    if (t_eph_ut .le. t_epoch) then
       t_stop = back_t_stop
       max_owlt = back_max_owlt
       forward = .false.
    else
       t_stop = fore_t_stop
       max_owlt = fore_max_owlt
       forward = .true.
    endif
    call intini(t_epoch, t_stop, t_ref, x, 7, .false., .false., .false.)

   ! Compute the position of the observatory with respect to both the Sun
   ! and the Solar System barycenter at the ephemerides time.
   call stn_pos(t_eph_ut, site_coords, .false., .true., &
        .true., t_eph_et, et_minus_ut, x_rec_ssb, x_earth_ssb, x_rec, &
        errcod)
   if (errcod > 0) then
      ! error in stn
      errcod = 1
      return
   endif

   !------ PROPAGATE TO EMIT TIME ---------------------------------------
   ! Compute positions at ephemerides time.
   call propagate_elements(t_eph_et, max_owlt, &
        x, forward, x_rec, out_elems, errcod)

  end subroutine ssd_propagate_elements


  subroutine propagate_elements(t, max_owlt, cart, forward, &
       cart_rec, out_elems, errcod)
    real (kind=wp), intent(in) :: t, max_owlt
    real (kind=wp), intent(in) :: cart(6, 7)
    logical, intent(in) :: forward
    real (kind=wp) :: cart_rec(3)
    real (kind=wp), intent(out) :: out_elems(6)
    integer, intent(out) :: errcod
    
    integer :: nca, j, n_retries = 100000
    logical :: single_step, success
    real (kind=wp) :: t_obj, rho, t_int
    real (kind=wp) :: cart_at_t(6, 7)
    

    ! Init output vars.
    errcod = 0
    
    single_step = .false.
    t_int = 0
    cart_at_t(:, :) = cart(:, :)
    call intgrt(t - max_owlt, single_step, t_int, cart_at_t, nca)
    
    ! Go one step at a time unitl we can interpolate to get 
    ! state at t_obj (time light left object)
    single_step = .true.
    t_obj = 0
    rho = 0
    ! Initialize DT value somethingorother.
    call objsdt(0)
    do j=1, n_retries
       call objsta(forward, t, t_int, cart_at_t(1:3,1), &
            cart_rec(1:3), t_obj, cart_at_t(:,1), rho, success)
       if(success) then
          exit
       else
          call intgrt(t - max_owlt, single_step, t_int, cart_at_t, nca)
       endif
    enddo
    if(.not. success .and. j .eq. n_retries) then
       errcod = 9
       return
    end if

    call intint(t_obj, 7, cart_at_t)
    call pvtce(cart_at_t, gm_sun, cos_eps, sin_eps, out_elems, errcod)
    if (errcod /= 0) then
        return
    endif
    out_elems(3) = out_elems(3) + t_obj
    errcod = 0

  end subroutine propagate_elements


  subroutine compute_src(covar, src)
    ! Given the simple covariance matrix, return the corresponding SRC (square
    ! root covariance). This is done via Cholesky decomposition so that
    ! COV = SRC * SRC**T
    real (kind=wp), intent(in) :: covar(21)
    real (kind=wp), intent(out) :: src(21)
   
    ! Synce the routine we use modifies the input in place, we copy covar into 
    ! src and work on src directly.
    src(:) = covar(:)
    call cov2ri(src, 6)
  end subroutine compute_src
end module ssd
