'''
Created on 5. jun. 2018

@author: Ole-Martin
'''

from matplotlib import pyplot as plt
import os
import sys
import numpy as np
import pandas as pd
import matplotlib.dates as md
import scipy.signal as sign
import glob
from scipy.signal import butter, lfilter,stft, periodogram, welch,csd



import git_FDDtools

git_FDDtools.git_setCurrentDirectory()







def plotSensorLocation_horizontal(points,plotRealSensors=0):
    #Plot stationary sensor locations, top view. Draw lines to show a top view of the bridge as a grid.
    #The plot is generated by constructing a 5x2 grid matrix for both the X and Y coordinates,
    # where each point in the matrix corresponds to the physical point of the figure to be plotted

    
    gridMatIndex = [0,2,4,6,8]
    copyIndex = [0,1,2,3,4]
    gridMatX = np.empty((5,2),dtype=float) 
    gridMatY = np.empty((5,2),dtype=float) 
    
    for i in range(5):
        for j in range(2):
            gridMatX[i,j] = points[gridMatIndex[i]+j,1]
            gridMatY[i,j] = points[gridMatIndex[i]+j,2]
    
    
    for i in range(5):
        plt.plot(gridMatX[i,:],gridMatY[i,:],'gray',zorder=0)
    for i in range(2):
        plt.plot(gridMatX[:,i],gridMatY[:,i],'gray',zorder=0) 
        
    if(plotRealSensors == 1):    
        realSensorListX = [gridMatX[1,0],gridMatX[2,0],gridMatX[3,0],gridMatX[2,1]]
        realSensorListY = [gridMatY[1,0],gridMatY[2,0],gridMatY[3,0],gridMatY[2,1]]
        sensorID = ['1','2','3','4']
        #for i in range(len(realSensorListX)):
        #    plt.text(realSensorListX[i], realSensorListY[i], '%s' % sensorID[i] ,size = 10)
        
    
    plt.xlabel('Global X')
    plt.ylabel('Global Y')
    plt.ylim([-40000,40000])


def scalePoints(mode,points,scaleFactor,plotRealSensors=0):
    #This is a function adapted for the visualization of horizontal mode shapes. 
    #Displacements in the Y-direction will be scaled only.
    
    modeMatrix_horizontal = np.ones((5,2))
    modeMatrix_horizontal[0,:] = 0
    modeMatrix_horizontal[4,:] = 0
    modeMatrix_horizontal[1,:] = mode[0]*scaleFactor
    modeMatrix_horizontal[3,:] = mode[2]*scaleFactor
    modeMatrix_horizontal[2,0] = mode[1]*scaleFactor
    modeMatrix_horizontal[2,1] = mode[3]*scaleFactor
    
    sensorID = ['1','2','3','4']
    
 
    gridMatIndex = [0,2,4,6,8]
    copyIndex = [0,1,2,3,4]
    gridMatX = np.empty((5,2),dtype=float) 
    gridMatY = np.empty((5,2),dtype=float) 
    
    for i in range(5):
        for j in range(2):
            gridMatX[i,j] = points[gridMatIndex[i]+j,1]
            gridMatY[i,j] = points[gridMatIndex[i]+j,2]
    
    
    
    gridMatY= gridMatY + modeMatrix_horizontal
    #Visualizing Case6 Mode3
    if(plotRealSensors == 1):    
        realSensorListX = [gridMatX[1,0],gridMatX[2,0],gridMatX[3,0],gridMatX[2,1]]
        realSensorListY = [gridMatY[1,0],gridMatY[2,0],gridMatY[3,0],gridMatY[2,1]]
        
        for i in range(len(realSensorListX)):
            plt.text(realSensorListX[i], realSensorListY[i], '%s' % sensorID[i] ,size = 20)
            plt.plot(realSensorListX[i], realSensorListY[i],'o',zorder = 2)
        
    
   
    
    
    
    for i in range(5):
        plt.plot(gridMatX[i,:],gridMatY[i,:],'y',linewidth =3.0,zorder=1)
        
    for i in range(2):
        plt.plot(gridMatX[:,i],gridMatY[:,i],'y',linewidth =3.0,zorder=1) 
        

    plt.xlabel('Global X')
    plt.ylabel('Global Y')
    

#scalePoints(realMode,pointsMat,1000)    


def caseControl(modeNumber,caseNumber,listOfCaseShapes):
    #A help function which is used to quickly create input for case analysis.
    
    modeShape = listOfCaseShapes[caseNumber-1]
    case1_ms1 =  modeShape[2:,modeNumber-1]
    modeFreq = modeShape[1,modeNumber-1]
    print(modeShape[1,0])
    print(modeShape[1,1])
    realMode = np.empty(len(case1_ms1),dtype=float)
    for i in range(0,len(case1_ms1)):
        complexNumber = complex(case1_ms1[i])
        print(complexNumber.real)
        realMode[i] = complexNumber.real
          
    return realMode, modeFreq


def plotModeShapeComplexityVertical(modeShape,title):
    #A function adapted to plot the complexity of vertical mode shpaes. 
    #The reason for this is that the vertical mode shapes have the x-direction included in its scaling. However, it seems like the displacements is the X-direction is nigligible.
    
    realValuesX = []
    imagValuesX = []
    
    realValuesZ = []
    imagValuesZ = []
    
    
    for i in range(2,len(modeShape)):
        number = complex(modeShape[i])
        
        if(i%2 ==0):
            realValuesX.append(number.real)
            imagValuesX.append(number.imag)
        else:
            realValuesZ.append(number.real)
            imagValuesZ.append(number.imag)
    
    newArrReX = np.zeros((len(realValuesX)*2))
    newArrImX = np.zeros((len(realValuesX)*2))
    
    newArrReZ = np.zeros((len(realValuesZ)*2))
    newArrImZ = np.zeros((len(realValuesZ)*2))
    oldArrIndex = 0
    for i in range(1,len(newArrImX),2):
        newArrReX[i] = realValuesX[oldArrIndex]
        newArrImX[i] = imagValuesX[oldArrIndex]
        
        newArrReZ[i] = realValuesZ[oldArrIndex]
        newArrImZ[i] = imagValuesZ[oldArrIndex]
        oldArrIndex +=1
        
    plt.figure(title)
    for i in range(0,len(newArrImX),2):
        plt.plot([newArrReX[i],newArrReX[i+1]],[newArrImX[i],newArrImX[i+1]],'g')
        plt.plot([newArrReZ[i],newArrReZ[i+1]],[newArrImZ[i],newArrImZ[i+1]],'g')
    
    sensorID = ['71','72','73','75']
    for i in range(len(imagValuesX)):
        plt.text(realValuesX[i], imagValuesX[i], '%s' % 'X'+sensorID[i] ,size = 20)
        plt.text(realValuesZ[i], imagValuesZ[i], '%s' % 'Z'+sensorID[i] ,size = 20)
    
    plt.plot(realValuesX,imagValuesX,'o')
    plt.plot(realValuesZ,imagValuesZ,'o')
    plt.grid()
    plt.xlabel('Re', size = 16)
    plt.ylabel('Im', size = 16)
    plt.ylim(-0.7,0.7)
    plt.xlim(-0.7,0.7)
    plt.axhline(y=0, color='k')
    plt.axvline(x=0, color='k')


def plotModeShapeComplexityVertical_onlyZ(modeShape):
    
    

    realValuesZ = []
    imagValuesZ = []
    
    
    for i in range(len(modeShape)):
        number = complex(modeShape[i])
        realValuesZ.append(number.real)
        imagValuesZ.append(number.imag)
    

    
    newArrReZ = np.zeros((len(realValuesZ)*2))
    newArrImZ = np.zeros((len(realValuesZ)*2))
    oldArrIndex = 0
    for i in range(1,len(newArrImZ),2):

        newArrReZ[i] = realValuesZ[oldArrIndex]
        newArrImZ[i] = imagValuesZ[oldArrIndex]
        oldArrIndex +=1
        
    
    for i in range(0,len(newArrImZ),2):
        plt.plot([newArrReZ[i],newArrReZ[i+1]],[newArrImZ[i],newArrImZ[i+1]],'g')
        
    
    sensorID = ['1','2','3','4']
    for i in range(len(imagValuesZ)):

        plt.text(realValuesZ[i], imagValuesZ[i], '%s' % sensorID[i] ,size = 20)
    
 
    plt.plot(realValuesZ,imagValuesZ,'o')
    plt.grid()
    plt.xlabel('Re', size = 20)
    plt.ylabel('Im', size = 20)
    plt.ylim(-0.7,0.7)
    plt.xlim(-0.7,0.7)
    plt.axhline(y=0, color='k')
    plt.axvline(x=0, color='k')


def scalePointsVertical_SingleZ(Vmode,points,scaleFactor=20000,plotRealSensors=0):
    #This is a function adapted for the visualization of horizontal mode shapes. Displacements in the Y-direction will be scaled only.
    
    sensorID = ['1','2','3','4']
    plotGridRows = 5
    plotGridCols = 2
    modeMatrix_VZ = np.ones((plotGridRows,plotGridCols))
    numbSensors = len(Vmode)
    
    realVal = []

    for i in range(numbSensors):
        number = np.complex64(Vmode[i])
        realVal.append(number.real)
        

    modeMatrix_VZ[0,:] = 0
    
    modeMatrix_VZ[1,:] = int(realVal[0]*scaleFactor)
    modeMatrix_VZ[3,:] = int(realVal[2]*scaleFactor)
    modeMatrix_VZ[2,0] = int(realVal[1]*scaleFactor)
    modeMatrix_VZ[2,1] = int(realVal[3]*scaleFactor)
    
    modeMatrix_VZ[4,:] = 0
   
  
 
    gridMatIndex = [0,2,4,6,8]
    copyIndex = [0,1,2,3,4]
    gridMatX = np.empty((5,2),dtype=float) 
    gridMatZ = np.empty((5,2),dtype=float) 
    
    for i in range(plotGridRows):
        for j in range(plotGridCols):
            gridMatX[i,j] = points[gridMatIndex[i]+j,1]
            gridMatZ[i,j] = points[gridMatIndex[i]+j,3]
    
    
    '''
    for i in range(plotGridRows):
        plt.plot(gridMatX[i,:],gridMatZ[i,:],'gray',zorder=1)
        
        
    for i in range(plotGridCols):
        plt.plot(gridMatX[:,i],gridMatZ[:,i],'gray',zorder=1) 
     '''   
        
    #gridMatX= gridMatX + modeMatrix_VX
    gridMatZ= gridMatZ + modeMatrix_VZ
    
    #Visualizong Case6 Mode3
    if(plotRealSensors == 1):    
        realSensorListX = [gridMatX[1,0],gridMatX[2,0],gridMatX[3,0],gridMatX[2,1]]
        realSensorListY = [gridMatZ[1,0],gridMatZ[2,0],gridMatZ[3,0],gridMatZ[2,1]]
        
        for i in range(len(realSensorListX)):
            plt.text(realSensorListX[i], realSensorListY[i], '%s' % sensorID[i] ,size = 20)
            plt.plot(realSensorListX[i], realSensorListY[i],'o',zorder = 2)
        
    
   
    
    
    #plt.plot(eastline_X,eastline_Y,'b')
    #plt.plot(westline_X,westline_Y,'b')
    leftEdge = [988.31323349475, 102774.89610296187]
    rightEdge = [237.20618390647, -4065.89048708391]
    plt.plot(leftEdge,rightEdge,'grey',linewidth =3.0,zorder=1)
    
    for i in range(plotGridRows):
        plt.plot(gridMatX[i,:],gridMatZ[i,:],'k',linewidth =3.0,zorder=1)
        
    for i in range(plotGridCols):
        plt.plot(gridMatX[:,i],gridMatZ[:,i],'k',linewidth =3.0,zorder=1) 
        

    plt.xlabel('Global X')
    plt.ylabel('Global Z')
    plt.ylim(-40000,40000)


def scalePointsVertical(Vmode,points,plotRealSensors=0,scaleFactor=10000):
    #This is a function adapted for the visualization of horizontal mode shapes. Displacements in the Y-direction will be scaled only.
    modeRows = int((len(Vmode)-2))
    realMode = np.ones(len(Vmode))
    for i in range(2,len(Vmode)):
        thatComplexNumb = complex(Vmode[i])
        thatRealNumb = thatComplexNumb.real
        realMode[i] = thatRealNumb

    V_input_mode_X = np.ones(int(modeRows/2))
    V_input_mode_Z2 = np.ones(int(modeRows/2))
    Xindex = 0
    Zindex = 0
  
    
    for i in range(2,len(realMode)):
       
        if(i%2 == 0):
            V_input_mode_X[Xindex] = realMode[i]
            Xindex += 1
            
        else:
            
            V_input_mode_Z2[Zindex] = realMode[i]
            if(Zindex==3):
                print('Zindex = 3')
                print('zindex: '+str(Zindex))
                print('V_input_mode_Z[Zindex]: '+str(V_input_mode_Z2[Zindex]))
           
            Zindex  += 1
        

    

    modeMatrix_VX = np.ones((5,2))
    modeMatrix_VZ = np.ones((5,2))
    
    
    modeMatrix_VX[0,:] = 0
    modeMatrix_VX[4,:] = 0
    modeMatrix_VX[1,:] = V_input_mode_X[0]*scaleFactor
    modeMatrix_VX[3,:] = V_input_mode_X[2]*scaleFactor
    modeMatrix_VX[2,0] = V_input_mode_X[1]*scaleFactor
    modeMatrix_VX[2,1] = V_input_mode_X[3]*scaleFactor
    
    modeMatrix_VZ[0,:] = 0
    modeMatrix_VZ[4,:] = 0
    modeMatrix_VZ[1,:] = V_input_mode_Z2[0]*scaleFactor
    modeMatrix_VZ[3,:] = V_input_mode_Z2[2]*scaleFactor
    modeMatrix_VZ[2,0] = V_input_mode_Z2[1]*scaleFactor
    modeMatrix_VZ[2,1] = V_input_mode_Z2[3]*scaleFactor
    print(V_input_mode_Z2[3]*scaleFactor)
    print('3972: '+str(modeMatrix_VZ[2,0]))
  
    print('3975: '+str(modeMatrix_VZ[2,1]))
  
 
    gridMatIndex = [0,2,4,6,8]
    copyIndex = [0,1,2,3,4]
    gridMatX = np.empty((5,2),dtype=float) 
    gridMatZ = np.empty((5,2),dtype=float) 
    
    for i in range(5):
        for j in range(2):
            gridMatX[i,j] = points[gridMatIndex[i]+j,1]
            gridMatZ[i,j] = points[gridMatIndex[i]+j,3]
    
    
    #gridMatX= gridMatX + modeMatrix_VX
    gridMatZ= gridMatZ + modeMatrix_VZ
    
    #Visualizong Case6 Mode3
    if(plotRealSensors == 1):    
        realSensorListX = [gridMatX[1,0],gridMatX[2,0],gridMatX[3,0],gridMatX[2,1]]
        realSensorListY = [gridMatZ[1,0],gridMatZ[2,0],gridMatZ[3,0],gridMatZ[2,1]]
        sensorID = ['3971','3972','3973','3975']
        for i in range(len(realSensorListX)):
            plt.text(realSensorListX[i], realSensorListY[i], '%s' % sensorID[i] ,size = 28)
            plt.plot(realSensorListX[i], realSensorListY[i],'o',zorder = 2)
        
    
   
    
    
    #plt.plot(eastline_X,eastline_Y,'b')
    #plt.plot(westline_X,westline_Y,'b')
   
    
    for i in range(5):
        plt.plot(gridMatX[i,:],gridMatZ[i,:],'y',linewidth =3.0,zorder=1)
        
    for i in range(2):
        plt.plot(gridMatX[:,i],gridMatZ[:,i],'y',linewidth =3.0,zorder=1) 
        

    plt.xlabel('Global X')
    plt.ylabel('Global Z')
    plt.ylim(-50000,50000)

def plotModeShapeComplexityHorizontal(modeShape):
    realValues = []
    imagValues = []
    sensorID = ['1','2','3','4']
    
    for i in range(2,len(modeShape)):
        number = complex(modeShape[i])
        realValues.append(number.real)
        imagValues.append(number.imag)
    
    newArrRe = np.zeros((len(realValues)*2))
    newArrIm = np.zeros((len(realValues)*2))
    oldArrIndex = 0
    for i in range(1,len(newArrIm),2):
        newArrRe[i] = realValues[oldArrIndex]
        newArrIm[i] = imagValues[oldArrIndex]
        oldArrIndex +=1
        
    #plt.figure(title)
    for i in range(0,len(newArrIm),2):
        print(newArrRe[i])
        plt.plot([newArrRe[i],newArrRe[i+1]],[newArrIm[i],newArrIm[i+1]],'g')
    
    
    for i in range(len(imagValues)):
        plt.text(realValues[i], imagValues[i], '%s' % sensorID[i] ,size = 10)
    
    plt.plot(realValues,imagValues,'o')
    plt.grid()
    plt.xlabel('Re', size = 16)
    plt.ylabel('Im', size = 16)
    plt.ylim(-0.7,0.7)
    plt.xlim(-0.7,0.7)
    plt.axhline(y=0, color='k')
    plt.axvline(x=0, color='k')


def git_plotModeShapeComplexityHorizontal(modeShape):
    
    #Creating empty lists to hold the different values.
    realValues = []
    imagValues = []
    sensorID = ['1','2','3','4']
    
    #Filling the lists of imaginary and real numbers, now, if we only take into account that we are handling singl
    for i in range(len(modeShape)):
        number = complex(modeShape[i])
        realValues.append(number.real)
        imagValues.append(number.imag)
        
    
    
    
    
    newArrRe = np.zeros((len(realValues)*2))
    newArrIm = np.zeros((len(realValues)*2))
    oldArrIndex = 0
    for i in range(1,len(newArrIm),2):
        newArrRe[i] = realValues[oldArrIndex]
        newArrIm[i] = imagValues[oldArrIndex]
        oldArrIndex +=1
        
    #plt.figure(title)
    for i in range(0,len(newArrIm)-1):
        
        plt.plot([newArrRe[i],newArrRe[i+1]],[newArrIm[i],newArrIm[i+1]],'g')
    
    
    for i in range(len(imagValues)):
        plt.text(realValues[i], imagValues[i], '%s' % sensorID[i] ,size = 10)
    
    plt.plot(realValues,imagValues,'o')
    plt.grid()
    plt.xlabel('Re', size = 16)
    plt.ylabel('Im', size = 16)
    plt.ylim(-0.7,0.7)
    plt.xlim(-0.7,0.7)
    plt.axhline(y=0, color='k')
    plt.axvline(x=0, color='k')
modeshapeNumb = 1


def plotModeShape(modeResult,points,scalefactor=20000):
    #I want to 
    #plot mode shape complexity
    #    use the mode shape complexity functions?
    #plot mode shapes
    #    plot points
    #    Scale points
    
    
    modeResShape = np.shape(modeResult)
    numberOfModes = modeResShape[1]
    numberOfNodes = modeResShape[0]

         
    for i in range(numberOfModes):
        plt.figure("Mode Shape Complexity, Mode #"+str(i+1))
        git_plotModeShapeComplexityHorizontal(modeResult[:,i])
        
    
    
    for i in range(numberOfModes):
        plt.figure("Modeshape #"+str(i+1))
        plotSensorLocation_horizontal(points, plotRealSensors = 1)
        scalePoints(modeResult[:,i].real, points, scalefactor, plotRealSensors = 1)
        plt.ylim([-20000,20000])
        
        

def plotModeShapes_subPlot_horizontal(modeResult,points,scalefactor=10000):
    
    
    
    modeResShape = np.shape(modeResult)
    numberOfModes = modeResShape[1]
    numberOfNodes = modeResShape[0]

    
    for i in range(numberOfModes):
        plt.figure("Mode #"+str(i+1))
        plt.suptitle("Mode #"+str(i+1))
        plt.subplot(1,2,1)
        plt.title("Complexity")
        git_plotModeShapeComplexityHorizontal(modeResult[:,i])
        plt.subplot(1,2,2)
        plt.title("Mode Shape")
        plotSensorLocation_horizontal(points, plotRealSensors = 1)
        scalePoints(modeResult[:,i].real, points, scalefactor, plotRealSensors = 1)
        plt.ylim([-20000,20000])
        
    
    
   
        
def plotModeShapes_subPlot_vertical(modeResult, points, scalefactor=10000):   
         
    modeResShape = np.shape(modeResult)
    numbModes = modeResShape[1]
   

    
    for i in range(numbModes):
        plt.figure("Vertical Mode #"+str(i+1))
        plt.suptitle("Mode #"+str(i+1))
        plt.subplot(1,2,1)
        plt.title("Complexity")
        plotModeShapeComplexityVertical_onlyZ(modeResult[:,i])
        print(modeResult[:,i])
        plt.subplot(1,2,2)
        plt.title("Mode Shape")
        scalePointsVertical_SingleZ(modeResult[:,i], points, plotRealSensors = 1)
        plt.ylim([-20000,20000])
        
    
    
   
        



'''
plotSensorLocation_horizontal(pointsMat)


modeNumber = 3
caseNumber = 6

realModeOut, modeFreqOut = caseControl(modeNumber,caseNumber,listOfCaseShapes)
#figtitle = 'Case'+str(caseNumber)+'_Mode_'+str(modeNumber)+'_'+str(modeFreqOut)+'Hz'
figtitle = 'case6viz'
fig = plt.figure(figtitle)

plt.xticks([])
plt.yticks([])
plt.title('Case '+str(caseNumber)+', Mode '+str(modeNumber)+' at '+str(modeFreqOut)+' Hz.',size=38)
plotSensorLocation_horizontal(pointsMat)

scalePoints(realModeOut,pointsMat,8000,1)
plt.ylim([-40000,40000])
plt.show()
#plt.savefig('F:/00 - Master/00 - Master SSD2/00 - Selve greia/Chapter 4 - Analysis/03 - Modal analysis/FDD horizontal/ModeShapes/png/'+figtitle+'.png')   # save the figure to file
'''
