/****************************************************************************
*
*                                 M U E S L I   v 1.8
*
*
*     Copyright 2020 IMDEA Materials Institute, Getafe, Madrid, Spain
*     Contact: muesli.materials@imdea.org
*     Author: Ignacio Romero (ignacio.romero@imdea.org)
*
*     This file is part of MUESLI.
*
*     MUESLI is free software: you can redistribute it and/or modify
*     it under the terms of the GNU General Public License as published by
*     the Free Software Foundation, either version 3 of the License, or
*     (at your option) any later version.
*
*     MUESLI is distributed in the hope that it will be useful,
*     but WITHOUT ANY WARRANTY; without even the implied warranty of
*     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*     GNU General Public License for more details.
*
*     You should have received a copy of the GNU General Public License
*     along with MUESLI.  If not, see <http://www.gnu.org/licenses/>.
*
****************************************************************************/



#include "reducedfinitestrain.h"

/*
 *
 * reducedsmallstrain.cpp
 * reduced models with zero stresses, such as shell and beams
 * D. Portillo, may 2017
 *
 * see "Using finite strain 3D-material models in beam and
 *      shell elements", S. Klinkel & S. Govindjee
 *
 * Note that since C = 2E + I is positive semidefinite, the problem
 * in finite strain theory might not have solution.
 */

using namespace muesli;
using namespace std;

const int maxiter = 350;
const double tol = 1.0e-10;
const double coefE[6] ={1.0, 1.0, 1.0, 2.0, 2.0, 2.0};

double fcomputeBeta(vector<double> dx0, vector<double> dx, vector<double> s);


rFiniteStrainMP::rFiniteStrainMP(finiteStrainMP *mp) 
:
_method(_fnlcg),   
theFiniteStrainMP(mp)
{
}




rFiniteStrainMP::rFiniteStrainMP(finiteStrainMP *mp, std::string method)
:
_method(_fnlcg),
theFiniteStrainMP(mp)
{
    if (method == "nlcg")   _method = _fnlcg;
}




void rFiniteStrainMP::CauchyStressVector(double S[6]) const
{
    istensor sigma;
    theFiniteStrainMP->CauchyStress(sigma);
    muesli::tensorToVector(sigma, S);
}




void rFiniteStrainMP::commitCurrentState()
{
    theFiniteStrainMP->commitCurrentState();
}




/*istensor& rFiniteStrainMP::getCurrentGLStrain()
 {
 itensor F;
 F = theFiniteStrainMP->deformationGradient();
 //return 0.5*(istensor::tensorTransposedTimesTensor(F) - istensor::identity());
 return istensor::identity();
 }*/




itensor& rFiniteStrainMP::deformationGradient()
{
    return theFiniteStrainMP->deformationGradient();
}




const itensor& rFiniteStrainMP::deformationGradient() const
{
    return theFiniteStrainMP->deformationGradient();
}




double rFiniteStrainMP::effectiveStoredEnergy() const
{
    return theFiniteStrainMP->effectiveStoredEnergy();
}




void rFiniteStrainMP::energyMomentumTensor(itensor &T) const
{
    theFiniteStrainMP->energyMomentumTensor(T);
}




void rFiniteStrainMP::firstPiolaKirchhoffStress(itensor &P) const
{
    theFiniteStrainMP->firstPiolaKirchhoffStress(P);
}




materialState rFiniteStrainMP::getConvergedState() const
{
    return theFiniteStrainMP->getConvergedState();
}




materialState rFiniteStrainMP::getCurrentState() const
{
    return theFiniteStrainMP->getCurrentState();
}




void rFiniteStrainMP::KirchhoffStress(istensor &tau) const
{
    theFiniteStrainMP->KirchhoffStress(tau);
}





void rFiniteStrainMP::KirchhoffStressVector(double tauv[6]) const
{
    theFiniteStrainMP->KirchhoffStressVector(tauv);
}




void rFiniteStrainMP::resetCurrentState()
{
    theFiniteStrainMP->resetCurrentState();
}




void rFiniteStrainMP::secondPiolaKirchhoffStress(istensor& S) const
{
    theFiniteStrainMP->secondPiolaKirchhoffStress(S);
}




void  rFiniteStrainMP::setRandom()
{
    theFiniteStrainMP->setRandom();
}




double rFiniteStrainMP::volumetricStiffness() const
{
    return theFiniteStrainMP->volumetricStiffness();
}




bool rFiniteStrainMP::testImplementation(std::ostream& of, const bool testDE, const bool testDDE) const
{
    bool isok = true;
    // set a random update in the material
    itensor F;
    F.setRandom();
    F *= 1.0e-10;
    F = itensor::identity();
    if (F.determinant() < 0.0) F *= -1.0;
    rFiniteStrainMP& theMP = const_cast<rFiniteStrainMP&>(*this);
    theMP.updateCurrentState(0.0, F);
    theMP.commitCurrentState();

    double tn1 = muesli::randomUniform(0.1,1.0);
    istensor u;
    u.setRandom();
    u *= 1e-2;
    F += u;
    istensor FtF;
    FtF = istensor::tensorTransposedTimesTensor(F);
    istensor Uc;
    Uc = istensor::squareRoot(FtF);
    itensor R;
    R = F*Uc.inverse();
    istensor Ec;
    Ec = 0.5 * ( istensor::tensorTransposedTimesTensor(F) - istensor::identity() );

    if (F.determinant() < 0.0) F *= -1.0;
    theMP.updateCurrentState(tn1, F);


    istensor S;
    secondPiolaKirchhoffStress(S);

    itensor4 nC;
    istensor dS, Sp1, Sp2, Sm1, Sm2;


    // numerical differentiation stress
    istensor numS;
    numS.setZero();
    
    const double inc = 1.0e-5*Ec.norm();
    for (unsigned i=0; i<3; i++)
    {
        for (unsigned j=i; j<3; j++)
        {
            double original = Ec(i,j);

            Ec(i,j) = Ec(j,i) =  original + inc;
            FtF = 2.0*Ec + istensor::identity();
            Uc = istensor::squareRoot(FtF);
            F = R*Uc;
            theMP.updateCurrentState(tn1, F);
            double Wp1 = effectiveStoredEnergy();
            secondPiolaKirchhoffStress(Sp1);

            Ec(i,j) = Ec(j,i) = original + 2.0*inc;
            FtF = 2.0*Ec + istensor::identity();
            Uc = istensor::squareRoot(FtF);
            F = R*Uc;
            theMP.updateCurrentState(tn1, F);
            double Wp2 = effectiveStoredEnergy();
            secondPiolaKirchhoffStress(Sp2);

            Ec(i,j) = Ec(j,i) = original - inc;
            FtF = 2.0*Ec + istensor::identity();
            Uc = istensor::squareRoot(FtF);
            F = R*Uc;
            theMP.updateCurrentState(tn1, F);
            double Wm1 = effectiveStoredEnergy();
            secondPiolaKirchhoffStress(Sm1);

            Ec(i,j) = Ec(j,i) = original - 2.0*inc;
            FtF = 2.0*Ec + istensor::identity();
            Uc = istensor::squareRoot(FtF);
            F = R*Uc;
            theMP.updateCurrentState(tn1, F);
            double Wm2 = effectiveStoredEnergy();
            secondPiolaKirchhoffStress(Sm2);

            // fourth order approximation of the derivative
            numS(i,j) = (-Wp2 + 8.0*Wp1 - 8.0*Wm1 + Wm2)/(12.0*inc);
            if (i!=j) numS(i,j) *= 0.5;
            numS(j,i) = numS(i,j);

            // derivative of PK stress
            dS = (-Sp2 + 8.0*Sp1 - 8.0*Sm1 + Sm2)/(12.0*inc);
            if (i!=j) dS *= 0.5;
            for (unsigned k=0; k<3; k++)
                for (unsigned l=0; l<3; l++)
                    nC(k,l,i,j) = nC(k,l,j,i) = dS(k,l);

            Ec(i,j) = Ec(j,i) = original;
            FtF = 2.0*Ec + istensor::identity();
            Uc = istensor::squareRoot(FtF);
            F = R*Uc;
            theMP.updateCurrentState(tn1, F);
        }
    }
    // compare DEnergy with the derivative of Energy
    {
        if (testDE)
        {
            istensor errorS = numS - S;
            isok = (errorS.norm()/S.norm() < 1e-2);
            of << "\n   1. Comparing S with derivative of Weff.";
            if (isok)
            {
                of << " Test passed.";
            }
            else
            {
                of << "\n Relative error in DE computation: " << errorS.norm()/S.norm() << ". Test failed.";
                of << "\n S: \n" << S;
                of << "\n numS: \n" << numS;
            }
        }
        else
        {
            of << "\n   1. Comparing S with derivative of Weff ::: not run for this material";
        }
        of << std::flush;
    }

    // (2) compare tensor c with derivative of stress reduced formulation
    if ((_method == _fnr))
    {
        // get the voigt notation
        double nCv[6][6];
        muesli::tensorToMatrix(nC,nCv);

        // get the tangent in voigt notation
        double tgv[6][6];
        convectedTangentMatrix(tgv);

        // relative error less than 0.01%
        double error = 0.0;
        double norm = 0.0;
        for (unsigned i=0; i<6; i++)
            for (unsigned j=0; j<6; j++)
            {
                error += pow(nCv[i][j]-tgv[i][j],2);
                norm  += pow(tgv[i][j],2);
            }
        error = sqrt(error);
        norm = sqrt(norm);
        isok = (error/norm < 1e-2);

        of << "\n   2. Comparing tensor C with DStress (reduced model).";
        if (isok)
        {
            of << " Test passed.";
        }
        else
        {
            of << "\n      Test failed.";
            of << "\n      Relative error in DWeff computation: " <<  error/norm;
        }
    }


    return isok;
}




double rFiniteStrainMP::plasticSlip() const
{
    return theFiniteStrainMP->plasticSlip();
}




void rFiniteStrainMP::CauchyStress(istensor &sigma) const
{
    return theFiniteStrainMP->CauchyStress(sigma);
}




double rFiniteStrainMP::storedEnergy() const
{
    return theFiniteStrainMP->storedEnergy();
}




double rFiniteStrainMP::waveVelocity() const
{
    return theFiniteStrainMP->waveVelocity();
}




double rFiniteStrainMP::density() const
{
    return theFiniteStrainMP->density();
}




double rFiniteStrainMP::dissipatedEnergy() const
{
    return theFiniteStrainMP->dissipatedEnergy();
}




/*********************************************************************************
 reduced1zFMP
 **********************************************************************************/


reduced1zFMP::reduced1zFMP(finiteStrainMP *mp, unsigned inmapim[5], unsigned inmapiz[1]) :
muesli::rFiniteStrainMP(mp)
{
    for (unsigned i=0; i<5; i++)
        mapim[i] = inmapim[i];

    mapiz[0] = inmapiz[0];
} 




reduced1zFMP::reduced1zFMP(finiteStrainMP *mp, unsigned inmapim[5], unsigned inmapiz[1], std::string method) :
muesli::rFiniteStrainMP(mp, method)
{
    for (unsigned i=0; i<5; i++)
        mapim[i] = inmapim[i];

    mapiz[0] = inmapiz[0];
} 




reduced1zFMP::~reduced1zFMP()
{

}




void reduced1zFMP::convectedTangentMatrix(double C[6][6]) const
{
    itensor4 tg;
    theFiniteStrainMP->convectedTangent(tg);
    muesli::tensorToMatrix(tg, C);

    unsigned iz = mapiz[0]; // Cc's index such that Cc[iz][iz] = Czz
    double Czz = C[iz][iz];
    double Cmz[5] = {0.0};
    double Czm[5] = {0.0};

    for (int i=0; i<5; i++)
    {
        Cmz[i] = C[mapim[i]][iz      ];
        Czm[i] = C[iz      ][mapim[i]];
    }

    // new tangent
    for (int i=0; i<5; i++)
    {
        C[mapim[i]][iz      ] = 0.0;
        C[iz      ][mapim[i]] = 0.0;
    }
    C[iz][iz] = 0.0;

    for (int i=0; i<5; i++)
        for (int j=0; j<5; j++)
            C[mapim[i]][mapim[j]] = C[mapim[i]][mapim[j]] - Cmz[i]*Czm[j]/Czz;

}




void reduced1zFMP::secondPiolaKirchhoffStressVector(double S[6]) const
{
    theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
}




void reduced1zFMP::updateCurrentState(const double theTime, itensor& F)
{
    theFiniteStrainMP->updateCurrentState(theTime, F);

    if (_method == _fnr)
    {
        F = theFiniteStrainMP->deformationGradient();
        const unsigned iz = mapiz[0];
        // get R
        istensor FtF;
        FtF = istensor::tensorTransposedTimesTensor(F);
        istensor Uc;
        Uc = istensor::squareRoot(FtF);
        itensor R;
        R = F*Uc.inverse();

        double S[6];
        theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
        double Sz;
        Sz = S[iz];
        double normSz;
        normSz = std::sqrt(Sz*Sz);

        // set constants for iteration loop
        double initnormS = 0.0;
        for (int i=0; i<6; i++)
            initnormS += S[i]*S[i];

        double minSz = tol*std::sqrt(initnormS);
        int iter = 0;
        double Ecz=0.0, Czz=0.0, Ezi=0.0;
        double Cc[6][6]={{0.0}};
        istensor Ec;
        while ( (normSz>minSz) && (iter<maxiter) )
        {

            F = theFiniteStrainMP->deformationGradient();
            Ec = 0.5 * ( istensor::tensorTransposedTimesTensor(F) - istensor::identity() );
            Ecz = coefE[iz] * Ec(voigt(0,iz),voigt(1,iz));
            theFiniteStrainMP->convectedTangentMatrix(Cc);

            Czz = Cc[iz][iz];
            if (std::abs(Czz)>=tol)
            {
                Ezi = Ecz - Sz/Czz;
            }
            else
            {
                break;
            }

            // new F
            Ec(voigt(0,iz),voigt(1,iz)) = Ec(voigt(1,iz), voigt(0,iz)) = Ezi/coefE[iz];
            FtF = 2.0*Ec + istensor::identity();
            Uc = istensor::squareRoot(FtF);
            F = R*Uc;


            theFiniteStrainMP->updateCurrentState(theTime, F);

            // Sz
            theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
            Sz = S[iz];
            normSz = std::sqrt(Sz*Sz);
            iter++;
        }

        if (iter>=maxiter) std::cout << " Reduced model (1 zero model) has not converged, normSz = " << normSz << std::endl;

    }
    else if (_method == _fnlcg)
    {
        vector<unsigned> _mapiz;
        _mapiz.push_back(mapiz[0]);

        nlcg(theTime,_mapiz);
    }
}




/*********************************************************************************
 reduced3zFMP
 **********************************************************************************/

reduced3zFMP::reduced3zFMP(finiteStrainMP *mp, unsigned inmapim[3], unsigned inmapiz[3]) :
muesli::rFiniteStrainMP(mp)
{
    for (unsigned i=0; i<3; i++)
        mapim[i] = inmapim[i];

    for (unsigned i=0; i<3; i++)
        mapiz[i] = inmapiz[i];
} 




reduced3zFMP::reduced3zFMP(finiteStrainMP *mp, unsigned inmapim[3], unsigned inmapiz[3], std::string method) :
muesli::rFiniteStrainMP(mp, method)
{
    for (unsigned i=0; i<3; i++)
        mapim[i] = inmapim[i];

    for (unsigned i=0; i<3; i++)
        mapiz[i] = inmapiz[i];
} 




reduced3zFMP::~reduced3zFMP()
{

}




void reduced3zFMP::convectedTangentMatrix(double C[6][6]) const
{
    itensor4 tg;
    theFiniteStrainMP->convectedTangent(tg);
    muesli::tensorToMatrix(tg, C);

    itensor Czz;
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            Czz(i,j) = C[mapiz[i]][mapiz[j]];
    itensor Cmz;
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            Cmz(i,j) = C[mapim[i]][mapiz[j]];
    itensor Czm;
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            Czm(i,j) = C[mapiz[i]][mapim[j]];
    itensor Cmm;
    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            Cmm(i,j) = C[mapim[i]][mapim[j]];

    itensor extraTg;
    extraTg = Cmz * Czz.inverse() * Czm;

    // new tangent
    for (unsigned i=0; i<6; i++)
        for (unsigned j=0; j<6; j++)
            C[i][j] = 0.0;

    for (unsigned i=0; i<3; i++)
        for (unsigned j=0; j<3; j++)
            C[mapim[i]][mapim[j]] = Cmm(i,j) - extraTg(i,j);

}




void reduced3zFMP::secondPiolaKirchhoffStressVector(double S[6]) const
{
    theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
}




void reduced3zFMP::updateCurrentState(const double theTime, itensor& F)
{
    theFiniteStrainMP->updateCurrentState(theTime, F);

    if (_method == _fnr)
    {
        F = theFiniteStrainMP->deformationGradient();
        // get R
        istensor FtF;
        FtF = istensor::tensorTransposedTimesTensor(F);
        istensor Uc;
        Uc = istensor::squareRoot(FtF);
        itensor R;
        R = F*Uc.inverse();

        double S[6];
        theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
        ivector Sz(S[mapiz[0]], S[mapiz[1]], S[mapiz[2]]);
        double normSz=0.0;
        normSz = Sz.norm();

        // set constants for iteration loop
        double initnormS = 0.0;
        for (unsigned i=0; i<6; i++) initnormS += S[i]*S[i];

        double minSz = tol * std::sqrt(initnormS);
        int iter = 0;
        ivector Ecz, Ezi;
        istensor Ec;
        istensor Czz; Czz.setZero();
        double Cc[6][6];

        bool condition1 = normSz > minSz;
        bool condition2 = iter < maxiter;

        while ( condition1 && condition2 )
        {
            F = theFiniteStrainMP->deformationGradient();

            Ec = 0.5 * ( istensor::tensorTransposedTimesTensor(F) - istensor::identity() );

            for (unsigned i=0; i<3; i++)
                Ecz(i) = coefE[mapiz[i]] * Ec(voigt(0,mapiz[i]),voigt(1,mapiz[i]));

            
            theFiniteStrainMP->convectedTangentMatrix(Cc);
            for (unsigned i=0; i<3; i++)
                for (unsigned j=0; j<3; j++)
                    Czz(i,j) = Cc[mapiz[i]][mapiz[j]];

            if (Czz.determinant()>=tol)
            {
                Ezi = Ecz - Czz.inverse()*Sz;
            }
            else
            {
                break;
            }
            // new F
            for (unsigned i=0; i<3; i++) Ec(voigt(0,mapiz[i]), voigt(1,mapiz[i])) = Ec(voigt(1,mapiz[i]),voigt(0,mapiz[i])) = Ezi(i)/coefE[mapiz[i]];

            FtF = 2.0*Ec + istensor::identity();
            Uc = istensor::squareRoot(FtF);
            F = R*Uc;
            theFiniteStrainMP->updateCurrentState(theTime, F);

            // Sz
            theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
            for (unsigned i=0; i<3; i++) Sz(i) = S[mapiz[i]];
            
            normSz = Sz.norm();
            iter++;

            condition1 = normSz > minSz;
            condition2 = iter < maxiter;
        }
    }
    else if (_method == _fnlcg)
    {
        vector<unsigned> _mapiz;
        for (unsigned i=0; i<3; i++) _mapiz.push_back(mapiz[i]);

        nlcg(theTime,_mapiz);
    }

}




/*********************************************************************************
 reduced5zFMP
 **********************************************************************************/

reduced5zFMP::reduced5zFMP(finiteStrainMP *mp, unsigned inmapim[1], unsigned inmapiz[5]) :
muesli::rFiniteStrainMP(mp)
{
    mapim[0] = inmapim[0];

    for (unsigned i=0; i<5; i++)    mapiz[i] = inmapiz[i];
} 




reduced5zFMP::reduced5zFMP(finiteStrainMP *mp, unsigned inmapim[1], unsigned inmapiz[5], std::string method) :
muesli::rFiniteStrainMP(mp, method)
{
    mapim[0] = inmapim[0];

    for (unsigned i=0; i<5; i++)    mapiz[i] = inmapiz[i];
} 




reduced5zFMP::~reduced5zFMP()
{

}




void reduced5zFMP::convectedTangentMatrix(double C[6][6]) const
{
    itensor4 tg;
    theFiniteStrainMP->convectedTangent(tg);
    muesli::tensorToMatrix(tg, C);
    // TO DO
    size_t const nz = 5;
    size_t const nm = 1;
    
    matrix Czz(nz,nz);
    for (unsigned i=0; i<nz; i++)
        for (unsigned j=0; j<nz; j++)
            Czz(i,j) = C[mapiz[i]][mapiz[j]];
    
    matrix Cmz(nm,nz);
    for (unsigned i=0; i<nm; i++)
        for (unsigned j=0; j<nz; j++)
            Cmz(i,j) = C[mapim[i]][mapiz[j]];

    matrix Czm(nz,nm);
    for (unsigned i=0; i<nz; i++)
        for (unsigned j=0; j<nm; j++)
            Czm(i,j) = C[mapiz[i]][mapim[j]];

    matrix Cmm(nm,nm);
    for (unsigned i=0; i<nm; i++)
        for (unsigned j=0; j<nm; j++)
            Cmm(i,j) = C[mapim[i]][mapim[j]];

    Czz.invert();
    matrix Czzinverse(nz,nz);
    Czzinverse = Czz;
    matrix extraTg(nm,nm);
    extraTg = Cmz * Czzinverse * Czm;

    // new tangent
    for (unsigned i=0; i<6; i++)
        for (unsigned j=0; j<6; j++)
            C[i][j] = 0.0;

    for (unsigned i=0; i<nm; i++)
        for (unsigned j=0; j<nm; j++)
            C[mapim[i]][mapim[j]] = Cmm(i,j) - extraTg(i,j);
   
    

}       




void reduced5zFMP::secondPiolaKirchhoffStressVector(double S[6]) const
{
    theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
}




void reduced5zFMP::updateCurrentState(const double theTime, itensor& F)
{
    theFiniteStrainMP->updateCurrentState(theTime, F);

    if (_method == _fnr)
    {
        // TO DO
        const size_t nz = 5;
        //const size_t nm = 1;
        // TO DO
        
        F = theFiniteStrainMP->deformationGradient();
        // get R
        istensor FtF;
        FtF = istensor::tensorTransposedTimesTensor(F);
        istensor Uc;
        Uc = istensor::squareRoot(FtF);
        itensor R;
        R = F*Uc.inverse();
   
        double S[6];
        theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
        realvector Sz(nz);
        for (unsigned i=0; i<nz; i++) Sz[i] = S[mapiz[i]];
        double normSz=0.0;
        normSz = Sz.norm();

        // set constants for iteration loop
        double initnormS = 0.0;
        for (unsigned i=0; i<6; i++) initnormS += S[i]*S[i];

        double minSz = tol * std::sqrt(initnormS);
        int iter = 0;
        realvector Ecz(nz), Ezi(nz);
        istensor Ec;    
        matrix Czz(nz,nz);  
        double Cc[6][6];

        bool condition1 = normSz > minSz;
        bool condition2 = iter < maxiter;

        while ( condition1 && condition2 )
        {
            F = theFiniteStrainMP->deformationGradient();

            Ec = 0.5 * ( istensor::tensorTransposedTimesTensor(F) - istensor::identity() );

            for (unsigned i=0; i<nz; i++)
                Ecz[i] = coefE[mapiz[i]] * Ec(voigt(0,mapiz[i]),voigt(1,mapiz[i]));
            
            theFiniteStrainMP->convectedTangentMatrix(Cc);
            for (unsigned i=0; i<nz; i++)
                for (unsigned j=0; j<nz; j++)
                    Czz(i,j) = Cc[mapiz[i]][mapiz[j]];

            if (abs(Czz.determinant())>0.0)
            {
                Czz.invert();
                Ezi = Ecz - Czz*Sz;
            }
            else
            {
                std::cout << "reducedfinitestrain.cpp det(Czz)=0, det(Czz) = " << Czz.determinant()<< std::endl;
                std::cout << "C = " << std::endl;
                for (unsigned i=0; i<6; i++)
                {
                    std::cout << "[" ;
                    for (unsigned j=0; j<6; j++)
                    {
                        std::cout << " " << Cc[i][j] << " " ;
                    }
                    std::cout << "]" << std::endl;
                }
                std::cout << "Czz = " << std::endl;
                for (unsigned i=0; i<nz; i++)
                {
                    std::cout << "[" ;
                    for (unsigned j=0; j<nz; j++)
                    {
                        std::cout << " " << Czz(i,j) << " " ;
                    }
                    std::cout << "]" << std::endl;
                }
                exit(-1);
                break;
            }
            // new F
            for (unsigned i=0; i<nz; i++) Ec(voigt(0,mapiz[i]), voigt(1,mapiz[i])) = Ec(voigt(1,mapiz[i]),voigt(0,mapiz[i])) = Ezi[i]/coefE[mapiz[i]];

            FtF = 2.0*Ec + istensor::identity();
            Uc = istensor::squareRoot(FtF);
            F = R*Uc;
            theFiniteStrainMP->updateCurrentState(theTime, F);

            // Sz
            theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
            for (unsigned i=0; i<nz; i++) Sz[i] = S[mapiz[i]];
            
            normSz = Sz.norm();
            iter++;

            condition1 = normSz > minSz;
            condition2 = iter < maxiter;
        }
    }
    else if (_method == _fnlcg)
    {
        vector<unsigned> _mapiz;
        for (size_t i=0; i<5; i++) _mapiz.push_back(mapiz[i]);

        nlcg(theTime,_mapiz);
    }

}




/*********************************************************************************
 fbeam
 **********************************************************************************/

unsigned mapim_fbeam[3] = {2, 3, 4};
unsigned mapiz_fbeam[3] = {0, 1, 5};

fbeamMP::fbeamMP(finiteStrainMP *mp) :
muesli::reduced3zFMP(mp,mapim_fbeam,mapiz_fbeam)
{
} 




fbeamMP::fbeamMP(finiteStrainMP *mp, std::string method) :
muesli::reduced3zFMP(mp,mapim_fbeam,mapiz_fbeam, method)
{
} 




fbeamMP::~fbeamMP()
{

}




/*********************************************************************************
 fshell
 **********************************************************************************/

unsigned mapim_fshell[5] = {0, 1, 3, 4, 5};
unsigned mapiz_fshell[1] = {2};

fshellMP::fshellMP(finiteStrainMP *mp) :
muesli::reduced1zFMP(mp,mapim_fshell,mapiz_fshell)
{
} 




fshellMP::fshellMP(finiteStrainMP *mp, std::string method) :
muesli::reduced1zFMP(mp,mapim_fshell,mapiz_fshell, method)
{
} 




fshellMP::~fshellMP()
{

}




/*********************************************************************************
 fplane
 **********************************************************************************/

unsigned mapim_fplane[3] = {0, 1, 5};
unsigned mapiz_fplane[3] = {2, 3, 4};

fplaneMP::fplaneMP(finiteStrainMP *mp) :
muesli::reduced3zFMP(mp,mapim_fplane,mapiz_fplane)
{
} 




fplaneMP::fplaneMP(finiteStrainMP *mp, std::string method) :
muesli::reduced3zFMP(mp,mapim_fplane,mapiz_fplane, method)
{
} 




fplaneMP::~fplaneMP()
{

}




/*********************************************************************************
 fplane
 **********************************************************************************/

unsigned mapim_fbar[1] = {0};
unsigned mapiz_fbar[5] = {1, 2, 3, 4, 5};

fbarMP::fbarMP(finiteStrainMP *mp) :
muesli::reduced5zFMP(mp,mapim_fbar,mapiz_fbar)
{
} 




fbarMP::fbarMP(finiteStrainMP *mp, std::string method) :
muesli::reduced5zFMP(mp,mapim_fbar,mapiz_fbar, method)
{
} 




fbarMP::~fbarMP()
{

}




double fcomputeBeta(vector<double> dx0, vector<double> dx, vector<double> s)
{
    // Polak-Ribere
    size_t n = dx0.size();

    double den = 0.0;
    for (size_t i=0; i<n; i++) den += dx0[i]*dx0[i];

    if (den > tol)
    {
        double num=0.0;

        //for (size_t i=0; i<n; i++) num+=dx[i]*dx[i];
        for (size_t i=0; i<n; i++) num+=dx[i]*(dx[i]-dx0[i]);

        return std::max(0.0, num/den);
        //return 0.0;
    }
    else
        return 0.0;
}




double rFiniteStrainMP::linesearch(const double theTime, vector<double> d, vector<unsigned> mapiz)
{
    //const double tolfa = 1.0e-16;
    const int itmax = 100;
    size_t n = mapiz.size();

    double normS = 1.0;


    itensor F;
    F = theFiniteStrainMP->deformationGradient();
    istensor FtF;
    FtF = istensor::tensorTransposedTimesTensor(F);
    istensor Uc;
    Uc = istensor::squareRoot(FtF);
    itensor R;
    R = F*Uc.inverse();

    istensor strain;
    strain = 0.5*(istensor::tensorTransposedTimesTensor(F) - istensor::identity());

    // Armijo
    double alpha = 1.0;
    double eta = 2.0;
    double eps = 1.0e-4;
    double c2  = 1.0e-1;

    double f0;
    f0 = theFiniteStrainMP->effectiveStoredEnergy();

    double S[6];
    theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
    vector<double> gradE;
    for (size_t i=0; i<n; i++) gradE.push_back(S[mapiz[i]]/normS);

    double normalpha = theFiniteStrainMP->parentMaterial().getProperty(PR_BULK);
    //double normgradE = 0.0;
    //for (size_t i=0; i<n; i++) normgradE += gradE[i]*gradE[i];
    //normgradE = sqrt(normgradE);
    //if (normgradE>=1.0) normalpha = normgradE;
    alpha /= normalpha;

    double fpr0 = 0.0;
    for (size_t i=0; i<n; i++) fpr0 += gradE[i]*d[i];

    vector<double> strain0;
    for (size_t i=0; i<n; i++) strain0.push_back(strain(voigt(0,mapiz[i]),voigt(1,mapiz[i])));

    vector<double> inc;
    for (size_t i=0; i<n; i++) inc.push_back(alpha * d[i]);

    for (size_t i=0; i<n; i++) strain(voigt(0,mapiz[i]),voigt(1,mapiz[i])) = strain(voigt(1,mapiz[i]),voigt(0,mapiz[i])) = strain0[i] + inc[i];

    FtF = 2.0*strain + istensor::identity();
    Uc = istensor::squareRoot(FtF);
    F = R*Uc;
    theFiniteStrainMP -> updateCurrentState(theTime, F);

    double fa;
    fa = theFiniteStrainMP->effectiveStoredEnergy();


    theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
    for (size_t i=0; i<n; i++) gradE[i]=(S[mapiz[i]]/normS);
    double fpra = 0.0;
    for (size_t i=0; i<n; i++) fpra += gradE[i]*d[i];


    int it=0;

    //double fa0 = fa;
    //double fan = fa;

    bool condition1 = fa - (f0 + eps*fpr0*alpha) > 1.0e-6*fa;
    bool condition2 = fabs(fpra) > fabs(c2*fpr0);

    double alphal = alpha;
    double fprl = fpra;
    bool condition1l = true;
    while ( (condition1 || condition2) && (it<itmax))
    {
        alpha /= eta;
        for (size_t i=0; i<n; i++) inc[i] = alpha * d[i];
        
        for (size_t i=0; i<n; i++) strain(voigt(0,mapiz[i]),voigt(1,mapiz[i])) = strain(voigt(1,mapiz[i]),voigt(0,mapiz[i])) = strain0[i] + inc[i];

        FtF = 2.0*strain + istensor::identity();
        Uc = istensor::squareRoot(FtF);
        F = R*Uc;
        theFiniteStrainMP -> updateCurrentState(theTime, F);

        fa = theFiniteStrainMP->effectiveStoredEnergy();

        theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
        for (size_t i=0; i<n; i++) gradE[i]=(S[mapiz[i]]/normS);
        fpra = 0.0;
        for (size_t i=0; i<n; i++) fpra += gradE[i]*d[i];

        condition1 = fa - (f0 + eps*fpr0*alpha) > 1.0e-6*fa;
        condition2 = fabs(fpra) > fabs(c2*fpr0);

        if (!condition1 && !condition1l && condition2)
        {
            if (fabs(fpra)>fabs(fprl))
            {
                // the solution must be between (alpha, alphal]
                alpha = (alpha+alphal);
                condition1l = false;
            }
            else
            {
                if (fpra*fprl>0)
                {
                    // the solution must be between (
                    alphal = alpha;
                    fprl = fpra;
                    condition1l = condition1;
                }
                else
                {
                    // the solution must be between (alpha, alphal]
                    alpha = (alpha+alphal);
                    condition1l = false;
                }
            }
        }
        else
        {
            alphal = alpha;
            fprl = fpra;
            condition1l = condition1;
        }
        

        it ++;
    }
    
//    if (it>=itmax)    std::cout << "Error in back linesearch fa, fa - (f0+eps*fpr0*alpha), fpra, c2*fpr0 = " << fa << " , " << fa-(f0+eps*fpr0*alpha)  << " ; " << fpra << " , " << c2*fpr0 << std::endl;


    for (size_t i=0; i<n; i++) strain(voigt(0,mapiz[i]),voigt(1,mapiz[i])) = strain(voigt(1,mapiz[i]),voigt(0,mapiz[i])) = strain0[i];

    FtF = 2.0*strain + istensor::identity();
    Uc = istensor::squareRoot(FtF);
    F = R*Uc;
    theFiniteStrainMP -> updateCurrentState(theTime, F);

    return alpha;
}




void rFiniteStrainMP::nlcg(const double theTime, vector<unsigned> mapiz)
{
    int const maxit = 100;
    size_t n = mapiz.size();

    double normS = 1.0;//theFiniteStrainMP->parentMaterial().getProperty(PR_BULK);

    itensor F;
    F = theFiniteStrainMP->deformationGradient();
    istensor FtF;
    FtF = istensor::tensorTransposedTimesTensor(F);
    istensor Uc;
    Uc = istensor::squareRoot(FtF);
    itensor R;
    R = F*Uc.inverse();

    istensor strain;
    strain = 0.5*(istensor::tensorTransposedTimesTensor(F) - istensor::identity());

    double S[6];
    theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);

    double beta;

    vector<double> x;
    for (size_t i=0; i<n; i++) x.push_back(strain(voigt(0,mapiz[i]),voigt(1,mapiz[i])));

    vector<double> dx0;
    for (size_t i=0; i<n; i++) dx0.push_back(-S[mapiz[i]]/normS);

    vector<double> dv;
    for (size_t i=0; i<n; i++) dv.push_back(dx0[i]);

    double alpha;
    alpha = linesearch(theTime,dv,mapiz);

    for (size_t i=0; i<n; i++) x[i] = x[i] + alpha*dx0[i];

    for (size_t i=0; i<n; i++) strain(voigt(0,mapiz[i]),voigt(1,mapiz[i])) = strain(voigt(1,mapiz[i]),voigt(0,mapiz[i])) = x[i];

    // update current state
    FtF = 2.0*strain + istensor::identity();
    Uc = istensor::squareRoot(FtF);
    F = R*Uc;
    theFiniteStrainMP -> updateCurrentState(theTime, F);

    vector<double> s;
    for (size_t i=0; i<n ; i++) s.push_back(dx0[i]);

    theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);
    vector<double> dx;
    for (size_t i=0; i<n; i++) dx.push_back(-S[mapiz[i]]/normS);

    int it = 0;

    double toldx=0.0;
    for (size_t i=0; i<n; i++) toldx += dx0[i]*dx0[i];
    toldx = sqrt(toldx);
    toldx *= 1.0e-6;
    toldx = max(1.0e-16,toldx);

    double dxnorm = 0.0;
    for (size_t i=0; i<n; i++) dxnorm+= dx[i]*dx[i];
    dxnorm = sqrt(dxnorm);

    while ((abs(dxnorm) >= toldx) && (it<maxit))
    {

        beta = fcomputeBeta(dx0,dx,s);
        for (size_t i=0; i<n ; i++) s[i] = dx[i] + beta*s[i];

        double fp = 0.0;
        for (size_t i=0; i<n ; i++) fp += -dx[i]*s[i];

        if (fp>0.0)
        {
            // restart search direction
            for (size_t i=0; i<n; i++) s[i] = dx[i];
        }

        alpha = linesearch(theTime,s,mapiz);

        for (size_t i=0; i<n; i++)
        {
            x[i] = x[i] + alpha*s[i];
            strain(voigt(0,mapiz[i]),voigt(1,mapiz[i])) = strain(voigt(1,mapiz[i]),voigt(0,mapiz[i])) = x[i];
            dx0[i] = dx[i];
        }

        // update current state
        FtF = 2.0*strain + istensor::identity();
        Uc = istensor::squareRoot(FtF);
        F = R*Uc;
        theFiniteStrainMP -> updateCurrentState(theTime, F);
        theFiniteStrainMP->secondPiolaKirchhoffStressVector(S);

        for (size_t i=0; i<n; i++) dx[i] = -S[mapiz[i]]/normS;

        dxnorm = 0.0;
        for (size_t i=0; i<n; i++) dxnorm+= dx[i]*dx[i];
        dxnorm = sqrt(dxnorm);

        it ++;
    }

    //if (it>=maxit)    std::cout << "Error in nlcg" << std::endl;

}
