; docformat = 'rst'
;+
;   This file converts position vector to navigation coordinates.
;   lla=xyz_to_lla(v) converts v, a cartesian earth-centered vector, into a vector
;   containing geodetic latitude and longitude in radians, and altitude above
;   ellipsoid, in that order. North latitude and East longitude are positive
;   numbers. ALL LOCATIONS IN THE USA WILL HAVE NEGATIVE LONGITUDES!
;
;   The distance unit used in the input vector matches the distance unit used
;   for the ellipsoid radius, kilometers by default.
;
;   This is a straightforward implemtation of the algorithm presented in
;   KAZIMIERZ M. BORKOWSKI, TRANSFORMATION OF GEOCENTRIC TO GEODETIC
;   COORDINATES WITHOUT APPROXIMATIONS, Astrophysics and Space Science, 139 (1987), 1-4
;                                       Erratum: vol. 146, (No. 1, July 1988), 201
;   http://www.astro.uni.torun.pl/~kb/Papers/geod/Geod-GK.htm (in polish,
;   but contains fortran code commented in english)
;   http://www.astro.uni.torun.pl/~kb/Papers/ASS/Geod-ASS.htm (Derivation
;   in english)
;   This algorithm is used because it requires no iterations, and can therefore
;   be easily vectorized. An entire grid of input coordinates are done at once,
;   using the full-blown IDL array operation optimization.
;
;   There is a singlarity in the algorithm at the equator and pole. In cases where
;   the input value is exactly on the equator (z=0) or axis (x=y=0) this is detected
;   and handled properly. There seems to be no loss of precision close to the equator,
;   but there is a minor loss of precision near the pole. It is subtracting two nearly
;   equal numbers there. This causes errors of as much as a few centimeters a millionth
;   of a degree from the pole. See, I said it was minor.
;
; :Examples:
;   The south goalpost at Folsom Field, University of Colorado
;   at Boulder is located at ECEF location: X (km) = -1288.4889373,
;   Y (km) = -4720.6209617, Z (km) =  4079.7783407.

;   >> print,xyz_to_lla([-1288.4889373d,-4720.6209617d,4079.7783407d])
;   
;   lat = 0.69828684115439, lon = -1.83725477406124, alt = 1612.59993154183
;-

;this is a standard function in FORTRAN to return a number with the same
;magnitude as a, with the sign of b.
function dsign,a,b
  result=0*b+abs(a); This makes result the right size. if either a or b 
                   ; is scalar and other vector, or both scalar, or both
                   ; are arrays of same size, this works
  OnAxis=(b lt 0)
  OnAxis=where(OnAxis,Count)
  if Count ne 0 then begin  
    result[OnAxis]*=-1;
  end
  return,result
end

;constrain a value to the range of +-q/2. Distinct from mlmod(),
;that constrains a value from 0-q
function constrain, a, q
  b = a mod q
  neg = where(b lt -q/2, count)
  if count gt 0 then b[neg] = b[neg]+q
  neg = where(b gt q/2, count)
  if count gt 0 then b[neg] = b[neg]-q
  return, b
end

;+
; :Params:
;   vec : in, required, type=array of double
;     The input vector may be a single vector, expressed as a 1D array, or
;     an nD grid of vectors, expressed as an (n+1)D array where the vector 
;     component is the last index. In other words, vec(*,0)=x, vec(*,1)=y, 
;     vec(*,2)=z. Currently only n=0,1,2 (A single vector, or a 1D or 2D 
;     grid) are supported, but this is easily extended.
;   
; :Keywords:
;   sid : in, optional, type=double
;     Optional parameter sid is greenwich mean siderial time at correct 
;     moment, in radians. This causes the input vector to be interpreted 
;     as an ECI vector. If sid is zero or unspecified, the input vector is
;     interpreted as being ECEF.
;   a : in, optional, type=double
;     A is the equatorial radius of the ellipsoid. By default, this 
;     function uses the equatorial radius of the WGS-84 ellipsoid, in 
;     kilometers. Lat and Lon remain in radians, and the units of alt are 
;     the same as A.
;   flat : in, optional, type=double
;     Flat is the flattening ratio (f=1-a/b, where b is the polar radius).
;     By default this function uses the equatorial radius of the WGS-84
;     ellipsoid, in kilometers. Lat and Lon remain in radians. 
;     
; :Returns:
;   Returns the Longitude Latitude Altitude navigational coordinates of
;   a rectangular coordinate position vector.
;-
function xyz_to_lla,vec, sid=sid, a=a, flat=flat

  resolve_grid,vec,x=x,y=y,z=z

  ;Default ellipsoid if none is specified
  if n_elements(a)    eq 0 then a=get_wgs84_const(/a);
  if n_elements(flat) eq 0 then flat=get_wgs84_const(/f);
  if n_elements(sid)  eq 0 then sid=0.0d;

  ; Nothing special about this for an ellipsoid, it's just like a sphere
  lon=constrain(atan(y,x)-sid,2*!dpi);

  ; Set the size for the other vars
  lat=z;
  alt=z;

  ; Length of projection of vector to equatorial plane
  r=sqrt(x*x+y*y);
  ; x or y should not appear below here
  ; Ellipsoid polar radius, with same sign as z
  b=dsign(a*(1.0d - flat),z);

  ; On the rotation axis?
  OnAxis=where((0.0d eq r),nOnAxis,complement=NotOnAxis,ncomplement=nNotOnAxis)
  if(nNotOnAxis ne 0) then begin
    ; Not on the rotation axis.
    ; On the equator?
    OnEqu=where(NotOnAxis and (0.0d eq z),nOnEqu,complement=NotOnEqu,ncomplement=nNotOnEqu)
    if(nNotOnEqu ne 0) then begin
      ;Not on equator or axis, chug through the hard part
      ;Set the temporary variables to the size of the input
      ;All of these variable names match those given in Borkowski
      E=z;
      F=z;
      P=z;
      Q=z;
      D=z;
      s=z;
      v=z;
      G=z;
      t=z;
      E[NotOnEqu]=((z[NotOnEqu]+b[NotOnEqu])*b[NotOnEqu]/a-a)/r[NotOnEqu];
      F[NotOnEqu]=((z[NotOnEqu]-b[NotOnEqu])*b[NotOnEqu]/a+a)/r[NotOnEqu];
      P[NotOnEqu]=4.0d*(E[NotOnEqu]*F[NotOnEqu]+1.0d)/3.0d;
      Q[NotOnEqu]=(E[NotOnEqu]^2.0d -F[NotOnEqu]^2.0d)*2.0d;
      D[NotOnEqu]=P[NotOnEqu]^3.0d +Q[NotOnEqu]^2.0d;
      s[NotOnEqu]=sqrt(D[NotOnEqu])+Q[NotOnEqu];
      s[NotOnEqu]=dsign(abs(s[NotOnEqu])^(1.0d/3.0d),s[NotOnEqu]);
      v[NotOnEqu]=P[NotOnEqu]/s[NotOnEqu]-s[NotOnEqu];
      v[NotOnEqu]=-(2.0d*Q[NotOnEqu]+v[NotOnEqu]^3.0d)/(3.0d*P[NotOnEqu]);
      G[NotOnEqu]=(E[NotOnEqu]+sqrt(E[NotOnEqu]^2.0d +v[NotOnEqu]))/2.0d;
      t[NotOnEqu]=sqrt(G[NotOnEqu]^2.0d +(F[NotOnEqu]-v[NotOnEqu]*G[NotOnEqu])/(2*G[NotOnEqu]-E[NotOnEqu]))-G[NotOnEqu];
      lat[NotOnEqu]=atan((1.0d -t[NotOnEqu]*t[NotOnEqu])*a/(2.0d*b[NotOnEqu]*t[NotOnEqu]));
      alt[NotOnEqu]=(r[NotOnEqu]-a*t[NotOnEqu])*cos(lat[NotOnEqu])+(z[NotOnEqu]-b[NotOnEqu])*sin(lat[NotOnEqu]);
    end
    if(nOnEqu ne 0) then begin
      ; On the equatorial plane. Shortcut.
      lat[OnEqu]=0d;
      alt[OnEqu]=r[OnEqu]-a;
    end
  end
  if(nOnAxis ne 0) then begin
    ; On the axis. Shortcut
    lat[OnAxis]=dsign(!dpi/2.0d,z[OnAxis]);
    alt[OnAxis]=abs(z[OnAxis])-abs(b[OnAxis]);
  end

  ;Set up size of result
  result=compose_grid(lat,lon,alt)
  return,result;
end