%init
clear all
close all
clc


E=200000; % [MPa]
NU=0.3; 
rho=7.85e-9; % [ton/mm^3]

% Creazione prisma in base a vari parametri
subsX=6; % suddivisioni in X e Y uguali
subsZ=51; % e Z

hx=20; % dim in X [mm]
hy=20; % dim in Y [mm]
hz=200; % dim in Z [mm]

% Creazione nodi e elementi
nx0=(0:hx/(subsX-1):hx)';
ny0=(0:hy/(subsX-1):hy)';
ny=(sort(repmat(ny0',1,length(nx0))))';
nx=(repmat(nx0',1,length(nx0)))';

nz0=(0:hz/(subsZ-1):hz)';
nz=sort(repmat(nz0',1,length(nx)))';

nxy=repmat([nx,ny]',1,subsZ)';

nodi=[(1:length(nxy))',nxy,nz];

i=1;
for ex=1:subsX-1
    
    el0(i,:)=[ex,ex+1,ex+subsX,ex+subsX+1];
    
    
    i=i+1;
end

el1=el0;
for ey=1:subsX-2
    el1(ey*(subsX-1)+1:ey*(subsX-1)+(subsX-1),1:2)=el1((ey-1)*(subsX-1)+1:(ey-1)*(subsX-1)+(subsX-1),3:4);
    el1(ey*(subsX-1)+1:ey*(subsX-1)+(subsX-1),3:4)=el1(ey*(subsX-1)+1:ey*(subsX-1)+(subsX-1),1:2)+subsX;
end

se=length(el1);
sn=length(nx);
el2=el1;
for ez=1:subsZ-2
    el2((ez-1)*se+1:(ez-1)*se+se,5:8)= el2((ez-1)*se+1:(ez-1)*se+se,1:4)+sn;
    el2((ez)*se+1:(ez)*se+se,1:4)=el2((ez-1)*se+1:(ez-1)*se+se,5:8);
end
el2((ez)*se+1:(ez)*se+se,5:8)= el2((ez)*se+1:(ez)*se+se,1:4)+sn;

nnodi=size(nodi,1);
nele=size(el2,1);
elementi=[(1:nele)',el2];;

% vincoli=vin(:,1);
% 
% carichi=For(:,1);

elementi(:,:)=elementi(:,[1 2 3 5 4 6 7 9 8]);
Gflag=0;
PlotMesh(nodi(:,2:4),elementi(:,2:9),0)



tic
[K,Mass_coupled,Mass,Gyro,Corio,vtot,distorsione,Te,BK,BG,SG,map,elemconv]=poliFEMo_sparse(nodi,elementi,E,NU,rho,Gflag);
toc

K=simmetrization(K);
Mass=simmetrization(Mass);
% [avf,evf]=eigs(K,Mass,50,'sm');
% evdf=diag(evf);
% [evdsf,ind]=sort(evdf);
% EVALfree=(evdsf.^0.5)/2/pi; %[Hz]

% ESEMPI PER APPLICAZIONE FORZA E VINCOLI
% vincoli=vin(:,1); % vettore colonna con il NUmero del nodo da vincolare
% vincoli=(1:25)'; % faccia in Z=0;
vincoli = find(nodi(:,4)==0);
% carichi=For(:,1); % vettore colonna con il NUmero del nodo da forzare
% carichi=(476:500)'; % faccia a Z=max
carichi = find(nodi(:,4)==hz);
nodi_est(:,1)= find(nodi(:,2)==0);
nodi_est(:,2)= find(nodi(:,2)==max(nodi(:,2)));
nodi_est(:,3)= find(nodi(:,3)==0);
nodi_est(:,4)= find(nodi(:,3)==max(nodi(:,3)));
nodi_est=reshape(nodi_est,1,[])';
nodi_est=unique(nodi_est);


% COSTRUZIONE VETTORE FORZANTE
% FORZA FX, totale  1200N
F=zeros(size(K,1),1);
Fmodulus=1200/size(carichi,1);
for cf=1:size(carichi,1)
    
    [row,col]=find(carichi(cf)==elemconv);
    DOF=1; % X DIR - da cambiare per altre direzioni e da ripetere tutto se 
    % si vuole pi� di una direzione
    pos=map(row(1),3*(col(1)-1)+DOF); %X dir
    
    F(pos,1)=Fmodulus;
   
end


%VINCOLI
%blocco fine della travetta
mapdof=[1:size(K)]';
mapnodes=nodi(:,1);

for cc=1:size(vincoli,1)
    
    [row,col]=find(elemconv==vincoli(cc));
    % blocca tutti i dof in questo caso
%     if nodi(vincoli(cc),2)==7.5
    pos=map(row(1),3*(col(1)-1)+[1:3]); %XYZ dof
%     else
%      pos=map(row(1),3*(col(1)-1)+3); %Z dof   
%     end
    
    mapdof(pos,1)=0;
    mapnodes(row,1)=0;
end
mapfree=unique(mapdof);
mapdof=[1:length(mapdof)]';

% APPLICAZIONE VINCOLI
Kbv=K(mapfree(2:end),mapfree(2:end));


Mbv=Mass(mapfree(2:end),mapfree(2:end));
% MLv=ML(mapfree(2:end),mapfree(2:end));

Fbv=F(mapfree(2:end),1);
tic
% ANALISI STATICA
D=Kbv\Fbv;
toc
Displ=zeros(size(K,1),1);
Displ(mapfree(2:end))=D;

Spost=zeros(nnodi,3);
Spost(:,1)=Displ(1:3:end);
Spost(:,2)=Displ(2:3:end);
Spost(:,3)=Displ(3:3:end);

Scale=Spost(:,1); % mappa per colorbar
% plot mesh deformata
PlotDefoMesh(nodi(:,2:4),elementi(:,2:9),100,Spost,Scale)



% ANALISI MODALE
[V,Dee]=eigs(K,Mass,30,'sm');
V=V./max(abs(V));
eval=abs(diag(Dee));
% eval=eval(7:end);
[eigval,ind]=sort(eval);
eigvect=abs(V(:,ind));
eigval=sqrt(eigval)/2/pi;




% ANALISI DINAMICA
k=1;
% m = [1,3,8,20,60,150,0]
mapfree(1)=[];

 for m = 0
    i=1;
    clear dofmaster
for j=1:length(vincoli)
    dofmaster(i:i+2,1)=[vincoli(j)*3-2,vincoli(j)*3-1,vincoli(j)*3];
    i=i+3;
end
cont1=i;
for j=1:m:length(nodi_est)
    if m==0 break
    else
    dofmaster(i:i+2,1)=[nodi_est(j)*3-2,nodi_est(j)*3-1,nodi_est(j)*3];
    i=i+3;
    end
end
% for j=469:1:504
% dofmaster(i:i+2,1)=[nodi(j)*3-2,nodi(j)*3-1,nodi(j)*3];
% i=i+3;
% end
% 
% for j=901:1:936
% dofmaster(i:i+2,1)=[nodi(j)*3-2,nodi(j)*3-1,nodi(j)*3];
% i=i+3;
% end
% for j=1369:1:1404
% dofmaster(i:i+2,1)=[nodi(j)*3-2,nodi(j)*3-1,nodi(j)*3];
% i=i+3;
% end
cont2=i;
con(k)=(cont2-cont1)/3;
for j=1:length(carichi)
    dofmaster(i:i+2,1)=[carichi(j)*3-2,carichi(j)*3-1,carichi(j)*3];
    i=i+3;
end

% masters=unique(dofmaster(dofmaster>length(vincoli)*3)-length(vincoli)*3);
masters=unique(dofmaster);
m=length(masters);
dof_MN=[masters(end-107:end)];
dof_CB=[masters(1:end-108)];
slaves=[1:length(mapdof)]';
d=[dof_CB;dof_MN];
s=slaves;
s(d)=[];

CB=length(dof_CB);
MN=length(dof_MN);
dofcontrolled=[];

n=20;
[Khcb,Mhcb,Thcb,phiN,psiC,K1,M1]=riduzioneCB(K,Mass,d,s,dofcontrolled,n);

Khcb=simmetrization(Khcb);
Mhcb=simmetrization(Mhcb);


% n=20;
% [Vm,Dem]=eigs(Kii,Mii,n);
% Vm=[zeros(length(Kbb),n);Vm];
% Psi=[eye(length(Kbb),length(Kbb));-Kii^(-1)*Kib];
% Thcb=[Psi Vm];

% Khcb=Thcb'*K1*Thcb;
% Mhcb=Thcb'*M1*Thcb;
% Khcb=simmetrization(Khcb);
% Mhcb=simmetrization(Mhcb);

% Kcon=zeros(size(K,1),size(K,2));
% for i=m/5+1:4*m/5
%     Kcon(masters(i),masters(i))=1;
% end
% d=dof_CB;
% s=slaves;
% Kbb=Kcon([d],[d]);
% Kii=Kcon([s],[s]);
% Kib=Kcon([s],[d]);
% Kbi=Kcon([d],[s]);
% Kcon=[Kbb Kbi; Kib Kii];
% [r,~]=find(Kcon~=0);
% dof=unique(r);
end


dof=[1:CB+MN]';
dofslaves=[1:length(mapdof)]';
dofslaves([dof])=[];
Nmodes=20;

[Kr,Mr,Tr,A]=riduzioneMacNeal(K1,M1,dofslaves,mapdof,Nmodes);

Kr=simmetrization(Kr);
Mr=simmetrization(Mr);


Ts=Tr;
Th=Thcb;
% T=[Ts Th];

% psiC(:,CB+1:end)=[];
% Ts(dof,:)=[];
% Ta=Ts(:,1:Nmodes);
% Ts(:,1:Nmodes+CB)=[];
% T=[eye(CB,CB) zeros(CB,MN+Nmodes+n); zeros(MN,CB) eye(MN,MN) zeros(MN,n+Nmodes);psiC Ts phiN Ta];
%      
psiC(:,CB+1:end)=[];
Ts(1:CB,:)=0;
Ta=Ts(:,1:Nmodes);
Ts(:,1:Nmodes+CB)=[];
T=[[eye(CB,CB);zeros(MN,CB);psiC],Ts,[zeros(MN+CB,n);phiN],Ta];

Kf=T'*K1*T;
Mf=T'*M1*T;
Kf=simmetrization(Kf);
Mf=simmetrization(Mf);


% Kv=Kf;
% Mv=Mf;
% Fbv=[Fbv(d);Fbv(s)];
% Fv=T'*Fbv;
% D=Kv\Fv;
% Displ1=zeros(length(Kv),1);
% Displ1(1:end)=D(1:end);
% Displ1=T*Displ1;
 
[~,ordine]=sort([d;s]);
% Displ2=Displ1(ordine);
% 
% Spost1=zeros(nnodi,3);
% Spost1(length(vincoli)+1:end,1)=Displ2(1:3:end);
% Spost1(length(vincoli)+1:end,2)=Displ2(2:3:end);
% Spost1(length(vincoli)+1:end,3)=Displ2(3:3:end);
% 
% Scale=Spost1(:,1); % mappa per colorbar
% % plot mesh deformata
% PlotDefoMesh(nodi(:,2:4),elementi(:,2:9),100,Spost1,Scale)

Kv=Kf;
Mv=Mf;

[V1,Dee1]=eigs(Kv,Mv,30,'sm');
% [V1,Dee1]=eig(full(Kv),full(Mv));
% [~,Dmax]=eigs(Kf,Mf,1,'lm');
% [~,Dmin]=eigs(Kf,Mf,1,'sm');
% d(k)=Dmax/Dmin;
V1=T*V1;
V1=V1(ordine,:);
V1=V1./max(abs(V1));
eval1=abs(diag(Dee1));
[eigval1,ind1]=sort(eval1);
eigvect1=abs(V1(:,ind1));
eigval1=sqrt(eigval1)/2/pi;


figure();
plot(eigvect(:,7));
figure();
plot(eigvect1(:,7));

i=1;
for j=1:3:size(eigvect,1)
    NodesPhi_l([j,j+1,j+2],1)=i;
    NodesPhi_l([j,j+1,j+2],2)=[1;2;3];
    i=i+1;
end
i=1;
for j=1:3:size(eigvect1,1)
    NodesPhi_r([j,j+1,j+2],1)=i;
    NodesPhi_r([j,j+1,j+2],2)=[1;2;3];
    i=i+1;
end
[MAC_data,EMA_FEA] = MacW2_mod(10,eigvect,NodesPhi_l,eigval,eigvect1,NodesPhi_r,eigval1,1);