##########################################################################
# program: mdof_modal_enforced_acceleration_rk4.py
# author: Tom Irvine
# version: 1.0
# date: December 30, 2011
# description:
#
#      Calculate the response of an MDOF system to enforced
#      acceleration using the Runge-Kutta Fourth-order method.
#
#      The system is uncoupled using normal modes as an
#      intermediate step.
#
#      The input file must have two columns: time(sec) & acceleration
##########################################################################

from tompy import read_two_columns
from tompy import GetInteger2
from tompy import read_array,enter_int,enter_float

from ode_plots import ode_plots

from Newmark import Newmark_coefficients

from generalized_eigen import generalized_eigen

import matplotlib.pyplot as plt

from numpy import zeros,linspace,interp,dot,pi
from numpy import delete,concatenate,sort,ones
from numpy import sqrt

from numpy import linalg

from scipy import eye
import scipy.integrate as igt


########################################################################

print("  ")
print(" Select units:      ")
print(" 1=English 2=metric ")

iu=GetInteger2()

print("  ")
if(iu==1):
    print(" units: mass (lbm)")
    print("        damping coefficients (lbf sec/in)")
    print("        stiffness (lbf/in)")
    print("        velocity (in/sec)")
    print("        displacement (in)")
else:
    print(" units: mass (kg)")
    print("        damping coefficients (N sec/m)")
    print("        stiffness (N/m)")
    print("        velocity (m/sec)")
    print("        displacement (m)")

print("  ")

mass=read_array("mass matrix")
stiff=read_array("stiffness matrix")


if(iu==1):
    mass/=386

print(" ")



ndof,fn,MS =generalized_eigen(stiff,mass,2)

num=ndof

print "\n number of dofs =%d \n" %num


print " "
print " Select damping input method "
print "  1=uniform damping ratio"
print "  2=damping ratio vector "
idm=GetInteger2()

if(idm==1):
    damp=zeros(ndof,float)
    print " "
    print " Enter uniform damping ratio "
    udamp = enter_float()
    for i in range(0,ndof):
        damp[i]=udamp
else:
    damp=read_array(" Enter damping ratio vector")



max_fn=max(fn)
min_fn=min(fn)

if(min_fn>0):
    period_max=1/min_fn
else:
    period_min=1/max_fn

period_min=1/max_fn

dtr=period_min/20

########################################################################


print " "
print " Enter the duration (sec) "
dur=enter_float()

srr=20*max_fn

print " "
print " Enter the sample rate (recommend > %8.4g) " %srr
sr=enter_float()

dt=1/sr



########################################################################


nt=1+int(dur/dt)

t =  linspace(0,dur,nt)

##############################################################################

if iu==1:
    print "Each acceleration file must have two columns: time(sec) & accel(G) ";
else:
    print "Each acceleration file must two columns: time(sec) & accel(m/sec^2) ";


print " "
print "Enter the number of acceleration files "

nff=enter_int()

MAX=100000

tt=zeros((MAX,nff),float)
ff=zeros((MAX,nff),float)

accel_dof=zeros(ndof,float)

for i in range(0,ndof):
    accel_dof[i]=-999


print " "
print "Note: the first dof is 1 "

for i in range(0,nff):
    ii=i+1
    print " "
    print " Enter acceleration filename %d " %ii

    a,b,L=read_two_columns()

    if(L>MAX):
        L=MAX

    tt[0:L,i]=a[0:L]
    ff[0:L,i]=b[0:L]

    print " "
    print "Enter the number of dofs at which this acceleration is applied "

    nfa=enter_int()

    for j in range(0,nfa):
        print " "

        if(j==0 and nfa==0):
            print "Enter the dof number for this acceleration "

        if(j==0 and nfa>0):
            print "Enter the first dof number for this acceleration "

        if(j>0 and nfa>0):
            print "Enter the next dof number for this acceleration "

        nn=enter_int()
        accel_dof[nn-1]=i


# interpolate acceleration

print "begin interpolation "

FFI=zeros((nt,nff),float)

for i in range(0,nff):

    tstart=tt[0,i]
    tt[:,i]=tt[:,i]-tstart

    last=MAX

    for j in range(1,MAX):
        if(tt[j,i]<=tt[j-1,i]):
            last=j
            break

    tint=tt[0:last,i]
    fint=ff[0:last,i]

    FFI[:,i] = interp(t,tint,fint)




if(iu==1):
    FFI=FFI*386

j=0
k=0

temp_enforced_dof=zeros(num)
temp_free_dof=zeros(num)

print " "
print "accel_dof"
print accel_dof
print " "

for i in range(0,num):
    if(accel_dof[i]>=0):
        temp_enforced_dof[k]=i
        k=k+1
    else:
        temp_free_dof[j]=i
        j=j+1


nem=k

nfree=j

enforced_dof=zeros(k,int)
free_dof=zeros(j,int)

enforced_dof[:]=temp_enforced_dof[0:k]
free_dof[:]=temp_free_dof[0:j]


ngw=zeros(ndof,int)

print nem,nfree,ndof

ngw=concatenate((enforced_dof, free_dof), axis=0)


temp=sort(enforced_dof)
enforced_dof=temp[::-1]


temp=sort(free_dof)
free_dof=temp[::-1]

print "end interpolation "


##############################################################################


forig=zeros((nt,nem),float)

k=0
for i in range(0,num):
    nfi=accel_dof[i]
    if(nfi>=0):
        forig[:,k]=FFI[:,nfi]
        k=k+1


FFI=forig

dvelox=zeros((nt,nem),float)
ddisp=zeros((nt,nem),float)


for i in range(0,nem):
    A=FFI[:,i]
    dv=igt.cumtrapz(A)
    dvelox[1:nt,i]=dv*dt


for i in range(0,nem):
    A=dvelox[:,i]
    dd=igt.cumtrapz(A)
    ddisp[1:nt,i]=dd*dt


############################################################

#  Partition Mass & Stiffness Matrices

##################### Mdd Kdd ##############################

Mdd=mass
Kdd=stiff

for i in range (0,nfree):
    row=free_dof[i]
    col=row

    Mdd=delete(Mdd, row, 0)
    Mdd=delete(Mdd, col, 1)

    Kdd=delete(Kdd, row, 0)
    Kdd=delete(Kdd, col, 1)

##################### Mdf Kdf ############################################%

Mdf=mass
Kdf=stiff

for i in range (0,nfree):
    row=free_dof[i]

    Mdf=delete(Mdf, row, 0)
    Kdf=delete(Kdf, row, 0)




for i in range(0,nem):
    col=enforced_dof[i]

    Mdf=delete(Mdf, col, 1)
    Kdf=delete(Kdf, col, 1)

##################### Mfd Kfd ##############################################################%

Mfd=Mdf.T
Kfd=Kdf.T

####################### Mff Kff ############################################################%

Mff=mass
Kff=stiff
for i in range(0,nem):
    row=enforced_dof[i]
    col=row

    Mff=delete(Mff, row, 0)
    Mff=delete(Mff, col, 1)

    Kff=delete(Kff, row, 0)
    Kff=delete(Kff, col, 1)


##########################################################################################

I=eye(nem)
T2=eye(nfree)
nKff=max([Kff.shape[0],Kff.shape[1]])

invKff=linalg.inv(Kff)

print "invKff"
print invKff

print "Kfd"
print Kfd

T1=-dot(invKff,Kfd)

TT=zeros((num,num),float)

TT[0:nem,0:nem]=I
TT[nem:num,0:nem]=T1
TT[nem:num,nem:num]=T2

MP=zeros((num,num),float)
MP[0:nem,0:nem]=Mdd
MP[0:nem,nem:num]=Mdf
MP[nem:num,0:nem]=Mfd
MP[nem:num,nem:num]=Mff

KP=zeros((num,num),float)
KP[0:nem,0:nem]=Kdd
KP[0:nem,nem:num]=Kdf
KP[nem:num,0:nem]=Kfd
KP[nem:num,nem:num]=Kff

print "MP"
print MP

print "KP"
print KP

print "TT"
print TT


MT=dot(TT.T,dot(MP,TT))
KT=dot(TT.T,dot(KP,TT))

############################################################

print "MT"
print MT

print "KT"
print KT


MST=MS.T

print " "

print "nem,num"
print nem,num

Mwd=MT[nem:num,0:nem]
Mww=MT[nem:num,nem:num]
Kww=KT[nem:num,nem:num]

print "Mww"
print Mww

print "Kww"
print Kww


nq,fn,MS =generalized_eigen(Kww,Mww,1)
MST=MS.T

omegan=2*pi*fn

r=ones((nfree,1))

part = dot(MST,dot(Mww,r))


print " "
print "Participation Factors  "
print part

############################################################

#  Modal Transient

omegad=zeros(nfree)

for i in range (0,nfree):
    omegad[i]=omegan[i]*sqrt(1-damp[i]**2)



Q=MS
QT=Q.T

############################################################

#  displacement

#  d
#  v
#  acc


##
##
n=nfree

kk=zeros(n,float)
cc=zeros(n,float)

for i in range(0,n):
    kk[i]=(omegan[i])**2.
    cc[i]=2*damp[i]*omegan[i]


##

nx=zeros((nt,nfree),float)
ny=zeros((nt,nfree),float)
na=zeros((nt,nfree),float)

d=zeros((nt,n),float)
v=zeros((nt,n),float)
acc=zeros((nt,n),float)

jj=1

##############################%

ntm=nt-1


for i in range (0,ntm):

    h=t[i+1]-t[i]

    hd2=h/2

    nFA=-dot(QT,dot(Mwd,FFI[i,:]))
    nFB=-dot(QT,dot(Mwd,FFI[i+1,:]))

    FF1= nFA
    FF2= (nFB + nFA)/2.
    FF3= FF2
    FF4= nFB

    for j in range(0,nfree):
        xx=nx[i,j]
        yy=ny[i,j]

        X1= xx
        Y1= yy
        F1= (FF1[j]-cc[j]*Y1-kk[j]*X1)

        X2= X1+Y1*hd2
        Y2= Y1+F1*hd2
        F2= (FF2[j]-cc[j]*Y2-kk[j]*X2)

        X3= X1+Y2*hd2
        Y3= Y1+F2*hd2
        F3= (FF3[j]-cc[j]*Y3-kk[j]*X3)

        X4= X1+Y3*h
        Y4= Y1+F3*h
        F4= (FF4[j]-cc[j]*Y4-kk[j]*X4)

        nx[i+1,j]= (X1+(h/6.)*( Y1 + 2*Y2 + 2*Y3 + Y4 ))     # displacement
        ny[i+1,j]= (Y1+(h/6.)*( F1 + 2*F2 + 2*F3 + F4 ))     # velocity
        na[i+1,j]= FF4[j] -cc[j]*ny[i+1,j] -kk[j]*nx[i+1,j]  # acceleration

    d[i+1,:]=dot(Q,nx[i+1,:])
    v[i+1,:]=dot(Q,ny[i+1,:])
    acc[i+1,:]=dot(Q,na[i+1,:])


############################################################

d_save=d
v_save=v
a_save=acc

#    Transformation back to xd xf

##    dT=TT*d'
##    vT=TT*v'
##    accT=TT*acc'

dT=(dot(T1,ddisp.T) + dot(T2,d.T)).T

vT=(dot(T1,dvelox.T) + dot(T2,v.T)).T

accT=(dot(T1,FFI.T) + dot(T2,acc.T)).T


#  Put in original order

ZdT=zeros((nt,num),float);
ZvT=zeros((nt,num),float);
ZaccT=zeros((nt,num),float);

ZdT[0:nt,0:nem]=ddisp
ZdT[0:nt,nem:num]=dT

ZvT[0:nt,0:nem]=dvelox
ZvT[0:nt,nem:num]=vT

ZaccT[0:nt,0:nem]=forig
ZaccT[0:nt,nem:num]=accT



d=zeros((nt,num),float)
v=zeros((nt,num),float)
acc=zeros((nt,num),float)

for i in range(0,num):
    for j in range(0,nt):
        d[j,ngw[i]]=  (ZdT[j,i])
        v[j,ngw[i]]=  (ZvT[j,i])
        acc[j,ngw[i]]=(ZaccT[j,i])


########################################################################

if(iu==1):
    a=acc/386
else:
    a=acc/9.81

fignum=1
ode_plots(fignum,nt,num,t,d,v,a,iu)

plt.show()