// -*- c++ -*- (enables emacs c++ mode)
//===========================================================================
//
// Copyright (C) 2002-2008 Yves Renard, Julien Pommier.
//
// This file is a part of GETFEM++
//
// Getfem++  is  free software;  you  can  redistribute  it  and/or modify it
// under  the  terms  of the  GNU  Lesser General Public License as published
// by  the  Free Software Foundation;  either version 2.1 of the License,  or
// (at your option) any later version.
// This program  is  distributed  in  the  hope  that it will be useful,  but
// WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
// or  FITNESS  FOR  A PARTICULAR PURPOSE.  See the GNU Lesser General Public
// License for more details.
// You  should  have received a copy of the GNU Lesser General Public License
// along  with  this program;  if not, write to the Free Software Foundation,
// Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301, USA.
//
//===========================================================================

/**
 * Goal : scalar Dirichlet problem with Xfem.
 *
 * Research program.
 */

#include <iostream>
#include <fstream>
using namespace std;


#include "getfem/getfem_assembling.h" /* import assembly methods      */
#include "getfem/getfem_export.h"     /* export functions             */
#include "getfem/getfem_derivatives.h"
#include "getfem/getfem_regular_meshes.h"
#include "getfem/getfem_model_solvers.h"
#include "getfem/getfem_mesh_im_level_set.h"
#include "getfem/getfem_partial_mesh_fem.h"
#include "getfem/getfem_Coulomb_friction.h"
#include "getfem/getfem_import.h"
#include "getfem/getfem_inter_element.h"
#include "gmm/gmm.h"

/* some Getfem++ types that we will be using */
using bgeot::base_small_vector; /* special class for small (dim<16) vectors */
using bgeot::base_vector;
using bgeot::base_node;  /* geometrical nodes(derived from base_small_vector)*/
using bgeot::scalar_type; /* = double */
using bgeot::short_type;  /* = short */
using bgeot::size_type;   /* = unsigned long */
using bgeot::dim_type;
using bgeot::base_matrix; /* small dense matrix. */

/* definition of some matrix/vector types. These ones are built
 * using the predefined types in Gmm++
 */
typedef getfem::modeling_standard_sparse_vector sparse_vector;
typedef getfem::modeling_standard_sparse_matrix sparse_matrix;
typedef getfem::modeling_standard_plain_vector  plain_vector;

typedef gmm::row_matrix<sparse_vector> sparse_row_matrix;

int Nxy;

int DIRICHLET_BOUNDARY_NUM = 0;
int NEUMANN_BOUNDARY_NUM = 1;

scalar_type nu;
double Radius;
int u_version;


/*
 * Exact solution for the velocity u
 */

  base_small_vector u_exact(const base_node &p){  
	  return base_small_vector	(							 
		 cos(M_PI*p[0])*sin(M_PI*p[1])	,
		 -sin(M_PI*p[0])*cos(M_PI*p[1])
		 ) ;  
  }
  

/* 
 * Exact solution for the pressure p
 */

  double p_exact(const base_node &p) {
      //double R = Radius;
      return (p[1]-0.5)*cos(2.0*M_PI*p[0])+(p[0]-0.5)*sin(2.0*M_PI*p[1]);
  }


/*
 * Right-hand-side corresponding to the exact solution
 */

  base_small_vector rhs(const base_node &p){
    return  base_small_vector( 
	    nu*2.0*M_PI*M_PI*cos(M_PI*p[0])*sin(M_PI*p[1]) -2.0*M_PI*(p[1]-0.5)*sin(2.0*M_PI*p[0])+sin(2.0*M_PI*p[1]) ,
	    -nu*2.0*M_PI*M_PI*sin(M_PI*p[0])*cos(M_PI*p[1]) +2.0*M_PI*(p[0]-0.5)*cos(2.0*M_PI*p[1])+cos(2.0*M_PI*p[0])
	    );
  }


/*
 * The level-set : a circle of radius R = Radius
 */

  double ls_value(const base_node &p) {
      double R = Radius;
      return  (p[0]-0.5)*(p[0]-0.5) + (p[1]-0.5)*(p[1]-0.5)- R*R;
  }
  

/*
 * Test procedure
 */

void test_mim(getfem::mesh_im_level_set &mim, getfem::mesh_fem &mf_rhs,
	      bool bound) {
  if (!u_version) {
    unsigned N = unsigned(mim.linked_mesh().dim());
    size_type nbdof = mf_rhs.nb_dof();
    plain_vector V(nbdof), W(1);
    std::fill(V.begin(), V.end(), 1.0);
    
    getfem::generic_assembly assem("u=data(#1); V()+=comp(Base(#1))(i).u(i);");
    assem.push_mi(mim); assem.push_mf(mf_rhs); assem.push_data(V);
    assem.push_vec(W);
    assem.assembly(getfem::mesh_region::all_convexes());
    double exact(0), R2 = Radius*Radius, R3 = R2*Radius;
    switch (N) {
      case 1: exact = bound ? 1.0 : 2.0*Radius; break;
      case 2: exact = bound ? Radius*M_PI : R2*M_PI; break;
      case 3: exact = bound ? 2.0*M_PI*R2 : 4.0*M_PI*R3/3.0; break;
      default: assert(N <= 3);
    }
    if (bound) cout << "Boundary length: "; else cout << "Area: ";
    cout << W[0] << " should be " << exact << endl;
    assert(gmm::abs(exact-W[0])/exact < 0.01); 
  }
}

/*
 * Assembly of stabilization terms
 */


template<typename VECT1> class level_set_unit_normal 
  : public getfem::nonlinear_elem_term {
  const getfem::mesh_fem &mf;
  const VECT1 &U;
  size_type N;
  base_matrix gradU;
  bgeot::base_vector coeff;
  bgeot::multi_index sizes_;
public:
  level_set_unit_normal(const getfem::mesh_fem &mf_, const VECT1 &U_) 
    : mf(mf_), U(U_), N(mf_.linked_mesh().dim()), gradU(1, N)
  { sizes_.resize(1); sizes_[0] = short_type(N); }
  const bgeot::multi_index &sizes() const {  return sizes_; }
  virtual void compute(getfem::fem_interpolation_context& ctx,
		       bgeot::base_tensor &t) {
    size_type cv = ctx.convex_num();
    coeff.resize(mf.nb_basic_dof_of_element(cv));
    gmm::copy(gmm::sub_vector(U, gmm::sub_index(mf.ind_basic_dof_of_element(cv))),
	      coeff);
    ctx.pf()->interpolation_grad(ctx, coeff, gradU, 1);
    scalar_type norm = gmm::vect_norm2(gmm::mat_row(gradU, 0));
    for (size_type i = 0; i < N; ++i) t[i] = - gradU(0, i) / norm;
    
  }
};


template<class MAT>
void asm_stabilization_mixed_term
(const MAT &RM_, const getfem::mesh_im &mim, const getfem::mesh_fem &mf, 
 const getfem::mesh_fem &mf_mult, getfem::level_set &ls,
 const getfem::mesh_region &rg = getfem::mesh_region::all_convexes()) {
  MAT &RM = const_cast<MAT &>(RM_);

  level_set_unit_normal<std::vector<scalar_type> >
    nterm(ls.get_mesh_fem(), ls.values());

  getfem::generic_assembly assem("t=comp(vBase(#2).vGrad(#1).NonLin(#3));"
				 "M(#2, #1)+= t(:,j,:,j,i,i)");
  assem.push_mi(mim);
  assem.push_mf(mf);
  assem.push_mf(mf_mult);
  assem.push_mf(ls.get_mesh_fem());
  assem.push_mat(RM);
  assem.push_nonlinear_term(&nterm);
  assem.assembly(rg);
}


template<class MAT>
void asm_stabilization_symm_term(const MAT &RM_, const getfem::mesh_im &mim, const getfem::mesh_fem &mf, 
 getfem::level_set &ls,const getfem::mesh_region &rg = getfem::mesh_region::all_convexes()) {

  MAT &RM = const_cast<MAT &>(RM_);

  level_set_unit_normal<std::vector<scalar_type> > nterm(ls.get_mesh_fem(), ls.values());

  getfem::generic_assembly assem("t=comp(vGrad(#1).NonLin(#2).vGrad(#1).NonLin(#2));"
	  "M(#1, #1)+= sym(t(:,k,i,i,:,k,j,j))");

  assem.push_mi(mim);
  assem.push_mf(mf);
  assem.push_mf(ls.get_mesh_fem());
  assem.push_mat(RM);
  assem.push_nonlinear_term(&nterm);
  assem.assembly(rg);
}





template<class MAT>
void asm_mass_matrix_pl(const MAT &RM_, const getfem::mesh_im &mim,  const getfem::mesh_fem &mf_p, const getfem::mesh_fem &mf, 
						 getfem::level_set &ls,
						 const getfem::mesh_region &rg = getfem::mesh_region::all_convexes()) {
	MAT &RM = const_cast<MAT &>(RM_);
	
	level_set_unit_normal<std::vector<scalar_type> > nterm(ls.get_mesh_fem(), ls.values());
	
	getfem::generic_assembly assem("t=comp(Base(#1).vBase(#2).NonLin(#3));"
		  "M(#1, #2)+= t(:,:,i,i)");

	assem.push_mi(mim);
	assem.push_mf(mf_p);
	assem.push_mf(mf);
	assem.push_mf(ls.get_mesh_fem());
	assem.push_mat(RM);
	assem.push_nonlinear_term(&nterm);
	assem.assembly(rg);
}
					


template<class MAT>
void asm_mass_matrix_up(const MAT &RM_, const getfem::mesh_im &mim, const getfem::mesh_fem &mf, const getfem::mesh_fem &mf_p,
					getfem::level_set &ls,
					const getfem::mesh_region &rg = getfem::mesh_region::all_convexes()) {
	MAT &RM = const_cast<MAT &>(RM_);
	
	level_set_unit_normal<std::vector<scalar_type> > nterm(ls.get_mesh_fem(), ls.values());

	getfem::generic_assembly assem("t=comp(vGrad(#2).NonLin(#3).Base(#1).NonLin(#3));"
		  "M(#2, #1)+= t(:,i,j,j,:,i)");
	assem.push_mi(mim);
	assem.push_mf(mf_p);
	assem.push_mf(mf);
	assem.push_mf(ls.get_mesh_fem());
	assem.push_mat(RM);
	assem.push_nonlinear_term(&nterm);
	assem.assembly(rg);
}

/*
 * Elementary extrapolation matrices
 */

void compute_mass_matrix_extra_element(base_matrix &M, const getfem::mesh_im &mim, const getfem::mesh_fem &mf,
 size_type cv1, size_type cv2) {

  getfem::pfem pf1_old = 0;
  static getfem::pfem_precomp pfp1 = 0;
  static getfem::papprox_integration pai1_old = 0;
  bgeot::geotrans_inv_convex gic;
  bgeot::base_tensor t1, t2;
  getfem::base_matrix G1, G2;
  
  const getfem::mesh &m(mf.linked_mesh());
  
  GMM_ASSERT1(mf.convex_index().is_in(cv1) && mim.convex_index().is_in(cv1) &&
	      mf.convex_index().is_in(cv2) && mim.convex_index().is_in(cv2),
	      "Bad element");
    
  bgeot::pgeometric_trans pgt1 = m.trans_of_convex(cv1);
  getfem::pintegration_method pim1 = mim.int_method_of_element(cv1);
  getfem::papprox_integration pai1 =
    getfem::get_approx_im_or_fail(pim1);
  getfem::pfem pf1 = mf.fem_of_element(cv1);
  size_type nbd1 = pf1->nb_dof(cv1);
  
  if (pf1 != pf1_old || pai1 != pai1_old) {
    pfp1 = fem_precomp(pf1, &pai1->integration_points(), pim1);
    pf1_old = pf1; pai1_old = pai1;
  }
  
  bgeot::vectors_to_base_matrix(G1, m.points_of_convex(cv1));
  getfem::fem_interpolation_context ctx1(pgt1, pfp1, 0, G1, cv1,size_type(-1));
  
  getfem::pfem pf2 = mf.fem_of_element(cv2);
  size_type nbd2 = pf1->nb_dof(cv2);
  base_node xref2(pf2->dim());
  bgeot::pgeometric_trans pgt2 = m.trans_of_convex(cv2);
  gic.init(m.points_of_convex(cv2), pgt2);

  gmm::resize(M, nbd1, nbd2); gmm::clear(M);

  bgeot::vectors_to_base_matrix(G2, m.points_of_convex(cv2));
  
  getfem::fem_interpolation_context ctx2(pgt2, pf2, base_node(pgt2->dim()),
					 G2, cv2, size_type(-1));

  for (unsigned ii=0; ii < pai1->nb_points_on_convex(); ++ii) {
    ctx1.set_ii(ii);
    scalar_type coeff = pai1->integration_coefficients()[ii] * ctx1.J();
    bool converged;
    gic.invert(ctx1.xreal(), xref2, converged);
    GMM_ASSERT1(converged, "geometric transformation not well inverted ... !");
    
    ctx2.set_xref(xref2);

    pf1->real_base_value(ctx1, t1);
    pf2->real_base_value(ctx2, t2);
    
    for (size_type i = 0; i < nbd1; ++i)
      for (size_type j = 0; j < nbd2; ++j)
	M(i,j) += t1[i] * t2[j] * coeff;
  }
  
}



/* 
 * Main program 
 */

int main(int argc, char *argv[]) {

  GMM_SET_EXCEPTION_DEBUG; // Exceptions make a memory fault, to debug.
  FE_ENABLE_EXCEPT;        // Enable floating point exception for Nan.
    
  // Read parameters.
  bgeot::md_param PARAM;
  PARAM.read_command_line(argc, argv);
  nu = double(PARAM.real_value("NU", "Viscosité"));
  u_version = int(PARAM.int_value("EXACT_SOL", "Which exact solution"));
  
  
  // Load the mesh
  getfem::mesh mesh;
 
   std::string MESH_FILE = PARAM.string_value("MESH_FILE", "Mesh file");
   getfem::import_mesh(MESH_FILE, mesh);

  dim_type N = mesh.dim();
  
  // center the mesh in (0, 0).
  base_node Pmin(N), Pmax(N);
  mesh.bounding_box(Pmin, Pmax);
  Pmin += Pmax; Pmin /= -2.0;
  
  scalar_type h = mesh.minimal_convex_radius_estimate();
  cout << "h = " << h << endl;
  
/*
 * Level set definition
 */

  unsigned lsdeg = unsigned(PARAM.int_value("LEVEL_SET_DEGREE", "level set degree"));
  bool simplify_level_set = 
    (PARAM.int_value("SIMPLIFY_LEVEL_SET",
		     "simplification or not of the level sets") != 0);
  Radius = PARAM.real_value("RADIUS", "Domain radius");
  getfem::level_set ls(mesh, dim_type(lsdeg));

  const getfem::mesh_fem &lsmf = ls.get_mesh_fem();
  for (unsigned i = 0; i < lsmf.nb_dof(); ++i) {
    ls.values()[i] = ls_value(lsmf.point_of_basic_dof(i));
	  }
 
	
  if (simplify_level_set) {
    scalar_type simplify_rate = std::min(0.03, 0.05 * sqrt(h));
    cout << "Simplification of level sets with rate: " <<
      simplify_rate << endl;
    ls.simplify(simplify_rate);

  }
  
	getfem::mesh_level_set mls(mesh);
  mls.add_level_set(ls);
  mls.adapt();
  
  getfem::mesh mcut;
  mls.global_cut_mesh(mcut);
  mcut.write_to_file("cut.mesh");
	

  // Integration method on the domain
  std::string IM = PARAM.string_value("IM", "Mesh file");
  std::string IMS = PARAM.string_value("IM_SIMPLEX", "Mesh file");
  int intins = getfem::mesh_im_level_set::INTEGRATE_OUTSIDE;
  getfem::mesh_im uncutmim(mesh);
  uncutmim.set_integration_method(mesh.convex_index(),
				  getfem::int_method_descriptor(IM));
  getfem::mesh_im_level_set mim(mls, intins,
				getfem::int_method_descriptor(IMS));
  mim.set_integration_method(mesh.convex_index(),
			     getfem::int_method_descriptor(IM));
  mim.adapt();
  
  
  // Integration methods on the boudary
  int intbound = getfem::mesh_im_level_set::INTEGRATE_BOUNDARY;
  getfem::mesh_im_level_set mimbound(mls, intbound,
					 getfem::int_method_descriptor(IMS));
  mimbound.set_integration_method(mesh.convex_index(),
				      getfem::int_method_descriptor(IM));
  mimbound.adapt();

  // Finite element method for the unknown velocity u
  getfem::mesh_fem pre_mf(mesh);
  std::string FEM = PARAM.string_value("FEM", "finite element method");
  pre_mf.set_qdim(N);
  pre_mf.set_finite_element(mesh.convex_index(),
			    getfem::fem_descriptor(FEM));
	
  getfem::partial_mesh_fem mf(pre_mf);
	
  dal::bit_vector kept_dof = select_dofs_from_im(pre_mf, mim);
	
  dal::bit_vector rejected_elt;
  for (dal::bv_visitor cv(mim.convex_index()); !cv.finished(); ++cv)
    if (mim.int_method_of_element(cv) == getfem::im_none())
      rejected_elt.add(cv);
  mf.adapt(kept_dof, rejected_elt);
  size_type nb_dof = mf.nb_dof();
  cout << "nb_dof = " << nb_dof << endl;
  
  // Finite element method for the pressure p
  getfem::mesh_fem pre_mf_p(mesh);
  std::string FEM_p = PARAM.string_value("FEM_p", "fem for pressure");
  pre_mf_p.set_finite_element(mesh.convex_index(),
				 getfem::fem_descriptor(FEM_p));
  getfem::partial_mesh_fem mf_p(pre_mf_p);
  dal::bit_vector kept_dof_p = select_dofs_from_im(pre_mf_p, mim);
  dal::bit_vector rejected_elt_p;
  for (dal::bv_visitor cv(mim.convex_index()); !cv.finished(); ++cv)
    if (mim.int_method_of_element(cv) == getfem::im_none())
      rejected_elt_p.add(cv);
  mf_p.adapt(kept_dof_p, rejected_elt_p);

  size_type nb_dof_pre_p = pre_mf_p.nb_dof();
  size_type nb_dof_p = mf_p.nb_dof();
  cout << "nb_dof_p = " << nb_dof_p << endl;
  
  // Finite element method for the rhs
  getfem::mesh_fem mf_rhs(mesh);
  std::string FEMR = PARAM.string_value("FEM_RHS", "finite element method");
  mf_rhs.set_qdim(N);
  mf_rhs.set_finite_element(mesh.convex_index(),
			    getfem::fem_descriptor(FEMR));
  size_type nb_dof_rhs = mf_rhs.nb_dof();
  cout << "nb_dof_rhs = " << nb_dof_rhs << endl;
  
  // A P0 finite element
  const getfem::mesh_fem &mf_P0 = getfem::classical_mesh_fem(mesh, 0);
  
  // Finite element method for the multipliers
  getfem::mesh_fem pre_mf_mult(mesh);
  std::string FEMM = PARAM.string_value("FEM_MULT", "fem for multipliers");
  pre_mf_mult.set_qdim(N);
  pre_mf_mult.set_finite_element(mesh.convex_index(),
				 getfem::fem_descriptor(FEMM));
  getfem::partial_mesh_fem mf_mult(pre_mf_mult);
	
	
	
	
	
  dal::bit_vector kept_dof_mult
    = select_dofs_from_im(pre_mf_mult, mimbound, N-1);
  mf_mult.adapt(kept_dof_mult, rejected_elt);
	
	
  size_type nb_dof_mult = mf_mult.nb_dof();
  cout << "nb_dof_mult = " << nb_dof_mult << endl;

	
	
  // Mass matrix on the boundary
  sparse_matrix B2(mf_rhs.nb_dof(), pre_mf.nb_dof());
  getfem::asm_mass_matrix(B2, mimbound, mf_rhs, pre_mf);
	
  sparse_matrix B(nb_dof_mult, nb_dof);
  getfem::asm_mass_matrix(B, mimbound, mf_mult, mf);
	
  int stabilized_dirichlet =
    int(PARAM.int_value("STABILIZED_DIRICHLET", "Stabilized version of "
			"Dirichlet condition or not"));
  scalar_type dir_gamma0(0);
  sparse_matrix MA(nb_dof_mult, nb_dof_mult);
  sparse_matrix KA(nb_dof, nb_dof);
  sparse_matrix BA(nb_dof_mult, nb_dof);
	
  // Stabilization of the pressure
  sparse_matrix Mup(nb_dof, nb_dof_p);
  sparse_matrix Mpl(nb_dof_p, nb_dof_mult);
  sparse_matrix Mpp(nb_dof_p, nb_dof_p);
	
  asm_mass_matrix_up(Mup,mimbound,mf,mf_p,ls);
  asm_mass_matrix_pl(Mpl,mimbound,mf_p,mf_mult,ls);
  getfem::asm_mass_matrix(Mpp,mimbound,mf_p,mf_p);
		
	
  if (stabilized_dirichlet > 0) {

    sparse_row_matrix E1(nb_dof, nb_dof);

    if (stabilized_dirichlet == 2) {
      // Computation of the extrapolation operator
      scalar_type min_ratio =
	PARAM.real_value("MINIMAL_ELT_RATIO",
			 "Threshold ratio for the fully stab Dirichlet");

      cout << "Computation of the extrapolation operator" << endl;
      dal::bit_vector elt_black_list, dof_black_list;
      size_type nbe = mf_P0.nb_dof();
      plain_vector ratios(nbe);
      sparse_matrix MC1(nbe, nbe), MC2(nbe, nbe);
      getfem::asm_mass_matrix(MC1, mim, mf_P0);
      getfem::asm_mass_matrix(MC2, uncutmim, mf_P0);
      for (size_type i = 0; i < nbe; ++i) {
	size_type cv = mf_P0.first_convex_of_basic_dof(i);
	ratios[cv] = gmm::abs(MC1(i,i)) / gmm::abs(MC2(i,i));
	if (ratios[cv] > 0 && ratios[cv] < min_ratio) elt_black_list.add(cv);
      }
      
	
      sparse_matrix EO(nb_dof, nb_dof);
      sparse_row_matrix T1(nb_dof, nb_dof), EX(nb_dof, nb_dof);
      asm_mass_matrix(EO, uncutmim, mf);

      for (size_type i = 0; i < nb_dof; ++i) {
	bool found = false;
	getfem::mesh::ind_cv_ct ct = mf.convex_to_basic_dof(i);
	getfem::mesh::ind_cv_ct::const_iterator it;
	for (it = ct.begin(); it != ct.end(); ++it)
	  if (!elt_black_list.is_in(*it)) found = true;
	if (found)
	  { gmm::clear(gmm::mat_col(EO, i)); EO(i,i) = scalar_type(1); }
	else
	  dof_black_list.add(i);
      }

      bgeot::mesh_structure::ind_set is;
      base_matrix Mloc;
      for (dal::bv_visitor i(elt_black_list); !i.finished(); ++i) {
	mesh.neighbours_of_convex(i, is);
	size_type cv2 = size_type(-1);
	scalar_type ratio = scalar_type(0);
	for (size_type j = 0; j < is.size(); ++j) {
	  scalar_type r = ratios[is[j]];
	  if (r > ratio) { ratio = r; cv2 = is[j]; }
	}
	GMM_ASSERT1(cv2 != size_type(-1), "internal error");
	compute_mass_matrix_extra_element(Mloc, uncutmim, mf, i, cv2);
	for (size_type ii = 0; ii < gmm::mat_nrows(Mloc); ++ii) 
	  for (size_type jj = 0; jj < gmm::mat_ncols(Mloc); ++jj)
	    EX(mf.ind_basic_dof_of_element(i)[ii], mf.ind_basic_dof_of_element(cv2)[jj])
	      += Mloc(ii, jj);
      }

      gmm::copy(gmm::identity_matrix(), E1);
      gmm::copy(gmm::identity_matrix(), T1);
      for (dal::bv_visitor i(dof_black_list); !i.finished(); ++i)
	gmm::copy(gmm::mat_row(EX, i), gmm::mat_row(T1, i));

      plain_vector BE(nb_dof), BS(nb_dof);
      for (dal::bv_visitor i(dof_black_list); !i.finished(); ++i) {
	BE[i] = scalar_type(1);
	
	double rcond; 
	gmm::SuperLU_solve(EO, BS, BE, rcond);
	gmm::mult(gmm::transposed(T1), BS, gmm::mat_row(E1, i));
	BE[i] = scalar_type(0);
      }
      gmm::clean(E1, 1e-13);
      
      cout << "Extrapolation operator computed" << endl;

    }

    dir_gamma0 = PARAM.real_value("DIRICHLET_GAMMA0",
				  "Stabilization parameter for "
				  "Dirichlet condition");
    getfem::asm_mass_matrix(MA, mimbound, mf_mult);
    asm_stabilization_mixed_term(BA, mimbound, mf, mf_mult, ls);
    asm_stabilization_symm_term(KA, mimbound, mf, ls);
    gmm::scale(MA, -dir_gamma0 * h);
    gmm::scale(BA, -dir_gamma0 * h);
    gmm::scale(KA, -dir_gamma0 * h);

	gmm::scale(Mup,  dir_gamma0 * h);
	gmm::scale(Mpp, -dir_gamma0 * h);  
	gmm::scale(Mpl, dir_gamma0 * h); 
        	  
	  
    if (stabilized_dirichlet == 2) {
      sparse_matrix A1(nb_dof_mult, nb_dof);
      gmm::copy(BA, A1);
      gmm::mult(gmm::transposed(E1), gmm::transposed(A1), gmm::transposed(BA));
      sparse_matrix A2(nb_dof, nb_dof);
      gmm::mult(gmm::transposed(E1), KA, A2);
      gmm::mult(gmm::transposed(E1), gmm::transposed(A2), gmm::transposed(KA));
    }
  
     gmm::add(BA, B);

  }

  // Tests
  test_mim(mim, mf_rhs, false);
  test_mim(mimbound, mf_rhs, true);

  /* 
   * Choose boundaries
   */
  getfem::mesh_region r;
  getfem::outer_faces_of_mesh(mesh,r);

  for (getfem::mr_visitor i(r); !i.finished(); ++i) {
     mesh.region(DIRICHLET_BOUNDARY_NUM).add(i.cv(),i.f());
  }


	
  // Model system

  getfem::model MS;


  // Linearized elasticity brick
  MS.add_initialized_fixed_size_data("nu", plain_vector(1, nu));
  MS.add_fem_variable("u", mf);
  getfem::add_generic_elliptic_brick(MS, mim, "u", "nu");

  // Linearized incompressibility condition brick
  MS.add_fem_variable("p", mf_p);  // Adding the pressure as variable
  add_linear_incompressibility(MS, mim, "u", "p");	

  MS.add_fixed_size_variable("p_cond", 1);

  plain_vector Cv(nb_dof_p);
  plain_vector Aire(nb_dof_p);
  for (size_type i = 0; i < nb_dof_p; ++i) {
       Aire[i] = 1.0;
  }
  getfem::asm_source_term(Cv, mim, mf_p, mf_p, Aire);
  plain_vector C_rhs(1);
  C_rhs[0] = 0.0;
  getfem::add_explicit_matrix(MS, "p_cond", "p", gmm::row_vector(Cv));
  getfem::add_explicit_matrix(MS, "p", "p_cond", gmm::transposed(gmm::row_vector(Cv)));
  getfem::add_explicit_rhs(MS, "p_cond", C_rhs);


  // Dirichlet condition on the box boundary
  plain_vector Hbox(nb_dof_rhs);
  getfem::interpolation_function(mf_rhs, Hbox, u_exact);
  plain_vector Hbox2(nb_dof);
  getfem::interpolation(mf_rhs, mf, Hbox, Hbox2);
  MS.add_initialized_fem_data("Dirichletdata", mf, Hbox2);
  getfem::add_Dirichlet_condition_with_multipliers(MS, mim, "u", mf, DIRICHLET_BOUNDARY_NUM, "Dirichletdata");


  // Dirichlet condition on the level-set
  MS.add_fem_variable("LAMBDA", mf_mult);
  plain_vector H(nb_dof_rhs);
  getfem::interpolation_function(mf_rhs, H, u_exact);	
  plain_vector H3(nb_dof_mult);
  getfem::asm_source_term(H3,mimbound,mf_mult,mf_rhs,H);
  getfem::add_constraint_with_multipliers(MS, "u", "LAMBDA", B, H3);
	
  if (stabilized_dirichlet > 0){
       getfem::add_explicit_matrix(MS, "LAMBDA", "LAMBDA", MA);
       getfem::add_explicit_matrix(MS, "u", "u", KA);
       getfem::add_explicit_matrix(MS, "p", "p", Mpp); 
       getfem::add_explicit_matrix(MS, "p", "u", gmm::transposed(Mup));
       getfem::add_explicit_matrix(MS, "u", "p", Mup);
       getfem::add_explicit_matrix(MS, "p", "LAMBDA", Mpl);
       getfem::add_explicit_matrix(MS, "LAMBDA", "p", gmm::transposed(Mpl));        
  }


  // Volumic source term
  plain_vector F(nb_dof_rhs); 
  getfem::interpolation_function(mf_rhs, F, rhs);
  MS.add_initialized_fem_data("VolumicData", mf_rhs, F);
  getfem::add_source_term_brick(MS, mim, "u", "VolumicData");

  // Solving the problem
  gmm::iteration iter(1e-9, 1, 40000);
  getfem::standard_solve(MS, iter);
  plain_vector U(nb_dof);
  gmm::copy(MS.real_variable("u"), U);
	
  plain_vector pressure(nb_dof_p);
  gmm::copy(MS.real_variable("p"), pressure);

  plain_vector LAMBDA(nb_dof_mult);
  gmm::copy(MS.real_variable("LAMBDA"), LAMBDA);

  // interpolation of the solution on mf_rhs
  plain_vector Urhs(nb_dof_rhs), Eint(nb_dof_rhs), Vex(nb_dof_rhs), Uex(nb_dof), Pex(nb_dof_pre_p) /*, Mex(nb_dof_mult), M(nb_dof_rhs)*/;
  
  getfem::interpolation_function(mf_rhs, Vex, u_exact);
  getfem::interpolation(mf_rhs, mf, Vex, Uex);

  getfem::interpolation(mf, mf_rhs, U, Urhs);

  getfem::interpolation_function(pre_mf_p, Pex, p_exact);
	
  for (size_type i = 0; i < nb_dof_rhs; ++i) {
       Eint[i] = gmm::abs(Urhs[i] - Vex[i]);
  }


  // computation of error on u.
  double errmax = 0.0, exactmax = 0.0;
  for (size_type i = 0; i < nb_dof_rhs; ++i)
    if (ls_value(mf_rhs.point_of_basic_dof(i)) >= 0.0) {
      errmax = std::max(errmax, Eint[i]);
      exactmax = std::max(exactmax, Vex[i]);
    }
    else Eint[i] = 0.0;

  cout << "Linfty error: (absolute) " << errmax << \
						"(relative) " <<100.0 * errmax / exactmax << "%" << endl;

  double erruL2 = 100.0 * getfem::asm_L2_dist(mim, mf, U, mf_rhs, Vex) / getfem::asm_L2_norm(mim, mf_rhs, Vex);
  cout << "L2 error: " << getfem::asm_L2_dist(mim, mf, U, mf_rhs, Vex) << "(relative) " << erruL2 << "%" << endl;
	
  double erruH1 = 100.0 * getfem::asm_H1_dist(mim, mf, U, mf_rhs, Vex) / getfem::asm_H1_norm(mim, mf_rhs, Vex);
  cout << "H1 error: " << getfem::asm_H1_dist(mim, mf, U, mf_rhs, Vex) << "(relative) " << erruH1 << "%" << endl;

  // computation of error on p.

  double errpL2 = 100.0 * getfem::asm_L2_dist(mim, mf_p, pressure, pre_mf_p, Pex) / getfem::asm_L2_norm(mim, pre_mf_p, Pex);
  cout << "L2 error on the pressure: " << errpL2 << "%" << endl;

  // computation of error on multipliers.

  gmm::resize(KA, nb_dof_rhs, nb_dof_rhs);  gmm::clear(KA);
  asm_stabilization_symm_term(KA, mimbound, mf_rhs, ls);

  gmm::resize(Mpp, nb_dof_pre_p, nb_dof_pre_p);  gmm::clear(Mpp);
  getfem::asm_mass_matrix(Mpp, mimbound, pre_mf_p);

  gmm::resize(MA, nb_dof_mult, nb_dof_mult); gmm::clear(MA);
  getfem::asm_mass_matrix(MA, mimbound, mf_mult);

  gmm::resize(Mup, nb_dof_rhs, nb_dof_pre_p); gmm::clear(Mup);
  asm_mass_matrix_up(Mup, mimbound, mf_rhs, pre_mf_p, ls);

  gmm::resize(Mpl, nb_dof_pre_p, nb_dof_mult); gmm::clear(Mpl);
  asm_mass_matrix_pl(Mpl, mimbound, pre_mf_p, mf_mult, ls);

  gmm::resize(BA, nb_dof_mult, nb_dof_rhs); gmm::clear(BA);
  asm_stabilization_mixed_term(BA, mimbound, mf_rhs, mf_mult, ls);

  scalar_type err_l2_mult =
    ( gmm::vect_sp(MA, LAMBDA, LAMBDA) + gmm::vect_sp(KA, Vex, Vex) + gmm::vect_sp(Mpp, Pex, Pex)
      + 2 * gmm::vect_sp(BA, Vex, LAMBDA) + 2* gmm::vect_sp(Mup, Pex, Vex) - 2 * gmm::vect_sp(Mpl, LAMBDA, Pex) ) 
    / ( gmm::vect_sp(KA, Vex, Vex) + gmm::vect_sp(Mpp, Pex, Pex) + 2* gmm::vect_sp(Mup, Pex, Vex) );
    
  double errmL2 = gmm::sgn(err_l2_mult) * gmm::sqrt(gmm::abs(err_l2_mult)) * 100.0;
  cout << "L2 error on multipliers: " << errmL2 << "%" << endl;


  // exporting velocity solution in vtk format.
  {
    getfem::vtk_export exp("xfem_stokes_u.vtk", (2==1));
    exp.exporting(mf); 
    exp.write_point_data(mf, U, "solution_u");  
  }

  // exporting exact velocity solution in vtk format.
  {
    getfem::vtk_export exp("xfem_stokes_exact_u.vtk", (2==1));
    exp.exporting(mf); 
    exp.write_point_data(mf, Uex, "solution_exact_u");
  }

  // exporting error in vtk format.
  {
    getfem::vtk_export exp("xfem_stokes_error.vtk", (2==1));
    exp.exporting(mf_rhs); 
    exp.write_point_data(mf_rhs, Eint, "error");
  }

  // exporting pressure solution in vtk format.
  {
    getfem::vtk_export exp("xfem_stokes_p.vtk", (2==1));
    exp.exporting(mf_p); 
    exp.write_point_data(mf_p, pressure, "solution_p");  
  }

  // exporting exact pressure solution in vtk format.
  {
    getfem::vtk_export exp("xfem_stokes_exact_p.vtk", (2==1));
    exp.exporting(pre_mf_p); 
    exp.write_point_data(pre_mf_p, Pex, "solution_exact_p");
  }

  cout << "export done in vtk format" << endl;

  return 0; 
}