function [K1r,M1r,TRJ1,g1]= StepAssembly(g1,K1,M1,allmasters1,slaveschar1,data,iii)
%% CALCULATION OF THE ROTATION MATRICES FOR PINION AND GEAR
thetap=2*pi/g1.params.z/data.optimicro(1).params.NumPosizioni*(iii-1)*g1.params.rot; % 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.nodes,1));
    rotp= blkdiag(tempcell{:});
    clear tempcell
    % For loop on each angular position on the mesh cycle data.optimicro(1).params.NumPosizioni
    
%     % Rotate nodes, pinion
    pnodes=g1.FEM.nodes(:,2:4);
    pnodesv=reshape(pnodes',[],1);
    g1.FEM.nodes(:,2:4)=reshape(rotp*pnodesv,3,[])';

          K1r=simmetrization(rotp'*K1*rotp);
          M1r=simmetrization(rotp'*M1*rotp);
  

 
    
% begin assembly of matrix TRJ1
% main matrix
TRJ1=sparse(length(K1)+6,length(allmasters1));
TRJ1(allmasters1,:)=speye(length(allmasters1));
% slaves
if g1.params.origin(2)~=0
    g1.FEM.nodes(:,3)=g1.FEM.nodes(:,3)+g1.params.origin(2);
end

for sn=1:length(slaveschar1)
    % Distances

    chi1=-g1.FEM.nodes(slaveschar1(sn),2:4)+[g1.params.origin,g1.params.b/2];
    % 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

end % Function end
