
% -----------------------------STRESS RECOVERY
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_Gauss1=zeros(6,8,length(g1.FEM.elems));
Stress_Gauss1=zeros(6,8,length(g1.FEM.elems));
Strain_node1=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);
    [Strain_Gauss1(:,:,count), Stress_Gauss1(:,:,count), Strain_node1(:,:,count),...
        Stress_node1(:,:,count)] = Elem_Stress(B1(:,:,:,count),D,L,xstat);
end

SNode1=zeros(length(g1.FEM.nodes),6);
ENode1=zeros(length(g1.FEM.nodes),6);
SigmaVM1=zeros(length(g1.FEM.nodes),1);
EpslonVM1=zeros(length(g1.FEM.nodes),1);
% Stress averaging for every node (brutal version, must be improved)
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))';
        ENode1(count,:)=ENode1(count,:)+Strain_node1(:,posy(lun),posx(lun))';
       end
        SNode1(count,:)=SNode1(count,:)/length(posx);
        ENode1(count,:)=ENode1(count,:)/length(posx);
       SigmaVM1(count)=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));
       EpslonVM1(count)=sqrt(0.5*((ENode1(count,1)-ENode1(count,2))^2+...
          (ENode1(count,2)-ENode1(count,3))^2+ ...
          (ENode1(count,3)-ENode1(count,1))^2) ...
           + 3*(ENode1(count,4)^2+ENode1(count,5)^2+ENode1(count,6)^2));
end




PlotDefoMesh_mod(g1.FEM.nodes(:,2:4),g1.FEM.elems(:,2:9),4/max(abs(scaleZ1)),...
    displ13,SigmaVM1,...
    g1.FEM.surffaces,g1.FEM.elfa)
c=colorbar;
c.Label.String='[MPa]';
title('Stress pignone');
PlotDefoMesh_mod(g1.FEM.nodes(:,2:4),g1.FEM.elems(:,2:9),4/max(abs(scaleZ1)),...
    displ13,EpslonVM1,...
    g1.FEM.surffaces,g1.FEM.elfa)
title('Deformazioni pignone');
%------------ Gear Stresses -----------------------------------------------

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

% Stress recovery for all elements
Strain_Gauss2=zeros(6,8,length(g2.FEM.elems));
Stress_Gauss2=zeros(6,8,length(g2.FEM.elems));
Strain_node2=zeros(6,8,length(g2.FEM.elems));
Stress_node2=zeros(6,8,length(g2.FEM.elems));
for count=1:length(g2.FEM.elems)
    dofi=map2(count,:);
    xstat=displ2(dofi,1);
    [Strain_Gauss2(:,:,count), Stress_Gauss2(:,:,count), Strain_node2(:,:,count),...
        Stress_node2(:,:,count)] = Elem_Stress(B2(:,:,:,count),D,L,xstat);
end

SNode2=zeros(length(g2.FEM.nodes),6);
ENode2=zeros(length(g2.FEM.nodes),6);
SigmaVM2=zeros(length(g2.FEM.nodes),1);
EpslonVM2=zeros(length(g2.FEM.nodes),1);
% Stress averaging for every node (brutal version, must be improved)
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))';
        ENode2(count,:)=ENode2(count,:)+Strain_node2(:,posy(lun),posx(lun))';
       end
       SNode2(count,:)=SNode2(count,:)/length(posx);
       ENode2(count,:)=ENode2(count,:)/length(posx);
       SigmaVM2(count)=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));
        EpslonVM2(count)=sqrt(0.5*((ENode2(count,1)-ENode2(count,2))^2+...
          (ENode2(count,2)-ENode2(count,3))^2+ ...
          (ENode2(count,3)-ENode2(count,1))^2) ...
           + 3*(ENode2(count,4)^2+ENode2(count,5)^2+ENode2(count,6)^2));
end




PlotDefoMesh_mod(g2.FEM.nodes(:,2:4),g2.FEM.elems(:,2:9),4/max(abs(scaleZ2)),...
    displ23,SigmaVM2,...
    g2.FEM.surffaces,g2.FEM.elfa)
c=colorbar;
c.Label.String='[MPa]';
title('Stress condotta');
PlotDefoMesh_mod(g2.FEM.nodes(:,2:4),g2.FEM.elems(:,2:9),4/max(abs(scaleZ2)),...
    displ23,EpslonVM2,...
    g2.FEM.surffaces,g2.FEM.elfa)
title('Deformazioni condotta');