/*===========================================================================

 Copyright (C) 2018-2019 Tetsuo Koyama

 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 3 of the License,  or
 (at your option) any later version along with the GCC Runtime Library
 Exception either version 3.1 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 and GCC Runtime Library Exception 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.

===========================================================================*/

/address@hidden elastodynamic_side_boundary.cc
   @brief 2.5D plan wave elastodynamic analysis problem.
*/
#include <math.h>
#include "getfem/getfem_mesher.h"
#include "getfem/getfem_regular_meshes.h"
#include "getfem/getfem_generic_assembly.h"
#include "getfem/getfem_assembling.h"
#include "getfem/getfem_models.h"
#include "getfem/getfem_model_solvers.h"
#include "gmm/gmm.h"
#include "gmm/gmm_inoutput.h"

using std::cout;
using std::endl;
using bgeot::scalar_type; /* = double */
using bgeot::size_type;   /* = unsigned long */
using bgeot::base_small_vector; /* special class for small (dim<16) vectors */
using bgeot::base_node; /* geometrical nodes () */

/* definition of some matrix/vector types. These ones are built
 * using the predefined types in Gmm++
 */
typedef gmm::rsvector<scalar_type> sparse_vector_type;
typedef gmm::row_matrix<sparse_vector_type> sparse_matrix_type;
typedef gmm::csc_matrix<double> csc_matrix_type;
typedef std::vector<scalar_type> plain_vector;

base_small_vector sol_K; /* a coefficient */
scalar_type sol_t, Tp, Vp;
scalar_type e = 2.718281828459045;
/*
  structure for the 2.5D plan wave elastodynamic analysis problem
*/
struct elastodynamic_side_boundary_problem {
  scalar_type LX, LY, LZ; /* geometrical parameter */
  int NX, NY, NZ, N;

  enum { ALL = -1, RIGHT = 1, LEFT = 2, BEFORE = 3, AFTER = 4, TOP = 5, BOTTOM = 6 };

  scalar_type clambda; /* First Lame coefficient (N/m^2) */
  scalar_type cmu;     /* Second Lame coefficient (N/m^2) */
  scalar_type nu;      /* Poisson's ratio */
  scalar_type rho;     /* Mass Density*/
  scalar_type Vp;      /* P wave velocity */
  scalar_type Vs;      /* S wave velocity */
  scalar_type my_k;

  scalar_type sol_t;
  scalar_type thetav;
  scalar_type thetah;
  scalar_type thetap;
  scalar_type thetas;

  getfem::mesh mesh;
  getfem::mesh_im mim;   /* integration methods. */
  getfem::mesh_fem mf_u; /* the vector mesh_fem, for the Palne Wave solution */
  getfem::mesh_fem mf_d; /* the scalar mesh_fem, for the Plane Wave solution */
  getfem::mesh_fem mf_rhs; /* the mesh_fem for the right hand side(f(x),..) */

  scalar_type dt, T, beta, gamma; /* parameter of Newmark scheme */
  scalar_type residual;        /* max residual for the iterative solvers     */

  plain_vector external_force_for_Dot_u; /* External Force Coefficient for Dot_u */

  std::string datafilename;
  bgeot::md_param PARAM;

  int init(void);
  void assembly(void);
  bool solve(void);
  elastodynamic_side_boundary_problem(void) :
  mim(mesh),
  mf_u(mesh, 3), mf_d(mesh, 1), mf_rhs(mesh, 3) {}

};

/* Read parameters from the .param file, build the mesh, set finite element
 * and integration methods and selects the boundaries.
 */
int elastodynamic_side_boundary_problem::init(void) {

  std::string FEM_TYPE = PARAM.string_value("FEM_TYPE", "FEM name");
  std::string INTEGRATION = PARAM.string_value("INTEGRATION",
                                               "Name of integration method");

  /* Physical parameters */
  cmu = PARAM.real_value("cmu", "Second Lame coefficient (N/m^2)");
  nu  = PARAM.real_value("nu", "Poisson's ratio");
  rho = PARAM.real_value("rho", "Mass Density");
  clambda = 2.0*cmu*nu/(1.0-2.0*nu);
  Vp = sqrt((clambda+2.0*cmu)/rho);
  Vs = sqrt(cmu/rho);

  /* Geometrical parameters */
  thetav = PARAM.real_value("thetav", "Input Wave Tilt");
  thetah = PARAM.real_value("thetah", "Input Wave Azimuth");
  my_k = -sin(thetav)*cos(thetah)/Vs;
  thetas = thetav;
  thetap = asin(Vp/Vs*sin(thetas));

  /* output filename */
  datafilename = PARAM.string_value("ROOTFILENAME","Base name of data files.");

  /* First step : build the mesh */
  cout << "Mesh generation\n";
  LX = PARAM.real_value("LX", "Size in X");
  LY = PARAM.real_value("LY", "Size in Y");
  LZ = PARAM.real_value("LZ", "Size in Z");
  NX = PARAM.int_value("NX", "number of space steps ");
  NY = PARAM.int_value("NY", "number of space steps ");
  NZ = PARAM.int_value("NZ", "number of space steps ");
  N = 3;
  std::vector<base_node> fixed;
  double hx = LX/NX;
  double hy = LY/NY;
  double hz = LZ/NZ;
  base_node org(0.0, 0.0, 0.0);
  std::vector<bgeot::base_small_vector> vect(3);
  vect[0] = bgeot::base_small_vector( hx,0.0, 0.0);
  vect[1] = bgeot::base_small_vector(0.0, hy, 0.0);
  vect[2] = bgeot::base_small_vector(0.0,0.0,  hz);
  std::vector<int> ref(3);
  ref[0] = NX;
  ref[1] = NY;
  ref[2] = NZ;
  getfem::parallelepiped_regular_mesh(
    mesh, 3, org, vect.begin(), ref.begin()
  );
  getfem::pfem pf_u = getfem::fem_descriptor(FEM_TYPE);
  getfem::pintegration_method ppi = getfem::int_method_descriptor(INTEGRATION);

  /* set the region of mesh */
  cout << "Selecting regions in boundaries\n";
  getfem::mesh_region border_faces;
  getfem::outer_faces_of_mesh(mesh, border_faces);

  getfem::mesh_region fbox
    = getfem::select_faces_in_box(mesh, border_faces, base_node(0.01, 0.01, 0.01),
                                  base_node(LX-0.01, LY-0.01, LZ-0.01));

  getfem::mesh_region fright
    = getfem::select_faces_of_normal(mesh, border_faces,
                                     base_small_vector(+1.0, 0.0, 0.0), 0.01);

  getfem::mesh_region fleft
    = getfem::select_faces_of_normal(mesh, border_faces,
                                     base_small_vector(-1.0, 0.0, 0.0), 0.01);

  getfem::mesh_region fbefore
    = getfem::select_faces_of_normal(mesh, border_faces,
                                     base_small_vector( 0.0,-1.0, 0.0), 0.01);

  getfem::mesh_region fafter
    = getfem::select_faces_of_normal(mesh, border_faces,
                                     base_small_vector( 0.0,+1.0, 0.0), 0.01);

  getfem::mesh_region ftop
    = getfem::select_faces_of_normal(mesh, border_faces,
                                     base_small_vector( 0.0, 0.0,+1.0), 0.01);

  getfem::mesh_region fbottom
    = getfem::select_faces_of_normal(mesh, border_faces,
                                     base_small_vector( 0.0, 0.0,-1.0), 0.01);

  mesh.region( RIGHT) = getfem::mesh_region::subtract( fright, fbox);
  mesh.region(  LEFT) = getfem::mesh_region::subtract(  fleft, fbox);
  mesh.region(BEFORE) = getfem::mesh_region::subtract(fbefore, fbox);
  mesh.region( AFTER) = getfem::mesh_region::subtract( fafter, fbox);
  mesh.region(   TOP) = getfem::mesh_region::subtract(   ftop, fbox);
  mesh.region(BOTTOM) = getfem::mesh_region::subtract(fbottom, fbox);

  mesh.write_to_file(datafilename + ".msh2");
  getfem::vtk_export mesh_exp(datafilename + ".vtk", false);
  mesh_exp.exporting(mesh);
  mesh_exp.write_mesh();

  /* set the finite element on the mf_u and mf_d */
  mf_u.set_finite_element(mesh.convex_index(), pf_u);
  mf_d.set_finite_element(mesh.convex_index(), pf_u);

  mf_rhs.set_finite_element(mesh.convex_index(), pf_u);

  mim.set_integration_method(mesh.convex_index(), ppi);

  /* set the solve parameter */
  beta = PARAM.real_value("BETA", "Newmark theme beta parameter");
  gamma = PARAM.real_value("GAMMA", "Newmark theme gamma parameter");
  dt = PARAM.real_value("DT", "Time step");

  residual = PARAM.real_value("RESIDUAL");
  if (residual == 0.) residual = 1e-10;
  T = PARAM.real_value("T", "final time");
  Tp = PARAM.real_value("Tp", "Time Parameter of Ricker Wave");
  Vp = PARAM.real_value("Vp", "Amplitude Parameter of Ricker Wave");

  return 0;
};

void elastodynamic_side_boundary_problem::assembly(void) {

};

bool elastodynamic_side_boundary_problem::solve(void) {

  getfem::model model;

  cout << "Setting Model Parameter\n";

  // Material Parameter
  model.add_initialized_scalar_data("lambda", clambda);
  model.add_initialized_scalar_data("mu", cmu);
  model.add_initialized_scalar_data("rho", rho);
  model.add_initialized_scalar_data("Vp", Vp);
  model.add_initialized_scalar_data("Vs", Vs);

  // Geometoric parameter
  model.add_initialized_scalar_data("my_k", my_k);

  // Main unkown of the problem
  getfem::partial_mesh_fem mf_u1(mf_u);
  getfem::partial_mesh_fem mf_u2(mf_u);
  getfem::partial_mesh_fem mf_u3(mf_u);
  getfem::partial_mesh_fem mf_u4(mf_u);
  dal::bit_vector kept_dof1 = mf_u1.basic_dof_on_region(LEFT  );
  dal::bit_vector kept_dof2 = mf_u2.basic_dof_on_region(RIGHT );
  dal::bit_vector kept_dof3 = mf_u3.basic_dof_on_region(BEFORE);
  dal::bit_vector kept_dof4 = mf_u4.basic_dof_on_region(AFTER );
  mf_u1.adapt(kept_dof1);
  mf_u2.adapt(kept_dof2);
  mf_u3.adapt(kept_dof3);
  mf_u4.adapt(kept_dof4);
  model.add_fem_variable("c1", mf_u1);
  model.add_fem_variable("c2", mf_u2);
  model.add_fem_variable("c3", mf_u3);
  model.add_fem_variable("c4", mf_u4);
  model.add_fem_variable("u1", mf_u1);
  model.add_fem_variable("u2", mf_u2);
  model.add_fem_variable("u3", mf_u3);
  model.add_fem_variable("u4", mf_u4);
  model.add_fem_variable("u", mf_u);
  model.add_macro("Ix", "[1, 0, 0]");
  model.add_macro("Iy", "[0, 1, 0]");
  model.add_macro("Iz", "[0, 0, 1]");
  model.add_macro("Ir", "[+1,  0,  0]");
  model.add_macro("Il", "[-1,  0,  0]");
  model.add_macro("Ib", "[ 0, +1,  0]");
  model.add_macro("Ia", "[ 0, -1,  0]");
  model.add_macro("my_k2", "my_k*my_k");
  model.add_macro("Vps", "[[Vp, Vs, Vs], [Vs, Vp, Vs], [Vs, Vs, Vp]]");
  model.add_macro("D", "[[lambda+2.0*mu, mu, mu], [mu, lambda+2.0*mu, mu], [mu, mu, lambda+2.0*mu]]");

  // Newmark beta theme
  getfem::add_Newmark_scheme(model, "c1", beta, gamma);
  getfem::add_Newmark_scheme(model, "c2", beta, gamma);
  getfem::add_Newmark_scheme(model, "c3", beta, gamma);
  getfem::add_Newmark_scheme(model, "c4", beta, gamma);
  getfem::add_Newmark_scheme(model, "u1", beta, gamma);
  getfem::add_Newmark_scheme(model, "u2", beta, gamma);
  getfem::add_Newmark_scheme(model, "u3", beta, gamma);
  getfem::add_Newmark_scheme(model, "u4", beta, gamma);
  getfem::add_Newmark_scheme(model, "u", beta, gamma);

  // Stiffness brick
  getfem::add_linear_term(model, mim, "mu*(Grad_c1*Iz.Ix)*(Grad_Test_c1*Iz.Ix)");
  getfem::add_linear_term(model, mim, "mu*(Grad_c1*Iz.Iy)*(Grad_Test_c1*Iz.Iy)");
  getfem::add_linear_term(model, mim, "(lambda+2.0*mu)*(Grad_c1*Iz.Iz)*(Grad_Test_c1*Iz.Iz)");
  getfem::add_linear_term(model, mim, "mu*(Grad_c2*Iz.Ix)*(Grad_Test_c2*Iz.Ix)");
  getfem::add_linear_term(model, mim, "mu*(Grad_c2*Iz.Iy)*(Grad_Test_c2*Iz.Iy)");
  getfem::add_linear_term(model, mim, "(lambda+2.0*mu)*(Grad_c2*Iz.Iz)*(Grad_Test_c2*Iz.Iz)");
  getfem::add_linear_term(model, mim, "mu*(Grad_c3*Iz.Ix)*(Grad_Test_c3*Iz.Ix)");
  getfem::add_linear_term(model, mim, "mu*(Grad_c3*Iz.Iy)*(Grad_Test_c3*Iz.Iy)");
  getfem::add_linear_term(model, mim, "(lambda+2.0*mu)*(Grad_c3*Iz.Iz)*(Grad_Test_c3*Iz.Iz)");
  getfem::add_linear_term(model, mim, "mu*(Grad_c4*Iz.Ix)*(Grad_Test_c4*Iz.Ix)");
  getfem::add_linear_term(model, mim, "mu*(Grad_c4*Iz.Iy)*(Grad_Test_c4*Iz.Iy)");
  getfem::add_linear_term(model, mim, "(lambda+2.0*mu)*(Grad_c4*Iz.Iz)*(Grad_Test_c4*Iz.Iz)");
  getfem::add_isotropic_linearized_elasticity_brick(model, mim, "u1", "lambda", "mu");
  getfem::add_isotropic_linearized_elasticity_brick(model, mim, "u2", "lambda", "mu");
  getfem::add_isotropic_linearized_elasticity_brick(model, mim, "u3", "lambda", "mu");
  getfem::add_isotropic_linearized_elasticity_brick(model, mim, "u4", "lambda", "mu");
  getfem::add_isotropic_linearized_elasticity_brick(model, mim, "u", "lambda", "mu");

  // Damping matrix on Dot_u.
  getfem::add_linear_term(model, mim, std::string("(+my_k*lambda*(Grad_c1*Iz.Ix))*(Test_c1.Iz)+")
                                     +std::string("(-my_k*mu*(c1.Ix))*(Grad_Test_c1*Iz.Iz)"));
  getfem::add_linear_term(model, mim, std::string("(+my_k*lambda*(Grad_c1*Iz.Iz))*(Test_c1.Ix)+")
                                     +std::string("(-my_k*mu*(c1.Iz))*(Grad_Test_c1*Iz.Ix)"));
  getfem::add_linear_term(model, mim, std::string("(+my_k*lambda*(Grad_c2*Iz.Ix))*(Test_c2.Iz)+")
                                     +std::string("(-my_k*mu*(c2.Ix))*(Grad_Test_c2*Iz.Iz)"));
  getfem::add_linear_term(model, mim, std::string("(+my_k*lambda*(Grad_c2*Iz.Iz))*(Test_c2.Ix)+")
                                     +std::string("(-my_k*mu*(c2.Iz))*(Grad_Test_c2*Iz.Ix)"));
  getfem::add_linear_term(model, mim, std::string("(+my_k*lambda*(Grad_c3*Iz.Iy))*(Test_c3.Iz)+")
                                     +std::string("(-my_k*mu*(c3.Iy))*(Grad_Test_c3*Iz.Iz)"));
  getfem::add_linear_term(model, mim, std::string("(+my_k*lambda*(Grad_c3*Iz.Iz))*(Test_c3.Iy)+")
                                     +std::string("(-my_k*mu*(c3.Iz))*(Grad_Test_c3*Iz.Iy)"));
  std::string c;
  c = std::string("[[               0, my_k*(lambda+mu), my_k*(lambda+mu)],")
    + std::string(" [my_k*(lambda+mu),                0,                0],")
    + std::string(" [my_k*(lambda+mu),                0,                0]]");
  getfem::add_linear_term(model, mim, "(Grad("+c+"*Dot_u1)*Iz).Test_u1");
  getfem::add_linear_term(model, mim, "(Grad("+c+"*Dot_u2)*Iz).Test_u2");
  c = std::string("[[               0, my_k*(lambda+mu),                0],")
    + std::string(" [my_k*(lambda+mu),                0, my_k*(lambda+mu)],")
    + std::string(" [               0, my_k*(lambda+mu),                0]]");
  getfem::add_linear_term(model, mim, "(Grad("+c+"*Dot_u3)*Iz).Test_u3");
  getfem::add_linear_term(model, mim, "(Grad("+c+"*Dot_u4)*Iz).Test_u4");

  // Mass brick.
  getfem::add_linear_term(model, mim, "((rho-my_k2*(lambda+2.0*mu))*(Dot2_c1.Ix))*(Test_Dot2_c1.Ix)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*mu)*(Dot2_c1.Iy))*(Test_Dot2_c1.Iy)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*mu)*(Dot2_c1.Iz))*(Test_Dot2_c1.Iz)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*(lambda+2.0*mu))*(Dot2_c2.Ix))*(Test_Dot2_c2.Ix)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*mu)*(Dot2_c2.Iy))*(Test_Dot2_c2.Iy)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*mu)*(Dot2_c2.Iz))*(Test_Dot2_c2.Iz)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*mu)*(Dot2_c3.Ix))*(Test_Dot2_c3.Ix)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*(lambda+2.0*mu))*(Dot2_c3.Iy))*(Test_Dot2_c3.Iy)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*mu)*(Dot2_c3.Iz))*(Test_Dot2_c3.Iz)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*mu)*(Dot2_c4.Ix))*(Test_Dot2_c4.Ix)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*(lambda+2.0*mu))*(Dot2_c4.Iy))*(Test_Dot2_c4.Iy)");
  getfem::add_linear_term(model, mim, "((rho-my_k2*mu)*(Dot2_c4.Iz))*(Test_Dot2_c4.Iz)");
  getfem::add_linear_term(model, mim, "([rho-my_k2*(lambda+2.0*mu), rho-my_k2*mu, rho-my_k2*mu].*Dot2_u1).Test_Dot2_u1");
  getfem::add_linear_term(model, mim, "([rho-my_k2*(lambda+2.0*mu), rho-my_k2*mu, rho-my_k2*mu].*Dot2_u2).Test_Dot2_u2");
  getfem::add_linear_term(model, mim, "([rho-my_k2*mu, rho-my_k2*(lambda+2.0*mu), rho-my_k2*mu].*Dot2_u3).Test_Dot2_u3");
  getfem::add_linear_term(model, mim, "([rho-my_k2*mu, rho-my_k2*(lambda+2.0*mu), rho-my_k2*mu].*Dot2_u4).Test_Dot2_u4");
  getfem::add_mass_brick(model, mim, "Dot2_u", "rho");

  // Damper at RIGHT
  getfem::add_linear_term(model, mim, "((rho*Vps*Ix).*Dot_u).Dot_u", RIGHT);

  // Damper at LEFT
  getfem::add_linear_term(model, mim, "((rho*Vps*Ix).*Dot_u).Dot_u", LEFT);

  // Damper at BEFORE
  getfem::add_linear_term(model, mim, "((rho*Vps*Iy).*Dot_u).Dot_u", BEFORE);

  // Damper at AFTER
  getfem::add_linear_term(model, mim, "((rho*Vps*Iy).*Dot_u).Dot_u", AFTER);

  // Damper at TOP
  getfem::add_linear_term(model, mim, "((rho*Vps*Iz).*Dot_c1).Test_c1", TOP);
  getfem::add_linear_term(model, mim, "((rho*Vps*Iz).*Dot_c2).Test_c2", TOP);
  getfem::add_linear_term(model, mim, "((rho*Vps*Iz).*Dot_c3).Test_c3", TOP);
  getfem::add_linear_term(model, mim, "((rho*Vps*Iz).*Dot_u4).Test_c4", TOP);
  getfem::add_linear_term(model, mim, "((rho*Vps*Iz).*Dot_u1).Test_u1", TOP);
  getfem::add_linear_term(model, mim, "((rho*Vps*Iz).*Dot_u2).Test_u2", TOP);
  getfem::add_linear_term(model, mim, "((rho*Vps*Iz).*Dot_u3).Test_u3", TOP);
  getfem::add_linear_term(model, mim, "((rho*Vps*Iz).*Dot_u4).Test_u4", TOP);
  getfem::add_linear_term(model, mim, "((rho*Vps*Iz).*Dot_u).Dot_u", TOP);

  // Null initial value.
  cout << "Null initial value.\n";

  gmm::clear(model.set_real_variable("Previous_c1"));
  gmm::clear(model.set_real_variable("Previous_c2"));
  gmm::clear(model.set_real_variable("Previous_c3"));
  gmm::clear(model.set_real_variable("Previous_c4"));
  gmm::clear(model.set_real_variable("Previous_u1"));
  gmm::clear(model.set_real_variable("Previous_u2"));
  gmm::clear(model.set_real_variable("Previous_u3"));
  gmm::clear(model.set_real_variable("Previous_u4"));
  gmm::clear(model.set_real_variable("Previous_u"));

  gmm::clear(model.set_real_variable("Previous_Dot_c1"));
  gmm::clear(model.set_real_variable("Previous_Dot_c2"));
  gmm::clear(model.set_real_variable("Previous_Dot_c3"));
  gmm::clear(model.set_real_variable("Previous_Dot_c4"));
  gmm::clear(model.set_real_variable("Previous_Dot_u1"));
  gmm::clear(model.set_real_variable("Previous_Dot_u2"));
  gmm::clear(model.set_real_variable("Previous_Dot_u3"));
  gmm::clear(model.set_real_variable("Previous_Dot_u4"));
  gmm::clear(model.set_real_variable("Previous_Dot_u"));

  gmm::iteration iter(residual, 0, 40000);
  iter.set_noisy(2);

  // freeze variable
  model.disable_variable("c1");
  model.disable_variable("Dot_c1");
  model.disable_variable("Dot2_c1");
  model.disable_variable("c2");
  model.disable_variable("Dot_c2");
  model.disable_variable("Dot2_c2");
  model.disable_variable("c3");
  model.disable_variable("Dot_c3");
  model.disable_variable("Dot2_c3");
  model.disable_variable("c4");
  model.disable_variable("Dot_c4");
  model.disable_variable("Dot2_c4");
  model.disable_variable("u1");
  model.disable_variable("Dot_u1");
  model.disable_variable("Dot2_u1");
  model.disable_variable("u2");
  model.disable_variable("Dot_u2");
  model.disable_variable("Dot2_u2");
  model.disable_variable("u3");
  model.disable_variable("Dot_u3");
  model.disable_variable("Dot2_u3");
  model.disable_variable("u4");
  model.disable_variable("Dot_u4");
  model.disable_variable("Dot2_u4");
  model.disable_variable("u");
  model.disable_variable("Dot_u");
  model.disable_variable("Dot2_u");

  // TOP boundary source term.
  sol_K.resize(N);
  sol_K[0] = rho*Vs;
  sol_K[1] = 0.0;
  sol_K[2] = 0.0;
  std::vector<scalar_type> F;
  getfem::add_source_term_generic_assembly_brick(model, mim, "1.0*Test_u", TOP);

  sol_t = 0.;
  csc_matrix_type SM1, SM2, SM3;

  // Iterations in time
  cout << "Iterations in time start\n";
  model.set_time(0.);        // Init time is 0 (not mandatory)
  model.set_time_step(dt);   // Init of the time step.

  for (scalar_type t = 0.; t < T; t += dt) {

    sol_t = t+dt;

    cout << "solving for t = " << sol_t << endl;
    F.resize(mf_rhs.nb_dof());

    // Debug
    if (PARAM.int_value("EXPORT_DEBUG") != 0) {
      char s[100]; sprintf(s, "step%d", int(t/dt)+1);
      gmm::vecsave(datafilename + s + ".F", F);
    }

    // Corner
    cout << "solving at Corner" << endl;

    model.enable_variable("c1");
    model.enable_variable("Dot_c1");
    model.enable_variable("Dot2_c1");
    model.enable_variable("c2");
    model.enable_variable("Dot_c2");
    model.enable_variable("Dot2_c2");
    model.enable_variable("c3");
    model.enable_variable("Dot_c3");
    model.enable_variable("Dot2_c3");
    model.enable_variable("c4");
    model.enable_variable("Dot_c4");
    model.enable_variable("Dot2_c4");

    // Debug
    model.assembly(getfem::model::build_version(3));
    if (PARAM.int_value("EXPORT_DEBUG") != 0) {
      char s[100]; sprintf(s, "step%d", int(t/dt)+1);
      gmm::copy(model.real_tangent_matrix(), SM1);
      gmm::MatrixMarket_save("SM1.mtx", SM1);
      gmm::vecsave(datafilename + s + ".FF1", model.real_rhs());
    }

    iter.init();
    getfem::standard_solve(model, iter);

    model.disable_variable("c1");
    model.disable_variable("Dot_c1");
    model.disable_variable("Dot2_c1");
    model.disable_variable("c2");
    model.disable_variable("Dot_c2");
    model.disable_variable("Dot2_c2");
    model.disable_variable("c3");
    model.disable_variable("Dot_c3");
    model.disable_variable("Dot2_c3");
    model.disable_variable("c4");
    model.disable_variable("Dot_c4");
    model.disable_variable("Dot2_c4");

    // Side
    cout << "solving at Side" << endl;

    model.enable_variable("u1");
    model.enable_variable("Dot_u1");
    model.enable_variable("Dot2_u1");
    model.enable_variable("u2");
    model.enable_variable("Dot_u2");
    model.enable_variable("Dot2_u2");
    model.enable_variable("u3");
    model.enable_variable("Dot_u3");
    model.enable_variable("Dot2_u3");
    model.enable_variable("u4");
    model.enable_variable("Dot_u4");
    model.enable_variable("Dot2_u4");

    // Debug
    model.assembly(getfem::model::build_version(3));
    if (PARAM.int_value("EXPORT_DEBUG") != 0) {
      char s[100]; sprintf(s, "step%d", int(t/dt)+1);
      gmm::copy(model.real_tangent_matrix(), SM2);
      gmm::MatrixMarket_save("SM2.mtx", SM2);
      gmm::vecsave(datafilename + s + ".FF2", model.real_rhs());
    }

    iter.init();
    getfem::standard_solve(model, iter);

    model.disable_variable("u1");
    model.disable_variable("Dot_u1");
    model.disable_variable("Dot2_u1");
    model.disable_variable("u2");
    model.disable_variable("Dot_u2");
    model.disable_variable("Dot2_u2");
    model.disable_variable("u3");
    model.disable_variable("Dot_u3");
    model.disable_variable("Dot2_u3");
    model.disable_variable("u4");
    model.disable_variable("Dot_u4");
    model.disable_variable("Dot2_u4");

    // Main
    cout << "solving at Main" << endl;

    model.enable_variable("u");
    model.enable_variable("Dot_u");
    model.enable_variable("Dot2_u");

    // Debug
    model.assembly(getfem::model::build_version(3));
    if (PARAM.int_value("EXPORT_DEBUG") != 0) {
      char s[100]; sprintf(s, "step%d", int(t/dt)+1);
      gmm::copy(model.real_tangent_matrix(), SM3);
      gmm::MatrixMarket_save("SM3.mtx", SM3);
      gmm::vecsave(datafilename + s + ".FF3", model.real_rhs());
    }

    iter.init();
    getfem::standard_solve(model, iter);

    model.disable_variable("u");
    model.disable_variable("Dot_u");
    model.disable_variable("Dot2_u");

    model.shift_variables_for_time_integration();

  }

  return (iter.converged());

}

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

  try {
    elastodynamic_side_boundary_problem p;
    p.PARAM.read_command_line(argc, argv);
    p.init();
    p.assembly();
    p.solve();
  }
  GMM_STANDARD_CATCH_ERROR;

  return 0;
}