function fem3DHyperelastic()
% function femSpace3DHyperelastic
% Computes r1, r2I, r3I, r4IJ and r5IJ matrices
% Universidad de Zaragoza - 2015
global E nu 
global coords tet num_iter_prev Fprev
global r1 r2 r3 r5 r4

dof = 3; % Degrees of freedom per node
numNodes = size(coords,1);
numTet = size(tet,1);

% r1 is a sparse matrix which is calculated only at the first time
% increment.
% r2 is a cell containing all matrices r2I. r2{I}=r2I (same for r3)
% r4 is a cell containing all matrices r4IJ (same for r5)
% Nic is the number of increments in wich matrices are already calculated.
% Only new matrices are calculated. 
NIc = 0;
if num_iter_prev > 0
    NIc = size(r2,1); % Number of matrices r2 already calculated.
    if isempty(r2)==1, NIc = 0; end
end    
if NIc > 0
    % Save previously calculated matrices on auxiliary variables.
    r2_aux = r2; r3_aux = r3; r4_aux = r4; r5_aux = r5; 
end
%
% ALLOCATE MEMORY AND INITIALIZE
%
if num_iter_prev == 0
    r1 = sparse(dof*numNodes,dof*numNodes);
end
r2 = cell(num_iter_prev,1);
r4 = cell(num_iter_prev*num_iter_prev,1);
for i1=1:num_iter_prev, r2{i1} = sparse(dof*numNodes,dof*numNodes); end
for i1=1:num_iter_prev*num_iter_prev
    r4{i1} = sparse(dof*numNodes,dof*numNodes);
end
r3 = r2; r5 = r4;
%
% ADD ALREADY CALCULATED MATRICES
%
if NIc > 0
    r2(1:NIc) = r2_aux; r3(1:NIc) = r3_aux;    
    NewIncre = num_iter_prev-NIc;
    if NewIncre > 0
        IndMat = zeros(NIc*NIc,NIc); IndMat(:,1) = [1:NIc*NIc]'; 
        for i1=2:NIc
            IndMat(:,i1) = IndMat(:,i1-1) + NewIncre.*ones(size(IndMat,1),1);
        end
        IndMat = reshape(IndMat(:),[NIc,NIc*NIc]);
        IM = IndMat(:,1:NIc+1:end);
        r4(IM(:)) = r4_aux; r5(IM(:)) = r5_aux;
    else
        r4 = r4_aux; r5 = r5_aux;        
    end
end
%
% CALCULATE NEW MATRICES
%
%
% MATERIAL MATRIX
%
D = zeros(6); cte = E*(1-nu)/(1+nu)/(1-2*nu);
D(1) = cte; D(8) = D(1); D(15) = D(1);
D(2) = cte*nu/(1-nu); D(3) = D(2); D(7) = D(2); D(9) = D(2); D(13) = D(2);
D(14) = D(2); D(22) = cte*(1-2*nu)/2/(1-nu); D(29) = D(22); D(36) = D(22);
%
% INTEGRATION POINTS: 4 HAMMER POINTS
%
sg = zeros(3,4); wg = 1./24.*ones(4,1); nph = numel(wg);
a = (5.0 - sqrt(5))/20.0; b= (5.0 + 3.0*sqrt(5))/20.0;
sg(:,1) = [a; a; a]; sg(:,2) = [a; a; b]; 
sg(:,3) = [a; b; a];  sg(:,4) = [b; a; a]; 
%
% ELEMENT LOOP
%
for i1=1:numTet
    elnodes = tet(i1,:);
    xcoord = coords(elnodes,:);
    % System degrees of freedom associated with each element.
    index = [3*elnodes-2;3*elnodes-1;3*elnodes];
    index = reshape(index,1,4*dof);
    %
    % JACOBIAN
    %
    v1 = xcoord(1,:)-xcoord(2,:); v2 = xcoord(2,:)-xcoord(3,:);
    v3 = xcoord(3,:)-xcoord(4,:);
    jcob = abs(det([v1;v2;v3]));
    %
    % SHAPE FUNCTION CONSTANTS
    %
    a1 = det([xcoord(2,:); xcoord(3,:); xcoord(4,:)]);
    a2 = -det([xcoord(1,:); xcoord(3,:); xcoord(4,:)]);
    a3 = det([xcoord(1,:); xcoord(2,:); xcoord(4,:)]);
    a4 = -det([xcoord(1,:); xcoord(2,:); xcoord(3,:)]); 
    b1 = -det([1 xcoord(2,2:end); 1 xcoord(3,2:end); 1 xcoord(4,2:end)]);
    b2 = det([1 xcoord(1,2:end); 1 xcoord(3,2:end); 1 xcoord(4,2:end)]);
    b3 = -det([1 xcoord(1,2:end); 1 xcoord(2,2:end); 1 xcoord(4,2:end)]);
    b4 = det([1 xcoord(1,2:end); 1 xcoord(2,2:end); 1 xcoord(3,2:end)]);    
    c1 = det([1 xcoord(2,1) xcoord(2,end); 1 xcoord(3,1) xcoord(3,end);...
        1 xcoord(4,1) xcoord(4,end)]);
    c2 = -det([1 xcoord(1,1) xcoord(1,end); 1 xcoord(3,1) xcoord(3,end);...
        1 xcoord(4,1) xcoord(4,end)]);
    c3 = det([1 xcoord(1,1) xcoord(1,end); 1 xcoord(2,1) xcoord(2,end);...
        1 xcoord(4,1) xcoord(4,end)]);
    c4 = -det([1 xcoord(1,1) xcoord(1,end); 1 xcoord(2,1) xcoord(2,end);...
        1 xcoord(3,1) xcoord(3,end)]);
    d1 = -det([1 xcoord(2,1:end-1); 1 xcoord(3,1:end-1);...
        1 xcoord(4,1:end-1)]);
    d2 = det([1 xcoord(1,1:end-1); 1 xcoord(3,1:end-1);...
        1 xcoord(4,1:end-1)]);
    d3 = -det([1 xcoord(1,1:end-1); 1 xcoord(2,1:end-1);...
        1 xcoord(4,1:end-1)]);
    d4 = det([1 xcoord(1,1:end-1); 1 xcoord(2,1:end-1);...
        1 xcoord(3,1:end-1)]);
    %
    % INTEGRATION POINTS LOOP
    %    
    for j1=1:nph
        chi = sg(3*(j1-1)+1);
        eta = sg(3*(j1-1)+2);
        tau = sg(3*j1);
        %
        % GEOMETRY APPROACH
        %
        SHPa(4) = tau;
        SHPa(3) = eta; SHPa(2) = chi; SHPa(1) = 1.-chi-eta-tau;
        chiG = 0.0; etaG = 0.0; tauG = 0.0;
        for k1=1:4
            chiG = chiG + SHPa(k1)*xcoord(k1,1);
            etaG = etaG + SHPa(k1)*xcoord(k1,2);
            tauG = tauG + SHPa(k1)*xcoord(k1,3);
        end
        %
        % SHAPE FUNCTION COMPUTATION
        %
        SHP(1) = (a1 + b1*chiG + c1*etaG + d1*tauG)/jcob;
        dSHPx(1) = b1/jcob; dSHPy(1) = c1/jcob; dSHPz(1) = d1/jcob;
        SHP(2) = (a2 + b2*chiG + c2*etaG + d2*tauG)/jcob;
        dSHPx(2) = b2/jcob; dSHPy(2) = c2/jcob; dSHPz(2) = d2/jcob;
        SHP(3) = (a3 + b3*chiG + c3*etaG + d3*tauG)/jcob;
        dSHPx(3) = b3/jcob; dSHPy(3) = c3/jcob; dSHPz(3) = d3/jcob;
        SHP(4) = (a4 + b4*chiG + c4*etaG + d4*tauG)/jcob;
        dSHPx(4) = b4/jcob; dSHPy(4) = c4/jcob; dSHPz(4) = d4/jcob;
        %
        % COMPUTE MATRICES OF SHAPE FUNCTION DERIVATIVES
        %
        B = [dSHPx(1) 0 0 dSHPx(2) 0 0 dSHPx(3) 0 0 dSHPx(4) 0 0; ...
            0 dSHPy(1) 0 0 dSHPy(2) 0 0 dSHPy(3) 0 0 dSHPy(4) 0;...
            0 0 dSHPz(1) 0 0 dSHPz(2) 0 0 dSHPz(3) 0 0 dSHPz(4);...
            dSHPy(1) dSHPx(1) 0 dSHPy(2) dSHPx(2) 0 dSHPy(3) dSHPx(3)...
            0 dSHPy(4) dSHPx(4) 0;...
            dSHPz(1) 0 dSHPx(1) dSHPz(2) 0 dSHPx(2) dSHPz(3) 0 dSHPx(3)...
            dSHPz(4) 0 dSHPx(4);...
            0 dSHPz(1) dSHPy(1) 0 dSHPz(2) dSHPy(2) 0 dSHPz(3) dSHPy(3)...
            0 dSHPz(4) dSHPy(4)];
        Ghat = [dSHPx(1) 0 0 dSHPx(2) 0 0 dSHPx(3) 0 0 dSHPx(4) 0 0; ...
            0 dSHPx(1) 0 0 dSHPx(2) 0 0 dSHPx(3) 0 0 dSHPx(4) 0;...
            0 0 dSHPx(1) 0 0 dSHPx(2) 0 0 dSHPx(3) 0 0 dSHPx(4);...
            dSHPy(1) 0 0 dSHPy(2) 0 0 dSHPy(3) 0 0 dSHPy(4) 0 0; ...
            0 dSHPy(1) 0 0 dSHPy(2) 0 0 dSHPy(3) 0 0 dSHPy(4) 0;...
            0 0 dSHPy(1) 0 0 dSHPy(2) 0 0 dSHPy(3) 0 0 dSHPy(4);...
            dSHPz(1) 0 0 dSHPz(2) 0 0 dSHPz(3) 0 0 dSHPz(4) 0 0; ...
            0 dSHPz(1) 0 0 dSHPz(2) 0 0 dSHPz(3) 0 0 dSHPz(4) 0;...
            0 0 dSHPz(1) 0 0 dSHPz(2) 0 0 dSHPz(3) 0 0 dSHPz(4)];

        if num_iter_prev == 0
            % Only stiffness matrix r1 is nonzero at the begining.
            r1(index,index) = r1(index,index) + (B'*D*B)*jcob*wg(j1);
        else
            for indI=NIc+1:num_iter_prev % summation over I.
                %
                % CALCULATE Ahat
                %
                AhatI=zeros(6,9);
                thetaI = Ghat*Fprev(index,indI);
                thetaxt=thetaI(1:3)';
                thetayt=thetaI(4:6)';
                thetazt=thetaI(7:9)';
                AhatI(1,1:3)=thetaxt;
                AhatI(2,4:6)=thetayt;
                AhatI(3,7:9)=thetazt;
                AhatI(4,1:6)=[thetayt thetaxt];
                AhatI(5,4:9)=[thetazt thetayt];
                AhatI(6,1:3)=thetazt;
                AhatI(6,7:9)=thetaxt;
                %
                % CALCULATE f1
                %
                XvecI = D*B*Fprev(index,indI); 
                XI = zeros(3); %matrix expresion of XvecI.
                XI([1 5 9]) = XvecI([1 2 3]);
                XI(2) = XvecI(4); XI(4) = XI(2);
                XI(3) = XvecI(6); XI(7) = XI(3);
                XI(6) = XvecI(5); XI(8) = XI(6);
                f1I = [XI(1).*eye(3) XI(4).*eye(3) XI(7).*eye(3); ...
                    XI(2).*eye(3) XI(5).*eye(3) XI(8).*eye(3);...
                    XI(3).*eye(3) XI(6).*eye(3) XI(9).*eye(3)];
                %
                % CALCULATE r2I AND r3I
                %
                r2{indI}(index,index) = r2{indI}(index,index) + ...
                    B'*D*AhatI*Ghat*jcob*wg(j1);
                r3{indI}(index,index) = r3{indI}(index,index) + ...
                    Ghat'*f1I*Ghat*jcob*wg(j1);
                %
                % SECOND LOOP TO CALCULATE r4IJ AND r5IJ
                %
                for indJ=1:num_iter_prev % summation over J.
                    %
                    % CALCULATE Ahat
                    %
                    AhatJ=zeros(6,9);
                    thetaJ = Ghat*Fprev(index,indJ);
                    thetaxt=thetaJ(1:3)';
                    thetayt=thetaJ(4:6)';
                    thetazt=thetaJ(7:9)';
                    AhatJ(1,1:3)=thetaxt;
                    AhatJ(2,4:6)=thetayt;
                    AhatJ(3,7:9)=thetazt;
                    AhatJ(4,1:6)=[thetayt thetaxt];
                    AhatJ(5,4:9)=[thetazt thetayt];
                    AhatJ(6,1:3)=thetazt;
                    AhatJ(6,7:9)=thetaxt;
                    %
                    % CALCULATE f1
                    %
                    XvecIJ = D*AhatI*thetaJ; 
                    XIJ = zeros(3);
                    XIJ([1 5 9]) = XvecIJ([1 2 3]); 
                    XIJ(2) = XvecIJ(4); XIJ(4) = XIJ(2);
                    XIJ(3) = XvecIJ(6); XIJ(7) = XIJ(3);
                    XIJ(6) = XvecIJ(5); XIJ(8) = XIJ(6);
                    f1IJ =[XIJ(1).*eye(3) XIJ(4).*eye(3) XIJ(7).*eye(3);...
                        XIJ(2).*eye(3) XIJ(5).*eye(3) XIJ(8).*eye(3);...
                        XIJ(3).*eye(3) XIJ(6).*eye(3) XIJ(9).*eye(3)];
                    %
                    % CALCULATE r4IJ and r5IJ
                    %
                    r4{(indI-1)*num_iter_prev+indJ}(index,index) = ... 
                        r4{(indI-1)*num_iter_prev+indJ}(index,index) + ...
                        Ghat'*AhatI'*D*AhatJ*Ghat*jcob*wg(j1);
                    r5{(indI-1)*num_iter_prev+indJ}(index,index) = ...
                        r5{(indI-1)*num_iter_prev+indJ}(index,index) + ...
                        Ghat'*f1IJ*Ghat*jcob*wg(j1);
               end
            end
            for indI=1:NIc
                %
                % CALCULATE Ahat
                %
                AhatI=zeros(6,9);
                thetaI = Ghat*Fprev(index,indI);
                thetaxt=thetaI(1:3)';
                thetayt=thetaI(4:6)';
                thetazt=thetaI(7:9)';
                AhatI(1,1:3)=thetaxt;
                AhatI(2,4:6)=thetayt;
                AhatI(3,7:9)=thetazt;
                AhatI(4,1:6)=[thetayt thetaxt];
                AhatI(5,4:9)=[thetazt thetayt];
                AhatI(6,1:3)=thetazt;
                AhatI(6,7:9)=thetaxt;
                for indJ=NIc+1:num_iter_prev
                    %
                    % CALCULATE Ahat
                    %
                    AhatJ=zeros(6,9);
                    thetaJ = Ghat*Fprev(index,indJ);
                    thetaxt=thetaJ(1:3)';
                    thetayt=thetaJ(4:6)';
                    thetazt=thetaJ(7:9)';
                    AhatJ(1,1:3)=thetaxt;
                    AhatJ(2,4:6)=thetayt;
                    AhatJ(3,7:9)=thetazt;
                    AhatJ(4,1:6)=[thetayt thetaxt];
                    AhatJ(5,4:9)=[thetazt thetayt];
                    AhatJ(6,1:3)=thetazt;
                    AhatJ(6,7:9)=thetaxt;
                    %
                    % CALCULATE f1
                    %
                    XvecIJ = D*AhatI*thetaJ; 
                    XIJ = zeros(3);
                    XIJ([1 5 9]) = XvecIJ([1 2 3]); 
                    XIJ(2) = XvecIJ(4); XIJ(4) = XIJ(2);
                    XIJ(3) = XvecIJ(6); XIJ(7) = XIJ(3);
                    XIJ(6) = XvecIJ(5); XIJ(8) = XIJ(6);
                    f1IJ =[XIJ(1).*eye(3) XIJ(4).*eye(3) XIJ(7).*eye(3);...
                        XIJ(2).*eye(3) XIJ(5).*eye(3) XIJ(8).*eye(3);...
                        XIJ(3).*eye(3) XIJ(6).*eye(3) XIJ(9).*eye(3)];
                    %
                    % CALCULATE r4IJ and r5IJ
                    %
                    r4{(indI-1)*num_iter_prev+indJ}(index,index) = ...
                        r4{(indI-1)*num_iter_prev+indJ}(index,index) + ...
                        Ghat'*AhatI'*D*AhatJ*Ghat*jcob*wg(j1);
                    r5{(indI-1)*num_iter_prev+indJ}(index,index) = ...
                        r5{(indI-1)*num_iter_prev+indJ}(index,index) + ...
                        Ghat'*f1IJ*Ghat*jcob*wg(j1);
               end
            end
        end
    end
end
return
