AbstractNonlinearElasticitySolver.hpp

/*

Copyright (c) 2005-2017, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef ABSTRACTNONLINEARELASTICITYSOLVER_HPP_
#define ABSTRACTNONLINEARELASTICITYSOLVER_HPP_

#include <vector>
#include <cmath>
#include "AbstractContinuumMechanicsSolver.hpp"
#include "LinearSystem.hpp"
#include "LogFile.hpp"
#include "MechanicsEventHandler.hpp"
#include "ReplicatableVector.hpp"
#include "FourthOrderTensor.hpp"
#include "CmguiDeformedSolutionsWriter.hpp"
#include "AbstractMaterialLaw.hpp"
#include "QuadraticBasisFunction.hpp"
#include "SolidMechanicsProblemDefinition.hpp"
#include "Timer.hpp"
#include "AbstractPerElementWriter.hpp"
#include "petscsnes.h"

//#define MECH_USE_HYPRE    // uses HYPRE to solve linear systems, requires PETSc to be installed with HYPRE

typedef enum StrainType_
{
    DEFORMATION_GRADIENT_F = 0,
    DEFORMATION_TENSOR_C,
    LAGRANGE_STRAIN_E
} StrainType;

// Bizarrely PETSc 2.2 has this, but doesn't put it in the petscksp.h header...
#if (PETSC_VERSION_MAJOR == 2 && PETSC_VERSION_MINOR == 2) //PETSc 2.2
extern PetscErrorCode KSPInitialResidual(KSP,Vec,Vec,Vec,Vec,Vec);
#endif

//  Globals functions used by the SNES solver

template<unsigned DIM>
PetscErrorCode AbstractNonlinearElasticitySolver_ComputeResidual(SNES snes,
                                                                 Vec currentGuess,
                                                                 Vec residualVector,
                                                                 void* pContext);

#if ((PETSC_VERSION_MAJOR==3) && (PETSC_VERSION_MINOR>=5))

    template<unsigned DIM>
    PetscErrorCode AbstractNonlinearElasticitySolver_ComputeJacobian(SNES snes,
                                                                     Vec currentGuess,
                                                                     Mat globalJacobian,
                                                                     Mat preconditioner,
                                                                     void* pContext);
#else

template<unsigned DIM>
PetscErrorCode AbstractNonlinearElasticitySolver_ComputeJacobian(SNES snes,
                                                                 Vec currentGuess,
                                                                 Mat* pGlobalJacobian,
                                                                 Mat* pPreconditioner,
                                                                 MatStructure* pMatStructure,
                                                                 void* pContext);
#endif

template <unsigned DIM>
class  AbstractNonlinearElasticitySolver; //Forward declaration

template<unsigned DIM>
class StressPerElementWriter : public AbstractPerElementWriter<DIM, DIM, DIM*DIM>
{
private:
    AbstractNonlinearElasticitySolver<DIM>* mpSolver; 
public:

    StressPerElementWriter(AbstractTetrahedralMesh<DIM, DIM>* pMesh,
                           AbstractNonlinearElasticitySolver<DIM>* pSolver)
     : AbstractPerElementWriter<DIM, DIM, DIM*DIM>(pMesh),
       mpSolver(pSolver)
    {
    }

    void Visit(Element<DIM, DIM>* pElement, unsigned localIndex, c_vector<double, DIM*DIM>& rData)
    {
        assert(localIndex == pElement->GetIndex()); //Will fail when we move to DistributedQuadraticMesh
        //Flatten the matrix
        c_matrix<double, DIM, DIM> data = mpSolver->GetAverageStressPerElement(localIndex);
        for (unsigned i=0; i<DIM; i++)
        {
            for (unsigned j=0; j<DIM; j++)
            {
                rData[i*DIM+j] = data(i,j);
            }
        }
    }
};

template<unsigned DIM>
class AbstractNonlinearElasticitySolver : public AbstractContinuumMechanicsSolver<DIM>
{
    friend class StressRecoveror<DIM>;
    friend class VtkNonlinearElasticitySolutionWriter<DIM>;


protected:

    static const size_t NUM_VERTICES_PER_ELEMENT = DIM+1;

    static const size_t NUM_NODES_PER_ELEMENT = (DIM+1)*(DIM+2)/2;      // assuming quadratic

    static const size_t NUM_NODES_PER_BOUNDARY_ELEMENT = DIM*(DIM+1)/2; // assuming quadratic

    static const size_t BOUNDARY_STENCIL_SIZE = DIM*NUM_NODES_PER_BOUNDARY_ELEMENT;

    static double MAX_NEWTON_ABS_TOL;

    static double MIN_NEWTON_ABS_TOL;

    static double NEWTON_REL_TOL;

    SolidMechanicsProblemDefinition<DIM>& mrProblemDefinition;

    Mat& mrJacobianMatrix;

    c_matrix<double,DIM,DIM> mChangeOfBasisMatrix;

    double mKspAbsoluteTol;

    bool mWriteOutputEachNewtonIteration;

    FourthOrderTensor<DIM,DIM,DIM,DIM> dTdE;

    unsigned mNumNewtonIterations;

    double mCurrentTime;


    bool mCheckedOutwardNormals;

    bool mUseSnesSolver;

    double mLastDampingValue;

    bool mFirstStep;

    bool mTakeFullFirstNewtonStep;

    bool mPetscDirectSolve;

    bool mIncludeActiveTension;

    bool mSetComputeAverageStressPerElement;

    std::vector<c_vector<double,DIM*(DIM+1)/2> > mAverageStressesPerElement;

    void AddStressToAverageStressPerElement(c_matrix<double,DIM,DIM>& rT, unsigned elementIndex);

    virtual void SetKspSolverAndPcType(KSP solver);


    virtual void AssembleSystem(bool assembleResidual, bool assembleLinearSystem)=0;

    virtual void FinishAssembleSystem(bool assembleResidual, bool assembleLinearSystem);

    //
    // Element level methods
    //

    void GetElementCentroidStrain(StrainType strainType,
                                  Element<DIM,DIM>& rElement,
                                  c_matrix<double,DIM,DIM>& rDeformationGradient);

    virtual void AddActiveStressAndStressDerivative(c_matrix<double,DIM,DIM>& rC,
                                                    unsigned elementIndex,
                                                    unsigned currentQuadPointGlobalIndex,
                                                    c_matrix<double,DIM,DIM>& rT,
                                                    FourthOrderTensor<DIM,DIM,DIM,DIM>& rDTdE,
                                                    bool addToDTdE)
    {
        // does nothing - subclass needs to overload this if there are active stresses
    }

    virtual void SetupChangeOfBasisMatrix(unsigned elementIndex, unsigned currentQuadPointGlobalIndex)
    {
    }

    void AssembleOnBoundaryElement(BoundaryElement<DIM-1, DIM>& rBoundaryElement,
                                   c_matrix<double, BOUNDARY_STENCIL_SIZE, BOUNDARY_STENCIL_SIZE>& rAelem,
                                   c_vector<double, BOUNDARY_STENCIL_SIZE>& rBelem,
                                   bool assembleResidual,
                                   bool assembleJacobian,
                                   unsigned boundaryConditionIndex);


    bool ShouldAssembleMatrixTermForPressureOnDeformedBc();

    void AssembleOnBoundaryElementForPressureOnDeformedBc(BoundaryElement<DIM-1,DIM>& rBoundaryElement,
                                                          c_matrix<double,BOUNDARY_STENCIL_SIZE,BOUNDARY_STENCIL_SIZE>& rAelem,
                                                          c_vector<double,BOUNDARY_STENCIL_SIZE>& rBelem,
                                                          bool assembleResidual,
                                                          bool assembleJacobian,
                                                          unsigned boundaryConditionIndex);

    //
    //    These methods form the non-SNES nonlinear solver
    //

    double ComputeResidualAndGetNorm(bool allowException);

    double CalculateResidualNorm();

    void VectorSum(std::vector<double>& rX, ReplicatableVector& rY, double a, std::vector<double>& rZ);

    void PrintLineSearchResult(double s, double residNorm);

    double TakeNewtonStep();

    double UpdateSolutionUsingLineSearch(Vec solution);

    virtual void PostNewtonStep(unsigned counter, double normResidual);

    void SolveNonSnes(double tol=-1.0);


    //
    //    These methods form the SNES nonlinear solver
    //
public: // need to be public as are called by global functions
    void ComputeResidual(Vec currentGuess, Vec residualVector);

    void ComputeJacobian(Vec currentGuess, Mat* pJacobian, Mat* pPreconditioner);

private:
    void SolveSnes();

public:

    AbstractNonlinearElasticitySolver(AbstractTetrahedralMesh<DIM,DIM>& rQuadMesh,
                                      SolidMechanicsProblemDefinition<DIM>& rProblemDefinition,
                                      std::string outputDirectory,
                                      CompressibilityType compressibilityType);

    virtual ~AbstractNonlinearElasticitySolver();

    void Solve(double tol=-1.0);

    void SetIncludeActiveTension(bool includeActiveTension = true)
    {
        mIncludeActiveTension = includeActiveTension;
    }

    unsigned GetNumNewtonIterations();


    void SetWriteOutputEachNewtonIteration(bool writeOutputEachNewtonIteration=true)
    {
        mWriteOutputEachNewtonIteration = writeOutputEachNewtonIteration;
    }

    void SetKspAbsoluteTolerance(double kspAbsoluteTolerance)
    {
        assert(kspAbsoluteTolerance > 0);
        mKspAbsoluteTol = kspAbsoluteTolerance;
    }

    void SetTakeFullFirstNewtonStep(bool takeFullFirstStep = true)
    {
        mTakeFullFirstNewtonStep = takeFullFirstStep;
    }

    void SetUsePetscDirectSolve(bool usePetscDirectSolve = true)
    {
        mPetscDirectSolve = usePetscDirectSolve;
    }


    void SetCurrentTime(double time)
    {
        mCurrentTime = time;
    }

    void CreateCmguiOutput();


    void WriteCurrentStrains(StrainType strainType, std::string fileName, int counterToAppend = -1);

    void SetComputeAverageStressPerElementDuringSolve(bool setComputeAverageStressPerElement = true);

    void WriteCurrentAverageElementStresses(std::string fileName, int counterToAppend = -1);

    std::vector<c_vector<double,DIM> >& rGetSpatialSolution();

    std::vector<c_vector<double,DIM> >& rGetDeformedPosition();

    c_matrix<double,DIM,DIM> GetAverageStressPerElement(unsigned elementIndex);
};

// Implementation: first, the non-nonlinear-solve methods

template<unsigned DIM>
AbstractNonlinearElasticitySolver<DIM>::AbstractNonlinearElasticitySolver(AbstractTetrahedralMesh<DIM,DIM>& rQuadMesh,
                                                                          SolidMechanicsProblemDefinition<DIM>& rProblemDefinition,
                                                                          std::string outputDirectory,
                                                                          CompressibilityType compressibilityType)
    : AbstractContinuumMechanicsSolver<DIM>(rQuadMesh, rProblemDefinition, outputDirectory, compressibilityType),
      mrProblemDefinition(rProblemDefinition),
      mrJacobianMatrix(this->mSystemLhsMatrix),
      mKspAbsoluteTol(-1),
      mWriteOutputEachNewtonIteration(false),
      mNumNewtonIterations(0),
      mCurrentTime(0.0),
      mCheckedOutwardNormals(false),
      mLastDampingValue(0.0),
      mIncludeActiveTension(true),
      mSetComputeAverageStressPerElement(false)
{
    mUseSnesSolver = (mrProblemDefinition.GetSolveUsingSnes() ||
                      CommandLineArguments::Instance()->OptionExists("-mech_use_snes") );

    mChangeOfBasisMatrix = identity_matrix<double>(DIM,DIM);

    mTakeFullFirstNewtonStep = CommandLineArguments::Instance()->OptionExists("-mech_full_first_newton_step");
    mPetscDirectSolve = CommandLineArguments::Instance()->OptionExists("-mech_petsc_direct_solve");
}

template<unsigned DIM>
AbstractNonlinearElasticitySolver<DIM>::~AbstractNonlinearElasticitySolver()
{
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::FinishAssembleSystem(bool assembleResidual, bool assembleJacobian)
{
    PetscVecTools::Finalise(this->mResidualVector);

    if (assembleJacobian)
    {
        PetscMatTools::SwitchWriteMode(mrJacobianMatrix);
        PetscMatTools::SwitchWriteMode(this->mPreconditionMatrix);

        VecCopy(this->mResidualVector, this->mLinearSystemRhsVector);
    }

    // Apply Dirichlet boundary conditions
    if (assembleJacobian)
    {
        this->ApplyDirichletBoundaryConditions(NONLINEAR_PROBLEM_APPLY_TO_EVERYTHING, this->mCompressibilityType==COMPRESSIBLE);
    }
    else if (assembleResidual)
    {
        this->ApplyDirichletBoundaryConditions(NONLINEAR_PROBLEM_APPLY_TO_RESIDUAL_ONLY, this->mCompressibilityType==COMPRESSIBLE);
    }

    if (assembleResidual)
    {
        PetscVecTools::Finalise(this->mResidualVector);
    }
    if (assembleJacobian)
    {
        PetscMatTools::Finalise(mrJacobianMatrix);
        PetscMatTools::Finalise(this->mPreconditionMatrix);
        PetscVecTools::Finalise(this->mLinearSystemRhsVector);
    }
}

template<unsigned DIM>
std::vector<c_vector<double,DIM> >& AbstractNonlinearElasticitySolver<DIM>::rGetSpatialSolution()
{
    this->mSpatialSolution.clear();
    this->mSpatialSolution.resize(this->mrQuadMesh.GetNumNodes(), zero_vector<double>(DIM));
    for (unsigned i=0; i<this->mrQuadMesh.GetNumNodes(); i++)
    {
        for (unsigned j=0; j<DIM; j++)
        {
            this->mSpatialSolution[i](j) = this->mrQuadMesh.GetNode(i)->rGetLocation()[j] + this->mCurrentSolution[this->mProblemDimension*i+j];
        }
    }
    return this->mSpatialSolution;
}

template<unsigned DIM>
std::vector<c_vector<double,DIM> >& AbstractNonlinearElasticitySolver<DIM>::rGetDeformedPosition()
{
    return rGetSpatialSolution();
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::WriteCurrentStrains(StrainType strainType, std::string fileName, int counterToAppend)
{
    if (!this->mWriteOutput)
    {
        return;
    }

    std::stringstream file_name;
    file_name << fileName;
    if (counterToAppend >= 0)
    {
        file_name << "_" << counterToAppend;
    }
    file_name << ".strain";

    out_stream p_file = this->mpOutputFileHandler->OpenOutputFile(file_name.str());

    c_matrix<double,DIM,DIM> strain;

    for (typename AbstractTetrahedralMesh<DIM,DIM>::ElementIterator iter = this->mrQuadMesh.GetElementIteratorBegin();
         iter != this->mrQuadMesh.GetElementIteratorEnd();
         ++iter)
    {
        GetElementCentroidStrain(strainType, *iter, strain);
        for (unsigned i=0; i<DIM; i++)
        {
            for (unsigned j=0; j<DIM; j++)
            {
                *p_file << strain(i,j) << " ";
            }
        }
        *p_file << "\n";
    }
    p_file->close();
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::WriteCurrentAverageElementStresses(std::string fileName, int counterToAppend)
{
    if (!this->mWriteOutput)
    {
        return;
    }

    if (!mSetComputeAverageStressPerElement)
    {
        EXCEPTION("Call SetComputeAverageStressPerElementDuringSolve() before solve if calling WriteCurrentAverageElementStresses()");
    }

    std::stringstream file_name;
    file_name << fileName;
    if (counterToAppend >= 0)
    {
        file_name << "_" << counterToAppend;
    }
    file_name << ".stress";
    assert(mAverageStressesPerElement.size()==this->mrQuadMesh.GetNumElements());

    StressPerElementWriter<DIM> stress_writer(&(this->mrQuadMesh),this);
    stress_writer.WriteData(*(this->mpOutputFileHandler), file_name.str());
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::CreateCmguiOutput()
{
    if (this->mOutputDirectory == "")
    {
        EXCEPTION("No output directory was given so no output was written, cannot convert to cmgui format");
    }

    CmguiDeformedSolutionsWriter<DIM> writer(this->mOutputDirectory + "/cmgui",
                                             "solution",
                                             this->mrQuadMesh,
                                             WRITE_QUADRATIC_MESH);

    std::vector<c_vector<double,DIM> >& r_deformed_positions = this->rGetDeformedPosition();
    writer.WriteInitialMesh(); // this writes solution_0.exnode and .exelem
    writer.WriteDeformationPositions(r_deformed_positions, 1); // this writes the final solution as solution_1.exnode
    writer.WriteCmguiScript(); // writes LoadSolutions.com
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::SetComputeAverageStressPerElementDuringSolve(bool setComputeAverageStressPerElement)
{
    mSetComputeAverageStressPerElement = setComputeAverageStressPerElement;
    if (setComputeAverageStressPerElement && mAverageStressesPerElement.size()==0)
    {
        mAverageStressesPerElement.resize(this->mrQuadMesh.GetNumElements(), zero_vector<double>(DIM*(DIM+1)/2));
    }
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::AddStressToAverageStressPerElement(c_matrix<double,DIM,DIM>& rT, unsigned elemIndex)
{
    assert(mSetComputeAverageStressPerElement);
    assert(elemIndex<this->mrQuadMesh.GetNumElements());

    // In 2d the matrix is
    // [T00 T01]
    // [T10 T11]
    // where T01 = T10. We store this as a vector
    // [T00 T01 T11]
    //
    // Similarly, for 3d we store
    // [T00 T01 T02 T11 T12 T22]
    for (unsigned i=0; i<DIM*(DIM+1)/2; i++)
    {
        unsigned row;
        unsigned col;
        if (DIM == 2)
        {
            row = i<=1 ? 0 : 1;
            col = i==0 ? 0 : 1;
        }
        else // DIM == 3
        {
            row = i<=2 ? 0 : (i<=4? 1 : 2);
            col = i==0 ? 0 : (i==1 || i==3? 1 : 2);
        }

        this->mAverageStressesPerElement[elemIndex](i) += rT(row,col);
    }
}

template<unsigned DIM>
c_matrix<double,DIM,DIM> AbstractNonlinearElasticitySolver<DIM>::GetAverageStressPerElement(unsigned elementIndex)
{
    if (!mSetComputeAverageStressPerElement)
    {
        EXCEPTION("Call SetComputeAverageStressPerElementDuringSolve() before solve if calling GetAverageStressesPerElement()");
    }
    assert(elementIndex<this->mrQuadMesh.GetNumElements());

    c_matrix<double,DIM,DIM> stress;

    // In 2d the matrix is
    // [T00 T01]
    // [T10 T11]
    // where T01 = T10, and was stored as
    // [T00 T01 T11]
    //
    // Similarly, for 3d the matrix was stored as
    // [T00 T01 T02 T11 T12 T22]
    if (DIM == 2)
    {
        stress(0,0) = mAverageStressesPerElement[elementIndex](0);
        stress(1,0) = stress(0,1) = mAverageStressesPerElement[elementIndex](1);
        stress(1,1) = mAverageStressesPerElement[elementIndex](2);
    }
    else
    {
        stress(0,0) = mAverageStressesPerElement[elementIndex](0);
        stress(1,0) = stress(0,1) = mAverageStressesPerElement[elementIndex](1);
        stress(2,0) = stress(0,2) = mAverageStressesPerElement[elementIndex](2);
        stress(1,1) = mAverageStressesPerElement[elementIndex](3);
        stress(2,1) = stress(1,2) = mAverageStressesPerElement[elementIndex](4);
        stress(2,2) = mAverageStressesPerElement[elementIndex](5);
    }

    return stress;
}

// Methods at the 'element level'.

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::GetElementCentroidStrain(StrainType strainType,
                                                                      Element<DIM,DIM>& rElement,
                                                                      c_matrix<double,DIM,DIM>& rStrain)
{
    static c_matrix<double,DIM,DIM> jacobian;
    static c_matrix<double,DIM,DIM> inverse_jacobian;
    double jacobian_determinant;

    this->mrQuadMesh.GetInverseJacobianForElement(rElement.GetIndex(), jacobian, jacobian_determinant, inverse_jacobian);

    // Get the current displacement at the nodes
    static c_matrix<double,DIM,NUM_NODES_PER_ELEMENT> element_current_displacements;
    for (unsigned II=0; II<NUM_NODES_PER_ELEMENT; II++)
    {
        for (unsigned JJ=0; JJ<DIM; JJ++)
        {
            element_current_displacements(JJ,II) = this->mCurrentSolution[this->mProblemDimension*rElement.GetNodeGlobalIndex(II) + JJ];
        }
    }

    // Allocate memory for the basis functions values and derivative values
    static c_matrix<double, DIM, NUM_NODES_PER_ELEMENT> grad_quad_phi;
    static c_matrix<double,DIM,DIM> grad_u; // grad_u = (du_i/dX_M)

    // we need the point in the canonical element which corresponds to the centroid of the
    // version of the element in physical space. This point can be shown to be (1/3,1/3).
    ChastePoint<DIM> quadrature_point;
    if (DIM == 2)
    {
        quadrature_point.rGetLocation()(0) = 1.0/3.0;
        quadrature_point.rGetLocation()(1) = 1.0/3.0;
    }
    else
    {
        assert(DIM==3);
        quadrature_point.rGetLocation()(0) = 1.0/4.0;
        quadrature_point.rGetLocation()(1) = 1.0/4.0;
        quadrature_point.rGetLocation()(2) = 1.0/4.0;
    }

    QuadraticBasisFunction<DIM>::ComputeTransformedBasisFunctionDerivatives(quadrature_point, inverse_jacobian, grad_quad_phi);

    // Interpolate grad_u
    grad_u = zero_matrix<double>(DIM,DIM);
    for (unsigned node_index=0; node_index<NUM_NODES_PER_ELEMENT; node_index++)
    {
        for (unsigned i=0; i<DIM; i++)
        {
            for (unsigned M=0; M<DIM; M++)
            {
                grad_u(i,M) += grad_quad_phi(M,node_index)*element_current_displacements(i,node_index);
            }
        }
    }

    c_matrix<double,DIM,DIM> deformation_gradient;

    for (unsigned i=0; i<DIM; i++)
    {
        for (unsigned M=0; M<DIM; M++)
        {
            deformation_gradient(i,M) = (i==M?1:0) + grad_u(i,M);
        }
    }

    switch(strainType)
    {
        case DEFORMATION_GRADIENT_F:
        {
            rStrain = deformation_gradient;
            break;
        }
        case DEFORMATION_TENSOR_C:
        {
            rStrain = prod(trans(deformation_gradient),deformation_gradient);
            break;
        }
        case LAGRANGE_STRAIN_E:
        {
            c_matrix<double,DIM,DIM> C = prod(trans(deformation_gradient),deformation_gradient);
            for (unsigned M=0; M<DIM; M++)
            {
                for (unsigned N=0; N<DIM; N++)
                {
                    rStrain(M,N) = 0.5* ( C(M,N)-(M==N?1:0) );
                }
            }
            break;
        }
        default:
        {
            NEVER_REACHED;
            break;
        }
    }
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::AssembleOnBoundaryElement(
            BoundaryElement<DIM-1,DIM>& rBoundaryElement,
            c_matrix<double,BOUNDARY_STENCIL_SIZE,BOUNDARY_STENCIL_SIZE>& rAelem,
            c_vector<double,BOUNDARY_STENCIL_SIZE>& rBelem,
            bool assembleResidual,
            bool assembleJacobian,
            unsigned boundaryConditionIndex)
{
    if (this->mrProblemDefinition.GetTractionBoundaryConditionType() == PRESSURE_ON_DEFORMED
        || this->mrProblemDefinition.GetTractionBoundaryConditionType() == FUNCTIONAL_PRESSURE_ON_DEFORMED)
    {
        AssembleOnBoundaryElementForPressureOnDeformedBc(rBoundaryElement, rAelem, rBelem,
                                                         assembleResidual, assembleJacobian, boundaryConditionIndex);
        return;
    }

    rAelem.clear();
    rBelem.clear();

    if (assembleJacobian && !assembleResidual)
    {
        // Nothing to do
        return;
    }

    c_vector<double, DIM> weighted_direction;
    double jacobian_determinant;
    this->mrQuadMesh.GetWeightedDirectionForBoundaryElement(rBoundaryElement.GetIndex(), weighted_direction, jacobian_determinant);

    c_vector<double,NUM_NODES_PER_BOUNDARY_ELEMENT> phi;

    for (unsigned quad_index=0; quad_index<this->mpBoundaryQuadratureRule->GetNumQuadPoints(); quad_index++)
    {
        double wJ = jacobian_determinant * this->mpBoundaryQuadratureRule->GetWeight(quad_index);

        const ChastePoint<DIM-1>& quad_point = this->mpBoundaryQuadratureRule->rGetQuadPoint(quad_index);

        QuadraticBasisFunction<DIM-1>::ComputeBasisFunctions(quad_point, phi);

        // Get the required traction, interpolating X (slightly inefficiently,
        // as interpolating using quad bases) if necessary
        c_vector<double,DIM> traction = zero_vector<double>(DIM);
        switch (this->mrProblemDefinition.GetTractionBoundaryConditionType())
        {
            case FUNCTIONAL_TRACTION:
            {
                c_vector<double,DIM> X = zero_vector<double>(DIM);
                for (unsigned node_index=0; node_index<NUM_NODES_PER_BOUNDARY_ELEMENT; node_index++)
                {
                    X += phi(node_index)*this->mrQuadMesh.GetNode( rBoundaryElement.GetNodeGlobalIndex(node_index) )->rGetLocation();
                }
                traction = this->mrProblemDefinition.EvaluateTractionFunction(X, this->mCurrentTime);
                break;
            }
            case ELEMENTWISE_TRACTION:
            {
                traction = this->mrProblemDefinition.rGetElementwiseTractions()[boundaryConditionIndex];
                break;
            }
            default:
                NEVER_REACHED;
        }


        for (unsigned index=0; index<NUM_NODES_PER_BOUNDARY_ELEMENT*DIM; index++)
        {
            unsigned spatial_dim = index%DIM;
            unsigned node_index = (index-spatial_dim)/DIM;

            assert(node_index < NUM_NODES_PER_BOUNDARY_ELEMENT);

            rBelem(index) -=   traction(spatial_dim)
                             * phi(node_index)
                             * wJ;
        }
    }
}

template<unsigned DIM>
bool AbstractNonlinearElasticitySolver<DIM>::ShouldAssembleMatrixTermForPressureOnDeformedBc()
{
    if (mUseSnesSolver)
    {
        // although not using this in the first few steps might be useful when the deformation
        // is large, the snes solver is more robust, so we have this on all the time. (Also because
        // for cardiac problems and in timesteps after the initial large deformation, we want this on
        // in the first step
        return true;

        // could do something like this, if make the snes a member variable
        //PetscInt iteration_number;
        //SNESGetIterationNumber(mSnes,&iteration_number);
        //return (iteration_number >= 3);
    }
    else
    {
        return (mLastDampingValue >= 0.5);
    }
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::AssembleOnBoundaryElementForPressureOnDeformedBc(
            BoundaryElement<DIM-1,DIM>& rBoundaryElement,
            c_matrix<double,BOUNDARY_STENCIL_SIZE,BOUNDARY_STENCIL_SIZE>& rAelem,
            c_vector<double,BOUNDARY_STENCIL_SIZE>& rBelem,
            bool assembleResidual,
            bool assembleJacobian,
            unsigned boundaryConditionIndex)
{
    assert(   this->mrProblemDefinition.GetTractionBoundaryConditionType()==PRESSURE_ON_DEFORMED
           || this->mrProblemDefinition.GetTractionBoundaryConditionType()==FUNCTIONAL_PRESSURE_ON_DEFORMED);

    rAelem.clear();
    rBelem.clear();

    c_vector<double, DIM> weighted_direction;
    double jacobian_determinant;
    // note: jacobian determinant may be over-written below
    this->mrQuadMesh.GetWeightedDirectionForBoundaryElement(rBoundaryElement.GetIndex(), weighted_direction, jacobian_determinant);

    // Find the volume element of the mesh which
    // contains this boundary element

    Element<DIM,DIM>* p_containing_vol_element = nullptr;

    std::set<unsigned> potential_elements = rBoundaryElement.GetNode(0)->rGetContainingElementIndices();
    for (std::set<unsigned>::iterator iter = potential_elements.begin();
        iter != potential_elements.end();
        iter++)
    {
        p_containing_vol_element = this->mrQuadMesh.GetElement(*iter);

        bool this_vol_ele_contains_surf_ele = true;
        // loop over the nodes of boundary element and see if they are in the volume element
        for (unsigned i=1; i<NUM_NODES_PER_BOUNDARY_ELEMENT; i++) // don't need to start at 0, given looping over contain elems of node 0
        {
            unsigned surf_element_node_index = rBoundaryElement.GetNodeGlobalIndex(i);
            bool found_this_node = false;
            for (unsigned j=0; j<p_containing_vol_element->GetNumNodes(); j++)
            {
                unsigned vol_element_node_index = p_containing_vol_element->GetNodeGlobalIndex(j);
                if (surf_element_node_index == vol_element_node_index)
                {
                    found_this_node = true;
                    break;
                }
            }
            if (!found_this_node)
            {
                this_vol_ele_contains_surf_ele = false;
                break;
            }
        }
        if (this_vol_ele_contains_surf_ele)
        {
            break;
        }
    }

    // Find the local node index in the volume element for each node in the boundary element
    std::vector<unsigned> surf_to_vol_map(NUM_NODES_PER_BOUNDARY_ELEMENT);
    for (unsigned i=0; i<NUM_NODES_PER_BOUNDARY_ELEMENT; i++)
    {
        unsigned index = rBoundaryElement.GetNodeGlobalIndex(i);
        for (unsigned j=0; j<NUM_NODES_PER_ELEMENT; j++)
        {
            if (p_containing_vol_element->GetNodeGlobalIndex(j)==index)
            {
                surf_to_vol_map[i] = j;
                break;
            }
        }
    }


    // We require the volume element to compute F, which requires grad_phi on the volume element. For this we will
    // need the inverse jacobian for the volume element
    static c_matrix<double,DIM,DIM> jacobian_vol_element;
    static c_matrix<double,DIM,DIM> inverse_jacobian_vol_element;
    double jacobian_determinant_vol_element;
    this->mrQuadMesh.GetInverseJacobianForElement(p_containing_vol_element->GetIndex(), jacobian_vol_element, jacobian_determinant_vol_element, inverse_jacobian_vol_element);

    // Get the current displacements at each node of the volume element, to be used in computing F
    static c_matrix<double,DIM,NUM_NODES_PER_ELEMENT> element_current_displacements;
    for (unsigned II=0; II<NUM_NODES_PER_ELEMENT; II++)
    {
        for (unsigned JJ=0; JJ<DIM; JJ++)
        {
            element_current_displacements(JJ,II) = this->mCurrentSolution[this->mProblemDimension*p_containing_vol_element->GetNodeGlobalIndex(II) + JJ];
        }
    }


    // We will need both {grad phi_i} for the quadratic bases of the volume element, for computing F..
    static c_matrix<double, DIM, NUM_NODES_PER_ELEMENT> grad_quad_phi_vol_element;
    // ..the phi_i for each of the quadratic bases of the surface element, for the standard FE assembly part.
    c_vector<double,NUM_NODES_PER_BOUNDARY_ELEMENT> quad_phi_surf_element;
    // We need this too, which is obtained by taking a subset of grad_quad_phi_vol_element
    static c_matrix<double, DIM, NUM_NODES_PER_BOUNDARY_ELEMENT> grad_quad_phi_surf_element;

    c_matrix<double,DIM,DIM> F;
    c_matrix<double,DIM,DIM> invF;

    c_vector<double,DIM> normal = rBoundaryElement.CalculateNormal();
    c_matrix<double,1,DIM> normal_as_mat;
    for (unsigned i=0; i<DIM; i++)
    {
        normal_as_mat(0,i) = normal(i);
    }

    double normal_pressure;
    switch (this->mrProblemDefinition.GetTractionBoundaryConditionType())
    {
        case PRESSURE_ON_DEFORMED:
            normal_pressure = this->mrProblemDefinition.GetNormalPressure();
            break;
        case FUNCTIONAL_PRESSURE_ON_DEFORMED:
            normal_pressure = this->mrProblemDefinition.EvaluateNormalPressureFunction(this->mCurrentTime);
            break;
        default:
            NEVER_REACHED;
    }

    for (unsigned quad_index=0; quad_index<this->mpBoundaryQuadratureRule->GetNumQuadPoints(); quad_index++)
    {
        double wJ = jacobian_determinant * this->mpBoundaryQuadratureRule->GetWeight(quad_index);

        // Get the quadrature point on this surface element (in canonical space) - so eg, for a 2D problem,
        // the quad point is in 1D space
        const ChastePoint<DIM-1>& quadrature_point = this->mpBoundaryQuadratureRule->rGetQuadPoint(quad_index);
        QuadraticBasisFunction<DIM-1>::ComputeBasisFunctions(quadrature_point, quad_phi_surf_element);

        // We will need the xi coordinates of this quad point in the volume element. We could do this by figuring
        // out how the nodes of the surface element are ordered in the list of nodes in the volume element,
        // however it is less fiddly to compute directly. Firstly, compute the corresponding physical location
        // of the quad point, by interpolating
        c_vector<double,DIM> X = zero_vector<double>(DIM);
        for (unsigned node_index=0; node_index<NUM_NODES_PER_BOUNDARY_ELEMENT; node_index++)
        {
            X += quad_phi_surf_element(node_index)*rBoundaryElement.GetNode(node_index)->rGetLocation();
        }


        // Now compute the xi coordinates of the quad point in the volume element
        c_vector<double,DIM+1> weight = p_containing_vol_element->CalculateInterpolationWeights(X);
        c_vector<double,DIM> xi;
        for (unsigned i=0; i<DIM; i++)
        {
            xi(i) = weight(i+1); // Note, in 2d say, weights = [1-xi(0)-xi(1), xi(0), xi(1)]
        }

        // Check one of the weights was zero, as the quad point is on the boundary of the volume element
        if (DIM == 2)
        {
            assert( DIM!=2 || (fabs(weight(0))<1e-6) || (fabs(weight(1))<1e-6) || (fabs(weight(2))<1e-6) );
        }
        else
        {
            assert( DIM!=3 || (fabs(weight(0))<1e-6) || (fabs(weight(1))<1e-6) || (fabs(weight(2))<1e-6)  || (fabs(weight(3))<1e-6) ); // LCOV_EXCL_LINE
        }

        // Now we can compute the grad_phi and then interpolate F
        QuadraticBasisFunction<DIM>::ComputeTransformedBasisFunctionDerivatives(xi, inverse_jacobian_vol_element, grad_quad_phi_vol_element);

        F = identity_matrix<double>(DIM,DIM);
        for (unsigned node_index=0; node_index<NUM_NODES_PER_ELEMENT; node_index++)
        {
            for (unsigned i=0; i<DIM; i++)
            {
                for (unsigned M=0; M<DIM; M++)
                {
                    F(i,M) += grad_quad_phi_vol_element(M,node_index)*element_current_displacements(i,node_index);
                }
            }
        }

        double detF = Determinant(F);
        invF = Inverse(F);

        if (assembleResidual)
        {
            c_vector<double,DIM> traction = detF*normal_pressure*prod(trans(invF),normal);

            // assemble
            for (unsigned index=0; index<NUM_NODES_PER_BOUNDARY_ELEMENT*DIM; index++)
            {
                unsigned spatial_dim = index%DIM;
                unsigned node_index = (index-spatial_dim)/DIM;

                assert(node_index < NUM_NODES_PER_BOUNDARY_ELEMENT);

                rBelem(index) -=   traction(spatial_dim)
                                 * quad_phi_surf_element(node_index)
                                 * wJ;
            }
        }

        // Sometimes we don't include the analytic jacobian for this integral
        // in the jacobian that is assembled - the ShouldAssembleMatrixTermForPressureOnDeformedBc()
        // bit below - see the documentation for this method to see why.
        if (assembleJacobian && ShouldAssembleMatrixTermForPressureOnDeformedBc())
        {
            for (unsigned II=0; II<NUM_NODES_PER_BOUNDARY_ELEMENT; II++)
            {
                for (unsigned N=0; N<DIM; N++)
                {
                    grad_quad_phi_surf_element(N,II) = grad_quad_phi_vol_element(N,surf_to_vol_map[II]);
                }
            }

            static FourthOrderTensor<DIM,DIM,DIM,DIM> tensor1;
            for (unsigned N=0; N<DIM; N++)
            {
                for (unsigned e=0; e<DIM; e++)
                {
                    for (unsigned M=0; M<DIM; M++)
                    {
                        for (unsigned d=0; d<DIM; d++)
                        {
                            tensor1(N,e,M,d) = invF(N,e)*invF(M,d) - invF(M,e)*invF(N,d);
                        }
                    }
                }
            }

            // tensor2(II,e,M,d) = tensor1(N,e,M,d)*grad_quad_phi_surf_element(N,II)
            static FourthOrderTensor<NUM_NODES_PER_BOUNDARY_ELEMENT,DIM,DIM,DIM> tensor2;
            tensor2.template SetAsContractionOnFirstDimension<DIM>( trans(grad_quad_phi_surf_element), tensor1);

            // tensor3 is really a third-order tensor
            // tensor3(II,e,0,d) = tensor2(II,e,M,d)*normal(M)
            static FourthOrderTensor<NUM_NODES_PER_BOUNDARY_ELEMENT,DIM,1,DIM> tensor3;
            tensor3.template SetAsContractionOnThirdDimension<DIM>( normal_as_mat, tensor2);

            for (unsigned index1=0; index1<NUM_NODES_PER_BOUNDARY_ELEMENT*DIM; index1++)
            {
                unsigned spatial_dim1 = index1%DIM;
                unsigned node_index1 = (index1-spatial_dim1)/DIM;

                for (unsigned index2=0; index2<NUM_NODES_PER_BOUNDARY_ELEMENT*DIM; index2++)
                {
                    unsigned spatial_dim2 = index2%DIM;
                    unsigned node_index2 = (index2-spatial_dim2)/DIM;

                    rAelem(index1,index2) -=    normal_pressure
                                              * detF
                                              * tensor3(node_index2,spatial_dim2,0,spatial_dim1)
                                              * quad_phi_surf_element(node_index1)
                                              * wJ;
                }
            }
        }
    }
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::Solve(double tol)
{
    // Check the problem definition is set up correctly (and fully).
    mrProblemDefinition.Validate();

    // If the problem includes specified pressures on deformed surfaces (as opposed
    // to specified tractions), the code needs to compute normals, and they need
    // to be consistently all facing outward (or all facing inward). Check the undeformed
    // mesh boundary elements has nodes that are ordered so that all normals are
    // outward-facing
    if (mrProblemDefinition.GetTractionBoundaryConditionType()==PRESSURE_ON_DEFORMED && mCheckedOutwardNormals==false)
    {
        this->mrQuadMesh.CheckOutwardNormals();
        mCheckedOutwardNormals = true;
    }

    // Write the initial solution
    this->WriteCurrentSpatialSolution("initial", "nodes");

    if (mUseSnesSolver)
    {
        SolveSnes();
    }
    else
    {
        SolveNonSnes(tol);
    }

    // Remove pressure dummy values (P=0 at internal nodes, which should have been
    // been the result of the solve above), by linear interpolating from vertices of
    // edges to the internal node of the edge
    if (this->mCompressibilityType==INCOMPRESSIBLE)
    {
        this->RemovePressureDummyValuesThroughLinearInterpolation();
    }

    // Write the final solution
    this->WriteCurrentSpatialSolution("solution", "nodes");
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::SetKspSolverAndPcType(KSP solver)
{
    // Four alternatives
    //   (a) Petsc direct solve
    //   Otherwise iterative solve with:
    //   (b) Incompressible: GMRES with ILU preconditioner (or bjacobi=ILU on each process) [default]. Very poor on large problems.
    //   (c) Incompressible: GMRES with AMG preconditioner. Uncomment #define MECH_USE_HYPRE above. Requires Petsc3 with HYPRE installed.
    //   (d) Compressible: CG with ICC

    PC pc;
    KSPGetPC(solver, &pc);

    if (mPetscDirectSolve)
    {
        if (this->mCompressibilityType==COMPRESSIBLE)
        {
            KSPSetType(solver,KSPPREONLY);

        }
        PCSetType(pc, PCLU);

        // See #2057
        // PCFactorSetMatSolverPackage(pc,"mumps");
    }
    else
    {
        if (this->mCompressibilityType==COMPRESSIBLE)
        {
            KSPSetType(solver,KSPCG);
            if (PetscTools::IsSequential())
            {
                PCSetType(pc, PCICC);
                //Note that PetscOptionsSetValue is dangerous because we can't easily do
                //regression testing.  If a name changes, then the behaviour of the code changes
                //because it won't recognise the old name.  However, it won't fail to compile/run.
                #if (PETSC_VERSION_MAJOR == 3 && PETSC_VERSION_MINOR >= 1) //PETSc 3.1 or later
                    PetscTools::SetOption("-pc_factor_shift_type", "positive_definite");
                #else
                    PetscTools::SetOption("-pc_factor_shift_positive_definite", "");
                #endif
            }
            else
            {
                PCSetType(pc, PCBJACOBI);
            }
        }
        else
        {
            unsigned num_restarts = 100;
            KSPSetType(solver,KSPGMRES);
            KSPGMRESSetRestart(solver,num_restarts);

            #ifndef MECH_USE_HYPRE
                PCSetType(pc, PCBJACOBI); // BJACOBI = ILU on each block (block = part of matrix on each process)
            #else
                // Speed up linear solve time massively for larger simulations (in fact GMRES may stagnate without
                // this for larger problems), by using a AMG preconditioner -- needs HYPRE installed
                PetscTools::SetOption("-pc_hypre_type", "boomeramg");
                // PetscTools::SetOption("-pc_hypre_boomeramg_max_iter", "1");
                // PetscTools::SetOption("-pc_hypre_boomeramg_strong_threshold", "0.0");

                PCSetType(pc, PCHYPRE);
                #if (PETSC_VERSION_MAJOR == 3 && PETSC_VERSION_MINOR >=2) //PETSc 3.2 or later
                    KSPSetPCSide(solver, PC_RIGHT);
                #else
                    KSPSetPreconditionerSide(solver, PC_RIGHT);
                #endif

                // other possible preconditioners..
                //PCBlockDiagonalMechanics* p_custom_pc = new PCBlockDiagonalMechanics(solver, this->mPreconditionMatrix, mBlock1Size, mBlock2Size);
                //PCLDUFactorisationMechanics* p_custom_pc = new PCLDUFactorisationMechanics(solver, this->mPreconditionMatrix, mBlock1Size, mBlock2Size);
                //remember to delete memory..
                //KSPSetPreconditionerSide(solver, PC_RIGHT);
            #endif
        }
    }
}

//  The code for the non-SNES solver - maybe remove all this
//  as SNES solver appears better

template<unsigned DIM>
double AbstractNonlinearElasticitySolver<DIM>::ComputeResidualAndGetNorm(bool allowException)
{
    if (!allowException)
    {
        // Assemble the residual
        AssembleSystem(true, false);
    }
    else
    {
        try
        {
            // Try to assemble the residual using this current solution
            AssembleSystem(true, false);
        }
        catch(Exception&)
        {
            // If fail (because e.g. ODEs fail to solve, or strains are too large for material law), return infinity
            return DBL_MAX;
        }
    }

    // Return the scaled norm of the residual
    return CalculateResidualNorm();
}

template<unsigned DIM>
double AbstractNonlinearElasticitySolver<DIM>::CalculateResidualNorm()
{
    double norm;

    //\todo Change to NORM_1 and remove the division by mNumDofs...
    VecNorm(this->mResidualVector, NORM_2, &norm);
    return norm/this->mNumDofs;
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::VectorSum(std::vector<double>& rX,
                                                       ReplicatableVector& rY,
                                                       double a,
                                                       std::vector<double>& rZ)
{
    assert(rX.size()==rY.GetSize());
    assert(rY.GetSize()==rZ.size());
    for (unsigned i=0; i<rX.size(); i++)
    {
        rZ[i] = rX[i] + a*rY[i];
    }
}

template<unsigned DIM>
double AbstractNonlinearElasticitySolver<DIM>::TakeNewtonStep()
{
    if (this->mVerbose)
    {
        Timer::Reset();
    }

    // Assemble Jacobian (and preconditioner)
    MechanicsEventHandler::BeginEvent(MechanicsEventHandler::ASSEMBLE);
    AssembleSystem(true, true);
    MechanicsEventHandler::EndEvent(MechanicsEventHandler::ASSEMBLE);
    if (this->mVerbose)
    {
        Timer::PrintAndReset("AssembleSystem");
    }

    // Solve the linear system.
    MechanicsEventHandler::BeginEvent(MechanicsEventHandler::SOLVE);

    Vec solution;
    VecDuplicate(this->mResidualVector,&solution);

    KSP solver;
    KSPCreate(PETSC_COMM_WORLD,&solver);

#if ((PETSC_VERSION_MAJOR==3) && (PETSC_VERSION_MINOR>=5))
    KSPSetOperators(solver, mrJacobianMatrix, this->mPreconditionMatrix);
#else
    KSPSetOperators(solver, mrJacobianMatrix, this->mPreconditionMatrix, DIFFERENT_NONZERO_PATTERN /*in precond between successive solves*/);
#endif

    // Set the type of KSP solver (CG, GMRES etc) and preconditioner (ILU, HYPRE, etc)
    SetKspSolverAndPcType(solver);

    //PetscTools::SetOption("-ksp_monitor","");
    //PetscTools::SetOption("-ksp_norm_type","natural");

    KSPSetFromOptions(solver);
    KSPSetUp(solver);


    // Set the linear system absolute tolerance.
    // This is either the user provided value, or set to
    // max {1e-6 * initial_residual, 1e-12}
    if (mKspAbsoluteTol < 0)
    {
        Vec temp;
        VecDuplicate(this->mResidualVector, &temp);
        Vec temp2;
        VecDuplicate(this->mResidualVector, &temp2);
        Vec linsys_residual;
        VecDuplicate(this->mResidualVector, &linsys_residual);

        KSPInitialResidual(solver, solution, temp, temp2, linsys_residual, this->mLinearSystemRhsVector);
        double initial_resid_norm;
        VecNorm(linsys_residual, NORM_2, &initial_resid_norm);

        PetscTools::Destroy(temp);
        PetscTools::Destroy(temp2);
        PetscTools::Destroy(linsys_residual);

        double ksp_rel_tol = 1e-6;
        double absolute_tol = ksp_rel_tol * initial_resid_norm;
        if (absolute_tol < 1e-12)
        {
            absolute_tol = 1e-12;
        }
        KSPSetTolerances(solver, 1e-16, absolute_tol, PETSC_DEFAULT, 1000 /* max iters */); // Note: some machines - max iters seems to be 1000 whatever we give here
    }
    else
    {
        KSPSetTolerances(solver, 1e-16, mKspAbsoluteTol, PETSC_DEFAULT, 1000 /* max iters */); // Note: some machines - max iters seems to be 1000 whatever we give here
    }

    if (this->mVerbose)
    {
        Timer::PrintAndReset("KSP Setup");
    }

    KSPSolve(solver,this->mLinearSystemRhsVector,solution);

//    ///// For printing matrix when debugging
//    OutputFileHandler handler("TEMP",false);
//    std::stringstream ss;
//    static unsigned counter = 0;
//    ss << "all_" << counter++ << ".txt";
//    out_stream p_file = handler.OpenOutputFile(ss.str());
//    *p_file << std::setprecision(10);
//    for (unsigned i=0; i<this->mNumDofs; i++)
//    {
//        for (unsigned j=0; j<this->mNumDofs; j++)
//        {
//            *p_file << PetscMatTools::GetElement(mrJacobianMatrix, i, j) << " ";
//        }
//        *p_file << PetscVecTools::GetElement(this->mLinearSystemRhsVector, i) << " ";
//        *p_file << PetscVecTools::GetElement(solution, i) << "\n";
//    }
//    p_file->close();


    // Error checking for linear solve

    // warn if ksp reports failure
    KSPConvergedReason reason;
    KSPGetConvergedReason(solver,&reason);

    if (reason != KSP_DIVERGED_ITS)
    {
        // Throw an exception if the solver failed for any reason other than DIVERGED_ITS.
        // This is not covered as would be difficult to cover - requires a bad matrix to
        // assembled, for example.
        // LCOV_EXCL_START
        KSPEXCEPT(reason);
        // LCOV_EXCL_STOP
    }
    else
    {
        // DIVERGED_ITS just means it didn't converge in the given maximum number of iterations,
        // which is potentially not a problem, as the nonlinear solver may (and often will) still converge.
        // Just warn once.
        // (Very difficult to cover in normal tests, requires relative and absolute ksp tols to be very small, there
        // is no interface for setting both of these. Could be covered by setting up a problem the solver
        // finds difficult to solve, but this would be overkill.)
        // LCOV_EXCL_START
        WARN_ONCE_ONLY("Linear solve (within a mechanics solve) didn't converge, but this may not stop nonlinear solve converging")
        // LCOV_EXCL_STOP
    }

    // quit if no ksp iterations were done
    int num_iters;
    KSPGetIterationNumber(solver, &num_iters);
    if (num_iters==0)
    {
        PetscTools::Destroy(solution);
        KSPDestroy(PETSC_DESTROY_PARAM(solver));
        EXCEPTION("KSP Absolute tolerance was too high, linear system wasn't solved - there will be no decrease in Newton residual. Decrease KspAbsoluteTolerance");
    }


    if (this->mVerbose)
    {
        Timer::PrintAndReset("KSP Solve");
        std::cout << "[" << PetscTools::GetMyRank() << "]: Num iterations = " << num_iters << "\n" << std::flush;
    }

    MechanicsEventHandler::EndEvent(MechanicsEventHandler::SOLVE);

    // Update the solution
    //  Newton method:       sol = sol - update, where update=Jac^{-1}*residual
    //  Newton with damping: sol = sol - s*update, where s is chosen
    //   such that |residual(sol)| is minimised. Damping is important to
    //   avoid initial divergence.
    //
    // Normally, finding the best s from say 0.05,0.1,0.2,..,1.0 is cheap,
    // but this is not the case in cardiac electromechanics calculations.
    // Therefore, we initially check s=1 (expected to be the best most of the
    // time, then s=0.9. If the norm of the residual increases, we assume
    // s=1 is the best. Otherwise, check s=0.8 to see if s=0.9 is a local min.
    MechanicsEventHandler::BeginEvent(MechanicsEventHandler::UPDATE);
    double new_norm_resid = UpdateSolutionUsingLineSearch(solution);
    MechanicsEventHandler::EndEvent(MechanicsEventHandler::UPDATE);

    PetscTools::Destroy(solution);
    KSPDestroy(PETSC_DESTROY_PARAM(solver));

    return new_norm_resid;
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::PrintLineSearchResult(double s, double residNorm)
{
    if (this->mVerbose)
    {
        std::cout << "\tTesting s = " << s << ", |f| = " << residNorm << "\n" << std::flush;
    }
}

template<unsigned DIM>
double AbstractNonlinearElasticitySolver<DIM>::UpdateSolutionUsingLineSearch(Vec solution)
{
    double initial_norm_resid = CalculateResidualNorm();
    if (this->mVerbose)
    {
        std::cout << "\tInitial |f| [corresponding to s=0] is " << initial_norm_resid << "\n"  << std::flush;
    }

    ReplicatableVector update(solution);

    std::vector<double> old_solution = this->mCurrentSolution;

    std::vector<double> damping_values; // = {1.0, 0.9, .., 0.2, 0.1, 0.05} ie size 11
    for (unsigned i=10; i>=1; i--)
    {
        damping_values.push_back((double)i/10.0);
    }
    damping_values.push_back(0.05);
    assert(damping_values.size()==11);

    // let mCurrentSolution = old_solution - damping_val[0]*update; and compute residual
    unsigned index = 0;
    VectorSum(old_solution, update, -damping_values[index], this->mCurrentSolution);
    double current_resid_norm = ComputeResidualAndGetNorm(true);
    PrintLineSearchResult(damping_values[index], current_resid_norm);

    // let mCurrentSolution = old_solution - damping_val[1]*update; and compute residual
    index = 1;
    VectorSum(old_solution, update, -damping_values[index], this->mCurrentSolution);
    double next_resid_norm = ComputeResidualAndGetNorm(true);
    PrintLineSearchResult(damping_values[index], next_resid_norm);

    index = 2;
    // While f(s_next) < f(s_current), [f = residnorm], keep trying new damping values,
    // ie exit thus loop when next norm of the residual first increases
    while (    (next_resid_norm==DBL_MAX) // the residual is returned as infinity if the deformation is so large to cause exceptions in the material law/EM contraction model
            || ( (next_resid_norm < current_resid_norm) && (index<damping_values.size()) ) )
    {
        current_resid_norm = next_resid_norm;

        // let mCurrentSolution = old_solution - damping_val*update; and compute residual
        VectorSum(old_solution, update, -damping_values[index], this->mCurrentSolution);
        next_resid_norm = ComputeResidualAndGetNorm(true);
        PrintLineSearchResult(damping_values[index], next_resid_norm);

        index++;
    }

    unsigned best_index;

    if (index==damping_values.size() && (next_resid_norm < current_resid_norm))
    {
        // Difficult to come up with large forces/tractions such that it had to
        // test right down to s=0.05, but overall doesn't fail.
        // The possible damping values have been manually temporarily altered to
        // get this code to be called, it appears to work correctly. Even if it
        // didn't tests wouldn't fail, they would just be v. slightly less efficient.
        // LCOV_EXCL_START
        // if we exited because we got to the end of the possible damping values, the
        // best one was the last one (excl the final index++ at the end)
        current_resid_norm = next_resid_norm;
        best_index = index-1;
        // LCOV_EXCL_STOP
    }
    else
    {
        // else the best one must have been the second last one (excl the final index++ at the end)
        // (as we would have exited when the resid norm first increased)
        best_index = index-2;
    }

    // See documentation for SetTakeFullFirstNewtonStep()
    bool full_first_step = mTakeFullFirstNewtonStep && mFirstStep;


    // Check out best was better than the original residual-norm
    if (initial_norm_resid < current_resid_norm && !full_first_step)
    {
        // LCOV_EXCL_START
        EXCEPTION("Residual does not appear to decrease in newton direction, quitting");
        // LCOV_EXCL_STOP
    }

    // See documentation for SetTakeFullFirstNewtonStep()
    if (full_first_step)
    {
        if (this->mVerbose)
        {
            std::cout << "\tTaking full first Newton step...\n";
        }
        best_index = 0;
    }

    if (this->mVerbose)
    {
        std::cout << "\tChoosing s = " << damping_values[best_index] << "\n"  << std::flush;
    }


//
//    double l_inf_disp = 0.0;
//    double l_inf_pressure = 0.0;
//
//    if (this->mCompressibilityType==INCOMPRESSIBLE)
//    {
//        for (unsigned i=0; i<this->mrQuadMesh.GetNumNodes(); i++)
//        {
//            for (unsigned j=0; j<DIM; j++)
//            {
//                double value = update[(DIM+1)*i + j]*damping_values[best_index];
//                l_inf_disp = std::max(l_inf_disp, fabs(value));
//            }
//            l_inf_pressure = std::max(l_inf_pressure, fabs(update[(DIM+1)*i + DIM]*damping_values[best_index]));
//        }
//        std::cout << "l_inf_disp, l_inf_pressure = " << l_inf_disp << " " << l_inf_pressure << "\n";
//    }
//    else
//    {
//        for (unsigned i=0; i<this->mrQuadMesh.GetNumNodes(); i++)
//        {
//            for (unsigned j=0; j<DIM; j++)
//            {
//                double value = update[DIM*i + j]*damping_values[best_index];
//                l_inf_disp = std::max(l_inf_disp, fabs(value));
//            }
//        }
//        std::cout << "l_inf_disp = " << l_inf_disp << "\n";
//    }

    VectorSum(old_solution, update, -damping_values[best_index], this->mCurrentSolution);

    mLastDampingValue = damping_values[best_index];
    return current_resid_norm;
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::PostNewtonStep(unsigned counter, double normResidual)
{
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::SolveNonSnes(double tol)
{
    mLastDampingValue = 0;

    if (mWriteOutputEachNewtonIteration)
    {
        this->WriteCurrentSpatialSolution("newton_iteration", "nodes", 0);
    }

    // Compute residual
    double norm_resid = ComputeResidualAndGetNorm(false);
    if (this->mVerbose)
    {
        std::cout << "\nNorm of residual is " << norm_resid << "\n";
    }

    mNumNewtonIterations = 0;
    unsigned iteration_number = 1;

    if (tol < 0) // i.e. if wasn't passed in as a parameter
    {
        tol = NEWTON_REL_TOL*norm_resid;

        // LCOV_EXCL_START // not going to have tests in cts for everything
        if (tol > MAX_NEWTON_ABS_TOL)
        {
            tol = MAX_NEWTON_ABS_TOL;
        }
        if (tol < MIN_NEWTON_ABS_TOL)
        {
            tol = MIN_NEWTON_ABS_TOL;
        }
        // LCOV_EXCL_STOP
    }

    if (this->mVerbose)
    {
        std::cout << "Solving with tolerance " << tol << "\n";
    }

    while (norm_resid > tol)
    {
        if (this->mVerbose)
        {
            std::cout <<  "\n-------------------\n"
                      <<   "Newton iteration " << iteration_number
                      << ":\n-------------------\n";
        }

        // take newton step (and get returned residual)
        mFirstStep = (iteration_number==1);
        norm_resid = TakeNewtonStep();

        if (this->mVerbose)
        {
            std::cout << "Norm of residual is " << norm_resid << "\n";
        }

        if (mWriteOutputEachNewtonIteration)
        {
            this->WriteCurrentSpatialSolution("newton_iteration", "nodes", iteration_number);
        }

        mNumNewtonIterations = iteration_number;

        PostNewtonStep(iteration_number,norm_resid);

        iteration_number++;
        if (iteration_number==20)
        {
            // LCOV_EXCL_START
            EXCEPTION("Not converged after 20 newton iterations, quitting");
            // LCOV_EXCL_STOP
        }
    }

    if (norm_resid > tol)
    {
        // LCOV_EXCL_START
        EXCEPTION("Failed to converge");
        // LCOV_EXCL_STOP
    }
}

template<unsigned DIM>
unsigned AbstractNonlinearElasticitySolver<DIM>::GetNumNewtonIterations()
{
    return mNumNewtonIterations;
}

//  SNES version of the nonlinear solver

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::SolveSnes()
{
    // Set up solution guess for residuals
    Vec initial_guess;
    VecDuplicate(this->mResidualVector, &initial_guess);
    double* p_initial_guess;
    VecGetArray(initial_guess, &p_initial_guess);
    int lo, hi;
    VecGetOwnershipRange(initial_guess, &lo, &hi);
    for (int global_index=lo; global_index<hi; global_index++)
    {
        int local_index = global_index - lo;
        p_initial_guess[local_index] = this->mCurrentSolution[global_index];
    }
    VecRestoreArray(initial_guess, &p_initial_guess);
    PetscVecTools::Finalise(initial_guess);

    Vec snes_residual_vec;
    VecDuplicate(this->mResidualVector, &snes_residual_vec);

    SNES snes;

    SNESCreate(PETSC_COMM_WORLD, &snes);
    SNESSetFunction(snes, snes_residual_vec, &AbstractNonlinearElasticitySolver_ComputeResidual<DIM>, this);
    SNESSetJacobian(snes, mrJacobianMatrix, this->mPreconditionMatrix, &AbstractNonlinearElasticitySolver_ComputeJacobian<DIM>, this);
#if (PETSC_VERSION_MAJOR == 3 && PETSC_VERSION_MINOR >= 4) //PETSc 3.4 or later
    SNESSetType(snes, SNESNEWTONLS);
#else
    SNESSetType(snes, SNESLS);
#endif
    SNESSetTolerances(snes, 1e-5, 1e-5, PETSC_DEFAULT, PETSC_DEFAULT, PETSC_DEFAULT);

#if (PETSC_VERSION_MAJOR == 3 && PETSC_VERSION_MINOR == 3) //PETSc 3.3
    SNESLineSearch linesearch;
    SNESGetSNESLineSearch(snes, &linesearch);
    // Use 'critical point' line search algorithm.  This was changed from 'backtracking'; see #2916
    SNESLineSearchSetType(linesearch, "cp");
#elif (PETSC_VERSION_MAJOR == 3 && PETSC_VERSION_MINOR >= 4) //PETSc 3.4 or later
    SNESLineSearch linesearch;
    SNESGetLineSearch(snes, &linesearch);
    // Use 'critical point' line search algorithm.  This was changed from 'backtracking'; see #2916
    SNESLineSearchSetType(linesearch, "cp");
#endif

    SNESSetMaxLinearSolveFailures(snes,100);

    KSP ksp;
    SNESGetKSP(snes, &ksp);

    KSPSetTolerances(ksp, PETSC_DEFAULT, PETSC_DEFAULT, PETSC_DEFAULT, 1000 /* max iters */); // Note: some machines - max iters seems to be 1000 whatever we give here

    // Set the type of KSP solver (CG, GMRES etc) and preconditioner (ILU, HYPRE, etc)
    SetKspSolverAndPcType(ksp);

    if (this->mVerbose)
    {
        PetscTools::SetOption("-snes_monitor","");
    }
    SNESSetFromOptions(snes);

#if (PETSC_VERSION_MAJOR == 2 && PETSC_VERSION_MINOR == 2) //PETSc 2.2
    PetscErrorCode err = SNESSolve(snes, initial_guess);
#else
    PetscErrorCode err = SNESSolve(snes, PETSC_NULL, initial_guess);
#endif

    SNESConvergedReason reason;
    SNESGetConvergedReason(snes,&reason);

// LCOV_EXCL_START
    if (err != 0)
    {
        std::stringstream err_stream;
        err_stream << err;
        PetscTools::Destroy(initial_guess);
        PetscTools::Destroy(snes_residual_vec);
        SNESDestroy(PETSC_DESTROY_PARAM(snes));
        EXCEPTION("Nonlinear Solver failed. PETSc error code: "+err_stream.str()+" .");
    }

    if (reason < 0)
    {
        std::stringstream reason_stream;
        reason_stream << reason;
        PetscTools::Destroy(initial_guess);
        PetscTools::Destroy(snes_residual_vec);
        SNESDestroy(PETSC_DESTROY_PARAM(snes));
        EXCEPTION("Nonlinear Solver did not converge. PETSc reason code: "+reason_stream.str()+" .");
    }
// LCOV_EXCL_STOP

    PetscInt num_iters;
    SNESGetIterationNumber(snes,&num_iters);
    mNumNewtonIterations = num_iters;

    PetscTools::Destroy(initial_guess);
    PetscTools::Destroy(snes_residual_vec);
    SNESDestroy(PETSC_DESTROY_PARAM(snes));
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::ComputeResidual(Vec currentGuess, Vec residualVector)
{
    // Note: AssembleSystem() assumes the current solution is in this->mCurrentSolution and assembles
    // this->mResiduaVector and/or this->mrJacobianMatrix. Since PETSc wants us to use the input
    // currentGuess, and write the output to residualVector, we have to copy do some copies below.

    ReplicatableVector guess_repl(currentGuess);
    for (unsigned i=0; i<guess_repl.GetSize(); i++)
    {
        this->mCurrentSolution[i] = guess_repl[i];
    }
    AssembleSystem(true,false);
    VecCopy(this->mResidualVector, residualVector);
}

template<unsigned DIM>
void AbstractNonlinearElasticitySolver<DIM>::ComputeJacobian(Vec currentGuess, Mat* pJacobian, Mat* pPreconditioner)
{
    // Note: AssembleSystem() assumes the current solution is in this->mCurrentSolution and assembles
    // this->mResiduaVector and/or this->mrJacobianMatrix.
    // We need to copy the input currentGuess into the local mCurrentGuess.
    // We don't have to copy mrJacobianMatrix to pJacobian, which would be expensive, as they will
    // point to the same memory.

    // check Petsc data corresponds to internal Mats
    assert(mrJacobianMatrix==*pJacobian);
    assert(this->mPreconditionMatrix==*pPreconditioner);

    MechanicsEventHandler::BeginEvent(MechanicsEventHandler::ASSEMBLE);
    ReplicatableVector guess_repl(currentGuess);
    for (unsigned i=0; i<guess_repl.GetSize(); i++)
    {
        this->mCurrentSolution[i] = guess_repl[i];
    }

    AssembleSystem(false,true);
    MechanicsEventHandler::EndEvent(MechanicsEventHandler::ASSEMBLE);
}

template<unsigned DIM>
PetscErrorCode AbstractNonlinearElasticitySolver_ComputeResidual(SNES snes,
                                                                 Vec currentGuess,
                                                                 Vec residualVector,
                                                                 void* pContext)
{
    // Extract the solver from the void*
    AbstractNonlinearElasticitySolver<DIM>* p_solver = (AbstractNonlinearElasticitySolver<DIM>*)pContext;
    p_solver->ComputeResidual(currentGuess, residualVector);
    return 0;
}

template<unsigned DIM>
#if ((PETSC_VERSION_MAJOR==3) && (PETSC_VERSION_MINOR>=5))
    PetscErrorCode AbstractNonlinearElasticitySolver_ComputeJacobian(SNES snes,
                                                                     Vec currentGuess,
                                                                     Mat globalJacobian,
                                                                     Mat preconditioner,
                                                                     void* pContext)
    {
        // Extract the solver from the void*
        AbstractNonlinearElasticitySolver<DIM>* p_solver = (AbstractNonlinearElasticitySolver<DIM>*) pContext;
        p_solver->ComputeJacobian(currentGuess, &globalJacobian, &preconditioner);
        return 0;
    }
#else
    PetscErrorCode AbstractNonlinearElasticitySolver_ComputeJacobian(SNES snes,
                                                                     Vec currentGuess,
                                                                     Mat* pGlobalJacobian,
                                                                     Mat* pPreconditioner,
                                                                     MatStructure* pMatStructure,
                                                                     void* pContext)
    {
        // Extract the solver from the void*
        AbstractNonlinearElasticitySolver<DIM>* p_solver = (AbstractNonlinearElasticitySolver<DIM>*) pContext;
        p_solver->ComputeJacobian(currentGuess, pGlobalJacobian, pPreconditioner);
        return 0;
    }
#endif


// Constant setting definitions

template<unsigned DIM>
double AbstractNonlinearElasticitySolver<DIM>::MAX_NEWTON_ABS_TOL = 1e-7;

template<unsigned DIM>
double AbstractNonlinearElasticitySolver<DIM>::MIN_NEWTON_ABS_TOL = 1e-10;

template<unsigned DIM>
double AbstractNonlinearElasticitySolver<DIM>::NEWTON_REL_TOL = 1e-4;

#endif /*ABSTRACTNONLINEARELASTICITYSOLVER_HPP_*/