%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,ML,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)==60);
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);


% 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)
%%

% CALCOLO STRESS
% [StressC,StrainC,StressN,StressG,SL]=ComputeStressesE(Displ,Te,BK,SG,map,E,NU,nodi,elemconv);


% 
% %mappa degli stress
% StressScale=zeros(nnodi,6);
% % StressScale conterr� gli stress nodali mediati per il NUmero di elementi
% % di cui il nodo fa parte. Usato solo per visualizzazione
% for cc=1:size(nodi,1)
%     [row,col]=find(elemconv==nodi(cc));
%     NUmN=size(row,1);
%     for nn=1:NUmN
%         StressScale(cc,1:6)=StressScale(cc,1:6)+StressN(6*(col(nn)-1)+[1:6],row(nn))'/NUmN; %media
%     end
% end

D = (E/((1+NU)*(1-2*NU)))*[1-NU NU NU 0 0 0 ;
    NU 1-NU NU 0 0 0 ;
    NU NU 1- NU 0 0 0 ;
    0 0 0 (1-2*NU)/2 0 0 ;
    0 0 0 0 (1- 2*NU)/2 0 ;
    0 0 0 0 0 (1- 2*NU)/2]; %Material matrix
[L]=Lmatrix; % Transformation matrix from Gauss points to nodes
% Stress recovery for all elements
Strain_Gauss=zeros(6,8,length(elementi));
Stress_Gauss=zeros(6,8,length(elementi));
Strain_node=zeros(6,8,length(elementi));
Stress_node=zeros(6,8,length(elementi));
for count=1:length(elementi)
    dofi=map(count,:);
    xstat=Displ(dofi,1);
    [Strain_Gauss(:,:,count), Stress_Gauss(:,:,count), Strain_node(:,:,count),...
        Stress_node(:,:,count)] = Elem_Stress(BG(:,:,:,count),D,L,xstat);
end

SNode=zeros(length(nodi),6);
SigmaVM=zeros(length(nodi),1);
% Stress averaging for every node (brutal version, must be improved)
for count=1:length(nodi)
       ni=nodi(count,1); % node Number
       [posx,posy]=find(ni==elementi(:,2:9));
       for lun=1:length(posx)
        SNode(count,:)=SNode(count,:)+Stress_node(:,posy(lun),posx(lun))';
       end
       SNode(count,:)=SNode(count,:)/length(posx);
       SigmaVM(count)=sqrt(0.5*((SNode(count,1)-SNode(count,2))^2+...
          (SNode(count,2)-SNode(count,3))^2+ ...
          (SNode(count,3)-SNode(count,1))^2) ...
           + 3*(SNode(count,4)^2+SNode(count,5)^2+SNode(count,6)^2));
end


% plot mesh deformata con stress (colobar stress medio)
PlotDefoMesh(nodi(:,2:4),elementi(:,2:9),100,Spost,SigmaVM)



% 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;

% 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
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
% 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);
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]);
% 
% 
% 
n=20;
% [Vm,Dem]=eigs(Kii,Mii,n);
% Vm=[zeros(length(Kbb),n);Vm];
% % Psi=[eye(length(Kbb),length(Kbb));-Kii^(-1)*Kib];
% Thcb=[ Vm];
% K=[Kbb Kbi; Kib Kii];
% M=[Mbb Mbi; Mib Mii];
% Khcb=Thcb'*K*Thcb;
% Mhcb=Thcb'*M*Thcb;
% dofslaves(1:length(vincoli)*3)=[];
% dofslaves=dofslaves-length(vincoli)*3;
% dofmaster=dofmaster-length(vincoli)*3;



[Khcb,Mhcb,Thcb,phiN,psiC]=riduzioneCB(K,Mass,dofmaster,dofslaves,n);
Khcb=simmetrization(Khcb);
Mhcb=simmetrization(Mhcb);
% 
% 
% 
% pos=1:cont1-1;
Kv=Khcb;
% Kv([pos],:)=[];
% Kv(:,[pos])=[];
Mv=Mhcb;
% Mv([pos],:)=[];
% Mv(:,[pos])=[];
% Fv=zeros(length(Kv),1);
% Fv(end-length(carichi)*3-n+1:3:end-n)=Fmodulus;
%  
% 
% D=Kv\Fv;
% Displ1=zeros(length(Khcb),1);
% Displ1(cont1:end)=D(1:end);
% Displ1=Thcb*Displ1;
% 
% [~,ordine]=sort([dofmaster;dofslaves]);
% Displ2=Displ1(ordine);




% 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);
% Fv=T1'*Fbv;


% D=Kf\Fv;
% Displ1=zeros(length(Kf),1);
% Displ1=D;
% Displ1=T1*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');
% [~,Dmax]=eigs(Kf,Mf,1,'lm');
% [~,Dmin]=eigs(Kf,Mf,1,'sm');
% d(k)=Dmax/Dmin;
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;
% V1=Tr*V1;
% V1=V1./max(abs(V1));
% d(:,:,k)=abs((V1-V)./V)*100;
 figure();
 bar(dif(7:end,k));
 legend(num2str(con(k)));
 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));