clear all; close all; clc; g=9.81;
set(groot,'defaulttextinterpreter','latex');  
set(groot, 'defaultAxesTickLabelInterpreter','latex');  
set(groot, 'defaultLegendInterpreter','latex');

N=56*2;                 % pocet prvkov
nnel=2;                 % pocet dof na element
uzoldof=2;              % pocet dof na uzol
nnode=(nnel-1)*N+1;     % celkovy pocet nodov
Ndof=nnode*uzoldof;     % celkovy pocet supnov volnosti 
%________________________________________________________________________________________________________
%% PARAMETRE
L=56e-3;            % dlzka nosnika celkova [m]
Offset = 1e-3;      % poloha pieza a elektrody od votknutia [m]

Lp=43e-3;           % dlzka pieza [m]
Le=43e-3;
Lz=6e-3;            % dlzka konca [m]

b=10e-3;            % sirka nosnika [m]
be=7.5e-3;          % sirka elektrody [m]

hs=.3e-3;           % hrubka substratu [m]
hp=.13e-3;          % hrubka PVDF [m]

Es=2.1e11;          % Youngov modul substrat [Pa]
Ep=2e9;             % Youngov modul PVDF [Pa]
mi_p=0.34;

d31 = -1.9e-11;     % piezoelektrická konštanta d31 [C/N]
d33 = 2e-11;        % piezoelektrická konštanta d33 [C/N]
d51 = 2e-11;        % piezoelektrická konštanta d51 [C/N]

eps0 = 8.854e-12;   % permtivita vakua [F/m]
eps33 = 12;         % relativna permitivita [-]
eps11 = 12;         % relativna permitivita [-]

mM = 5.3e-3;        % hmotnost zavazia [kg]
xi = 0.01;        % tlmenie [-]
 
accel = 50e-3*g;    % amplituda zrychlenia [m/s^2]
Rz =1e7;            % odpor zataze [Ohm]

rho_s=7850;         % hustota substratu [kg/m^3]
rho_p=1760;         % hustota PVDF [kg/m^3]
rho_z = mM/(Lz*b*hs); % hustota zavazia [kg/m^3]

%prvky
elem=L/N;           % dlzka 1 prvku
elemE = [round(Offset/elem) round((Le+Offset)/elem)];   % cislo prvku z L, kde zacina a konci elektroda
elemP = [round(Offset/elem) round((Lp+Offset)/elem)];   % cislo prvku z L, kde zacina a konci piezo
elemZ = [round((L-Lz)/elem) N];                         % cislo prvku z L, kde zacina a konci zavazie

%% vlastnosti - smery
sPVDFmat = [    1/Ep -mi_p/Ep  -mi_p/Ep 0 0 0;  % matica poddajnosti
                -mi_p/Ep 1/Ep -mi_p/Ep 0 0 0;
                -mi_p/Ep -mi_p/Ep 1/Ep 0 0 0;
                0 0 0 (2*(1+mi_p))/Ep 0 0;
                0 0 0 0 (2*(1+mi_p))/Ep 0;
                0 0 0 0 0 (2*(1+mi_p))/Ep];
cPVDFmat = inv(sPVDFmat);                       % matica tuhosti

d_mat = [   0 0 d31;    % matica piezo konstanty d
            0 0 d31;
            0 0 d33;
            0 d51 0;
            d51 0 0;
            0 0 0];
e_mat = cPVDFmat*d_mat; % matica piezo konstanty e

e31=e_mat(1,3); 
epsTmat = [     eps11*eps0 0 0;         % absolutna permitivita, konstantne napatie [F]
                0 eps11*eps0 0;
                0 0 eps33*eps0];
            
epsilonSmat = epsTmat-(d_mat'*e_mat);   % absolutna permitivita, konstantne pretvorenie [F] 
epsilonTrelmat = epsilonSmat/eps0;      % relativna permitivita, konstantne napatie [F]
k_konst = sqrt(inv(epsTmat)*((d_mat'.^2)*inv(sPVDFmat))); % k-faktor
eps33_t = epsilonTrelmat(3,3); 
eps33_s = epsilonSmat(3,3);

%________________________________________________________________________________________________________
%% NEUTRALNA OS
SS = (hs+hp)/2;                                     % geom. stred
zTs = -(SS-(hs/2));                                 % vzd. tazisko substratu -geom. stred
zTp = hs+(hp/2)-SS;                                 % vzd. tazisko pieza -geom. stred
zn = ((Es*zTs*hs)+(Ep*zTp*hp))/((Es*hs)+(Ep*hp));   % neutralna os

% PRIEREZY
as=b*hs; % substrat
ap=b*hp; % PVDF

%________________________________________________________________________________________________________
%% osove kvadraticke momenty 
Js=((b*hs^3)/12); % substrat
Jp=((b*hp^3)/12); % PVDF
%________________________________________________________________________________________________________
%% HMOTNOST
%HMOTNOST elementu
me=[    156 22*elem 54 -13*elem;...
        22*elem 4*elem^2 13*elem -3*elem^2;...
        54 13*elem 156 -22*elem;...
        -13*elem -3*elem^2 -22*elem 4*elem^2];
ms =((rho_s*as*elem)/420)*me;                   %substrat
mps =( (((rho_p*ap)+(rho_s*as) )*elem)/420)*me; %PVDF  
mz =(((rho_z+rho_s)*as*elem)/420)*me;           %zavazie

% GLOBALNA HMOTNOST
M_G=zeros(Ndof,Ndof);
for j = 1:N
    if (j <= elemP(1) )   ||   ((j > elemP(2)) && (j <= elemZ(1)) )
        M_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4)=M_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4)+ms;
    elseif ( (j > elemP(1)) && (j <= elemP(2) )  )
        M_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4)=M_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4)+mps;
    elseif((j > elemZ(1)) && (j <= elemZ(2)) )
        M_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4)=M_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4)+mz;
    end
end
M=M_G; % Globalna hmotnost 

%________________________________________________________________________________________________________
%% TUHOST
%laminatova teoria - konstanty
konst_A = (Es*hs)+(Ep*hp);
konst_B = 0.5*hs*hp*(Ep-Es);
konst_D = (1/12)*(    (  Es*((hs^3)+(3*hs*hp^2))  ) + (  Ep*((hp^3)+(3*hp*hs^2))  )      );
EspJsp = ((konst_D-(konst_B^2/konst_A))*b);

% TUHOST ELEMENTU
ke=[    12 6*elem -12 6*elem;...
        6*elem 4*elem^2 -6*elem 2*elem^2;...
        -12 -6*elem 12 -6*elem;...
         6*elem 2*elem^2 -6*elem 4*elem^2];  
ks=((Es*Js)/elem^3)*ke;   % substrat 
kps=(EspJsp/elem^3)*ke;   % PVDF

% GLOBALNA TUHOST
K_G=zeros(Ndof,Ndof);
for j = 1:N
     if ( (j > elemP(1)) && (j <= elemP(2) )  )
        K_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4) = K_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4)+kps;
     else
        K_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4) = K_G((j-1)*2+1:(j-1)*2+4,(j-1)*2+1:(j-1)*2+4)+ks;
     end
end
K=K_G; % globalna tuhost

%% OKRAJOVE PODMIENKY votknutie
OP =3;
M=M(OP:end,OP:end); %okr. podmienka
K=K(OP:end,OP:end); %okr. podmienka

%% VLASTNA FREKVENCIA
Omega0 = min(abs(sqrt(eig(K,M))));
f_0 = Omega0/(2*pi);
fprintf('Vlastna frekvencia NDOF %.4f Hz \n',f_0(1))

%% VLASTNY TVAR
[Eigenvectors,Eigenvalues] = eig(K,M);
[~,ind]=sort(diag(Eigenvalues),'ascend');
Modeshapes_sorted = Eigenvectors(:,ind);
FI = Modeshapes_sorted(:,1);
FI = FI/(sqrt(FI'*M*FI));               % vlastny tvar

%________________________________________________________________________________________________________
%% TLMENIE
C = 2*xi*M*min(abs(sqrt(eig(K,M))));    % globalne tlmenie

%________________________________________________________________________________________________________
%% KAPACITA
Cp=(Le*be*eps33_t*eps0)/hp ;            % globalna kapacita

%________________________________________________________________________________________________________
%% COUPLING
phi = FI(1:2:2*N);                      % neparne=posuvy
phi_d = FI(2:2:2*N);                    % parne=natocenia
Theta = -e31 * be* (zTp-zn)* (phi_d(elemE(2))- phi_d(elemE(1))) /phi(elemZ(2)); % fyzikalne suradnice

%________________________________________________________________________________________________________
%% HARMONICKA ANALYZA - U-f, Y-f charakteristia
%frekvencny rozsah
f_start=20;
f_end=21.5; 
Om_start=2*pi*f_start; 
Om_end = f_end*2*pi;
d_Om=0.01;

U=zeros(1,numel(Om_start:d_Om:Om_end));
V=zeros(1,numel(Om_start:d_Om:Om_end));
j=0;
for omega=Om_start:d_Om:Om_end 
    j=j+1;
    konst = ((Cp*i*omega)+(1/Rz));
    U(j) = (-accel*konst)/((((-omega^2)+(2*xi*i*omega*Omega0)+Omega0^2)*konst)+((Theta^2)*i*omega));
    V(j)= (Theta*i*omega*accel) / (  ((-omega^2+(2*xi*i*omega*Omega0)+Omega0^2)*konst) +((Theta^2)*i*omega) );
end

save('NDOFMatlabPlotAF','U','V','Omega0','Om_start','d_Om','Om_end');

%________________________________________________________________________________________________________
%% TRANZIENTNA ANALYZA
t_end = 2;
U0=-2e-3;
om=Omega0;
A0 = accel/om^2;
%SIMULINK
out = sim('SDOF_model_TR.slx',t_end);
t_mat = out.ScopeData.time;
U_mat = out.ScopeData.signals(1).values;
V_mat = out.ScopeData.signals(2).values;
save('NDOFMatlabPlotTR','t_mat','U_mat','V_mat');






















