;; From Elia Battistelli
;; http://actexperiment.info/roundtable/uploads/ActWiki/filter_pro
;; Modified by Joe Fowler to include the delta-function response
;; Modified by Mandana Amiri for Scuba2 settings, 9.5kHz sampling and decimated to 190Hz
pro filter

device,decomposed=0
loadct,39

;b_1_1=-1.9587428340882587
;b_1_2=0.96134553442399129
;b_2_1=-1.9066292518523
;b_2_2=0.90916270571237567

b_1_1=double(-2.*32092./2.^15.)
b_1_2=double(2.*15750./2.^15.)
b_2_1=double(-2.*31238./2.^15.)
b_2_2=double(2.*14895./2.^15.)

CLOCK_PERIOD=20e-9              ; 50 MHz clock
ROW_DWELL=128                   ; time to dwell at each row (in clocks)
NUM_ROWS=41                     ; number of rows addressed
delta_time = CLOCK_PERIOD*ROW_DWELL*NUM_ROWS
f_samp=1./delta_time

nf=30000.
freq=(1./(nf-1.))* findgen(nf)* f_samp

!p.multi=[0,1,3]
!p.charsize=2.4

;window,5,xs=850,ys=600,title='filter'

;H(Z) = [1 + 2*Z^(-1) + Z^(-2)] / [1+b1*Z^(-1) + b2*Z^(-2)]

omega=freq/max(freq)*2.*!pi

h1_omega=complex(n_elements(omega))
h2_omega=complex(n_elements(omega))
h_omega=complex(n_elements(omega))

h1_omega=(1.+2.*complex(cos(-omega),sin(-omega))+ $
          complex(cos(-2.*omega),sin(-2.*omega)))/ $
  (1.+b_1_1*complex(cos(-omega),sin(-omega))+ $
   b_1_2*complex(cos(-2.*omega),sin(-2.*omega)))
h2_omega=(1.+2.*complex(cos(-omega),sin(-omega))+ $
          complex(cos(-2.*omega),sin(-2.*omega)))/ $
  (1.+b_2_1*complex(cos(-omega),sin(-omega))+ $
   b_2_2*complex(cos(-2.*omega),sin(-2.*omega)))

h_omega=h1_omega*h2_omega/2048.;/max(h1_omega*h2_omega/2048.)

ind_f3db=where(abs(abs(h_omega(0:300))-max(abs(h_omega(0:300)))/sqrt(2)) eq min(abs(abs(h_omega(0:300))-max(abs(h_omega(0:300)))/sqrt(2.))))

phase=atan(imaginary(h_omega),float(h_omega))/2./!pi*360.
delta_response = float(reverse(fft(h_omega)))

for i=0,n_elements(phase)-1 do if imaginary(h_omega(i)) gt 0 then phase(i)=phase(i)-360.

; forward Plots to eps file
;set_plot, 'ps'
;device, encapsulate
;device, filename='/home/mandana/filter_response.ps', /landscape, ysize=18, xsize=26


; Plot the amplitude gain
plot,freq,abs(h_omega),xr=[0.,300.],xtit='Frequency (Hz)', $
  ytit='Gain'                   ;,/ylog
oplot,[0,n_elements(h_omega)-1],[max(abs(h_omega(0:300)))/sqrt(2.),max(abs(h_omega(0:300)))/sqrt(2.)],linestyle=2
oplot,[freq(ind_f3db),freq(ind_f3db)],[min(abs(h_omega(0:300))),max(abs(h_omega(0:300)))],linestyle=2
xyouts, 25.5, 600, 'F3dB= '+string(freq(ind_f3db))+' Hz', charsize=1.4

; Plot the phase
plot,freq,phase,xr=[0.,300.],xtit='Frequency (Hz)',ytit='Phase (deg)'

; Plot the delta-function response
n_time_samp=300
fast_times=findgen(n_time_samp)*delta_time
plot,1e3*fast_times,delta_response,xtit='Time (ms)', $
  ytit='!4d!X-function response'
oplot,[9,10],15+[0,0]
xyouts,10.5,15-1,'9.5 kHz response',charsize=1.4

DECIMATION_FACTOR=50            ; How many array reads per sample stored
slow_times = rebin(fast_times, n_time_samp/DECIMATION_FACTOR, $
                   /sample)
delta_decimated = rebin(delta_response[0:n_time_samp-1], $
                        n_time_samp/DECIMATION_FACTOR, /sample)
oplot,1e3*slow_times,delta_decimated,psym=2
print,total(delta_response)
oplot,[10],[10],psym=2
xyouts,10.5,10-1,'190.54 Hz decimated response',charsize=1.4

;device, /close
;set_plot, 'win'

; Text report
print,'DC amplification= '+string(max(abs(h_omega)))
print,'F3dB= '+string(freq(ind_f3db))+' Hz'
;print,'Omega= '+string(omega(ind_f3db))
print,'Gain 100Hz= '+string(abs(h_omega(190))/max(abs(h_omega)))
end