import numpy as np
import matplotlib.pyplot as plt

import gwpy.timeseries

times = [1436808081]
span = 8*60

channels = ['H1:SQZ-DCPD_RATIO_1_DB_MON', 'H1:SQZ-DCPD_RATIO_2_DB_MON', 'H1:SQZ-DCPD_RATIO_3_DB_MON',
            'H1:SQZ-DCPD_RATIO_4_DB_MON', 'H1:SQZ-DCPD_RATIO_5_DB_MON', 'H1:SQZ-DCPD_RATIO_6_DB_MON',
           'H1:SQZ-CLF_REFL_RF6_PHASE_PHASEDEG',  'H1:SQZ-ADF_OMC_TRANS_SQZ_ANG',
           'H1:SQZ-ADF_OMC_TRANS_DEMOD_I_OUT16', 'H1:SQZ-ADF_OMC_TRANS_DEMOD_Q_OUT16', 
           'H1:SQZ-ADF_VCXO_PLL_PHASE', 'H1:SQZ-ADF_OMC_TRANS_PHASE','H1:SQZ-LO_SERVO_IN1POL', 
           'H1:SQZ-ADF_OMC_TRANS_NORM_I', 'H1:SQZ-ADF_OMC_TRANS_NORM_Q' ]
data_sets = {}
for ii in range(len(times)):
    data_sets[ii] = gwpy.timeseries.TimeSeriesDict.fetch(channels,times[ii],times[ii]+span,verbose=True)


recalc_ang = {}
G = 0.195
offset_I = [0]#[-1.,-0.3,-0.6,-1.2]
offset_Q = [0]#[-0.5, -1, -0.7, -0.1]

fig, [ax1, ax3] = plt.subplots(1,2, figsize=(16,8))
#fig2, ax3 = plt.subplots(1,1, figsize=(8,8))
for ii in data_sets.keys():
#ii = 0
    data = data_sets[ii]
    recalc_ang[ii] = np.arctan2(((data['H1:SQZ-ADF_OMC_TRANS_DEMOD_Q_OUT16'].value + offset_Q[ii])/G),((data['H1:SQZ-ADF_OMC_TRANS_DEMOD_I_OUT16'].value+ offset_I[ii]))) *180/np.pi
    VCXO_phase = np.mean(data['H1:SQZ-ADF_VCXO_PLL_PHASE'].value)
    adf_phase = np.mean(data['H1:SQZ-ADF_OMC_TRANS_PHASE'].value)
    label = f' VCXO phase {VCXO_phase:0.1f} ADF phase {adf_phase:0.1f}'

    ax1.plot(data['H1:SQZ-ADF_OMC_TRANS_DEMOD_Q_OUT16'].value, data['H1:SQZ-ADF_OMC_TRANS_DEMOD_I_OUT16'].value,  label=label)
    ax1.plot((data['H1:SQZ-ADF_OMC_TRANS_NORM_Q'].value), data['H1:SQZ-ADF_OMC_TRANS_NORM_I'].value,
              label=label + ' norm', linestyle = '--')
    ax3.plot(data['H1:SQZ-CLF_REFL_RF6_PHASE_PHASEDEG'].value, data[ 'H1:SQZ-ADF_OMC_TRANS_SQZ_ANG'].value, label = label )


for ax in [ax1]:
    ax.legend()
    ax.set_xlabel('ADF Q')
    ax.set_ylabel('ADF I')
ax1.set_xlim([-6,6])
ax1.set_ylim([-6,6])
ax2_lim = 1
#ax2.set_xlim([-1*ax2_lim,ax2_lim])
#ax2.set_ylim([-1*ax2_lim,ax2_lim])


ax3.legend()
ax3.set_xlabel('RF6 demod phase [deg]')
ax3.set_ylabel('ADF readback of sqz angle [deg]')

fig.savefig('ADF_angle_vs_RF6_demod_angle_w_recalc.png')
#fig2.savefig('ADF_IQ_vs_RF6_demod_angle_w_recalc.png')


#after adjusting offsets we did scan with two polarities
#angle_with_sign_flip= data_linearized['H1:SQZ-LO_SERVO_IN1POL'].value * -180 + data_linearized['H1:SQZ-CLF_REFL_RF6_PHASE_PHASEDEG'].value
#adf_angle = data_linearized[ 'H1:SQZ-ADF_OMC_TRANS_SQZ_ANG'].value
#manual_arctan =(180/np.pi)* np.arctan2(data_linearized['H1:SQZ-ADF_OMC_TRANS_NORM_I'].value, data_linearized['H1:SQZ-ADF_OMC_TRANS_NORM_Q'].value)
#ratio = data_linearized['H1:SQZ-ADF_OMC_TRANS_NORM_I'].value/data_linearized['H1:SQZ-ADF_OMC_TRANS_NORM_Q'].value



fig4, [ax1,ax2] = plt.subplots(2,1, figsize=(8,8))
ax1.plot(data['H1:SQZ-CLF_REFL_RF6_PHASE_PHASEDEG'].value, data['H1:SQZ-ADF_OMC_TRANS_SQZ_ANG'].value , marker='.', label = 'sqz angle readback')
ax2.plot(data['H1:SQZ-CLF_REFL_RF6_PHASE_PHASEDEG'].value, data['H1:SQZ-DCPD_RATIO_2_DB_MON'].value , marker='.', label = '90-250 Hz bandpass 2')
ax2.plot(data['H1:SQZ-CLF_REFL_RF6_PHASE_PHASEDEG'].value, data['H1:SQZ-DCPD_RATIO_3_DB_MON'].value , marker='.', label = '300-400 Hz bandpass 3')
ax2.plot(data['H1:SQZ-CLF_REFL_RF6_PHASE_PHASEDEG'].value, data['H1:SQZ-DCPD_RATIO_4_DB_MON'].value , marker='.', label = '700-1750 Hz bandpass 4')
ax2.plot(data['H1:SQZ-CLF_REFL_RF6_PHASE_PHASEDEG'].value, data['H1:SQZ-DCPD_RATIO_6_DB_MON'].value , marker='.', label = '1650-1750 Hz bandpass 6')
ax2.legend()
fig4.savefig('after_adjustments_scan_angle.png')