;docformat = 'rst'
;+
;This function converts position vector to navigation coordinates.
;
;http://www.astro.uni.torun.pl/~kb/Papers/geod/Geod-GK.htm (in polish, but
;contains a fortran script commented in english).
;
;http://www.astro.uni.torun.pl/~kb/Papers/geod/Geod-ASS.htm (derivation in
;english).
;
; :Examples:
;   The south goalpost at Folsom Field, University of Colorado at
;   Boulder is located at: X (meters): -1288488.9373, Y (meters) 
;   -4720620.9617, Z (meters): 4079778.3407 so then:
;   
;   [lat, lon, alt] = xyz2llagrid([X, Y, Z]) 
;   
;   returns lat = 0.69828684115439, lon = -1.83725477406124,
;   alt = 1612.59993154183
;-
;+
; :Params:
;   vec : in, required, type=vector of double or array of vectors
;     The xyz coordinates of a location in meters.
;     
; :Keywords:
;   sid : in, optional, type=boolean
;     Optional parameter sid is Greenwich Mean Sidereal time at correct 
;     moment, in radians.
;   a : in, optional, type=double
;     A is the equatorial radius of the ellipsoid. If this is not set it 
;     defaults to the WGS-84 ellipsoid constants.
;   flat : in, optional, type=double
;     Flat is the flattening ratio (f=1-(a/b)) where b is the polar radius.
;     If this is not set it defaults to the WGS-84 ellipsoid constants.
;     
; :Returns:
;   If the keywords are not set, returns geodetic latitude and longitude 
;   in radians, and altitude above ellipsoid in meters. North latitude and
;   East longitude are positive numbers. ALL LOCATIONS IN THE USA WILL 
;   HAVE NEGATIVE LONGITUDES! This program uses a closed-form algorithm.
;   By default, this uses the size and shape of the WGS-84 ellipsoid, in
;   meters. If the keywords are set, returns geodetic coordinates and 
;   altitude with a custom ellipsoid. Lat and lon remain in radians, and
;   the units of alt are the same as A. 
;-
function xyz2llagrid,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
  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 z radius
  b=dsign(a*(1.0d - flat),z);

  ; On the rotation axis?
  Cond1=(0.0d eq r)
  NotCond1=1b - Cond1;
  Where1=where(Cond1,Count1)
  NotWhere1=where(NotCond1,NotCount1)
  ;if(0.0d eq r) then begin
  if(Count1 ne 0) then begin
    ; Yup, we are at a pole. Take the quick way out
    lat[Where1]=dsign(!dpi/2.0d,z[Where1]);
    alt[Where1]=abs(z[Where1])-abs(b[Where1]);
  ;end else begin
  end
  if(NotCount1 ne 0) then begin
    ; On the equator?
    Cond2=NotCond1 and (0.0d eq z);
    NotCond2=NotCond1 and (1b - (0.0d eq z));
    Where2=where(Cond2,Count2)
    NotWhere2=where(NotCond2,NotCount2)
    ;if(0.0d eq z) then begin
    if(Count2 ne 0) then begin
      lat[Where2]=0d;
      alt[Where2]=r[Where2]-a;
    ;end else begin
    end
    if(NotCount2 ne 0) then begin
      ; Nope, chug through the hard part
      E=z;
      F=z;
      P=z;
      Q=z;
      D=z;
      s=z;
      v=z;
      G=z;
      t=z;
      E[NotWhere2]=((z[NotWhere2]+b[NotWhere2])*b[NotWhere2]/a-a)/r[NotWhere2];
      F[NotWhere2]=((z[NotWhere2]-b[NotWhere2])*b[NotWhere2]/a+a)/r[NotWhere2];
      P[NotWhere2]=4.0d*(E[NotWhere2]*F[NotWhere2]+1.0d)/3.0d;
      Q[NotWhere2]=(E[NotWhere2]^2.0d -F[NotWhere2]^2.0d)*2.0d;
      D[NotWhere2]=P[NotWhere2]^3.0d +Q[NotWhere2]^2.0d;
      s[NotWhere2]=sqrt(D[NotWhere2])+Q[NotWhere2];
      s[NotWhere2]=dsign(abs(s[NotWhere2])^(1.0d/3.0d),s[NotWhere2]);
      v[NotWhere2]=P[NotWhere2]/s[NotWhere2]-s[NotWhere2];
      v[NotWhere2]=-(2.0d*Q[NotWhere2]+v[NotWhere2]^3.0d)/(3.0d*P[NotWhere2]);
      G[NotWhere2]=(E[NotWhere2]+sqrt(E[NotWhere2]^2.0d +v[NotWhere2]))/2.0d;
      t[NotWhere2]=sqrt(G[NotWhere2]^2.0d +(F[NotWhere2]-v[NotWhere2]*G[NotWhere2])/(2*G[NotWhere2]-E[NotWhere2]))-G[NotWhere2];
      lat[NotWhere2]=atan((1.0d -t[NotWhere2]*t[NotWhere2])*a/(2.0d*b[NotWhere2]*t[NotWhere2]));
      alt[NotWhere2]=(r[NotWhere2]-a*t[NotWhere2])*cos(lat[NotWhere2])+(z[NotWhere2]-b[NotWhere2])*sin(lat[NotWhere2]);
    end
  end
  return,compose_grid(lat,lon,alt);
end