!===============================================================================
!
! PS_EPH - A program that is intended to demonstrate some uses of the 
!          JPL Rapid Comet and Asteroid Orbit Software Library
!
!-------------------------------------------------------------------------------
!  v1.0  Nov. 23, 2004    Steve Chesley
!
!  $Id:$
!===============================================================================
program ps_eph

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

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

! 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

! External function


! Misc.
integer :: errcod, k, inp_unit
character (len=64) :: object
real (kind=wp) :: eph_start, eph_stop
real (kind=wp) :: t_ref = 2450000.5_wp ! Time origin in JED, 1995-10-10.0
logical :: j2000

! Ephemeris variables
real (kind=wp) :: t_eph_ut
character (len=3) :: obscod
real (kind=wp) :: site_coords(3)
real (kind=wp) :: t_eph_et, et_minus_ut, x_rec_ssb(6), x_earth_ssb(6), x_rec(3)
real (kind=wp) :: t_epoch, elems(6), sky_cov(2,2)
real (kind=wp) :: x(6, 7), src_matrix(21), radec_partial(2, 6), w(2, 6)
logical :: forward, success, single_step
real (kind=wp) :: max_owlt
real (kind=wp) :: t_int, rho, t_obj, t_stop
integer :: nca ! Number of close approaches
real (kind=wp) :: ra_comp, dec_comp, radec_part(2,4)
character (len=128) :: temp_str
real (kind=wp) :: temp_vec(6), temp_real




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

!------ MISC INITIALIZATIONS ---------------------------------------------------
! Initialize the system and STDERR device, verify execution authorization.
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

!------ 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, 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))
write (*, "('0EOP File Label: ',a/' EOP Creation Date: ',a)") &
   trim(eoplbl), trim(eoptim)

!------ LOAD INPUTS FOR TEST ---------------------------------------------------
! Presumably these will be coming from an SQL query, or something like that...
inp_unit = get_unit()
open(inp_unit, file='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_epoch
  endif

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

  ! Sqrt Covariance Matrix
  if (index(temp_str, "SRC")>0) then
    read(inp_unit, *) src_matrix(1:3)
    read(inp_unit, *) src_matrix(4:6)
    read(inp_unit, *) src_matrix(7:9)
    read(inp_unit, *) src_matrix(10:12)
    read(inp_unit, *) src_matrix(13:15)
    read(inp_unit, *) src_matrix(16:21)
  endif

  if (index(temp_str, "OBSCODE")>0) then
    read(inp_unit, *) obscod
  endif

  if (index(temp_str, "TIME")>0) then
    read(inp_unit, *) t_eph_ut
  endif

enddo
close(inp_unit)

print *
print *, "Input Epoch and Elements"
print "(f11.2,a)", t_epoch, " (JED)"
print "(6es20.10)", elems

!------ HOUSEKEEPING -----------------------------------------------------------

! Internally, tp is measured wrt epoch, but it was loaded as JED, so...
elems(3) = elems(3) - t_epoch

! Times are always measured wrt t_ref
t_eph_ut = t_eph_ut - t_ref
t_epoch = t_epoch - t_ref

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

! Compute the ephemeris time and the various observer position/velocity vectors
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) call errabt(prog_name_version, trim(stn_pos_msg))

! Convert Elements to Cartesian State in J2000 Equatorial Frame
call cetpv(elems, gm_sun, cos_eps, sin_eps, .true., x, errcod)
if (errcod /= 0) then

  ! This is our old style (F77) error handling approach:
  call cetmsg(errcod, temp_str)
  call errabt(prog_name_version, trim(temp_str))

endif

! Ensure solution epoch is not off the ephemeris.
if (t_epoch + t_ref < eph_start .or. t_epoch + t_ref > eph_stop) then
  write (temp_str,"(' eph_start=',f13.4,' eph_stop=',f13.4,' epoch=',f13.4)") &
     eph_start, eph_stop, t_epoch+t_ref
  call errmsg(trim(temp_str))
  call errabt(prog_name, 'No ephemeris information for EPOCH.')
endif

! Ensure ephemeris time is not off the ephemeris.
if (t_eph_et + t_ref < eph_start .or. t_eph_et + t_ref > eph_stop) then
  write (temp_str,"(' eph_start=',f13.4,' eph_stop=',f13.4,' t_eph=',f13.4)") &
     eph_start, eph_stop, t_eph_et+t_ref
  call errmsg(trim(temp_str))
  call errabt(prog_name, 'No ephemeris information for EPOCH.')
endif

!------ PROPAGATE TO EMIT TIME -------------------------------------------------

! Which direction are we going?
forward = t_eph_et > t_epoch

! Compute 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.
if (forward) then

  ! 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) < 1.0_wp) then
    max_owlt = (1._wp + elems(2) * &
       (1.0_wp + elems(1)) / (1.0_wp - elems(1))) / v_light
  else
    max_owlt = 1.0_wp
  endif

  t_stop = 9.e99_wp

else

  ! When going backward use zero.
  max_owlt = 0._wp
  t_stop = -9.e99_wp

endif

! Intitialize integrator and get to a time "close" to the observation.
call intini(t_epoch, t_stop, t_ref, x, 7, .false., .false., .false.)
single_step = .false.
call intgrt(t_eph_et - max_owlt, single_step, t_int, x, nca)

! Go one step at a time unitl we can interpolate to get 
! state at t_obj (time light left object)
single_step = .true.
do
  call objsta(forward, t_eph_et, t_int, x(1:3,1), &
     x_rec(1:3), t_obj, x(:,1), rho, success)
  if (success) then
    exit
  else
    call intgrt(t_eph_et - max_owlt, single_step, t_int, x, nca)
  endif
enddo
call intint(t_obj, 7, x)

! Compute predicted positions and partials wrt to current state.
call radec(x(:3,1), x_rec(:3), t_eph_et, .true., &
   ra_comp, dec_comp, radec_part(1,:), radec_part(2,:))
if (ra_comp < 0.0_wp) ra_comp = ra_comp + two_pi

! Compute partials wrt to initial elements, a 2 x 6 matrix
radec_partial = matmul(radec_part(:,:3), x(:3,2:7))

! Output Time and (RA,DEC)
temp_str= ""
k=0
call fmtstr("0Prediction Time (UTC)= ", temp_str, k)
call fmtcds( t_eph_ut + t_ref, 5, temp_str, k)
write(*, "(a)") temp_str(:k)
temp_str= ""
k=0
call fmtstr(" (RA,DEC)= (", temp_str, k)
call fmtdps( ra_comp*deg_per_rad, 12, temp_str, k)
call fmtstr(", ", temp_str, k)
call fmtdps( dec_comp*deg_per_rad, 12, temp_str, k)
call fmtstr(") (deg)", temp_str, k)
write(*, "(a)") temp_str(:k)

! Compute and output uncertainty information
print *
print *, "Elems Sqrt. Covariance"
print "(6es20.10)",  transpose(vsmat2mat(src_matrix, 6))
print *
print *, "Partial Derivatives: d(RA,DEC)/d(X_t):"
print "(2es20.10)",  radec_part(:,:3)
print *
print *, "Partial Derivatives: d(X_t)/d(E_0):"
print "(6es20.10)",  transpose(x(:3,2:7))
print *
print *, "Partial Derivatives: d(RA,DEC)/d(E_0):"
print "(2es20.10)",  radec_partial

! d(RA,DEC)/d(E_0) * Elements Sqrt Cov = (RA,DEC) Sqrt Cov
call vsmat_mat_mul(src_matrix, radec_partial, 'PRE', w, errcod)
if (errcod > 0) call errabt(prog_name_version, "Error in vsmat_mat_mul.")
sky_cov = matmul(w,transpose(w)) ! Square it up to get full covariance

print *
print *, "(RA,DEC) Covariance (radians):"
print "(2es20.10)",  sky_cov

print *
print "(a,f8.3,1x,f8.3)", " (RA,DEC) uncertainty (arcsec): ", &
   sqrt(sky_cov(1,1))*sec_per_rad, &
   sqrt(sky_cov(2,2))*sec_per_rad

end program ps_eph
