 clear all, clc,
close all
load('Kahraman_appdata.mat')% data is just data=app.data;
% load('Data_test_nuovo3.mat')
r=data.optimicro(end).results;
inp = data.coupling.input;
g1=data.coupling.gear1;
g2=data.coupling.gear2;
numpos = data.optimicro(1).params.NumPosizioni;
g1.FEM.nodes_original=g1.FEM.nodes;
g2.FEM.nodes_original=g2.FEM.nodes;

% plot3(g1.rigid_contact.x_3D(r(1).gearpair.CNall{1,1}{1,2}(:,2),r(1).gearpair.CNall{1,1}{1,2}(:,1)),...
%     g1.rigid_contact.y_3D(r(1).gearpair.CNall{1,1}{1,2}(:,2),r(1).gearpair.CNall{1,1}{1,2}(:,1)),...
%     g1.rigid_contact.z_3D(r(1).gearpair.CNall{1,1}{1,2}(:,2),r(1).gearpair.CNall{1,1}{1,2}(:,1)),'og');

%% SELECTION OF THE NODES FOR CONTACT STIFFNESS AT EACH ANGULAR POSITION AND TOOTH
[MasterC1,masterC_list1,weightC1,ia1] = SelectContact(inp,g1,r,numpos);
[MasterC2,masterC_list2,weightC2,ia2] = SelectContact(inp,g2,r,numpos);

%% SELECTION OF THE SLAVE NODES ON THE HUB

[th1,rr1]=cart2pol(g1.FEM.surfnodes(:,2)-g1.params.origin(1),g1.FEM.surfnodes(:,3)-g1.params.origin(2));
[slavehub1]=find(rr1<g1.params.r_shaft_e*1.01);



[th2,rr2]=cart2pol(g2.FEM.surfnodes(:,2)-g2.params.origin(1),g2.FEM.surfnodes(:,3)-g2.params.origin(2));
[slavehub2]=find(rr2<g2.params.r_shaft_e*1.01);



% Plot di controllo selezione nodi
figure, hold on;
plot3(g1.FEM.nodes(:,2),g1.FEM.nodes(:,3),g1.FEM.nodes(:,4),'.k');
% plot3(g2.FEM.nodes(:,2),g2.FEM.nodes(:,3),g2.FEM.nodes(:,4),'.r');
plot3(g1.FEM.surfnodes(slavehub1,2),g1.FEM.surfnodes(slavehub1,3),g1.FEM.surfnodes(slavehub1,4),'xr');
xlabel('X [mm]');
ylabel('Y [mm]');
zlabel('Z [mm]');
plot3(g1.params.origin(1),g1.params.origin(2),g1.params.b/2,'xr');
xlabel('X [mm]');
ylabel('Y [mm]');
zlabel('Z [mm]');
% for j=1:100:length(slavehub1)
%     x=[g1.FEM.surfnodes(slavehub1(j),2);g1.params.origin(1)];
%     y=[g1.FEM.surfnodes(slavehub1(j),3);g1.params.origin(2)];
%     z=[g1.FEM.surfnodes(slavehub1(j),4);g1.params.b/2];
%     plot3(x,y,z);
% end

% plot3(g2.FEM.surfnodes(slavehub2,2),g2.FEM.surfnodes(slavehub2,3),g2.FEM.surfnodes(slavehub2,4),'xk');
axis equal,


%% FEM ASSEMBLY
E=2e+5;
nu=0.3;
rho=7.8e-9;
% Pinion ------------------------------------------------------------------
% Assemble FE matrices
[K1,M1,Ml1,G1,C1,vtot1,~,~,~,B1,~,map1,elem1]=poliFEMo_sparse(g1.FEM.nodes,g1.FEM.elems,E,nu,rho,0,1);
% find and store master and slave dofs for the hub RJ
masterRJ1=[length(K1)+1:length(K1)+6]';
slavet1=g1.FEM.surfnodes(slavehub1,1);
masterd1=g1.FEM.nodes(:,1);
masterd1(slavet1)=[];
slaveschar1=[slavet1,(slavet1-1)*3+1,(slavet1-1)*3+2,(slavet1-1)*3+3];
slavedofs1=reshape(slaveschar1(:,2:4)',[],1);
allmasters1=[1:length(K1)+6]';
allmasters1(slavedofs1)=[];

%simmetrization
M1=simmetrization(M1);
K1=simmetrization(K1);


% dofs transformation matrix
Tdofs1=speye(length(g1.FEM.nodes)+1);
Tdofs1(:,slaveschar1(:,1))=[];
Nlist1=Tdofs1'*[g1.FEM.nodes(:,1);
                length(g1.FEM.nodes)+1];


% Gear --------------------------------------------------------------------
% Assemble FE matrices
[K2,M2,Ml2,G2,C2,vtot2,~,~,~,B2,~,map2,elem2]=poliFEMo_sparse(g2.FEM.nodes,g2.FEM.elems,E,nu,rho,0,1);
% find and store master and slave dofs for the hub RJ
masterRJ2=[length(K2)+1:length(K2)+6]';
slavet2=g2.FEM.surfnodes(slavehub2,1);
masterd2=g2.FEM.nodes(:,1);
masterd2(slavet2)=[];
slaveschar2=[slavet2,(slavet2-1)*3+1,(slavet2-1)*3+2,(slavet2-1)*3+3];
slavedofs2=reshape(slaveschar2(:,2:4)',[],1);
allmasters2=[1:length(K2)+6]';
allmasters2(slavedofs2)=[];

%simmetrization
M2=simmetrization(M2);
K2=simmetrization(K2);

% dofs transformation matrix
Tdofs2=speye(length(g2.FEM.nodes)+1);
Tdofs2(:,slaveschar2(:,1))=[];
Nlist2=Tdofs2'*[g2.FEM.nodes(:,1);
                length(g2.FEM.nodes)+1];

% Base fem assembly completed, next the rotations for each angular position are computed            
            
%% CALCULATION OF THE ROTATION MATRICES FOR PINION AND GEAR
iii=1;
[K1r,M1r,TRJ1,g1]= StepAssembly(g1,K1,M1,allmasters1,slaveschar1,data,iii);
K1r=simmetrization(K1r);
M1r=simmetrization(M1r);
g2.FEM.nodes(:,3)=g2.FEM.nodes(:,3)-g2.params.origin(2);
[K2r,M2r,TRJ2,g2]= StepAssembly(g2,K2,M2,allmasters2,slaveschar2,data,iii);
K2r=simmetrization(K2r);
M2r=simmetrization(M2r);
% Application of the TRJs to the matrices of the pinion and
% gear
Kt1=TRJ1'*blkdiag(K1r,zeros(6))*TRJ1;
Mt1=TRJ1'*blkdiag(M1r,zeros(6))*TRJ1;
f1=zeros(length(Kt1),1);
f1(end)=150000; % end is valid only because i added the virtual node at the bottom
constdofs1=[length(Kt1)-5:length(Kt1)-1]';
shiftk1=length(Kt1);

Kt1=simmetrization(Kt1);
Mt1=simmetrization(Mt1);

Kt2=TRJ2'*blkdiag(K2r,zeros(6))*TRJ2;
Mt2=TRJ2'*blkdiag(M2r,zeros(6))*TRJ2;
f2=zeros(length(Kt2),1);
constdofs2=[length(Kt2)-5:length(Kt2)]';
Kt2=simmetrization(Kt2);
Mt2=simmetrization(Mt2);
           
%% ASSEMBLY OF THE SYSTEM AND ADDITION OF THE CONTACT STIFFNESS 
Ks=blkdiag(Kt1,Kt2);
Ms=blkdiag(Mt1,Mt2);
F=[f1;f2];
clear sdof1
clear sdof2
% Kcont=spalloc(length(Ks),length(Ks),1000);
Kcon=spalloc(length(K1)+length(K2)+12,length(K1)+length(K2)+12,1000);
Kcona=spalloc(length(K1)+length(K2)+12,length(K1)+length(K2)+12,1000);
kcont=[NaN 5e+8 10e+8 15e+8]; %this will be an input for each tooth, each position, each torque
dof1joint=[slavet1*3-2;slavet1*3-1;slavet1*3];
dof1joint=unique(dof1joint);
dof2joint=[slavet2*3-2;slavet2*3-1;slavet2*3];
dof2joint=unique(dof2joint);

l=1;
for tk=2:4 %loop for all teeth in contact
    
    % make it so that the total weight is 1, each value will be multiplied
    % for the contact stiffness scalar value at each angular position for
    % each tooth
    % use only weight of pinion
    weightk1{iii,tk}=weightC1{iii,tk}(ia1{iii,tk})/sum(sum(weightC1{iii,tk}(ia1{iii,tk})));
%     weightk2{iii,tk}=weightC2{iii,tk}(ia2{tk})/sum(sum(weightC2{iii,tk}(ia2{tk})));
    
    nel1=g1.FEM.surfnodes(masterC_list1{iii,tk},1);
    nel2=g2.FEM.surfnodes(masterC_list2{iii,tk},1);
    
    for ni=1:length(nel1)

    kel=kcont(tk)*weightk1{iii,tk}(ni)*[cosd(20) 0 0;
                                        0 sind(20) 0;
                                        0 0 0];
    kc=[kel,-kel;
        -kel,kel];
%     idn1=find(nel1(ni)==Nlist1);
%     idn2=find(nel2(ni)==Nlist2);
    % use the original numbering and dof of the connection nodes, then
    % reduce the final matrix using TRJ
    idn1=nel1(ni); %
    idn2=nel2(ni); 
    dof1=[(idn1-1)*3+1:(idn1-1)*3+3]';
    dof2=[(idn2-1)*3+1:(idn2-1)*3+3]'+length(K1)+6;
    Kcon([dof1;dof2],[dof1;dof2])=Kcon([dof1;dof2],[dof1;dof2])+kc;
    Kcona([dof1;dof2],[dof1;dof2])=Kcona([dof1;dof2],[dof1;dof2])+eye(6,6);
    nodicont1(l,1)=idn1;
    nodicont2(l,1)=idn2;
    l=l+1;
    
    end
end


Kconta=blkdiag(TRJ1',TRJ2')*Kcona*blkdiag(TRJ1,TRJ2);
Kcont=blkdiag(TRJ1',TRJ2')*Kcon*blkdiag(TRJ1,TRJ2);

% % trovo i dof di contatto per una sola posizione angolare
% Kconta1=Kconta(1:length(Kt1),1:length(Kt1));
% Kconta2=Kconta(length(Kt1)+1:end,length(Kt1)+1:end);
% [a,~]=find(Kconta1~=0);
% [b,~]=find(Kconta2~=0);
% t=1;
% for i=3:3:length(dofmaster1cont)
% nodi1cont(t)=[dofmaster1cont(i)/3];
% t=t+1;
% end
% t=1;
% for i=3:3:length(dofmaster2cont)
% nodi2cont(t)=[dofmaster2cont(i)/3];
% t=t+1;
% end
% nodi1cont=nodi1cont';
% nodi2cont=nodi2cont';

% % trovo i dof e i nodi di contatto per tutte le posizioni angolari
% dof1=[g1.FEM.surfnodes(MasterC1)*3-2;g1.FEM.surfnodes(MasterC1)*3-1;g1.FEM.surfnodes(MasterC1)*3];
% dof1=unique(dof1);
% dof2=[g2.FEM.surfnodes(MasterC2)*3-2;g2.FEM.surfnodes(MasterC2)*3-1;g2.FEM.surfnodes(MasterC2)*3];
% dof2=unique(dof2);
% Kcons1=spalloc(length(K1)+6,length(K1)+6,1000);
% Kcons1([dof1],[dof1])=eye(length(dof1));
% Kconst1=TRJ1'*Kcons1*TRJ1;
% [dofmaster1cont,~]=find(Kconst1~=0);
% Kcons2=spalloc(length(K2)+6,length(K2)+6,1000);
% Kcons2([dof2],[dof2])=eye(length(dof2));
% Kconst2=TRJ2'*Kcons2*TRJ2;
% [dofmaster2cont,~]=find(Kconst2~=0);
% t=1;
% for i=3:3:length(dofmaster1cont)
% nodi1cont(t)=[dofmaster1cont(i)/3];
% t=t+1;
% end
% t=1;
% for i=3:3:length(dofmaster2cont)
% nodi2cont(t)=[dofmaster2cont(i)/3];
% t=t+1;
% end
% nodi1cont=nodi1cont';
% nodi2cont=nodi2cont';

% trovo i dof e i nodi di contatto per tutte le posizioni angolari di tutti i denti
% for i=1:g1.params.z
%     dof1=[g1.FEM.surfnodes(MasterC1)*3-2;g1.FEM.surfnodes(MasterC1)*3-1;g1.FEM.surfnodes(MasterC1)*3];
%     dof1=unique(dof1);
%     Kcons1=spalloc(length(K1)+6,length(K1)+6,1000);
%     Kcons1([dof1],[dof1])=eye(length(dof1));
% Kconst1=TRJ1'*Kcons1*TRJ1;
% [dofmaster1cont(:,i),~]=find(Kconst1~=0);
%     [nodi1cont(:,i)]=g1.FEM.surfnodes(MasterC1);
%       dof2=[g2.FEM.surfnodes(MasterC2)*3-2;g2.FEM.surfnodes(MasterC2)*3-1;g2.FEM.surfnodes(MasterC2)*3];
%     dof2=unique(dof2);
%     Kcons2=spalloc(length(K2)+6,length(K2)+6,1000);
%     Kcons2([dof2],[dof2])=eye(length(dof2));
% Kconst2=TRJ2'*Kcons2*TRJ2;
% [dofmaster2cont(:,i),~]=find(Kconst2~=0);
%     [nodi2cont(:,i)]=g2.FEM.surfnodes(MasterC2);
% 
% thetap=2*pi/g1.params.z; % pinion step angle
% rotap=sparse([cos(thetap) sin(thetap) 0; % positive, counterclockwise
%         -sin(thetap) cos(thetap) 0;
%         0 0 1]); % pinion rotation around Z axis
%     
%     tempcell = repmat({rotap}, 1, size(g1.FEM.surfnodes,1));
%     rotp= blkdiag(tempcell{:});
%     clear tempcell
%     % For loop on each angular position on the mesh cycle data.optimicro(1).params.NumPosizioni
% %     % Rotate nodes, pinion
%     pnodes1=g1.FEM.surfnodes(:,2:4);
%     pnodesv1=reshape(pnodes1',[],1);
%     g1.FEM.surfnodes(:,2:4)=reshape(rotp*pnodesv1,3,[])';
%         [MasterC1,~,~,~] = SelectContact(inp,g1,r,numpos);
% 
%     thetap=2*pi/g1.params.z; % pinion step angle
% rotap=sparse([cos(thetap) sin(thetap) 0; % positive, counterclockwise
%         -sin(thetap) cos(thetap) 0;
%         0 0 1]); % pinion rotation around Z axis
%     
%     tempcell = repmat({rotap}, 1, size(g1.FEM.surfnodes,1));
%     rotp= blkdiag(tempcell{:});
%     clear tempcell
%     g2.FEM.surfnodes(:,3)=g2.FEM.surfnodes(:,3)-g2.params.origin(2);
%        pnodes2=g2.FEM.surfnodes(:,2:4);
%     pnodesv2=reshape(pnodes2',[],1);
%     g2.FEM.surfnodes(:,2:4)=reshape(rotp*pnodesv2,3,[])';
%        g2.FEM.surfnodes(:,3)=g2.FEM.surfnodes(:,3)+g2.params.origin(2);
%     [MasterC2,~,~,~] = SelectContact(inp,g2,r,numpos);
%     
% 
% end





% Test on single gear to debug
% ktt1=Kt1;
% ktt1(constdofs1,:)=[];
% ktt1(:,constdofs1)=[];
% f1c=f1;
% f1c(constdofs1)=[];
% % STATIC SOLUTION
% displc=ktt1\f1c;
% % expand and rearrange all the displacements
% displall=zeros(length(Kt1),1);
% unconstr=1:length(Kt1);
% unconstr(constdofs1)=[];
% displall(unconstr)=displc;
% 
% % pinion displ
% displ1rj=displall(1:length(Kt1));
% displ1=TRJ1*displ1rj;
% displ13=reshape(displ1(1:end-6)',3,[])';
% scaleZ=sqrt(abs(displ13(:,1).^2+displ13(:,2).^2));
% 
% PlotDefoMesh_mod(g1.FEM.nodes(:,2:4),g1.FEM.elems(:,2:9),4/max(abs(scaleZ)),...
%     displ13,scaleZ,...
%     g1.FEM.surffaces,g1.FEM.elfa)
% % % end single test

% Apply constraints
Ksc=Ks;
Ksc([constdofs1;constdofs2+shiftk1],:)=[];
Ksc(:,[constdofs1;constdofs2+shiftk1])=[];
Msc=Ms;
Msc([constdofs1;constdofs2+shiftk1],:)=[];
Msc(:,[constdofs1;constdofs2+shiftk1])=[];
Kcontc=Kcont;
Kcontc([constdofs1;constdofs2+shiftk1],:)=[];
Kcontc(:,[constdofs1;constdofs2+shiftk1])=[];
Fc=F;
Fc([constdofs1;constdofs2+shiftk1],:)=[];

% STATIC SOLUTION
displc=(Ksc+Kcontc)\Fc;

% expand and rearrange all the displacements
displall=zeros(length(Ks),1);
unconstr=1:length(Ks);
unconstr([constdofs1;constdofs2+shiftk1])=[];
displall(unconstr)=displc;

% pinion displ
displ1rj=displall(1:length(Kt1));
displ1=TRJ1*displ1rj;
displ13=reshape(displ1(1:end-6)',3,[])';



% gear displ
displ2rj=displall(length(Kt2)+1:end);
displ2=TRJ2*displ2rj;
displ23=reshape(displ2(1:end-6)',3,[])';

displtot=[displ1;displ2];
[scaleZ1,teta1,scaleZ2,teta2]=DisplRadCirc(g1,displ13,g2,displ23);


%% Eigenproblem solution



Ksc1=Ksc(1:(end+1)/2,1:(end+1)/2);
Ksc2=Ksc((end+1)/2+1:end,(end+1)/2+1:end);
Msc1=Msc(1:(end+1)/2,1:(end+1)/2);
Msc2=Msc((end+1)/2+1:end,(end+1)/2+1:end);



[V1,Dee1]=eigs(Ksc+Kcontc,Msc,30,'sm');
% V1=V1./max(abs(V1));
eval1=abs(diag(Dee1));
[eigval1,ind1]=sort(eval1);
eigvect1=abs(V1(:,ind1));
eigval1=sqrt(eigval1)/2/pi;

% % selezione nodi master con MoGeSec
Ksc11=Ksc1;
Ksc11(end,:)=[];
Ksc11(:,end)=[];
Msc11=Msc1;
Msc11(end,:)=[];
Msc11(:,end)=[];
Kcontcm=Kcontc;
Kcontcm(constdofs1(1),:)=[];
Kcontcm(:,constdofs1(1))=[];

Ksca=blkdiag(Ksc11,Ksc2)+Kcontcm;
Msca=blkdiag(Msc11,Msc2)+Kcontcm;

[va,da]=eigs(Ksca,Msca,90,'sm');
[eiga,sorta]=sort(diag(da)/2/pi);

% [v1,d1]=eigs(Ksc11,Msc11,y,'sm');
% [eig1,sort1]=sort(diag(d1)/2/pi);
% [v2,d2]=eigs(Ksc2,Msc2,y,'sm');
% [eig2,sort2]=sort(diag(d2)/2/pi);
k1=2;
k2=1;

% i=1;
% % for j=1:3:length(K1)
% %     NodiFi1([j,j+1,j+2],1)=i;
% %     NodiFi1([j,j+1,j+2],2)=[1;2;3];
% %     i=i+1;
% % end
% i=1;
% for j=1:3:length(Ksc11)
%     NodiFi1([j,j+1,j+2],1)=i;
%     NodiFi1([j,j+1,j+2],2)=[1;2;3];
%     i=i+1;
% end
i=1;
for j=1:3:length(Ksca)
    NodiFia([j,j+1,j+2],1)=i;
    NodiFia([j,j+1,j+2],2)=[1;2;3];
    i=i+1;
end


% clear NodiFibase1
% clear NodiFibase2
% 
% NodiFibase1(:,1)=[(nodi1cont);(nodi1cont);(nodi1cont)];
% NodiFibase1=sort(NodiFibase1);
% NodiFibase2(:,1)=[(nodi2cont);(nodi2cont);(nodi2cont)];
% NodiFibase2=sort(NodiFibase2);
% % 
% for j=1:3:length(NodiFibase1)
%     NodiFibase1([j,j+1,j+2],2)=[1;2;3];
% end
% % 
% for j=1:3:length(NodiFibase2)
%     NodiFibase2([j,j+1,j+2],2)=[1;2;3];
% end
% % 
NodiFibase=[];
g1.FEM.nodes_joint=g1.FEM.nodes;
g2.FEM.nodes_joint=g2.FEM.nodes;
g1.FEM.nodes_joint(slavet1,:)=[];
g2.FEM.nodes_joint(slavet2,:)=[];
g1.FEM.nodes_joint(:,1)=[1:length(masterd1)]';
% g2.FEM.nodes_joint(:,1)=[1:length(masterd2)]';
g2.FEM.nodes_joint(:,1)=[length(masterd1)+1:length(masterd1)+length(masterd2)]';
ga=[g1.FEM.nodes_joint;g2.FEM.nodes_joint];

[NodiFibase]=NodiSelez_k1k2(eiga,va,NodiFia,NodiFibase,ga,k1,k2);
Nodia=NodiFibase(1:3:length(NodiFibase),1);
sdofa=[Nodia*3-2;Nodia*3-1;Nodia*3];
sdofa=unique(sdofa);
dofmasteradd=sdofa;
% [NodiFibase1]=NodiSelez_k1k2(eig1,v1,NodiFi1,NodiFibase1,g1.FEM.nodes_joint,k1,k2);
% Nodi1=NodiFibase1(1:3:length(NodiFibase1),1);
% sdof1=[Nodi1*3-2;Nodi1*3-1;Nodi1*3];
% sdof1=unique(sdof1);
% [NodiFibase2]=NodiSelez_k1k2(eig2,v2,NodiFi1,NodiFibase2,g2.FEM.nodes_joint,k1,k2);
% Nodi2=NodiFibase2(1:3:length(NodiFibase2),1);
% sdof2=[Nodi2*3-2;Nodi2*3-1;Nodi2*3];
% sdof2=unique(sdof2);
% % Kcona1=spalloc(length(K1)+6,length(K1)+6,1000);
% % Kcona2=spalloc(length(K2)+6,length(K2)+6,1000);
% % Kcona1(sdof1,sdof1)=Kcona1(sdof1,sdof1)+eye(length(sdof1),length(sdof1));
% % Kcona2(sdof2,sdof2)=Kcona2(sdof2,sdof2)+eye(length(sdof2),length(sdof2));
% % Kcona1=TRJ1'*Kcona1*TRJ1;
% % Kcona2=TRJ2'*Kcona2*TRJ2;
% % [dofmaster1add,~]=find(Kcona1~=0);
% % [dofmaster2add,~]=find(Kcona2~=0);
% dofmaster1add=sdof1;
% dofmaster2add=sdof2;
% 
% figure, hold on;
% plot3(ga(:,2),ga(:,3),ga(:,4),'.k');
% axis equal,
% plot3(ga(Nodia,2),ga(Nodia,3),ga(Nodia,4),'xr')
% plot3(0,0,0,'xr');
% xlabel('X [mm]');
% ylabel('Y [mm]');
% zlabel('Z [mm]');

% figure, hold on;
% plot3(g1.FEM.nodes(:,2),g1.FEM.nodes(:,3),g1.FEM.nodes(:,4),'.k');
% % plot3(g2.FEM.nodes(:,2),g2.FEM.nodes(:,3),g2.FEM.nodes(:,4),'.r');
% axis equal,
% % plot3(g1.FEM.surfnodes(slavehub1,2),g1.FEM.surfnodes(slavehub1,3),g1.FEM.surfnodes(slavehub1,4),'xr');
% axis equal,
% plot3(g1.FEM.nodes(Nodi1,2),g1.FEM.nodes(Nodi1,3),g1.FEM.nodes(Nodi1,4),'xr')
% xlabel('X [mm]');
% ylabel('Y [mm]');
% zlabel('Z [mm]');

% Nodi1=nodi1cont;
% 
% 
% figure, hold on;
% plot3(g1.FEM.nodes_joint(:,2),g1.FEM.nodes_joint(:,3),g1.FEM.nodes_joint(:,4),'.k');
% plot3(g2.FEM.nodes_joint(:,2),g2.FEM.nodes_joint(:,3),g2.FEM.nodes_joint(:,4),'.k');
% axis equal,
% plot3(g2.FEM.nodes_joint(Nodi2,2),g2.FEM.nodes_joint(Nodi2,3),g2.FEM.nodes_joint(Nodi2,4),'xr');
% axis equal,
% plot3(g1.FEM.nodes_joint(Nodi1,2),g1.FEM.nodes_joint(Nodi1,3),g1.FEM.nodes_joint(Nodi1,4),'xr')
% plot3(0,0,0,'xr');
% xlabel('X [mm]');
% ylabel('Y [mm]');
% zlabel('Z [mm]');
% 



%% Riduzione matrici
% 
% dofmaster1add=a;
% dofmaster2add=b;
% 
% dofmaster1=[dofmaster1add;constdofs1(1)];
% dofmaster1=unique(dofmaster1);
% dofmaster2=[dofmaster2add];
% dofmaster2=unique(dofmaster2);
% 
% 
% dofslaves1=[1:length(Ksc1)]';
% dofslaves1(dofmaster1(1:end))=[];
% dofslaves2=[1:length(Ksc2)]';
% dofslaves2(dofmaster2)=[];
% dofcontrolled1=[];
% dofcontrolled2=[];
% t=1;
% for i=1:30:length(dof1joint)
% dofmaster1([t;t+1;t+2],1)=[dof1joint(i);dof1joint(i+1);dof1joint(i+2)];
% t=t+3;
% end
% dofslaves1=[1:length(K1)]';
% dofslaves1(dofmaster1)=[];

dofmaster=[dofmasteradd];
for i=1:length(dofmaster)
    if dofmaster(i)>=constdofs1(1)
        dofmaster(i)=dofmaster(i)+1;
    end
end
dofmaster=[dofmaster;constdofs1(1)];
dofmaster=unique(dofmaster);
dofslaves=[1:length(Ksc)]';
dofslaves(dofmaster)=[];
dofcontrolled=[];

% caso svincolato
% dofmaster1=[dofmaster1add;constdofs2];
% dofslaves1=(1:length(Kt1))';
% dofslaves1(dofmaster1)=[];
% dofmaster2=[dofmaster2;constdofs2];
% dofslaves2=(1:length(K2))';
% dofslaves2(dofmaster2)=[];


% for j=1:length(dofmaster1cont)
%     r=find(dofmaster1==dofmaster1cont(j));
%     dofmaster1(r)=[]; 
% end


% caso CB_controlled
% clear dofcontrolled1
% clear dofcontrolled2
% d=[];
% l=1;
% for j=1:length(dofmaster1cont)
%     r=find(dofmaster1cont(j)==a);
%     if length(r)~=1
%     m=0;
% for i=1:length(dofmaster1)
%     if dofmaster1cont(j)>dofmaster1(i)
%         m=m+1;
%     else break
%     end
% end
%    dofcontrolled1(l,1)=dofmaster1cont(j);
% d(l,1)=dofmaster1cont(j)-m;
% l=l+1;
%     end
% end
% dofslaves1c=dofslaves1;
% dofslaves1c(d)=[];
% dofslaves1=[dofcontrolled1;dofslaves1c];
% 
% d=[];
% l=1;
% for j=1:length(dofmaster2cont)
%      r=find(dofmaster2cont(j)==b);
%     if length(r)~=1
%     m=0;
% for i=1:length(dofmaster2)
%     if dofmaster2cont(j)>=dofmaster2(i)
%         m=m+1;
%     else break
%     end
% end
%  dofcontrolled2(l,1)=dofmaster2cont(j);
% d(l,1)=dofmaster2cont(j)-m;
% l=l+1;
%     end
% end
% dofslaves2c=dofslaves2;
% dofslaves2c(d)=[];
% dofslaves2=[dofcontrolled2;dofslaves2c];

% Ksc1=Ksc1+Kcontc;
% n=20;
% [Khcb,Mhcb,Thcb,phiN,psiC]=riduzioneCB(Ksc1,Msc1,dofmaster1,dofslaves1,dofcontrolled1,n);
% 
% Khcb=simmetrization(Khcb);
% Mhcb=simmetrization(Mhcb);
% Kv1=Khcb;
% Mv1=Mhcb;
% T1=Thcb;
% % % 
% [Khcb,Mhcb,Thcb,phiN,psiC]=riduzioneCB(Ksc2,Msc2,dofmaster2,dofslaves2,dofcontrolled2,n);
% 
% Khcb=simmetrization(Khcb);
% Mhcb=simmetrization(Mhcb);
% Kv2=Khcb;
% Mv2=Mhcb;
% T2=Thcb;
% % 
% Kv=blkdiag(Kv1,Kv2);
% Mv=blkdiag(Mv1,Mv2);
% T=blkdiag(T1,T2);
% 
Kscm=Ksc+Kcontc;
n=20;
[Khcb,Mhcb,Thcb,phiN,psiC]=riduzioneCB(Kscm,Msc,dofmaster,dofslaves,dofcontrolled,n);

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

[~,ordine]=sort([dofmaster;dofslaves]);
% [~,ordine]=sort([dofmaster1;dofslaves1;dofmaster2+length(Ksc1);dofslaves2+length(Ksc1)]);
% [~,ordine1]=sort([dofmaster1;dofslaves1]);
% [~,ordine2]=sort([dofmaster2;dofslaves2]);


[Vr1,Deer]=eigs(Kv,Mv,30,'sm');
Vr1=T*Vr1;
Vr1=Vr1(ordine,:);
% Vr1=Vr1./max(abs(Vr1));
evalr=abs(diag(Deer));
[eigvalr,indr]=sort(evalr);
eigvectr=abs(Vr1(:,indr));
eigvalr=sqrt(eigvalr)/2/pi;
% 
% Kvc=Kv;
% [r,c]=find(Kcontc~=0);
% for i=1:length(r)
%     if r(i)<=length(Ksc1)
%        [a]=find(dofmaster1==r(i));
%        a=a;
%     else
%        [a]=find(dofmaster2==r(i)-length(Ksc1));
%         a=a+length(Kv1);
%     end
%      if c(i)<=length(Ksc1)
%        [b]=find(dofmaster1==c(i));
%         b=b;
%     else
%        [b]=find(dofmaster2==c(i)-length(Ksc1));
%         b=b+length(Kv1);
%     end
%     Kvc(a,b)=Kvc(a,b)+Kcontc(r(i),c(i));
% end

% % Moltiplicare Kcont per matrice trasformazione
% Kmm=Kcontc(dofmaster1,dofmaster1);
% Kss=Kcontc(dofslaves1,dofslaves1);
% Kms=Kcontc(dofmaster1,dofslaves1);
% Ksm=Kcontc(dofslaves1,dofmaster1);
% % 
% % 
% Kcontc=[Kmm Kms; Ksm Kss];
% % 
% Kcontr=T'*Kcontc*T;
% % 
% % 
% Fcs=Fc([dofmaster1;dofslaves1;dofmaster2+length(Ksc1);dofslaves2+length(Ksc1)]);
Fcs=Fc([dofmaster;dofslaves]);
Fcr=T'*Fcs;
% % Kvc=Kv+Kcontr;
% % 
% % % STATIC SOLUTION
D=(Kv)\Fcr;
% D1=D(1:(end+1)/2);
% D2=D((end+1)/2+1:end);
Displall=T*D;
Displall=Displall(ordine);
% % expand and rearrange all the displacements
% % Displ1=T1*D1;
% % Displ1=Displ1(ordine1);
% % displ1rj=[Displ1(1:end-1);zeros(length(constdofs1),1);Displ1(end)];
% % Displ2=T2*D2;
% % Displ2=Displ2(ordine2);
% % displ2rj=[Displ2;zeros(length(constdofs2),1)];
displ1rj=[Displall(1:constdofs1(1)-1);zeros(length(constdofs1),1);Displall(constdofs1(1))];
displ2rj=[Displall(constdofs1(1)+1:end);zeros(length(constdofs2),1)];

% pinion displ
displ1=TRJ1*displ1rj;
displ13=reshape(displ1(1:end-6)',3,[])';



% gear displ
displ2=TRJ2*displ2rj;
displ23=reshape(displ2(1:end-6)',3,[])';

Displtot=[displ1;displ2];
[scaleZ1,teta1,scaleZ2,teta2]=DisplRadCirc(g1,displ13,g2,displ23);


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 
i=1; 
for j=1:3:size(eigvectr,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

[MAC_data,EMA_FEA] = MacW2_mod(50,eigvect1,NodesPhi_l,eigval1,eigvectr,NodesPhi_r,eigvalr,1);

% figure();
% plot(eigvect1(:,7));
% figure();
% plot(eigvectr(:,7));