;+
; PROJECT:
;	HESSI
;
; NAME:
;	hsi_remove_sinusoid
;
; PURPOSE:
; 	Identifies the contribution of a sinusoid of known period, P, to an input function, y(x) of the form,
; 		y = A(x) + C*cos(2pi*x/P) + S*sin(2pi*x/P) where it is assumed that A(x) has no Fourier content
;			with period, P.
; 	Output can be one or more of several forms:
;		(XM,YM) = specifies a centroid point which is independent of the amplitude and phase of the sinuisoid.
;		(C,S) 	= constants which specify the cosine and sine components of the best-fit sinusoid.
;		Yresid	= values of y(x) after removal of the sinusoid.
; 	Although derived from Lagrange multipliers, the final result is mathematically equivalent to a
;		least squares fit.
;
; CATEGORY:
;	HESSI/UTIL
;
; CALLING SEQUENCE:
;	output = hsi_remove_sinusoid (x,y, period, YRESID=yresid, DOUBLE=double, NOMODPARM=nomodparm, POLAR=polar)
;
; CALLS:
;	none
;
; INPUTS:
; 	x,y are a pair of N-element vectors defining y(x)
;
; OPTIONAL INPUTS:
; 	period of sinusoid (If not specified, default = 2pi)
;
; OUTPUTS:
; 	Output = vector, [XM,YM,C,S].
;
; OPTIONAL OUTPUTS:
; 	yresid specifies an N-element vector containing the input values, A(x), with sinusoid removed.
;   yerror = rms value of yresid.
;
; KEYWORDS:
; 	NOMODPARM 	= 1 if only XM,YM are required on output. (default = 0)
; 	DOUBLE    	= 1 if calculation should be done in double precision. (default = 0)
;	POLAR		= 1 if C,S output is to be returned as amplitude and phase(degrees) instead of default, cosine, sine.
;
; COMMON BLOCKS:
;	none
; SIDE EFFECTS:
;	none
;
; RESTRICTIONS:
; 	Algorithm assumes that x is specified at 3 or more values which are well-spaced, modulo P.
;
; MODIFICATION HISTORY:
; 27-Dec-03 	Initial version (ghurford@ssl.berkeley.edu) adapted from a Fortran SRBL
;						routine, MODFIT as documented in modfit.doc (4-10-94).
; 12-Apr-04 gh	Generalized to accomodate Period input parameter
;				Added output yresid and double output keywords.
;				Minor improvements to improve speed and transparency of code.
; 14-Apr-04 gh	Added ssw wrapper
;  2-Nov-04 gh	Provide graceful return if input arrays are too small.
;				Add POLAR keyword.
;-

FUNCTION hsi_remove_sinusoid, x,y, period, YRESID=yresid, DOUBLE=double, NOMODPARM=nomodparm, POLAR=polar
;
; First convert x-array to phases.
IF KEYWORD_SET(double) EQ 0 THEN twopi = 2*!PI ELSE twopi = 2*!DPI
IF N_PARAMS() EQ 3 THEN phz = x * twopi/period ELSE phz = x		;
IF N_ELEMENTS(x) LT 3 THEN RETURN, [0,0,0,0]		; return zeros if input arrays are too small
;
; To detemine the centroid point, XM,YM, first calculate some sums of interest.
sinphz	= SIN(phz)
cosphz	= COS(phz)
tsin 	= TOTAL(sinphz)
tcos 	= TOTAL(cosphz)
tsin2 	= TOTAL(sinphz^2)
tcos2	= TOTAL(cosphz^2)
tsincos	= TOTAL(sinphz*cosphz)
denom	= tsin2*tcos2 - tsincos^2
p		= (tsin*tcos2 - tcos*tsincos) / denom
q 		= (tcos*tsin2 - tsin*tsincos) / denom
;
; This yields an optimized set of weights,w, which can be used to calculate XM,YM values that are
;		independent of C,S, while the variance in w is minimized.
;
w		= p*sinphz + q*cosphz -1
wsum	= TOTAL(w)
XM		= TOTAL(w*x)/wsum
YM		= TOTAL(w*y)/wsum
;
; RETURN if /nomodparm was set, otherwise calculate the C,S too.
IF KEYWORD_SET(nomodparm) THEN RETURN, [XM,YM,0., 0.]
;
; Otherwise calculate the fitted parameters and residual function
tzsin	= TOTAL((y-YM)*sinphz)
tzcos	= TOTAL((y-YM)*cosphz)
c 		= (tsin2*tzcos - tzsin*tsincos)/denom
s		= (tcos2*tzsin - tzcos*tsincos)/denom
yresid	= y - C * cosphz - S * sinphz
IF KEYWORD_SET(polar) NE 0 THEN BEGIN
	out = CV_COORD(FROM_RECT=[c,s], DOUBLE=KEYWORD_SET(double), /TO_POLAR, /DEGREES)
	c = out[1]
	s = out[0]
ENDIF
RETURN, [XM,YM,C,S]
END
