 clear all, clc,
% close all
% load('Kahraman_appdata.mat') % data is just data=app.data;
load('Kahraman_appdata.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');
axis equal,
% plot3(g1.FEM.surfnodes(slavehub1,2),g1.FEM.surfnodes(slavehub1,3),g1.FEM.surfnodes(slavehub1,4),'xr');
plot3(g2.FEM.surfnodes(slavehub2,2),g2.FEM.surfnodes(slavehub2,3),g2.FEM.surfnodes(slavehub2,4),'xk');



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

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

% Base fem assembly completed, next the rotations for each angular position are computed            
            
%% CALCULATION OF THE ROTATION MATRICES FOR PINION AND GEAR
for iii=1:20

[K1r,M1r,TRJ1,g1]= StepAssembly_cicli(g1,K1,M1,allmasters1,slaveschar1,data,iii);
g2.FEM.nodes(:,3,1)=g2.FEM.nodes_original(:,3)-g2.params.origin(2);
[K2r,M2r,TRJ2,g2]= StepAssembly_cicli(g2,K2,M2,allmasters2,slaveschar2,data,iii);

g2.FEM.nodes(:,3,1)=g2.FEM.nodes_original(:,3);

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

Kt2=TRJ2'*blkdiag(K2r,zeros(6))*TRJ2;
Mt2=TRJ2'*blkdiag(M2r,zeros(6))*TRJ2;
f2=zeros(length(Kt2),1);
constdofs2=[length(Kt1)-5:length(Kt1)]';
           
%% ASSEMBLY OF THE SYSTEM AND ADDITION OF THE CONTACT STIFFNESS 
Ks=blkdiag(Kt1,Kt2);
Ms=blkdiag(Mt1,Mt2);
F=[f1;f2];
% Kcont=spalloc(length(Ks),length(Ks),1000);
Kcon=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
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(1,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;
    end
end

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

% %% 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(:,:,iii)=reshape(displ1(1:end-6)',3,[])';


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

Stress_Gauss1=zeros(6,8,length(g1.FEM.elems));
Stress_node1=zeros(6,8,length(g1.FEM.elems));

for count=1:length(g1.FEM.elems)
    dofi=map1(count,:);
    xstat=displ1(dofi,1);
    [~,Stress_Gauss1(:,:,count),~,Stress_node1(:,:,count)] = Elem_Stress(B1(:,:,:,count),D,L,xstat);
end

SNode1=zeros(length(g1.FEM.nodes),6);
SigmaVM1(:,iii)=zeros(length(g1.FEM.nodes),1);

for count=1:length(g1.FEM.nodes)
       ni=g1.FEM.nodes(count,1); % node number
       [posx,posy]=find(ni==g1.FEM.elems(:,2:9));
       for lun=1:length(posx)
        SNode1(count,:)=SNode1(count,:)+Stress_node1(:,posy(lun),posx(lun))';
       end
        SNode1(count,:)=SNode1(count,:)/length(posx);
       
       SigmaVM1(count,iii)=sqrt(0.5*((SNode1(count,1)-SNode1(count,2))^2+...
          (SNode1(count,2)-SNode1(count,3))^2+ ...
          (SNode1(count,3)-SNode1(count,1))^2) ...
           + 3*(SNode1(count,4)^2+SNode1(count,5)^2+SNode1(count,6)^2));
end

Stress_Gauss2=zeros(6,8,length(g1.FEM.elems));
Stress_node2=zeros(6,8,length(g1.FEM.elems));

for count=1:length(g2.FEM.elems)
    dofi=map2(count,:);
    xstat=displ2(dofi,1);
    [~,Stress_Gauss2(:,:,count),~,Stress_node2(:,:,count)] = Elem_Stress(B2(:,:,:,count),D,L,xstat);
end

SNode2=zeros(length(g2.FEM.nodes),6);
SigmaVM2(:,iii)=zeros(length(g2.FEM.nodes),1);

for count=1:length(g2.FEM.nodes)
       ni=g2.FEM.nodes(count,1); % node number
       [posx,posy]=find(ni==g2.FEM.elems(:,2:9));
       for lun=1:length(posx)
        SNode2(count,:)=SNode2(count,:)+Stress_node2(:,posy(lun),posx(lun))';
       end
        SNode2(count,:)=SNode2(count,:)/length(posx);
       
       SigmaVM2(count,iii)=sqrt(0.5*((SNode2(count,1)-SNode2(count,2))^2+...
          (SNode2(count,2)-SNode2(count,3))^2+ ...
          (SNode2(count,3)-SNode2(count,1))^2) ...
           + 3*(SNode2(count,4)^2+SNode2(count,5)^2+SNode2(count,6)^2));
end
% 

[~,d1]=eigs(K1,M1,20,'sm');
[~,d2]=eigs(K2,M2,20,'sm');
eig1(:,iii)=diag(d1);
eig2(:,iii)=diag(d2);
eigval1=sqrt(eigval1(:,iii))/2/pi;
eigval2=sqrt(eigval2(:,iii))/2/pi;
dif(:,iii)=eig1(:,iii)-eig2(:,iii);
end

% [scaleZ1,teta1,scaleZ2,teta2]=DisplRadCirc_cicli(g1,displ13,g2,displ23);
