;+
; NAME: ls_amp_pha_ctr.pro
;       
; PURPOSE: Returns the least-square fit of y(angle) to the function
;          A*cos(phase_map_ctr(angle))+B*sin(phase_map_ctr(angle))
;          or equivalently, AMP*cos(phase_map_ctr(angle)-PHA).
;          The result can be used to fit visibilities to HESSI
;          countrates, even in the ranges of slow modulation.
;
; METHOD:
;     Theta=phase_map_ctr
;     Minimize the total squared deviation H of the fit to
;     y[i] = A*cos(Theta[i])+B*sin(Theta[i]))
;
;     H=total{ [y - A*cos(Theta) - B*sin(Theta)]^2 }
;
;    If the angle[i] are chosen to make the cos and sin terms uncorrelated
;    then the regression matrix is diagonal.  If, they are well correlated
;    or nearly anti-correlated, the matrix is singular, and the method fails.
;    The results may be meaningless unless r_p has been
;    chosen such that phi[angle] ranges over a large fraction of [0,2*!pi].
; 
; INPUTS:
;       y=modulation profile (countrates), one for each angle
;       angle=vector of angles (radians), possibly irregular
;       phase_map_ctr=Array of phases of map center relative to line
;          of maximum response, one for each roll angle
; 
; OUTPUTS:
;        [A,B] cos,sin amplitudes, OR
;        [amplitude,phase] if optional amp_pha is set, 
;        If sigma exists as an argument, the RMS deviation of the fit
;        is returned in sigma.
;        If y has only 2 elements, sigma is necessarily 0 (exact fit).
;        If yfit is a defined argument, fit to y will be returned in it.
;
; EXAMPLE:
;     ;Simple simulation of map-center phases
;     ;Create an array of angles: 
;         N=512
;         angle=findgen(N)*!pi/N - !pi/4
;     ;Create some aspect coords:
;         x_asp=1.0*randomn(seed,N)
;         y_asp=1.0*randomn(seed,N)
;     ;Select an inertial map center:
;         X_map=600.
;         Y_map=200.
;     ;Compute the spacecraft coords of map center:
;         x_map_sc = x_asp + X_map*cos(angle) - Y_map*sin(angle) 
;         y_map_sc = y_asp + Y_map*cos(angle) + X_map*sin(angle) 
;     ;Choose a value for the peak_resp_offset (normally from hessi_grm):
;          peak_resp_offset=!pi/6
;     ;Pick a sub_collimator:
;          ang_pitch=39.45 & det_index=4
;          grid_angle=0.    & h=1 
;     ;Now make the array of map-center phases:
;
;     phase_map_ctr=2*!pi*h*(x_map_sc*cos(grid_angle)+y_map_sc*sin(grid_angle)$
;         +peak_resp_offset)/ang_pitch 
;     ;Select a range of indices:
;          inx=180+findgen(20) 
;           y=cos(phase_map_ctr)+0.25*randomn(seed,N)
;     z=ls_amppha_pmc(angle(inx),y(inx),phase_map_ctr(inx),yfit,sig)
;           plot,angle,y,psym=10,xsty=1 & oplot,angle(inx),yfit,thick=3
;
; RESTRICTIONS:
;       If the data set is enormous, this is slow.  
;       For a way to speed it up, 
;       see Press and Rybicki, Ap.J., 338, 277, 1989.
;
;         angle must =0 or !pi where the modulation is slowest, and
;         angle must= +!pi/2 or -!pi/2 where modulation is fastest
;
; REVISION HISTORY:
;  Version 1.0, Derived from ls_coscos, EJS, June 15, 1999.
;  Version 2, Added double and bad-fit flag, EJS Sept 30, 1999
;  NOTE: if the sine vector is nearly constant, a bad fit results.
;   This is because the sine is at an extremum and the cosine is 
;   very small, so it has to cover all the y variability, and its
;   coefficient will be large. (Similarly if cosine = const.)
;-

function ls_amp_pha_ctr,angl,y,phase_map_ctr,yfit,sigma, $
               AMP_PHA=amp_pha,verb=verb,double=double,flag=flag
; message,'stopping'
; Compute the sine and cosine arrays

if keyword_set(double) then begin
 if keyword_set(verb) then message,'Going to double precision',/continue
 phase_map_ctr=double(phase_map_ctr)
endif

cosine=cos(phase_map_ctr)
sine=sin(phase_map_ctr)   
if (keyword_set(verb)) then message, $
   'Working with '+String(n_elements(phi))+' angles',/cont
            ; The regression matrix
m00=total(cosine^2)  & m01=total(sine*cosine)
m11= total(sine^2)

del=m00*m11-m01^2
if abs(del) lt 1.e-2*(abs(m00)+abs(m11)) then begin
  message,'Ill-conditioned LS fit.',/continue
  ;message,string(del)+string(m00)+string(m01)+string(m11),/continue
  flag=1
endif
c0=total(y*cosine)
c1=total(y*sine)

A=(c0*m11-c1*m01)/del  ; Solution to M # [A,B] = [c0,c1]
B=(c1*m00-c0*m01)/del

AB=[A,B]

yfit=A*cosine+B*sine  ; = amp*cos(phase_map_ctr-pha)
; compute the goodness of fit
if (n_elements(sigma) GT 0) then sigma=sqrt(variance(y-yfit))


if keyword_set(amp_pha) then AB=[sqrt(a^2+b^2),atan(B,A)]

return,AB

end