#include <gmm.h>
#include <getfem_mesh.h> 
#include <getfem_regular_meshes.h> 
#include <getfem_mesh_fem.h> 
#include <getfem_mesh_im.h> 
#include <getfem_model_solvers.h>
#include <getfem_import.h>
#include <getfem_export.h>
#include <getfem_nonlinear_elasticity.h>


// 1 = gmres with ilu, 2 = gmres with ilut, 3 = superlu
#define LSOLVER 1

template <typename MAT, typename VECT> 
  struct my_linear_solver
  : public getfem::abstract_linear_solver<MAT, VECT> {
    void operator ()(const MAT &M, VECT &x, const VECT &b,
		     gmm::iteration &iter)  const {

#if LSOLVER == 0
      gmm::identity_matrix P;
      gmm::gmres(M, x, b, P, 500, iter);
      if (!iter.converged()) DAL_WARNING2("gmres did not converge!");
#elif LSOLVER == 1
      gmm::ilu_precond<MAT> P(M);
      gmm::gmres(M, x, b, P, 500, iter);
      if (!iter.converged()) DAL_WARNING2("gmres did not converge!");
#elif LSOLVER == 2
      gmm::ilut_precond<MAT> P(M, 20, 1E-5);
      gmm::gmres(M, x, b, P, 500, iter);
      if (!iter.converged()) DAL_WARNING2("gmres did not converge!");
#elif LSOLVER == 3
      double rcond;
      SuperLU_solve(M, x, b, rcond);
      if (iter.get_noisy()) cout << "condition number: " << 1.0/rcond<< endl;
#endif
    }
  };


template <typename MODEL_STATE> void 
my_solve(MODEL_STATE &MS, getfem::mdbrick_abstract<MODEL_STATE> &problem,
 gmm::iteration &iter) {


  TYPEDEF_MODEL_STATE_TYPES;
  typename getfem::useful_types<MODEL_STATE>::plsolver_type
    lsolver(new my_linear_solver<T_MATRIX, VECTOR>);

  // see gmm_solver_Newton.h for parameters
  gmm::default_newton_line_search ls(size_t(-1), 5.0/3.0, 1.0/1000.0, 3.0/5.0);
  getfem::model_problem<MODEL_STATE> mdpb(MS, problem, ls);
  
  MS.adapt_sizes(problem);
  
  getfem::classical_Newton(mdpb, iter, *lsolver);
}





enum { TOP = 0, BOTTOM = 1};

int main() {

  DAL_SET_EXCEPTION_DEBUG; 
  FE_ENABLE_EXCEPT;
  dal::set_warning_level(1); // To avoid ilu Warnings.

  try {    

     getfem::mesh m;
     bgeot::base_node origine(0.0 , 0.0 , 0.0);
     std::vector <bgeot::base_small_vector> repere(3);

     // MESH **********************************************************************************
     cout << "Mesh\n";
     
     /*
     // ROUGH MESH
     repere[0]=bgeot::base_small_vector(0.01 , 0.0 , 0.0);
     repere[1]=bgeot::base_small_vector(0.0 , 0.01 , 0.0);
     repere[2]=bgeot::base_small_vector(0.0 , 0.0 , 0.01);

     std::vector<int> ref(3);

     ref[0]=ref[1]=5;
     ref[2]=10;
     // END
     */

     // FINE MESH
     repere[0]=bgeot::base_small_vector(0.005 , 0.0 , 0.0);
     repere[1]=bgeot::base_small_vector(0.0 , 0.005 , 0.0);
     repere[2]=bgeot::base_small_vector(0.0 , 0.0 , 0.005);

     std::vector<int> ref(3);

     ref[0]=ref[1]=10;
     ref[2]=20;
     // END

     getfem::parallelepiped_regular_simplex_mesh(m, 3, origine, repere.begin(), ref.begin() );

     
     // GROUPS ********************************************************************************
     cout << "Groups\n";
     getfem::mesh_region bord_libre;
     getfem::outer_faces_of_mesh(m, bord_libre);
     for (getfem::mr_visitor it(bord_libre); !it.finished(); ++it) {
       assert(it.is_face());
       bgeot::base_node un = m.normal_of_face_of_convex(it.cv(), it.f());
       un /= gmm::vect_norm2(un);
       if (gmm::abs(un[2] - 1.0) < 1.0E-7) { 
         m.region(TOP).add(it.cv(),it.f());
       } else if (gmm::abs(un[2] + 1.0) < 1.0E-7) { 
         m.region(BOTTOM).add(it.cv(),it.f());
       } 
     }


     // MEF ************************************************************************************
     cout << "MEF\n";
     getfem::mesh_fem mfu(m);
     getfem::mesh_fem mfp(m);
     getfem::mesh_fem mfdata(m);

     mfu.set_qdim(3);
     mfu.set_finite_element(m.convex_index(), getfem::fem_descriptor("FEM_PK(3,2)") );
     mfp.set_finite_element(m.convex_index(), getfem::fem_descriptor("FEM_PK(3,1)") );
     mfdata.set_finite_element(m.convex_index(), getfem::fem_descriptor("FEM_PK(3,2)") );     

     getfem::mesh_im mim(m);
     getfem::pintegration_method ppi = 
     getfem::int_method_descriptor("IM_TETRAHEDRON(6)");

     mim.set_integration_method(ppi);

     cout << "Number of dofs for u:" << mfu.nb_dof() << endl;
     cout << "Number of dofs for p:" << mfp.nb_dof() << endl;


     // BRICKS *********************************************************************************
     cout << "Bricks\n";
     bgeot::base_vector p(2); p[0] = 0.02; p[1] = 0.5;

     getfem::Mooney_Rivlin_hyperelastic_law HypLaw;
     getfem::mdbrick_nonlinear_elasticity<> elasbrick(HypLaw, mim, mfu, p  );
     getfem::mdbrick_nonlinear_incomp<> incompbrick(elasbrick, mfp);
     getfem::mdbrick_Dirichlet<> dirichbrick1(incompbrick, TOP);
     getfem::mdbrick_Dirichlet<> dirichbrick2(dirichbrick1, BOTTOM);


     // CALCULATION ***************************************************************************
     cout << "Calculation start\n";
     getfem::standard_model_state MS(dirichbrick2);

     getfem::modeling_standard_plain_vector F(mfdata.nb_dof()*3);

     for (long j =0; j<mfdata.nb_dof()*3; j+=3) {
        F[j]=F[j+1]=0.0;F[j+2]=0.01;
     };

     dirichbrick2.rhs().set(mfdata,F);

     gmm::iteration iter(1e-6, 2, 2000);
     
     my_solve(MS, dirichbrick2, iter);
   

     // WRITING *******************************************************************************
     cout << "Writing results\n";
     getfem::modeling_standard_plain_vector U(mfu.nb_dof());
     gmm::copy(elasbrick.get_solution(MS), U);
     getfem::vtk_export exp( "result.vtk");
     exp.exporting(mfu); 
     exp.write_point_data(mfu, U, "elastostatic_displacement");

  }
  DAL_STANDARD_CATCH_ERROR;

  return 0; 
}