#ifndef DUNE_MICROSTRUCTURE_MATRIXOPERATIONS_HH
#define DUNE_MICROSTRUCTURE_MATRIXOPERATIONS_HH

namespace MatrixOperations {

	using namespace Dune;

	using MatrixRT = FieldMatrix< double, 3, 3>;
	using VectorRT = FieldVector< double, 3>;	

	using std::sin;
	using std::cos;

	static MatrixRT Id (){ 
		MatrixRT Id(0);
	    for (int i=0;i<3;i++)
	        Id[i][i]=1.0;
	    return Id;
	}
	
	
    static double norm(VectorRT v){
		return sqrt(pow(v[0],2) + pow(v[1],2) + pow(v[2],2));
	}

	static double norm(MatrixRT M){
		return sqrt(
			  pow(M[0][0],2) + pow(M[0][1],2) + pow(M[0][2],2)
			+ pow(M[1][0],2) + pow(M[1][1],2) + pow(M[1][2],2)
			+ pow(M[2][0],2) + pow(M[2][1],2) + pow(M[2][2],2));
	}
	

	static MatrixRT sym (MatrixRT M) { // 1/2 (M^T + M)
    	MatrixRT ret(0);
    	for (int i = 0; i< 3; i++)
    		for (int j = 0; j< 3; j++)
    			ret[i][j] = 0.5 * (M[i][j] + M[j][i]);
    	return ret;
	}

	static VectorRT crossProduct (VectorRT v, VectorRT w) { // v otimes w
    	return {v[1]*w[2] - v[2]*w[1], -1*(v[0]*w[2] - v[2]*w[0]), v[0]*w[1] - v[1]*w[0]};
	}

	static MatrixRT rankoneTensorproduct (VectorRT v, VectorRT w) { // v otimes w
    	return 
    	{{v[0]*w[0], v[1]*w[0], v[2]*w[0]}, 
	     {v[0]*w[1], v[1]*w[1], v[2]*w[1]}, 
	     {v[0]*w[2], v[1]*w[2], v[2]*w[2]}};
	}

	static MatrixRT nematicLiquidCrystal (double p, VectorRT n){ //B = 1/6*p*Id + 1/2*p*(n otimes n)
		MatrixRT B(0);
	    for (int i=0;i<3;i++)
	        B[i][i]=p/6.0;
	    MatrixRT n_ot_n = rankoneTensorproduct(n,n);
	    n_ot_n*=p/2.0;
	    B += n_ot_n;
	    return B;
	}

	static MatrixRT biotStrainApprox (VectorRT U, VectorRT k, VectorRT e_cs){ //E_h = (U +  k x e_cs, 0, 0)
	    VectorRT k_x_ecs = crossProduct(k, e_cs);
	    VectorRT U_plus_k_x_ecs = U + k_x_ecs;
	    VectorRT e_1 = {1, 0, 0};
	    return rankoneTensorproduct(U_plus_k_x_ecs, e_1);
	}

	static MatrixRT crossSectionDirectionScaling(double w, MatrixRT M){
		return {M[0], w*M[1], w*M[2]};
	}

	static double trace (MatrixRT M){ 
	    return M[0][0]+ M[1][1] + M[2][2];
	}

	static double scalarProduct (MatrixRT M1, MatrixRT M2){ 
		double sum = 0;
		for (int i=0; i<3; i++)
		    for (int j=0; j<3; j++)
		    	sum += M1[i][j] * M2[i][j];
	    return sum;
	}

// 	static double linearizedStVenantKirchhoffDensity(double mu, double lambda, MatrixRT E1, MatrixRT E2){    // ?? Check with Robert
// 		E1= sym(E1);
// 		E2 = sym(E2);
// 		double t1 = scalarProduct(E1,E2);
// 		t1 *= 2* mu;
// 		double t2 = trace(E1)*trace(E2); 
// 		t2 *= lambda;
// 		return t1 + t2;
// 	}
	
    static double linearizedStVenantKirchhoffDensity(double mu, double lambda, MatrixRT E1, MatrixRT E2)  // CHANGED
    {  
        auto t1 = 2.0 * mu * sym(E1) + lambda * trace(sym(E1)) * Id();
        return scalarProduct(t1,E2);

	}

    // --- Generalization: Define Quadratic QuadraticForm
    
    static double QuadraticForm(const double mu, const double lambda, const MatrixRT M){
        
        auto tmp1 = sym(M);
        double tmp2 = norm(tmp1);
        return lambda*std::pow(trace(M),2) + 2*mu*pow( tmp2 ,2);
//         return lambda*std::pow(trace(M),2) + 2*mu*pow( norm( sym(M) ),2);
    }

    
	static double generalizedDensity(const double mu, const double lambda, MatrixRT F, MatrixRT G){
     /// Write this whole File as a Class that uses lambda,mu as members ? 
        
     // Define L via Polarization-Identity from QuadratifForm
     // <LF,G> := (1/2)*(Q(F+G) - Q(F) - Q(G) ) 
        return (1.0/2.0)*(QuadraticForm(mu,lambda,F+G) - QuadraticForm(mu,lambda,F) - QuadraticForm(mu,lambda,G) );
    }

	static MatrixRT matrixSqrt(MatrixRT M){
		std::cout << "matrixSqrt not implemented!!!" << std::endl;//implement this
		return M;
	}

	static double lameMu(double E, double nu){
		return 0.5 * 1.0/(1.0 + nu) * E;
	}

	static double lameLambda(double E, double nu){
		return nu/(1.0-2.0*nu) * 1.0/(1.0+nu) * E;
	}

	static bool isInRotatedPlane(double phi, double x1, double x2){
		return cos(phi)*x1 + sin(phi)*x2 > 0;
	}



	/*
	template<double phi>
	static bool isInRotatedPlane(double x1, double x2){
		return cos(phi)*x1 + sin(phi)*x2 > 0;
	}*/

}



// OPTIONAL H1-Seminorm ... 

/*
template<class VectorCT>
double semi_H1_vectorScalarProduct(VectorCT& coeffVector1, VectorCT& coeffVector2)
{

double ret(0);
auto localView = basis_.localView();

for (const auto& e : elements(basis_.gridView()))
  	{
	    localView.bind(e);

	    auto geometry = e.geometry();

	    const auto& localFiniteElement = localView.tree().child(0).finiteElement();
	    const auto nSf = localFiniteElement.localBasis().size();

	    int order = 2*(dim*localFiniteElement.localBasis().order()-1);
	    const QuadratureRule<double, dim>& quad = QuadratureRules<double, dim>::rule(e.type(), order);

	   	for (const auto& quadPoint : quad)
    	{
    		double elementScp(0);

    		auto pos = quadPoint.position();
    		const auto jacobian = geometry.jacobianInverseTransposed(pos);
    		const double integrationElement = geometry.integrationElement(pos);

    		std::vector<FieldMatrix<double,1,dim> > referenceGradients;
    		localFiniteElement.localBasis().evaluateJacobian(pos, referenceGradients);

    		std::vector< VectorRT > gradients(referenceGradients.size());
    		for (size_t i=0; i< gradients.size(); i++)
      		jacobian.mv(referenceGradients[i][0], gradients[i]);

    		for (size_t i=0; i < nSf; i++)
    			for (size_t j=0; j < nSf; j++)
    			for (size_t k=0; k < dimworld; k++)
    				for (size_t l=0; l < dimworld; l++)
          		{
          			MatrixRT defGradient1(0);
          			defGradient1[k] = gradients[i];

          			MatrixRT defGradient2(0);
          			defGradient2[l] = gradients[j];

		          	size_t localIdx1 = localView.tree().child(k).localIndex(i);
		          	size_t globalIdx1 = localView.index(localIdx1);

		          	size_t localIdx2 = localView.tree().child(l).localIndex(j);
		          	size_t globalIdx2 = localView.index(localIdx2);

        			double scp(0);
        			for (int ii=0; ii<dimworld; ii++)
            		for (int jj=0; jj<dimworld; jj++)
        						scp += coeffVector1[globalIdx1] * defGradient1[ii][jj] * coeffVector2[globalIdx2] * defGradient2[ii][jj];

        				elementScp += scp;
        			}

            ret += elementScp * quadPoint.weight() * integrationElement;
      } //qp
  	} //e
	  return ret;
}*/






/*
	class BiotStrainApprox
	{

		// E_h = h^{-1} (R^T F_h - Id)
		//
		//E_h = (U +  K e_cs, 0, 0)
		//

		Func2Tensor E_a = [] (const Domain& z) //extra Klasse
	  	{
			return biotStrainApprox({1, 0, 0}, {0, 0, 0}, {0, z[1], z[2]}); //MatrixRT{{1.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}; 
	  	};

	  	// twist in x2-x3 plane E_K1 = sym (e1 x (0,x2,x3)^T ot e1)
	    Func2Tensor E_K1 = [] (const Domain& z) 
	    {
	    	return biotStrainApprox({0, 0, 0}, {1, 0, 0}, {0, z[1], z[2]}); //MatrixRT{{0.0,-0.5*z[2], 0.5*z[1]}, {-0.5*z[2], 0.0 , 0.0 }, {0.5*z[1],0.0,0.0}}; 
	  	};

	    //  bend strain in x1-x2 plane	E_K2 = sym (e2 x (0,x2,x3)^T ot e1)
	  	Func2Tensor E_K2 = [] (const Domain& z) 
	  	{
	  		return biotStrainApprox({0, 0, 0}, {0, 1, 0}, {0, z[1], z[2]}); //MatrixRT{{z[2], 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0}}; 
	  	};

	  	// bend strain in x1-x3 plane	E_K3 = sym (e3 x (0,x2,x3)^T ot e1)
	  	Func2Tensor E_K3 = [] (const Domain& z) 
	  	{
	  		return biotStrainApprox({0, 0, 0}, {0, 0, 1}, {0, z[1], z[2]}); //MatrixRT{{-z[1], 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}; 
	  	};
	};
*/

#endif