;+
; NAME:
;	COMPENSATOR
;
; PURPOSE:
;	Return the transfer functions of an AO system 
;
; EXPLANATION:
;       
;
; CALLING SEQUENCE:
;	
; INPUTS:
;    frame_rate : frame rate of AO system
;    nFrames : number of frames in diagnostic: defines the sampling in frequency space
;    delay: compute delay of AO system
;    filt: the digital filter used by the controller
;
; OUTPUTS:
;     rejectionTF: rejection transfer function of AO system
;     noiseTF: noise transfer function of AO system
;     openLoopTF: open loop transfer function of AO system
;
; OPTIONAL INPUT KEYWORDS:
;     ttmirror: set to one to use tip-tilt mirror dynamics
;     statespace: uses state space modeling for the dynamnics of the CCD, giving more accurate transfer functions
;     fsamp: the frequency range needed (-fsamp/2,fsamp/2) is different from frame rate 
;     gainVector: the vector of gains over which to optimize if different from default
;
; EXAMPLE:
;     COMPENSATOR,frame_rate,Nframes,3e-4,filt,rejectionTF,gainVector=0.6,/st
;	
; NOTES:
;     State space modeling from Doug Looze
;     Explanation of transfer functions in van Dam, Le Mignant and Macintosh, Applied Optics    
;
; PROCEDURES USED:
;	Functions: TTMIRRORDYNAMICS, SSCCD
;	Procedures	
;
; MODIFICATION HISTORY:
;	Original version: Jan 2006, Marcos van Dam (rewrite of old version for the NGWFC)
;-
PRO COMPENSATOR,frame_rate,nFrames,delay,filt,rejectionTF,noiseTF,openLoopTF,gainVector=gainVector,fsamp=fsamp,ttmirror=ttmirror,statespace=statespace
  
  COMPILE_OPT idl2
  
  IF N_ELEMENTS(fsamp) EQ 0 THEN fsamp=frame_rate ; the maximum frequency at which we want the power spectrum
  IF N_ELEMENTS(ttmirror) EQ 0 THEN ttmirror='0'

  nFrames=FLOAT(nFrames)
  sampfreq=SHIFT((FINDGEN(nFrames)-(nFrames/2.-1))*fsamp/nFrames,nFrames/2.+1)
  
  i=COMPLEX(0,1)
  idx=WHERE(sampfreq EQ 0)
  IF idx[0] ne -1 THEN sampfreq[idx]=1.e-6 ; prevent division by zero at f=0
  s=i*2.*!pi*sampfreq           
  
  T = 1./frame_rate           
  z=EXP(s*T)

  numer=0.
  denom=1.

  sz=size(filt)
  if sz[2] ne 2 then begin
     for k=0,sz[1]-1 do numer=numer+filt[k]*z^(-k)   
     denom=1-z^(-1)
  endif else begin
     for k=0,sz[1]-1 do begin
        numer=numer+filt[k,0]*z^(-k)
        if k ge 1 then denom=denom+filt[k,1]*z^(-k) 
     endfor
  endelse

  filtLaplace=numer/denom
  
; model the mirror dynamics
  CASE ttmirror OF
     'Keck I':  mirrorLaplace = ttmirrordynamics(s,telescope='Keck I')
     'Keck II':  mirrorLaplace = ttmirrordynamics(s,telescope='Keck II')
     ELSE:  mirrorLaplace = 1.
  ENDCASE
    
  IF KEYWORD_SET(statespace) AND (delay GE 2.*T) THEN BEGIN
     MESSAGE,/INFORMATIONAL,'The state space method does not work if the delay is longer than twice the integration time. Using continuous time method and continuing'     
     statespace=0.
  ENDIF
  
  IF KEYWORD_SET(statespace) THEN BEGIN
     sysmir = CREATE_STRUCT('d',1.0)
     sysmird = SSCCD(sysmir,T,delay)
     nf = (SIZE(z))[1]
     ns = (SIZE(sysmird.a))[1]
     mirtf = COMPLEXARR(nf)
     FOR ii = 0 , nf-1 DO mirtf[ii] = sysmird.c ## INVERT(z[ii]*IDENTITY(ns) - sysmird.a) ## sysmird.b + sysmird.d
     plant = mirtf / T
  ENDIF ELSE BEGIN
     stare=(1-exp(-s*T))/(s*T)  ;
     zeroOrderHold=stare
     delayLaplace=exp(-s*delay) ;
     plant = stare*zeroOrderHold*delayLaplace
  ENDELSE
  
  IF NOT KEYWORD_SET(gainVector) THEN gainVector=1.
  
; calculate the maximum gain that is stable: for what gain is the amplitude higher than 1 when the phase is greater than or equal to -180 deg? Limit the gain to those values only.
  
  openLoopTF0=plant*filtLaplace*mirrorLaplace
  wPhase180=MIN(WHERE(ATAN(openloopTF0,/PHASE) GT 0))
  amp180=ABS(openloopTF0[wPhase180])
  maxgain=1./amp180
  
  whereValidGain=WHERE(gainVector LT maxgain)
  IF whereValidGain[0] EQ -1 THEN BEGIN
     MESSAGE,/INFO, 'Control loop is unstable, values are not valid'
  ENDIF ELSE BEGIN
     gainVector=gainVector[whereValidGain]  
     openLoopTF=plant*filtLaplace*mirrorLaplace#gainVector
     rejectionTF = abs(1./(1+openLoopTF))^2.
     noiseTF = abs(openLoopTF/(1+openLoopTF))^2.
  ENDELSE  
  
END