!===============================================================================
!
! PS_OD -  A program that is intended to demonstrate some uses of the 
!          JPL Rapid Comet and Asteroid Orbit Software Library
!
!-------------------------------------------------------------------------------
!  v1.0  Mar 01, 2005    Steve Chesley
!
!  $Id:$
!===============================================================================
program ps_od

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
use obs_mod, only: max_files, obsmsg, num_obs, &
   obs_ini, obs_get, 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
use od_mod, only: od_msg, od_def, od_ini, od_epoch, od_prt_res, od_prt_sum, &
   od_stat_summ
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 =====================================================

! Program Version and Usage Strings:
character (len=*), parameter :: prog_name = "ps_od"
character (len=*), parameter :: version = "1.0"
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 = 2450000.5_wp ! Time origin in JED, 1995-10-10.0
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 = ' '

! 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

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

! OD variables
real (kind=wp) :: t_elems, epoch, t_int
real (kind=wp), allocatable :: elements(:), initial_elements(:)
real (kind=wp), allocatable :: pv(:), initial_pv(:)
real (kind=wp), allocatable :: final_corr(:), total_corr(:), sigma(:), x(:,:)
real (kind=wp), allocatable :: src_matrix(:), cov_matrix(:), corr_matrix(:)
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
integer :: u=1000
real (kind=wp) :: runoff

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

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

!------ 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

!------ LOAD EXTERNAL OBS DATABASE ---------------------------------------------
! IDEA: Load the observations from a standard .obs file and access the obs_mod
! it as though it is an external database. Transfer this "external" info
! into a separate obs_mod data area using the obs_load routine, which was
! developed for PanSTARRS use. 

! Because we need two separate obs_mod data areas (one to act as the PS 
! external DB and one to be loaded by PS for OD) we need to override the 
! single area default (max_files=1). This statement can be skipped if only 
! one area is needed, which will be the case for MOPS operations.
max_files = 2 

! Load the file into the 2nd obs_mod data area:
call obs_ini("test.obs", errcod, file_num=2, tref=t_ref)
if (errcod /= 0) call errabt(prog_name_version, trim(obsmsg))
print *, "Database has been loaded."

! ------ ACCESS EXTERNAL DATABASE AND LOAD OBS_MOD -----------------------------
! Loop on "database" observations, loading them sequentially into the obs_mod 
i_obs = 0
do i=1,num_obs(2)

  ! Retrieve some basic information, that is used for all observation types.
  call obs_get(i, errcod, file_num=2, &
     reject=reject, obs_type=obs_type, obs_code=obs_code, time_utc=time_utc)
  if (errcod /= 0) call errabt(prog_name_version, trim(obsmsg))

  select case(obs_type)
  case("R","P")
    
    ! Radar observations
    call errmsg("WARNING: Skipping radar observations for now.")
    cycle
    
  case("V","S")
    
    ! Roving or satelliate observations
    call errmsg("WARNING: Skipping ROV/SAT observation for now.")
    cycle
    
  case default
    
    ! *** Optical observations ***

    ! 1a) Get the optical data from the "database" (file_num=2)
    call obs_get(i, errcod, file_num=2, &
       ra_real=ra, dec_real=dec, ra_sigma_real=ra_sig, dec_sigma_real=dec_sig)
    if (errcod /= 0) call errabt(prog_name_version, trim(obsmsg))

    ! 1b) Get photometry from database
    call obs_get(i, errcod, file_num=2, &
       magnitude=mag_str, mag_reject=mag_reject, mag_sigma_real=mag_sig)
    if (errcod /= 0) call errabt(prog_name_version, trim(obsmsg))
    mag_avail = len_trim(mag_str)>0
    if (mag_avail) then
      ! Parse magnitude string to extract the magnitude and the filter.
      ! The only allowed filters are BVRI, where " " is taken as "B" and 
      ! "v" is taken as "V". Also JHK are ignored silently. Anything else will
      ! cause the mag_mod to complain during computation of absolute magnitude.
      read(mag_str, "(1x,f5.0,a1)") mag_real, mag_filter
    endif
    
    ! 2a) Push the data back into the obs_mod, this time using obs_load to
    ! put it into the 1st data area.
    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) call errabt(prog_name_version, &
       "Bad observation counting somewhere.")

    ! 2b) Add photometry to obs_mod if it is in the database.
    if (mag_avail) then
      ! Now push the database-derived magnitude, filter, sigma and rejection 
      ! flag into the obs_mod. We must first form the magnitude string.
      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) call errabt(prog_name_version, trim(obsmsg))
    endif

  end select

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) call errabt(prog_name_version, trim(obsmsg))

! We can print the obs_load data to file for comparison with the input file. 
call obs_write("test.obs.out", errcod, make_backup=.false.)
if (errcod /= 0) call errabt(prog_name_version, trim(obsmsg))

!------ 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 '/)
estpar_name(1:6) = (/ ' X ',' Y ',' Z ','VX ','VY ','VZ '/)

! Dimension relevant arrays
allocate (&
   initial_elements(n_elems), &
   elements(n_elems), &
   initial_pv(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), &
   stat=errcod)
if (errcod /= 0) call errabt(prog_name, " Memory allocation failure.")

!------ LOAD INTIAL ORBIT -------------------------------------------------
! Grab an initial orbit from file.
inp_unit = get_unit()
open(inp_unit, 'test.inp')
do

  read(inp_unit, "(a)", iostat= errcod) temp_str
  if (errcod < 0) exit  ! EOF
  if (errcod > 0) call errabt(prog_name_version, "Bogus input file.")
  if (len_trim(temp_str)==0) cycle

  if (index(temp_str, "EPOCH")>0) then
    read(inp_unit, *) t_elems
    t_elems = t_elems
  endif

  ! Elements [e, q (AU), tp (JED), Om (deg), w (deg), i (deg)]
  if (index(temp_str, "ELEMENTS")>0) then
    read(inp_unit, *) elements(1:3)
    read(inp_unit, *) elements(4:6)
  endif

enddo
close(inp_unit)

! Output the inputs
print *
print *, "Input Epoch and Elements"
print "(f11.2,a)", t_elems, " (JED)"
print "(6es20.10)", elements

! 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)"
! 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.

!------ 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, asteroid=asteroid)

! Intitialize od_mod
!!$ RADB shold be added to the VARLIST if it is needed for PP radar obs
call od_ini(j2000, pleph_start, pleph_stop, t_ref, .false., radius_body, &
   errcod)
if (errcod /= 0) call errabt(prog_name, trim(od_msg))

! Compute the weighted midpoint of optical observation times.
call od_epoch(epoch, errcod)
if (errcod /= 0) then
  call errmsg(trim(od_msg))
  call errabt(prog_name, "Could not compute epoch.")
endif

! 1) Get cartesian elements
call cetpv(elements, gm_sun, cos_eps, sin_eps, .false., x, errcod)
if (errcod /= 0) then
  call cetmsg(errcod, temp_str)
  call errabt(prog_name_version, trim(temp_str))
endif

! Propagate the elements to the epoch, if necessary
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)

endif

!------ PERFORM OD -------------------------------------------------------------
write(*, "(a)") "0****** DIFFERENTIAL CORRECTIONS  *******"

! Iterate until convergence. (This routine may not be appropriate for
! PanSTARRS ops, but should be good for simulations.)
pv  = x(:,1)
print *
print *, "Input Position and Velocity"
print "(6es20.10)", pv
initial_pv = x(:,1) ! Save starting point
call od_converge(asteroid, epoch, pv, src_matrix, final_corr, &
   converge_code, iterate= .true., elem_type = 'CAR')

! Compute absolute magnitude
abs_mag=-99.9_wp
if (est_mag .and. asteroid) then
  call mag_est(g_param, abs_mag, rms_mag, errcod, accept_all=acc_all_mag, &
     verbose=mag_verbose)
  if (errcod > 0) then
    call errabt(prog_name, trim(mag_msg))
  elseif (errcod < 0) then
    call errmsg(trim(mag_msg))
    abs_mag=-99.9_wp
  endif
elseif (est_mag) then
  call errmsg("Cannot EST_MAG for comets.") 
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) call errabt(prog_name, trim(od_msg))

!------ POSTFIT REPORTS --------------------------------------------------------
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
call prtutm(cov_matrix, 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 = pv - initial_pv
call prtrow(total_corr*io_conversion(1:n_elems), n_elems, estpar_name, &
   "Total Correction:")

! Output statistical summary
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
n_alt_param = 0

! 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
call prtels(system, new_soln, epoch, pv*io_conversion, sigma, &
   estpar_name, n_elems, "Post-Fit Std.Dev.", n_alt_param, alt_param, alt_corr)

! Write absolute magnitude result
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
!$$ if (asteroid) then
!$$   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
!$$ endif
!$$ 
!$$ ! Generate MOID report
!$$ write (*, "(a)") "0**** MOID REPORT ****"
!$$ call moid_rep(epoch, elements, small_bodies, n_small_bodies)

! Residuals to STDOUT
call od_prt_res(6, errcod, system, wrt_del_res)

end program ps_od
