# -*- coding: utf-8 -*-
"""
Éditeur de Spyder

Ce script temporaire est sauvegardé ici :
/home/vincent/.spyder2/.temp.py
"""

from scipy import optimize
from pylab import *
import pyfits as pyfits

print("interactive", isinteractive())


def plot_met(filename):
    
    print("open :", filename)
    gravi_data_p2vm=pyfits.open(filename, mode='readonly')
    met_tab=gravi_data_p2vm['METROLOGY'].data
#    fddl_tab=gravi_data_p2vm['FDDL'].data
#    cmd_ddl=fddl_tab["SC_DDL_CMD"]#[:,0]
#    time_ddl=fddl_tab["TIME"]
    time_met=met_tab["TIME"]#[0:2900]
    volt=met_tab["VOLT"]#[0:2900]

    fddl_tab=gravi_data_p2vm['FDDL'].data
    fddl_time=fddl_tab["TIME"]
    fddl_sc_pos=fddl_tab["SC_POS"]
    fddl_ft_pos=fddl_tab["FT_POS"]

    ind_sin_sc=[64,66,68,70,72,74,76,78]
    ind_cos_sc=[65,67,69,71,73,75,77,79]

    X_=volt[:,ind_sin_sc]
    Y_=volt[:,ind_cos_sc]

    ind_max=volt[:,0].size
    # fit ellipse

    phase_unwrap_=ndarray(X_.shape, dtype=float64)
#phase_unwrap_=ndarray((ind_max,size(volt[0,:])), dtype=float64)

    figure(3)
    clf()
    suptitle(filename)

    figure(2)
    clf()
    suptitle(filename)
    
    figure(6)
    clf()
    suptitle(filename)
    
    for diode in range(size(ind_sin_sc)) :
    #for diode in range(size(volt[0,:])) :
        Xf=0
        Yf=0
        X=X_[:,diode]
        Y=Y_[:,diode]
        #plot(X, Y, ".")
        optimize_func = lambda param: sqrt(pow(param[0]*X+param[1]*Y+param[2], 2)+pow(param[3]*Y+param[4],2)) - 1
        est_a, est_b, est_c, est_d, est_e = optimize.leastsq(optimize_func, [2/(max(X)-min(X)),0,-mean(X),2/(max(Y)-min(Y)),-mean(Y)])[0]
        print(est_a, est_b, est_c, est_d, est_e)
        Xf=est_a*X+est_b*Y+est_c
        Yf=est_d*Y+est_e
        
#        figure(3)
#        subplot(212)
#        plot(time_met, sqrt(Xf*Xf+Yf*Yf))
    
        figure(2)
        subplot(4,2,diode+1)
        plot(X, Y, ",", label="Diode %d"%(diode+1))
        xlabel("X = sin")
        ylabel("Y = cos")
    
        #legend("Tel "+str(diode))
        legend()


        figure(6)
        subplot(4,2,diode+1)
        plot(time_met, sqrt(Y*Y+X*X), ",", label="Diode %d"%(diode+1))
        xlabel("Time")
        ylabel("Met Module [V]")
    
        #legend("Tel "+str(diode))
        legend()

        #show()
        phase_unwrap_[:,diode]=arctan2(Yf, Xf)
    
        k=0
        for i in range(phase_unwrap_[:,diode].size):
            if i != 0:
                if phase_unwrap_[i,diode]+k*2*pi-phase_unwrap_[i-1, diode] > pi : k-=1
                else:
                    if phase_unwrap_[i, diode]+k*2*pi-phase_unwrap_[i-1, diode] < - pi : k+=1
            phase_unwrap_[i, diode]=phase_unwrap_[i, diode]+k*2*pi
#legend(('Tel 1', 'Tel 2', 'Tel 3', 'Tel 4'))


#k=0
#for i in range(phase_unwrap.size):
#    if i != 0:
#        if phase_unwrap[i]+k*2*pi-phase_unwrap[i-1] > pi : k-=1
#        else:
#            if phase_unwrap[i]+k*2*pi-phase_unwrap[i-1] < - pi : k+=1
#    phase_unwrap[i]=phase_unwrap[i]+k*2*pi
#
    figure(3)
    clf()
    suptitle(filename)
    subplot(311)
    plot(time_met, phase_unwrap_)
#    plot(time_met, phase_unwrap_)
#plot(time_met[0:phase_unwrap_.size/4], arctan2(Yf, Xf))
#plot(phase_unwrap_)
#plot(time_ddl, cmd_ddl)
    legend(('Tel 1 FT', 'Tel 2 FT', 'Tel 3 FT', 'Tel 4 FT','Tel 1 SC', 'Tel 2 SC', 'Tel 3 SC', 'Tel 4 SC'))
    xlabel("Time [10-6 s]")
    ylabel("Phase met [radian]")
    subplot(312)
#    plot(time_met, sqrt(X_[:,0]*X_[:,0]+Y_[:,0]*Y_[:,0]))
#    plot(time_met, sqrt(X_[:,1]*X_[:,1]+Y_[:,1]*Y_[:,1]))
#    plot(time_met, sqrt(X_[:,2]*X_[:,2]+Y_[:,2]*Y_[:,2]))
#    plot(time_met, sqrt(X_[:,3]*X_[:,3]+Y_[:,3]*Y_[:,3]))
#    legend(('Tel 1 FT', 'Tel 2 FT', 'Tel 3 FT', 'Tel 4 FT','Tel 1 SC', 'Tel 2 SC', 'Tel 3 SC', 'Tel 4 SC'))
#    xlabel("Time [10-6 s]")
#    ylabel("Contrast [VOLT]")
     #plot(time_met, sqrt(Xf*Xf+Yf*Yf))
    #plot(time_ddl, cmd_ddl)
    plot(time_met, phase_unwrap_[:,1]-phase_unwrap_[:,0])
    plot(time_met, phase_unwrap_[:,2]-phase_unwrap_[:,0])
    plot(time_met, phase_unwrap_[:,3]-phase_unwrap_[:,0])
    plot(time_met, phase_unwrap_[:,2]-phase_unwrap_[:,1])
    plot(time_met, phase_unwrap_[:,3]-phase_unwrap_[:,1])
    plot(time_met, phase_unwrap_[:,3]-phase_unwrap_[:,2])
    legend(('Base 1 FT', 'Base 2 FT', 'Base 3 FT', 'Base 4 FT','Base 5 FT', 'Base 6 FT'))
    xlabel("Time [10-6 s]")
    ylabel("OPD FT [radian]")

    subplot(313)
    plot(time_met, phase_unwrap_[:,5]-phase_unwrap_[:,4])
    plot(time_met, phase_unwrap_[:,6]-phase_unwrap_[:,4])
    plot(time_met, phase_unwrap_[:,7]-phase_unwrap_[:,4])
    plot(time_met, phase_unwrap_[:,6]-phase_unwrap_[:,5])
    plot(time_met, phase_unwrap_[:,7]-phase_unwrap_[:,5])
    plot(time_met, phase_unwrap_[:,7]-phase_unwrap_[:,6])
    legend(('Base 1 SC', 'Base 2 SC', 'Base 3 SC', 'Base 4 SC','Base 5 SC', 'Base 6 SC'))
    xlabel("Time [10-6 s]")
    ylabel("OPD SC [radian]")

    figure(5)
    clf()
    #plot(interp(time_ft, time_met[0:phase_unwrap_[:,3].size], phase_unwrap_[:,3]-phase_unwrap_[:,0]), phase_ft, ",")
    #plot(time_ddl, cmd_ddl, "+")
    #plot(X_ft, Y_ft)
    #xlabel("Time [10-6 s]")
    #ylabel("[m]")
    plot(time_met, phase_unwrap_[:,4]-phase_unwrap_[:,0])
    plot(time_met, phase_unwrap_[:,5]-phase_unwrap_[:,1])
    plot(time_met, phase_unwrap_[:,6]-phase_unwrap_[:,2])
    plot(time_met, phase_unwrap_[:,7]-phase_unwrap_[:,3])
    legend(('OPL_met_1', 'OPL_met_2', 'OPL_met_3', 'OPL_met_4'))
    xlabel("Time [10-6 s]")
    ylabel("OPL met [radian]")

    figure(1)
    clf()
    for tel in range(4):
        subplot(2,2,tel+1)
        #plot(fddl_time, fddl_sc_pos[:,tel])
#        plot(interp(time_met, fddl_time, fddl_sc_pos[:,tel]-fddl_ft_pos[:,tel]),phase_unwrap_[:,tel+4]-phase_unwrap_[:,tel] )
        plot(interp(time_met, fddl_time, fddl_sc_pos[:,tel]),phase_unwrap_[:,tel+4] )
#        plot(time_met,phase_unwrap_[:,tel+4]-phase_unwrap_[:,tel] )
#        plot(fddl_time,fddl_sc_pos[:,tel]-fddl_ft_pos[:,tel] )
        xlabel("FDDL_POS_SC_{} [V?]".format(tel))
        ylabel("Phi_met_SC_{} [radian]".format(tel))
   
#    figure(6)
#    clf()
#    diode=2
##    plot(time_met, volt[:,ind_sin_sc[diode]])
##    plot(time_met, volt[:,ind_cos_sc[diode]])
##    plot(time_met, volt[:,ind_sin_sc[diode]]+volt[:,ind_cos_sc[diode]])
#    plot(time_met, X_[:,diode])
#    plot(time_met, Y_[:,diode])
#    plot(time_met, X_[:,diode]+Y_[:,diode])
#    #legend(('Tel1 A', 'Tel 1 B', 'Tel 1 C', 'Tel 1 D', 'Tel 1 A+B+C+D'))
#    legend(('Tel1 SIN', 'Tel1 COS', 'Tel1 SIN+COS'))
#    #plot(time_ddl, cmd_ddl)
#    xlabel("Time [10-6 s]")
#    ylabel("[VOLT]")
    
    
    #show()
    
    for diode in range(size(ind_sin_sc)) :
        figure(4)
        clf()
        optimize_func = lambda param: param[0]*cos(phase)+param[1]*sin(phase)+param[2] - data
        data=volt[:,ind_sin_sc[diode]]
        phase=phase_unwrap_[:,diode]
        guess_c=mean(data)
        guess_a=0
        guess_b=(max(data)-min(data))/2
        
        est_a, est_b, est_c = optimize.leastsq(optimize_func, [guess_a, guess_b, guess_c])[0]
        plot(phase, data, "+")  
        plot(phase, optimize_func([est_a, est_b, est_c])+data, ".")  
        #plot(phase, optimize_func([est_a, est_b, est_c]))  
        phiRef=arctan2(est_b, est_a)
        phiA=arctan2(est_b, est_a)-phiRef
        print('Diode ', diode+1)
        print('Phase_Sin=', phiA/pi*180., '\t\tC_Sin =', sqrt(pow(est_a,2)+pow(est_b,2)), '\tOff_Sin=', est_c) 
    
        data=volt[:,ind_cos_sc[diode]]
        phase=phase_unwrap_[:,diode]
        est_a, est_b, est_c = optimize.leastsq(optimize_func, [guess_a, guess_b, guess_c])[0]
        plot(phase, data)  
        plot(phase, optimize_func([est_a, est_b, est_c])+data)  
        #plot(phase, optimize_func([est_a, est_b, est_c]))  
        phiB=arctan2(est_b, est_a)-phiRef
        if phiB < 0 : phiB+=2*pi
        print('Phase_Cos=', phiB/pi*180., '\tC_Cos =', sqrt(pow(est_a,2)+pow(est_b,2)), '\tOff_Cos=', est_c)

    
    # Data FT base 34
    nwave=4
    ft_tab=gravi_data_p2vm['IMAGING_DATA_FT'].data
    time_ft=ft_tab["TIME"]
    #ft_A=ft_tab["PIX"][:,[0,24,48,72]]
    #ft_C=ft_tab["PIX"][:,[0,24,48,72]+ones(nwave,dtype=int)*1]
    #ft_B=ft_tab["PIX"][:,[0,24,48,72]+ones(nwave,dtype=int)*2]
    #ft_D=ft_tab["PIX"][:,[0,24,48,72]+ones(nwave,dtype=int)*3]
    ft_A=ft_tab["PIX"][:,:,0]
    ft_C=ft_tab["PIX"][:,:,1]
    ft_B=ft_tab["PIX"][:,:,2]
    ft_D=ft_tab["PIX"][:,:,3]
    
    X_ft=ft_C-ft_A
    Y_ft=ft_D-ft_B
    phase_ft=arctan2(X_ft, Y_ft)
    
    k=[0,0, 0, 0, 0]
    k=zeros(nwave)
    for i in range(phase_ft[:,0].size):
        for wave in range(nwave):
            if i != 0:
                if phase_ft[i,wave]+k[wave]*2*pi-phase_ft[i-1, wave] > pi : k[wave]-=1
                else:
                    if phase_ft[i, wave]+k[wave]*2*pi-phase_ft[i-1, wave] < - pi : k[wave]+=1
            phase_ft[i, wave]=phase_ft[i, wave]+k[wave]*2*pi
    
#    figure(3)
#    subplot(212)
    #plot(X_[:,16]*X_[:,16]+Y_[:,16]*Y_[:,16])
    #plot(time_ft, phase_ft)
    
#    figure(5)
#    clf()
    #plot(interp(time_ft, time_met[0:phase_unwrap_[:,3].size], phase_unwrap_[:,3]-phase_unwrap_[:,0]), phase_ft, ",")
    #plot(time_ddl, cmd_ddl, "+")
#    plot(X_ft, Y_ft)
#    xlabel("Time [10-6 s]")
#    ylabel("[m]")
    
#    figure(1)
#    clf()
#    plot(interp(time_ft, time_met[0:phase_unwrap_[:,3].size], phase_unwrap_[:,3]-phase_unwrap_[:,0]), phase_ft, ",")
#    #plot(time_ddl, cmd_ddl, "+")
#    xlabel("Time [10-6 s]")
#    ylabel("[m]")
    
#    figure(7)
#    clf()
#    sc_data=gravi_data_p2vm['IMAGING_DATA_SC'].data
#    plot(sc_data[:, 91, 1024]-sc_data[:, 66, 1024], sc_data[:, 140, 1024]-sc_data[:, 116, 1024])


if __name__ == '__main__':
 #   filename="/home/vincent/Data/gravity/data_2015-03-23/GRAV.2015-03-23T19-26-19.fits"
    filename="/home/vincent/Data/gravity/data_2015-04-01/GRAV.2015-04-01T20-40-00.fits"
    filename="/home/vincent/Data/gravity/vrac/GRAV.2015-04-13T19-20-48.fits"
    filename="/home/vincent/Data/gravity/data_2015-04-04/calib_med_pol/up_GRAV.2015-04-04T17-40-19.fits"
    filename="/home/vincent/Data/gravity/data_2015-04-18/medium_split/GRAV.2015-04-18T20-38-15.fits"
    filename="/home/vincent/Data/gravity_paranal/data_2015-08-16/GRAVITY.2015-08-16T19-28-51.fits"  
    filename="/home/vincent/Data/gravity_paranal/data_2015_09_10/GRAVITY.2015-09-11T04-31-39.fits"
    filename="/home/vincent/Data/gravity_paranal/data_2015-09-13/GRAVITY.2015-09-13T18-30-21.fits"
    filename="/home/vincent/Data/gravity_paranal/data_2015-08-16/GRAVITY.2015-08-16T19-28-51.fits"
    filename="/home/vincent/Data/gravity_paranal/data_2015-09-03/GRAVITY.2015-09-03T03-50-18.fits"
    filename="/home/vincent/Data/gravity_paranal/data_2015-09-06/GRAVITY.2015-09-06T13-43-20.fits"
    filename="/home/vincent/Data/gravity_paranal/data_2015-09-20/GRAVITY.2015-09-19T17-19-24.fits"
    filename="/home/vincent/Data/gravity_paranal/p2vms/GRAVITY.2015-09-22T02-29-42.fits"
    filename="/home/vincent/Data/gravity_paranal/p2vms/GRAVITY.2015-09-22T06-19-52.fits"
    filename="/home/vincent/Data/gravity_paranal/p2vms/GRAVITY.2015-09-22T05-50-48.fits"
    filename="/home/vincent/Data/gravity_paranal/p2vms/GRAVITY.2015-09-22T08-20-26.fits"
    filename="/home/vincent/Data/gravity_paranal/p2vms/pb/GRAVITY.2015-09-24T10-25-35.fits"
    filename="/home/vincent/Data/gravity_paranal/data_2015_10_11/GRAVITY.2015-10-11T17-54-52.fits"   
    filename="/home/vincent/Data/gravity_paranal/COM4/2016-03-18/GRAVITY.2016-03-18T05-04-01.fits"
#    filename="/home/vincent/Data/gravity_paranal/COM4/p2vm_ref/GRAVITY.2016-02-23T12-31-36.fits"
#    filename="/home/vincent/Data/gravity_paranal/COM4/p2vm_ref/GRAVITY.2016-01-11T20-50-33.fits"
#    filename="/home/vincent/Data/gravity_paranal/COM4/p2vm_ref/GRAVITY.2016-01-06T06-25-03.fits"
    filename="/home/vincent/Data/gravity_paranal/COM4/p2vm_ref/GRAVITY.2016-03-24T09-12-34.fits"
    filename="/home/vincent/Data/gravity_paranal/COM4/check_wave/GRAVITY.2016-03-27T17-19-40.fits"
    filename="/home/vincent/Data/gravity_paranal/COM4/check_wave/GRAVITY.2016-03-28T10-01-07.fits"
    filename="/home/vincent/Data/gravity_paranal/COM7/GRAVI.2016-06-04T11:46:14.785.fits"
    filename="/data/1/2017-06-05/GRAVITY.2017-06-05T12-51-30.fits"
#    filename="/home/vincent/Bureau/GRAVI.2015-10-14T17:38:54.480.fits"
#    filename="/home/vincent/Bureau/GRAVI.2015-10-14T16:13:38.874.fits"
#    filename="/home/vincent/Bureau/GRAVI.2015-10-14T17:04:45.984.fits"
#    filename="/home/vincent/Bureau/GRAVI.2015-10-14T16:38:19.928.fits"
    
    if len(sys.argv) == 2 :
        filename=sys.argv[1]
#	filename = '/home/vincent/Data/gravity/data_2015-02-12/conf2/Flat_1.fits'
#	split_flat(filename)
#	filename = '/home/vincent/Data/gravity/data_2015-02-12/conf2/Flat_2.fits'
#	split_flat(filename)
#	filename = '/home/vincent/Data/gravity/data_2015-02-12/conf2/Flat_3.fits'
#	split_flat(filename)
#	filename = '/home/vincent/Data/gravity/data_2015-02-12/conf2/Flat_4.fits'
#	split_flat(filename)
    plot_met(filename)
