;+
; NAME:
;	sgp4_eph
; PURPOSE:
;	Get location of Coriolis spacecraft in ECI coordinates
;	(at current epoch; see Procedure).
; CATEGORY:
;	gen/idl/ephem
; CALLING SEQUENCE:
	FUNCTION sgp4_eph, tt_obs, body_, km=km,	$
		location=location, source=source, precess=precess
; INPUTS:
;	tt_obs			array[n]; type: time structure
;						UT time
; OPTIONAL INPUTS:
;	body_			scalar; type: string; default: 'sat27640'
;						spacecraft designation
;						The default sat27640 is Coriolis.
;	km=km			if set, return distance in km instead of AU
;	source=source	scalar; type: string; default: who_am_i(/dir)/sgp
;						directory where TLE file is located (by default the
;						subdir sgp of the dir where this file is located.
;	/precess		precess position and velocity vector to J2000 coordinates
;						(used by href=big_eph=)
; OUTPUTS:
;	rr[6,n]			location and velocity
;						if /location is set only r[0:2,n] is returned
;						if the required file with TLEs is not found
;						then the output vector contains NaNs.
; INCLUDE:
	@compile_opt.pro		; On error, return to caller
; CALLS:
;	txt_read, InitVar, TimeSet, TimeOp, TimeUnit, hide_env, sgp_alias
; PROCEDURE:
;	The file with orbital elements is 'body.txt' i.e. sat27640.txt for 
;	Coriolis. The file is looked for in the subdirectory sgp of the
;	directory where this source code is located. Used keyword 'source'
;	to point to another subdirectory.
;	Orbital elements can be retrieved from www.space-track.org
;
;	Excerpt from href=http://celestrak.com/columns/v02n01/=:
;
;	"As it turns out, the NORAD SGP4 orbital model takes the standard
;	two-line orbital element set and the time and produces an ECI
;	position and velocity for the satellite. In particular, it puts it
;	in an ECI frame relative to the "true equator and mean equinox of
;	the epoch" of the element set. This specific distinction is
;	necessary because the direction of the Earth's true rotation axis
;	(the North Pole) wanders slowly over time, as does the true direction
;	of the vernal equinox. Since observations of satellites are made
;	by stations fixed to the Earth's surface, the elements generated
;	will be referenced relative to the true equator. However, since the
;	direction of vernal equinox is not tied to the Earth's surface, but
;	rather to the Earth's orientation in space, an approximation must
;	be made of its true direction. The approximation made in this case is
;	to account for the precession of the vernal equinox but to ignore the
;	nutation (nodding) of the Earth's obliquity (the angle between the
;	equatorial plane and the ecliptic)."
; MODIFICATION HISTORY:
;	MAR-2005, Paul Hick (UCSD/CASS)
;	SEP-2005, Paul Hick (UCSD/CASS; pphick@ucsd.edu)
;		Reduced memory requirements by extracting TLE bracketing the
;		input time range tt_obs
;-

InitVar, km, /key
InitVar, location, /key
InitVar, source, filepath(root=who_am_i(/dir),'sgp') 
InitVar, body_, 'sat27640', set=body
InitVar, precess, /key

; Translate aliases

body = sgp_alias(body)

CASE location OF
0: BadVector = replicate(BadValue(0d0),6)
1: BadVector = replicate(BadValue(0d0),3)
ENDCASE

tmp = filepath(root=source,body+'.txt')
IF NOT txt_read(tmp,tlm,/silent) THEN BEGIN
	message, /info, 'no ephemeris file, '+hide_env(tmp)
	RETURN, BadVector
ENDIF

nobs	= n_elements(tt_obs)
szobs	= size(tt_obs, /dim)
tt_obs	= reform(tt_obs,nobs,/overwrite)

tlm 	= tlm[5:n_elements(tlm)-2]

ntlm	= n_elements(tlm)/2
tlm 	= reform(tlm,2,ntlm,/overwrite)
tt_tlm	= TimeSet(yr=TimeFixYear(long( reform(strmid(tlm[0,*],18,2)))),doy=double(reform(strmid(tlm[0,*],20,12))))

umin = TimeUnit(/minute)

; Extract the TLE vectors bracketing the requested time period
; (strictly speaking not necessary, but reduces the size of uu_obs below
; usually by a lot if the ephemeris file gets big)

i = where( TimeOp(/subtract,tt_tlm,TimeLimits(tt_obs,/min),umin) LT 0 )
IF i[0] EQ -1 THEN i = 0 ELSE i = i[n_elements(i)-1]	; Preceeds time range

j = where( TimeOp(/subtract,tt_tlm,TimeLimits(tt_obs,/max),umin) GT 0 )
IF j[0] EQ -1 THEN j = ntlm-1 ELSE j = j[0] 			; Follows time range

tlm    = tlm   [*,i:j]
tt_tlm = tt_tlm[  i:j]
ntlm   = n_elements(tt_tlm)

CASE ntlm EQ 1 OF						; Happens if all tt_obs are outside the
										; the time range covered by the ephemeris
0: BEGIN

	uu_obs	= replicate(!thetime,nobs,ntlm)
	uu_tlm	= replicate(!thetime,nobs,ntlm)

	uu_obs.(0) = tt_obs.(0)#replicate(1,ntlm)
	uu_obs.(1) = tt_obs.(1)#replicate(1,ntlm)

	uu_tlm.(0) = replicate(1,nobs)#tt_tlm.(0)
	uu_tlm.(1) = replicate(1,nobs)#tt_tlm.(1)

	dtt 	= TimeOp(/subtract,uu_obs,uu_tlm,umin)

	tmp 	= min(abs(dtt), dim=2, i)
	i		= ArrayLocation(i,size=size(dtt))
	i		= reform(i[1,*])

	tlm 	= tlm   [*,i]
	dtt 	= dtt[lindgen(nobs),i]

END

1: BEGIN
	tlm = SuperArray(tlm,nobs,/trail)
	dtt = TimeOp(/subtract,tt_obs,tt_tlm,umin)
END

ENDCASE

DE2RA	= 0.174532925d-1
E6A 	= 1d-6
PI		= 3.14159265d0
PIO2	= 1.57079633d0
QO		= 120d0
SO		= 78d0
TOTHRD	= 0.66666667d0
TWOPI	= 6.2831853d0
X3PIO2	= 4.71238898d0
XJ2 	= 1.082616d-3
XJ3 	= -0.253881d-5
XJ4 	= -1.65597d-6
XKE 	= 0.743669161d-1
XKMPER	= 6378.135d0		; Radius Earth
XMNPDA	= 1440d0			; # minutes per day
AE		= 1d0

AE2 	=  AE*AE
CK2 	=  0.5d0*XJ2*AE2
CK4 	= -0.375d0*XJ4*AE2*AE2
QOMS2T	= ((QO-SO)*AE/XKMPER)^4d0
S		= AE*(1d0+SO/XKMPER)


r		= dblarr(12,nobs)


; rr(2) and rr(4) are read with format F6.5. rr(8) with F7.7
; The corresponding numbers in the TLE record do not contain an explicit decimal point.
; E.g. rr(2) and rr(4) could be 54147. rr(8) could be 0013639.
; IDL 6.1 reads this as 54157.0 instead of 0.54157 (it works correctly in Fortran).
; Rather than messing with the format the numbers are explicitly corrected below.

format = '18X,D14.8,1X,F10.8,2(1X,F6.5,I2),/,7X,2(1X,F8.4),1X,F7.7,2(1X,F8.4),1X,F11.8'

; This should work seems to me, but it doesn't. Use explicit FOR loop instead. Yuck.
;reads, tlm, format='('+format+')', r

tmp = dblarr(12)
FOR i=0,nobs-1 DO BEGIN
	reads, tlm[*,i], format='('+format+')', tmp
	r[*,i] = tmp
ENDFOR

EPOCH	= reform(r[ 0,*])
XNDT2O	= reform(r[ 1,*])
XNDD6O	= reform(r[ 2,*])*1d-5		; Fix read error
IEXP	= reform(r[ 3,*])
BSTAR	= reform(r[ 4,*])*1d-5		; Fix read error
IBEXP	= reform(r[ 5,*])
XINCL	= reform(r[ 6,*])
XNODEO	= reform(r[ 7,*])
EO		= reform(r[ 8,*])*1d-7		; Fix read error
OMEGAO	= reform(r[ 9,*])
XMO 	= reform(r[10,*])
XNO 	= reform(r[11,*])

XNDD6O	= XNDD6O*(10d0^IEXP)
XNODEO	= XNODEO*DE2RA
OMEGAO	= OMEGAO*DE2RA
XMO 	= XMO*DE2RA
XINCL	= XINCL*DE2RA
TEMP	= TWOPI/XMNPDA/XMNPDA
XNO		= XNO*TEMP*XMNPDA
XNDT2O	= XNDT2O*TEMP
XNDD6O	= XNDD6O*TEMP/XMNPDA

A1		= (XKE/XNO)^TOTHRD
TEMP	= cos(XINCL)
TEMP	= 1.5d0*CK2*(3d0*TEMP*TEMP-1d0)/(1d0-EO*EO)^1.5d0
DEL1	= TEMP/(A1*A1)
AO		= A1*(1d0-DEL1*(0.5d0*TOTHRD+DEL1*(1d0+134d0/81d0*DEL1)))
DELO	= TEMP/(AO*AO)
XNODP	= XNO/(1d0+DELO)
BSTAR	= BSTAR*(10d0^IBEXP)/AE

; Recover original mean motion (XNODP) and semimajor axis (AODP) from input elements

A1	   = (XKE/XNO)^TOTHRD
COSIO  = cos(XINCL)
THETA2 = COSIO*COSIO
X3THM1 = 3d0*THETA2-1d0
EOSQ   = EO*EO
BETAO2 = 1d0-EOSQ
BETAO  = sqrt(BETAO2)
DEL1   = 1.5d0*CK2*X3THM1/(A1*A1*BETAO*BETAO2)
AO	   = A1*(1d0-DEL1*(0.5d0*TOTHRD+DEL1*(1d0+134d0/81d0*DEL1)))
DELO   = 1.5d0*CK2*X3THM1/(AO*AO*BETAO*BETAO2)
XNODP  = XNO/(1d0+DELO)
AODP   = AO/(1d0-DELO)

; Initialization
; For perigee less than 220 kilometers, the isimp flag is set and
; the equations are truncated to linear variation in sqrt a and
; quadratic variation in mean anomaly. also, the c3 term, the
; delta omega term, and the delta m term are dropped.

ISIMP = AODP*(1d0-EO)/AE LT 220d0/XKMPER+AE

; For perigee below 156 km, the values of S and QOMS2T are altered

S4		= replicate(S	  ,nobs)
QOMS24	= replicate(QOMS2T,nobs)
PERIGE	= (AODP*(1d0-EO)-AE)*XKMPER

i = where(PERIGE LT 156d0)
j = where(PERIGE LE  98d0)

IF i[0] NE -1 THEN BEGIN
	S4[i] = PERIGE[i]-78d0
	IF j[0] NE -1 THEN S4[j] = 20d0
	QOMS24[i] = ((120d0-S4[i])*AE/XKMPER)^4d0
	S4[i] = S4[i]/XKMPER+AE
ENDIF

PINVSQ	= 1d0/(AODP*AODP*BETAO2*BETAO2)
TSI 	= 1d0/(AODP-S4)
ETA 	= AODP*EO*TSI
ETASQ	= ETA*ETA
EETA	= EO*ETA
PSISQ	= abs(1d0-ETASQ)
COEF	= QOMS24*TSI^4d0
COEF1	= COEF/PSISQ^3.5d0
C2		= COEF1*XNODP*(AODP*(1d0+1.5d0*ETASQ+EETA*(4d0+ETASQ))+0.75d0*CK2*TSI/PSISQ*X3THM1*(8d0+3d0*ETASQ*(8d0+ETASQ)))
C1		= BSTAR*C2
SINIO	= sin(XINCL)
A3OVK2	= -XJ3/CK2*AE*AE*AE
C3		= COEF*TSI*A3OVK2*XNODP*AE*SINIO/EO
X1MTH2	= 1d0-THETA2
C4		= 2d0*XNODP*COEF1*AODP*BETAO2*(ETA*(2d0+0.5d0*ETASQ)+EO*(0.5d0+2d0*ETASQ)-2d0*CK2*TSI/				$
			(AODP*PSISQ)*(-3d0*X3THM1*(1d0-2d0*EETA+ETASQ*(1.5d0-0.5d0*EETA))+0.75d0*X1MTH2*(2d0*ETASQ-EETA*	$
			(1d0+ETASQ))*cos(2d0*OMEGAO)))
C5		= 2d0*COEF1*AODP*BETAO2*(1d0+2.75d0*(ETASQ+EETA)+EETA*ETASQ)
THETA4	= THETA2*THETA2
TEMP1	= 3d0*CK2*PINVSQ*XNODP
TEMP2	= TEMP1*CK2*PINVSQ
TEMP3	= 1.25d0*CK4*PINVSQ*PINVSQ*XNODP
XMDOT	= XNODP+0.5d0*TEMP1*BETAO*X3THM1+0.0625d0*TEMP2*BETAO*(13d0-78d0*THETA2+137d0*THETA4)
X1M5TH	= 1d0-5d0*THETA2
OMGDOT	= -0.5d0*TEMP1*X1M5TH+0.0625d0*TEMP2*(7d0-114d0*THETA2+395d0*THETA4)+TEMP3*(3d0-36d0*THETA2+49d0*THETA4)
XHDOT1	= -TEMP1*COSIO
XNODOT	= XHDOT1+(0.5d0*TEMP2*(4d0-19d0*THETA2)+2d0*TEMP3*(3d0-7d0*THETA2))*COSIO
OMGCOF	= BSTAR*C3*cos(OMEGAO)
XMCOF	= -TOTHRD*COEF*BSTAR*AE/EETA
XNODCF	= 3.5d0*BETAO2*XHDOT1*C1
T2COF	= 1.5d0*C1
XLCOF	= 0.125d0*A3OVK2*SINIO*(3d0+5d0*COSIO)/(1d0+COSIO)
AYCOF	= 0.25d0*A3OVK2*SINIO
DELMO	= (1d0+ETA*cos(XMO))^3d0
SINMO	= sin(XMO)
X7THM1	= 7d0*THETA2-1d0

i = where(ISIMP NE 1)
IF i[0] NE -1 THEN BEGIN
	C1SQ= C1[i]*C1[i]
	D2	= 4d0*AODP[i]*TSI[i]*C1SQ[i]
	TEMP= D2*TSI[i]*C1[i]/3d0
	D3	= (17d0*AODP[i]+S4[i])*TEMP
	D4	= 0.5d0*TEMP*AODP[i]*TSI[i]*(221d0*AODP[i]+31d0*S4[i])*C1[i]
	T3COF= D2+2d0*C1SQ
	T4COF= 0.25d0*(3d0*D3+C1[i]*(12d0*D2+10d0*C1SQ))
	T5COF= 0.2d0*(3d0*D4+12d0*C1[i]*D3+6d0*D2*D2+15d0*C1SQ*(2d0*D2+C1SQ))
ENDIF

; Update for secular gravity and atmospheric drag

XMDF	= XMO+XMDOT*dtt
OMGADF	= OMEGAO+OMGDOT*dtt
XNODDF	= XNODEO+XNODOT*dtt
OMEGA	= OMGADF
XMP 	= XMDF
TSQ 	= dtt*dtt
XNODE	= XNODDF+XNODCF*TSQ
TEMPA	= 1d0-C1*dtt
TEMPE	= BSTAR*C4*dtt
TEMPL	= T2COF*TSQ

IF i[0] NE -1 THEN BEGIN
	DELOMG	= OMGCOF[i]*dtt[i]
	DELM	= XMCOF[i]*((1d0+ETA[i]*cos(XMDF[i]))^3d0-DELMO[i])
	TEMP	= DELOMG+DELM
	XMP[i] 	= XMDF[i]+TEMP
	OMEGA[i]= OMGADF[i]-TEMP
	TCUBE	= TSQ[i]*dtt[i]
	TFOUR	= dtt[i]*TCUBE
	TEMPA[i]= TEMPA[i]-D2*TSQ[i]-D3*TCUBE-D4*TFOUR
	TEMPE[i]= TEMPE[i]+BSTAR[i]*C5[i]*(sin(XMP[i])-SINMO[i])
	TEMPL[i]= TEMPL[i]+T3COF*TCUBE+TFOUR*(T4COF+dtt*T5COF)
ENDIF

A		= AODP*TEMPA*TEMPA
E		= EO-TEMPE
XL		= XMP+OMEGA+XNODE+XNODP*TEMPL
BETA	= sqrt(1d0-E*E)
XN		= XKE/A^1.5d0

; Long period periodics

AXN 	= E*cos(OMEGA)
TEMP	= 1d0/(A*BETA*BETA)
XLL 	= TEMP*XLCOF*AXN
AYNL	= TEMP*AYCOF
XLT 	= XL+XLL
AYN 	= E*sin(OMEGA)+AYNL

; Solve keplers equation

;CAPU	= AngleRange(XLT-XNODE)
CAPU	= (((XLT-XNODE) mod TWOPI)+TWOPI) mod TWOPI
EPW 	= CAPU


TEMP2	= EPW
SINEPW	= sin(TEMP2)
COSEPW	= cos(TEMP2)
TEMP3	= AXN*SINEPW
TEMP4	= AYN*COSEPW
TEMP5	= AXN*COSEPW
TEMP6	= AYN*SINEPW
EPW 	= (CAPU-TEMP4+TEMP3-TEMP2)/(1d0-TEMP5-TEMP6)+TEMP2

i = 1
j = where( abs(EPW-TEMP2) GT E6A )

WHILE i NE 10 AND j[0] NE -1 DO BEGIN
	TEMP2 [j] = EPW[j]
	SINEPW[j] = sin(TEMP2[j])
	COSEPW[j] = cos(TEMP2[j])
	TEMP3 [j] = AXN[j]*SINEPW[j]
	TEMP4 [j] = AYN[j]*COSEPW[j]
	TEMP5 [j] = AXN[j]*COSEPW[j]
	TEMP6 [j] = AYN[j]*SINEPW[j]
	EPW   [j] = (CAPU[j]-TEMP4[j]+TEMP3[j]-TEMP2[j])/(1d0-TEMP5[j]-TEMP6[j])+TEMP2[j]
	i = i+1
	j = where( abs(EPW-TEMP2) GT E6A )
ENDWHILE

; Short period preliminary quantities

ECOSE	= TEMP5+TEMP6
ESINE	= TEMP3-TEMP4
ELSQ	= AXN*AXN+AYN*AYN
TEMP	= 1d0-ELSQ
PL		= A*TEMP
R		= A*(1d0-ECOSE)
TEMP1	= 1d0/R
RDOT	= XKE*sqrt(A)*ESINE*TEMP1
RFDOT	= XKE*sqrt(PL)*TEMP1
TEMP2	= A*TEMP1
BETAL	= sqrt(TEMP)
TEMP3	= 1d0/(1d0+BETAL)
COSU	= TEMP2*(COSEPW-AXN+AYN*ESINE*TEMP3)
SINU	= TEMP2*(SINEPW-AYN-AXN*ESINE*TEMP3)
;U		= AngleRange( atan(SINU,COSU) )
U		= (atan(SINU,COSU)+TWOPI) mod TWOPI
SIN2U	= 2d0*SINU*COSU
COS2U	= 2d0*COSU*COSU-1d0
TEMP	= 1d0/PL
TEMP1	= CK2*TEMP
TEMP2	= TEMP1*TEMP

; Update for short periodics

RK		= R*(1d0-1.5d0*TEMP2*BETAL*X3THM1)+0.5d0*TEMP1*X1MTH2*COS2U
UK		= U-0.25d0*TEMP2*X7THM1*SIN2U
XNODEK	= XNODE+1.5d0*TEMP2*COSIO*SIN2U
XINCK	= XINCL+1.5d0*TEMP2*COSIO*SINIO*COS2U
RDOTK	= RDOT-XN*TEMP1*X1MTH2*SIN2U
RFDOTK	= RFDOT+XN*TEMP1*(X1MTH2*COS2U+1.5d0*X3THM1)

; Orientation vectors

SINUK	= sin(UK)
COSUK	= cos(UK)
SINIK	= sin(XINCK)
COSIK	= cos(XINCK)
SINNOK	= sin(XNODEK)
COSNOK	= cos(XNODEK)

XMX 	= -SINNOK*COSIK
XMY 	=  COSNOK*COSIK

UX		= XMX*SINUK+COSNOK*COSUK
UY		= XMY*SINUK+SINNOK*COSUK
UZ		= SINIK*SINUK

VX		= XMX*COSUK-COSNOK*SINUK
VY		= XMY*COSUK-SINNOK*SINUK
VZ		= SINIK*COSUK

; Position and velocity

CASE location OF

0: BEGIN

	rr = [[RK*UX],[RK*UY],[RK*UZ],[RDOTK*UX+RFDOTK*VX],[RDOTK*UY+RFDOTK*VY],[RDOTK*UZ+RFDOTK*VZ]]
	rr = transpose(rr)
	rr = rr*(XKMPER/AE)
	rr[3:5,*] = rr[3:5,*]*(XMNPDA/86400d0)

	IF precess THEN BEGIN
		t0 = TimeSet(jepoch=2000.0d0)
		rr[0:2,*] = CvPrecess(tt_obs,t0,rr[0:2,*],/rectangular)
		rr[3:5,*] = CvPrecess(tt_obs,t0,rr[3:5,*],/rectangular)
	ENDIF

END

1: BEGIN

	rr = [[RK*UX],[RK*UY],[RK*UZ]]
	rr = transpose(rr)
	rr = rr*(XKMPER/AE)
	rr = reform(rr,[3,szobs],/overwrite)

	IF precess THEN BEGIN
		t0 = TimeSet(jepoch=2000.0d0)
		rr = CvPrecess(tt_obs,t0,rr,/rectangular)
	ENDIF

END

ENDCASE

IF NOT km THEN rr = rr/(!sun.au*1.0d8)

RETURN, rr  & END