clear all, clc,
% close all
load('Kahraman_appdata.mat') % data is just data=app.data;

r=data.optimicro(end).results;
inp = data.coupling.input;
g1=data.coupling.gear1;
g2=data.coupling.gear2;


% 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
 % 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,
    
for iii= 1:20 % loop for the angular position counter
for tk=2:3 %loop for all teeth - inp.params.sel_teeth
    for lk=2:inp.params.slices-1 %loop for alls slices
        % First and last slice are excluded otherwise i get points on the
        % face of the gear - To be addressed in the future
        % Pinion nodes (index is lk-1 because i'm starting from 2)
        p1{iii,tk}(lk-1,1) =  g1.rigid_contact.xmat(r(1).gearpair.CNall{1,iii}{iii,tk}(lk,2),r(1).gearpair.CNall{1,iii}{iii,tk}(lk,1),tk);
        p1{iii,tk}(lk-1,2) =  g1.rigid_contact.ymat(r(1).gearpair.CNall{1,iii}{iii,tk}(lk,2),r(1).gearpair.CNall{1,iii}{iii,tk}(lk,1),tk);
        p1{iii,tk}(lk-1,3) =  g1.rigid_contact.zmat(r(1).gearpair.CNall{1,iii}{iii,tk}(lk,2),r(1).gearpair.CNall{1,iii}{iii,tk}(lk,1),tk);
        % Gear nodes
        p2{iii,tk}(lk-1,1) =  g2.rigid_contact.xmat(r(1).gearpair.CNall{1,iii}{iii,tk}(lk,3),r(1).gearpair.CNall{1,iii}{iii,tk}(lk,1),tk);
        p2{iii,tk}(lk-1,2) =  g2.rigid_contact.ymat(r(1).gearpair.CNall{1,iii}{iii,tk}(lk,3),r(1).gearpair.CNall{1,iii}{iii,tk}(lk,1),tk);
        p2{iii,tk}(lk-1,3) =  g2.rigid_contact.zmat(r(1).gearpair.CNall{1,iii}{iii,tk}(lk,3),r(1).gearpair.CNall{1,iii}{iii,tk}(lk,1),tk);
    end %loop for all slices
end %loop for all teeth

for tk=2:3 %loop for all teeth - inp.params.sel_teeth
    % Pinion -------------------------
    [nod1{iii,tk},distC1{iii,tk}]=contact2fem([g1.FEM.surfnodes(:,2),g1.FEM.surfnodes(:,3),g1.FEM.surfnodes(:,4)],...
        p1{iii,tk});
    [masterC_list1{iii,tk},ia1{tk},~]=unique(nod1{iii,tk},'stable');
    % Calculation of the weight to be assigned to each node to spread the
    % contact stiffness over the contact area
    for sl=1:size(distC1{iii,tk},1)
        di=distC1{iii,tk}(sl,:);
        w=di/sum(di);
        weightC1{iii,tk}(sl,:)=w;
    end
    
    % Gear -------------------------
    [nod2{iii,tk},distC2{iii,tk}]=contact2fem([g2.FEM.surfnodes(:,2),g2.FEM.surfnodes(:,3),g2.FEM.surfnodes(:,4)],...
        p2{iii,tk});
    [masterC_list2{iii,tk},ia2{tk},~]=unique(nod2{iii,tk},'stable');
    % Calculation of the weight to be assigned to each node to spread the
    % contact stiffness over the contact area
    for sl=1:size(distC2{iii,tk},1)
        di=distC2{iii,tk}(sl,:);
        w=di/sum(di);
        weightC2{iii,tk}(sl,:)=w;
    end
    
   
    plot3(g1.FEM.surfnodes(masterC_list1{iii,tk},2),g1.FEM.surfnodes(masterC_list1{iii,tk},3),g1.FEM.surfnodes(masterC_list1{iii,tk},4),'xr'); hold on,
    plot3(p1{iii,tk}(:,1),p1{iii,tk}(:,2),p1{iii,tk}(:,3),'ob');
%         plot3(p2{iii,tk}(:,1),p2{iii,tk}(:,2),p2{iii,tk}(:,3),'og');
    
%         % sort p1 and p2 pairs by minimal distance
%         for rd=1:size(p1{iii,tk},1)
%         dist=sqrt((p2{iii,tk}(:,1)-p1{iii,tk}(rd,1)).^2+(p2{iii,tk}(:,2)-p1{iii,tk}(rd,2)).^2+(p2{iii,tk}(:,3)-p1{iii,tk}(rd,3)).^2)
%         end
        
end
end % end loop for all angular positions

% All unique nodes needed to attach the contact stiffness, in ascending
% order. masterC_list will be used for each angular position since it's
% divided by angular position and tooth number
MasterC1=unique(cell2mat(masterC_list1));
MasterC2=unique(cell2mat(masterC_list2));


%% 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,vtot]=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)=[];

% begin assembly of matrix TRJ1
% main matrix
TRJ1=sparse(length(K1)+6,length(allmasters1));
TRJ1(allmasters1,:)=speye(length(allmasters1));
% slaves
for sn=1:length(slaveschar1)
    % Distances
    chi1=g1.FEM.nodes(slaveschar1(sn),2:4)-[g1.params.origin,0];
    % submatrix
    Ti = [1 0 0 0 -chi1(1,3) +chi1(1,2)
        0 1 0 +chi1(1,3) 0 -chi1(1,1)
        0 0 1 -chi1(1,2) +chi1(1,1) 0];
    % Assembly in TRJ
    TRJ1(slaveschar1(sn,2:4),end-5:end)=Ti; % This is slow, must be improved
end

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

Kt1=TRJ1'*blkdiag(K1,zeros(6))*TRJ1;
Mt1=TRJ1'*blkdiag(M1,zeros(6))*TRJ1;
f1=zeros(length(Kt1),1);
f1(end)=-150000;
constdofs1=[length(Kt1)-5:length(Kt1)-1]';
shiftk1=length(Kt1);

% Gear --------------------------------------------------------------------
% Assemble FE matrices
[K2,M2,Ml2,G2,C2,vtot]=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)=[];

% begin assembly of matrix TRJ1
% main matrix
TRJ2=sparse(length(K2)+6,length(allmasters2));
TRJ2(allmasters2,:)=speye(length(allmasters2));
% slaves
for sn=1:length(slaveschar2)
    % Distances
    chi2=g2.FEM.nodes(slaveschar2(sn),2:4)-[g2.params.origin,0];
    % submatrix
    Ti = [1 0 0 0 -chi2(1,3) +chi2(1,2)
        0 1 0 +chi2(1,3) 0 -chi2(1,1)
        0 0 1 -chi2(1,2) +chi2(1,1) 0];
    % Assembly in TRJ
    TRJ2(slaveschar2(sn,2:4),end-5:end)=Ti; % This is slow, must be improved
end

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

Kt2=TRJ2'*blkdiag(K2,zeros(6))*TRJ2;
Mt2=TRJ2'*blkdiag(M2,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);

iii=1;
kcont=[NaN 5e+8 10e+8]; %this will be an input for each tooth, each position, each torque
for tk=2:3 %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{tk})/sum(sum(weightC1{iii,tk}(ia1{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(iii,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=reshape(displ1(1:end-6)',3,[])';
% scaleZ=sqrt(abs(displ13(:,1).^2+displ13(:,2).^2));
scaleZ=displ13(:,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)

% gear displ
displ2rj=displall(length(Kt1)+1:end);
displ2=TRJ2*displ2rj;
displ23=reshape(displ2(1:end-6)',3,[])';
scaleZ2=sqrt(abs(displ23(:,1).^2+displ23(:,2).^2));

PlotDefoMesh_mod2(101,g1.FEM.nodes(:,2:4),g1.FEM.elems(:,2:9),4/max(abs(scaleZ)),...
    displ13,scaleZ,...
    g1.FEM.surffaces,g1.FEM.elfa)

PlotDefoMesh_mod2(101,g2.FEM.nodes(:,2:4),g2.FEM.elems(:,2:9),4/max(abs(scaleZ2)),...
    displ23,scaleZ2,...
    g2.FEM.surffaces,g2.FEM.elfa)

PlotDefoMesh_mod(g2.FEM.nodes(:,2:4),g2.FEM.elems(:,2:9),4/max(abs(scaleZ2)),...
    displ23,scaleZ2,...
    g2.FEM.surffaces,g2.FEM.elfa)

%% Eigenproblem solution
M1b=tril(M1);
M1b=M1b+tril(M1b,-1)';
K1b=tril(K1);
K1b=K1b+tril(K1b,-1)';
[v1,d1]=eigs(K1b,M1b,30,'sm');
[eig1,sort1]=sort(sqrt(diag(d1))/2/pi);
eval1=v1(:,sort1);

displ13=(reshape(eval1(:,8),3,[])');
% scaleZ=sqrt(displ13(:,1).^2+displ13(:,2).^2);
scaleZ=sqrt(displ13(:,1).^2+displ13(:,2).^2+displ13(:,3).^2);
% scaleZ=displ13(:,3);
PlotDefoMesh_mod(g1.FEM.nodes(:,2:4),g1.FEM.elems(:,2:9),4/max(abs(scaleZ)),...
    displ13,scaleZ,...
    g1.FEM.surffaces,g1.FEM.elfa)