/* Solution of a simple truss structure:
 *        |       |
 *        V P     V P
 *        1-------3
 *       / \     / \
 *      /   \   /   \           ^ v
 *     /     \ /     \          |
 *    0-------2-------4         ----> u
 *                   
 *    all bars are the same:   stiffness EA = 1
 *                             length    a  = 1
 *    Load: P = 1
 *    Constraints  (displacements: u,v)   v_0 = 0
 *                                        u_2 = 0
 *                                        v_4 = 0
 */ 
#include <stdlib.h> // for EXIT_SUCCESS
#include <stdio.h>
#include <iostream>
#include <vector>
#include <map>
#include <string>

#include "getfem/getfem_model_solvers.h"
#include "getfem/getfem_models.h"
#include "getfem/getfem_export.h"
#include "getfem/getfem_regular_meshes.h"
#include "getfem/getfem_mesh_slice.h"
#include "getfem/bgeot_comma_init.h"
#include "getfem/getfem_mesh_fem_global_function.h"
#include "gmm/gmm.h"
#include <string>

int main_body(int argc, char** argv);

/** main function
 * 
 * This function should contain command line parsing and initialization
 * code. The real work should be done by calling main_body().
 */
int main(int argc, char **argv) {
  int result =  EXIT_SUCCESS;
  result = main_body(argc, argv);

  return result;
}

/** The function where the main work is done after configuration
 * and command line parsing.
 */
int main_body(int argc, char** argv) {
  try {  
    double a = 1.0;
    double h = a * sqrt(3.0)/2.0;

    getfem::mesh mesh;

    std::vector<bgeot::size_type> ind(5);
    ind[0] = mesh.add_point(bgeot::base_node(0.0, 0.0));
    ind[1] = mesh.add_point(bgeot::base_node(0.5*a, h));
    ind[2] = mesh.add_point(bgeot::base_node(a, 0.0));
    ind[3] = mesh.add_point(bgeot::base_node(1.5*a, h));
    ind[4] = mesh.add_point(bgeot::base_node(2*a, 0.0));
    
    mesh.add_segment(ind[0], ind[1]);
    mesh.add_segment(ind[0], ind[2]);
    mesh.add_segment(ind[1], ind[2]);
    mesh.add_segment(ind[1], ind[3]);
    mesh.add_segment(ind[2], ind[3]);
    mesh.add_segment(ind[2], ind[4]);
    mesh.add_segment(ind[3], ind[4]);
   
    getfem::mesh_fem fem(mesh);
    fem.set_qdim(2);
    fem.set_finite_element(getfem::fem_descriptor("FEM_PK(1,1)"));

    getfem::mesh_im mim(mesh);
    getfem::pintegration_method pim = 
    getfem::int_method_descriptor("IM_GAUSS1D(2)");
    mim.set_integration_method(mesh.convex_index(), pim);

    getfem::size_type Ndof = fem.nb_dof();
    std::cout << "NUMBER OF DEGREES OF FREEDOM " << Ndof << "\n";
    /* Global stiffness matrix */
    gmm::dense_matrix<double> K(Ndof, Ndof); /* characteristic matrix */
    /* Global right hand side vector */
    std::vector<double> f(Ndof);
    gmm::clear(f);

    getfem::generic_assembly assem;
    assem.push_mi(mim);
    assem.push_mf(fem);
    assem.push_mat(K);

    /* The matrix M should be scaled by bar stiffness EA */
    assem.set("k=comp(vGrad(#1).vGrad(#1));"
              "M(#1,#1)+= k(:,i,i,:,j,j);");
    assem.assembly();

    std::cout << K << "\n";
    f[3] = -1;
    f[7] = -1; 

    double penalty=1e20;


    /* Simple way of handling boundary conditions */
    K(1,1)+=penalty;
    K(9,9)+=penalty;
    K(4,4)+=penalty;

    std::vector<double> x(Ndof);
    gmm::lu_solve(K,x,f);
    std::cout << x << "\n";

    return EXIT_SUCCESS;
  } GMM_STANDARD_CATCH_ERROR;
 
  return EXIT_FAILURE;
}