
%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);
a=find(nodi_est<=vincoli(end));
b=find(nodi_est>=carichi(1));
nodi_est(b)=[];
nodi_est(a)=[];


% 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
% mapdof([carichi(1)*3:end])=0;

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)
c=colorbar;
c.Label.String='[mm]';



% ANALISI MODALE
[V,Dee]=eigs(Kbv,Mbv,23,'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;

% f=find(nodi(:,4)==60);
% c=find(nodi(f,3)==7.5);
% d=find(nodi(f(c),2)==7.5);
% carichi=nodi(f(c(d)));
% f=find(nodi(nodi_est,4)==60);
% c=[find(nodi(nodi_est(f),2)==0); find(nodi(nodi_est(f),2)==15)];
% d=[find(nodi(nodi_est(f(c)),3)==0); find(nodi(nodi_est(f(c)),3)==15)];
% carichi=[carichi;nodi(nodi_est(f(c(d))))];
% f=find(nodi(:,4)==0);
% c=find(nodi(f,3)==7.5);
% d=find(nodi(f(c),2)==7.5);
% vincoli=nodi(f(c(d)));
% f=find(nodi(nodi_est,4)==0);
% c=[find(nodi(nodi_est(f),2)==0); find(nodi(nodi_est(f),2)==15)];
% d=[find(nodi(nodi_est(f(c)),3)==0); find(nodi(nodi_est(f(c)),3)==15)];
% vincoli=[vincoli;nodi(nodi_est(f(c(d))))];
% carichi=[];
% 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: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
% 
% figure()
% PlotMesh(nodi(:,2:4),elementi(:,2:9),0)
% % hold on
% % plot3(nodi(1:36,2),nodi(1:36,3),nodi(1:36,4),'xr')
% hold on
% plot3(nodi(end-35:end,2),nodi(end-35:end,3),nodi(end-35:end,4),'xr')
% hold on
% plot3(nodi(901:936,2),nodi(901:936,3),nodi(901:936,4),'xr')

% caso vincolato
dofmaster=unique(dofmaster);
dofmaster=dofmaster(dofmaster>length(vincoli)*3)-length(vincoli)*3;
dofslaves=[1:length(mapfree)]';
dofslaves([dofmaster])=[];
% 
% % caso svincolato
% dofmaster=unique(dofmaster);
% % for j=length(nodi_est):-1:1
% % a=find(dofmaster==nodi_est(j)*3);
% % a=[a;a-1;a-2];
% % dofmaster(a)=[];
% % end
% dofslaves=[1:length(mapdof)]';
% dofslaves([dofmaster])=[];

% 
% Kbb=K([dofmaster],[dofmaster]);
% Kii=K([dofslaves],[dofslaves]);
% Kib=K([dofslaves],[dofmaster]);
% Kbi=K([dofmaster],[dofslaves]);
% 
% Mbb=Mass([dofmaster],[dofmaster]);
% Mii=Mass([dofslaves],[dofslaves]);
% Mib=Mass([dofslaves],[dofmaster]);
% Mbi=Mass([dofmaster],[dofslaves]);
% % 
% K1=[Kbb Kbi; Kib Kii];
% M1=[Mbb Mbi; Mib Mii];
% 
% K1=simmetrization(K1);
% M1=simmetrization(M1);
% 
% % % 
% % 
% % 
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;

% dofslaves(1:length(vincoli)*3)=[];
% dofslaves=dofslaves-length(vincoli)*3;
% dofmaster=dofmaster-length(vincoli)*3;
% 
% [Khcb,Mhcb,T,Kss]=GuyanReduction( K , Mass , dofslaves);
% 
% % CB_controlled
t=1;
l=1;
% for j=469:1:504
%      j=j-length(vincoli);
%     m=0;
% for i=1:length(dofmaster)
%     if 3*j>=dofmaster(i)
%         m=m+1;
%     else break
%     end
% end
% dofcontrolled(t:t+2,1)=[dofslaves(j*3-2-m),dofslaves(j*3-1-m),dofslaves(j*3-m)];
% d(l,:)=[j*3-2-m;j*3-1-m;j*3-m];
% l=l+1;
% t=t+3;
% end

% for j=901:936
%     j=j-length(vincoli);
%     m=0;
% for i=1:length(dofmaster)
%     if 3*j>=dofmaster(i)
%         m=m+1;
%     else break
%     end
% end
% dofcontrolled(t:t+2,1)=[dofslaves(j*3-2-m),dofslaves(j*3-1-m),dofslaves(j*3-m)];
% d(l,:)=[j*3-2-m;j*3-1-m;j*3-m];
% l=l+1;
% t=t+3;
% end

% for j=1369:1:1404
%     j=j-length(vincoli);
%     m=0;
% for i=1:length(dofmaster)
%     if 3*j>=dofmaster(i)
%         m=m+1;
%     else break
%     end
% end
% dofcontrolled(t:t+2,1)=[dofslaves(j*3-2-m),dofslaves(j*3-1-m),dofslaves(j*3-m)];
% d(l,:)=[j*3-2-m;j*3-1-m;j*3-m];
% l=l+1;
% t=t+3;
% end
% 
% dofslaves1=dofslaves;
% dofslaves1(d)=[];
% dofslaves=[dofcontrolled;dofslaves1];
% CB controlled
% 
% % CB normale
% dofcontrolled=[];
% % CB normale
dofcontrolled=[];

[Khcb,Mhcb,Thcb,phiN,psiC,K1,M1]=riduzioneCB(Kbv,Mbv,dofmaster,dofslaves,dofcontrolled,n);

Khcb=simmetrization(Khcb);
Mhcb=simmetrization(Mhcb);
Kv=Khcb;
Mv=Mhcb;
T=Thcb;



% [Tc1,Tmc1,Kv,Mv]=riduzioneCB_controlled(Khcb,Mhcb,dofslaves,dofmaster,dofcontrolled,dofslaves2,psiC,phiN,n);

% Kv=simmetrization(Kv);
% Mv=simmetrization(Mv);

% pos=1:cont1-1;
% Kv([pos],:)=[];
% Kv(:,[pos])=[];
% % Kv([cont2+1:end],:)=[];
% % Kv(:,[cont2+1:end])=[];

% Mv([pos],:)=[];
% Mv(:,[pos])=[];
% Mv([cont2+1:end],:)=[];
% Mv(:,[cont2+1:end])=[];
% Fv=zeros(length(Kv),1);
% Fv(end-length(carichi)*3-n+1:3:end-n)=Fmodulus;
% Fbbv=[Fbv(dofmaster);Fbv(dofslaves)];
% Fv=T'*Fbbv;
% % 
% D=Kv\Fv;
% D1=T*D;
% % Dc=Tc1*D;
% % Spostc=zeros(length(dofcontrolled)/3,3);
% % Spostc(:,1)=Dc(1:3:end);
% % Spostc(:,2)=Dc(2:3:end);
% % Spostc(:,3)=Dc(3:3:end);
% 
% 
% 
[~,ordine]=sort([dofmaster;dofslaves]);
% 
% 
% Displ1=D1(ordine);
% Displ2=[zeros(length(vincoli)*3,1);Displ1];
% 
% 
% 
% 
% Spost1=zeros(nnodi,3);
% Spost1(:,1)=Displ2(1:3:end);
% Spost1(:,2)=Displ2(2:3:end);
% Spost1(:,3)=Displ2(3:3:end);
% 
% Scale=Spost1(:,1); % mappa per colorbar
% % plot mesh deformata
% PlotDefoMesh(nodi(:,2:4),elementi(:,2:9),100,Spost1,Scale)


% Nmodes=20;
% [Kf,Mf,Tr,A]=riduzioneMacNeal(Kbv,Mbv,dofslaves,mapfree,Nmodes);
% Kf=simmetrization(Kf);
% Mf=simmetrization(Mf);
% Kv=Kf;
% Mv=Mf;
% T=Tr;
% 
% Fv=Tr'*Fbv;
% 
% 
% D=Kv\Fv;
% Displ1=zeros(length(Kf),1);
% Displ1=D;
% Displ1=Tr*Displ1;
% 
% 
% Displ2=[zeros(length(vincoli)*3,1);Displ1];
% % Displ2=Displ1;
% Spost1=zeros(nnodi,3);
% Spost1(:,1)=Displ2(1:3:end);
% Spost1(:,2)=Displ2(2:3:end);
% Spost1(:,3)=Displ2(3:3:end);
% % 
% Scale=Spost1(:,1); % mappa per colorbar
% % plot mesh deformata
% PlotDefoMesh(nodi(:,2:4),elementi(:,2:9),100,Spost1,Scale)

[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;
eigvalf(:,k)=eigval1;
% dif(:,k)=(eigval1-eigval)./eigval*100;
% d(:,:,k)=abs((V1-V)./V)*100;
%  figure();
%  bar(dif(7:end,k));
%  legend(num2str(con(k)));

% s(k,1)=max(abs(Displ2-Displ));
% s(k,2)=min(abs(Displ2-Displ));
 k=k+1;

end
% 
% diff(1,:)=max(dif);
% diff(2,:)=min(dif);
% figure();
% plot(d(:,7,1));
% figure();
% bar(V(:,7));
% hold on
% bar(V1(:,7));
% con=[15:5:30]';
figure();
plot(eigvect(:,7));
title('Modello intero');
xlabel('elementi autovettore');
ylabel('valore normalizzato');
figure();
plot(eigvect1(:,7));
title('Modello ridotto');
xlabel('elementi autovettore');
ylabel('valore normalizzato');
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);