/* Author: Wolfgang Bangerth, University of Heidelberg, 2000 */

/*    $Id: step-6.cc 27877 2012-12-31 00:55:14Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2000-2004, 2006-2008, 2010-2012 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}

// The first few files have already been covered in previous examples and will
// thus not be further commented on.
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/compressed_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.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/grid/tria_boundary_lib.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.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>

// From the following include file we will import the declaration of
// H1-conforming finite element shape functions. This family of finite
// elements is called FE_Q, and was used in all examples before
// already to define the usual bi- or tri-linear elements, but we will now use
// it for bi-quadratic elements:
#include <deal.II/fe/fe_q.h>
// We will not read the grid from a file as in the previous example, but
// generate it using a function of the library. However, we will want to write
// out the locally refined grids (just the grid, not the solution) in each
// step, so we need the following include file instead of
// grid_in.h:
#include <deal.II/grid/grid_out.h>

// When using locally refined grids, we will get so-called hanging
// nodes. However, the standard finite element methods assumes that the
// discrete solution spaces be continuous, so we need to make sure that the
// degrees of freedom on hanging nodes conform to some constraints such that
// the global solution is continuous. We are also going to store the boundary
// conditions in this object. The following file contains a class which is
// used to handle these constraints:
#include <deal.II/lac/constraint_matrix.h>

// In order to refine our grids locally, we need a function from the library
// that decides which cells to flag for refinement or coarsening based on the
// error indicators we have computed. This function is defined here:
#include <deal.II/grid/grid_refinement.h>

// Finally, we need a simple way to actually compute the refinement indicators
// based on some error estimat. While in general, adaptivity is very
// problem-specific, the error indicator in the following file often yields
// quite nicely adapted grids for a wide class of problems.
#include <deal.II/numerics/error_estimator.h>

// Finally, this is as in previous programs:
using namespace dealii;

// @sect3{The Step6 class template}

// The main class is again almost unchanged. Two additions, however, are made:
// we have added the refine_grid function, which is used to
// adaptively refine the grid (instead of the global refinement in the
// previous examples), and a variable which will hold the constraints. In
// addition, we have added a destructor to the class for reasons that will
// become clear when we discuss its implementation.
template <int dim>
class Step6
{
public:
  Step6 ();
  ~Step6 ();

  void run ();

private:
  void setup_system ();
  void assemble_system ();
  void solve ();
  void refine_grid ();
  void output_results (const unsigned int cycle) const;

  Triangulation<dim>   triangulation;

  DoFHandler<dim>      dof_handler;
  FE_Q<dim>            fe;

  // This is the new variable in the main class. We need an object which holds
  // a list of constraints to hold the hanging nodes and the boundary
  // conditions.
  ConstraintMatrix     constraints;

  SparsityPattern      sparsity_pattern;
  SparseMatrix<double> system_matrix;

  Vector<double>       solution;
  Vector<double>       system_rhs;
};

// @sect3{Nonconstant coefficients}

// The implementation of nonconstant coefficients is copied verbatim from
// step-5:

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

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

  virtual void value_list (const std::vector<Point<dim> > &points,
                           std::vector<double>            &values,
                           const unsigned int              component = 0) const;
};

template <int dim>
double Coefficient<dim>::value (const Point<dim> &p,
                                const unsigned int) const
{
  if (p.square() < 0.5*0.5)
    return 20;
  else
    return 1;
}

template <int dim>
void Coefficient<dim>::value_list (const std::vector<Point<dim> > &points,
                                   std::vector<double>            &values,
                                   const unsigned int              component) const
{
  const unsigned int n_points = points.size();

  Assert (values.size() == n_points,
          ExcDimensionMismatch (values.size(), n_points));

  Assert (component == 0,
          ExcIndexRange (component, 0, 1));

  for (unsigned int i=0; i<n_points; ++i)
    {
      if (points[i].square() < 0.5*0.5)
        values[i] = 20;
      else
        values[i] = 1;
    }
}

// @sect3{The Step6 class implementation}

// @sect4{Step6::Step6}

// The constructor of this class is mostly the same as before, but this time
// we want to use the quadratic element. To do so, we only have to replace the
// constructor argument (which was 1 in all previous examples) by
// the desired polynomial degree (here 2):
template <int dim>
Step6<dim>::Step6 ()
  :
  dof_handler (triangulation),
  fe (2)
{}

// @sect4{Step6::~Step6}

// Here comes the added destructor of the class. The reason why we want to add
// it is a subtle change in the order of data elements in the class as
// compared to all previous examples: the dof_handler object was
// defined before and not after the fe object. Of course we could
// have left this order unchanged, but we would like to show what happens if
// the order is reversed since this produces a rather nasty side-effect and
// results in an error which is difficult to track down if one does not know
// what happens.
//
// Basically what happens is the following: when we distribute the degrees of
// freedom using the function call dof_handler.distribute_dofs(),
// the dof_handler also stores a pointer to the finite element in
// use. Since this pointer is used every now and then until either the degrees
// of freedom are re-distributed using another finite element object or until
// the dof_handler object is destroyed, it would be unwise if we
// would allow the finite element object to be deleted before the
// dof_handler object. To disallow this, the DoF handler
// increases a counter inside the finite element object which counts how many
// objects use that finite element (this is what the
// Subscriptor/SmartPointer class pair is used for,
// in case you want something like this for your own programs; see step-7 for
// a more complete discussion of this topic). The finite element object will
// refuse its destruction if that counter is larger than zero, since then some
// other objects might rely on the persistence of the finite element
// object. An exception will then be thrown and the program will usually abort
// upon the attempt to destroy the finite element.
//
// To be fair, such exceptions about still used objects are not particularly
// popular among programmers using deal.II, since they only tell us that
// something is wrong, namely that some other object is still using the object
// that is presently being destructed, but most of the time not who this user
// is. It is therefore often rather time-consuming to find out where the
// problem exactly is, although it is then usually straightforward to remedy
// the situation. However, we believe that the effort to find invalid
// references to objects that do no longer exist is less if the problem is
// detected once the reference becomes invalid, rather than when non-existent
// objects are actually accessed again, since then usually only invalid data
// is accessed, but no error is immediately raised.
//
// Coming back to the present situation, if we did not write this destructor,
// the compiler will generate code that triggers exactly the behavior sketched
// above. The reason is that member variables of the Step6 class
// are destructed bottom-up (i.e. in reverse order of their declaration in the
// class), as always in C++. Thus, the finite element object will be
// destructed before the DoF handler object, since its declaration is below
// the one of the DoF handler. This triggers the situation above, and an
// exception will be raised when the fe object is
// destructed. What needs to be done is to tell the dof_handler
// object to release its lock to the finite element. Of course, the
// dof_handler will only release its lock if it really does not
// need the finite element any more, i.e. when all finite element related data
// is deleted from it. For this purpose, the DoFHandler class has
// a function clear which deletes all degrees of freedom, and
// releases its lock to the finite element. After this, you can safely
// destruct the finite element object since its internal counter is then zero.
//
// For completeness, we add the output of the exception that would have been
// triggered without this destructor, to the end of the results section of
// this example.
template <int dim>
Step6<dim>::~Step6 ()
{
  dof_handler.clear ();
}

// @sect4{Step6::setup_system}

// The next function is setting up all the variables that describe the linear
// finite element problem, such as the DoF handler, the matrices, and
// vectors. The difference to what we did in step-5 is only that we now also
// have to take care of handing node constraints. These constraints are
// handled almost transparently by the library, i.e. you only need to know
// that they exist and how to get them, but you do not have to know how they
// are formed or what exactly is done with them.
//
// At the beginning of the function, you find all the things that are the same
// as in step-5: setting up the degrees of freedom (this time we have
// quadratic elements, but there is no difference from a user code perspective
// to the linear -- or cubic, for that matter -- case), generating the
// sparsity pattern, and initializing the solution and right hand side
// vectors. Note that the sparsity pattern will have significantly more
// entries per row now, since there are now 9 degrees of freedom per cell, not
// only four, that can couple with each other. The
// dof_Handler.max_couplings_between_dofs() call will take care
// of this, however:
template <int dim>
void Step6<dim>::setup_system ()
{
  dof_handler.distribute_dofs (fe);

  solution.reinit (dof_handler.n_dofs());
  system_rhs.reinit (dof_handler.n_dofs());

  // After setting up all the degrees of freedoms, here are now the
  // differences compared to step-5, all of which are related to constraints
  // associated with the hanging nodes. In the class desclaration, we have
  // already allocated space for an object constraints that will
  // hold a list of these constraints (they form a matrix, which is reflected
  // in the name of the class, but that is immaterial for the moment). Now we
  // have to fill this object. This is done using the following function calls
  // (the first clears the contents of the object that may still be left over
  // from computations on the previous mesh before the last adaptive
  // refinement):
  constraints.clear ();
  DoFTools::make_hanging_node_constraints (dof_handler,
                                           constraints);

  // Now we are ready to interpolate the ZeroFunction to our boundary with
  // indicator 0 (the whole boundary) and store the resulting constraints in
  // our constraints object. Note that we do not to apply the
  // boundary conditions after assembly, like we did in earlier steps.  As
  // almost all the stuff, the interpolation of boundary values works also for
  // higher order elements without the need to change your code for that. We
  // note that for proper results, it is important that the elimination of
  // boundary nodes from the system of equations happens *after* the
  // elimination of hanging nodes. For that reason we are filling the boundary
  // values into the ContraintMatrix after the hanging node constraints.
  VectorTools::interpolate_boundary_values (dof_handler,
                                            0,
                                            ZeroFunction<dim>(),
                                            constraints);

  // The next step is closing this object. After all constraints
  // have been added, they need to be sorted and rearranged to perform some
  // actions more efficiently. This postprocessing is done using the
  // close() function, after which no further constraints may be
  // added any more:
  constraints.close ();

  // Now we first build our compressed sparsity pattern like we did in the
  // previous examples. Nevertheless, we do not copy it to the final sparsity
  // pattern immediately.  Note that we call a variant of
  // make_sparsity_pattern that takes the ConstraintMatrix as the third
  // argument. We are letting the routine know that we will never write into
  // the locations given by constraints by setting the argument
  // keep_constrained_dofs to false (in other words, that we will
  // never write into entries of the matrix that correspond to constrained
  // degrees of freedom). If we were to condense the
  // constraints after assembling, we would have to pass true
  // instead because then we would first write into these locations only to
  // later set them to zero again during condensation.
  CompressedSparsityPattern c_sparsity(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler,
                                  c_sparsity,
                                  constraints,
                                  /*keep_constrained_dofs = */ false);

  // Now all non-zero entries of the matrix are known (i.e. those from
  // regularly assembling the matrix and those that were introduced by
  // eliminating constraints). We can thus copy our intermediate object to the
  // sparsity pattern:
  sparsity_pattern.copy_from(c_sparsity);

  // Finally, the so-constructed sparsity pattern serves as the basis on top
  // of which we will create the sparse matrix:
  system_matrix.reinit (sparsity_pattern);
}

// @sect4{Step6::assemble_system}

// Next, we have to assemble the matrix again. There are two code changes
// compared to step-5:
//
// First, we have to use a higher-order quadrature formula to account for the
// higher polynomial degree in the finite element shape functions. This is
// easy to change: the constructor of the QGauss class takes the
// number of quadrature points in each space direction. Previously, we had two
// points for bilinear elements. Now we should use three points for
// biquadratic elements.
//
// Second, to copy the local matrix and vector on each cell into the global
// system, we are no longer using a hand-written loop. Instead, we use
// ConstraintMatrix::distribute_local_to_global that internally
// executes this loop and eliminates all the constraints at the same time.
//
// The rest of the code that forms the local contributions remains
// unchanged. It is worth noting, however, that under the hood several things
// are different than before. First, the variables dofs_per_cell
// and n_q_points now are 9 each, where they were 4
// before. Introducing such variables as abbreviations is a good strategy to
// make code work with different elements without having to change too much
// code. Secondly, the fe_values object of course needs to do
// other things as well, since the shape functions are now quadratic, rather
// than linear, in each coordinate variable. Again, however, this is something
// that is completely transparent to user code and nothing that you have to
// worry about.
template <int dim>
void Step6<dim>::assemble_system ()
{
  const QGauss<dim>  quadrature_formula(3);

  FEValues<dim> fe_values (fe, quadrature_formula,
                           update_values    |  update_gradients |
                           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();

  FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
  Vector<double>       cell_rhs (dofs_per_cell);

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

  const Coefficient<dim> coefficient;
  std::vector<double>    coefficient_values (n_q_points);

  typename DoFHandler<dim>::active_cell_iterator
  cell = dof_handler.begin_active(),
  endc = dof_handler.end();
  for (; cell!=endc; ++cell)
    {
      cell_matrix = 0;
      cell_rhs = 0;

      fe_values.reinit (cell);

      coefficient.value_list (fe_values.get_quadrature_points(),
                              coefficient_values);

      for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          {
            for (unsigned int j=0; j<dofs_per_cell; ++j)
              cell_matrix(i,j) += (coefficient_values[q_point] *
                                   fe_values.shape_grad(i,q_point) *
                                   fe_values.shape_grad(j,q_point) *
                                   fe_values.JxW(q_point));

            cell_rhs(i) += (fe_values.shape_value(i,q_point) *
                            1.0 *
                            fe_values.JxW(q_point));
          }

      // Finally, transfer the contributions from @p cell_matrix and
      // @p cell_rhs into the global objects.
      cell->get_dof_indices (local_dof_indices);
      constraints.distribute_local_to_global(cell_matrix,
                                             cell_rhs,
                                             local_dof_indices,
                                             system_matrix, system_rhs);
    }
  // Now we are done assembling the linear system.  The constrained nodes are
  // still in the linear system (there is a one on the diagonal of the matrix
  // and all other entries for this line are set to zero) but the computed
  // values are invalid. We compute the correct values for these nodes at the
  // end of the solve function.
}

// @sect4{Step6::solve}

// We continue with gradual improvements. The function that solves the linear
// system again uses the SSOR preconditioner, and is again unchanged except
// that we have to incorporate hanging node constraints. As mentioned above,
// the degrees of freedom from the ConstraintMatrix corresponding to hanging
// node constraints and boundary values have been removed from the linear
// system by giving the rows and columns of the matrix a special
// treatment. This way, the values for these degrees of freedom have wrong,
// but well-defined values after solving the linear system. What we then have
// to do is to use the constraints to assign to them the values that they
// should have. This process, called distributing constraints,
// computes the values of constrained nodes from the values of the
// unconstrained ones, and requires only a single additional function call
// that you find at the end of this function:

template <int dim>
void Step6<dim>::solve ()
{
  SolverControl           solver_control (1000, 1e-12);
  SolverCG<>              solver (solver_control);

  PreconditionSSOR<> preconditioner;
  preconditioner.initialize(system_matrix, 1.2);

  solver.solve (system_matrix, solution, system_rhs,
                preconditioner);

  constraints.distribute (solution);
}

// @sect4{Step6::refine_grid}

// Instead of global refinement, we now use a slightly more elaborate
// scheme. We will use the KellyErrorEstimator class which
// implements an error estimator for the Laplace equation; it can in principle
// handle variable coefficients, but we will not use these advanced features,
// but rather use its most simple form since we are not interested in
// quantitative results but only in a quick way to generate locally refined
// grids.
//
// Although the error estimator derived by Kelly et al. was originally
// developed for the Laplace equation, we have found that it is also well
// suited to quickly generate locally refined grids for a wide class of
// problems. Basically, it looks at the jumps of the gradients of the solution
// over the faces of cells (which is a measure for the second derivatives) and
// scales it by the size of the cell. It is therefore a measure for the local
// smoothness of the solution at the place of each cell and it is thus
// understandable that it yields reasonable grids also for hyperbolic
// transport problems or the wave equation as well, although these grids are
// certainly suboptimal compared to approaches specially tailored to the
// problem. This error estimator may therefore be understood as a quick way to
// test an adaptive program.
//
// The way the estimator works is to take a DoFHandler object
// describing the degrees of freedom and a vector of values for each degree of
// freedom as input and compute a single indicator value for each active cell
// of the triangulation (i.e. one value for each of the
// triangulation.n_active_cells() cells). To do so, it needs two
// additional pieces of information: a quadrature formula on the faces
// (i.e. quadrature formula on dim-1 dimensional objects. We use
// a 3-point Gauss rule again, a pick that is consistent and appropriate with
// the choice bi-quadratic finite element shape functions in this program.
// (What constitutes a suitable quadrature rule here of course depends on
// knowledge of the way the error estimator evaluates the solution field. As
// said above, the jump of the gradient is integrated over each face, which
// would be a quadratic function on each face for the quadratic elements in
// use in this example. In fact, however, it is the square of the jump of the
// gradient, as explained in the documentation of that class, and that is a
// quartic function, for which a 3 point Gauss formula is sufficient since it
// integrates polynomials up to order 5 exactly.)
//
// Secondly, the function wants a list of boundaries where we have imposed
// Neumann value, and the corresponding Neumann values. This information is
// represented by an object of type FunctionMap::type that is
// essentially a map from boundary indicators to function objects describing
// Neumann boundary values (in the present example program, we do not use
// Neumann boundary values, so this map is empty, and in fact constructed
// using the default constructor of the map in the place where the function
// call expects the respective function argument).
//
// The output, as mentioned is a vector of values for all cells. While it may
// make sense to compute the *value* of a degree of freedom very accurately,
// it is usually not helpful to compute the *error indicator* corresponding to
// a cell particularly accurately. We therefore typically use a vector of
// floats instead of a vector of doubles to represent error indicators.
template <int dim>
void Step6<dim>::refine_grid ()
{
  Vector<float> estimated_error_per_cell (triangulation.n_active_cells());

  KellyErrorEstimator<dim>::estimate (dof_handler,
                                      QGauss<dim-1>(3),
                                      typename FunctionMap<dim>::type(),
                                      solution,
                                      estimated_error_per_cell);

  // The above function returned one error indicator value for each cell in
  // the estimated_error_per_cell array. Refinement is now done
  // as follows: refine those 30 per cent of the cells with the highest error
  // values, and coarsen the 3 per cent of cells with the lowest values.
  //
  // One can easily verify that if the second number were zero, this would
  // approximately result in a doubling of cells in each step in two space
  // dimensions, since for each of the 30 per cent of cells, four new would be
  // replaced, while the remaining 70 per cent of cells remain untouched. In
  // practice, some more cells are usually produced since it is disallowed
  // that a cell is refined twice while the neighbor cell is not refined; in
  // that case, the neighbor cell would be refined as well.
  //
  // In many applications, the number of cells to be coarsened would be set to
  // something larger than only three per cent. A non-zero value is useful
  // especially if for some reason the initial (coarse) grid is already rather
  // refined. In that case, it might be necessary to refine it in some
  // regions, while coarsening in some other regions is useful. In our case
  // here, the initial grid is very coarse, so coarsening is only necessary in
  // a few regions where over-refinement may have taken place. Thus a small,
  // non-zero value is appropriate here.
  //
  // The following function now takes these refinement indicators and flags
  // some cells of the triangulation for refinement or coarsening using the
  // method described above. It is from a class that implements several
  // different algorithms to refine a triangulation based on cell-wise error
  // indicators.
  GridRefinement::refine_and_coarsen_fixed_number (triangulation,
                                                   estimated_error_per_cell,
                                                   0.3, 0.03);

  // After the previous function has exited, some cells are flagged for
  // refinement, and some other for coarsening. The refinement or coarsening
  // itself is not performed by now, however, since there are cases where
  // further modifications of these flags is useful. Here, we don't want to do
  // any such thing, so we can tell the triangulation to perform the actions
  // for which the cells are flagged:
  triangulation.execute_coarsening_and_refinement ();
}

// @sect4{Step6::output_results}

// At the end of computations on each grid, and just before we continue the
// next cycle with mesh refinement, we want to output the results from this
// cycle.
//
// In the present program, we will not write the solution (except for in the
// last step, see the next function), but only the meshes that we generated,
// as a two-dimensional Encapsulated Postscript (EPS) file.
//
// We have already seen in step-1 how this can be achieved. The only thing we
// have to change is the generation of the file name, since it should contain
// the number of the present refinement cycle provided to this function as an
// argument. The most general way is to use the std::stringstream class as
// shown in step-5, but here's a little hack that makes it simpler if we know
// that we have less than 10 iterations: assume that the %numbers `0' through
// `9' are represented consecutively in the character set used on your machine
// (this is in fact the case in all known character sets), then '0'+cycle
// gives the character corresponding to the present cycle number. Of course,
// this will only work if the number of cycles is actually less than 10, and
// rather than waiting for the disaster to happen, we safeguard our little
// hack with an explicit assertion at the beginning of the function. If this
// assertion is triggered, i.e. when cycle is larger than or
// equal to 10, an exception of type ExcNotImplemented is raised,
// indicating that some functionality is not implemented for this case (the
// functionality that is missing, of course, is the generation of file names
// for that case):
template <int dim>
void Step6<dim>::output_results (const unsigned int cycle) const
{
  Assert (cycle < 10, ExcNotImplemented());

  std::string filename = "grid-";
  filename += ('0' + cycle);
  filename += ".eps";

  std::ofstream output (filename.c_str());

  GridOut grid_out;
  grid_out.write_eps (triangulation, output);
}

// @sect4{Step6::run}

// The final function before main() is again the main driver of
// the class, run(). It is similar to the one of step-5, except
// that we generate a file in the program again instead of reading it from
// disk, in that we adaptively instead of globally refine the mesh, and that
// we output the solution on the final mesh in the present function.
//
// The first block in the main loop of the function deals with mesh
// generation. If this is the first cycle of the program, instead of reading
// the grid from a file on disk as in the previous example, we now again
// create it using a library function. The domain is again a circle, which is
// why we have to provide a suitable boundary object as well. We place the
// center of the circle at the origin and have the radius be one (these are
// the two hidden arguments to the function, which have default values).
//
// You will notice by looking at the coarse grid that it is of inferior
// quality than the one which we read from the file in the previous example:
// the cells are less equally formed. However, using the library function this
// program works in any space dimension, which was not the case before.
//
// In case we find that this is not the first cycle, we want to refine the
// grid. Unlike the global refinement employed in the last example program, we
// now use the adaptive procedure described above.
//
// The rest of the loop looks as before:
template <int dim>
void Step6<dim>::run ()
{
  for (unsigned int cycle=0; cycle<8; ++cycle)
    {
      std::cout << "Cycle " << cycle << ':' << std::endl;

      if (cycle == 0)
        {
          GridGenerator::hyper_ball (triangulation);

          static const HyperBallBoundary<dim> boundary;
          triangulation.set_boundary (0, boundary);

          triangulation.refine_global (1);
        }
      else
        refine_grid ();

      std::cout << "   Number of active cells:       "
                << triangulation.n_active_cells()
                << std::endl;

      setup_system ();

      std::cout << "   Number of degrees of freedom: "
                << dof_handler.n_dofs()
                << std::endl;

      assemble_system ();
      solve ();
      output_results (cycle);
    }

  // After we have finished computing the solution on the finest mesh, and
  // writing all the grids to disk, we want to also write the actual solution
  // on this final mesh to a file. As already done in one of the previous
  // examples, we use the EPS format for output, and to obtain a reasonable
  // view on the solution, we rescale the z-axis by a factor of four.
  DataOutBase::EpsFlags eps_flags;
  eps_flags.z_scaling = 4;

  DataOut<dim> data_out;
  data_out.set_flags (eps_flags);

  data_out.attach_dof_handler (dof_handler);
  data_out.add_data_vector (solution, "solution");
  data_out.build_patches ();

  std::ofstream output ("final-solution.eps");
  data_out.write_eps (output);
}

// @sect3{The main function}

// The main function is unaltered in its functionality from the previous
// example, but we have taken a step of additional caution. Sometimes,
// something goes wrong (such as insufficient disk space upon writing an
// output file, not enough memory when trying to allocate a vector or a
// matrix, or if we can't read from or write to a file for whatever reason),
// and in these cases the library will throw exceptions. Since these are
// run-time problems, not programming errors that can be fixed once and for
// all, this kind of exceptions is not switched off in optimized mode, in
// contrast to the Assert macro which we have used to test
// against programming errors. If uncaught, these exceptions propagate the
// call tree up to the main function, and if they are not caught
// there either, the program is aborted. In many cases, like if there is not
// enough memory or disk space, we can't do anything but we can at least print
// some text trying to explain the reason why the program failed. A way to do
// so is shown in the following. It is certainly useful to write any larger
// program in this way, and you can do so by more or less copying this
// function except for the try block that actually encodes the
// functionality particular to the present application.
int main ()
{

  // The general idea behind the layout of this function is as follows: let's
  // try to run the program as we did before...
  try
    {
      deallog.depth_console (0);

      Step6<2> laplace_problem_2d;
      laplace_problem_2d.run ();
    }
  // ...and if this should fail, try to gather as much information as
  // possible. Specifically, if the exception that was thrown is an object of
  // a class that is derived from the C++ standard class
  // exception, then we can use the what member
  // function to get a string which describes the reason why the exception was
  // thrown.
  //
  // The deal.II exception classes are all derived from the standard class,
  // and in particular, the exc.what() function will return
  // approximately the same string as would be generated if the exception was
  // thrown using the Assert macro. You have seen the output of
  // such an exception in the previous example, and you then know that it
  // contains the file and line number of where the exception occured, and
  // some other information. This is also what the following statements would
  // print.
  //
  // Apart from this, there isn't much that we can do except exiting the
  // program with an error code (this is what the return 1;
  // does):
  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;
    }
  // If the exception that was thrown somewhere was not an object of a class
  // derived from the standard exception class, then we can't do
  // anything at all. We then simply print an error message and exit.
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  // If we got to this point, there was no exception which propagated up to
  // the main function (there may have been exceptions, but they were caught
  // somewhere in the program or the library). Therefore, the program
  // performed as was expected and we can return without error.
  return 0;
}


#include "visualizer.h"

#include <iostream>
//#include <algorithm>
#include <string>
//#include <numeric>

#define CERES_FOUND true

#include <opencv2/opencv.hpp>
//#include <opencv2/sfm.hpp>
#include <utility>
#include <opencv2/viz.hpp>
#include <opencv2/core/utils/logger.hpp>
//#include <opencv2/core/utils/filesystem.hpp>
//#include <opencv2/xfeatures2d.hpp>

#include <boost/filesystem.hpp>
//#include <boost/graph/graph_traits.hpp>
//#include <boost/graph/adjacency_list.hpp>
//#include <boost/graph/connected_components.hpp>
//#include <boost/graph/graphviz.hpp>
#include <boost/tokenizer.hpp>

#include <unistd.h>

#define _USE_OPENCV true
//#include <MVS/Interface.h>

using namespace cv;
using namespace cv::utils::logging;
using namespace std;
using namespace boost;
namespace fs = boost::filesystem;

#define TYPE_CALIBRATION_MATRIX 0
#define TYPE_CAMERA 1
#define TYPE_POINT 2

class Visualizer {
private:
    const string dir;    // directory with file
    ifstream infile; // opened file
    string file_info;

    vector<Matx33d> Rs;
    vector<Vec3d> Ts;
    vector<int> camera_ids;

//    vector<Mat> points;
    vector<Vec3d> points;
    vector<Vec3b> point_colors;
    vector<int> point_ids;

    Matx33d K;
    viz::Viz3d window;

public:
    explicit Visualizer(string dir): dir(std::move(dir)) {

    }

    int run() {
        int ret;

        cout << "Loading file" << endl;
        ret = load_file();

        if (ret != 0) {
            cout << "[Error] Couldn't open file" << endl;
            return -1;
        }

        cout << "Parsing file" << endl;
        parse_file();

        cout << "Generating visualization" << endl;
        visualize3D();

        return 0;
    }

private:
    int load_file() {
//        infile = ifstream(dir);
//        string data(dir);

        cout << "Trying to open file " << dir << endl;
        infile = ifstream(dir);
        if (!infile.is_open()) return 1;

        return 0;
    }

    void parse_file() {
        int item_type;
        int item_id;
        string item_data;
        string line;

        typedef tokenizer< escaped_list_separator<char> > Tokenizer;

        tokenizer<escaped_list_separator<char>>::iterator beg;

        // Start by geting the file info
        getline(infile, file_info);
        cout << "File info: " << file_info << endl << endl;

        int counter = 0;

        // Then proceed to getting the rest
        while (getline(infile, line))
        {
            counter++;
            Tokenizer tok(line);
            beg = tok.begin();

            item_type = stoi(*beg++);
            item_id = stoi(*beg++);

//            cout << endl;
//            cout << "item type: " << item_type << endl;
//            cout << "item id  : " << item_id   << endl;

            switch(item_type) {
                case TYPE_CALIBRATION_MATRIX: {
//                    cout << "Loading Camera Matrix... ";
                    K = Matx33d(
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++)
                            );
//                    cout << "DONE" << endl;
                    break;
                }

                case TYPE_CAMERA: {
                    Vec3d t;
                    Matx33d R;

//                    cout << "Loading R: ";
                    R = Matx33d(
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++)
                    );
//                    cout << "DONE" << endl;

//                    cout << "Loading T: ";
                    t = Vec3d(
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++)
                    );
//                    t = Mat(3, 3, CV_32FC3, t_values);
//                    cout << "DONE" << endl;

//                    cout << "R: " << R << endl;
//                    cout << "t: " << t << endl;

                    Rs.push_back(R);
                    Ts.push_back(t);
                    camera_ids.push_back(item_id);

                    break;
                }
                case TYPE_POINT: {
                    vector<float> point_color_values;
                    Vec3d point;
                    Vec3b point_color;

//                    cout << "Loading Point... ";
                    point = Vec3d(
                            stod(*beg++),
                            stod(*beg++),
                            stod(*beg++)
                    );
//                    cout << "DONE" << endl;

                    if (beg == tok.end()) {
//                        cout << "Point Color not found" << endl;
                        point_color = Vec3b(0, 0, 0);
                    } else {
                        point_color = Vec3d(
                                stoi(*beg++),
                                stoi(*beg++),
                                stoi(*beg++)
                        );
//                        cout << "DONE" << endl;
                    }

                    points.push_back(point);
                    point_colors.push_back(point_color);
                    point_ids.push_back(item_id);
                }
            }
//            cout << endl;
        }
    }

    static void mouseCallback(int event, int x, int y, int flags, void* userdata) {
        cout << "Mouse event" << endl;
    }

    void visualize3D() {
        cout << "Visualize reconstruction" << endl;

        // Create 3D windows
        window = viz::Viz3d("SfM");
        window.setWindowSize(Size(2500, 1500));
        window.setWindowPosition(Point(150, 150));
        window.setBackgroundColor(viz::Color::white());

        // Recovering cameras
        vector<Affine3d> path;
        for (size_t index = 0; index < Rs.size(); ++index) {
            path.push_back(Affine3d(Rs[index], Ts[index]));
        }

        // Add the pointcloud
        viz::WCloud cloud_widget(points, point_colors);
        cloud_widget.setRenderingProperty( cv::viz::POINT_SIZE, 8 );
        window.showWidget("point_cloud", cloud_widget);
        // Add cameras
        window.showWidget("camera_path", viz::WTrajectory(path, viz::WTrajectory::PATH, 0.1, viz::Color::black()));
        window.showWidget("camera_frames", viz::WTrajectory(path, viz::WTrajectory::FRAMES, 0.1, viz::Color::black()));
        window.showWidget("cameras_frustums", viz::WTrajectoryFrustums(path, K, .1, viz::Color::navy()));
        window.setViewerPose(path[0]);

        bool show_viewer_pose = false;
        bool use_viewer_pose = false;

        if (!show_viewer_pose) {
            window.spin();
        } else {
            Affine3d pose;

            const double pose_vals[] = {0.97492, 0.0146819, -0.222069, 4.80515, 0.020948, 0.987338, 0.157242, -2.38275, 0.221565, -0.157951, 0.962268, -12.7444, 0, 0, 0, 1, };
            Affine3d pose2 = Affine3d(pose_vals);

            if (use_viewer_pose) {
                window.setViewerPose(pose2);
            }

            int counter = 0;
            window.spinOnce(200, true);
            while (!window.wasStopped()) {
                pose = window.getViewerPose();
                for (int i = 0; i < 16; i++) {
                    cout << pose.matrix.val[i] << ", ";
                }
                cout << endl;
                window.spinOnce(1000, true);
            }
        }
        cout << "Visualisation closed" << endl;
    }

//    static void mouseCallback(const viz::MouseEvent::Type& type, const viz::MouseEvent::MouseButton& button, const Point& p, int modifiers) {
//        cout << "Mouse event" << endl;
//    }
};

int main(int argc, char** argv) {
    utils::logging::setLogLevel(utils::logging::LOG_LEVEL_DEBUG);

    cv::CommandLineParser parser(argc, argv,
                                 "{help h ? |       | help message}"
                                 "{@dir     | .     | directory with image files for reconstruction }"
    );

    if (parser.has("help"))
    {
        parser.printMessage();
        return 0;
    }

    Visualizer visualizer(parser.get<string>("@dir"));
    visualizer.run();

    return 0;
}


#include "relpose.hpp"
#include "get_valtonenornhag_arxiv_2021.hpp"
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>
#include <stdexcept>
#include <vector>

namespace py = pybind11;

py::list get_valtonenornhag_arxiv_2021_fEf_wrapper(const Eigen::Ref<const Eigen::MatrixXd> &x1,
                                                   const Eigen::Ref<const Eigen::MatrixXd> &x2,
                                                   const Eigen::Ref<const Eigen::MatrixXd> &R1,
                                                   const Eigen::Ref<const Eigen::MatrixXd> &R2)
{

    if (x1.rows() != 2 || x1.cols() != 3) {
        throw std::invalid_argument("First argument should be of size 2x3");
    }
    if (x2.rows() != 2 || x2.cols() != 3) {
        throw std::invalid_argument("Second argument should be of size 2x3");
    }
    if (R1.rows() != 3 || R1.cols() != 3) {
        throw std::invalid_argument("Third argument should be of size 3x3");
    }
    if (R2.rows() != 3 || R2.cols() != 3) {
        throw std::invalid_argument("Fourth argument should be of size 3x3");
    }

    std::vector<DronePoseLib::RelPose> posedata = DronePoseLib::ValtonenOrnhagArxiv2021::get_fEf(x1, x2, R1, R2);

    py::list lst;
    for (int i = 0; i < posedata.size(); i++) {
        py::dict d;
        d["F"] = posedata[i].F;
        d["f"] = posedata[i].f;
        d["t"] = posedata[i].t;
        lst.append(d);
    }

    return lst;
}

py::list get_valtonenornhag_arxiv_2021_frEfr_wrapper(const Eigen::Ref<const Eigen::MatrixXd> &x1,
                                                     const Eigen::Ref<const Eigen::MatrixXd> &x2,
                                                     const Eigen::Ref<const Eigen::MatrixXd> &R1,
                                                     const Eigen::Ref<const Eigen::MatrixXd> &R2,
                                                     const bool use_fast_solver)
{

    if (x1.rows() != 2 || x1.cols() != 4) {
        throw std::invalid_argument("First argument should be of size 2x4");
    }
    if (x2.rows() != 2 || x2.cols() != 4) {
        throw std::invalid_argument("Second argument should be of size 2x4");
    }
    if (R1.rows() != 3 || R1.cols() != 3) {
        throw std::invalid_argument("Third argument should be of size 3x3");
    }
    if (R2.rows() != 3 || R2.cols() != 3) {
        throw std::invalid_argument("Fourth argument should be of size 3x3");
    }

    std::vector<DronePoseLib::RelPose> posedata =
        DronePoseLib::ValtonenOrnhagArxiv2021::get_frEfr(x1, x2, R1, R2, use_fast_solver);

    py::list lst;
    for (int i = 0; i < posedata.size(); i++) {
        py::dict d;
        d["F"] = posedata[i].F;
        d["f"] = posedata[i].f;
        d["r"] = posedata[i].r;
        d["t"] = posedata[i].t;
        lst.append(d);
    }

    return lst;
}

py::list get_valtonenornhag_arxiv_2021_rEr_wrapper(const Eigen::Ref<const Eigen::MatrixXd> &x1,
                                                   const Eigen::Ref<const Eigen::MatrixXd> &x2,
                                                   const Eigen::Ref<const Eigen::MatrixXd> &R1,
                                                   const Eigen::Ref<const Eigen::MatrixXd> &R2,
                                                   const double focal_length)
{

    if (x1.rows() != 2 || x1.cols() != 3) {
        throw std::invalid_argument("First argument should be of size 2x3");
    }
    if (x2.rows() != 2 || x2.cols() != 3) {
        throw std::invalid_argument("Second argument should be of size 2x3");
    }
    if (R1.rows() != 3 || R1.cols() != 3) {
        throw std::invalid_argument("Third argument should be of size 3x3");
    }
    if (R2.rows() != 3 || R2.cols() != 3) {
        throw std::invalid_argument("Fourth argument should be of size 3x3");
    }

    std::vector<DronePoseLib::RelPose> posedata =
        DronePoseLib::ValtonenOrnhagArxiv2021Extra::get_rEr(x1, x2, R1, R2, focal_length);

    py::list lst;
    for (int i = 0; i < posedata.size(); i++) {
        py::dict d;
        d["F"] = posedata[i].F;
        d["f"] = posedata[i].f;
        d["r"] = posedata[i].r;
        d["t"] = posedata[i].t;
        lst.append(d);
    }

    return lst;
}

PYBIND11_MODULE(droneposelib, m) {
  m.doc() = R"doc(
        Python module
        -----------------------
        .. currentmodule:: droneposelib
        .. autosummary::
           :toctree: _generate

           get_valtonenornhag_arxiv_2021_fEf
           get_valtonenornhag_arxiv_2021_frEfr
           get_valtonenornhag_arxiv_2021_rEr
    )doc";

  m.def("get_valtonenornhag_arxiv_2021_fEf", &get_valtonenornhag_arxiv_2021_fEf_wrapper, R"doc(
        Valtonen Ornhag et al. (ArXiV, 2021) 3-point relative pose using IMU data.

        Minimal solver using three point correspondences. Computes the relative pose with unknown and
        equal focal length. No (or negligble) radial distortion assumed.
    )doc");

  m.def("get_valtonenornhag_arxiv_2021_frEfr", &get_valtonenornhag_arxiv_2021_frEfr_wrapper, R"doc(
        Valtonen Ornhag et al. (ArXiV, 2021) 4-point radial relative pose using IMU data.

        Minimal solver using four point correspondences. Computes the relative pose with unknown and
        equal focal length and a single radial distortion parameter (Fitzgibbon's division model assumed).
    )doc");

  m.def("get_valtonenornhag_arxiv_2021_rEr", &get_valtonenornhag_arxiv_2021_rEr_wrapper, R"doc(
        Valtonen Ornhag et al. (ArXiV extra, 2021) 3-point radial relative pose using IMU data.

        Minimal solver using three point correspondences. Computes the relative pose with unknown and
        equal radial distortion parameter (Fitzgibbon's division model assumed). Focal length is assumed
        to be known and equal in both poses.
    )doc");
}


/*
 *  Copyright 2011-2015 Maxim Milakov
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include "elastic_deformation_2d_data_transformer.h"

#include "neural_network_exception.h"
#include "data_transformer_util.h"

#include <opencv2/imgproc/imgproc.hpp>
#include <boost/format.hpp>

namespace nnforge
{
	elastic_deformation_2d_data_transformer::elastic_deformation_2d_data_transformer(
		float sigma,
		float alpha,
		unsigned char border_value)
		: alpha(alpha)
		, sigma(sigma)
		, border_value(border_value)
		, gen(rnd::get_random_generator())
		, displacement_distribution(nnforge_uniform_real_distribution<float>(-1.0F, 1.0F))
		, work(io_service)
	{
		for(unsigned int i = 0; i < 2; ++i)
			threadpool.create_thread(boost::bind(&boost::asio::io_service::run, &io_service));
	}

	elastic_deformation_2d_data_transformer::~elastic_deformation_2d_data_transformer()
	{
		io_service.stop();
		threadpool.join_all();
	}

	void elastic_deformation_2d_data_transformer::transform(
		const void * data,
		void * data_transformed,
		neuron_data_type::input_type type,
		const layer_configuration_specific& original_config,
		unsigned int sample_id)
	{
		if (type != neuron_data_type::type_byte)
			throw neural_network_exception("intensity_2d_data_transformer is implemented for data stored as bytes only");

		if (original_config.dimension_sizes.size() < 2)
			throw neural_network_exception((boost::format("intensity_2d_data_transformer is processing at least 2d data, data is passed with number of dimensions %1%") % original_config.dimension_sizes.size()).str());

		int ksize = static_cast<int>((sigma - 0.8F) * 3.0F + 1.0F) * 2 + 1;

		cv::Mat1f x_disp(original_config.dimension_sizes[1], original_config.dimension_sizes[0]);
		for(int row_id = 0; row_id < x_disp.rows; ++row_id)
		{
			float * row_ptr = x_disp.ptr<float>(row_id);
			for(int column_id = 0; column_id < x_disp.cols; ++column_id)
				row_ptr[column_id] = displacement_distribution(gen);
		}
		nnforge_shared_ptr<smooth_info> info_x(new smooth_info());
		info_x->disp = x_disp;
		info_x->is_x = true;
		info_x->ksize = ksize;
		info_x->sigma = sigma;
		info_x->alpha = alpha;
		io_service.post(boost::bind(smooth_worker, info_x));

		cv::Mat1f y_disp(original_config.dimension_sizes[1], original_config.dimension_sizes[0]);
		for(int row_id = 0; row_id < y_disp.rows; ++row_id)
		{
			float * row_ptr = y_disp.ptr<float>(row_id);
			for(int column_id = 0; column_id < y_disp.cols; ++column_id)
				row_ptr[column_id] = displacement_distribution(gen);
		}
		nnforge_shared_ptr<smooth_info> info_y(new smooth_info());
		info_y->disp = y_disp;
		info_y->is_x = false;
		info_y->ksize = ksize;
		info_y->sigma = sigma;
		info_y->alpha = alpha;
		io_service.post(boost::bind(smooth_worker, info_y));

		{
			boost::unique_lock<boost::mutex> lock(info_x->is_ready_mutex);
			while (!info_x->is_ready)
				info_x->is_ready_condition.wait(lock);
		}

		{
			boost::unique_lock<boost::mutex> lock(info_y->is_ready_mutex);
			while (!info_y->is_ready)
				info_y->is_ready_condition.wait(lock);
		}

		unsigned int neuron_count_per_image = original_config.dimension_sizes[0] * original_config.dimension_sizes[1];
		unsigned int image_count = original_config.get_neuron_count() / neuron_count_per_image;
		for(unsigned int image_id = 0; image_id < image_count; ++image_id)
		{
			cv::Mat1b image(static_cast<int>(original_config.dimension_sizes[1]), static_cast<int>(original_config.dimension_sizes[0]), static_cast<unsigned char *>(data_transformed) + (image_id * neuron_count_per_image));
			cv::Mat1b image_cloned = image.clone();

			cv::remap(image_cloned, image, x_disp, y_disp, cv::INTER_LINEAR, cv::BORDER_CONSTANT, border_value);
		}
	}

 	bool elastic_deformation_2d_data_transformer::is_deterministic() const
	{
		return false;
	}

	elastic_deformation_2d_data_transformer::smooth_info::smooth_info()
		: is_ready(false)
	{
	}

	void elastic_deformation_2d_data_transformer::smooth_worker(nnforge_shared_ptr<smooth_info> info)
	{
		cv::GaussianBlur(info->disp, info->disp, cv::Size(info->ksize, info->ksize), info->sigma, info->sigma, cv::BORDER_REFLECT_101);
		float sum_of_squared = 0.0F;
		for(int row_id = 0; row_id < info->disp.rows; ++row_id)
		{
			float * row_ptr = info->disp.ptr<float>(row_id);
			float sum_of_squared_local = 0.0F;
			for(int column_id = 0; column_id < info->disp.cols; ++column_id)
			{
				float val = row_ptr[column_id];
				sum_of_squared_local += val * val;
			}
			sum_of_squared += sum_of_squared_local;
		}
		float disp_norm = sqrtf(sum_of_squared);
		float mult = info->alpha / disp_norm;

		if (info->is_x)
		{
			for(int row_id = 0; row_id < info->disp.rows; ++row_id)
			{
				float * row_ptr = info->disp.ptr<float>(row_id);
				for(int column_id = 0; column_id < info->disp.cols; ++column_id)
				{
					float column_id_f = static_cast<float>(column_id);
					row_ptr[column_id] = row_ptr[column_id] * mult + column_id_f;
				}
			}
		}
		else
		{
			for(int row_id = 0; row_id < info->disp.rows; ++row_id)
			{
				float row_id_f = static_cast<float>(row_id);
				float * row_ptr = info->disp.ptr<float>(row_id);
				for(int column_id = 0; column_id < info->disp.cols; ++column_id)
				{
					row_ptr[column_id] = row_ptr[column_id] * mult + row_id_f;
				}
			}
		}

		{
			boost::lock_guard<boost::mutex> lock(info->is_ready_mutex);
			info->is_ready = true;
		}
		info->is_ready_condition.notify_one();
	}
}


// Copyright (c) 2020, ETH Zurich
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Pascal Sommer (pasommer-at-student-dot-ethz-dot-ch)

#include "estimators/gravity_relative_pose.h"

#include <Eigen/Eigenvalues>

#include "base/pose.h"
#include "estimators/utils.h"

namespace colmap {

void GravityRelativePoseEstimator::SetRotationAxis(
    const Eigen::Vector3d& rotation_axis) {
  rotation_axis_ = rotation_axis.normalized();
}

std::vector<Pose> GravityRelativePoseEstimator::Estimate(
    const std::vector<Eigen::Vector2d>& points1,
    const std::vector<Eigen::Vector2d>& points2) const {
  // Assemble the QEP, see equation 5 and 6 in Section 3.1 of
  // Sweeney et al. 2014

  const Eigen::Vector3d& v = rotation_axis_;

  Eigen::Matrix3d M;  // quadratic part
  Eigen::Matrix3d C;  // linear part
  Eigen::Matrix3d K;  // constant part

  for (int i = 0; i < 3; ++i) {
    const Eigen::Vector3d p1 = points1[i].homogeneous();
    const Eigen::Vector3d p2 = points2[i].homogeneous();

    M.row(i) = p2.cross(p1);
    C.row(i) = 2. * p2.cross(v.cross(p1));
    K.row(i) = 2. * v.dot(p1) * p2.cross(v) - p2.cross(p1);
  }

  // Solve QEP

  std::vector<Pose> candidate_solutions;

  Eigen::Matrix3d M_inv;
  bool M_invertible;
  M.computeInverseWithCheck(M_inv, M_invertible, 1e-12);

  if (M_invertible) {
    Eigen::Matrix<double, 6, 6> SEP;
    SEP <<
        -M_inv * C,                  -M_inv * K,
        Eigen::Matrix3d::Identity(), Eigen::Matrix3d::Zero();
    Eigen::EigenSolver<Eigen::Matrix<double, 6, 6>> eigensolver(SEP);

    candidate_solutions.reserve(4);
    for (int i = 0; i < 6; ++i) {
      if (std::abs(eigensolver.eigenvalues()[i].imag()) < 0.001) {
        const double s = eigensolver.eigenvalues()[i].real();

        Eigen::Vector4d rotation(s, v[0], v[1], v[2]);
        rotation.normalize();

        const Eigen::Vector3d translation =
            eigensolver.eigenvectors().col(i).tail<3>().real().normalized();

        // brace-enclosed initializer doesn't work here for gcc 4.8
        Pose cand;
        cand.qvec = rotation;
        cand.tvec = translation;
        candidate_solutions.push_back(cand);
      }
    }

    // assert(candidate_solutions.size() >= 4);
  } else {
    candidate_solutions.reserve(2);

    const Eigen::Vector4d zero_rot = ComposeIdentityQuaternion();

    Eigen::JacobiSVD<Eigen::Matrix3d> svd = M.jacobiSvd(Eigen::ComputeFullV);
    const Eigen::Vector3d nullspace_M = svd.matrixV().col(2).normalized();

    Pose cand;
    cand.qvec = zero_rot;
    cand.tvec = nullspace_M;
    candidate_solutions.push_back(cand);
    cand.tvec *= -1;
    candidate_solutions.push_back(cand);
  }

  return candidate_solutions;
}

void GravityRelativePoseEstimator::Residuals(
    const std::vector<Eigen::Vector2d>& points1,
    const std::vector<Eigen::Vector2d>& points2, const Pose& transformation,
    std::vector<double>* residuals) {
  const Eigen::Matrix3d E = CrossProductMatrix(transformation.tvec) *
                            QuaternionToRotationMatrix(transformation.qvec);
  ComputeSquaredSampsonError(points1, points2, E, residuals);
}

}  // namespace colmap


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SUCCESSOR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SUCCESSOR_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/function/simd/adds.hpp>
#include <boost/simd/function/simd/bitfloating.hpp>
#include <boost/simd/function/simd/bitinteger.hpp>
#include <boost/simd/function/simd/if_allbits_else.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_gez.hpp>
#include <boost/simd/function/simd/is_greater.hpp>
#include <boost/simd/function/simd/is_nan.hpp>
#include <boost/simd/function/simd/is_not_equal.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/oneplus.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/seladd.hpp>
#include <boost/assert.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(successor_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return seladd(is_not_equal(a0, Valmax<A0>()), a0, One<A0>());
      }
   };

   BOOST_DISPATCH_OVERLOAD(successor_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return if_allbits_else(is_nan(a0), bitfloating(oneplus(bitinteger(a0))));
      }
   };

   BOOST_DISPATCH_OVERLOAD(successor_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::integer_<A0>, X>
                          , bs::pack_<bd::integer_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG(assert_all(is_gez(a1)), "successor rank must be non negative");
        return if_else( bs::is_greater(Valmax<A0>()-a1, a0),
                        a0+a1,
                        Valmax<A0>());
      }
   };

   BOOST_DISPATCH_OVERLOAD(successor_
                          , (typename A0, typename A1, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::integer_<A1>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const  A1&  a1) const BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG(assert_all(is_gez(a1)), "successor rank must be non negative");
        return if_allbits_else(is_nan(a0), bitfloating(adds(bitinteger(a0), a1)));
      }
   };

} } }

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "unproject.h"

#include <Eigen/Dense>
#include <Eigen/LU>

template <
  typename Derivedwin,
  typename Derivedmodel,
  typename Derivedproj,
  typename Derivedviewport,
  typename Derivedscene>
IGL_INLINE void igl::unproject(
  const Eigen::MatrixBase<Derivedwin>&  win,
  const Eigen::MatrixBase<Derivedmodel>& model,
  const Eigen::MatrixBase<Derivedproj>& proj,
  const Eigen::MatrixBase<Derivedviewport>&  viewport,
  Eigen::PlainObjectBase<Derivedscene> & scene)
{
  if(win.cols() != 3)
  {
    assert(win.rows() == 3);
    // needless transposes
    Eigen::Matrix<typename Derivedscene::Scalar,1,3> sceneT;
    unproject(win.transpose().eval(),model,proj,viewport,sceneT);
    scene = sceneT.head(3);
    return;
  }
  assert(win.cols() == 3);
  const int n = win.rows();
  scene.resize(n,3);
  for(int i = 0;i<n;i++)
  {
    typedef typename Derivedscene::Scalar Scalar;
    Eigen::Matrix<Scalar,4,4> Inverse =
      (proj.template cast<Scalar>() * model.template cast<Scalar>()).inverse();

    Eigen::Matrix<Scalar,4,1> tmp;
    tmp << win.row(i).head(3).transpose(), 1;
    tmp(0) = (tmp(0) - viewport(0)) / viewport(2);
    tmp(1) = (tmp(1) - viewport(1)) / viewport(3);
    tmp = tmp.array() * 2.0f - 1.0f;

    Eigen::Matrix<Scalar,4,1> obj = Inverse * tmp;
    obj /= obj(3);

    scene.row(i).head(3) = obj.head(3);
  }
}

template <typename Scalar>
IGL_INLINE Eigen::Matrix<Scalar,3,1> igl::unproject(
  const    Eigen::Matrix<Scalar,3,1>&  win,
  const    Eigen::Matrix<Scalar,4,4>& model,
  const    Eigen::Matrix<Scalar,4,4>& proj,
  const    Eigen::Matrix<Scalar,4,1>&  viewport)
{
  Eigen::Matrix<Scalar,3,1> scene;
  unproject(win,model,proj,viewport,scene);
  return scene;
}

#ifdef IGL_STATIC_LIBRARY
template Eigen::Matrix<float, 3, 1, 0, 3, 1> igl::unproject<float>(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 4, 0, 4, 4> const&, Eigen::Matrix<float, 4, 1, 0, 4, 1> const&);
template Eigen::Matrix<double, 3, 1, 0, 3, 1> igl::unproject<double>(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1> const&);
template void igl::unproject<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<float, 4, 4, 0, 4, 4>, Eigen::Matrix<float, 4, 4, 0, 4, 4>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 4, 0, 4, 4> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 4, 0, 4, 4> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::unproject<Eigen::Matrix<float, 3, 1, 0, 3, 1>, Eigen::Matrix<float, 4, 4, 0, 4, 4>, Eigen::Matrix<float, 4, 4, 0, 4, 4>, Eigen::Matrix<float, 4, 1, 0, 4, 1>, Eigen::Matrix<float, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Matrix<float, 3, 1, 0, 3, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 4, 0, 4, 4> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 4, 0, 4, 4> > const&, Eigen::MatrixBase<Eigen::Matrix<float, 4, 1, 0, 4, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, 3, 1, 0, 3, 1> >&);
#endif


#include <string>
#include <vector>
#include <ncurses.h>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

int width;
int height;
using cpp_int = boost::multiprecision::cpp_int;
cpp_int cookie = cpp_int("0x8000000000000");

void printCookie(WINDOW * window);
void printItem(WINDOW * window);

struct Item
{
	std::string name;
	cpp_int const baseCost;
	cpp_int const baseCps;
	cpp_int cost;
	cpp_int cps;
	cpp_int number;

	Item(std::string && name, cpp_int && baseCost, cpp_int && baseCps)
	:
	name(name),
	baseCost(baseCost),
	baseCps(baseCps),
	cost(baseCost),
	cps(baseCps),
	number(0)
	{
	}

	void buy()
	{
		if (cookie >= cost)
		{
			++number;
			cookie -= cost;
			//cost = baseCost * std::pow(1.15, (double)number);
			cost = baseCost * (int)std::pow(1.15, (int)number);
		}
	}

};

std::vector<Item> item
{
	{"++", cpp_int("0x10"), cpp_int("0x1")},
	{"$EDITOR", cpp_int("0x20"), cpp_int("0x4")},
	{"Farm", cpp_int("0x400"), cpp_int("0x10")},
	{"Mine", cpp_int("0x4000"), cpp_int("0x40")},
	{"Factory", cpp_int("0x5000"), cpp_int("0x200")},
	{"Bank", cpp_int("0x200000"), cpp_int("0xA00")},
	{"Temple", cpp_int("0x300000"), cpp_int("0x4000")},
	{"Wizard Tower", cpp_int("0x20000000"), cpp_int("0x10000")},
	{"Shipment", cpp_int("0x70000000"), cpp_int("0x80000")},
	{"Alchemy Lab", cpp_int("0x1000000000"), cpp_int("0x300000")},
	{"Portal", cpp_int("0x10000000000"), cpp_int("0x2000000")},
	{"Time Machine", cpp_int("0xE0000000000"), cpp_int("0x9000000")},
	{"Antimatter Condenser", cpp_int("0xB00000000000"), cpp_int("0x40000000")},
	{"Prism", cpp_int("0x8000000000000"), cpp_int("0x200000000")}
};

int main()
{
	initscr();
	clear();
	noecho();
	nonl();
	cbreak();
	curs_set(0);
	//halfdelay(1);
	timeout(1);
	nodelay(stdscr, true);
	keypad(stdscr, true);
	//mousemask(BUTTON1_RELEASED, NULL);
	mousemask(BUTTON1_CLICKED, NULL);
	getmaxyx(stdscr, height, width);
	notimeout(stdscr, false);

	/*attron(A_REVERSE);
	mvprintw(23, 1, "Click on Exit to quit (Works best in a virtual console)");
	attroff(A_REVERSE);
	refresh();
	*/

	WINDOW * windowCookie;
	windowCookie = newwin(height, width / 3, 0, 0);
	nodelay(windowCookie, true);

	WINDOW * windowItem;
	windowItem = newwin(height, width / 3, 0, ((width * 2) / 3));
	nodelay(windowItem, true);

	printCookie(windowCookie);
	doupdate();

	MEVENT event;
	while (true)
	{
		//int c = wgetch(windowCookie);
		int c = getch();
		static int jjj = 0;
		//mvwprintw(windowCookie, 2, 10, "%0X %0X", c, jjj++);
		switch (c)
		{
			case KEY_MOUSE:
			if (getmouse(&event) == OK)
			{
				if (event.bstate & BUTTON1_RELEASED)
				{
					++cookie;
					int a = ((event.y - 2) / 4);
					if ((0 <= a && a < item.size()) && (width * 2 / 3) <= event.x)
					{
						item[a].buy();
					}
				}
				//mvprintw(30, 0, "%d, %d", event.x, event.y);
			}
			break;

			case KEY_ENTER:
				++cookie;
			break;

			case 'a':
				++cookie;
			break;

			default:
			break;
		}
		{
			static int time = 0;
			if (time >= 1000)
			{
				for (auto & e : item)
				{
					cookie += e.number * e.cps;
				}
				time = 0;
			}
			else
			{
				++time;
			}
		}
		//napms(1000);
		//++cookie;
		printCookie(windowCookie);
		printItem(windowItem);
		doupdate();
	}
	endwin();
}

void printCookie(WINDOW * window)
{
	//wborder(window, ' ', ACS_VLINE, ' ', ' ', ' ', ACS_VLINE, ' ', ACS_VLINE);
	wborder(window, ACS_VLINE, ACS_VLINE, ' ', ' ', ACS_VLINE, ACS_VLINE, ACS_VLINE, ACS_VLINE);
	char image[][33] =
	{
	"        AAAAAAAAAAAAAAAA        ",
	"      AAAAAAAAAAAAAAAAAAAA      ",
    "    AAAAAAAAAAAAAAAAAAAAAAAA    ",
	"  AAAAAAAAAAAAAAAAAAAAAAAAAAAA  ",
	"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
    "AAAABBBBAAAAAAAAAAAAAAAAAAAAAAAA",
	"AABBBBBBBBAAAAAAAAAAAAAAAAAAAAAA",
	"AABBBBBBBBAAAAAAAAAAAAAAAAAAAAAA",
	"AAAABBBBAAAAAAAAAAAAAAAAAAAAAAAA",
	"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
	"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA",
	"AAAAAAAAAAAABBBBAAAAAAABBAAAAAAA",
	"  AAAAAAAAAABBBBAAAAAAAAAAAAAA  ",
	"    AAAAAAAAAAAAAAAAAAAAAAAA    ",
	"      AAAAAAAAAAAAAAAAAAAA      ",
	"        AAAAAAAAAAAAAAAA        "
	};
	for (int i = 0; i < 16; ++i)
	{
		for (int j = 0; j < 32; ++j)
		{
			if (image[i][j] == 'A')
			{
				mvwaddch(window, 6 + i, 10 + j, ACS_BLOCK);
			}
			else if (image[i][j] == 'B')
			{
				mvwaddch(window, 6 + i, 10 + j, ACS_CKBOARD);
			}
		}
	}
	mvwprintw(window, 3, 10, "0x%s [cookie]", cookie.str(0, std::ios_base::hex).c_str());

	cpp_int cps = 0;
	for (auto & e : item)
	{
		cps += e.number * e.cps;
	}
	mvwprintw(window, 4, 10, "0x%s [cps]", cps.str(0, std::ios_base::hex).c_str());
	wnoutrefresh(window);
}

void printItem(WINDOW * window)
{
	wborder(window, ACS_VLINE, ACS_VLINE, ' ', ' ', ACS_VLINE, ACS_VLINE, ACS_VLINE, ACS_VLINE);
	//wborder(window, ACS_VLINE, ' ', ' ', ' ', ACS_VLINE, ' ', ACS_VLINE, ' ');
	int y;
	int x;
	getbegyx(window, y, x);

	mvwprintw(window, 0, 3,  "Store");
	for (int i = 0; i < item.size(); ++i)
	{
		if (i == 3)
		{
			wattron(window, A_REVERSE);
		}
		mvwprintw(window, 2 + (i * 4), 2, "%s : 0x%s", item[i].name.c_str(), item[i].number.str(0, std::ios_base::hex).c_str());
		mvwprintw(window, 2 + (i * 4) + 1, 2, "0x%s [cps]", item[i].cps.str(0, std::ios_base::hex).c_str());
		mvwprintw(window, 2 + (i * 4) + 2, 2, "0x%s [cookie]", item[i].cost.str(0, std::ios_base::hex).c_str());
		wattroff(window, A_REVERSE);
	}
	wnoutrefresh(window);
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::model::src::main.cpp                                //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <boost/statistics/survival/constant.hpp>
#include <libs/statistics/survival/model/example/exponential.h>
#include <libs/statistics/survival/model/example/posterior_analysis.h>

int main(){
    using namespace boost;

    example_exponential(std::cout);
    typedef statistics::survival::constant<double> const_;

    example_posterior_analysis(
    // TODO n_bath should be deduced from input (getline)
        5e4, // n_batch,
        1e3, // n_batch_mod,
        1e4, // n_proposal,
        1e4, // n_t_pars,
        const_::inf_, //entry_bound
        "/Users/erwann/projets/2009/Xcode/survival/build/Release/",  // in_path
        "/Users/erwann/projets/2009/Xcode/survival/build/Release/",  // out_path
        std::cout
    );

    return 0;
}


#include <sparse.h>
#include <sparse_fill.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <iostream>  //  needed for compile testing output stream operators

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(io_test)
{
  typedef sparse::Block<4,1,float>   block4_type;
  typedef sparse::Vector<block4_type> vector_type;

  vector_type v;
  v.resize(2);
  sparse::fill(v);
  sparse::io::write_matlab_file("vector.m", v);

  typedef sparse::Block<4,4,float>   block4x4_type;
  sparse::DiagonalMatrix<block4x4_type> D(5);
  sparse::fill(D);
	sparse::io::write_matlab_file("diagonal.m", D);

  typedef sparse::Block<4,6,float>   block4x6_type;
  block4x6_type b46;
  sparse::fill(b46, 46.0f);

  sparse::TwoColumnMatrix<block4x6_type> TCM;
  TCM.resize(1,4,2);
  TCM(0,0) = b46;
  TCM(0,3) = b46;
 	sparse::io::write_matlab_file("two_column_matrix.m", TCM);

  typedef sparse::Block<6,4,float>   block6x4_type;
  block6x4_type b64;
  sparse::fill( b64, 64.0f );

  sparse::CompressedRowMatrix<block6x4_type> CRM;
  CRM.resize(5,3,4);
  CRM(0,0) = b64;
  CRM(2,1) = b64;
  CRM(4,1) = b64;
  CRM(4,2) = b64;
	sparse::io::write_matlab_file("compressed_row_matrix.m", CRM);

  {
    using namespace sparse::io;

    std::cout << v << std::endl;
    std::cout << D << std::endl;
    std::cout << TCM << std::endl;
    std::cout << CRM << std::endl;
  }

}

BOOST_AUTO_TEST_CASE(string_writer_test)
{
  typedef sparse::Block<2,1,float>                   block2x1_type;
  typedef sparse::Block<2,2,float>                   block2x2_type;

  sparse::Vector<block2x1_type> v;
  v.resize(2);
  sparse::fill(v);
  std::string v_expected = "Block at row: 0 => [ 0 ; 1 ] \nBlock at row: 1 => [ 0 ; 1 ] \n";
  std::string v_expected_compressed = "[ 0 ; 1 ] [ 0 ; 1 ] ";
  std::string v_to_string = sparse::io::to_string( v );
  std::string v_to_string_compressed = sparse::io::to_string( v, true );
  BOOST_CHECK( v_expected == v_to_string );
  BOOST_CHECK( v_expected_compressed == v_to_string_compressed );

  sparse::CompressedVector<block2x1_type> c(2,3);
  sparse::fill( c(0) );
  sparse::fill( c(2) );
  std::string c_expected = "Block at row: 0 => [ 0 ; 1 ] \nBlock at row: 2 => [ 0 ; 1 ] \n";
  std::string c_expected_compressed = "0: [ 0 ; 1 ] 2: [ 0 ; 1 ] ";
  std::string c_to_string = sparse::io::to_string( c );
  std::string c_to_string_compressed = sparse::io::to_string( c, true );
  BOOST_CHECK( c_expected == c_to_string );
  BOOST_CHECK( c_expected_compressed == c_to_string_compressed );

  sparse::DiagonalMatrix<block2x2_type> D(3);
  sparse::fill(D);
  std::string D_expected = "Block at row: 0, column: 0 => [ 0,1 ; 2,3 ] \nBlock at row: 1, column: 1 => [ 0,1 ; 2,3 ] \nBlock at row: 2, column: 2 => [ 0,1 ; 2,3 ] \n";
  std::string D_expected_compressed = "(0,0): [ 0,1 ; 2,3 ] (1,1): [ 0,1 ; 2,3 ] (2,2): [ 0,1 ; 2,3 ] ";
  std::string D_to_string = sparse::io::to_string( D );
  std::string D_to_string_compressed = sparse::io::to_string( D, true );
  BOOST_CHECK( D_expected == D_to_string );
  BOOST_CHECK( D_expected_compressed == D_to_string_compressed );

  sparse::CompressedRowMatrix<block2x2_type> C;
  C.resize(4,4,2);
  C(0,0) = D(0);
  C(0,3) = D(2);
  std::string C_expected = "Block at row: 0, column: 0 => [ 0,1 ; 2,3 ] \nBlock at row: 0, column: 3 => [ 0,1 ; 2,3 ] \n";
  std::string C_expected_compressed = "(0,0): [ 0,1 ; 2,3 ] (0,3): [ 0,1 ; 2,3 ] ";
  std::string C_to_string = sparse::io::to_string( C );
  std::string C_to_string_compressed = sparse::io::to_string( C, true );
  BOOST_CHECK( C_expected == C_to_string );
  BOOST_CHECK( C_expected_compressed == C_to_string_compressed );
}

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_lessgreater.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/quarter.hpp>
#include <cmath>

STF_CASE_TPL (" is_lessgreater real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_lessgreater;
  using r_t = decltype(is_lessgreater(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(is_lessgreater(bs::Inf<T>(), bs::Inf<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Minf<T>(), bs::Minf<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Nan<T>(), bs::Nan<T>()), r_t(false));
#endif
  STF_EQUAL(is_lessgreater(-bs::Zero<T>(), -bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Half<T>(), bs::Half<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Mone<T>(), bs::Mone<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::One<T>(), bs::One<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Quarter<T>(), bs::Quarter<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Two<T>(), bs::Two<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Zero<T>(), bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(-bs::Zero<T>(), -bs::One<T>()), r_t(true));
  STF_EQUAL(is_lessgreater(bs::One<T>(), bs::Half<T>()), r_t(true));
}

STF_CASE_TPL (" is_lessgreater signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_lessgreater;
  using r_t = decltype(is_lessgreater(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_lessgreater(bs::Mone<T>(), bs::Mone<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::One<T>(), bs::One<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Two<T>(), bs::Two<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Zero<T>(), bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Mone<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(is_lessgreater(bs::One<T>(), bs::Mone<T>()), r_t(true));
}

STF_CASE_TPL (" is_lessgreater unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_lessgreater;
  using r_t = decltype(is_lessgreater(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_lessgreater(bs::One<T>(), bs::One<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Two<T>(), bs::Two<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Zero<T>(), bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_lessgreater(bs::Zero<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(is_lessgreater(bs::Two<T>(), bs::Zero<T>()), r_t(true));
}

STF_CASE ( "is_lessgreater bool")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_lessgreater;
  using r_t = decltype(is_lessgreater(bool(), bool()));

  // return type conformity test
  STF_TYPE_IS(r_t, bool);

  // specific values tests
  STF_EQUAL(is_lessgreater(true, false), true);
  STF_EQUAL(is_lessgreater(false, true), true);
  STF_EQUAL(is_lessgreater(true, true), false);
  STF_EQUAL(is_lessgreater(false, false), false);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/dist.hpp>
#include <nt2/include/functions/splat.hpp>

#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/ten.hpp>
#include <nt2/include/constants/mten.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

#include <boost/dispatch/functor/meta/call.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL ( dist_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::dist;
  using nt2::tag::dist_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<dist_(T,T)>::type
                  , T
                  );

#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(dist(nt2::Inf<T>() , nt2::Inf<T>()) , nt2::Nan<T>());
  NT2_TEST_EQUAL(dist(nt2::Minf<T>(), nt2::Minf<T>()), nt2::Nan<T>());
  NT2_TEST_EQUAL(dist(nt2::Nan<T>() , nt2::Nan<T>()) , nt2::Nan<T>());
#endif

  NT2_TEST_EQUAL(dist(nt2::Zero<T>(), nt2::Zero<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(dist(nt2::Mone<T>(), nt2::One<T>()), nt2::Two<T>());
  NT2_TEST_EQUAL(dist(nt2::One<T>(), nt2::Three<T>()), nt2::Two<T>());
}

NT2_TEST_CASE_TPL ( dist_integer_ui,  BOOST_SIMD_SIMD_UNSIGNED_TYPES)
{
  using nt2::dist;
  using nt2::tag::dist_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<dist_(T,T)>::type
                  , T
                  );

  NT2_TEST_EQUAL(dist(nt2::Zero<T>(), nt2::Zero<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(dist(nt2::One<T>(), nt2::Three<T>()), nt2::Two<T>());
  NT2_TEST_EQUAL(dist(nt2::Valmax<T>(), nt2::Zero<T>()), nt2::Valmax<T>());
  NT2_TEST_EQUAL(dist(nt2::Zero<T>(), nt2::Valmax<T>()), nt2::Valmax<T>());
}

NT2_TEST_CASE_TPL ( dist_integer_si,  BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using nt2::dist;
  using nt2::tag::dist_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<dist_(T,T)>::type
                  , T
                  );

  NT2_TEST_EQUAL(dist(nt2::Zero<T>(), nt2::Zero<T>()), nt2::Zero<T>());
  NT2_TEST_EQUAL(dist(nt2::Mone<T>(), nt2::One<T>()), nt2::Two<T>());
  NT2_TEST_EQUAL(dist(nt2::One<T>(), nt2::Three<T>()), nt2::Two<T>());
  NT2_TEST_EQUAL(dist(nt2::Valmax<T>(), nt2::Zero<T>()), nt2::Valmax<T>());
  NT2_TEST_EQUAL(dist(nt2::Zero<T>(), nt2::Valmax<T>()), nt2::Valmax<T>());

  NT2_TEST_EQUAL(dist(nt2::Ten<T>(), nt2::Mten<T>()), nt2::splat<T>(20));
  NT2_TEST_EQUAL(dist(nt2::Mten<T>(), nt2::Ten<T>()), nt2::splat<T>(20));
}