%
%  mdof_half_sine_core.m  ver 1.2  October 2, 2012
%
function[tt,acc,rel_disp,abase]=mdof_half_sine_core(fn,damp,PF,ModeShapes,T1,T2,ngw)
%
sz=size(ModeShapes);
%
num_dof=sz(1);
num_modes=sz(2);
%
Q=ModeShapes;
MST=ModeShapes';
%
%
disp(' ');
out1=sprintf(' Enter number of modes to include in analysis (max=%d) ',num_modes);
disp(out1);
ninc=input('  ');
%
if(ninc>num_modes)
   ninc=num_modes;
end
%
num_modes=ninc;
%
disp(' ');
A=input(' Enter amplitude(G) ');
disp(' ');
%
T=input(' Enter pulse duration(sec) '); 
%
disp(' ');
dur=input(' Enter total analysis duration (sec) ');
disp(' ');
sug = 20*fn(num_modes);
if(10/T > sug )
   sug = 10/T;
end
disp(' Enter sample rate (samples/sec) '); 
out1=sprintf(' (suggest > %9.5g)',sug);
disp(out1);
sr=input(' ');
dt=1/sr;
% 
nt=round(dur/dt);
%
omega=pi/T;
omegan=2*pi*fn;
%
den=zeros(num_modes,1);
U1=zeros(num_modes,1);
U2=zeros(num_modes,1);
V1=zeros(num_modes,1);
V2=zeros(num_modes,1);
P=zeros(num_modes,1);
omegad=zeros(num_modes,1);
domegan=zeros(num_modes,1);
omn2=zeros(num_modes,1);
%
rd=zeros(nt,num_dof);
rv=zeros(nt,num_dof);
ra=zeros(nt,num_dof);
abase=zeros(nt,1);
dvelox=zeros(nt,1);
ddisp=zeros(nt,1);
%
An=zeros(num_modes,1);
%
om2=omega^2;
for i=1:num_modes
   omegad(i)=omegan(i)*sqrt(1-damp(i)^2);
   domegan(i)=damp(i)*omegan(i);
   omn2(i)=(omegan(i))^2;
   den(i)=( (om2-omn2(i))^2 + (2*damp(i)*omega*omegan(i))^2);
   U1(i)=2*damp(i)*omega*omegan(i);
   U2(i)=om2-omn2(i);
   V1(i)=2*damp(i)*omegan(i)*omegad(i);
   V2(i)=om2-omn2(i)*(1-2*(damp(i))^2);
   P(i)=omega/omegad(i);
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Transition points
%
t=T;
%
eee=exp(-domegan*t); 
%
arg=omega*t;
%
argd=omegad*t;
%
c1=cos(arg);
s1=sin(arg);
cd=cos(argd);
sd=sin(argd);
%
Tn=zeros(num_modes,1);
Tnv=zeros(num_modes,1);
W=zeros(num_modes,1);
deee=zeros(num_modes,1);
%
for j=1:num_modes
%
    An(j)=PF(j)*A;
%   
    term1=U1(j)*c1 + U2(j)*s1;
    term2=V1(j)*cd(j)+V2(j)*sd(j);
    dterm1=omega*( -U1(j)*s1 + U2(j)*c1 );
    dterm2=omegad(j)*( -V1(j)*sd(j) + V2(j)*cd(j) );
    deee(j)=-damp(j)*omegan(j)*eee(j);
%
    Tn(j)=An(j)*(term1-P(j)*eee(j)*term2)/den(j);
%
    Tnv(j)=An(j)*(dterm1 -P(j)*( deee(j)*term2 +eee(j)*dterm2))/den(j);  
%
    W(j)= ( Tnv(j) +   damp(j)*omegan(j)*Tn(j) )/omegad(j);
%
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
 disp_T=-A*sin(omega*T)/omega^2;
velox_T=-A*cos(omega*T)/omega;
%
tt=zeros(nt,1);
%
n=zeros(num_modes,1);
nv=zeros(num_modes,1);
na=zeros(num_modes,1);
%
progressbar;
for i=1:nt
    progressbar(i/nt);
    t=dt*(i-1);
    tt(i)=t;
%
    eee=exp(-domegan*t); 
%
    arg=omega*t;
%
    argd=omegad*t;
%
    c1=cos(arg);
    s1=sin(arg);
    cd=cos(argd);
    sd=sin(argd);
%
    if(t<=T)
        abase(i)= A*s1;
       dvelox(i)=-A*c1/omega;
        ddisp(i)=-A*s1/om2;
    else
        abase(i)=0; 
       dvelox(i)= velox_T;  
        ddisp(i)= velox_T*(t-T)+disp_T;   
    end    
%
    for j=1:num_modes
%
        if(t<=T)
%            
            An(j)=PF(j)*A;
%   
            term1=U1(j)*c1 + U2(j)*s1;
            term2=V1(j)*cd(j)+V2(j)*sd(j);
            dterm1=omega*( -U1(j)*s1 + U2(j)*c1 );
            dterm2=omegad(j)*( -V1(j)*sd(j) + V2(j)*cd(j) );
            deee(j)=-damp(j)*omegan(j)*eee(j);
%
            n(j)=An(j)*(term1-P(j)*eee(j)*term2)/den(j);
%
            nv(j)=An(j)*(dterm1 -P(j)*( deee(j)*term2 +eee(j)*dterm2))/den(j);  
%
            na(j)= -2*damp(j)*omegan(j)*nv(j) -omn2(j)*n(j)-An(j)*s1;
%
        else
            ts=t-T;
%
            eee(j)=exp(-domegan(j)*ts); 
            deee(j)=-damp(j)*omegan(j)*eee(j);
%
            arg=omega*ts;
%
            argd=omegad*ts;
%
            c1=cos(arg);
            s1=sin(arg);
            cd=cos(argd);
            sd=sin(argd);
%
            n(j)=eee(j)*( Tn(j)*cd(j)  +W(j)*sd(j) );
            nv(j)=   deee(j)*n(j)...
                           +omegad(j)*eee(j)*( -Tn(j)*sd(j)  +W(j)*cd(j) );  
            na(j)= -2*damp(j)*omegan(j)*nv(j) -omn2(j)*n(j);
        end

%
    end
%
    rd(i,:)=Q(:,1:num_modes)*n(1:num_modes);
    rv(i,:)=Q(:,1:num_modes)*nv(1:num_modes);
    ra(i,:)=Q(:,1:num_modes)*na(1:num_modes);  
%
end
%
pause(0.5);
progressbar(1);
%
sz=size(T1);
base_ddisp_th =(ones(sz(2),1))*ddisp'; 
base_dvelox_th=(ones(sz(2),1))*dvelox';
base_drive_th =(ones(sz(2),1))*abase';
%
  dT=(T1*base_ddisp_th  + T2*rd')';
  vT=(T1*base_dvelox_th + T2*rv')';
accT=(T1*base_drive_th  + T2*ra')';
%
clear rd;
clear rv;
clear ra;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Put in original order
%
disp(' ');
disp(' Put in original order ');
  ZdT=[ base_ddisp_th'   dT ];
  ZvT=[ base_dvelox_th'  vT ];
ZaccT=[ base_drive_th'  accT ];
%
clear dT;   
clear vT;
clear accT;
%
clear d;   
clear v;
clear acc;
%
sz=size(ZdT);
num_dof=sz(2);
%
         d=zeros(nt,num_dof);
         v=zeros(nt,num_dof);
       acc=zeros(nt,num_dof);
  rel_disp=zeros(nt,num_dof);
%
progressbar;
for i=1:num_dof
progressbar(i/num_dof);
%
        d(1:nt,ngw(i))=    (ZdT(1:nt,i));        
        v(1:nt,ngw(i))=    (ZvT(1:nt,i));     
      acc(1:nt,ngw(i))=(ZaccT(1:nt,i));
%
end
pause(0.5)
progressbar(1);
%
%
clear ZdT;   
clear ZvT;
clear ZaccT;
%
for i=1:num_dof
    rel_disp(:,i)=d(:,i)-ddisp(:);
end
%
rel_disp=rel_disp*386;
d=d*386;
v=v*386;
%