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 ! etc.
use open_eph_mod, only: open_eph_def, open_eph
use intpar_mod, only: intpar_def
use eop_mod, only: eop_def
use get_unit_mod, only: get_unit
use matvec_util_mod, only: vsmat2mat, vsmat_mat_mul, vsmat_diagmat_mul, vec_mag
use obs_mod, only: max_files, obsmsg, num_obs, &
   obs_ini, obs_get, obs_clear, obs_load, obs_put, obs_sort, obs_write, obs_load, &
   len_obs_code, len_obs_type, len_reject, len_magnitude, len_mag_reject
use estpar_mod, only: estpar_msg, estpar, estpar_def, estpar_name, &
   io_conversion, n_elems, n_param, n_utm_rhs, n_col, apr_sigs
use od_mod, only: od_msg, od_def, od_ini, od_epoch, od_prt_res, od_prt_sum, &
   od_stat_summ, od_clear, od_constraint, od_solve, od_get
use od_converge_mod, only: od_converge, od_converge_def
use mag_mod, only: mag_msg, mag_est
use moid_util_mod, only: moid_rep

implicit none

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

! Status flags for global initializations
integer :: did_od_ini = 0

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

! Misc.
integer :: errcod, inp_unit
character (len=64) :: object = ""
real (kind=wp) :: pleph_start, pleph_stop
real (kind=wp) :: t_ref = 2400000.5_wp  ! Time origin in JED, Use 2400000.5 for MJD
logical :: j2000
character (len=5) :: system
character (len=5)  :: pleph_name ! Names of the plantary ephemerides
character (len=12) :: sbeph_name ! Name of the small body ephemerides
character (len=128) :: temp_str
character (len=80) :: producer = 'PanSTARRS MOPS'
character (len=20) :: new_soln = ' '

! VARLIST options with defaults.
logical :: acc_all_mag = .false.
logical :: mag_verbose = .false.
logical :: asteroid = .true.
logical :: wrt_del_res = .true.


! Small bodiy perturbers
integer, parameter :: max_small_bodies = 128
character (len=40) :: small_bodies(max_small_bodies)
integer :: n_small_bodies


contains

!===============================================================================
  subroutine glue_init
    integer :: errcod
    
    !------ MISC INITIALIZATIONS ---------------------------------------------------
    ! Initialize the system and STDERR device
    call sysini
    call errini
    
    ! 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
    call estpar_def
    call od_def
    call od_converge_def
    
    !------ INITIALIZATIONS --------------------------------------------------------
    ! Get list of estimated parameters
    call estpar(errcod)
    if (errcod /= 0) then
       call errmsg(trim(estpar_msg))
       call errabt(prog_name, "Could not initialize estpar_mod.")
    endif
    estpar_name(1:6) = (/ 'EC ','QR ','TP ','OM ','W  ','IN '/)
    
    !------ MORE INITIALIZATIONS ---------------------------------------------------
    ! Open the ephemeris files and initialize the propagator
    call vladd( 'SBPERT= T', 0, errcod)
    call open_eph(object, pleph_start, pleph_stop, tref=t_ref, quiet=.true., &
         pleph_name=pleph_name, sbeph_name=sbeph_name, system=system, j2000=j2000, &
         req_num_sb=n_small_bodies, req_sb_name=small_bodies)
    call cadini(t_ref, n_col, .false., .true., .true.)
    
  end subroutine glue_init

  !===============================================================================
  subroutine glue_run_diffcor(obsdata,nobs,&
       iod_q_au,&
       iod_e,&
       iod_i_deg,&
       iod_node_deg,&
       iod_arg_peri_deg,&
       iod_time_peri_mjd,&
       iod_epoch_mjd,&
       
       out_q_au,&
       out_e,&
       out_i_deg,&
       out_node_deg,&
       out_arg_peri_deg,&
       out_time_peri_mjd,&
       out_mag,&
       out_epoch_mjd,&
       out_residual,&
       out_covariance,&
       out_converge_code,&
       exterrcod)
    
    character (len=*) :: obsdata
    integer :: nobs
    real (kind=wp) :: iod_q_au
    real (kind=wp) :: iod_e
    real (kind=wp) :: iod_i_deg
    real (kind=wp) :: iod_node_deg
    real (kind=wp) :: iod_arg_peri_deg
    real (kind=wp) :: iod_time_peri_mjd
    real (kind=wp) :: iod_epoch_mjd
    real (kind=wp) :: out_q_au
    real (kind=wp) :: out_e
    real (kind=wp) :: out_i_deg
    real (kind=wp) :: out_node_deg
    real (kind=wp) :: out_arg_peri_deg
    real (kind=wp) :: out_time_peri_mjd
    real (kind=wp) :: out_mag
    real (kind=wp) :: out_epoch_mjd
    real (kind=wp) :: out_residual
    real (kind=wp) :: out_covariance(21)
    integer :: out_converge_code
    integer :: exterrcod
    
    !! Observation handling
    integer :: i_obs, i, n_obs
    character (len=len_obs_code) :: obs_code
    character (len=len_obs_type) :: obs_type
    character (len=len_reject) :: reject
    character (len=len_magnitude) :: mag_str
    character (len=len_mag_reject) :: mag_reject
    real (kind=wp) :: time_utc, dec_sig, ra_sig, dec, ra, mag_sig
    real (kind=wp) :: mag_real
    character (len=1) :: mag_filter
    logical :: mag_avail = .true.
    
    ! OD variables
    real (kind=wp) :: t_elems, epoch, t_int, t_stop
    real (kind=wp), allocatable :: elements(:), initial_elements(:)
    real (kind=wp), allocatable :: pv(:)
    real (kind=wp), allocatable :: final_corr(:), total_corr(:), sigma(:), x(:,:)
    real (kind=wp), allocatable :: src_matrix(:), cov_matrix(:), corr_matrix(:)
    real (kind=wp), allocatable :: elements_5d(:), src_matrix_5d(:), final_corr_5d(:)
    real (kind=wp), allocatable :: elements_apr(:), src_matrix_apr(:), final_corr_apr(:)
    real (kind=wp), allocatable :: elements_4d(:), src_matrix_4d(:), final_corr_4d(:)
    real (kind=wp), allocatable :: elements_pv(:), src_matrix_pv(:), final_corr_pv(:)
    real (kind=wp), allocatable :: elements_6d(:), src_matrix_6d(:), final_corr_6d(:)
    real (kind=wp), allocatable :: ra_res(:), dec_res(:)
    real (kind=wp) :: abs_mag, g_param=0.15_wp, rms_mag, radius_body
    integer :: converge_code
    integer :: n_opt_sat, n_del, n_dop
    real (kind=wp) :: rms_vec(8), alt_param(4), alt_corr(4), scale
    integer :: n_alt_param, nca, converge_code_6d,  converge_code_5d, converge_code_4d, converge_code_apr
    integer :: u=1000
    real (kind=wp) :: runoff
    logical, save :: first_obs=.true.
    logical :: debug= .false.
    real (kind=wp) :: ra0, ra1, dec0, dec1, t0, t1, delta_t, delta_sky_am
    real (kind=wp) :: constraint(6,2), xe(6), geo_dist, norm_dq, rms_resid, geo_vec(3)
    integer, parameter :: max_nca = 100
    real(kind=wp):: ca_data(11,max_nca)
    integer :: ca_body(max_nca), cna, ncap
    real (kind=wp) :: norm_rms_max, half_per
    logical :: postfit_resids, close_approach_scan

    if (debug) print *, "DEBUG: Entering;..." 		   

    ! Clean Slate - Don't screw it up...
    exterrcod = 0
  
    ! Make space
    allocate (&
         initial_elements(n_elems), &
         elements(n_elems), &
         pv(n_elems), &
         final_corr(n_elems), &
         total_corr(n_elems), &
         sigma(n_param), &
         x(6, n_col), &
         src_matrix(n_utm_rhs), &
         cov_matrix(n_utm_rhs), &
         corr_matrix(n_utm_rhs), &
         elements_5d(n_elems), &
         src_matrix_5d(n_utm_rhs), &
         final_corr_5d(n_elems), &
         elements_apr(n_elems), &
         src_matrix_apr(n_utm_rhs), &
         final_corr_apr(n_elems), &
         elements_pv(n_elems), &
         src_matrix_pv(n_utm_rhs), &
         final_corr_pv(n_elems), &
         elements_4d(n_elems), &
         src_matrix_4d(n_utm_rhs), &
         final_corr_4d(n_elems), &
         elements_6d(n_elems), &
         src_matrix_6d(n_utm_rhs), &
         final_corr_6d(n_elems), &
         stat=errcod)
    if (errcod /= 0) then
       exterrcod = 1
       return
    end if
    
    !------ LOAD EXTERNAL OBSERVATIONS
    ! The observation data is assembled into an opaque array of structs by
    ! the calling C program.  A pointer to this block is passed as the obsdata
    ! argument, which is then passed back to the C routine to extract params
    ! from a particular numbered observation.
    
    ! ------ ACCESS EXTERNAL DATABASE AND LOAD OBS_MOD -----------------------------
    ! Loop on database observations, loading them sequentially into the obs_mod 
    if (.not.first_obs) then
       call obs_clear(errcod)
       if (errcod /=0) then
          exterrcod = 2
          return
       endif
    endif
    first_obs=.false.


    ! Set some basic information, that is used for all observation types.
    reject = ' '
    obs_type = 'C'
    ra_sig = 1
    dec_sig = 1
    mag_sig = 1
    mag_filter = 'V'
    mag_reject = ' '
    
    i_obs = 0
    n_obs = 0
    do i = 1, nobs
       
       call mopsod_get_obs(obsdata, i - 1, obs_code, time_utc, ra, ra_sig, dec, dec_sig, mag_real, mag_sig, errcod)
       if (errcod /= 0) then
          exterrcod = 1
          return
       end if
       ra = ra * two_pi / 360
       dec = dec * two_pi / 360
       
       ! Fix apparent bug when mag_sig < 0.05
       if (mag_sig < 0.05) mag_sig = 0.05
       
       ! Push the data into the obs_mod
       i_obs = i_obs + 1
       call obs_load(reject, obs_type, obs_code, time_utc, n_obs, errcod, &
            ra=ra, dec=dec, ra_sig=ra_sig, dec_sig=dec_sig, tref=t_ref)
       if (n_obs /= i_obs) then
          exterrcod = 4
          return
       endif
       
       ! Add photometry to obs_mod if it is in the database.
       ! Now push the database-derived magnitude, filter, sigma and rejection 
       ! flag into the obs_mod. We must first form the magnitude string.
       ! LD 
       write(mag_str, "(x,f5.2,a1)") mag_real, mag_filter
       call obs_put(i_obs, errcod, magnitude=mag_str, mag_sigma_real=mag_sig, &
            mag_reject=mag_reject)
       if (errcod /= 0) then
          exterrcod = 1
          return
       end if
       
    enddo
    
    ! Sort the observations after they are all loaded.
    ! NOTE: Sorting will be much faster if observations are already loaded in
    ! *reverse* chronological order.
    call obs_sort(errcod)
    if (errcod /= 0) then
       exterrcod = 1
       return
    end if
    
!!!$ We can print the obs_load data to file for comparison with the input file. 
!!!$ XXX LD
!!!$ call obs_write("test.obs.out", errcod, make_backup=.false.)
    
    !------ LOAD INTIAL ORBIT -------------------------------------------------
    ! Grab IOD from subroutine parameters.
    ! q (AU), e, i (deg), Om (deg), w (deg), tp (MJD), epoch (MJD)
    ! Elements [e, q (AU), tp (JED), Om (deg), w (deg), i (deg)]
    !iod_time_peri_mjd = iod_time_peri_mjd + 2400000.5
    t_elems = iod_epoch_mjd + t_ref
    
    elements(1:6) = (/ iod_e, iod_q_au, &
         iod_time_peri_mjd+t_ref,&
         iod_node_deg, iod_arg_peri_deg, iod_i_deg /)
    
    ! Internally, tp is measured wrt epoch, but it was loaded as JED, so...
    elements(3) = elements(3) - t_elems
    
    ! Set the Object String
    !object = "Minor Planet (2004 MN4)"
    ! XXX LD see notes
    object = ""
    ! The Object string takes one of the following forms:
    !
    !      Minor Planet (2004 MN4)
    !      Minor Planet 11311 Peleus
    !      Minor Planet 11398 (1998 YP11)
    !      Comet Spahr (P/1998 U4)
    !      Comet 125P/Spacewatch
    !
    ! This could in principle be set to a blank string in most cases. However,
    ! it should at least be set for low numbered asteroids, which may be in the
    ! small body perturber file, so that if the subject asteroid is in the file 
    ! it can be removed from the perturber list to avoid self-perturbation. If it
    ! is set properly then the "asteroid" flag will be set properly by open_eph.
    
    ! Intitialize od_mod
    if (did_od_ini /= 0) then
       call od_clear(errcod)
       if (errcod /= 0) then
          exterrcod = 1
          return
       end if
    endif
    call od_ini(j2000, pleph_start, pleph_stop, t_ref, .false., radius_body, &
         errcod)
    did_od_ini = 1
    if (errcod /= 0) then
       exterrcod = 1
       return
    end if

    ! Compute the weighted midpoint of optical observation times.
    call od_epoch(epoch, errcod)
    if (errcod /= 0) then
       exterrcod = 1
       call errmsg(trim(od_msg))
       call errmsg("Could not compute epoch.")
       return
    endif
    
    ! 1) Get cartesian state
    call cetpv(elements, gm_sun, cos_eps, sin_eps, .false., x, errcod)
    if (errcod /= 0) then
       call cetmsg(errcod, temp_str)
       call errmsg(trim(temp_str))
       exterrcod = 91
       return
    endif

    if (epoch /= t_elems) then
       ! 2) Propagate to epoch
       call intini(t_elems-t_ref, epoch-t_ref, t_ref, x, 1, &
            .false., .false., .false.)
       call intgrt(epoch-t_ref, .false., t_int, x, nca)
       call intint(epoch-t_ref, 1, x)
       
       ! 3) Revert to comet elements
       call pvtce(x, gm_sun, cos_eps, sin_eps, elements, errcod)
       if (errcod /= 0) then
          call pvtmsg(errcod, temp_str)
          call errmsg(trim(temp_str))
          exterrcod = 93
          return
       endif
    endif
    
    ! Save epoch state
    pv = x(:,1)

    ! Get geocentric distance at epoch
    call earth(epoch-t_ref, t_ref, xe)
    geo_vec = pv(1:3) - xe(1:3)
    geo_dist = vec_mag(geo_vec)
    geo_vec = geo_vec/geo_dist
!!$    print "(a, 6es22.13)", "IELEMS: ", elements
!!$    print *, "   X: ", x(1:3,1), vec_mag(x(1:3,1))
!!$    print *, "  XE: ", xe(1:3), vec_mag(xe(1:3))
!!$    print *, "X-XE: ", x(1:3,1) - xe(1:3), vec_mag(x(1:3,1) - xe(1:3))
    

    ! Get some information about the data arc
    call obs_get(1, errcod, ra_real=ra1, dec_real=dec1, time_utc=t1)
    call obs_get(nobs, errcod, ra_real=ra0, dec_real=dec0, time_utc=t0)
    if (errcod /= 0) then
       exterrcod = 84
       call errmsg(trim(obsmsg))
       return
    endif
    delta_t = t1-t0
    delta_sky_am = sqrt( (dec1-dec0)**2 + (cos(dec1)*(ra1-ra0))**2 )* deg_per_rad * 60

    print *, "boo"
    
!!!!!$$$$$$$$$$$$$$$$$$ START DIFFERENTIAL CORRECTIONS $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$

    ! Start with 6D  for longish arcs
    initial_elements = elements ! Save starting point
    converge_code = -9
    if (delta_t > 90) then

       call od_converge(.false., epoch, elements, src_matrix, final_corr, &
            converge_code, iterate= .true., silent=.not.debug, rms_resid=rms_resid)
       if (converge_code > 0 .and. rms_resid > norm_rms_max) converge_code = 0
       if (rms_resid > 4._wp) converge_code = -8

    endif

    if (converge_code /= 0) then
       print *, "foo1"
    
       ! Sometimes try r & rdot constraint. 
       converge_code_4d = -9
       if (geo_dist > 0.2 .and. delta_sky_am < 120 .and. delta_t < 60) then
          print *, "foo2"

          constraint = 0._wp
          constraint(1:3,1) = geo_vec
          constraint(4:6,2) = geo_vec
          call od_constraint(2, 6, constraint, errcod)
          if (errcod /= 0) then
             exterrcod = 85
             call errmsg(trim(od_msg))
             return
          endif
          call od_converge(.false., epoch, pv, src_matrix_pv, final_corr_pv, &
               converge_code_4d, iterate=.false., silent=.not.debug, elem_type='CAR')
          if (converge_code_4d >= 0) &
               call od_converge(.false., epoch, pv, src_matrix_pv, final_corr_pv, &
               converge_code_4d, silent=.not.debug, elem_type='CAR', rms_resid=rms_resid)
          constraint = 0._wp
          call od_constraint(0, n_elems, constraint, errcod)
          call pvtce(pv, gm_sun, cos_eps, sin_eps, elements_4d, errcod)
          if (errcod /= 0) then
             call pvtmsg(errcod, temp_str)
             call errmsg(trim(temp_str))
             exterrcod = 86
             return
          endif

          if (converge_code_4d > 0 .and. rms_resid < norm_rms_max) converge_code_4d = 0
          if (rms_resid > 4._wp) converge_code_4d = -8
          
          
       endif

       ! Always try with e-assumed constraint
       if(converge_code_4d == 0)then
          elements_5d = elements_4d
       else
          elements_5d = initial_elements
       endif

       print *, "foo3"
       constraint = 0._wp
       constraint(1,1) = 1._wp
       call od_constraint(1, n_elems, constraint(:,1), errcod)
       elements_5d = initial_elements
       call od_converge(asteroid, epoch, elements_5d, src_matrix_5d, final_corr_5d, &
            converge_code_5d, iterate= .false., silent=.not.debug)
       if (converge_code_5d >= 0) &
            call od_converge(asteroid, epoch, elements_5d, src_matrix_5d, final_corr_5d, &
            converge_code_5d, iterate= .true., silent=.not.debug, rms_resid=rms_resid)
       print *, "foo3a"
       constraint = 0._wp
       call od_constraint(0, n_elems, constraint, errcod)      
       if (converge_code_5d > 0 .and. rms_resid < norm_rms_max) converge_code_5d = 0
       if (rms_resid > 4._wp) converge_code_5d = -8
       print *, "foo3b"

       ! Second, try with 6D correction
       if (converge_code_5d >= 0)then
          elements_6d = elements_5d
       elseif  (converge_code_4d >= 0) then
          elements_6d = elements_4d
       else
          elements_6d = initial_elements
       endif
       print *, "foo3c"
       call od_converge(asteroid, epoch, elements_6d, src_matrix_6d, final_corr_6d, &
            converge_code_6d, iterate= .false., silent=.not.debug)
       print *, "foo3d"
       if (converge_code_6d >= 0) &
            call od_converge(asteroid, epoch, elements_6d, src_matrix_6d, final_corr_6d, &
            converge_code_6d, iterate= .true., silent=.not.debug, rms_resid=rms_resid)
       print *, "foo3e"
       if (converge_code_6d > 0 .and. rms_resid < norm_rms_max) converge_code_6d = 0
       if (rms_resid > 4._wp) converge_code_6d = -8

       ! Sort things out if 6D corrections failed
       print *, "foo4"
       if(converge_code_6d == 0) then
          
          print *, "6D success."
          converge_code = converge_code_6d
          elements = elements_6d
          src_matrix = src_matrix_6d
          final_corr = final_corr_6d
          
       elseif(converge_code_5d == 0) then 
          
          print *, "5D success."
          converge_code = converge_code_5d
          elements = elements_5d
          src_matrix = src_matrix_5d
          final_corr = final_corr_5d
          call od_solve(epoch, elements_5d, final_corr_5d, src_matrix_5d, norm_dq, errcod)
          
       elseif(converge_code_4d == 0) then 
          
          print *, "4D success."
          converge_code = converge_code_4d
          elements = elements_4d
          call od_solve(epoch, elements_4d, final_corr_4d, src_matrix_4d, norm_dq, errcod)
          src_matrix = src_matrix_4d
          final_corr = final_corr_4d
          
       else
          
          print *, "NO success."
       
       endif

    else

       print *, "AA success."       

    endif

!!!!!$$$$$$$$$$$$$$$$$$ DONE WITH DIFFERENTIAL CORRECTIONS $$$$$$$$$$$$$$$$$$$$$
    print *, "foo4"

    if (converge_code >= 0) then

       ! Normalize elements if needed.
       half_per = pi * sqrt( (elements(2)/(1._wp-elements(1)))**3 / gm_sun )
       if (elements(5) < 0._wp .or. elements(5) >= 360._wp .or. abs(elements(3)) > half_per) then
          call cetpv(elements, gm_sun, cos_eps, sin_eps, .false., x, errcod)
          if (errcod /= 0) then
             call cetmsg(errcod, temp_str)
             call errmsg(trim(temp_str))
             exterrcod = 191
             return
          endif
          call pvtce(x, gm_sun, cos_eps, sin_eps, elements, errcod)
          if (errcod /= 0) then
             call pvtmsg(errcod, temp_str)
             call errmsg(trim(temp_str))
             exterrcod = 192
             return
          endif
       endif
       
       ! Retrieve OD statistics summary from od_mod
       call od_stat_summ(n_opt_sat, n_del, n_dop, rms_vec, errcod)
       if (errcod /= 0) then
          exterrcod = 94
          return
          call errmsg(trim(od_msg))
       endif
       
       ! Print some reporting
       print "(a, 3i3, 6f10.3)", "REPORT:",converge_code_4d, converge_code_5d, converge_code_6d, &
            delta_t, delta_sky_am, geo_dist, delta_sky_am/delta_t, rms_vec(3), rms_vec(8)
       if (debug) print "(a, 6es22.13)", "init_elems: ", initial_elements
       if (debug) print "(a, 6es22.13)", "  4d_elems: ", elements_4d
       if (debug) print "(a, 6es22.13)", "  5d_elems: ", elements_5d
       if (debug) print "(a, 6es22.13)", "  6d_elems: ", elements_6d
       if (debug) print "(a, 6es22.13)", " out_elems: ", elements
       
       ! Compute absolute magnitude
       abs_mag=-99.9_wp
       if (converge_code >= 0) then
          call mag_est(g_param, abs_mag, rms_mag, errcod, accept_all=acc_all_mag, &
               verbose=mag_verbose)
          if (errcod > 0) then
             exterrcod = 95
             call errmsg(trim(mag_msg))
             return
          elseif (errcod < 0) then
             call errmsg(trim(mag_msg))
             abs_mag=-99.9_wp
          endif
       endif
       
       ! Extract the residuals
       ! The arrays ra_res and dec_res could actually be arguments,
       ! rather than local allocatable arrays.  
       ! A key problem is sorting of the residuals. The arrays below
       ! arrived sort in reverse chronological order.
       postfit_resids = .false.
       if (postfit_resids) then ! ON/OFF switch requested by LD.
          allocate (ra_res(nobs), dec_res(nobs), stat=errcod)
          if (errcod /= 0) then
             exterrcod = 101
             return
          end if
          call od_get(errcod, od_ra_res=ra_res, od_dec_res=dec_res)
          if (errcod /= 0) then
             exterrcod = 74
             call errmsg(trim(od_msg))
             return
          endif
          do i=1,nobs
             print "(2f9.4)", ra_res(i), dec_res(i)
          end do
          deallocate(ra_res, dec_res, stat=errcod)
          if (errcod /= 0) then
             exterrcod = 111
             return
          end if
       endif
       
       
       !***** Obtain ten year close approach table for NEAs ******
       ncap=0
       close_approach_scan= .false. ! ON/OFF switch requested by LD.
       if (elements(2) < 1. .and. close_approach_scan) then

          ! Get cartesian state
          call cetpv(elements, gm_sun, cos_eps, sin_eps, .true., x, errcod)
          if (errcod /= 0) then
             call cetmsg(errcod, temp_str)
             call errmsg(trim(temp_str))
             exterrcod = 91
             return
          endif
          
          ! Runout for ten years from epoch (3652.5 days)
          t_stop = epoch-t_ref+3652.5_wp
          t_int = -9.e99_wp
          call intini(epoch-t_ref, t_stop, t_ref, x, n_col, &
               .false., .false., .true.)
          do

             ! Step until reaching a close approach or t_stop
             call intgrt(t_stop, .false., t_int, x, nca)
             if (nca .gt. 0) then

                ! Grab close approach data
                do i= 1,nca

                   if (ncap .lt. max_nca) then

                      ! Grab data for this encounter
                      ncap= ncap + 1
                      call cadget( src_matrix, n_elems, ca_body(ncap), ca_data(:,ncap))
                      if (debug) print '(a,2i5,f10.2,f9.5,f8.3,f8.4,f8.4,es23.12)',&
                           "CLAP ", ncap, ca_body(ncap), &
                           (ca_data(1,ncap) - (epoch-t_ref))/365.26_wp, ca_data(2:6,ncap)

                   else

                      ! Stopped before over-running arrays
                      call errmsg("Too many close approaches.")

                   endif

                enddo

             endif
             if (t_int .ge. t_stop) exit
          enddo
          call intfin


       endif !Close approach section
       ! At the end of the close appraoch block, there are NCAP elements loaded
       ! into the arrays ca_body and ca_data
       !   ca_body(1..ncap)        Index of Close Approach Body. 
       !                           (Planets 1-9, moon=10, Ceres, Pallas, Vesta are 15-17)
       !   ca_data(1..11, 1..ncap)  Close approach data, as follows:
       !      ca_data(1, 1..ncap):  Time of CA (days relative to ref time).
       !      ca_data(2, 1..ncap):  Nominal close approach distance (AU).
       !      ca_data(3, 1..ncap):  Relative velocity at close approach (km/s).
       !      ca_data(4, 1..ncap):  Minimum 3-sigma close approach distance (AU).
       !      ca_data(5, 1..ncap):  Maximum 3-sigma close approach distance (AU).
       !      ca_data(6, 1..ncap):  Uncertainty in CA time, 3-sigma (min).
       !      ca_data(7, 1..ncap):  Target plane ellipse semi-major axis (km).
       !      ca_data(8, 1..ncap):  Target plane ellipse semi-minor axis (km).
       !      ca_data(9, 1..ncap):  Target plane ellipse angle from range (deg).
       !      ca_data(10,1..ncap):  Minimum number of sigmas for impact.
       !      ca_data(11,1..ncap):  Probability of impact.
 
    endif

!$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    
if(debug .and. converge_code >=0 )then          
   !------ POSTFIT REPORTS --------------------------------------------------------
   ! LD
   write(*, "(a)") "1*******  ESTIMATION SUMMARY  *******"
   
   ! Square the SRC and obtain the sigmas. Scale covariance by io_conversion.
   call ri2cov(src_matrix, n_param, sigma, cov_matrix, n_param, n_param)
   call covtcr(cov_matrix, sigma, n_param, corr_matrix)
   call vsmat_diagmat_mul(cov_matrix, io_conversion, "PRE", cov_matrix, errcod)
   if (errcod /= 0) call errabt(prog_name, "Failure in vsmat_diagmat_mul.")
   call vsmat_diagmat_mul(cov_matrix, io_conversion, "POST", cov_matrix, errcod)
   if (errcod /= 0) call errabt(prog_name, "Failure in vsmat_diagmat_mul.")
   
   ! Output estimation statistics
   !LD 
   call prtutm(out_covariance, n_param, estpar_name, "Covariance Matrix:")
   !call prtutm(corr_matrix, n_param, estpar_name, "Correlation Matrix:")
   call prtrow(sigma * io_conversion, n_param, estpar_name, "Formal Sigmas:")
   call prtrow(io_conversion, n_param, estpar_name, "I/O Conversion:")
   
   ! Output correction information
   call prtrow(final_corr*io_conversion(1:n_elems), n_elems, estpar_name, &
        "Final Correction:")
   total_corr = elements - initial_elements
   call prtrow(total_corr*io_conversion(1:n_elems), n_elems, estpar_name, &
        "Total Correction:")
   
   ! Output correction information
   call prtrow(final_corr*io_conversion(1:n_elems), n_elems, estpar_name, &
        "Final Correction:")
   total_corr = elements - initial_elements
   call prtrow(total_corr*io_conversion(1:n_elems), n_elems, estpar_name, &
        "Total Correction:")
   
   ! Output statistical summary
   ! LD
   call od_prt_sum(errcod, object, new_soln, producer, pleph_name, sbeph_name, &
        small_bodies, n_small_bodies)
   if (errcod /= 0) call errabt(prog_name, trim(od_msg))
   
   ! Compute auxiliary elements for elliptic orbits
   if (elements(1) < 1.0_wp) then
      call astelt(elements(:6), src_matrix, n_elems, alt_param, alt_corr)
   else
      alt_param(1) = 9.9e9_wp ! This just acts as a flag
   endif
   
   ! Decide how many auxiliary elements to report
   if (asteroid .and. elements(1) < 1.0_wp) then
      n_alt_param = 4
   elseif(alt_param(1) < 100._wp) then
      n_alt_param = 3
   else
      n_alt_param = 0
   endif
   
   ! Rescale the sigmas based on the number of estimated parameters and the RMS
   if (2.0_wp * n_opt_sat + n_dop + n_del - n_elems > 0) then
      scale = sqrt(rms_vec(8)**2 * (2.0_wp * n_opt_sat + n_dop + n_del) / &
           (2.0_wp * n_opt_sat + n_dop + n_del - n_elems))
   else
      call errmsg("*** WARNING: There are no excess observations for LS solution.")
      call errmsg("Using scale factor 1.0.")
      scale = 1
   endif
   sigma = scale * sigma
   alt_corr = scale * alt_corr
   
   ! Output parameter summary
   ! LD
   call prtels(system, new_soln, epoch, elements*io_conversion, sigma, &
        estpar_name, n_elems, "Post-Fit Std.Dev.", n_alt_param, alt_param, alt_corr)
   
   ! Write absolute magnitude result
   ! LD
   if(abs_mag > -99.0_wp) then
      write(*, "(a,f7.2,a,f7.2)") "0 Photometry: H =", abs_mag, ", H_RMS =", rms_mag
   endif
   
   ! Compute and print MPC's 'U' parameter
   ! LD
   call orbunc(alt_param(2), elements(1), alt_corr(2), sigma(3), runoff, u)
   write (*, "(a,i3,a,f14.2)") "0 Uncertainty: U =", u, ", Runoff=", runoff
   
   ! Generate MOID report
   ! LD
   write (*, "(a)") "0**** MOID REPORT ****"
   ! LD
   if (elements(1)<1._wp) call moid_rep(epoch, elements, small_bodies, n_small_bodies)
   
   ! Residuals to STDOUT
   ! LD
   call od_prt_res(6, errcod, system, wrt_del_res)
   
   call obs_write("test.obs.out", errcod, make_backup=.false.)
   if (errcod /= 0) call errabt(prog_name_version, trim(obsmsg))

   print *, "geo_dist: ", geo_dist
   print *, "delta_t: ", delta_t
   print *, "delta_ra: ", ra1*deg_per_rad, ra0*deg_per_rad, (ra1-ra0)*deg_per_rad
   print *, "delta_dec: ", dec1*deg_per_rad, dec0*deg_per_rad, (dec1-dec0)*deg_per_rad
   print *, "delta_sky: ", delta_sky_am
  
endif ! DEBUG


out_covariance(1:21) = src_matrix(1:21)
out_q_au = elements(2)
out_e = elements(1)
out_i_deg = elements(6)
out_node_deg = elements(4)
out_arg_peri_deg = elements(5)
out_time_peri_mjd = elements(3) + epoch - t_ref
out_mag = abs_mag            
out_epoch_mjd = epoch - t_ref
!out_residual = rms_vec(8)         ! normalized optical + radar
!out_residual = rms_vec(4)         ! weighted RMS
out_residual = rms_vec(3)         ! unweighted total optical RMS
out_converge_code = converge_code

deallocate (&
     initial_elements, &
     elements, &
     pv, &
     final_corr, &
     total_corr, &
     sigma, &
     x, &
     src_matrix, &
     cov_matrix, &
     corr_matrix, &
     elements_5d, &
     src_matrix_5d, &
     final_corr_5d, &
     elements_apr, &
     src_matrix_apr, &
     final_corr_apr, &
     elements_4d, &
     src_matrix_4d, &
     final_corr_4d, &
     elements_pv, &
     src_matrix_pv, &
     final_corr_pv, &
     elements_6d, &
     src_matrix_6d, &
     final_corr_6d, &
     stat=errcod)
if (errcod /= 0) then 
   exterrcod = 96
   return
endif

end subroutine glue_run_diffcor

end module ssd



!!$    ! Code -5 indicates hyperbolic asteroid
!!$    if (converge_code < 0 .and. initial_elements(1) > 1.1) then
!!$       call errmsg("Entering apriori constraint...")
!!$       call od_clear(errcod)
!!$       if (errcod /= 0) then
!!$          exterrcod = 1
!!$          return
!!$       end if
!!$       apr_sigs(1) = 0.001_wp * initial_elements(1)
!!$       apr_sigs(2) = 0.001_wp * initial_elements(2)
!!$       apr_sigs(3) = 0.001_wp * initial_elements(3)
!!$       call od_ini(j2000, pleph_start, pleph_stop, t_ref, .false., radius_body, &
!!$            errcod)
!!$       if (errcod /= 0) then
!!$          exterrcod = 1
!!$          return
!!$       end if
!!$       elements_apr = initial_elements
!!$       print *, "APR_DEBUG"
!!$       call od_converge(.false., epoch, elements_apr, src_matrix_apr, final_corr_apr, &
!!$            converge_code_apr, iterate= .true., silent=.not.debug)
!!$       print *, "APR_DEBUG", converge_code_apr
!!$       apr_sigs(1) = -1._wp
!!$       apr_sigs(2) = -1._wp
!!$       apr_sigs(3) = -1._wp
!!$    endif
