/****************************************************************************
*
*                                 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 "reducedsmallstrain.h"
#include <iostream>
#include <cmath>

/*
 *
 * 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
 *
 */

using namespace muesli;
using namespace std;

const int maxiter = 50;
const double tol = 1.0e-15;
const double tolzero = 1.0e-16;
const double coefE[6] ={1.0, 1.0, 1.0, 2.0, 2.0, 2.0};

double scomputeBeta(const vector<double>& dx0, const vector<double>& dx, const vector<double>& s);



rSmallStrainMP::rSmallStrainMP(smallStrainMP *mp)
:
    _method(_snr),
    _theSmallStrainMP(mp)
{
}




rSmallStrainMP::rSmallStrainMP(smallStrainMP *mp, std::string method)
:
_method(_snr),
_theSmallStrainMP(mp)
{
    if (method == "nlcg")   _method = _snlcg;
}




void rSmallStrainMP::commitCurrentState()
{
    _theSmallStrainMP->commitCurrentState();
}




void rSmallStrainMP::deviatoricStress(istensor& s) const
{
    istensor sigma;
    this->stress(sigma);
    s = istensor::deviatoricPart(sigma);
}




double rSmallStrainMP::density() const
{
    return _theSmallStrainMP->density();
}




double rSmallStrainMP::deviatoricEnergy() const
{
    return _theSmallStrainMP->deviatoricEnergy();
}




double rSmallStrainMP::dissipatedEnergy() const
{
    return _theSmallStrainMP->dissipatedEnergy();
}




double rSmallStrainMP::effectiveStoredEnergy() const
{
    return _theSmallStrainMP->effectiveStoredEnergy();
}




// necessary to compute in smallthermo material the dissipated energy
istensor rSmallStrainMP::getConvergedPlasticStrain() const
{
    return istensor();
}




materialState rSmallStrainMP::getConvergedState() const
{
    return _theSmallStrainMP->getConvergedState();
}




// necessary to compute in smallthermo material the dissipated energy
istensor rSmallStrainMP::getCurrentPlasticStrain() const
{
    return istensor();
}




materialState rSmallStrainMP::getCurrentState() const
{
    return _theSmallStrainMP->getCurrentState();
}




istensor& rSmallStrainMP::getCurrentStrain()
{
    return _theSmallStrainMP->getCurrentStrain();
}




const istensor& rSmallStrainMP::getCurrentStrain() const
{
    return _theSmallStrainMP->getCurrentStrain();
}




double rSmallStrainMP::plasticSlip() const
{
    return _theSmallStrainMP->plasticSlip();
}




double rSmallStrainMP::pressure() const
{
    return _theSmallStrainMP->pressure();
}




void rSmallStrainMP::resetCurrentState()
{
    _theSmallStrainMP->resetCurrentState();
}




void rSmallStrainMP::setRandom()
{
    _theSmallStrainMP->setRandom();
}




void rSmallStrainMP::stress(istensor& s) const
{
    _theSmallStrainMP->stress(s);
}




double rSmallStrainMP::storedEnergy() const
{
    return _theSmallStrainMP->storedEnergy();
}




smallStrainMP& rSmallStrainMP::theSmallStrainMP()
{
    return *_theSmallStrainMP;
}




thPotentials rSmallStrainMP::thermodynamicPotentials() const
{
    return _theSmallStrainMP->thermodynamicPotentials();
}




double rSmallStrainMP::volumetricEnergy() const
{
    return _theSmallStrainMP->volumetricEnergy();
}




bool rSmallStrainMP::testImplementation(std::ostream& os) const
{
    bool isok = true;

    // set a random update in the material
    istensor eps;
    eps.setZero();

    rSmallStrainMP* ssmp = const_cast<rSmallStrainMP*>(this);
    ssmp->updateCurrentState(0.0, eps);
    ssmp->commitCurrentState();

    istensor epsaux;
    epsaux.setRandom();
    eps += 1.0e-2 * epsaux;
    double tn1 = muesli::randomUniform(0.1,1.0);
    //eps.setRandom();

    rSmallStrainMP& theMP = const_cast<rSmallStrainMP&>(*this);;

    theMP.updateCurrentState(tn1, eps);


    // (1)  compare DEnergy with the derivative of Energy
    // programmed stress
    istensor sigma;
    _theSmallStrainMP->stress(sigma);

    
    // numerical differentiation stress
    istensor numSigma;
    numSigma.setZero();
    const double   inc = 1.0e-5*eps.norm();

    // numeric C
    itensor4 nC;
    nC.setZero();

    // numerical differentiation sigma
    istensor dsigma, sigmap1, sigmap2, sigmam1, sigmam2;

    for (size_t i=0; i<3; i++)
    {
        for (size_t j=i; j<3; j++)
        {
            double original = eps(i,j);

            eps(i,j) = eps(j,i) = original + inc;
            theMP.updateCurrentState(tn1, eps);
            stress(sigmap1);
            double Wp1 = effectiveStoredEnergy();

            eps(i,j) = eps(j,i) = original + 2.0*inc;
            theMP.updateCurrentState(tn1, eps);
            stress(sigmap2);
            double Wp2 = effectiveStoredEnergy();

            eps(i,j) = eps(j,i) = original - inc;
            theMP.updateCurrentState(tn1, eps);
            stress(sigmam1);
            double Wm1 = effectiveStoredEnergy();

            eps(i,j) = eps(j,i) = original - 2.0*inc;
            theMP.updateCurrentState(tn1, eps);
            stress(sigmam2);
            double Wm2 = effectiveStoredEnergy();

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

            numSigma(j,i) = numSigma(i,j);

            // fourth order approximation of the derivative
            dsigma = (-sigmap2 + 8.0*sigmap1 - 8.0*sigmam1 + sigmam2)/(12.0*inc);

            if (i != j) dsigma *= 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) = dsigma(k,l);

            eps(i,j) = eps(j,i) = original;
            theMP.updateCurrentState(tn1, eps);
        }
    }

    // DE
    if (true)
    {
        // relative error less than 0.01%
        istensor error = numSigma - sigma;
        isok = (error.norm()/sigma.norm() < 1e-2);


        os << "\n   1. Comparing stress with DWeff (reduced model).";
        if (isok)
        {
            os << " Test passed.";
        }
        else
        {
            os << "\n      Test failed.";
            os << "\n      Relative error in DWeff computation: " <<  error.norm()/sigma.norm();
            os << "\n      Stress:\n" << sigma;
            os << "\n      Numeric stress:\n" << numSigma;
        }
    }

    // DDE
    if (_method == _snr)
    {
        // get the voigt notation
        double nCv[6][6];
        muesli::tensorToMatrix(nC,nCv);

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

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

        os << "\n   2. Comparing tensor C with DStress (reduced model).";
        if (isok)
        {
            os << " Test passed.";
        }
        else
        {
            os << " nCv = " << std::endl;
            for (int i=0;i <6 ; i++)
            {
                os << " [ " << nCv[i][0];
                for (int j=1; j<6; j++)
                    os << " , " << nCv[i][j];
                os << " ] " << std::endl;
            }
            os << " tgv = " << std::endl;
            for (int i=0;i <6 ; i++)
            {
                os << " [ " << tgv[i][0];
                for (int j=1; j<6; j++)
                    os << " , " << tgv[i][j];

                os << " ] " << std::endl;
            }
            os << "\n      Test failed.";
            os << "\n      Relative error in DWeff computation: " <<  errordde/normdde;
        }
    }

    return isok;
}




double rSmallStrainMP::waveVelocity() const
{
    return _theSmallStrainMP->waveVelocity();
}




/*********************************************************************************
 reduced1zSMP
 **********************************************************************************/

reduced1zSMP::reduced1zSMP(smallStrainMP *mp, unsigned inmapim[5], unsigned inmapiz[1]) :
muesli::rSmallStrainMP(mp)
{
    for (unsigned i=0; i<5; i++)
        mapim[i] = inmapim[i];

    mapiz[0] = inmapiz[0];
} 




reduced1zSMP::reduced1zSMP(smallStrainMP *mp, unsigned inmapim[5], unsigned inmapiz[1], const std::string& method) :
muesli::rSmallStrainMP(mp, method)
{
    for (unsigned i=0; i<5; i++)
        mapim[i] = inmapim[i];

    mapiz[0] = inmapiz[0];
} 




reduced1zSMP::~reduced1zSMP()
{

}




void reduced1zSMP::tangentMatrix(double C[6][6]) const
{
    itensor4 tg;
    _theSmallStrainMP->tangentTensor(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 reduced1zSMP::stressVector(double S[6]) const
{
    _theSmallStrainMP->stressVector(S);
}
*/



void reduced1zSMP::updateCurrentState(const double theTime, istensor& strain)
{
    _theSmallStrainMP->updateCurrentState(theTime, strain);

    if (_method == _snr)
    {
        strain = _theSmallStrainMP->getCurrentStrain();

        unsigned iz = mapiz[0];
        double S[6];
        istensor stressT;
        _theSmallStrainMP->stress(stressT);
        ContraContraSymTensorToVector(stressT, S);

        double Sz; // shells
        Sz = S[iz]; // Sz = S33
        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, Czz, Ezi;
        double Cc[6][6];
        istensor Ec;

        while ( (normSz>minSz) && (iter<maxiter) )
        {
            strain = _theSmallStrainMP->getCurrentStrain();

            Ecz = coefE[iz] * strain(voigt(0,iz),voigt(1,iz));
            _theSmallStrainMP->tangentMatrix(Cc);
            Czz = Cc[iz][iz];
            if (std::abs(Czz)>=tolzero)
            {
                Ezi = Ecz - Sz/Czz;
            }
            else
            {
                break;
            }
            // new Fc
            strain(voigt(0,iz),voigt(1,iz)) = strain(voigt(1,iz),voigt(0,iz)) = Ezi/coefE[iz];
            _theSmallStrainMP->updateCurrentState(theTime, strain);

            // Sz
            istensor stressT;
            _theSmallStrainMP->stress(stressT);
            ContraContraSymTensorToVector(stressT, 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/initnormS << std::endl;

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

        nlcg(theTime,_mapiz);
    }
}




/*********************************************************************************
 reduced3zSMP
 **********************************************************************************/

reduced3zSMP::reduced3zSMP(smallStrainMP *mp, unsigned inmapim[3], unsigned inmapiz[3]) :
muesli::rSmallStrainMP(mp)
{
    for (unsigned i=0; i<3; i++)
        mapim[i] = inmapim[i];

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




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

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




reduced3zSMP::~reduced3zSMP()
{

}




void reduced3zSMP::tangentMatrix(double C[6][6]) const
{
    itensor4 tg;
    _theSmallStrainMP->tangentTensor(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 reduced3zSMP::stressVector(double S[6]) const
{
    _theSmallStrainMP->stressVector(S);
}
*/



void reduced3zSMP::updateCurrentState(const double theTime, istensor& strain)
{
    _theSmallStrainMP->updateCurrentState(theTime, strain);

    strain = _theSmallStrainMP->getCurrentStrain();

    if (_method == _snr)
    {
        double S[6];
        istensor stressT;
        _theSmallStrainMP->stress(stressT);
        ContraContraSymTensorToVector(stressT, S);

        ivector Sz(S[mapiz[0]], S[mapiz[1]], S[mapiz[2]]); // beams
        double normSz;
        normSz = Sz.norm();

        // 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);
        minSz = max(minSz, tolzero);

        int iter = 0;
        ivector Ecz, Ezi;
        itensor Czz;
        Czz.setZero();
        double Cc[6][6];
        istensor Ec;
        ivector Saux;

        while ( (normSz>minSz) && (iter<maxiter) )
        {
            strain = _theSmallStrainMP->getCurrentStrain();

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

            _theSmallStrainMP->tangentMatrix(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 eps_c
            //strain = istensor(strain(0,0),Ezi(0),Ezi(1),0.5*Ezi(2),strain(0,2),strain(0,1));
            for (unsigned i=0; i<3; i++)    strain(voigt(0,mapiz[i]), voigt(1,mapiz[i])) = strain(voigt(1,mapiz[i]),voigt(0,mapiz[i])) = Ezi(i)/coefE[mapiz[i]];

            _theSmallStrainMP->updateCurrentState(theTime, strain);

            // Sz
            istensor stressT;
            _theSmallStrainMP->stress(stressT);
            ContraContraSymTensorToVector(stressT, S);
            for (unsigned i=0; i<3; i++)    Sz(i) = S[mapiz[i]];

            normSz = Sz.norm();


            iter++;

        }

        if (iter>=maxiter)  
        {
            std::cout << " Reduced model (3 zero model) has not converged, normSz, minSz " << std::scientific << normSz << " , " << minSz << std::endl;
        }
    }
    else if (_method == _snlcg)
    {
        vector<unsigned> _mapiz;
        for (unsigned i=0; i<3; i++) _mapiz.push_back(mapiz[i]);

        nlcg(theTime,_mapiz);

    }

}




/*********************************************************************************
 reduced5zSMP
 **********************************************************************************/

reduced5zSMP::reduced5zSMP(smallStrainMP *mp, unsigned inmapim[1], unsigned inmapiz[5]) :
muesli::rSmallStrainMP(mp)
{
    mapim[0] = inmapim[0];

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




reduced5zSMP::reduced5zSMP(smallStrainMP *mp, unsigned inmapim[1], unsigned inmapiz[5], std::string method) :
muesli::rSmallStrainMP(mp, method)
{
    mapim[0] = inmapim[0];

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




reduced5zSMP::~reduced5zSMP()
{

}




void reduced5zSMP::tangentMatrix(double C[6][6]) const
{
    itensor4 tg;
    _theSmallStrainMP->tangentTensor(tg);
    muesli::tensorToMatrix(tg, C);

    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 reduced5zSMP::stressVector(double S[6]) const
{
    _theSmallStrainMP->stressVector(S);
}
*/



void reduced5zSMP::updateCurrentState(const double theTime, istensor& strain)
{
    _theSmallStrainMP->updateCurrentState(theTime, strain);

    if (_method == _snr)
    {
        const size_t nz = 5;
        //const size_t nm = 1;

        double S[6];
        istensor stressT;
        _theSmallStrainMP->stress(stressT);
        ContraContraSymTensorToVector(stressT, 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 )
        {
            strain = _theSmallStrainMP->getCurrentStrain();

            for (unsigned i=0; i<nz; i++)
                Ecz[i] = coefE[mapiz[i]] * strain(voigt(0,mapiz[i]),voigt(1,mapiz[i]));
            
            _theSmallStrainMP->tangentMatrix(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 << "reducedsmallstrain.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 eps_c
            for (unsigned i=0; i<nz; i++) strain(voigt(0,mapiz[i]), voigt(1,mapiz[i])) = strain(voigt(1,mapiz[i]),voigt(0,mapiz[i])) = Ezi[i]/coefE[mapiz[i]];

            _theSmallStrainMP->updateCurrentState(theTime, strain);

            // Sz
            istensor stressT;
            _theSmallStrainMP->stress(stressT);
            ContraContraSymTensorToVector(stressT, 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 == _snlcg)
    {
        vector<unsigned> _mapiz;
        for (size_t i=0; i<5; i++) _mapiz.push_back(mapiz[i]);

        nlcg(theTime,_mapiz);
    }

}




/*********************************************************************************
 sbeam
 **********************************************************************************/

//unsigned mapim_beam[3] = {0, 4, 5};
//unsigned mapiz_beam[3] = {1, 2, 3};

unsigned mapim_beam[3] = {2, 3, 4};
unsigned mapiz_beam[3] = {0, 1, 5};

sbeamMP::sbeamMP(smallStrainMP *mp) :
muesli::reduced3zSMP(mp,mapim_beam,mapiz_beam)
{
} 




sbeamMP::sbeamMP(smallStrainMP *mp, std::string method) :
muesli::reduced3zSMP(mp,mapim_beam,mapiz_beam, method)
{
} 




sbeamMP::~sbeamMP()
{
}




/*********************************************************************************
 sshell
 **********************************************************************************/

unsigned mapim_shell[5] = {0, 1, 3, 4, 5};
unsigned mapiz_shell[1] = {2};

sshellMP::sshellMP(smallStrainMP *mp) :
muesli::reduced1zSMP(mp, mapim_shell, mapiz_shell)
{
} 




sshellMP::sshellMP(smallStrainMP *mp, std::string method) :
muesli::reduced1zSMP(mp, mapim_shell, mapiz_shell, method)
{
} 




sshellMP::~sshellMP()
{
}




/*********************************************************************************
 splane
 **********************************************************************************/

unsigned mapim_plane[3] = {0, 1, 5};
unsigned mapiz_plane[3] = {2, 3, 4};

splaneMP::splaneMP(smallStrainMP *mp) :
muesli::reduced3zSMP(mp,mapim_plane,mapiz_plane)
{
} 




splaneMP::splaneMP(smallStrainMP *mp, std::string method) :
muesli::reduced3zSMP(mp,mapim_plane,mapiz_plane, method)
{
} 




splaneMP::~splaneMP()
{
}




/*********************************************************************************
 sbar
 **********************************************************************************/

unsigned mapim_sbar[1] = {0};
unsigned mapiz_sbar[5] = {1, 2, 3, 4, 5};

sbarMP::sbarMP(smallStrainMP *mp) :
muesli::reduced5zSMP(mp,mapim_sbar,mapiz_sbar)
{
} 




sbarMP::sbarMP(smallStrainMP *mp, std::string method) :
muesli::reduced5zSMP(mp,mapim_sbar,mapiz_sbar, method)
{
} 




sbarMP::~sbarMP()
{
}




double scomputeBeta(const vector<double>& dx0, const vector<double>& dx, const 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 > tolzero)
    {
        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 rSmallStrainMP::linesearch(const double theTime, vector<double> d, vector<unsigned> mapiz)
{
    const unsigned itmax = 100;
    size_t n = mapiz.size();

    double normS = 1.0;
   
    istensor strain;
    strain = _theSmallStrainMP->getCurrentStrain();

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

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

    double S[6];
    istensor stressT;
    _theSmallStrainMP->stress(stressT);
    ContraContraSymTensorToVector(stressT, S);

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

    double normalpha = _theSmallStrainMP->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];
    _theSmallStrainMP->updateCurrentState(theTime, strain);

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

    _theSmallStrainMP->stress(stressT);
    ContraContraSymTensorToVector(stressT, 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))
    //while ( (condition1) && (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];
        _theSmallStrainMP->updateCurrentState(theTime, strain);

        fa = _theSmallStrainMP->effectiveStoredEnergy();

        istensor stressT;
        _theSmallStrainMP->stress(stressT);
        ContraContraSymTensorToVector(stressT, 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;
        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" << 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];
    _theSmallStrainMP->updateCurrentState(theTime, strain);

    return alpha;
}




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

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

    istensor strain;
    strain = _theSmallStrainMP->getCurrentStrain();
    double S[6];
    istensor stressT;
    _theSmallStrainMP->stress(stressT);
    ContraContraSymTensorToVector(stressT, 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];
    _theSmallStrainMP->updateCurrentState(theTime, strain);

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

    _theSmallStrainMP->stress(stressT);
    ContraContraSymTensorToVector(stressT, 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 *= tol;
    toldx = max(tolzero, 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 = scomputeBeta(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];
        }

        _theSmallStrainMP->updateCurrentState(theTime, strain);
        _theSmallStrainMP->stress(stressT);
        ContraContraSymTensorToVector(stressT, 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;

}
