;+
;$Id: scc_triangulate.pro,v 1.3 2009/11/19 16:08:35 thompson Exp $
;
; Project     :	STEREO - SECCHI
;
; Name        :	SCC_TRIANGULATE()
;
; Purpose     :	Triangulate 3D coordinates from STEREO measurements
;
; Category    :	STEREO, SECCHI, Coordinates
;
; Explanation :	The (helioprojective-cartesian) angular coordinates of a given
;               solar feature, as seen by the STEREO Ahead and Behind
;               spacecraft, are intercompared to derive the three-dimensional
;               position.
;
;               The accuracy of this procedure depends critically on choosing
;               angular coordinates which represent the same point in space.
;               If the chosen lines of sight from the two spacecraft do not
;               intersect, that will introduce an error.  The optional
;               parameter GAP returns the distance of closest approach between
;               the two lines of sight.
;
; Syntax      :	Coord = SCC_TRIANGULATE(DATE, HPC_AHEAD, HPC_BEHIND  [, GAP ])
;
; Inputs      :	DATE       = The date and time of the data, in any format
;                            recognized by ANYTIM2UTC.  Can either be a scalar
;                            or an array consistent with HPC_AHEAD and
;                            HPC_BEHIND.
;
;               HPC_AHEAD  = Two-element array containing the
;                            helioprojective-cartesian longitude (X) and
;                            latitude (Y) for the Ahead spacecraft.  Can also
;                            have additional dimensions, e.g. [2,n1,n2,...].
;
;               HPC_BEHIND = Same for the Behind spacecraft.
;
; Opt. Inputs :	None.
;
; Outputs     :	The result of the function is an array containing the derived
;               3D coordinates in (x,y,z) format.  The array will have the
;               dimensions [3,n1,n2,...] corresponding to the dimensions of
;               HPC_AHEAD and HPC_BEHIND.
;
; Opt. Outputs:	GAP    = Closest distance between the lines of sight.
;
; Keywords    :	SYSTEM = Character string, giving one of the following
;                        standard coordinate systems:
;
;                               HCI     Heliocentric Inertial (default)
;                               HAE     Heliocentric Aries Ecliptic
;                               HEE     Heliocentric Earth Ecliptic
;                               HEEQ    Heliocentric Earth Equatorial (or HEQ)
;                               Carrington (can be abbreviated)
;
;                        The default is HEEQ.
;
;               CUNIT  = String containing the units of HPC_AHEAD and
;                        HPC_BEHIND.  Valid values are "deg", "arcmin",
;                        "arcsec", "mas" (milliarcseconds), and "rad"
;                        (radians).  The default is "arcsec".
;
;               METERS = If set, then the coordinates are returned in units of
;                        meters, instead of the default of kilometers.  Note
;                        that meters are required for FITS header keywords.
;
;               AU     = If set, then the coordinates are returned in
;                        Astronomical Units, instead of the default of
;                        kilometers.
;
;               SOLRAD = If set, then the coordinates are returned in solar
;                        radii, instead of the default of kilometers.
;
;               SPACECRAFT = Two-element string array giving the names of the
;                        spacecraft associated with data set.  The default is
;                        SPACECRAFT=['Ahead','Behind'], but one also substitute
;                        'Earth' or 'SOHO' for either of these.  The use of
;                        'SOHO' also requires that the SOHO ephemerides be
;                        loaded via LOAD_SOHO_SPICE.
;
; Calls       :	ANYTIM2UTC, GET_STEREO_COORD, CONVERT_STEREO_COORD
;
; Common      :	None.
;
; Restrictions:	Currently only works for STEREO Ahead and Behind images.  Does
;               not (yet) work for combinations of STEREO and other
;               observatories (e.g. SOHO).
;
; Side effects:	None.
;
; Prev. Hist. :	None.
;
; Written     : 22-Sep-2006, William Thompson, GSFC
;
; $Log: scc_triangulate.pro,v $
; Revision 1.3  2009/11/19 16:08:35  thompson
; Make loop variable long.
;
; Revision 1.2  2008-10-31 15:47:09  thompson
; Include support for Earth-based observatories, and SOHOd
;
; Revision 1.1  2006/09/27 17:04:24  nathan
; moved from dev/analysis
;
; Revision 1.2  2006/09/26 21:43:10  nathan
; add CVS keywords
;
; Contact     :	WTHOMPSON
;
;-
;
function scc_triangulate, date, hpc_ahead, hpc_behind, gap, cunit=k_cunit, $
                          system=k_system, spacecraft=spacecraft, $
                          meters=meters, au=au, solrad=solrad
on_error, 2
;
if n_elements(k_system) eq 1 then system=k_system else system='HEEQ'
if n_elements(spacecraft) eq 2 then sc=spacecraft else sc=['ahead','behind']
;
;  Check the input parameters.  Make sure that they have the right
;  dimensionality.  Convert DATE to UTC.
;
if n_params() lt 3 then message, $
  'Syntax:  Result = SCC_TRIANGULATE(DATE, HPC_AHEAD, HPC_BEHIND)'
;
if n_elements(date) eq 0 then message, 'DATE not defined'
message = ''
utc = anytim2utc(date, errmsg=message)
if message ne '' then message, message
n_date = n_elements(utc)
if n_date gt 1 then begin
    sz_date  = size(utc)
    dim_date = sz_date[1:sz_date[0]]
endif
;
sz_ahead = size(hpc_ahead)
if sz_ahead[0] eq 0 then message, 'HPC_AHEAD must be an array'
if sz_ahead[1] ne 2 then message, 'First dimension of HPC_AHEAD must be 2'
n_ahead = n_elements(hpc_ahead) / 2
if n_ahead gt 1 then dim_ahead = sz_ahead[2:sz_ahead[0]]
;
sz_behind = size(hpc_behind)
if sz_behind[0] eq 0 then message, 'HPC_BEHIND must be an array'
if sz_behind[1] ne 2 then message, 'First dimension of HPC_BEHIND must be 2'
n_behind = n_elements(hpc_behind) / 2
if n_behind gt 1 then dim_behind = sz_behind[2:sz_behind[0]]
;
;  Make sure that the dimensions of the arrays agree.
;
if n_ahead ne n_behind then message, $
  'Arrays HPC_AHEAD and HPC_BEHIND do not agree'
if n_ahead gt 1 then begin
    if n_elements(dim_ahead) ne n_elements(dim_behind) then message, $
      'Arrays HPC_AHEAD and HPC_BEHIND do not agree'
    w = where(dim_ahead ne dim_behind, count)
    if count gt 0 then message, $
      'Arrays HPC_AHEAD and HPC_BEHIND do not agree'
endif
if n_date gt 1 then begin
    if n_date ne n_ahead then message, $
      'Arrays DATE and HPC_AHEAD do not agree'
    if n_date ne n_behind then message, $
      'Arrays DATE and HPC_BEHIND do not agree'
    w = where(dim_date ne dim_ahead, count)
    if count gt 0 then message, $
      'Arrays DATE and HPC_AHEAD do not agree'
    w = where(dim_date ne dim_behind, count)
    if count gt 0 then message, $
      'Arrays DATE and HPC_BEHIND do not agree'
endif
;
;  Parse the CUNIT string, if passed.
;
if n_elements(k_cunit) eq 1 then cunit=strlowcase(k_cunit) else cunit='arcsec'
if strmid(cunit,0,3) eq 'deg'    then cunit = 'deg'
if strmid(cunit,0,6) eq 'arcmin' then cunit = 'arcmin'
if strmid(cunit,0,6) eq 'arcsec' then cunit = 'arcsec'
if strmid(cunit,0,3) eq 'rad'    then cunit = 'rad'
;
;  Convert the coordinates into radians.
;
conv = !dpi / 180.d0
case cunit of
    'arcmin': conv = conv / 60.d0
    'arcsec': conv = conv / 3600.d0
    'mas':    conv = conv / 3600.d3
    'rad':    conv = 1.d0
    else:     conv = conv
endcase
ahead  = conv * hpc_ahead
behind = conv * hpc_behind
;
;  Simplify the dimensions.
;
if n_date gt 1 then utc = reform(utc, n_date, /overwrite)
if n_ahead gt 1 then begin
    ahead  = reform(ahead,  2, n_ahead, /overwrite)
    behind = reform(behind, 2, n_ahead, /overwrite)
endif
;
;  Get the values of Dsun, in kilometers.
;
dsun_ahead  = get_stereo_coord(utc, 'ahead',  system='HCI')
dsun_behind = get_stereo_coord(utc, 'behind', system='HCI')
dsun_ahead  = sqrt(total(dsun_ahead^2,  1))
dsun_behind = sqrt(total(dsun_behind^2, 1))
;
;  Calculate the equivalent heliocentric coordinates for distances D within
;  +/- 100 Mm of Dsun.  This defines a line which can be extrapolated.
;
lon = reform(ahead[0,*])
lat = reform(ahead[1,*])
cosy = cos(lat)
dx = cosy * cos(lon)
dy = cosy * sin(lon)
dz = sin(lat)
d = dsun_ahead - 1d6
coord_ahead1 = reform(dblarr(3,n_ahead))
coord_ahead1[0,*] = dsun_ahead - d * dx
coord_ahead1[1,*] = d * dy
coord_ahead1[2,*] = d * dz
d = dsun_ahead + 1d6
coord_ahead2 = reform(dblarr(3,n_ahead))
coord_ahead2[0,*] = dsun_ahead - d * dx
coord_ahead2[1,*] = d * dy
coord_ahead2[2,*] = d * dz
;
lon = reform(behind[0,*])
lat = reform(behind[1,*])
cosy = cos(lat)
dx = cosy * cos(lon)
dy = cosy * sin(lon)
dz = sin(lat)
d = dsun_behind - 1d6
coord_behind1 = reform(dblarr(3,n_behind))
coord_behind1[0,*] = dsun_behind - d * dx
coord_behind1[1,*] = d * dy
coord_behind1[2,*] = d * dz
d = dsun_behind + 1d6
coord_behind2 = reform(dblarr(3,n_behind))
coord_behind2[0,*] = dsun_behind - d * dx
coord_behind2[1,*] = d * dy
coord_behind2[2,*] = d * dz
;
;  Convert into the requested coordinate system.
;
convert_stereo_coord, utc, coord_ahead1,  'HGRTN', system, spacecraft=sc[0]
convert_stereo_coord, utc, coord_ahead2,  'HGRTN', system, spacecraft=sc[0]
convert_stereo_coord, utc, coord_behind1, 'HGRTN', system, spacecraft=sc[1]
convert_stereo_coord, utc, coord_behind2, 'HGRTN', system, spacecraft=sc[1]
;
;  Find the points of closest approach of the two lines.
;
coord_ahead  = coord_ahead1
coord_behind = coord_behind1
for i=0L,n_ahead-1 do begin
    x1a = coord_ahead1 [0,i]  &  x2a = coord_ahead2 [0,i]
    y1a = coord_ahead1 [1,i]  &  y2a = coord_ahead2 [1,i]
    z1a = coord_ahead1 [2,i]  &  z2a = coord_ahead2 [2,i]
;
    x1b = coord_behind1[0,i]  &  x2b = coord_behind2[0,i]
    y1b = coord_behind1[1,i]  &  y2b = coord_behind2[1,i]
    z1b = coord_behind1[2,i]  &  z2b = coord_behind2[2,i]
;
    pya0 = (y1a*x2a-y2a*x1a) / (x2a-x1a)  &  pya1 = (y2a-y1a) / (x2a-x1a)
    pza0 = (z1a*x2a-z2a*x1a) / (x2a-x1a)  &  pza1 = (z2a-z1a) / (x2a-x1a)
    pyb0 = (y1b*x2b-y2b*x1b) / (x2b-x1b)  &  pyb1 = (y2b-y1b) / (x2b-x1b)
    pzb0 = (z1b*x2b-z2b*x1b) / (x2b-x1b)  &  pzb1 = (z2b-z1b) / (x2b-x1b)
;
    alpha = 1 + pya1*pyb1 + pza1*pzb1
    mat = [[1 + pya1^2 + pza1^2, -alpha], [alpha, -(1+pyb1^2+pzb1^2)]]
    vec = [pya1*(pyb0-pya0) + pza1*(pzb0-pza0), $
           pyb1*(pyb0-pya0) + pzb1*(pzb0-pza0)]
    xx = invert(mat) ## vec
    coord_ahead[0,i]  = xx[0]
    coord_ahead[1,i]  = pya0 + pya1*xx[0]
    coord_ahead[2,i]  = pza0 + pza1*xx[0]
    coord_behind[0,i] = xx[1]
    coord_behind[1,i] = pyb0 + pyb1*xx[1]
    coord_behind[2,i] = pzb0 + pzb1*xx[1]
endfor
;
;  Calculate the result as the point midway between the points of closest
;  approach.  Also calculate the distance between the points.
;
coord = (coord_ahead + coord_behind) / 2
gap = sqrt(total((coord_ahead-coord_behind)^2,1))
;
;  Convert to the requested coordinate system.
;
conv = 1
if keyword_set(meters) then conv = 1d-3
if keyword_set(au) then conv = 1.4959787d8
if keyword_set(solrad) then conv = 6.95508d5
if conv ne 1 then begin
    coord = temporary(coord) / conv
    gap   = temporary(gap)   / conv
endif
;
;  Reformat to the original dimensions.
;
if n_ahead gt 1 then begin
    coord = reform(coord, [3,dim_ahead], /overwrite)
    gap   = reform(gap, dim_ahead, /overwrite)
endif
;
return, coord
end