/* Author: Wolfgang Bangerth, Texas A&M University, 2005, 2006 */

/*    $Id: step-20.cc 28518 2013-02-21 20:26:17Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2005-2008, 2010-2013 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Include files}

// Since this program is only an adaptation of step-4, there is not much new
// stuff in terms of header files. In deal.II, we usually list include files
// in the order base-lac-grid-dofs-fe-numerics, followed by C++ standard
// include files:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/function.h>
#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/block_sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
// For our Schur complement solver, we need two new objects. One is a matrix
// object which acts as the inverse of a matrix by calling an iterative
// solver.
#include <deal.II/lac/iterative_inverse.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

#include <fstream>
#include <iostream>

// This is the only significant new header, namely the one in which the
// Raviart-Thomas finite element is declared:
#include <deal.II/fe/fe_raviart_thomas.h>

// Finally, as a bonus in this program, we will use a tensorial
// coefficient. Since it may have a spatial dependence, we consider it a
// tensor-valued function. The following include file provides the
// TensorFunction class that offers such functionality:
#include <deal.II/base/tensor_function.h>

// The last step is as in all previous programs:
namespace Step20
{
  using namespace dealii;

  // @sect3{The MixedLaplaceProblem class template}

  // Again, since this is an adaptation of step-6, the main class is almost
  // the same as the one in that tutorial program. In terms of member
  // functions, the main differences are that the constructor takes the degree
  // of the Raviart-Thomas element as an argument (and that there is a
  // corresponding member variable to store this value) and the addition of
  // the compute_error function in which, no surprise, we will
  // compute the difference between the exact and the numerical solution to
  // determine convergence of our computations:
  template <int dim>
  class MixedLaplaceProblem
  {
  public:
    MixedLaplaceProblem (const unsigned int degree);
    void run ();

  private:
    void make_grid_and_dofs ();
    void assemble_system ();
    void solve ();
    void compute_errors () const;
    void output_results () const;

    const unsigned int   degree;

    Triangulation<dim>   triangulation;
    FESystem<dim>        fe;
    DoFHandler<dim>      dof_handler;

    // The second difference is that the sparsity pattern, the system matrix,
    // and solution and right hand side vectors are now blocked. What this
    // means and what one can do with such objects is explained in the
    // introduction to this program as well as further down below when we
    // explain the linear solvers and preconditioners for this problem:
    BlockSparsityPattern      sparsity_pattern;
    BlockSparseMatrix<double> system_matrix;

    BlockVector<double>       solution;
    BlockVector<double>       system_rhs;
  };

  // @sect3{Right hand side, boundary values, and exact solution}

  // Our next task is to define the right hand side of our problem (i.e., the
  // scalar right hand side for the pressure in the original Laplace
  // equation), boundary values for the pressure, as well as a function that
  // describes both the pressure and the velocity of the exact solution for
  // later computations of the error. Note that these functions have one, one,
  // and dim+1 components, respectively, and that we pass the
  // number of components down to the Function@<dim@> base
  // class. For the exact solution, we only declare the function that actually
  // returns the entire solution vector (i.e. all components of it) at
  // once. Here are the respective declarations:
  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide () : Function<dim>(1) {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <int dim>
  class PressureBoundaryValues : public Function<dim>
  {
  public:
    PressureBoundaryValues () : Function<dim>(1) {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    ExactSolution () : Function<dim>(dim+1) {}

    virtual void vector_value (const Point<dim> &p,
                               Vector<double>   &value) const;
  };

  // And then we also have to define these respective functions, of
  // course. Given our discussion in the introduction of how the solution
  // should look like, the following computations should be straightforward:
  template <int dim>
  double RightHandSide<dim>::value (const Point<dim>  & /*p*/,
                                    const unsigned int /*component*/) const
  {
    return 0;
  }

  template <int dim>
  double PressureBoundaryValues<dim>::value (const Point<dim> &p,
                                             const unsigned int /*component*/) const
  {
    const double alpha = 0.3;
    const double beta = 1;
    return -(alpha*p[0]*p[1]*p[1]/2 + beta*p[0] - alpha*p[0]*p[0]*p[0]/6);
  }

  template <int dim>
  void
  ExactSolution<dim>::vector_value (const Point<dim> &p,
                                    Vector<double>   &values) const
  {
    Assert (values.size() == dim+1,
            ExcDimensionMismatch (values.size(), dim+1));

    const double alpha = 0.3;
    const double beta = 1;

    values(0) = alpha*p[1]*p[1]/2 + beta - alpha*p[0]*p[0]/2;
    values(1) = alpha*p[0]*p[1];
    values(2) = -(alpha*p[0]*p[1]*p[1]/2 + beta*p[0] - alpha*p[0]*p[0]*p[0]/6);
  }

  // @sect3{The inverse permeability tensor}

  // In addition to the other equation data, we also want to use a
  // permeability tensor, or better -- because this is all that appears in the
  // weak form -- the inverse of the permeability tensor,
  // KInverse. For the purpose of verifying the exactness of the
  // solution and determining convergence orders, this tensor is more in the
  // way than helpful. We will therefore simply set it to the identity matrix.
  //
  // However, a spatially varying permeability tensor is indispensable in
  // real-life porous media flow simulations, and we would like to use the
  // opportunity to demonstrate the technique to use tensor valued functions.
  //
  // Possibly unsurprising, deal.II also has a base class not only for scalar
  // and generally vector-valued functions (the Function base
  // class) but also for functions that return tensors of fixed dimension and
  // rank, the TensorFunction template. Here, the function under
  // consideration returns a dim-by-dim matrix, i.e. a tensor of rank 2 and
  // dimension dim. We then choose the template arguments of the
  // base class appropriately.
  //
  // The interface that the TensorFunction class provides is
  // essentially equivalent to the Function class. In particular,
  // there exists a value_list function that takes a list of
  // points at which to evaluate the function, and returns the values of the
  // function in the second argument, a list of tensors:
  template <int dim>
  class KInverse : public TensorFunction<2,dim>
  {
  public:
    KInverse () : TensorFunction<2,dim>() {}

    virtual void value_list (const std::vector<Point<dim> > &points,
                             std::vector<Tensor<2,dim> >    &values) const;
  };

  // The implementation is less interesting. As in previous examples, we add a
  // check to the beginning of the class to make sure that the sizes of input
  // and output parameters are the same (see step-5 for a discussion of this
  // technique). Then we loop over all evaluation points, and for each one
  // first clear the output tensor and then set all its diagonal elements to
  // one (i.e. fill the tensor with the identity matrix):
  template <int dim>
  void
  KInverse<dim>::value_list (const std::vector<Point<dim> > &points,
                             std::vector<Tensor<2,dim> >    &values) const
  {
    Assert (points.size() == values.size(),
            ExcDimensionMismatch (points.size(), values.size()));

    for (unsigned int p=0; p<points.size(); ++p)
      {
        values[p].clear ();

        for (unsigned int d=0; d<dim; ++d)
          values[p][d][d] = 1.;
      }
  }

  // @sect3{MixedLaplaceProblem class implementation}

  // @sect4{MixedLaplaceProblem::MixedLaplaceProblem}

  // In the constructor of this class, we first store the value that was
  // passed in concerning the degree of the finite elements we shall use (a
  // degree of zero, for example, means to use RT(0) and DG(0)), and then
  // construct the vector valued element belonging to the space $X_h$ described
  // in the introduction. The rest of the constructor is as in the early
  // tutorial programs.
  //
  // The only thing worth describing here is the constructor call of the
  // fe variable. The FESystem class to which this
  // variable belongs has a number of different constructors that all refer to
  // binding simpler elements together into one larger element. In the present
  // case, we want to couple a single RT(degree) element with a single
  // DQ(degree) element. The constructor to FESystem that does
  // this requires us to specity first the first base element (the
  // FE_RaviartThomas object of given degree) and then the number
  // of copies for this base element, and then similarly the kind and number
  // of FE_DGQ elements. Note that the Raviart Thomas element
  // already has dim vector components, so that the coupled
  // element will have dim+1 vector components, the first
  // dim of which correspond to the velocity variable whereas the
  // last one corresponds to the pressure.
  //
  // It is also worth comparing the way we constructed this element from its
  // base elements, with the way we have done so in step-8: there, we have
  // built it as fe (FE_Q@<dim@>(1), dim), i.e. we have simply
  // used dim copies of the FE_Q(1) element, one
  // copy for the displacement in each coordinate direction.
  template <int dim>
  MixedLaplaceProblem<dim>::MixedLaplaceProblem (const unsigned int degree)
    :
    degree (degree),
    fe (FE_RaviartThomas<dim>(degree), 1,
        FE_DGQ<dim>(degree), 1),
    dof_handler (triangulation)
  {}

  // @sect4{MixedLaplaceProblem::make_grid_and_dofs}

  // This next function starts out with well-known functions calls that create
  // and refine a mesh, and then associate degrees of freedom with it:
  template <int dim>
  void MixedLaplaceProblem<dim>::make_grid_and_dofs ()
  {
    GridGenerator::hyper_cube (triangulation, -1, 1);
    triangulation.refine_global (3);

    dof_handler.distribute_dofs (fe);

    // However, then things become different. As mentioned in the
    // introduction, we want to subdivide the matrix into blocks corresponding
    // to the two different kinds of variables, velocity and pressure. To this
    // end, we first have to make sure that the indices corresponding to
    // velocities and pressures are not intermingled: First all velocity
    // degrees of freedom, then all pressure DoFs. This way, the global matrix
    // separates nicely into a $2 \times 2$ system. To achieve this, we have to
    // renumber degrees of freedom base on their vector component, an
    // operation that conveniently is already implemented:
    DoFRenumbering::component_wise (dof_handler);

    // The next thing is that we want to figure out the sizes of these blocks,
    // so that we can allocate an appropriate amount of space. To this end, we
    // call the DoFTools::count_dofs_per_component function that
    // counts how many shape functions are non-zero for a particular vector
    // component. We have dim+1 vector components, and we have to
    // use the knowledge that for Raviart-Thomas elements all shape functions
    // are nonzero in all components. In other words, the number of velocity
    // shape functions equals the number of overall shape functions that are
    // nonzero in the zeroth vector component. On the other hand, the number
    // of pressure variables equals the number of shape functions that are
    // nonzero in the dim-th component. Let us compute these numbers and then
    // create some nice output with that:
    std::vector<unsigned int> dofs_per_component (dim+1);
    DoFTools::count_dofs_per_component (dof_handler, dofs_per_component);
    const unsigned int n_u = dofs_per_component[0],
                       n_p = dofs_per_component[dim];

    std::cout << "Number of active cells: "
              << triangulation.n_active_cells()
              << std::endl
              << "Total number of cells: "
              << triangulation.n_cells()
              << std::endl
              << "Number of degrees of freedom: "
              << dof_handler.n_dofs()
              << " (" << n_u << '+' << n_p << ')'
              << std::endl;

    // The next task is to allocate a sparsity pattern for the matrix that we
    // will create. The way this works is that we first obtain a guess for the
    // maximal number of nonzero entries per row (this could be done more
    // efficiently in this case, but we only want to solve relatively small
    // problems for which this is not so important). In the second step, we
    // allocate a $2 \times 2$ block pattern and then reinitialize each of the blocks
    // to its correct size using the n_u and n_p
    // variables defined above that hold the number of velocity and pressure
    // variables. In this second step, we only operate on the individual
    // blocks of the system. In the third step, we therefore have to instruct
    // the overlying block system to update its knowledge about the sizes of
    // the blocks it manages; this happens with the
    // sparsity_pattern.collect_sizes() call:
    const unsigned int
    n_couplings = dof_handler.max_couplings_between_dofs();

    sparsity_pattern.reinit (2,2);
    sparsity_pattern.block(0,0).reinit (n_u, n_u, n_couplings);
    sparsity_pattern.block(1,0).reinit (n_p, n_u, n_couplings);
    sparsity_pattern.block(0,1).reinit (n_u, n_p, n_couplings);
    sparsity_pattern.block(1,1).reinit (n_p, n_p, n_couplings);
    sparsity_pattern.collect_sizes();

    // Now that the sparsity pattern and its blocks have the correct sizes, we
    // actually need to construct the content of this pattern, and as usual
    // compress it, before we also initialize a block matrix with this block
    // sparsity pattern:
    DoFTools::make_sparsity_pattern (dof_handler, sparsity_pattern);
    sparsity_pattern.compress();

    system_matrix.reinit (sparsity_pattern);

    // Then we have to resize the solution and right hand side vectors in
    // exactly the same way:
    solution.reinit (2);
    solution.block(0).reinit (n_u);
    solution.block(1).reinit (n_p);
    solution.collect_sizes ();

    system_rhs.reinit (2);
    system_rhs.block(0).reinit (n_u);
    system_rhs.block(1).reinit (n_p);
    system_rhs.collect_sizes ();
  }

  // @sect4{MixedLaplaceProblem::assemble_system}

  // Similarly, the function that assembles the linear system has mostly been
  // discussed already in the introduction to this example. At its top, what
  // happens are all the usual steps, with the addition that we do not only
  // allocate quadrature and FEValues objects for the cell terms,
  // but also for face terms. After that, we define the usual abbreviations
  // for variables, and the allocate space for the local matrix and right hand
  // side contributions, and the array that holds the global numbers of the
  // degrees of freedom local to the present cell.
  template <int dim>
  void MixedLaplaceProblem<dim>::assemble_system ()
  {
    QGauss<dim>   quadrature_formula(degree+2);
    QGauss<dim-1> face_quadrature_formula(degree+2);

    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values    | update_gradients |
                             update_quadrature_points  | update_JxW_values);
    FEFaceValues<dim> fe_face_values (fe, face_quadrature_formula,
                                      update_values    | update_normal_vectors |
                                      update_quadrature_points  | update_JxW_values);

    const unsigned int   dofs_per_cell   = fe.dofs_per_cell;
    const unsigned int   n_q_points      = quadrature_formula.size();
    const unsigned int   n_face_q_points = face_quadrature_formula.size();

    FullMatrix<double>   local_matrix (dofs_per_cell, dofs_per_cell);
    Vector<double>       local_rhs (dofs_per_cell);

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    // The next step is to declare objects that represent the source term,
    // pressure boundary value, and coefficient in the equation. In addition
    // to these objects that represent continuous functions, we also need
    // arrays to hold their values at the quadrature points of individual
    // cells (or faces, for the boundary values). Note that in the case of the
    // coefficient, the array has to be one of matrices.
    const RightHandSide<dim>          right_hand_side;
    const PressureBoundaryValues<dim> pressure_boundary_values;
    const KInverse<dim>               k_inverse;

    std::vector<double> rhs_values (n_q_points);
    std::vector<double> boundary_values (n_face_q_points);
    std::vector<Tensor<2,dim> > k_inverse_values (n_q_points);

    // Finally, we need a couple of extractors that we will use to get at the
    // velocity and pressure components of vector-valued shape
    // functions. Their function and use is described in detail in the @ref
    // vector_valued report. Essentially, we will use them as subscripts on
    // the FEValues objects below: the FEValues object describes all vector
    // components of shape functions, while after subscription, it will only
    // refer to the velocities (a set of dim components starting
    // at component zero) or the pressure (a scalar component located at
    // position dim):
    const FEValuesExtractors::Vector velocities (0);
    const FEValuesExtractors::Scalar pressure (dim);

    // With all this in place, we can go on with the loop over all cells. The
    // body of this loop has been discussed in the introduction, and will not
    // be commented any further here:
    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();
    for (; cell!=endc; ++cell)
      {
        fe_values.reinit (cell);
        local_matrix = 0;
        local_rhs = 0;

        right_hand_side.value_list (fe_values.get_quadrature_points(),
                                    rhs_values);
        k_inverse.value_list (fe_values.get_quadrature_points(),
                              k_inverse_values);

        for (unsigned int q=0; q<n_q_points; ++q)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            {
              const Tensor<1,dim> phi_i_u     = fe_values[velocities].value (i, q);
              const double        div_phi_i_u = fe_values[velocities].divergence (i, q);
              const double        phi_i_p     = fe_values[pressure].value (i, q);

              for (unsigned int j=0; j<dofs_per_cell; ++j)
                {
                  const Tensor<1,dim> phi_j_u     = fe_values[velocities].value (j, q);
                  const double        div_phi_j_u = fe_values[velocities].divergence (j, q);
                  const double        phi_j_p     = fe_values[pressure].value (j, q);

                  local_matrix(i,j) += (phi_i_u * k_inverse_values[q] * phi_j_u
                                        - div_phi_i_u * phi_j_p
                                        - phi_i_p * div_phi_j_u)
                                       * fe_values.JxW(q);
                }

              local_rhs(i) += -phi_i_p *
                              rhs_values[q] *
                              fe_values.JxW(q);
            }

        for (unsigned int face_no=0;
             face_no<GeometryInfo<dim>::faces_per_cell;
             ++face_no)
          if (cell->at_boundary(face_no))
            {
              fe_face_values.reinit (cell, face_no);

              pressure_boundary_values
              .value_list (fe_face_values.get_quadrature_points(),
                           boundary_values);

              for (unsigned int q=0; q<n_face_q_points; ++q)
                for (unsigned int i=0; i<dofs_per_cell; ++i)
                  local_rhs(i) += -(fe_face_values[velocities].value (i, q) *
                                    fe_face_values.normal_vector(q) *
                                    boundary_values[q] *
                                    fe_face_values.JxW(q));
            }

        // The final step in the loop over all cells is to transfer local
        // contributions into the global matrix and right hand side
        // vector. Note that we use exactly the same interface as in previous
        // examples, although we now use block matrices and vectors instead of
        // the regular ones. In other words, to the outside world, block
        // objects have the same interface as matrices and vectors, but they
        // additionally allow to access individual blocks.
        cell->get_dof_indices (local_dof_indices);
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          for (unsigned int j=0; j<dofs_per_cell; ++j)
            system_matrix.add (local_dof_indices[i],
                               local_dof_indices[j],
                               local_matrix(i,j));
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          system_rhs(local_dof_indices[i]) += local_rhs(i);
      }
  }

  // @sect3{Linear solvers and preconditioners}

  // The linear solvers and preconditioners we use in this example have been
  // discussed in significant detail already in the introduction. We will
  // therefore not discuss the rationale for these classes here any more, but
  // rather only comment on implementational aspects.

  // @sect4{The SchurComplement class template}

  // The next class is the Schur complement class. Its rationale has also been
  // discussed in length in the introduction. The only things we would like to
  // note is that the class, too, is derived from the Subscriptor
  // class and that as mentioned above it stores pointers to the entire block
  // matrix and the inverse of the mass matrix block using
  // SmartPointer objects.
  //
  // The vmult function requires two temporary vectors that we do
  // not want to re-allocate and free every time we call this function. Since
  // here, we have full control over the use of these vectors (unlike above,
  // where a class called by the vmult function required these
  // vectors, not the vmult function itself), we allocate them
  // directly, rather than going through the VectorMemory
  // mechanism. However, again, these member variables do not carry any state
  // between successive calls to the member functions of this class (i.e., we
  // never care what values they were set to the last time a member function
  // was called), we mark these vectors as mutable.
  //
  // The rest of the (short) implementation of this class is straightforward
  // if you know the order of matrix-vector multiplications performed by the
  // vmult function:
  class SchurComplement : public Subscriptor
  {
  public:
    SchurComplement (const BlockSparseMatrix<double> &A,
                     const IterativeInverse<Vector<double> > &Minv);

    void vmult (Vector<double>       &dst,
                const Vector<double> &src) const;

  private:
    const SmartPointer<const BlockSparseMatrix<double> > system_matrix;
    const SmartPointer<const IterativeInverse<Vector<double> > > m_inverse;

    mutable Vector<double> tmp1, tmp2;
  };

  SchurComplement::SchurComplement (const BlockSparseMatrix<double> &A,
                                    const IterativeInverse<Vector<double> > &Minv)
    :
    system_matrix (&A),
    m_inverse (&Minv),
    tmp1 (A.block(0,0).m()),
    tmp2 (A.block(0,0).m())
  {}

  void SchurComplement::vmult (Vector<double>       &dst,
                               const Vector<double> &src) const
  {
    system_matrix->block(0,1).vmult (tmp1, src);
    m_inverse->vmult (tmp2, tmp1);
    system_matrix->block(1,0).vmult (dst, tmp2);
  }

  // @sect4{The ApproximateSchurComplement class template}

  // The third component of our solver and preconditioner system is the class
  // that approximates the Schur complement so we can form a an InverseIterate
  // object that approximates the inverse of the Schur complement. It follows
  // the same pattern as the Schur complement class, with the only exception
  // that we do not multiply with the inverse mass matrix in
  // vmult, but rather just do a single Jacobi
  // step. Consequently, the class also does not have to store a pointer to an
  // inverse mass matrix object.
  //
  // We will later use this class as a template argument to the
  // IterativeInverse class which will in turn want to use it as a
  // template argument for the PointerMatrix class. The latter class
  // has a function that requires us to also write a function that
  // provides the product with the transpose of the matrix this object
  // represents. As a consequence, in the code below, we also
  // implement a <tt>Tvmult</tt> function here that represents the
  // product of the transpose matrix with a vector. It is easy to see
  // how this needs to be implemented here: since the matrix is
  // symmetric, we can as well call vmult wherever the
  // product with the transpose matrix is required. (Note, however,
  // that even though we implement this function here, there will in
  // fact not be any need for it as long as we use SolverCG as the
  // solver since that solver does not ever call the function that
  // provides this operation.)
  class ApproximateSchurComplement : public Subscriptor
  {
  public:
    ApproximateSchurComplement (const BlockSparseMatrix<double> &A);

    void vmult (Vector<double>       &dst,
                const Vector<double> &src) const;
    void Tvmult (Vector<double>       &dst,
                 const Vector<double> &src) const;

  private:
    const SmartPointer<const BlockSparseMatrix<double> > system_matrix;

    mutable Vector<double> tmp1, tmp2;
  };

  ApproximateSchurComplement::ApproximateSchurComplement (const BlockSparseMatrix<double> &A)
    :
    system_matrix (&A),
    tmp1 (A.block(0,0).m()),
    tmp2 (A.block(0,0).m())
  {}

  void ApproximateSchurComplement::vmult (Vector<double>       &dst,
                                          const Vector<double> &src) const
  {
    system_matrix->block(0,1).vmult (tmp1, src);
    system_matrix->block(0,0).precondition_Jacobi (tmp2, tmp1);
    system_matrix->block(1,0).vmult (dst, tmp2);
  }

  void ApproximateSchurComplement::Tvmult (Vector<double>       &dst,
                                           const Vector<double> &src) const
  {
    vmult (dst, src);
  }

  // @sect4{MixedLaplace::solve}

  // After all these preparations, we can finally write the function that
  // actually solves the linear problem. We will go through the two parts it
  // has that each solve one of the two equations, the first one for the
  // pressure (component 1 of the solution), then the velocities (component 0
  // of the solution). Both parts need an object representing the inverse mass
  // matrix and an auxiliary vector, and we therefore declare these objects at
  // the beginning of this function.
  template <int dim>
  void MixedLaplaceProblem<dim>::solve ()
  {
    PreconditionIdentity identity;
    IterativeInverse<Vector<double> > m_inverse;
    m_inverse.initialize(system_matrix.block(0,0), identity);
    m_inverse.solver.select("cg");
    static ReductionControl inner_control(1000, 0., 1.e-13);
    m_inverse.solver.set_control(inner_control);

    Vector<double> tmp (solution.block(0).size());

    // Now on to the first equation. The right hand side of it is $B^TM^{-1}F-G$,
    // which is what we compute in the first few lines. We then declare the
    // objects representing the Schur complement, its approximation, and the
    // inverse of the approximation. Finally, we declare a solver object and
    // hand off all these matrices and vectors to it to compute block 1 (the
    // pressure) of the solution:
    {
      Vector<double> schur_rhs (solution.block(1).size());

      m_inverse.vmult (tmp, system_rhs.block(0));
      system_matrix.block(1,0).vmult (schur_rhs, tmp);
      schur_rhs -= system_rhs.block(1);

      SchurComplement
      schur_complement (system_matrix, m_inverse);

      ApproximateSchurComplement
      approximate_schur_complement (system_matrix);

      IterativeInverse<Vector<double> >
      preconditioner;
      preconditioner.initialize(approximate_schur_complement, identity);
      preconditioner.solver.select("cg");
      preconditioner.solver.set_control(inner_control);

      SolverControl solver_control (solution.block(1).size(),
                                    1e-12*schur_rhs.l2_norm());
      SolverCG<>    cg (solver_control);

      cg.solve (schur_complement, solution.block(1), schur_rhs,
                preconditioner);

      std::cout << solver_control.last_step()
                << " CG Schur complement iterations to obtain convergence."
                << std::endl;
    }

    // After we have the pressure, we can compute the velocity. The equation
    // reads $MU=-BP+F$, and we solve it by first computing the right hand
    // side, and then multiplying it with the object that represents the
    // inverse of the mass matrix:
    {
      system_matrix.block(0,1).vmult (tmp, solution.block(1));
      tmp *= -1;
      tmp += system_rhs.block(0);

      m_inverse.vmult (solution.block(0), tmp);
    }
  }

  // @sect3{MixedLaplaceProblem class implementation (continued)}

  // @sect4{MixedLaplace::compute_errors}

  // After we have dealt with the linear solver and preconditioners, we
  // continue with the implementation of our main class. In particular, the
  // next task is to compute the errors in our numerical solution, in both the
  // pressures as well as velocities.
  //
  // To compute errors in the solution, we have already introduced the
  // VectorTools::integrate_difference function in step-7 and
  // step-11. However, there we only dealt with scalar solutions, whereas here
  // we have a vector-valued solution with components that even denote
  // different quantities and may have different orders of convergence (this
  // isn't the case here, by choice of the used finite elements, but is
  // frequently the case in mixed finite element applications). What we
  // therefore have to do is to `mask' the components that we are interested
  // in. This is easily done: the
  // VectorTools::integrate_difference function takes as its last
  // argument a pointer to a weight function (the parameter defaults to the
  // null pointer, meaning unit weights). What we simply have to do is to pass
  // a function object that equals one in the components we are interested in,
  // and zero in the other ones. For example, to compute the pressure error,
  // we should pass a function that represents the constant vector with a unit
  // value in component dim, whereas for the velocity the
  // constant vector should be one in the first dim components,
  // and zero in the location of the pressure.
  //
  // In deal.II, the ComponentSelectFunction does exactly this:
  // it wants to know how many vector components the function it is to
  // represent should have (in our case this would be dim+1, for
  // the joint velocity-pressure space) and which individual or range of
  // components should be equal to one. We therefore define two such masks at
  // the beginning of the function, following by an object representing the
  // exact solution and a vector in which we will store the cellwise errors as
  // computed by integrate_difference:
  template <int dim>
  void MixedLaplaceProblem<dim>::compute_errors () const
  {
    const ComponentSelectFunction<dim>
    pressure_mask (dim, dim+1);
    const ComponentSelectFunction<dim>
    velocity_mask(std::make_pair(0, dim), dim+1);

    ExactSolution<dim> exact_solution;
    Vector<double> cellwise_errors (triangulation.n_active_cells());

    // As already discussed in step-7, we have to realize that it is
    // impossible to integrate the errors exactly. All we can do is
    // approximate this integral using quadrature. This actually presents a
    // slight twist here: if we naively chose an object of type
    // QGauss@<dim@>(degree+1) as one may be inclined to do (this
    // is what we used for integrating the linear system), one realizes that
    // the error is very small and does not follow the expected convergence
    // curves at all. What is happening is that for the mixed finite elements
    // used here, the Gauss points happen to be superconvergence points in
    // which the pointwise error is much smaller (and converges with higher
    // order) than anywhere else. These are therefore not particularly good
    // points for ingration. To avoid this problem, we simply use a
    // trapezoidal rule and iterate it degree+2 times in each
    // coordinate direction (again as explained in step-7):
    QTrapez<1>     q_trapez;
    QIterated<dim> quadrature (q_trapez, degree+2);

    // With this, we can then let the library compute the errors and output
    // them to the screen:
    VectorTools::integrate_difference (dof_handler, solution, exact_solution,
                                       cellwise_errors, quadrature,
                                       VectorTools::L2_norm,
                                       &pressure_mask);
    const double p_l2_error = cellwise_errors.l2_norm();

    VectorTools::integrate_difference (dof_handler, solution, exact_solution,
                                       cellwise_errors, quadrature,
                                       VectorTools::L2_norm,
                                       &velocity_mask);
    const double u_l2_error = cellwise_errors.l2_norm();

    std::cout << "Errors: ||e_p||_L2 = " << p_l2_error
              << ",   ||e_u||_L2 = " << u_l2_error
              << std::endl;
  }

  // @sect4{MixedLaplace::output_results}

  // The last interesting function is the one in which we generate graphical
  // output. Everything here looks obvious and familiar. Note how we construct
  // unique names for all the solution variables at the beginning, like we did
  // in step-8 and other programs later on. The only thing worth mentioning is
  // that for higher order elements, in seems inappropriate to only show a
  // single bilinear quadrilateral per cell in the graphical output. We
  // therefore generate patches of size (degree+1)x(degree+1) to capture the
  // full information content of the solution. See the step-7 tutorial program
  // for more information on this.
  //
  // Note that we output the dim+1 components of the solution
  // vector as a collection of individual scalars here. Most visualization
  // programs will then only offer to visualize them individually, rather than
  // allowing us to plot the flow field as a vector field. However, as
  // explained in the corresponding function of step-22 or the @ref VVOutput
  // "Generating graphical output" section of the @ref vector_valued module,
  // instructing the DataOut class to identify components of the FESystem
  // object as elements of a dim-dimensional vector is not
  // actually very difficult and will then allow us to show results as vector
  // plots. We skip this here for simplicity and refer to the links above for
  // more information.
  template <int dim>
  void MixedLaplaceProblem<dim>::output_results () const
  {
    std::vector<std::string> solution_names;
    switch (dim)
      {
      case 2:
        solution_names.push_back ("u");
        solution_names.push_back ("v");
        solution_names.push_back ("p");
        break;

      case 3:
        solution_names.push_back ("u");
        solution_names.push_back ("v");
        solution_names.push_back ("w");
        solution_names.push_back ("p");
        break;

      default:
        Assert (false, ExcNotImplemented());
      }

    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution, solution_names);

    data_out.build_patches (degree+1);

    std::ofstream output ("solution.gmv");
    data_out.write_gmv (output);
  }

  // @sect4{MixedLaplace::run}

  // This is the final function of our main class. It's only job is to call
  // the other functions in their natural order:
  template <int dim>
  void MixedLaplaceProblem<dim>::run ()
  {
    make_grid_and_dofs();
    assemble_system ();
    solve ();
    compute_errors ();
    output_results ();
  }
}

// @sect3{The main function}

// The main function we stole from step-6 instead of step-4. It is almost
// equal to the one in step-6 (apart from the changed class names, of course),
// the only exception is that we pass the degree of the finite element space
// to the constructor of the mixed laplace problem (here, we use zero-th order
// elements).
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step20;

      deallog.depth_console (0);

      MixedLaplaceProblem<2> mixed_laplace_problem(0);
      mixed_laplace_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#ifndef __fovis_internal_utils_hpp__
#define __fovis_internal_utils_hpp__

#include <stdio.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <emmintrin.h>

#include "options.hpp"

namespace fovis
{

#define FOVIS_IS_ALIGNED16(x) (((uintptr_t)(x) & 0xf) == 0)

static inline int
round_up_to_multiple(int x, int a)
{
  int rem = x % a;
  if(rem)
    return x + (a - rem);
  return x;
}

static inline Eigen::Vector3d
_quat_to_roll_pitch_yaw(const Eigen::Quaterniond&q)
{
  double roll_a = 2 * (q.w() * q.x() + q.y() * q.z());
  double roll_b = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
  double roll = atan2(roll_a, roll_b);

  double pitch_sin = 2 * (q.w() * q.y() - q.z() * q.x());
  double pitch = asin(pitch_sin);

  double yaw_a = 2 * (q.w() * q.z() + q.x() * q.y());
  double yaw_b = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
  double yaw = atan2(yaw_a, yaw_b);
  return Eigen::Vector3d(roll, pitch, yaw);
}

static inline Eigen::Quaterniond
_rpy_to_quat(const Eigen::Vector3d rpy)
{
  double roll = rpy(0), pitch = rpy(1), yaw = rpy(2);

  double halfroll = roll / 2;
  double halfpitch = pitch / 2;
  double halfyaw = yaw / 2;

  double sin_r2 = sin(halfroll);
  double sin_p2 = sin(halfpitch);
  double sin_y2 = sin(halfyaw);

  double cos_r2 = cos(halfroll);
  double cos_p2 = cos(halfpitch);
  double cos_y2 = cos(halfyaw);

  Eigen::Quaterniond q;
  q.w() = cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2;
  q.x() = sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2;
  q.y() = cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2;
  q.z() = cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2;
  return q;
}

static inline void
print_isometry(const Eigen::Isometry3d & iso)
{
  const Eigen::Vector3d & t = iso.translation();
  Eigen::Vector3d rpy = _quat_to_roll_pitch_yaw(Eigen::Quaterniond(iso.rotation()))*180.0/M_PI;
  fprintf(stderr, "trans:(% 6.3f % 6.3f % 6.3f) rot:(% 6.3f % 6.3f % 6.3f)",t(0),t(1),t(2),rpy(0),rpy(1),rpy(2));
  //    dbg("rot:(% 6.3f % 6.3f % 6.3f)",rpy(0),rpy(1),rpy(2));
}

bool optionsGetInt(const VisualOdometryOptions& options, std::string name,
        int* result);

bool optionsGetBool(const VisualOdometryOptions& options, std::string name,
        bool* result);

int optionsGetIntOrFromDefault(const VisualOdometryOptions& options,
        std::string name, const VisualOdometryOptions& defaults);

bool optionsGetBoolOrFromDefault(const VisualOdometryOptions& options,
        std::string name, const VisualOdometryOptions& defaults);

bool optionsGetDouble(const VisualOdometryOptions& options, std::string name,
        double* result);

double optionsGetDoubleOrFromDefault(const VisualOdometryOptions& options,
        std::string name, const VisualOdometryOptions& defaults);

}

#endif


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file standard_gaussian_mapping.hpp
 * \date May 2014
 * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <type_traits>

#include <fl/util/traits.hpp>
#include <fl/util/scalar_matrix.hpp>
#include <fl/distribution/interface/sampling.hpp>
#include <fl/distribution/standard_gaussian.hpp>

namespace fl
{

/**
 * \ingroup distribution_interfaces
 *
 * \brief Represents the interface which provides a mapping
 *        from a standard normal variate onto the underlying distribution which
 *        implements this interface
 *
 * \tparam Variate              The Distribution variate type. This is the type
 *                              which is being returned by the mapping or
 *                              sampling, respectively.
 * \tparam StdVariateDimension  Dimension of the source variate type which is
 *                              the standard normal variate
 *                              \f$x_{SNV}\sim{\cal N}(0, I)\f$
 */
template <typename Variate, int StdVariateDimension>
class StandardGaussianMapping
    : public Sampling<Variate>
{
public:
    typedef Eigen::Matrix<Real, StdVariateDimension, 1> StandardVariate;

    /**
     * StandardGaussianMapping constructor. It initializes the mapper
     *
     * \param snv_dimension     Dimension of the standard normal variate
     */
    explicit StandardGaussianMapping(
            int snv_dimension = DimensionOf<StandardVariate>())
        : standard_gaussian_(snv_dimension)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~StandardGaussianMapping() noexcept { }

    /**
     * \brief Mapps a standard normal variate onto a sample of the underlying
     *        distribution which implements this mapper
     *
     * \param sample  SNV sample which will be mapped onto a variate sampe
     *
     * \return A variate according to the underlying distribution
     */
    virtual Variate map_standard_normal(const StandardVariate& sample) const = 0;

    /**
     * \return A variate according to the underlying distribution
     */
    virtual Variate sample() const
    {
        return map_standard_normal(standard_gaussian_.sample());
    }

    /**
     * \return Dimension of the standard normal variate used for mapping
     */
    virtual int standard_variate_dimension() const
    {
        return standard_gaussian_.dimension();
    }

    /**
     * \brief Sets the dimension of the standard normal variate
     *
     * \param snv_dimension The new dimension of the SNV
     */
    virtual void standard_variate_dimension(int snv_dimension)
    {
        standard_gaussian_.dimension(snv_dimension);
    }

protected:
    /**
     * \brief SNV generator
     */
    mutable StandardGaussian<StandardVariate> standard_gaussian_;
};

/**
 * \ingroup distribution_interfaces
 *
 * \brief Represents the interface which provides a mapping
 *        from a scalar standard normal variate onto the underlying distribution
 *        which implements this interface.
 *
 * \tparam Variate          The Distribution variate type. This is the type
 *                          which is being returned by the mapping or sampling,
 *                          respectively.
 */
template <typename Variate>
class StandardGaussianMapping<Variate, 1>
    : public Sampling<Variate>
{
public:
    typedef ScalarMatrix StandardVariate;

    /// \todo fix this (unused argument)
    explicit StandardGaussianMapping(int snv_dimension = 1)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~StandardGaussianMapping() noexcept { }

    /**
     * \brief Mapps a one dimensional standard normal variate onto a sample of
     *        the underlying distribution which implements this mapper
     *
     * \param sample SNV sample which will be mapped onto a variate sampe
     *
     * \return A variate according to the underlying distribution
     */
    virtual Variate map_standard_normal(const StandardVariate& sample) const = 0;

    /**
     * \return A variate according to the underlying distribution
     */
    virtual Variate sample() const
    {
        return map_standard_normal(standard_gaussian_.sample());
    }

    /**
     * \return Dimension of the standard normal variate used for mapping
     */
    virtual int standard_variate_dimension() const
    {
        return 1;
    }

    /// \todo fix this (unused argument)
    virtual void standard_variate_dimension(int snv_dimension)
    {
    }

protected:
    /**
     * \brief One dimensional SNV generator
     */
    mutable StandardGaussian<Real> standard_gaussian_;
};

}


#include <vector>
#include <iostream>

#include <glm/glm.hpp>
using namespace glm;

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include "Types.h"
#include "Dynamic.h"

namespace{
    using namespace BalloonFEM;
    /* push mat3 coefficients into SpMat,
     * T(target_off + i, source_off + j, H[j][i])
     * pay attention that H is column major
     */
    void pushMat3(std::vector<T> &coeff, int target_off, int source_off, Mat3 H)
    {
        for(int i = 0; i < 3; i++)
            for(int j = 0; j < 3; j++)
			if (H[j][i] != 0)
                coeff.push_back( T(target_off + i, source_off + j, H[j][i]) );
    }

    /* push mat3 coefficients into SpMat,
     * T(target_off, source_off + i, H[i])
     */
    void pushVec3(std::vector<T> &coeff, int target_off, int source_off, Vec3 H)
    {
        for(int i = 0; i < 3; i++)
            coeff.push_back( T(target_off, source_off + i, H[i]) );
    }

    int sgn(double val){
        return ((double(0) < val) - (val < double(0)));
    }
}

namespace BalloonFEM
{
    SpMat Engine::bendingForceAndGradient(ObjState &state, Vvec3 &f_sum)
    {
        /* compute theta */
        Vvec3 &pos = state.world_space_pos;

        SpVec dphi = SpVec::Zero(m_tetra->num_hindges);
        SpMat ddphi(m_tetra->num_hindges, m_tetra->num_hindges);

        ///////////////////////////////////////////////////////////////////////
        /* compute hindge angle theta and phi, dphi, ddphi */
        int offset = 0;
        for(MIter f = m_tetra->films.begin(); f != m_tetra->films.end(); f++)
        {
			std::vector<Piece> &pieces = f->pieces;
            for(EIter h = f->hindges.begin(); h != f->hindges.end(); h++)
            {
                /* normal of piece_info[0] */
                iVec3 &id0 = pieces[h->piece_info[0].x].v_id;
                Vec3 n0 = cross( pos[id0[0]] - pos[id0[2]], pos[id0[1]] - pos[id0[2]] );
                n0 /= length(n0);

                /* normal of piece_info[1] */
                iVec3 &id1 = pieces[h->piece_info[1].x].v_id;
                Vec3 n1 = cross( pos[id1[0]] - pos[id1[2]], pos[id1[1]] - pos[id1[2]] );
                n1 /= length(n1);

                /* edge direction , is the positive direction of piece_info[0]*/
				int i = h->piece_info[0].y;
				int j = (i + 1) % 3, k = (i + 2) % 3;
                Vec3 e = pos[id0[k]] -  pos[id0[j]];
                e /= length(e);

				/* signed theta is defined as positive when n0 n1 point away from each other */
                /* energy is 2*sin(x/2)^2 */
				double tmp = max(min(dot(n0, n1), 1.0), -1.0);
                double theta = acos(tmp) * sgn(dot(e, cross(n0, n1)));
                dphi(offset) = m_bend_model->dphi(theta, h->theta);
                ddphi.coeffRef(offset, offset) = m_bend_model->ddphi(theta, h->theta);
                offset ++;
            }
        }

        //////////////////////////////////////////////////////////////////////
        /* compute hessian matrix and theta gradient */
		printf("compute bending force and hessian matrix \n");
        std::vector<T> theta_coeff;
        theta_coeff.clear();
		theta_coeff.reserve( 2 * 9 * m_tetra->num_hindges );

        std::vector<T> hessian_coeff;
        hessian_coeff.clear();
        hessian_coeff.reserve( 9 * 9 * m_tetra->num_pieces );

        /* compute detrivation of hindge angle theta */
        offset = 0;     /* since there may be more than one film obj */
        for(MIter f = m_tetra->films.begin(); f != m_tetra->films.end(); f++)
        {
            for(PIter p = f->pieces.begin(); p != f->pieces.end(); p++)
            {
                iVec3 &v_id = p->v_id;
                /* edge dire , length , norm and area*/
                Vec3 e[3] = {
                    pos[v_id[2]] - pos[v_id[1]],
                    pos[v_id[0]] - pos[v_id[2]],
                    pos[v_id[1]] - pos[v_id[0]],
                };

                Vec3 norm = cross( e[0], e[1] );
                double A2 = length(norm);
                norm /= A2;

				Vec3 l = Vec3(length(e[0]), length(e[1]), length(e[2]));
				for (int i = 0; i < 3; i++)
					e[i] /= l[i];

                /* height of vertex */
                Vec3 h_inv = l / A2;
				Mat3 w = outerProduct(h_inv, h_inv);

                /* cosine of vertex */

                double cos[3] = {
                    - dot(e[1], e[2]),
                    - dot(e[2], e[0]),
                    - dot(e[0], e[1]),
                };

                /* edge normal m */
                Vec3 m[3] = {
                    cross(e[0], norm),
                    cross(e[1], norm),
                    cross(e[2], norm),
                };

                /* intermediate var Mi */
                Mat3 M[3] = {
                    outerProduct(norm, m[0]),
					outerProduct(norm, m[1]),
					outerProduct(norm, m[2]),
                };

                /* intermediate var Ni */
                Mat3 N[3] = {
                    M[0] / (l.x * l.x), M[1] / (l.y * l.y), M[2] / (l.z * l.z)
                };

                /* intermediate var ci */
                double c[3] = {0, 0, 0};
                Mat3 R[3] = {Mat3(0), Mat3(0), Mat3(0)};
                for(int i = 0; i < 3; i++)
                    if (p->hindge_id[i] != -1)
                    {
						c[i] = dphi(p->hindge_id[i] + offset);
                        R[i] = c[i]*N[i];
                    }

                /* intermediate var di */
                double d[3] = {0, 0, 0};
                for(int i = 0; i < 3; i++)
                {
                    int j = (i+1) % 3, k = (i+2) % 3;
                    d[i] = c[j] * cos[k] + c[k] * cos[j] - c[i];
                }

                /* for each triangle, compute contribution to Heissen */
                for(int i = 0; i < 3; i++)
                {
                    int j = (i+1) % 3, k = (i+2) % 3;

                    /* j = i */
                    Mat3 H = w[i][i] * d[i] * (M[i] + transpose(M[i])) - R[j] - R[k];

                    pushMat3(hessian_coeff, 3 * v_id[i], 3 * v_id[i], H);

                    /* j = i + 1 */
                    H = w[i][j] * (d[i] * transpose(M[j]) + d[j] * M[i]) + R[k];

                    pushMat3(hessian_coeff, 3 * v_id[i], 3 * v_id[j], H);

                    /* symmetric j = i + 2 */
                    pushMat3(hessian_coeff, 3 * v_id[j], 3 * v_id[i], transpose(H));
                }

                /* for each vertex if its correspond edge is hindge
                 * compute gradient of theta
                 */
                for(int i = 0; i < 3; i++)
                {
                    if (p->hindge_id[i] != -1)
                    {
                        int h_id = p->hindge_id[i] + offset;

                        int j = (i+1) % 3;
                        int k = (i+2) % 3;

                        Vec3 tmp;

                        /* \dev_{x0} theta = - n / h0 */
                        tmp = - norm * h_inv[i];
                        pushVec3( theta_coeff, h_id, 3 * v_id[i], tmp);

                        /* \dev_{x1} theta = n * cos2 / h1 */
                        tmp = norm * cos[k] * h_inv[j];
                        pushVec3( theta_coeff, h_id, 3 * v_id[j], tmp);

                        /* \dev_{x1} theta = n * cos1 / h2 */
                        tmp = norm * cos[j] * h_inv[k];
                        pushVec3( theta_coeff, h_id, 3 * v_id[k], tmp);
                    }
                }

            }
            offset += f->hindges.size();
        }

        /* compute bending force */
        SpMat theta_grad(m_tetra->num_hindges, 3 * m_tetra->num_vertex);
        theta_grad.setFromTriplets(theta_coeff.begin(), theta_coeff.end());

        SpVec bendforce = - dphi.transpose() * theta_grad;

        for(size_t i = 0; i < f_sum.size(); i++)
        {
            f_sum[i] += Vec3(
                    bendforce(3*i    ),
                    bendforce(3*i + 1),
                    bendforce(3*i + 2));
        }

        SpMat H(3 * m_tetra->num_vertex, 3 * m_tetra->num_vertex);

        H.setFromTriplets(hessian_coeff.begin(), hessian_coeff.end());

		H +=  theta_grad.transpose() * ddphi * theta_grad;

        return H;
    }
}


#include <Eigen/LU>
#include <cmath>
#include <iostream>
#include <mathtoolbox/constants.hpp>
#include <mathtoolbox/log-determinant.hpp>
#include <mathtoolbox/probability-distributions.hpp>
#include <nlopt-util.hpp>
#include <sequential-line-search/gaussian-process-regressor.hpp>
#include <sequential-line-search/utils.hpp>

using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace
{
    using namespace sequential_line_search;

    const bool   use_log_normal_prior  = true;
    const double a_prior_mu            = std::log(0.500);
    const double a_prior_sigma_squared = 0.50;
    const double b_prior_mu            = std::log(1e-04);
    const double b_prior_sigma_squared = 0.50;
    const double r_prior_mu            = std::log(0.500);
    const double r_prior_sigma_squared = 0.50;

    inline VectorXd Concat(const double scalar, const VectorXd& vector)
    {
        VectorXd result(vector.size() + 1);

        result(0)                        = scalar;
        result.segment(1, vector.size()) = vector;

        return result;
    }

    double calc_grad_a_prior(const double a)
    {
        return mathtoolbox::GetLogOfLogNormalDistDerivative(a, a_prior_mu, a_prior_sigma_squared);
    }

    double calc_grad_b_prior(const double b)
    {
        return mathtoolbox::GetLogOfLogNormalDistDerivative(b, b_prior_mu, b_prior_sigma_squared);
    }

    double calc_grad_r_i_prior(const Eigen::VectorXd& r, const int index)
    {
        return mathtoolbox::GetLogOfLogNormalDistDerivative(r(index), r_prior_mu, r_prior_sigma_squared);
    }

    double calc_a_prior(const double a)
    {
        return mathtoolbox::GetLogOfLogNormalDist(a, a_prior_mu, a_prior_sigma_squared);
    }

    double calc_b_prior(const double b)
    {
        return mathtoolbox::GetLogOfLogNormalDist(b, b_prior_mu, b_prior_sigma_squared);
    }

    double calc_r_i_prior(const Eigen::VectorXd& r, const int index)
    {
        return mathtoolbox::GetLogOfLogNormalDist(r(index), r_prior_mu, r_prior_sigma_squared);
    }

    double calc_grad_b(const MatrixXd& X,
                       const MatrixXd& K_y_inv,
                       const VectorXd& y,
                       const double    a,
                       const double    b,
                       const VectorXd& r)
    {
        const MatrixXd K_y_grad_b = CalcLargeKYNoiseLevelDerivative(X, Concat(a, r), b);
        const double   term1      = +0.5 * y.transpose() * K_y_inv * K_y_grad_b * K_y_inv * y;
        const double   term2      = -0.5 * (K_y_inv * K_y_grad_b).trace();
        return term1 + term2 + (use_log_normal_prior ? calc_grad_b_prior(b) : 0.0);
    }

    VectorXd calc_grad_theta(const MatrixXd&             X,
                             const MatrixXd&             K_y_inv,
                             const VectorXd&             y,
                             const VectorXd&             kernel_hyperparams,
                             const KernelThetaDerivative kernel_theta_derivative)
    {
        const std::vector<MatrixXd> tensor = CalcLargeKYThetaDerivative(X, kernel_hyperparams, kernel_theta_derivative);

        VectorXd grad(kernel_hyperparams.size());
        for (unsigned i = 0; i < kernel_hyperparams.size(); ++i)
        {
            const MatrixXd& K_y_grad_r_i = tensor[i];

            const double term1 = +0.5 * y.transpose() * K_y_inv * K_y_grad_r_i * K_y_inv * y;
            const double term2 = -0.5 * (K_y_inv * K_y_grad_r_i).trace();

            const double prior =
                use_log_normal_prior
                    ? (i == 0
                           ? calc_grad_a_prior(kernel_hyperparams(i))
                           : calc_grad_r_i_prior(kernel_hyperparams.segment(1, kernel_hyperparams.size() - 1), i - 1))
                    : 0.0;

            grad(i) = term1 + term2 + prior;
        }

        return grad;
    }

    VectorXd calc_grad(const MatrixXd&             X,
                       const MatrixXd&             K_y_inv,
                       const VectorXd&             y,
                       const double                a,
                       const double                b,
                       const VectorXd&             r,
                       const KernelThetaDerivative kernel_theta_derivative)
    {
        const unsigned D = X.rows();

        VectorXd grad(D + 2);

        const VectorXd grad_theta = calc_grad_theta(X, K_y_inv, y, Concat(a, r), kernel_theta_derivative);

        grad(0)            = grad_theta(0);
        grad(1)            = calc_grad_b(X, K_y_inv, y, a, b, r);
        grad.segment(2, D) = grad_theta.segment(1, D);

        return grad;
    }

    struct Data
    {
        const MatrixXd              X;
        const VectorXd              y;
        const Kernel                kernel;
        const KernelThetaDerivative kernel_theta_derivative;
    };

    // For counting the number of function evaluations
    unsigned count;

    // Log likelihood that will be maximized
    double objective(const std::vector<double>& x, std::vector<double>& grad, void* data)
    {
        // For counting the number of function evaluations
        ++count;

        const MatrixXd& X = static_cast<const Data*>(data)->X;
        const VectorXd& y = static_cast<const Data*>(data)->y;

        const auto kernel                  = static_cast<const Data*>(data)->kernel;
        const auto kernel_theta_derivative = static_cast<const Data*>(data)->kernel_theta_derivative;

        const unsigned N = X.cols();

        const double   a = x[0];
        const double   b = x[1];
        const VectorXd r = Eigen::Map<const VectorXd>(&x[2], x.size() - 2);

        const MatrixXd K_y     = CalcLargeKY(X, Concat(a, r), b, kernel);
        const MatrixXd K_y_inv = K_y.inverse();

        // When the algorithm is gradient-based, compute the gradient vector
        if (grad.size() == x.size())
        {
            const VectorXd g = calc_grad(X, K_y_inv, y, a, b, r, kernel_theta_derivative);
            for (unsigned i = 0; i < g.rows(); ++i)
            {
                grad[i] = g(i);
            }
        }

        // Constant
        constexpr double prod_of_two_and_pi = 2.0 * mathtoolbox::constants::pi;

        const double term1 = -0.5 * y.transpose() * K_y_inv * y;
        const double term2 = -0.5 * mathtoolbox::CalcLogDetOfSymmetricPositiveDefiniteMatrix(K_y);
        const double term3 = -0.5 * N * std::log(prod_of_two_and_pi);

        // Computing the regularization terms from a prior assumptions
        const double a_prior = calc_a_prior(a);
        const double b_prior = calc_b_prior(b);
        const double r_prior = [&r]()
        {
            double sum = 0.0;
            for (unsigned i = 0; i < r.rows(); ++i)
            {
                sum += calc_r_i_prior(r, i);
            }
            return sum;
        }();
        const double regularization = use_log_normal_prior ? (a_prior + b_prior + r_prior) : 0.0;

        return term1 + term2 + term3 + regularization;
    }
} // namespace

namespace sequential_line_search
{
    GaussianProcessRegressor::GaussianProcessRegressor(const MatrixXd&  X,
                                                       const VectorXd&  y,
                                                       const KernelType kernel_type)
        : Regressor(kernel_type), m_X(X), m_y(y)
    {
        if (X.rows() == 0)
        {
            return;
        }

        PerformMapEstimation();

        m_K_y     = CalcLargeKY(X, m_kernel_hyperparams, m_noise_hyperparam, m_kernel);
        m_K_y_inv = m_K_y.inverse();
    }

    GaussianProcessRegressor::GaussianProcessRegressor(const Eigen::MatrixXd& X,
                                                       const Eigen::VectorXd& y,
                                                       const Eigen::VectorXd& kernel_hyperparams,
                                                       double                 noise_hyperparam,
                                                       const KernelType       kernel_type)
        : Regressor(kernel_type),
          m_X(X),
          m_y(y),
          m_kernel_hyperparams(kernel_hyperparams),
          m_noise_hyperparam(noise_hyperparam)
    {
        if (X.rows() == 0)
        {
            return;
        }

        m_K_y     = CalcLargeKY(X, m_kernel_hyperparams, m_noise_hyperparam, m_kernel);
        m_K_y_inv = m_K_y.inverse();
    }

    double GaussianProcessRegressor::PredictMu(const VectorXd& x) const
    {
        // TODO: Incorporate a mean function
        const VectorXd k = CalcSmallK(x, m_X, m_kernel_hyperparams, m_kernel);
        return k.transpose() * m_K_y_inv * m_y;
    }

    double GaussianProcessRegressor::PredictSigma(const VectorXd& x) const
    {
        const VectorXd k = CalcSmallK(x, m_X, m_kernel_hyperparams, m_kernel);

        // This code assumes that the kernel is either ARD squared exponential or ARD Matern and the first
        // hyperparameter represents the intensity of the kernel.
        assert(m_kernel_hyperparams.size() == x.size() + 1);
        const double intensity = m_kernel_hyperparams[0];

        return std::sqrt(intensity - k.transpose() * m_K_y_inv * k);
    }

    Eigen::VectorXd GaussianProcessRegressor::PredictMuDerivative(const Eigen::VectorXd& x) const
    {
        // TODO: Incorporate a mean function
        const MatrixXd k_x_derivative =
            CalcSmallKSmallXDerivative(x, m_X, m_kernel_hyperparams, m_kernel_first_arg_derivative);
        return k_x_derivative * m_K_y_inv * m_y;
    }

    Eigen::VectorXd GaussianProcessRegressor::PredictSigmaDerivative(const Eigen::VectorXd& x) const
    {
        const MatrixXd k_x_derivative =
            CalcSmallKSmallXDerivative(x, m_X, m_kernel_hyperparams, m_kernel_first_arg_derivative);
        const VectorXd k     = CalcSmallK(x, m_X, m_kernel_hyperparams, m_kernel);
        const double   sigma = PredictSigma(x);
        return -(1.0 / sigma) * k_x_derivative * m_K_y_inv * k;
    }

    void GaussianProcessRegressor::PerformMapEstimation()
    {
        const unsigned D = m_X.rows();

        Data data{m_X, m_y, m_kernel, m_kernel_theta_derivative};

        const VectorXd x_ini = [&]()
        {
            VectorXd x(D + 2);

            x(0)            = std::exp(a_prior_mu);
            x(1)            = std::exp(b_prior_mu);
            x.segment(2, D) = VectorXd::Constant(D, std::exp(r_prior_mu));

            return x;
        }();

        const VectorXd upper = VectorXd::Constant(D + 2, 5e+01);
        const VectorXd lower = VectorXd::Constant(D + 2, 1e-08);

        const VectorXd x_glo = nloptutil::solve(x_ini, upper, lower, objective, nlopt::GN_DIRECT, &data, true, 300);
        const VectorXd x_loc = nloptutil::solve(x_glo, upper, lower, objective, nlopt::LD_TNEWTON, &data, true, 1000);

        m_kernel_hyperparams = Concat(x_loc(0), x_loc.segment(2, D));
        m_noise_hyperparam   = x_loc(1);
    }
} // namespace sequential_line_search


#pragma once
#include "math.h"
#include "math.h"
#include <tuple>
#include "integration.hpp"
//#include "datatypes.hpp"
#include <boost/python.hpp>
#include <boost/math/special_functions.hpp>
#include "gsl/gsl_sf_bessel.h"

namespace gd {
using namespace std;

void py_export_profile();

class Density3d {
public:
	virtual double density(double x, double y, double z) = 0;
};

class Profile3d : public Density3d {
public:
	virtual double density(double x, double y, double z) = 0;
	virtual double potential(double x, double y, double z) = 0;
	virtual double dphidx(double x, double y, double z) = 0;
	virtual double dphidy(double x, double y, double z) = 0;
	virtual double dphidz(double x, double y, double z) = 0;

	/*double enclosed_mass(double r) {
		auto dmass = [this](double rp){ return this->densityr(rp) * rp*rp * 4 * M_PI; };
		IntegratorGSL<> integratorGSL(dmass); // the integrator
		double integral = integratorGSL.integrate(0,r);
		return integral;
	}
	double total_mass() {
		auto dmass = [this](double rp){ return this->densityr(rp) * rp*rp * 4 * M_PI; };
		IntegratorGSL<> integratorGSL(dmass); // the integrator
		double integral = integratorGSL.integrate_to_inf(0);
		return integral;
	}*/
};

class ProfileModel3d {
public:
	virtual double density(double x, double y, double z) = 0;
	virtual double potential(double x, double y, double z) = 0;
	virtual double dphidx(double x, double y, double z) = 0;
	virtual double dphidy(double x, double y, double z) = 0;
	virtual double dphidz(double x, double y, double z) = 0;
};

class ProfileModel3d1C : public ProfileModel3d {
public:
	ProfileModel3d1C(Profile3d* p) : p(p) {}
	virtual double density(double x, double y, double z) { return p->density(x, y, z); }
	virtual double potential(double x, double y, double z) { return p->potential(x, y, z); }
	virtual double dphidx(double x, double y, double z) { return p->dphidx(x, y, z); }
	virtual double dphidy(double x, double y, double z) { return p->dphidy(x, y, z); }
	virtual double dphidz(double x, double y, double z) { return p->dphidz(x, y, z); }
	Profile3d* p;
};

class ProfileModel3d2C : public ProfileModel3d {
public:
	ProfileModel3d2C(Profile3d* p1, Profile3d* p2) : p1(p1), p2(p2) {}
	virtual double density(double x, double y, double z) { return p1->density(x, y, z) +  p2->density(x, y, z); }
	virtual double potential(double x, double y, double z) { return p1->potential(x, y, z) + p2->potential(x, y, z); }
	virtual double dphidx(double x, double y, double z) { return p1->dphidx(x, y, z) + p2->dphidx(x, y, z); }
	virtual double dphidy(double x, double y, double z) { return p1->dphidy(x, y, z) + p2->dphidy(x, y, z); }
	virtual double dphidz(double x, double y, double z) { return p1->dphidz(x, y, z) + p2->dphidz(x, y, z); }
	Profile3d* p1;
	Profile3d* p2;
};

class NFW3d : public Profile3d {
public:
	NFW3d(double mass200, double rs, double G, double rho_crit) : mass200(mass200), rs(rs), G(G) {
		r200 = pow(mass200/200*3/(4*M_PI)/rho_crit, 1./3);
		c = r200/rs;
		double x = r200 / rs;
		rho0 = mass200 / (4*M_PI*pow(rs,3)*(log(1+x)-x/(1+x)));
	}
	virtual double density(double x, double y, double z) {
		return this->densityr(sqrt(x*x + y*y + z*z));
	}
	virtual double potential(double x, double y, double z) {
		return this->potentialr(sqrt(x*x + y*y + z*z));
	}
	virtual double dphidx(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double dphidr = this->dphidr(r);
		return -dphidr*x/r;
	}
	virtual double dphidy(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double dphidr = this->dphidr(r);
		return -dphidr*y/r;
	}
	virtual double dphidz(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double dphidr = this->dphidr(r);
		return -dphidr*z/r;
	}
	double potentialr(double r) {
		double x = r/rs;
		return - 4 * M_PI * G * rho0 * rs*rs * log(1+x)/x;
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (x * pow(1+x, 2));
	}
	double dphidr(double r) {
		double x = r/rs;
		double f = -4.0*M_PI*G*rho0*pow(rs, 3)/pow(r,2);
		double t1 = log(1.0 + x);
		double t2 = x/(1.0+x);
		//cout << x << ", " << f << ", " << t1 << ", " << t2 << endl;
		return -f*(t1 - t2);
	}
	double mass200, rho0, rs, r200, c, G;
};

class NFW3dTriax : public Profile3d {
public:
	NFW3dTriax(double mass200, double rs, double G, double rho_crit, double a, double b, double c, double ra) : mass200(mass200), rs(rs), G(G), a(a), b(b), c(c), ra(ra)  {
		r200 = pow(mass200/200*3/(4*M_PI)/rho_crit, 1./3);
		concentration = r200/rs;
		double x = r200 / rs;
		rho0 = mass200 / (4*M_PI*pow(rs,3)*(log(1+x)-x/(1+x)));
	}
	virtual double density(double x, double y, double z) {
		return 0 ;//this->densityr(sqrt(x*x + y*y + z*z));
	}
	virtual double potential(double x, double y, double z) {
		return this->potentialr( this->rtilde(x, y, z));
	}
	double rtilde(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double re = sqrt(x*x / (a*a) + y*y/ (b*b) + z*z/(c*c) );
		return (ra+r)*re / (ra+re);
	}
	virtual double dphidx(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double re = sqrt(x*x / (a*a) + y*y/ (b*b) + z*z/(c*c) );
		double rtilde = this->rtilde(x, y, z);
		double dphidrtilde = this->dphidr(rtilde);
		double drtildedx = x * (ra * ra * r + ra * r*r + a*a*pow(re,3)  + ra*a*a*re*re) / (a*a*r*re*pow(ra+re, 2));
		return -dphidrtilde*drtildedx;
	}
	virtual double dphidy(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double re = sqrt(x*x / (a*a) + y*y/ (b*b) + z*z/(c*c) );
		double rtilde = this->rtilde(x, y, z);
		double dphidrtilde = this->dphidr(rtilde);
		double drtildedy = y * (ra * ra * r + ra * r*r + b*b*pow(re,3)  + ra*b*b*re*re) / (b*b*r*re*pow(ra+re, 2));
		return -dphidrtilde*drtildedy;
	}
	virtual double dphidz(double x, double y, double z) {
		double r = sqrt(x*x + y*y + z*z);
		double re = sqrt(x*x / (a*a) + y*y/ (b*b) + z*z/(c*c) );
		double rtilde = this->rtilde(x, y, z);
		double dphidrtilde = this->dphidr(rtilde);
		double drtildedz = z * (ra * ra * r + ra * r*r + c*c*pow(re,3)  + ra*c*c*re*re) / (c*c*r*re*pow(ra+re, 2));
		return -dphidrtilde*drtildedz;
	}
	double potentialr(double r) {
		double x = r/rs;
		return - 4 * M_PI * G * rho0 * rs*rs * log(1+x)/x;
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (x * pow(1+x, 2));
	}
	double dphidr(double r) {
		double x = r/rs;
		double f = -4.0*M_PI*G*rho0*pow(rs, 3)/pow(r,2);
		double t1 = log(1.0 + x);
		double t2 = x/(1.0+x);
		//cout << x << ", " << f << ", " << t1 << ", " << t2 << endl;
		return -f*(t1 - t2);
	}
	double mass200, rho0, rs, r200, concentration, G, a, b, c, ra;
};

class  MiyamotoNagai3d : public Profile3d {
public:
	MiyamotoNagai3d(double mass, double G, double a, double b) : mass(mass), G(G), a(a), b(b)  {
	}
	virtual double density(double x, double y, double z) {
		return 0 ;//this->densityr(sqrt(x*x + y*y + z*z));
	}
	virtual double potential(double x, double y, double z) {
		return -G*mass*pow(x*x + y*y +pow(a + pow(z*z+b*b, .5), 2.), -.5);
		//return this->potentialr( this->rtilde(x, y, z));
	}
	virtual double dphidx(double x, double y, double z) {
		double part = G*mass*pow( x*x + y*y + pow(a+ pow(z*z+b*b,.5) , 2.), -1.5);
		return -x*part;
	}
	virtual double dphidy(double x, double y, double z) {
		double part = G*mass*pow( x*x + y*y + pow(a+ pow(z*z+b*b,.5) , 2.), -1.5);
		return -y*part;
	}
	virtual double dphidz(double x, double y, double z) {
		double part = G*mass*pow( x*x + y*y + pow(a+ pow(z*z+b*b,.5) , 2.), -1.5);
		return -z*part*(a/sqrt(b*b+z*z) + 1);
	}
	double mass, G, a, b;
};

}


#include "assembly.h"
#include <armadillo>
#include <iostream>

// Uncomment out the following line to disable assertions
//#define NDEBUG 1
#include <cassert>

using namespace arma;
using namespace std;

/*TODO work with linked lists instead of C-style arrays
 *       this will be easier for memory management */

/**
 * Assembles the global stiffness matrix given an array of elements, mutator
 *
 * @param [mat] kg Global stiffness matrix (by reference)
 * @param [MechElem*] elems Array of pointers to elements to assemble
 * @param [unsigned int] numElements Number of elements in array
 * @param [unsigned int] dofPerElem Total degrees of freedom per element
 * @return void
 */
void mglobalStiffness(mat &kg, MechElem **elems, unsigned int numElements,
        unsigned int dofPerElem, int pState)
{
    // TODO Can we find a way to relate this to global dof??
    //assert(kg.n_cols == dofPerElem);
    //assert(kg.n_rows == dofPerElem);
    /*
     * TODO reach a consistency where matrices passed by reference are either
     *      assumed to be zeroed before being passed or are zeroed on pass
     */
    kg.zeros();
    int *gdofs;
    unsigned int elemNum, i, j;
    MechElem *pElem;
    mat *ke = new mat(dofPerElem, dofPerElem);
    for (elemNum = 0; elemNum < numElements; elemNum++)
    {
        pElem = elems[elemNum];
        gdofs = pElem->getGdofs();
        pElem->mstiffness(*ke, pState);
        for (i = 0; i < dofPerElem; i++)
        {
            for (j = 0; j < dofPerElem; j++)
            {
                // TODO consider renumbering DOF so that we no longer require
                // this silly minus one business
                kg(gdofs[i]-1, gdofs[j]-1) += (*ke)(i, j);
            }
        }
    }
    // free dynamic memory
    delete ke;
}

/**
 * Assembles the global body force vector given an array of elements, mutator
 *
 * @param [mat] bg Global body force vector (by reference)
 * @param [MechElem*] elems Array of pointers to elements to assemble
 * @param [unsigned int] numElements Number of elements in array
 * @param [unsigned int] dofPerElem Total degrees of freedom per element
 * @return void
 */
void mglobalBodyForce(vec &bg, MechElem **elems, unsigned int numElements,
        unsigned int dofPerElem)
{
    // TODO Can we write this for global degrees of freedom?
    //assert(bg.n_cols == dofPerElem);

    bg.zeros();
    int *gdofs;
    unsigned int elemNum, i;
    MechElem *pElem;
    vec *be = new vec(dofPerElem);
    for (elemNum = 0; elemNum < numElements; elemNum++)
    {
        pElem = elems[elemNum];
        gdofs = pElem->getGdofs();
        pElem->mbodyForce(*be);
        for (i = 0; i < dofPerElem; i++)
            bg(gdofs[i]-1) += (*be)(i);
    }
    // free dynamic memory
    delete be;
}


#include "cpu/image_proc.h"

#include <Eigen/Dense>
#include <map>
#include <string>
#include <cmath>
#include <math.h>

#include "procrustes.h"

namespace image_proc {

    using namespace flow_data_gen;

    using Vec2d = Eigen::Vector2d;
    using Vec2f = Eigen::Vector2f;
    using Vec2i = Eigen::Vector2i;
    using Vec3d = Eigen::Vector3d;
    using Vec3f = Eigen::Vector3f;
    using Vec3i = Eigen::Vector3i;
	using Vec4d = Eigen::Vector4d;
	using Vec4f = Eigen::Vector4f;
	using Vec4i = Eigen::Vector4i;

	using Mat2d = Eigen::Matrix2d;
	using Mat2f = Eigen::Matrix2f;
	using Mat2i = Eigen::Matrix2i;
	using Mat3d = Eigen::Matrix3d;
	using Mat3f = Eigen::Matrix3f;
	using Mat3i = Eigen::Matrix3i;
	using Mat4d = Eigen::Matrix4d;
	using Mat4f = Eigen::Matrix4f;
	using Mat4i = Eigen::Matrix4i;

    void backproject_depth_ushort(py::array_t<unsigned short>& in, py::array_t<float>& out, float fx, float fy, float cx, float cy, float normalizer) {
        assert(in.ndim() == 2);
        assert(out.ndim() == 3);

        int width = in.shape(1);
        int height = in.shape(0);
        assert(out.shape(0) == 3);
        assert(out.shape(1) == height);
        assert(out.shape(2) == width);

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {
                float depth = float(*in.data(y, x)) / normalizer;

                if (depth > 0) {
                    float pos_x = depth * (x - cx) / fx;
                    float pos_y = depth * (y - cy) / fy;
                    float pos_z = depth;

                    *out.mutable_data(0, y, x) = pos_x;
                    *out.mutable_data(1, y, x) = pos_y;
                    *out.mutable_data(2, y, x) = pos_z;
                }
            }
        }
    }

    void backproject_depth_float(py::array_t<float>& in, py::array_t<float>& out, float fx, float fy, float cx, float cy) {
        assert(in.ndim() == 2);
        assert(out.ndim() == 3);

        int width = in.shape(1);
        int height = in.shape(0);
        assert(out.shape(0) == 3);
        assert(out.shape(1) == height);
        assert(out.shape(2) == width);

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {
                float depth = *in.data(y, x);

                if (depth > 0) {
                    float pos_x = depth * (x - cx) / fx;
                    float pos_y = depth * (y - cy) / fy;
                    float pos_z = depth;

                    *out.mutable_data(0, y, x) = pos_x;
                    *out.mutable_data(1, y, x) = pos_y;
                    *out.mutable_data(2, y, x) = pos_z;
                }
            }
        }
    }

    void compute_normals(const py::array_t<float>& point_image, py::array_t<float>& normals, float epsilon) {

        int height = point_image.shape(0);
        int width  = point_image.shape(1);

        for (int y = 1; y < (height - 1); y++) {
            for (int x = 1; x < (width - 1); x++) {

                Vec3f xp(*point_image.data(y, x + 1, 0), *point_image.data(y, x + 1, 1), *point_image.data(y, x + 1, 2));
                Vec3f xn(*point_image.data(y, x - 1, 0), *point_image.data(y, x - 1, 1), *point_image.data(y, x - 1, 2));

                Vec3f yp(*point_image.data(y + 1, x, 0), *point_image.data(y + 1, x, 1), *point_image.data(y + 1, x, 2));
                Vec3f yn(*point_image.data(y - 1, x, 0), *point_image.data(y - 1, x, 1), *point_image.data(y - 1, x, 2));

                if (!xp.allFinite() || !xn.allFinite() || !yp.allFinite() || !yn.allFinite()) {
                    continue;
                }

                Vec3f a = xp - xn;
                Vec3f b = yp - yn;

                Vec3f cross = b.cross(a);

                if (!cross.allFinite()) {
                    continue;
                }

                float norm = cross.norm();

                if (norm < epsilon) {
                    continue;
                }

                cross = cross / norm;

                *normals.mutable_data(y, x, 0) = cross.x();
                *normals.mutable_data(y, x, 1) = cross.y();
                *normals.mutable_data(y, x, 2) = cross.z();
            }
        }
    }

    void compute_normals_via_pca(const py::array_t<float>& point_image, py::array_t<float>& normals, const int kernel_size, const float max_distance) {

        int height = point_image.shape(0);
        int width  = point_image.shape(1);

        int ex = kernel_size;
        int ey = kernel_size;

        for (int y = 0; y < height; y++) {
            for (int x = 0; x < width; x++) {

                Vec3f point(*point_image.data(y, x, 0), *point_image.data(y, x, 1), *point_image.data(y, x, 2));

                if (!point.allFinite()) {
                    continue;
                }

                int counter = 0;
                Vec3f center(0, 0, 0);
                std::vector<Vec3f> valid_points;

                for (int yi = y - ey / 2; yi <= y + ey / 2; ++yi) {
                    for (int xi = x - ex / 2; xi <= x + ex / 2; ++xi) {
                        if (xi >= 0 && xi < width && yi >= 0 && yi < height) {
                            Vec3f point_neighbor(*point_image.data(yi, xi, 0), *point_image.data(yi, xi, 1), *point_image.data(yi, xi, 2));

                            if (point_neighbor.allFinite() && (point - point_neighbor).norm() <= max_distance) {
                                center += point_neighbor;
                                valid_points.push_back(point_neighbor);
                                ++counter;
                            }
                        }
                    }
                }
                center /= (float)counter;

                Eigen::MatrixXf A(3, counter);
                for (int idx = 0; idx < counter; ++idx) {
                    A.col(idx) = valid_points[idx] - center;
                }

                Eigen::Matrix3f cov = A * A.transpose();
                Eigen::EigenSolver<Eigen::Matrix3f> eigen_solver(cov, true);
                const Vec3f& eigen_value = eigen_solver.eigenvalues().real();

                int min_idx = 0;
                eigen_value.minCoeff(&min_idx);
                Vec3f min_eigen_vector = eigen_solver.eigenvectors().real().col(min_idx);

                Vec3f normal;
                // Use eigen vector facing camera as normal.
                if (min_eigen_vector.dot(point) < 0) {
                    normal = min_eigen_vector.normalized();
                }
                else {
                    normal = -min_eigen_vector.normalized();
                }

                *normals.mutable_data(y, x, 0) = normal.x();
                *normals.mutable_data(y, x, 1) = normal.y();
                *normals.mutable_data(y, x, 2) = normal.z();
            }
        }
    }

    float estimate_rigid_pose(
         const py::array_t<float>& sourcePointsArray, const py::array_t<float>& targetPointsArray,
         int numIterationsRANSAC, int numSamplesRANSAC, int minPointsRANSAC, float inlierDistanceRANSAC, float inlierDistanceFinal,
         py::array_t<float>& estimatedPose
    ) {
        if (numSamplesRANSAC > minPointsRANSAC) {
            std::cout << "The minimum number of points should be at least as big as the sample size." << std::endl;
            exit(1);
        }

        // Compute valid 3D matches.
        int nMatches = sourcePointsArray.shape(0);

        std::vector<Vec3f> sourcePoints, targetPoints;
        for (int i = 0; i < nMatches; i++) {
            Vec3f sourcePoint(
                *sourcePointsArray.data(i, 0),
                *sourcePointsArray.data(i, 1),
                *sourcePointsArray.data(i, 2)
            );
            Vec3f targetPoint(
                *targetPointsArray.data(i, 0),
                *targetPointsArray.data(i, 1),
                *targetPointsArray.data(i, 2)
            );

            sourcePoints.push_back(sourcePoint);
            targetPoints.push_back(targetPoint);
        }

        int nMatches3D = sourcePoints.size();
        if (nMatches3D < minPointsRANSAC) {
            return 0;
        }

        // Execute Procrustes with RANSAC.
        Mat4f globalRigidPose = ProcrustesRANSAC::estimatePose(
            sourcePoints, targetPoints, numIterationsRANSAC, numSamplesRANSAC, inlierDistanceRANSAC
        );

        // Compute the number of inliers.
        int nInliers = 0;
        for (int i = 0; i < nMatches3D; i++) {
            Eigen::Vector3f transformedSourcePoint = globalRigidPose.block(0, 0, 3, 3) * sourcePoints[i] + globalRigidPose.block(0, 3, 3, 1);
            float distance = (transformedSourcePoint - targetPoints[i]).norm();

            if (distance <= inlierDistanceFinal) {
                nInliers++;
            }
        }

        float inlierRatio = float(nInliers) / nMatches3D;

        // Store the estimated pose.
        estimatedPose.resize({ 4, 4 }, false);

        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                *estimatedPose.mutable_data(i, j) = globalRigidPose(i, j);
            }
        }

        return inlierRatio;
    }

    void compute_mesh_from_depth(
        const py::array_t<float>& pointImage, float maxTriangleEdgeDistance,
        py::array_t<float>& vertexPositions, py::array_t<int>& faceIndices
    ) {
        int width = pointImage.shape(2);
        int height = pointImage.shape(1);

        // Compute valid pixel vertices and faces.
        // We also need to compute the pixel -> vertex index mapping for
        // computation of faces.
        // We connect neighboring pixels on the square into two triangles.
        // We only select valid triangles, i.e. with all valid vertices and
        // not too far apart.
        // Important: The triangle orientation is set such that the normals
        // point towards the camera.
        std::vector<Eigen::Vector3f> vertices;
        std::vector<Eigen::Vector3i> faces;

        int vertexIdx = 0;
        std::vector<int> mapPixelToVertexIdx(width * height, -1);

        for (int y = 0; y < height - 1; y++) {
            for (int x = 0; x < width - 1; x++) {
                Eigen::Vector3f obs00(*pointImage.data(0, y, x), *pointImage.data(1, y, x), *pointImage.data(2, y, x));
                Eigen::Vector3f obs01(*pointImage.data(0, y + 1, x), *pointImage.data(1, y + 1, x), *pointImage.data(2, y + 1, x));
                Eigen::Vector3f obs10(*pointImage.data(0, y, x + 1), *pointImage.data(1, y, x + 1), *pointImage.data(2, y, x + 1));
                Eigen::Vector3f obs11(*pointImage.data(0, y + 1, x + 1), *pointImage.data(1, y + 1, x + 1), *pointImage.data(2, y + 1, x + 1));

                int idx00 = y * width + x;
                int idx01 = (y + 1) * width + x;
                int idx10 = y * width + (x + 1);
                int idx11 = (y + 1) * width + (x + 1);

                bool valid00 = obs00.z() > 0;
                bool valid01 = obs01.z() > 0;
                bool valid10 = obs10.z() > 0;
                bool valid11 = obs11.z() > 0;

                if (valid00 && valid01 && valid10) {
                    float d0 = (obs00 - obs01).norm();
                    float d1 = (obs00 - obs10).norm();
                    float d2 = (obs01 - obs10).norm();

                    if (d0 <= maxTriangleEdgeDistance && d1 <= maxTriangleEdgeDistance && d2 <= maxTriangleEdgeDistance) {
                        int vIdx0 = mapPixelToVertexIdx[idx00];
                        int vIdx1 = mapPixelToVertexIdx[idx01];
                        int vIdx2 = mapPixelToVertexIdx[idx10];

                        if (vIdx0 == -1) {
                            vIdx0 = vertexIdx;
                            mapPixelToVertexIdx[idx00] = vertexIdx;
                            vertices.push_back(obs00);
                            vertexIdx++;
                        }
                        if (vIdx1 == -1) {
                            vIdx1 = vertexIdx;
                            mapPixelToVertexIdx[idx01] = vertexIdx;
                            vertices.push_back(obs01);
                            vertexIdx++;
                        }
                        if (vIdx2 == -1) {
                            vIdx2 = vertexIdx;
                            mapPixelToVertexIdx[idx10] = vertexIdx;
                            vertices.push_back(obs10);
                            vertexIdx++;
                        }

                        faces.push_back(Eigen::Vector3i(vIdx0, vIdx1, vIdx2));
                    }
                }

                if (valid01 && valid10 && valid11) {
                    float d0 = (obs10 - obs01).norm();
                    float d1 = (obs10 - obs11).norm();
                    float d2 = (obs01 - obs11).norm();

                    if (d0 <= maxTriangleEdgeDistance && d1 <= maxTriangleEdgeDistance && d2 <= maxTriangleEdgeDistance) {
                        int vIdx0 = mapPixelToVertexIdx[idx11];
                        int vIdx1 = mapPixelToVertexIdx[idx10];
                        int vIdx2 = mapPixelToVertexIdx[idx01];

                        if (vIdx0 == -1) {
                            vIdx0 = vertexIdx;
                            mapPixelToVertexIdx[idx11] = vertexIdx;
                            vertices.push_back(obs11);
                            vertexIdx++;
                        }
                        if (vIdx1 == -1) {
                            vIdx1 = vertexIdx;
                            mapPixelToVertexIdx[idx10] = vertexIdx;
                            vertices.push_back(obs10);
                            vertexIdx++;
                        }
                        if (vIdx2 == -1) {
                            vIdx2 = vertexIdx;
                            mapPixelToVertexIdx[idx01] = vertexIdx;
                            vertices.push_back(obs01);
                            vertexIdx++;
                        }

                        faces.push_back(Eigen::Vector3i(vIdx0, vIdx1, vIdx2));
                    }
                }
            }
        }

        // Convert to numpy array.
        int nVertices = vertices.size();
        int nFaces = faces.size();

        if (nVertices > 0 && nFaces > 0) {
            // Reference check should be set to false otherwise there is a runtime
            // error. Check why that is the case.
            vertexPositions.resize({ nVertices, 3 }, false);
            faceIndices.resize({ nFaces, 3 }, false);

            for (int i = 0; i < nVertices; i++) {
                *vertexPositions.mutable_data(i, 0) = vertices[i].x();
                *vertexPositions.mutable_data(i, 1) = vertices[i].y();
                *vertexPositions.mutable_data(i, 2) = vertices[i].z();
            }

            for (int i = 0; i < nFaces; i++) {
                *faceIndices.mutable_data(i, 0) = faces[i].x();
                *faceIndices.mutable_data(i, 1) = faces[i].y();
                *faceIndices.mutable_data(i, 2) = faces[i].z();
            }
        }
    }

    void compute_validity_and_sign_mask(
        const py::array_t<float>& grid_points_cam,
        const py::array_t<bool>& valid_depth_mask_image,
        const py::array_t<float>& depth_image,
        float fx, float fy, float cx, float cy, int w, int h,
        float truncation,
        py::array_t<bool>& validity_grid_mask,
        py::array_t<float>& sign_grid_mask
    ) {

        int num_grid_points = grid_points_cam.shape(0);

        for (int pt_id = 0; pt_id < num_grid_points; pt_id++) {

            float x = *grid_points_cam.data(pt_id, 0);
            float y = *grid_points_cam.data(pt_id, 1);
            float z = *grid_points_cam.data(pt_id, 2);

            int px = int(round(fx * x / z + cx));
            int py = int(round(fy * y / z + cy));

            if (px < 0 || px >= w || py < 0 || py >= h) {
                continue;
            }

            bool is_valid_pixel = *valid_depth_mask_image.data(py, px);

            if (!is_valid_pixel) {
                continue;
            }

            float d = *depth_image.data(py, px);

            // If the z coordinate of the grid point is further away from the camera
            // than the surface + some truncation, then we invalidate the grid point
            if (z > (d + truncation)) {
                *validity_grid_mask.mutable_data(pt_id) = false;
            }

            if (z > d) {
                *sign_grid_mask.mutable_data(pt_id) = -1.0;
            }

        }
    }

} //namespace image_proc