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 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 :: 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 glue_init
        integer :: errcod

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

   
      subroutine glue_run_simple(t_epoch,elems,src_matrix,obscod,t_eph_ut,h_v,&
            out_ra,out_dec,out_mag,exterrcod)
        real (kind=wp) :: t_epoch, elems(6)
        real (kind=wp) :: src_matrix(21)
        character (len=3) :: obscod
        real (kind=wp) :: t_eph_ut, h_v
        real (kind=wp) :: out_ra, out_dec, out_mag

        ! Apparent magnitude variables
        real (kind=wp) :: x_topo(3), rho, r, v_mag, beta
        real (kind=wp) :: mag_param(5) = 0._wp
        logical :: temp_logical

        integer :: exterrcod

        ! Ephemeris variables
        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) :: t_eph_et, et_minus_ut, x_rec_ssb(6), x_earth_ssb(6), x_rec(3)
        real (kind=wp) :: sky_cov(2,2)
        real (kind=wp) :: x(6, 7), radec_partial(2, 6), w(2, 6)
        logical :: forward, success, single_step
        real (kind=wp) :: max_owlt
        real (kind=wp) :: t_int, 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

        !------ HOUSEKEEPING -----------------------------------------------------------
        integer :: errcod
        exterrcod = 0

        ! Internally, tp is measured wrt epoch
        elems(3) = elems(3) - t_epoch

        ! Times are always measured wrt t_ref, so assume inputs in MJD
        !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) then
          ! error in stn
          exterrcod = 1
          return
        endif

        ! 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
          ! error in conversion to J2000
          exterrcod = 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
          exterrcod = 3
          return
        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
          exterrcod = 4
          return
        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 apparent magnitude.
        x_topo =  x(:3,1) - x_rec(:3)
        rho = sqrt(dot_product(x_topo, x_topo))
        r = sqrt(dot_product(x(:3,1), x(:3,1)))
        mag_param(1) = h_v
        mag_param(2) = 0.15_wp ! G

        ! One option is to use the subroutine MAGNIT:
        !call magnit( x_topo, x(:3,1), rho, r, mag_param, .false., &
        !   v_mag, temp_logical)

        ! Or, a bit more simply, just compute V inline:
        beta = acos( dot_product(x_topo, x(:3,1))/ (r*rho) )
        v_mag = tan( 0.5d0 * beta )
        v_mag = mag_param(1) + 5.d0*log10(rho) + 5.d0*log10(r) - &
          2.5d0 * log10( (1.d0-mag_param(2)) * exp(-3.33d0 * v_mag**0.63d0) + &
          mag_param(2) * exp(-1.87d0 * v_mag**1.22d0) )






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

        out_mag = v_mag
        out_ra = ra_comp * deg_per_rad
        out_dec = dec_comp * deg_per_rad

      end subroutine

end module ssd
