#ifndef NEO_HOOKE_OPERATOR_ASSEMBLER_HH
#define NEO_HOOKE_OPERATOR_ASSEMBLER_HH


#include <dune/common/fvector.hh>
#include <dune/common/fmatrix.hh>

#include <dune/istl/matrix.hh>

#include <dune/fufem/quadraturerules/quadraturerulecache.hh>
#include <dune/fufem/assemblers/localoperatorassembler.hh>
#include <dune/fufem/staticmatrixtools.hh>
#include <dune/fufem/symmetrictensor.hh>
#include <dune/fufem/functions/virtualgridfunction.hh>

#include <dune/elasticity/common/elasticityhelpers.hh>

/** \brief Assemble the second derivative of a neo-hookean material.
 *
 *  Laursen:
 *    \f[
 *      W(u)= \frac{\lambda}4(J^2-1)-(\frac{\lambda}2+\mu)log(J)+\mu tr(E(u))
 *    \f]
 *
 *  Fischer/Wriggers:
 *    \f[
 *      W(u)= \frac{\lambda}2(J-1)^2-2\mu)log(J)+\mu tr(E(u))
 *    \f]
 *
 *
 *      - \f$D\varphi\f$: the deformation gradient
 *      - \f$E\f$: the nonlinear strain tensor
 *      - \f$E'_u(v)\f$: the linearized strain tensor at the point u in direction v
 *      - \f$\sigma(u) = C:E(u)\f$: the second Piola-Kirchhoff stress tensor
 *      - \f$C\f$: the Hooke tensor
 *      - \f$Dv\f$: the gradient of the test function
 *
 */
template <class GridType, class TrialLocalFE, class AnsatzLocalFE>
class NeoHookeOperatorAssembler 
    : public LocalOperatorAssembler < GridType, TrialLocalFE, AnsatzLocalFE, Dune::FieldMatrix<typename GridType::ctype ,GridType::dimension,GridType::dimension> >
{


    static const int dim = GridType::dimension;
    typedef typename GridType::ctype ctype;
    
    typedef VirtualGridFunction<GridType, Dune::FieldVector<ctype,GridType::dimension> > GridFunction;

public:
    typedef typename Dune::FieldMatrix<ctype,GridType::dimension,GridType::dimension> T;

    typedef typename LocalOperatorAssembler < GridType, TrialLocalFE, AnsatzLocalFE, T >::Element Element;
    typedef typename LocalOperatorAssembler < GridType, TrialLocalFE, AnsatzLocalFE,T >::BoolMatrix BoolMatrix;
    typedef typename LocalOperatorAssembler < GridType, TrialLocalFE, AnsatzLocalFE,T >::LocalMatrix LocalMatrix;


    NeoHookeOperatorAssembler(ctype lambda, ctype mu, const Dune::shared_ptr<GridFunction> displacement):
        lambda_(lambda), mu_(mu), displacement_(displacement)
    {}

    NeoHookeOperatorAssembler(ctype lambda, ctype mu):
        lambda_(lambda), mu_(mu)
    {}

    void indices(const Element& element, BoolMatrix& isNonZero, const TrialLocalFE& tFE, const AnsatzLocalFE& aFE) const
    {
        isNonZero = true;
    }

    void assemble(const Element& element, LocalMatrix& localMatrix, const TrialLocalFE& tFE, const AnsatzLocalFE& aFE) const
    {
        typedef Dune::FieldVector<ctype,dim> FVdim;
        typedef Dune::FieldMatrix<ctype,dim,dim> FMdimdim;
        typedef typename TrialLocalFE::Traits::LocalBasisType::Traits::JacobianType JacobianType;

        int rows = tFE.localBasis().size();
        int cols = aFE.localBasis().size();

        localMatrix.setSize(rows,cols);
        localMatrix = 0.0;

        // get quadrature rule
        const int order = (element.type().isSimplex()) 
            ? 4*(tFE.localBasis().order())
            : 4*(tFE.localBasis().order()*dim);
        const Dune::template QuadratureRule<ctype, dim>& quad = QuadratureRuleCache<ctype, dim>::rule(element.type(), order, IsRefinedLocalFiniteElement<TrialLocalFE>::value(tFE) );

        // store gradients of shape functions and base functions
        std::vector<JacobianType> referenceGradients(tFE.localBasis().size());
        std::vector<FVdim> gradients(tFE.localBasis().size());

        // store entity geometry mapping
        const typename Element::Geometry geometry = element.geometry();

        // loop over quadrature points
        for (size_t pt=0; pt < quad.size(); ++pt) {

            // get quadrature point
            const FVdim& quadPos = quad[pt].position();

            // get transposed inverse of Jacobian of transformation
            const FMdimdim& invJacobian = geometry.jacobianInverseTransposed(quadPos);

            // get integration factor
            const ctype integrationElement = geometry.integrationElement(quadPos);

            // get gradients of shape functions
            tFE.localBasis().evaluateJacobian(quadPos, referenceGradients);

            // compute gradients of base functions
            for (size_t i=0; i<gradients.size(); ++i) {

                // transform gradients
                gradients[i] = 0.0;
                invJacobian.umv(referenceGradients[i][0], gradients[i]);

            }

            // evaluate displacement gradients the configuration at the quadrature point
            typename GridFunction::DerivativeType localConfGrad;
            if (displacement_->isDefinedOn(element))
                displacement_->evaluateDerivativeLocal(element, quadPos, localConfGrad);
            else
                displacement_->evaluateDerivative(geometry.global(quadPos),localConfGrad);

            // compute linearization of the determinante of the deformation gradient
            FMdimdim linDefDet;
            Dune::Elasticity::linearisedDefDet(localConfGrad,linDefDet);

            // compute deformation gradient of the configuration
            for (int i=0;i<dim;i++)
                localConfGrad[i][i] += 1;                

            // evaluate the derminante of the deformation gradient
            const ctype J = localConfGrad.determinant();

            // /////////////////////////////////////////////////
            //   Assemble matrix
            // /////////////////////////////////////////////////
            ctype z = quad[pt].weight() * integrationElement;

            ctype coeff,coeff2;

#ifdef LAURSEN
            coeff= (0.5*lambda_*J-(lambda_*0.5+mu_)/J);
            coeff2 = (0.5*lambda_ + (lambda_*0.5+mu_)/(J*J));
#else
            coeff = lambda_*(J-1) -2*mu_/J;
            coeff2 = lambda_ + 2*mu_/(J*J);
#endif

            for (int row=0; row<rows; row++)
                    for (int col=0; col<cols; col++) {
                        
                        // second derivative of the determinat term 
                        localMatrix[row][col].axpy(z*coeff,hessDefDet(localConfGrad,gradients[row],gradients[col]));
                      
                        FVdim rowTerm(0),colTerm(0);
                        linDefDet.mv(gradients[row],rowTerm); 
                        linDefDet.mv(gradients[col],colTerm); 

                        // derivative of the coefficient term
                        for (int rcomp=0; rcomp<dim; rcomp++)
                            localMatrix[row][col][rcomp].axpy(coeff2*z*rowTerm[rcomp],colTerm);
 
                        // second derivative of the trace term
                        StaticMatrix::addToDiagonal(localMatrix[row][col],z*mu_*((gradients[row]*gradients[col])));

            }
        }

    }

    /** \brief Set new configuration to be assembled at. Needed e.g. during Newton iteration.*/
    void setConfiguration(Dune::shared_ptr<GridFunction> newConfig) {
        displacement_ = newConfig;
    }

private:
    /** \brief Lame constant */
    ctype lambda_;

    /** \brief Lame constant */
    ctype mu_;

    /** \brief The configuration at which the linearized operator is evaluated.*/
    Dune::shared_ptr<GridFunction> displacement_;


    /** \brief Compute the matrix entry that arises from the second derivative of the determinant. 
     *  Note that it is assumed the defGrad is the deformation gradient and not the displacement gradient   
     */
    Dune::FieldMatrix<ctype,dim,dim> hessDefDet(const Dune::FieldMatrix<ctype,dim,dim>& defGrad,
                            const Dune::FieldVector<ctype,dim>& testGrad, const Dune::FieldVector<ctype,dim>& ansatzGrad) const
    {
        Dune::FieldMatrix<ctype, dim, dim> res(0);
        
        if (dim==2) {
            res[0][1] = testGrad[0]*ansatzGrad[1]-ansatzGrad[0]*testGrad[1];
            res[1][0] = -res[0][1];
            return res;
        }

        //compute the cross product
        Dune::FieldVector<ctype,dim> cross(0);    
        for (int k=0; k<dim; k++)
            cross[k]=testGrad[(k+1)%dim]*ansatzGrad[(k+2)%dim] -testGrad[(k+2)%dim]*ansatzGrad[(k+1)%dim];

        // the resulting matrix is skew symmetric with entries <cross,degGrad[i]>
        for (int i=0; i<dim; i++)
            for (int j=i+1; j<dim; j++) {
                int k= (dim-(i+j))%dim;
            res[i][j] = (cross*defGrad[k])*std::pow(-1,k);
            res[j][i] = -res[i][j];
        }
        
        return res;
    }
};


#endif