#ifndef DUNE_MICROSTRUCTURE_MATRIXOPERATIONS_HH
#define DUNE_MICROSTRUCTURE_MATRIXOPERATIONS_HH

namespace MatrixOperations {

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

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

	// 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 MatrixRT sym (MatrixRT M) { // 1/2 (M^T + M)
    	MatrixRT ret(0);
    	for (int i=0; i<3; i++)
		{
			ret[i][i] = M[i][i];
    		for (int j=i+1; j<3; j++)
			{
    			ret[i][j] = 0.5*(M[i][j] + M[j][i]);
				ret[j][i] = ret[i][j];
			}
		}
    	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], M[1], w*M[2]};
// 	}
	
    static MatrixRT crossSectionDirectionScaling(double w, MatrixRT M){
		return {{M[0][0], M[0][1], w*M[0][2]},
                {M[1][0], M[1][1], w*M[1][2]},
                {M[2][0], M[2][1], w*M[2][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.0;
		for (int i=0; i<3; i++)
		    for (int j=0; j<3; j++)
		    	sum += M1[i][j] * M2[i][j];
	    return sum;
	}


	/**
	 * @brief Determines rotation matrix based on an axis and an angle
	 * 
	 * @param axis 
	 * @param angle 
	 * @return MatrixRT 
	 */
	static MatrixRT rotationMatrix(int axis, double angle){

		switch (axis)
		{
			case 0:
			{
				return {{1.0,          0,            0   },
						{  0, cos(angle), -1.0*sin(angle)},
						{  0, sin(angle),      cos(angle)}};
			}
			case 1:
			{
				return {{     cos(angle),   0, sin(angle)},
						{              0, 1.0,      0    },
						{-1.0*sin(angle),   0, cos(angle)}};
			}
			case 2:
			{
				return {{cos(angle), -1.0*sin(angle),    0},
						{sin(angle),      cos(angle),    0},
						{         0,               0,  1.0}};
			}
			default:
				DUNE_THROW(Dune::Exception, " axis not feasible. rotationMatrix is only implemented for 3x3-matrices. Choose between 0: x-axis, 1: y-axis, 2: z-axis");
		}
	}

	/**
	 * @brief 6x6 matrix that transforms the strain tensor. This matrix is used to 
	 * 		  transform the compliance matrix (given in Voigt notation) into another frame.
	 * 		  see 'https://en.wikipedia.org/wiki/Orthotropic_material#Condition_for_material_symmetry_2' 
	 * 		  for details.
	 * 
	 * @param axis 
	 * @param angle 
	 * @return MatrixRT 
	 */
	static Dune::FieldMatrix<double,6,6> rotationMatrixCompliance(int axis, double angle){
		
		MatrixRT R = rotationMatrix(axis,angle);

        return {{    R[0][0]*R[0][0],     R[0][1]*R[0][1],     R[0][2]*R[0][2],                 R[0][1]*R[0][2],                 R[0][0]*R[0][2],                 R[0][0]*R[0][1]},
                {    R[1][0]*R[1][0],     R[1][1]*R[1][1],     R[1][2]*R[1][2],                 R[1][1]*R[1][2],                 R[1][0]*R[1][2],                 R[1][0]*R[1][1]},
                {    R[2][0]*R[2][0],     R[2][1]*R[2][1],     R[2][2]*R[2][2],                 R[2][1]*R[2][2],                 R[2][0]*R[2][2],                 R[2][0]*R[2][1]},
                {2.0*R[1][0]*R[2][0], 2.0*R[1][1]*R[2][1], 2.0*R[1][2]*R[2][2], R[1][1]*R[2][2]+R[1][2]*R[2][1], R[1][0]*R[2][2]+R[1][2]*R[2][0], R[1][0]*R[2][1]+R[1][1]*R[2][0]},
                {2.0*R[0][0]*R[2][0], 2.0*R[0][1]*R[2][1], 2.0*R[0][2]*R[2][2], R[0][1]*R[2][2]+R[0][2]*R[2][1], R[0][0]*R[2][2]+R[0][2]*R[2][0], R[0][0]*R[2][1]+R[0][1]*R[2][0]},
                {2.0*R[0][0]*R[1][0], 2.0*R[0][1]*R[1][1], 2.0*R[0][2]*R[1][2], R[0][1]*R[1][2]+R[0][2]*R[1][1], R[0][0]*R[1][2]+R[0][2]*R[1][0], R[0][0]*R[1][1]+R[0][1]*R[1][0]}
               };
	}


	/**
	 * @brief  Scale last three columns of stiffness matrix by a factor of 2.
	 * 		   Inserting this facotr in the stiffness matrix allows to use the 
	 * 		   same Matrix-to-Vector mapping for both the stress and the strain.
	 * 
	 * @param S  Stiffness matrix
	 */
	static void scaleStiffnessMatrix(Dune::FieldMatrix<double,6,6>& S){
      for(size_t i = 0; i<6; i++) 
      for(size_t j = 3; j<6; j++)
      {
        S[i][j] = 2.0*S[i][j];
      }
	}

// 	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,sym(E2));
// 	}
// // 	
    static double linearizedStVenantKirchhoffDensity(double mu, double lambda, MatrixRT E1, MatrixRT E2)  // CHANGED
    {  
        auto t1 = 2.0 * mu * sym(E1) + MatrixRT(Dune::ScaledIdentityMatrix<double,3>(lambda * trace(sym(E1))));
        auto tmp1 = scalarProduct(t1,sym(E2));
        // auto tmp1 = scalarProduct(t1,E2);
//         auto t2 = 2.0 * mu * sym(E2) + lambda * trace(sym(E2)) * Id();
//         auto tmp2 = scalarProduct(t2,sym(E1));
//         
//         if(abs(tmp1-tmp2) > 1e-8 )
//         {
//             std::cout << "linearizedStVenantKirchhoffDensity NOT Symmetric!" << std::endl;
//             std::cout << "tmp1: " << tmp1 << std::endl;
//             std::cout << "tmp2: " << tmp2 << std::endl;
//         }
        return tmp1; 
        
	}

	// static MatrixRT elasticityTensor()
	// {




	// }

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

    
// 	static double linearizedStVenantKirchhoffDensity(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 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;
	}




	extern "C" 
	{

		MatrixRT new_sym(MatrixRT M)
		{
			return sym(M);
		}





	}







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