function [Kf,Mf,Tr,A]=riduzioneMacNeal(K,Mass,dofslaves,mapdof,Nmodes)
A=eye(length(mapdof),length(mapdof));
A([dofslaves],:)=[];
m=length(mapdof)-length(dofslaves)+Nmodes;
[teta,omega]=eigs(K,Mass,Nmodes,'sm');
% [teta1,omega1]=eigs(K,Mass,length(K)-Nmodes,'lm');
% K1=inv(K);
K1=pinv(full(K),10e-3);
Gr=0;
k=0;
for j=1:Nmodes
    if omega(j,j)<1
        k=1;
    else
        Gr=Gr+(teta(:,j)*teta(:,j)')./omega(j,j);
    end
end
Gr=K1-Gr;
Grbb=A*Gr*A';
Krbb=inv(Grbb);
if k==1
R=teta(:,1:6);
Rb=A*R;
KR=-Krbb*Rb;
Psi=teta(:,7:end);
else 
R=[];
KR=[];
Psi=teta;
end
T1=[R Psi Gr*A'];
Psib=A*Psi;
KPsi=-Krbb*Psib;

 T2=[eye(size(KR,2)+size(KPsi,2)) zeros(size(KR,2)+size(KPsi,2),size(Krbb,2)); KR, KPsi, Krbb];
% for i=size(T2,2)-Nmodes+1:size(T2,2)
%     j=i;
%     T2(i,j)=1;
% end
Tr=T1;
%  Tr=[[eye(length(mapdof)-length(dofslaves)); zeros(length(dofslaves),length(mapdof)-length(dofslaves))] R Psi];
Kf=Tr'*K*Tr;
% MacNeal
% Mf=T1'*Mass*T1;
% Mf(abs(Mf)>2)=0;
% Mf=T2'*Mf*T2;
% 
Mf=Tr'*Mass*Tr;
end
