;+
; NAME:
;   oc_fg2lla.pro
;
; PURPOSE:
;   Calculate the longitude and latitude for a given time and f, g
;
; DESCRIPTION:
;   Calculate the longitude and latitude for a given time and f, g
;
; CALLING SEQUENCE:
;   lonlatalt = oc_fg2lla(et,targ,radec_star, fgh, $
;      obs=obs,abcorr=abcorr,frame=frame)
;
; INPUTS:
;   et - the ET at which you want to know the coordinates (may be an
;        array).  Units are seconds of TDB (barycentric dynamical
;        time) after J2000.
;   targ = target code
;   radec_star - vector of;
;    A - right ascention in radians
;    D - declination in radians
;   fg - vector of [f, g] of TANGENT POINT - TARGET CENTER
; OPTIONAL INPUT PARAMETERS:
;
; KEYWORDS
;   obs - the keyword argument that indicates the observer location, 
;         default to Earth center (399).
;   abcorr  - aberation correction.  Default 'LT'
;   frame   - frame of state, ra, dec.  Default 'J2000'
;
; OUTPUTS:
;  lonlatalt - array of [east longitude (radian), 
;                        geodetic latitude (radian), 
;                        0]
; 
;  REVISON HISTORY:
;   2006 Jan 11 Leslie Young.  Based on single_eph
;-

function oc_fg2lla, et,targ, radec_star, fg, found, $
                       obs=obs,abcorr=abcorr,frame=frame

    ; set keywords
    if not keyword_set(obs) then obs = 399 ; Earth
    if not keyword_set(abcorr) then abcorr = 'LT' ; light-time only
    if not keyword_set(frame) then frame = 'J2000' ; 
    
    ; rotation matrices for frame/fgh and frame/EARTH
    ra  = radec_star[0]
    dec = radec_star[1]
    r_x2f = oc_x2f_rotmat(ra, dec)
    cspice_pxform, "IAU_EARTH", frame, et, r_e2x
    
    ;  positn   the double precision position 3-vector position
    ;           of an observer with respect to the center of an
    ;           ellipsoid expressed in the body fixed coordinates of 
    ;           the ellipsoid
    ; passed f[tang - target]
    ; calculate f[target-earth center]
    ; want positn = rotation of f[tang-earth] = f[tang-targ]+f[targ-earth]

    fgh_targ_earth = oc_naiffgh(et, targ, radec_star, $
                                obs=obs,abcorr=abcorr,frame=frame)
    fgh_tang_targ = [fg[0], fg[1], 0. ]
    fgh_tang_earth = fgh_tang_targ + fgh_targ_earth
    ; Rotate the vector fgh_tang_earth (position of "viewpoint"
    ; in fgh coordinates w/ r. to Earth center) to Earth coord.
    ; Usual matrix multiplication is y = R ## x (x and y are size [1,3])
    ; Equivalently, we can write y = transpose(R) # x (x,y are size [3])
    ;
    ; To rotate from fgh to xyz, multiply by inverse(r_x2f):
    ; 
    ; xyz_tang_earth = transpose( inverse(r_x2f) ) # fgh_tang_earth
    ;
    ; Since r_x2f is a rotation matrix, transpose( inverse(r_x2f) )=r_x2f.
    r_e2f = r_x2f ## r_e2x 
    positn = r_e2f # fgh_tang_earth

    ;  view     the double precision direction 3-vector eminating from
    ;           'positn'
    ;
    ; We are looking back at Earth (-h direction)
    view = r_e2f # [0.d, 0.d, -1.d]

    ; a,       the scalar double precision values of the ellipsoid's 
    ; b,       triaxial radii ellipsoid, where:
    ; c
    ;          'a' is length in kilometers of the semi-axis of the ellipsoid
    ;           parallel to the x-axis of the body-fixed reference frame 
    ;
    ;          'b' is length in kilometers of the semi-axis of the ellipsoid
    ;           parallel to the y-axis of the body-fixed reference frame 
    ;
    ;          'c' is length in kilometers of the semi-axis of the ellipsoid
    ;           parallel to the z-axis of the body-fixed reference frame 
    cspice_bodvar, obs, 'RADII', abc
    a = abc[0]
    b = abc[1]
    c = abc[2]

    cspice_surfpt, positn, view, a, b, c, point, found
   
print, 'point    ', point
 
    ;found   a boolean indicating whether the intersection
    ;          between the ellipse and 'view' exists (TRUE) or
    ;          not (FALSE)
    if not (found) then begin
        lonlatalt = [0.d, 0.d, 0.d]
        return, lonlatalt
    end

    ; point   a double precision 3-vector defining the location
    ;         on the ellipsoid at which the 'view' intercepts
    ;         the ellipsoid if the interception exists, 'point' returns
    ;         (0.d, 0.d, 0.d) if 'u' does not intersect the ellipsoid

    ; Convert rectangular, bodycentered vector to lon, lat
    re = a        ; equatorial radius
    f = (a-c)/a   ; flattening coefficient
    cspice_recgeo, point, re, f, lon, lat, alt
    lonlatalt = [lon, lat, alt]
    return, lonlatalt

end


pro oc_fg2lla_test

    cspice_str2et, '2005 JUL 11 03:36:16.334', et
    targ = 901L
;    radec_star = [ raparse('17 28 55.0167'), decparse('-15 00 54.726')]
;    lon_in = decparse('-70:48:52.7')
;    lat_in = decparse('-30:09:55.5')
;    alt_in = 0.d ; not 2.386
    ; test it with f=g=0
    radec_star = [ raparse('17 28 55.020'), decparse('-15 00 54.782')]
    lon_in = decparse('-80:51:13.771')
    lat_in = decparse('-15:06:57.549')
    alt_in = 0.d
    lonlatalt_in = [ lon_in, lat_in, alt_in ]  
    fgh_targ_site = oc_naiffgh(et,targ, radec_star, lonlatalt=lonlatalt)
    fg_tang_targ = -fgh_targ_site[0:1]
    stop
    lla = oc_fg2lla(et,targ, radec_star, fg_tang_targ, found)
    lon = lla[0]
    lat = lla[1]
    decstr, lon, 3, s & print, 'Charon lon :', s
    decstr, lat, 3, s & print, 'Charon lat :', s
    

    ; are they calculating the same body-coord xyz?
    cspice_bodvar, 399L, 'RADII', abc
    e = abc[0]
    f = (e-abc[2])/e
    cspice_georec,lon_in, lat_in, alt_in, e, f, pos_in
    print, 'point_in', pos_in
    
end

; Go through pieces step by step
pro oc_fg2lla_dev

    if (0) then begin
        print, 'oc_fg2lla_dev: '+$
          '1. Checking cspice_surfpt for central cases, unit sph. Earth'
        a = 1.d
        b = 1.d
        c = 1.d

        print, 'Just above N pole, looking down'
        positn = [0., 0., 2.d]
        view = [0., 0, -1.d]
        cspice_surfpt, positn, view, a, b, c, point, found
        print, point
;       0.0000000       0.0000000       1.0000000

        print, 'WAY above N pole, looking down'
        positn = [0., 0., 2.d6]
        view = [0., 0, -1.d]
        cspice_surfpt, positn, view, a, b, c, point, found
        print, point
;       0.0000000       0.0000000       1.0000000

        print, 'WAY above Meridian, looking back'
        positn = [2.d6, 0., 0.]
        view = [-1.d, 0., 0]
        cspice_surfpt, positn, view, a, b, c, point, found
        print, point
;       1.0000000       0.0000000       0.0000000
    end

    if (0) then begin
        print, 'oc_fg2lla_dev: '+$
          '2. Checking cspice_surfpt for non-central cases, unit sph. Earth'
        a = 1.d
        b = 1.d
        c = 1.d

        print, 'East of N pole, looking down'
        positn = [0.1d, 0., 2.d6]
        view = [0., 0, -1.d]
        cspice_surfpt, positn, view, a, b, c, point, found
        print, point

        print, 'North of Meridian/Equator, looking back'
        positn = [2.d6, 0., 0.1]
        view = [-1.d, 0., 0]
        cspice_surfpt, positn, view, a, b, c, point, found
        print, point
    end

    if (0) then begin
        print, 'oc_fg2lla_dev: '+$
          '3. Checking cspice_surfpt for non-central cases, real Earth'

        cspice_bodvar, 399L, 'RADII', abc
        e = abc[0]
        f = (e-abc[2])/e
        a = abc[0]
        b = abc[1]
        c = abc[2]

        print, 'East of N pole, looking down'
        positn = [a/10, 0., 2.d6*a]
        view = [0., 0, -1.d]
        cspice_surfpt, positn, view, a, b, c, point, found
        print, point

        print, 'North of Meridian/Equator, looking back'
        positn = [2.d6*a, 0., a/10]
        view = [-1.d, 0., 0]
        cspice_surfpt, positn, view, a, b, c, point, found
        print, point
    end
    
    if (0) then begin
        print, 'oc_fg2lla_dev: '+$
          '4. Checking cspice_surfpt for central C313.2, real Earth'

        cspice_bodvar, 399L, 'RADII', abc
        re = abc[0]
        f = (re-abc[2])/re
        a = abc[0]
        b = abc[1]
        c = abc[2]
        frame = 'J2000'

        cspice_str2et, '2005 JUL 11 03:36:16.334', et
        targ = 901L
        state = oc_naifstate(et,targ) ; geocentric state
        pos = state[0:2]
        cspice_pxform, "IAU_EARTH", frame, et, r_e2x
        print, 'radec of Charon'
;        radec = xyz2radec(pos)
        rad6 = rec6rad6([pos,0,0,0])
        radec = rad6[1:2]
        rastr,  radec[0], 3, s & print, 'Charon ra: ', s
        decstr, radec[1], 3, s & print, 'Charon dec :', s
;radec of Charon
;Charon ra: 17:28:55.020
;Charon dec :-15:00:54.782

        pos_e = r_e2x # pos
        print, 'geocentric lon, lat of Charon'
        lonlat = xyz2radec(pos_e)
        decstr, lonlat[0]-2.d*!dpi, 3, s & print, 'Charon lon :', s
        decstr, lonlat[1], 3, s & print, 'Charon lat :', s
;geocentric lon, lat of Charon
;Charon lon :-80:51:13.771
;Charon lat :-15:01:09.683

        positn = pos_e
        view = -pos_e
        print, 'geodetic lon, lat of Charon'
        cspice_surfpt, positn, view, a, b, c, point, found
        cspice_recgeo, point, re, f, lon, lat, alt
        decstr, lon, 3, s & print, 'Charon lon :', s
        decstr, lat, 3, s & print, 'Charon lat :', s
;geodetic lon, lat of Charon
;Charon lon :-80:51:13.771
;Charon lat :-15:06:57.549

        ra  = radec[0]
        dec = radec[1]
        r_x2f = oc_x2f_rotmat(ra, dec)
        r_e2f = r_x2f ## r_e2x 

        pos_f = [0.d, 0.d, 1.d] * vabs(pos)
        print, 'pos (fgh) = ', pos_f
        print, 'pos (xyz) = ', pos
        print, '          = ', r_x2f # pos_f
        print, 'pos (e)   = ', pos_e
        print, '          = ', r_e2x # pos
        print, '          = ', r_e2x # (r_x2f # pos_f)
        print, '          = ', r_e2f # pos_f
;pos (fgh) =        0.0000000       0.0000000   4.4984602e+09
;pos (xyz) =   -5.8746878e+08  -4.3049706e+09  -1.1654412e+09
;          =   -5.8746878e+08  -4.3049706e+09  -1.1654412e+09
;pos (e)   =    6.9062001e+08  -4.2895459e+09  -1.1657551e+09
;          =    6.9062001e+08  -4.2895459e+09  -1.1657551e+09
;          =    6.9062001e+08  -4.2895459e+09  -1.1657551e+09
;          =    6.9062001e+08  -4.2895459e+09  -1.1657551e+09


        print, 'geodetic latitude and longitudes of f, g, h vectors'
        fvec_e = r_e2f # [1.d, 0.d, 0.d]
        gvec_e = r_e2f # [0.d, 1.d, 0.d]
        hvec_e = r_e2f # [0.d, 0.d, 1.d]
        ll = xyz2radec(fvec_e)
        decstr, ll[0], 3, s & print, 'fvec lon :', s
        decstr, ll[1],          3, s & print, 'fvec lat :', s
        ll = xyz2radec(gvec_e)
        decstr, ll[0]-2d*!dpi, 3, s & print, 'gvec lon :', s
        decstr, ll[1],          3, s & print, 'gvec lat :', s
        ll = xyz2radec(hvec_e)
        decstr, ll[0]-2d*!dpi, 3, s & print, 'hvec lon :', s
        decstr, ll[1],          3, s & print, 'hvec lat :', s
;geodetic latitude and longitudes of f, g, h vectors
;fvec lon :+09:08:16.779
;fvec lat :+00:01:49.760
;gvec lon :-80:58:32.297
;gvec lat :+74:58:50.200
;hvec lon :-80:51:13.771
;hvec lat :-15:01:09.683
        
    end
    
    if (0) then begin
        print, 'oc_fg2lla_dev: '+$
          '5. Checking cspice_surfpt for central C313.2, real Earth'
        print, 'reproducing logic of oc_fg2lla'

        ; set up the variables
        cspice_str2et, '2005 JUL 11 03:36:16.334', et
        targ = 901L
        radec_star = (rec6rad6(oc_naifstate(et,targ)))[1:2]
        fg = [0.d, 0.d]

        ; ---------- start fg2lla code
        
        frame = 'J2000'
        cspice_bodvar, 399L, 'RADII', abc
        re = abc[0]
        f = (re-abc[2])/re
        a = abc[0]
        b = abc[1]
        c = abc[2]
        ; get the rotation matrices
        cspice_pxform, "IAU_EARTH", frame, et, r_e2x
        r_x2f = oc_x2f_rotmat(radec_star[0], radec_star[1])
        r_e2f = r_x2f ## r_e2x 

        ; get the tangent position-----------------
        ;fgh_targ_earth = oc_naiffgh(et, targ, radec_star, $
        ;                        obs=obs,abcorr=abcorr,frame=frame)
        ;fgh_tang_targ = [fg[0], fg[1], 0. ]
        ;fgh_tang_earth = fgh_tang_targ + fgh_targ_earth

        pos_f = oc_naiffgh(et, targ, radec_star) + [fg,0.d] 
        pos_f[2] = 0.d
        pos_e = r_e2f # pos_f
        view = r_e2x # (radec2xyz(radec_star))
        cspice_surfpt, pos_e, view, a, b, c, point, found
        cspice_recgeo, point, re, f, lon, lat, alt

        ; print diagnostics

print, 'pos (fgh) = ', pos_f
print, 'pos (e)   = ', pos_e
decstr, lon, 3, s & print, 'Charon lon :', s
decstr, lat, 3, s & print, 'Charon lat :', s
;pos (fgh) =    5.9042162e-07  -1.5161076e-07       0.0000000
;pos (e)   =    5.7676563e-07   1.3256947e-07  -1.4611728e-07
;Charon lon :-80:51:13.771
;Charon lat :-15:06:57.549

end

    if (1) then begin
        print, 'oc_fg2lla_dev: '+$
          '6. Checking cspice_surfpt for central C313.2, real Earth'
        print, 'g positive'
        print, 'reproducing logic of oc_fg2lla'

        ; set up the variables
        cspice_str2et, '2005 JUL 11 03:36:16.334', et
        targ = 901L
        radec_star = (rec6rad6(oc_naifstate(et,targ)))[1:2]
        fg = [0.d, 60.d*1.613]

        ; ---------- start fg2lla code
        
        frame = 'J2000'
        cspice_bodvar, 399L, 'RADII', abc
        re = abc[0]
        f = (re-abc[2])/re
        a = abc[0]
        b = abc[1]
        c = abc[2]
        ; get the rotation matrices
        cspice_pxform, "IAU_EARTH", frame, et, r_e2x
        r_x2f = oc_x2f_rotmat(radec_star[0], radec_star[1])
        r_e2f = r_x2f ## r_e2x 

        ; get the tangent position-----------------
        ;fgh_targ_earth = oc_naiffgh(et, targ, radec_star, $
        ;                        obs=obs,abcorr=abcorr,frame=frame)
        ;fgh_tang_targ = [fg[0], fg[1], 0. ]
        ;fgh_tang_earth = fgh_tang_targ + fgh_targ_earth

        pos_f = oc_naiffgh(et, targ, radec_star) + [fg,0.d] 
        pos_f[2] = 0.d
        pos_e = r_e2f # pos_f
        view = r_e2x # (radec2xyz(radec_star))
        cspice_surfpt, pos_e, view, a, b, c, point, found
        cspice_recgeo, point, re, f, lon, lat, alt

        fgh = oc_naiffgh(et,targ, radec_star, lonlatalt=[lon,lat,alt])

        ; print diagnostics

print, 'pos (fgh) = ', pos_f
print, 'pos (e)   = ', pos_e
decstr, lon, 3, s & print, 'Charon lon :', s
decstr, lat, 3, s & print, 'Charon lat :', s
print, 'fg ', fg 
print, '-fgh', -fgh[0:2]

;pos (fgh) =    5.9042162e-07       96.780002       0.0000000
;pos (e)   =        3.9339313      -24.769692       93.473822
;Charon lon :-80:51:15.550
;Charon lat :-14:14:28.596
;fg        0.0000000       96.780002
;-fgh   3.3845228e-08       96.780002  -4.4984539e+09

    end

    if (1) then begin
        print, 'oc_fg2lla_dev: '+$
          '7. Checking cspice_surfpt for southern star, real Earth'
        print, 'reproducing logic of oc_fg2lla'

        ; set up the variables
        cspice_str2et, '2005 JUL 11 03:36:16.334', et
        targ = 901L
        radec_star = (rec6rad6(oc_naifstate(et,targ)))[1:2]
        radec_star[1] = radec_star[1] + (-98.87/4.4984539e+09)
        
        fg = [0.d, 0.d]

        ; ---------- start fg2lla code
        
        frame = 'J2000'
        cspice_bodvar, 399L, 'RADII', abc
        re = abc[0]
        f = (re-abc[2])/re
        a = abc[0]
        b = abc[1]
        c = abc[2]
        ; get the rotation matrices
        cspice_pxform, "IAU_EARTH", frame, et, r_e2x
        r_x2f = oc_x2f_rotmat(radec_star[0], radec_star[1])
        r_e2f = r_x2f ## r_e2x 

        ; get the tangent position-----------------
        ;fgh_targ_earth = oc_naiffgh(et, targ, radec_star, $
        ;                        obs=obs,abcorr=abcorr,frame=frame)
        ;fgh_tang_targ = [fg[0], fg[1], 0. ]
        ;fgh_tang_earth = fgh_tang_targ + fgh_targ_earth

        pos_f = oc_naiffgh(et, targ, radec_star) + [fg,0.d] 
        pos_f[2] = 0.d
        pos_e = r_e2f # pos_f
        view = r_e2x # (radec2xyz(radec_star))
        cspice_surfpt, pos_e, view, a, b, c, point, found
        cspice_recgeo, point, re, f, lon, lat, alt

        fgh = oc_naiffgh(et,targ, radec_star, lonlatalt=[lon,lat,alt])

        ; print diagnostics

print, 'pos (fgh) = ', pos_f
print, 'pos (e)   = ', pos_e
decstr, lon, 3, s & print, 'Charon lon :', s
decstr, lat, 3, s & print, 'Charon lat :', s
print, 'fg ', fg 
print, '-fgh', -fgh[0:2]

;pos (fgh) =    5.9042162e-07       98.870139       0.0000000
;pos (e)   =        4.0188919      -25.304640       95.492556
;Charon lon :-80:51:15.588
;Charon lat :-14:13:20.586
;fg        0.0000000       0.0000000
;fgh  2.0628160e-08   -8.4938644e-08   -4.4984539e+09

end


    if (1) then begin
        print, 'oc_fg2lla_dev: '+$
          '8. Checking cspice_surfpt for southern star, pos g, real Earth'
        print, 'reproducing logic of oc_fg2lla'

        ; set up the variables
        cspice_str2et, '2005 JUL 11 03:36:16.334', et
        targ = 901L
        radec_star = (rec6rad6(oc_naifstate(et,targ)))[1:2]
        radec_star[1] = radec_star[1] + (-98.87/4.4984539e+09)
        
        fg = [0.d, 100.d]

        ; ---------- start fg2lla code
        
        frame = 'J2000'
        cspice_bodvar, 399L, 'RADII', abc
        re = abc[0]
        f = (re-abc[2])/re
        a = abc[0]
        b = abc[1]
        c = abc[2]
        ; get the rotation matrices
        cspice_pxform, "IAU_EARTH", frame, et, r_e2x
        r_x2f = oc_x2f_rotmat(radec_star[0], radec_star[1])
        r_e2f = r_x2f ## r_e2x 

        ; get the tangent position-----------------
        ;fgh_targ_earth = oc_naiffgh(et, targ, radec_star, $
        ;                        obs=obs,abcorr=abcorr,frame=frame)
        ;fgh_tang_targ = [fg[0], fg[1], 0. ]
        ;fgh_tang_earth = fgh_tang_targ + fgh_targ_earth

        pos_f = oc_naiffgh(et, targ, radec_star) + [fg,0.d] 
        pos_f[2] = 0.d
        pos_e = r_e2f # pos_f
        view = r_e2x # (radec2xyz(radec_star))
        cspice_surfpt, pos_e, view, a, b, c, point, found
        cspice_recgeo, point, re, f, lon, lat, alt

        fgh = oc_naiffgh(et,targ, radec_star, lonlatalt=[lon,lat,alt])

        ; print diagnostics

print, 'pos (fgh) = ', pos_f
print, 'pos (e)   = ', pos_e
decstr, lon, 3, s & print, 'Charon lon :', s
decstr, lat, 3, s & print, 'Charon lat :', s
print, 'fg ', fg 
print, '-fgh', -fgh[0:2]

;pos (fgh) =    5.9042162e-07       198.87014       0.0000000
;pos (e)   =        8.0837099      -50.898454       192.07637
;Charon lon :-80:51:17.412
;Charon lat :-13:19:05.899
;fg        0.0000000       100.00000
;-fgh  -1.6361550e-08       100.00000  -4.4984539e+09


    end

end