;+
;
; NAME:
;	ORBIT_FILL
;
; PURPOSE:
;	This procedure determines rudimentary orbital elements for the spacecraft
;	orbit.  If the BATSE DISCLA packets contain bad or null spacecraft coordinates,
;	this procedures replaces those coodinates with coordinates computed
;	by assuming circular rotation in the plane defined by the remaining valid
;	coordinates.  This procedure will work even with data gaps provided the 
;	set of coordinates is all contained within a single orbital time period of
;	order 95 minutes.  At least 10 good coordinates must be passed or the 
;	procedure rejects the data.
;
;
; CATEGORY:
;	BATSE
;
; CALLING SEQUENCE:
;	 orbit_fill,ut,sc_new=sc,sc_old=sc_old,bad_data=bad_data,c_period=period,$
;	 d_period=d_period, xyz_orbit=xyz_orbit
;
; CALLS:
;       none
;
; INPUTS:
;	SC_OLD - SPACECRAFT POSITION IN INERTIAL COORDINATES (CELESTIAL), INPUT
;
;	UT - TIME IN SECONDS CORRESPONDING TO THE POSITION VECTOR SC
;
; OPTIONAL INPUTS:
;       none
;
; OUTPUTS:
;	SC_NEW (SC) - CORRECTED SPACECRAFT POSITION.  IF TELEMETRY IS CORRECT THIS
;	       SHOULD BE THE SAME AS SC_OLD.  
;	XYZ_ORBIT= 3X3 ARRAY DESCRIBING ORBIT IN INERTIAL COORDINATES, NORMALIZED 
;	FIRST 3 ELEMENTS, X VECTOR GIVING S/C VECTOR AT UT(GOOD(0))  
;	SECOND 3 ELEMENTS, Y VECTOR IN ORBITAL PLANE 
;	THIRD 3 ELEMENTS, Z VECTOR NORMAL TO ORBITAL PLANE
;	XYZ_ORBIT MUST BE COMPUTED BECAUSE IT IS NEEDED IN OTHER ROUTINES
;	SUCH AS OCCTIME.
;
;	BAD_DATA - SET TO 1 IF THERE ARE TOO FEW ACCEPTABLE COORDINATES TO
;	CALCULATE THE ORBITAL ELEMENTS.
; 
;	C_PERIOD=PERIOD , ORBITAL PERIOD CALCULATED FROM RADIUS OF ORBIT 
;	D_PERIOD=PERIOD , ORBITAL PERIOD CALCULATED FROM ANGULAR CHANGES IN THE
;	SPACECRAFT POSITIONS (SC)
; OPTIONAL OUTPUTS:
;       none
;
; KEYWORDS:
;       none
; COMMON BLOCKS:
;       none
;
; SIDE EFFECTS:
;       none
;
; RESTRICTIONS:
;       none
;
; PROCEDURE:
;       none
;
; Modification History:
;	MODIFIED BY RAS, 92/09/15, TO RETURN XYZ_ORBIT 
;       Mod by AES, 95/06/22, to use f_crossp rather than crossp
;
;-
pro orbit_fill,ut,sc_new=sc,sc_old=sc_old,bad_data=bad_data,c_period=period,$
	d_period=d_period, xyz_orbit=xyz_orbit

sc_old=sc ;save input spacecraft position vectors
bad_data=0
nut=n_elements(ut)
r=sqrt((sc/4.)^2#replicate(1.,3,1)) ;DISTANCE FROM C OF EARTH TO SC IN KM
r3=(reform(r,nut,1)#replicate(1.,1,3)) ; R3 IS DIMENSIONED NUT X 3

;****************************************************************************
;NGOOD IS THE NUMBER OF GOOD COORDINATES
;TEST THE SPACECRAFT ALTITUDE (MEASURED FROM THE CENTER OF THE EARTH)

	good=where( abs(r-7000.) le 500., ngood) ;PROBABLY A VALID COORDINATE

	if ngood lt 2 then goto,BAD_ORBIT
	r_avg=avg(r(good)) ;average orbit
	if (ngood le 10) and (nut gt 2500) then goto,BAD_ORBIT ;ALGORITHM WOULD FAIL

	badxyz=where( abs(r-7000.) gt 500., nbad) ;PROBABLY BAD TELEMETRY

;****************************************************************************


;USE THE SPACECRAFT VECTORS FOUND TO BE IN RANGE (r_avg) TO CONSTRUCT THE
;PLANE OF THE ORBIT AND THE ANGULAR VELOCITY.  FROM THESE QUANTITIES AND THE
;ORBIT RADIUS (r_avg) FILL IN ANY POINTS FOUND TO BE OUT OF RANGE.
;ASSUME A CIRCULAR ORBIT AT A CONSTANT ANGULAR VELOCITY.
;THIS NEGLECTS ANY PRECESSION IN THE ORBITAL PLANE, BUT THIS IS LESS THAN
;HALF A DEGREE PER ORBIT.

;****************************************************************************
;SELECT COORDINATES WHICH HAVE ANGULAR VELOCITIES CLOSE TO THAT COMPUTED 
;USING THE ORBITAL RADIUS.

;1. COMPUTE THE ANGULAR VELOCITY IN RAD/S FROM THE ORBITAL RADIUS, 
;   ROUGHLY 6.28/5500. RAD/SEC
	
	period=9.9522e-3*r_avg^1.5 ;
	ang_vel=2*!pi/period 		

;2. COMPUTE ANGULAR VELOCITY IN RADIANS/SEC ALONG ORBIT BY COMPARING THE
;   ANGULAR DIFFERENCE BETWEEN TWO GROUPS OF COORDINATES.

	sc_unit=sc(good,*)/(4.*r3(good,*)) 	;SPACECRAFT UNIT VECTORS FOR VALID DATA
	ngood=50 < ngood/2			;USE ONLY THE FIRST 50 VALID COORDINATES

	sc1=sc_unit(indgen(ngood)*2,*) 		;FIRST GROUP OF GOOD COORDINATES
	sc2=sc_unit(indgen(ngood)*2+1,*) 	;SECOND GROUP OF GOOD COORDINATES

	ut_subset = ut(good)
	ut1=ut_subset(indgen(ngood)*2) 
	ut2=ut_subset(indgen(ngood)*2+1) 	;TIMES OF GOOD COORDINATES

	dang_dt=acos( (sc1*sc2)#replicate(1.,3,1))/(ut2-ut1) ;RAD/SEC
	good_dt=where( abs((dang_dt/ang_vel)-1.) le 0.1, ngood_dt) ;WITHIN RANGE?

	if ngood_dt eq 0 then goto,bad_orbit ; ADDED BY AKT ++++++++++++++

;3. FINALLY, AVERAGE THE ANGULAR VEL.

	av_dang_dt = avg(dang_dt(good_dt)) 
	d_period   = 2*!pi/av_dang_dt

;****************************************************************************
;NOW THAT WE HAVE FOUND AN AVERAGE RADIUS AND ANGULAR VELOCITY FROM COORDINATES
;WHICH HAVE REASONABLE VALUES AND THUS ARE BELIEVED TO BE CORRECT, TAKE A 
;SINGLE SPACECRAFT VECTOR AND GENERATE FILL VALUES AND A TRANSFORMATION
;MATRIX (XYZ_ORBIT) BETWEEN CELESTIAL AND ONE IN WHICH THE ORBIT LIES IN THE
;XY PLANE
 
;FIND THE AVERAGE UNIT NORMAL
;TAKE FIRST GOOD SC VECTOR AS X, GENERATE Y FROM NORMAL AND X
;THEN, GENERATE CIRCULAR MOTION FROM COS(THETA)*X+SIN(THETA)*Y

	normal= rebin(f_crossp(transpose(sc1),transpose(sc2)),3,1) 
	normal=normal/sqrt(total(normal^2)) 	      ;AVERAGE NORMAL
	xrot  =sc1(0,*)                               ;FIRST NORMALIZED POSITION VECTOR
	yrot  =f_crossp(normal,xrot)                    ;PERP. TO XROT IN ORBITAL PLANE
	xyz_orbit = reform([xrot(*),yrot(*),normal(*)],3,3) ;TRANSFORMATION MATRIX
;
;IF THERE ARE BAD POSITIONS, FILL THEM BY PROPAGATING THE ORBIT
;
	if nbad ge 1 then begin
		ang    =reform(av_dang_dt*(ut(badxyz)-ut1(0))) ;ANGLE MOVED IN ORBIT SINCE UT1(0)
		sc_fill= cos(ang)#xrot + sin(ang)#yrot        ;ORBITAL POSITION AT UT(BADXYZ)
		;FILL WITH EARTH-CENTERED INERTIAL X,Y,Z IN 1/4 KM UNITS
		sc(badxyz,*)=fix(r_avg*sc_fill*4)
	endif
	return 

bad_orbit: bad_data=1 ;SET FLAG FOR BAD COORDINATES IN ENTIRE ORBIT

return
end