;+
; NAME:
;	FitKeplerOrbit
; PURPOSE:
;	For a given set of orbital data, find the best-fitting orbital parameters
;	for a simple Kepler orbit
; CATEGORY:
;	Celestial mechanics
; CALLING SEQUENCE:
	PRO FitKeplerOrbit, input_data, orbit=Orbit, degrees=Degrees
; INPUTS:
; OPTIONAL INPUT PARAMETERS:
; OUTPUTS:
; OPTIONAL OUTPUT PARAMETERS:
; CALLS:
;	IsType, flt_read, ToRadians, EulerRotate
; INCLUDE:
	@compile_opt.pro		; On error, return to caller
; SIDE EFFECTS:
; RESTRICTIONS:
; 	Looking down along tav the motion is direct (counterclockwise), IF the two
; 	points used to determine tav are less then 180 deg away from each other
; 	(measured in the direction of motion).
; PROCEDURE:
; MODIFICATION HISTORY:
;	SEP-1997, Paul Hick (UCSD)
;-

Orbit = fltarr(7)									; Output array for orbital elements

tmp = size(input_data)
CASE IsType(input_data, /string) OF
0: pp = input_data
1: IF NOT flt_read(input_data[0],pp, error=tmp) THEN message, tmp ; File not found: exit
ENDCASE

tmp = size(pp)
IF tmp[0] NE 2 OR (tmp[1] NE 4 AND tmp[1] NE 5) THEN	$
	message, 'Improper input: must be (yr,doy,lng,lat,dist) or (jd,lng,lat,dist)'

nr = tmp[2]											; # points in orbit
IF nr LT 3 THEN message, 'Specify at least three points in orbit'

CASE tmp[1] OF								; Convert times to Julian days.
4: BEGIN									; (jd,lng,lat,dist)
	jd = pp[0,*]
	pp = pp[1:3,*]							; longitude, latitude, distance
END
5: BEGIN									; (yr,doy,lng,lat,dist)
	Julian, 0, round(pp[0,*]), pp[1,*], jd
	pp = pp[2:4,*]							; longitude, latitude, distance
END
ENDCASE

jd = reform(jd)
pp[0:1,*] = pp[0:1,*]*ToRadians(degrees=Degrees); Convert to radians

tmp = sort(jd)								; Sort the input data if necessary
IF (where(tmp NE indgen(nr)))[0] NE -1 THEN BEGIN
	message, 'Data not chronological: sorting input data'
	jd = jd[tmp]
	pp = pp[2:4,tmp]
ENDIF

rr = cv_coord(from_sphere=pp, /to_rect)		; Convert to rectangular (xyz-) coordinates

; Determine the normal to orbit. Unit vectors along the cross product for
; all different pairs of orbital vectors are averaged together to determine the
; normal vector of the orbital plane.

; To avoid problems with the cross products do not use pairs of orbital
; vectors which are nearly (anti-)parallel.

cosangmax = cos(15./!radeg)			; At least 15 deg away from being parallel

s = rr[*,0]#replicate(1,nr-1)		; First orbit vector
r = rr[*,1:nr-1]					; All remaining vectors

; Find the first pair of vectors with an angle of at least 15 deg between them

cosang = total(s*r,1)/sqrt(total(s*s,1)*total(r*r,1))	; cosine(angle(s,r))
tmp = where(abs(cosang) lt cosangmax)
IF tmp[0] EQ -1 THEN message, 'Error determining direction of orbit motion'

s = rr[*,0]							; Earliest point in orbit
r = rr[*,tmp[0]]					; First point more than 15 deg away
t = [s[1]*r[2]-s[2]*r[1],	$		; Cross product s x r
	 s[2]*r[0]-s[0]*r[2],	$
	 s[0]*r[1]-s[1]*r[0]]

; tav is a unit vector (almost) perpedicular to the orbital plane.
; Looking down along tav the motion is direct (counterclockwise), IF the two
; points used to determine tav are less then 180 deg away from each other
; (measured in the direction of motion).

tav = t/sqrt(total(t*t))			; First estimate of normal vector

FOR i=1,nr-1 DO BEGIN				; Calculate normal vectors for all different pairs
	s = (shift(rr,0,i))[*,0:nr-1-i]
	r = rr[*,i:nr-1]
									; cosine(angle between s and r)
	cosang = total(s*r,1)/sqrt(total(s*s,1)*total(r*r,1))
	tmp = where(abs(cosang) LT cosangmax, ntmp)

	; Only use pairs of data points which are more than 15 degrees away from being
	; parallel or anti-parallel.

	IF ntmp GT 0 THEN BEGIN
		s = s[*,tmp]
		r = r[*,tmp]
		s = [s[1,*]*r[2,*]-s[2,*]*r[1,*],	$		; Cross product s x r
			 s[2,*]*r[0,*]-s[0,*]*r[2,*],	$
			 s[0,*]*r[1,*]-s[1,*]*r[0,*]]
		s = s/([1,1,1]#[sqrt(total(s*s,1))])		; Unit vector

		; The unit vectors just calculated will be either parallel or
		; anti-parallel to the first estimate, tav, of the normal vector.
		; Find a anti-parallel unit vectors and convert them to parallel.

		tmp = total( (tav#replicate(1,ntmp))*s, 1)	; Should be almost +/-1
		tmp = round(tmp)							; Should be +/-1
		s = s*([1,1,1]#[tmp])						; Pick normal almost parallel to tav

		CASE n_elements(t) OF
		0   : t = s			; Collect all unit vectors in t
		ELSE: t = [[t],[s]]
		ENDCASE

	ENDIF
ENDFOR

; Average all the unit vectors. This is the normal vector of the orbital
; plane (note that it will not exactly be a unit vector).

i   = (size(t))[2]									; # unit vectors collected
tav = total(t,2)/i									; Average: normal to orbital plane
tsd = sqrt( (total(t*t,2)-i*tav*tav)/(i-1) )		; Standard deviation of normal

print, 'Normal to orbital plane (AU) = ',tav
print, 'Standard deviation of normal (AU) = ',tsd

; Convert orbital data to coordinate system with the normal
; vector as z-axis and the direction to the ascending node as x-axis.
; The Euler angles for the required rotation are ABG (in radians).

ABG = [ atan(tav[1],tav[0]), acos( tav[2]/sqrt(total(tav*tav)) ), !pi/2.]

Orbit[2] = ABG[1]*!radeg
Orbit[3] = (90+ABG[0]*!radeg+360) mod 360

print, 'Inclination of orbit     (deg) =',Orbit[2]
print, 'Longitude ascending node (deg) =',Orbit[3]

pp[0:1,*] = EulerRotate(ABG,pp[0:1,*])				; Rotate to orbital plane

; After the rotation to the orbital plane the declinations should be zero.

dav = total(pp[1,*])/nr								; Average declination (should be = 0)
dsd = sqrt( (total(pp[1,*]*pp[1,*])-nr*dav*dav)/(nr-1) )	; Standard deviation

print, 'After rotation to orbital plane average declination =',dav, ' +/-',dsd

sL 		= sin(pp[0,*])
cL 		= cos(pp[0,*])

sinL	= total( sL )
cosL	= total( cL )
sin2L	= total( sL*sL )
sinLcosL= total( sL*cL )
cos2L	= total( cL*cL )

tmp 	= 1/pp[2,*]
S		= total( tmp )
SsinL	= total( sL*tmp )
ScosL	= total( cL*tmp )

sL0 = SsinL*(nr*cos2L   -cosL*cosL)-ScosL*(nr*sinLcosL-sinL*cosL)-S*(sinL*cos2L   -cosL*sinLcosL)
cL0 = SsinL*(nr*sinLcosL-sinL*cosL)-ScosL*(nr*sin2L   -sinL*sinL)-S*(sinL*sinLcosL-cosL*sin2L   )
L0 = atan(sL0,-cL0)

Orbit[4] = Orbit[3]+L0*!radeg

sL0 = sin(L0)
cL0 = cos(L0)

aa	= cL0*cL0*(S*cos2L-ScosL*cosL)+sL0*cL0*(2*S*sinLcosL-ScosL*sinL-SsinL*cosL)+sL0*sL0*(S*sin2L-SsinL*sinL)
ee	= (cL0*(nr*ScosL-S*cosL)+sL0*(nr*SsinL-S*sinL))/aa
aa	= (cL0*cL0*(nr*cos2L-cosL*cosL)+2*sL0*cL0*(nr*sinLcosL-sinL*cosL)+sL0*sL0*(nr*sin2L-sinL*sinL))/(aa*(1-ee*ee))

Orbit[0] = aa
Orbit[1] = ee

print, 'Semi-major axis (AU) =',Orbit[0]
print, 'Eccentricity =',Orbit[1]
print, 'Argument  of perihelion (deg) =', Orbit[4]-Orbit[3]
print, 'Longitude of perihelion (deg) =', Orbit[4]

sinE = (pp[2,*]/aa)*(sL*cL0-cL*sL0)/sqrt(1-ee*ee)
cosE = (pp[2,*]/aa)*(cL*cL0+sL*sL0)+ee
E	 = atan(sinE,cosE)						; Mean anomaly: !pi*[-1,1]

mu		= 0.1*float(!sun.gm)				; units 10^27 cm^3/s^2
AU		=     float(!sun.au)				; units 10^13 cm
nn = sqrt(mu/(AU*aa)^3)*0.086400			; Mean (angular) motion (radians/day)
T  = 2*!pi/nn								; Orbital period (days)
t0 = jd-(E-ee*sinE)/nn

tmp = where(t0-min(t0) GT 0.5*T)
WHILE tmp[0] NE -1 DO BEGIN
	message, /info, 'Adjusting time array'
	t0[tmp] = t0[0]-T
	tmp = where(t0-min(t0) GT 0.5*T)
ENDWHILE

tmp	= t0[0]
t0	= t0-tmp
tav = total(t0)/nr
tsd = sqrt( (total(t0*t0)-nr*tav*tav)/(nr-1) )	; Standard deviation
tav	= tmp+tav								; Time of perihelion passage (JD)

Julian, 1,iYr,Doy,tav
Date_Doy, iYr, Mon, Day, Doy, month=Month, /getdate

Orbit[5] = iYr
Orbit[6] = Doy

print, 'Time of perihelion passage:'
print, '  JD',tav,' +/-',tsd
print, '        ',iYr,' - ',Month,' -',Day,' (Doy ',Doy,')'

RETURN  &  END