#!/usr/bin/env python
# coding: utf-8

# # MICH Calibration

# This document explains the calibration procedure and results of the iKAGRA MICH signal

# In[51]:


import numpy as np
pi = np.pi
import sympy as sp
from sympy.abc import s
sp.init_printing()
import matplotlib.pyplot as plt
get_ipython().run_line_magic('matplotlib', 'inline')
from unit import *
import ltiutils as ltu


# ## 1. Optical Gain 

# In order to measure the optical gain, MICH lock was first turned off. After adjusting the alignment to maximize the beam power coming back to REFL from the each arm, MICH was shaken sinusoidally by injecting a signal of amplitude 1000 count at 0.2Hz into the K1:LSC-MICH_EXC channel. All the filters in the filter bank were turned off and the gain was set to 1. So this signal was directly sent to the MICH actuator. The observed fringes in REFL DC and also in REFL RF error signal are shown below.

# In[52]:


DC=np.loadtxt('data/Fringes/DC.dat', comments='#')
RF=np.loadtxt('data/Fringes/RF.dat', comments='#')
t = DC[:,0]
t = t - t[0]
dc = DC[:,1]
rf = RF[:,1]
fig = plt.figure()
ax1 = fig.add_subplot(1,2,1)
ax2 = fig.add_subplot(1,2,2)
ax1.set_axisbelow(True)
ax2.set_axisbelow(True)

ax1.plot(t,dc)
ax2.plot(t,rf)

ax1.grid(True, color=(0.6,0.6,0.8),ls='-')
ax1.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')
ax2.grid(True, color=(0.6,0.6,0.8),ls='-')
ax2.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')

ax1.set_ylabel('Counts')
ax1.set_xlabel('Time [sec]')
ax2.set_xlabel('Time [sec]')

ax1.set_title('K1:LSC-REFL_PDA1_DC_OUT')
ax2.set_title('K1:LSC-REFL_PDA1_RF17_Q_ERR_EQ')
fig.set_size_inches(15,5,forward=True)


# REFL DC power max is 7000, minimum is 1500.

# In[53]:


Pmax = 7000.0
Pmin = 1500.0
Contrast = (Pmax - Pmin)/(Pmax + Pmin)
print("Contrast is {0:.3}%".format(100*Contrast))


# The amplitude of the DC signal Adc:

# In[54]:


Adc = Pmax - Pmin


# The amplitude of the RF signal Arf:

# In[55]:


Arf = np.max(rf)-np.min(rf)


# The slope of the signal at the middle fringe is $A/2\cdot 2\pi/\lambda\cdot 2\Delta L=\frac{2\pi A}{\lambda}\Delta L$

# In[56]:


wl = 1064*nm #Wave length
Hdc = 2*pi*Adc/wl #Optical Gain measured by the DC signal [cnts/m]
Hrf = 2*pi*Arf/wl #Optical Gain measured by the RF signal [cnts/m]
print("Hdc={0:.3}[cnt/m]\nHrf={1:.3}[cnt/m]".format(Hdc, Hrf))


# ## Actuator Efficiency

# Now we locked the MICH using the DC signal at the middle frindge. 
# An excitation signal is injected just before the LSC filter and the transfer function from the output of the filter (= input to the MICH actuator) to the input to the filter (= the error signal) was measured.

# In[57]:


TF=np.loadtxt('data/data/DC-Lock-MICH-OUT-to-DCPD-OUT.txt', comments='#')
f = TF[:,0]
real = TF[:,1]
imag = TF[:,2]
mag = np.sqrt(real**2+imag**2)
phase = rad2deg(np.angle(1j*imag+real))

fig = plt.figure()
ax1 = fig.add_subplot(2,1,1)
ax2 = fig.add_subplot(2,1,2)
ax1.set_axisbelow(True)
ax2.set_axisbelow(True)

ax1.semilogx(f, 20*np.log10(mag))
ax2.semilogx(f,phase)

ax1.grid(True, color=(0.6,0.6,0.8),ls='-')
ax1.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')
ax2.grid(True, color=(0.6,0.6,0.8),ls='-')
ax2.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')

ax1.set_ylabel('Magnitude [dB]')
ax2.set_ylabel('Phase [deg]')
ax2.set_xlabel('Frequency [Hz]')


ax1.set_title('K1:LSC-REFL_PDA1_DC_OUT/K1:LSC-MICH_OUT_DQ')
fig.set_size_inches(12,8,forward=True)


# The actuator efficiency at 100Hz is the following.

# In[58]:


tf100=np.interp(100,f,mag) #Transfer function magnitude at 100Hz. This is in the unit of [cnt/cnt]
print("Transfer function magnitude at 100Hz is {0:.3}".format(tf100))
#The actuator efficienty at 100Hz can be obtained by dividing tf100 with Hdc
A100 = tf100/Hdc # in the unit of m/cnt
print("Actuation efficiency at 100Hz is {0:.3}[m/cnt]".format(A100))


# The slope of the actuator efficiency should be $\propto 1/f^2$. The actuation efficiency above the resonant frequency of the pendulum ($\simeq$1Hz) can be written $A=\alpha/f^2$.

# In[59]:


alpha = A100*100**2 # [m*Hz^2/cnt]
print("Actuation efficiency is {0:.3}/f^2[m/cnt]".format(alpha))


# ## RF Optical Gain from actuation efficiency 

# Now we have the actuator efficiency. We can measure the RF optical gain by locking the MICH with RF and making a transfer function measurement.

# In[60]:


TF=np.loadtxt('data/data/RF-Lock-MICH-OUT-to-RFPD-OUT.txt', comments='#')
f = TF[:,0]
real = TF[:,1]
imag = TF[:,2]
mag = np.sqrt(real**2+imag**2)
phase = rad2deg(np.angle(1j*imag+real))

fig = plt.figure()
ax1 = fig.add_subplot(2,1,1)
ax2 = fig.add_subplot(2,1,2)
ax1.set_axisbelow(True)
ax2.set_axisbelow(True)

ax1.semilogx(f, 20*np.log10(mag))
ax2.semilogx(f,phase)

ax1.grid(True, color=(0.6,0.6,0.8),ls='-')
ax1.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')
ax2.grid(True, color=(0.6,0.6,0.8),ls='-')
ax2.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')

ax1.set_ylabel('Magnitude [dB]')
ax2.set_ylabel('Phase [deg]')
ax2.set_xlabel('Frequency [Hz]')


ax1.set_title('K1:LSC-MICH_IN1_DQ/K1:LSC-MICH_OUT_DQ')
fig.set_size_inches(12,8,forward=True)


# The magnitude of this transfer function at 100Hz can be divided by the actuation efficiency at 100Hz to give the RF optical gain.

# In[61]:


tf100=np.interp(100,f,mag) #Transfer function magnitude at 100Hz. This is in the unit of [cnt/cnt]
print("Transfer function magnitude at 100Hz is {0:.3}".format(tf100))
Hrf2 = tf100/A100
print("Hrf from the actuation efficiency = {0:.3}[cnt/m]\nHrf from fringe amplitude = {1:.3}[cnt/m]".format(Hrf2,Hrf))


# ## Open Loop Gain Model

# Now we try to make a theoretical model of the open loop gain

# ### Actuator

# In[75]:


#Actuator model
Pend = ltu.lpf2(fc=0.93,Q=4.85) #Pendulum LTI model
(w,h)=Pend.freqresp(w=2*pi*100) #Pendulum response at 100Hz
Pend = ltu.ltiprod(Pend, alpha/100**2/np.abs(h)) #Correct the gain of the pendulum response

#Coil inductance
Rout = 73 #[Ohm] Coil driver's output resistance
Lc = 10e-6 #[H] Coil inductance ** NEED TO CHECK THIS VALUE **
fc = Rout/Lc/2/pi
Coil = ltu.lpf1(fc=fc)

Act = ltu.ltiprod(Pend, Coil)

(w,h)=ltu.bode(Act)


# ### Feedback Filters

# In[63]:


#Feedback filters
LagLead = ltu.ltiprod(ltu.zero(70),ltu.lpf1(fc=2000),ltu.lpf1(fc=2000))
Boost = ltu.ltiprod(ltu.lpf1(fc=0.02),ltu.zero(0.2),10)

MICH_GAIN = 50 #Gain in the K1:LSC-MICH filter bank
UGF_SERVO_GAIN = 1/1.12 #Gain of the UGF Servo

(w,h)=ltu.bode(LagLead)
(w,h)=ltu.bode(Boost)


# ### DGS filters

# The total transfer function of the digital system from the analog AA filter to the analog AI filter is reported [here](http://gwwiki.icrr.u-tokyo.ac.jp/JGWwiki/CLIO/Tasks/DigitalControl/Caltech_setup?action=AttachFile&do=view&target=analog_system_investigation.pdf).

# In[64]:


#Time delay in the DGS
Td=70e-6 + 3000.0/3e8 #70usec DGS delay + 3000m light travel time

P0 = ltu.pole2(10.304784e3,1.0159584)
P1 = ltu.pole(51.53319e3)
P2 = ltu.pole(7.8430016e3)
P3 = ltu.pole(346.96389e3)
Z0 = ltu.zero2(67.205279e3,68.668973e6)

AA = ltu.ltiprod(P0,P1,P2,P3,Z0)
AI = ltu.ltiprod(P0,P1,P2,P3,Z0)

P0 = ltu.pole2(2.9126274e3,1.1663357)
Z0 = ltu.zero2(8.7381935e3, 3.7254539e6)
P2 = ltu.pole2(5.3800106e3,8.3770775)
Z2 = ltu.zero2(8.7381935e3, 5.5411258e6)

DAA = ltu.ltiprod(P0,Z0,P2,Z2)
DAI = ltu.ltiprod(P0,Z0,P2,Z2)

DGS = ltu.ltiprod(AA,DAA,DAI,AI)

f = np.logspace(2,4,1000)
a=ltu.bode(DGS, w=f)


# ### Open Loop Gain

# In[65]:


#Open Loop Gain
OPLG = ltu.ltiprod(Act, LagLead, Boost,Hrf,MICH_GAIN,UGF_SERVO_GAIN,DGS) #Open Loop Gain
f = np.logspace(-1,3,1000)
(w,h)=ltu.bode(OPLG,f)


# The theoretical open loop transfer function G has the following form.

# In[66]:


#Open Loop Gain as Rational Function
print('Open Loop Gain: G(s) =')
sp.Poly(OPLG.num, s)/sp.Poly(OPLG.den, s)


# In[67]:


print('Time delay is {:.5}usec'.format(Td/1e-6))
print('Multiply the above transfer function with the following to include the time delay.')
sp.exp(-Td*sp.I*s)


# ## Comparison with the measured Open Loop Gain

# Below is the comparison of the model and the measured open loop gain.We have a non-negligible difference between the model and the measurement above $\approx 300\mathrm{Hz}$.

# In[68]:


TF=np.loadtxt('data/data/RF-Lock-MICH-OPLG-UGFSRV-OnHold.txt', comments='#')
f = TF[:,0]
real = TF[:,1]
imag = TF[:,2]
mag1 = np.sqrt(real**2+imag**2)
phase1 = rad2deg(np.angle(-1j*imag-real))

(w,h) = OPLG.freqresp(w=2*pi*f)
mag2 = np.abs(h)
phase2 = rad2deg(np.angle(h*np.exp(-2j*pi*f*Td)))  #Time delay is explicitly introduced as a phase delay here.


fig = plt.figure()
ax1 = fig.add_subplot(2,1,1)
ax2 = fig.add_subplot(2,1,2)
ax1.set_axisbelow(True)
ax2.set_axisbelow(True)

ax1.semilogx(f, 20*np.log10(mag1))
ax1.semilogx(f, 20*np.log10(mag2))
ax2.semilogx(f,phase1)
ax2.semilogx(f,phase2)

ax1.grid(True, color=(0.6,0.6,0.8),ls='-')
ax1.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')
ax2.grid(True, color=(0.6,0.6,0.8),ls='-')
ax2.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')

ax1.set_ylabel('Magnitude [dB]')
ax2.set_ylabel('Phase [deg]')
ax2.set_xlabel('Frequency [Hz]')

ax1.legend(['Measured','Model'])

ax1.set_title('MICH Open Loop Gain')
fig.set_size_inches(10,6,forward=True)


# ### A missing LPF ?

# The discrepancy between the model and the measurement can be resolved (at least in gain) by introducing a 1st order LPF at 550Hz.So far, I have not been able to find where is this LPF. If the LPF is in the detection side (e.g. in the PD or I-Q demodulator), this will change the calibration factor at high frequencies, because we will use the error signal for the h(t) recovery in this frequency range, and the effective optical gain should include this LPF. 

# In[80]:


#A missing LPF
fc = 550
MLPF = ltu.lpf1(fc=fc)

#Open Loop Gain
OPLG2 = ltu.ltiprod(MLPF,Act, LagLead, Boost,Hrf,MICH_GAIN,UGF_SERVO_GAIN,DGS) #Open Loop Gain

TF=np.loadtxt('data/data/RF-Lock-MICH-OPLG-UGFSRV-OnHold.txt', comments='#')
f = TF[:,0]
real = TF[:,1]
imag = TF[:,2]
mag1 = np.sqrt(real**2+imag**2)
phase1 = rad2deg(np.angle(-1j*imag-real))

(w,h) = OPLG2.freqresp(w=2*pi*f)
mag2 = np.abs(h)
phase2 = rad2deg(np.angle(h*np.exp(-2j*pi*f*Td)))  #Time delay is explicitly introduced as a phase delay here.

fig = plt.figure()
ax1 = fig.add_subplot(2,1,1)
ax2 = fig.add_subplot(2,1,2)
ax1.set_axisbelow(True)
ax2.set_axisbelow(True)

ax1.semilogx(f, 20*np.log10(mag1))
ax1.semilogx(f, 20*np.log10(mag2))
ax2.semilogx(f,phase1)
ax2.semilogx(f,phase2)

ax1.grid(True, color=(0.6,0.6,0.8),ls='-')
ax1.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')
ax2.grid(True, color=(0.6,0.6,0.8),ls='-')
ax2.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')

ax1.set_ylabel('Magnitude [dB]')
ax2.set_ylabel('Phase [deg]')
ax2.set_xlabel('Frequency [Hz]')

ax1.legend(['Measured','Model'])

ax1.set_title('MICH Open Loop Gain')
fig.set_size_inches(10,6,forward=True)


# Below is the comparison between the DC and RF transfer function measurements.Since the two curves have very similar slopes, the missing LPF is common to both. This exclude the possibility of the LPF in RFPD and I-Q demodulator.

# In[83]:


TF=np.loadtxt('data/data/DC-Lock-MICH-OUT-to-DCPD-OUT.txt', comments='#')
fDC = TF[:,0]
real = TF[:,1]
imag = TF[:,2]
magDC = np.sqrt(real**2+imag**2)
phaseDC = rad2deg(np.angle(1j*imag+real))

TF=np.loadtxt('data/data/RF-Lock-MICH-OUT-to-RFPD-OUT.txt', comments='#')
fRF = TF[:,0]
real = TF[:,1]
imag = TF[:,2]
magRF = np.sqrt(real**2+imag**2)
phaseRF = rad2deg(np.angle(1j*imag+real))

fig = plt.figure()
ax1 = fig.add_subplot(2,1,1)
ax2 = fig.add_subplot(2,1,2)
ax1.set_axisbelow(True)
ax2.set_axisbelow(True)

ax1.semilogx(fDC, 20*np.log10(magDC))
ax2.semilogx(fDC,phaseDC)
ax1.semilogx(fRF, 20*np.log10(magRF))
ax2.semilogx(fRF,phaseRF)

ax1.grid(True, color=(0.6,0.6,0.8),ls='-')
ax1.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')
ax2.grid(True, color=(0.6,0.6,0.8),ls='-')
ax2.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')

ax1.set_ylabel('Magnitude [dB]')
ax2.set_ylabel('Phase [deg]')
ax2.set_xlabel('Frequency [Hz]')

ax1.legend(['REFL DC','REFL RF'])

ax1.set_title('K1:LSC-REFL_PDA1_DC_OUT/K1:LSC-MICH_OUT_DQ')
fig.set_size_inches(12,8,forward=True)


# ## Generation of h(t)

# h(t) can be generated either from the error signal e(t) or from the feedback signal f(t).
# From the error signal, the calbration filter is 
# $$h(t) = \frac{1}{H}\frac{1+G}{1}\frac{e(t)}{L}$$
# 
# ,where $L$ is the arm length, $G$ is the open loop gain, $H$ is the optical gain.
# 
# From the feedback signal, the calibration filter is
# 
# $$h(t) = A\frac{1+G}{G}\frac{f(t)}{L}$$
# 
# ,where $A$ is the actuator efficiency.

# In[70]:


#The arm length
L = 3000.

#Calibration filter for the error signal
C_err = ltu.ltiprod(ltu.ltiadd(OPLG,1),1/Hrf/L)

f = np.logspace(-1,4,1000)
a=ltu.bode(C_err,f)
print("C_err(s) = ")
sp.Poly(C_err.num, s)/sp.Poly(C_err.den, s)

#print("Numerator = {0}".format(C_err.num))
#print("Denominator = {0}".format(C_err.den))


# In[71]:


#Calibration filter for the error signal
C_fb = ltu.ltiprod(ltu.ltiadd(OPLG,1),ltu.ltiinv(OPLG),Act,1/L)

f = np.logspace(-1,4,1000)
a=ltu.bode(C_fb,f)
print("C_fb(s) = ")
sp.Poly(C_fb.num, s)/sp.Poly(C_fb.den, s)


# ## ETM individual actuation efficiency

# We also wanted to check the actuation efficiency of individual ETM. To do so, we first locked MICH with low UGF (around 10Hz). The open loop gain of this lock is shown below.Around 100Hz, the open loop gain is low enough that the feedback will not affect the actuation efficiency measurement.

# In[72]:


TF=np.loadtxt('data/data/RF-LBW-Lock-MICH-OPLG.txt', comments='#')
f = TF[:,0]
real = TF[:,1]
imag = TF[:,2]
mag = np.sqrt(real**2+imag**2)
phase = rad2deg(np.angle(1j*imag+real))

fig = plt.figure()
ax1 = fig.add_subplot(2,1,1)
ax2 = fig.add_subplot(2,1,2)
ax1.set_axisbelow(True)
ax2.set_axisbelow(True)

ax1.semilogx(f, 20*np.log10(mag))
ax2.semilogx(f,phase)

ax1.grid(True, color=(0.6,0.6,0.8),ls='-')
ax1.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')
ax2.grid(True, color=(0.6,0.6,0.8),ls='-')
ax2.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')

ax1.set_ylabel('Magnitude [dB]')
ax2.set_ylabel('Phase [deg]')
ax2.set_xlabel('Frequency [Hz]')


ax1.set_title('K1:LSC-MICH_IN1_DQ/K1:LSC-MICH_IN2_DQ')
fig.set_size_inches(12,8,forward=True)


# Now we injected a signal to ETMX and measured the transfer function to the MICH RF signal output. By dividing this with the RF optical gain, we can compute the actuation efficiency of ETMX. Note that this calculation does not take into account the open loop gain. So the values at low frequencies (around near 10Hz) is not so accurate. However, the measurement at 100Hz is valid because the open loop gain there is small enough.

# In[73]:


TF=np.loadtxt('data/data/RF-Lock-ETMX-to-RFPD-OUT.txt', comments='#')
f = TF[:,0]
real = TF[:,1]
imag = TF[:,2]
mag = np.sqrt(real**2+imag**2)/Hrf
phase = rad2deg(np.angle(1j*imag+real))

fig = plt.figure()
ax1 = fig.add_subplot(2,1,1)
ax2 = fig.add_subplot(2,1,2)
ax1.set_axisbelow(True)
ax2.set_axisbelow(True)

ax1.loglog(f, mag)
ax2.semilogx(f,phase)

ax1.grid(True, color=(0.6,0.6,0.8),ls='-')
ax1.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')
ax2.grid(True, color=(0.6,0.6,0.8),ls='-')
ax2.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')

ax1.set_ylabel('Actuator Efficiency [m/cnt]')
ax2.set_ylabel('Phase [deg]')
ax2.set_xlabel('Frequency [Hz]')


ax1.set_title('K1:LSC-MICH_IN1_DQ/K1:VIS-EX_TM_ISCINF_L_OUT/Hrf')
fig.set_size_inches(10,8,forward=True)


# We did the same measurement for ETMY

# In[74]:


TF=np.loadtxt('data/data/RF-Lock-ETMY-to-RFPD-OUT.txt', comments='#')
f = TF[:,0]
real = TF[:,1]
imag = TF[:,2]
mag = np.sqrt(real**2+imag**2)/Hrf
phase = rad2deg(np.angle(1j*imag+real))

fig = plt.figure()
ax1 = fig.add_subplot(2,1,1)
ax2 = fig.add_subplot(2,1,2)
ax1.set_axisbelow(True)
ax2.set_axisbelow(True)

ax1.loglog(f, mag)
ax2.semilogx(f,phase)

ax1.grid(True, color=(0.6,0.6,0.8),ls='-')
ax1.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')
ax2.grid(True, color=(0.6,0.6,0.8),ls='-')
ax2.grid(True, which='minor', color=(0.6,0.6,0.6),ls='--')

ax1.set_ylabel('Actuator Efficiency [m/cnt]')
ax2.set_ylabel('Phase [deg]')
ax2.set_xlabel('Frequency [Hz]')


ax1.set_title('K1:LSC-MICH_IN1_DQ/K1:VIS-EY_TM_ISCINF_L_OUT/Hrf')
fig.set_size_inches(10,8,forward=True)