import ezca
import cdsutils
import time
import numpy

e = ezca.Ezca()

dt = 30.

# suspension movements
susp = ['SUS-ITMX_M0_OPTICALIGN_P_OFFSET',
        'SUS-ETMX_M0_OPTICALIGN_P_OFFSET',
        'SUS-TMSX_M1_OPTICALIGN_P_OFFSET',
        'SUS-ITMY_M0_OPTICALIGN_P_OFFSET',
        'SUS-ETMY_M0_OPTICALIGN_P_OFFSET',
        'SUS-TMSY_M1_OPTICALIGN_P_OFFSET',
        'SUS-ITMX_M0_OPTICALIGN_Y_OFFSET',
        'SUS-ETMX_M0_OPTICALIGN_Y_OFFSET',
        'SUS-TMSX_M1_OPTICALIGN_Y_OFFSET',
        'SUS-ITMY_M0_OPTICALIGN_Y_OFFSET',
        'SUS-ETMY_M0_OPTICALIGN_Y_OFFSET',
        'SUS-TMSY_M1_OPTICALIGN_Y_OFFSET']

# QPD signals
channels = ['H1:ASC-X_TR_A_PIT_INMON',
            'H1:ASC-X_TR_B_PIT_INMON',
            'H1:ASC-Y_TR_A_PIT_INMON',
            'H1:ASC-Y_TR_B_PIT_INMON',
            'H1:ASC-X_TR_A_YAW_INMON',
            'H1:ASC-X_TR_B_YAW_INMON',
            'H1:ASC-Y_TR_A_YAW_INMON',
            'H1:ASC-Y_TR_B_YAW_INMON'
]

# motion names
motion_names = ['CSOFT_PIT', 'DSOFT_PIT', 'TMS X PIT', 'TMS Y PIT',
                'CSOFT_YAW', 'DSOFT_YAW', 'TMS X YAW', 'TMS Y YAW']

# matrix of motions
#           ITMX   ETMX  TMSX  ITMY  ETMY  TMSY
motions = [[ 1.0,  0.87, 0.0,  1.0,  0.87, 0.0,    # PIT     # CSOFT_PIT
             0.0,  0.0,  0.0,  0.0,  0.0,  0.0],   # YAW
           [ 1.0,  0.87, 0.0, -1.0, -0.87, 0.0,    # PIT     # DSOFT_PIT
             0.0,  0.0,  0.0,  0.0,  0.0,  0.0],   # YAW     
           [ 0.0,  0.0,  1.0,  0.0,  0.0,  0.0,    # PIT     # TMS X PIT
             0.0,  0.0,  0.0,  0.0,  0.0,  0.0],   # YAW
           [ 0.0,  0.0,  0.0,  0.0,  0.0,  1.0,    # PIT     # TMS Y PIT
             0.0,  0.0,  0.0,  0.0,  0.0,  0.0],   # YAW
           [ 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,    # PIT     # CSOFT YAW
             1.0, -0.87, 0.0, -1.0,  0.87, 0.0],   # YAW
           [ 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,    # PIT     # DSOFT YAW
             1.0, -0.87, 0.0,  1.0, -0.87, 0.0],   # YAW
           [ 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,    # PIT     # TMS X YAW
             0.0,  0.0,  1.0,  0.0,  0.0,  0.0],   # YAW
           [ 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,    # PIT     # TMS Y YAW
             0.0,  0.0,  0.0,  0.0,  0.0,  1.0]]   # YAW

# rescaling factors for alignment offsets
#           CSOFT_PIT DSOFT_PIT TMSX_PIT  TMSY_PIT CSOFT_YAW DSOFT_YAW TMSX_YAW  TMSY_YAW 
scale = [   0.3,      0.3,      0.5,      0.5,     0.3,      0.3,      0.5,      0.5,  ]

motions = numpy.array(motions)
nmotions = motions.shape[0]
nsusp = len(susp)
nsensors = len(channels)

# measurement matrix
M = numpy.zeros((nsensors, nmotions))

# loop over all motions
for i in range(nmotions):
    print('\n\n****** Measuring %s ******' % motion_names[i])
    # initial positions
    pos0 = numpy.zeros((nsusp,))
    for j,s in enumerate(susp):
        pos0[j] = e[s]
    # measure sensor values
    print('** Measuring initial values...')
    sens0 = cdsutils.avg(dt, channels)
    # move suspensions
    for sus,mot,p0 in zip(susp, motions[i,:], pos0):
        e[sus] = p0 + mot*scale[i]
    time.sleep(5)
    # taking another measurement
    print('** Measuring new values...')
    sens1 = cdsutils.avg(dt, channels)
    M[:,i] = (numpy.array(sens1) - numpy.array(sens0))/scale[i]
    # move suspensions back
    for sus,p0 in zip(susp, pos0):
        e[sus] = p0
    time.sleep(5)

# save measurement to file
numpy.savetxt('trans_qpd_matrix.txt', M)

# print full matrix
import sys
sys.stdout.write(' ' * len(channels[0]) + '\t')
for j in range(M.shape[1]):
    sys.stdout.write('%s\t' % motion_names[j])
sys.stdout.write('\n')
for i in range(M.shape[0]):
    sys.stdout.write('%s\t' % channels[i])
    for j in range(M.shape[1]):
        sys.stdout.write('% f\t' % M[i,j])
    sys.stdout.write('\n')

# select pitch and yaw sub matrices
Mp = M[0:4,0:4]
My = M[4:8,4:8]

# print pitch matrix
print('')
sys.stdout.write(' ' * len(channels[0]) + '\t')
for j in range(Mp.shape[1]):
    sys.stdout.write('%s\t' % motion_names[j])
sys.stdout.write('\n')
for i in range(Mp.shape[0]):
    sys.stdout.write('%s\t' % channels[i])
    for j in range(Mp.shape[1]):
        sys.stdout.write('% f\t' % Mp[i,j])
    sys.stdout.write('\n')

# print yaw matrix
print('')
sys.stdout.write('\n')
sys.stdout.write(' ' * len(channels[0]) + '\t')
for j in range(Mp.shape[1]):
    sys.stdout.write('%s\t' % motion_names[j+4])
sys.stdout.write('\n')
for i in range(My.shape[0]):
    sys.stdout.write('%s\t' % channels[i+4])
    for j in range(My.shape[1]):
        sys.stdout.write('% f\t' % My[i,j])
    sys.stdout.write('\n')

# invert sub matrices
from numpy.linalg import inv
Mp_inv = inv(Mp)
My_inv = inv(My)

# print inverse pitch matrix
print('')
sys.stdout.write(' ' * len(motion_names[0]) + '\t')
for j in range(Mp_inv.shape[1]):
    sys.stdout.write('%s\t' % channels[j])
sys.stdout.write('\n')
for i in range(Mp_inv.shape[0]):
    sys.stdout.write('%s\t' % motion_names[i])
    for j in range(Mp_inv.shape[1]):
        sys.stdout.write('% 23f\t' % Mp_inv[i,j])
    sys.stdout.write('\n')

# print inverse yaw matrix
print('')
sys.stdout.write(' ' * len(motion_names[0]) + '\t')
for j in range(My_inv.shape[1]):
    sys.stdout.write('%s\t' % channels[j+4])
sys.stdout.write('\n')
for i in range(My_inv.shape[0]):
    sys.stdout.write('%s\t' % motion_names[i+4])
    for j in range(My_inv.shape[1]):
        sys.stdout.write('% 23f\t' % My_inv[i,j])
    sys.stdout.write('\n')

## print HTML versions

f = sys.stdout

# print pitch matrix
f.write('<table>\n')
f.write('<tr><td></td>\n')
for j in range(Mp.shape[1]):
    f.write('<td><b>%s</b></td>' % motion_names[j])
f.write('</tr>\n')
for i in range(Mp.shape[0]):
    f.write('<tr><td><b>%s</b></td>' % channels[i])
    for j in range(Mp.shape[1]):
        f.write('<td>%f</td>' % Mp[i,j])
    f.write('</tr>\n')
f.write('</table>')

# print yaw matrix
f.write('<table>\n')
f.write('<tr><td></td>\n')
for j in range(My.shape[1]):
    f.write('<td><b>%s</b></td>' % motion_names[j+4])
f.write('</tr>\n')
for i in range(My.shape[0]):
    f.write('<tr><td><b>%s</b></td>' % channels[i+4])
    for j in range(My.shape[1]):
        f.write('<td>%f</td>' % My[i,j])
    f.write('</tr>\n')
f.write('</table>')

# print inverse pitch matrix
f.write('<table>\n')
f.write('<tr><td></td>\n')
for j in range(Mp_inv.shape[1]):
    f.write('<td><b>%s</b></td>' % channels[j])
f.write('</tr>\n')
for i in range(Mp_inv.shape[0]):
    f.write('<tr><td><b>%s</b></td>' % motion_names[i])
    for j in range(Mp_inv.shape[1]):
        f.write('<td>%f</td>' % Mp_inv[i,j])
    f.write('</tr>\n')
f.write('</table>')

# print inverse pitch matrix
f.write('<table>\n')
f.write('<tr><td></td>\n')
for j in range(My_inv.shape[1]):
    f.write('<td><b>%s</b></td>' % channels[j+4])
f.write('</tr>\n')
for i in range(My_inv.shape[0]):
    f.write('<tr><td><b>%s</b></td>' % motion_names[i+4])
    for j in range(My_inv.shape[1]):
        f.write('<td>%f</td>' % My_inv[i,j])
    f.write('</tr>\n')
f.write('</table>')
