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

/*    $Id: step-11.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2001-2004, 2006, 2009, 2011-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.                        */

// As usual, the program starts with a rather long list of include files which
// you are probably already used to by now:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/table_handler.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_boundary_lib.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_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/mapping_q.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>

// Just this one is new: it declares a class
// CompressedSparsityPattern, which we will use and explain
// further down below.
#include <deal.II/lac/compressed_sparsity_pattern.h>

// We will make use of the std::find algorithm of the C++ standard library, so
// we have to include the following file for its declaration:
#include <algorithm>
#include <iostream>
#include <iomanip>
#include <cmath>

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

  // Then we declare a class which represents the solution of a Laplace
  // problem. As this example program is based on step-5, the class looks
  // rather the same, with the sole structural difference that the functions
  // assemble_system now calls solve itself, and is
  // thus called assemble_and_solve, and that the output function
  // was dropped since the solution function is so boring that it is not worth
  // being viewed.
  //
  // The only other noteworthy change is that the constructor takes a value
  // representing the polynomial degree of the mapping to be used later on,
  // and that it has another member variable representing exactly this
  // mapping. In general, this variable will occur in real applications at the
  // same places where the finite element is declared or used.
  template <int dim>
  class LaplaceProblem
  {
  public:
    LaplaceProblem (const unsigned int mapping_degree);
    void run ();

  private:
    void setup_system ();
    void assemble_and_solve ();
    void solve ();

    Triangulation<dim>   triangulation;
    FE_Q<dim>            fe;
    DoFHandler<dim>      dof_handler;
    MappingQ<dim>        mapping;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    ConstraintMatrix     mean_value_constraints;

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

    TableHandler         output_table;
  };

  // Construct such an object, by initializing the variables. Here, we use
  // linear finite elements (the argument to the fe variable
  // denotes the polynomial degree), and mappings of given order. Print to
  // screen what we are about to do.
  template <int dim>
  LaplaceProblem<dim>::LaplaceProblem (const unsigned int mapping_degree) :
    fe (1),
    dof_handler (triangulation),
    mapping (mapping_degree)
  {
    std::cout << "Using mapping with degree " << mapping_degree << ":"
              << std::endl
              << "============================"
              << std::endl;
  }

  // The first task is to set up the variables for this problem. This includes
  // generating a valid DoFHandler object, as well as the
  // sparsity patterns for the matrix, and the object representing the
  // constraints that the mean value of the degrees of freedom on the boundary
  // be zero.
  template <int dim>
  void LaplaceProblem<dim>::setup_system ()
  {
    // The first task is trivial: generate an enumeration of the degrees of
    // freedom, and initialize solution and right hand side vector to their
    // correct sizes:
    dof_handler.distribute_dofs (fe);
    solution.reinit (dof_handler.n_dofs());
    system_rhs.reinit (dof_handler.n_dofs());

    // Next task is to construct the object representing the constraint that
    // the mean value of the degrees of freedom on the boundary shall be
    // zero. For this, we first want a list of those nodes which are actually
    // at the boundary. The DoFTools class has a function that
    // returns an array of boolean values where true indicates
    // that the node is at the boundary. The second argument denotes a mask
    // selecting which components of vector valued finite elements we want to
    // be considered. This sort of information is encoded using the
    // ComponentMask class (see also @ref GlossComponentMask). Since we have a
    // scalar finite element anyway, this mask in reality should have only one
    // entry with a true value. However, the ComponentMask class
    // has semantics that allow it to represents a mask of indefinite size
    // whose every element equals true when one just default
    // constructs such an object, so this is what we'll do here.
    std::vector<bool> boundary_dofs (dof_handler.n_dofs(), false);
    DoFTools::extract_boundary_dofs (dof_handler,
                                     ComponentMask(),
                                     boundary_dofs);

    // Now first for the generation of the constraints: as mentioned in the
    // introduction, we constrain one of the nodes on the boundary by the
    // values of all other DoFs on the boundary. So, let us first pick out the
    // first boundary node from this list. We do that by searching for the
    // first true value in the array (note that
    // std::find returns an iterator to this element), and
    // computing its distance to the overall first element in the array to get
    // its index:
    const unsigned int first_boundary_dof
      = std::distance (boundary_dofs.begin(),
                       std::find (boundary_dofs.begin(),
                                  boundary_dofs.end(),
                                  true));

    // Then generate a constraints object with just this one constraint. First
    // clear all previous content (which might reside there from the previous
    // computation on a once coarser grid), then add this one line
    // constraining the first_boundary_dof to the sum of other
    // boundary DoFs each with weight -1. Finally, close the constraints
    // object, i.e. do some internal bookkeeping on it for faster processing
    // of what is to come later:
    mean_value_constraints.clear ();
    mean_value_constraints.add_line (first_boundary_dof);
    for (unsigned int i=first_boundary_dof+1; i<dof_handler.n_dofs(); ++i)
      if (boundary_dofs[i] == true)
        mean_value_constraints.add_entry (first_boundary_dof,
                                          i, -1);
    mean_value_constraints.close ();

    // Next task is to generate a sparsity pattern. This is indeed a tricky
    // task here. Usually, we just call
    // DoFTools::make_sparsity_pattern and condense the result
    // using the hanging node constraints. We have no hanging node constraints
    // here (since we only refine globally in this example), but we have this
    // global constraint on the boundary. This poses one severe problem in
    // this context: the SparsityPattern class wants us to state
    // beforehand the maximal number of entries per row, either for all rows
    // or for each row separately. There are functions in the library which
    // can tell you this number in case you just have hanging node constraints
    // (namely DoFHandler::max_coupling_between_dofs), but how is
    // this for the present case? The difficulty arises because the
    // elimination of the constrained degree of freedom requires a number of
    // additional entries in the matrix at places that are not so simple to
    // determine. We would therefore have a problem had we to give a maximal
    // number of entries per row here.
    //
    // Since this can be so difficult that no reasonable answer can be given
    // that allows allocation of only a reasonable amount of memory, there is
    // a class CompressedSparsityPattern, that can help us out
    // here. It does not require that we know in advance how many entries rows
    // could have, but allows just about any length. It is thus significantly
    // more flexible in case you do not have good estimates of row lengths,
    // however at the price that building up such a pattern is also
    // significantly more expensive than building up a pattern for which you
    // had information in advance. Nevertheless, as we have no other choice
    // here, we'll just build such an object by initializing it with the
    // dimensions of the matrix and calling another function
    // DoFTools::make_sparsity_pattern to get the sparsity
    // pattern due to the differential operator, then condense it with the
    // constraints object which adds those positions in the sparsity pattern
    // that are required for the elimination of the constraint.
    CompressedSparsityPattern csp (dof_handler.n_dofs(),
                                   dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern (dof_handler, csp);
    mean_value_constraints.condense (csp);

    // Finally, once we have the full pattern, we can initialize an object of
    // type SparsityPattern from it and in turn initialize the
    // matrix with it. Note that this is actually necessary, since the
    // CompressedSparsityPattern is so inefficient compared to
    // the SparsityPattern class due to the more flexible data
    // structures it has to use, that we can impossibly base the sparse matrix
    // class on it, but rather need an object of type
    // SparsityPattern, which we generate by copying from the
    // intermediate object.
    //
    // As a further sidenote, you will notice that we do not explicitly have
    // to compress the sparsity pattern here. This, of course, is
    // due to the fact that the copy_from function generates a
    // compressed object right from the start, to which you cannot add new
    // entries anymore. The compress call is therefore implicit
    // in the copy_from call.
    sparsity_pattern.copy_from (csp);
    system_matrix.reinit (sparsity_pattern);
  }

  // The next function then assembles the linear system of equations, solves
  // it, and evaluates the solution. This then makes three actions, and we
  // will put them into eight true statements (excluding declaration of
  // variables, and handling of temporary vectors). Thus, this function is
  // something for the very lazy. Nevertheless, the functions called are
  // rather powerful, and through them this function uses a good deal of the
  // whole library. But let's look at each of the steps.
  template <int dim>
  void LaplaceProblem<dim>::assemble_and_solve ()
  {

    // First, we have to assemble the matrix and the right hand side. In all
    // previous examples, we have investigated various ways how to do this
    // manually. However, since the Laplace matrix and simple right hand sides
    // appear so frequently in applications, the library provides functions
    // for actually doing this for you, i.e. they perform the loop over all
    // cells, setting up the local matrices and vectors, and putting them
    // together for the end result.
    //
    // The following are the two most commonly used ones: creation of the
    // Laplace matrix and creation of a right hand side vector from body or
    // boundary forces. They take the mapping object, the
    // DoFHandler object representing the degrees of freedom and
    // the finite element in use, a quadrature formula to be used, and the
    // output object. The function that creates a right hand side vector also
    // has to take a function object describing the (continuous) right hand
    // side function.
    //
    // Let us look at the way the matrix and body forces are integrated:
    const unsigned int gauss_degree
      = std::max (static_cast<unsigned int>(std::ceil(1.*(mapping.get_degree()+1)/2)),
                  2U);
    MatrixTools::create_laplace_matrix (mapping, dof_handler,
                                        QGauss<dim>(gauss_degree),
                                        system_matrix);
    VectorTools::create_right_hand_side (mapping, dof_handler,
                                         QGauss<dim>(gauss_degree),
                                         ConstantFunction<dim>(-2),
                                         system_rhs);
    // That's quite simple, right?
    //
    // Two remarks are in order, though: First, these functions are used in a
    // lot of contexts. Maybe you want to create a Laplace or mass matrix for
    // a vector values finite element; or you want to use the default Q1
    // mapping; or you want to assembled the matrix with a coefficient in the
    // Laplace operator. For this reason, there are quite a large number of
    // variants of these functions in the MatrixCreator and
    // MatrixTools classes. Whenever you need a slightly
    // different version of these functions than the ones called above, it is
    // certainly worthwhile to take a look at the documentation and to check
    // whether something fits your needs.
    //
    // The second remark concerns the quadrature formula we use: we want to
    // integrate over bilinear shape functions, so we know that we have to use
    // at least a Gauss2 quadrature formula. On the other hand, we want to
    // have the quadrature rule to have at least the order of the boundary
    // approximation. Since the order of Gauss-r is 2r, and the order of the
    // boundary approximation using polynomials of degree p is p+1, we know
    // that 2r@>=p+1. Since r has to be an integer and (as mentioned above)
    // has to be at least 2, this makes up for the formula above computing
    // gauss_degree.
    //
    // Since the generation of the body force contributions to the right hand
    // side vector was so simple, we do that all over again for the boundary
    // forces as well: allocate a vector of the right size and call the right
    // function. The boundary function has constant values, so we can generate
    // an object from the library on the fly, and we use the same quadrature
    // formula as above, but this time of lower dimension since we integrate
    // over faces now instead of cells:
    Vector<double> tmp (system_rhs.size());
    VectorTools::create_boundary_right_hand_side (mapping, dof_handler,
                                                  QGauss<dim-1>(gauss_degree),
                                                  ConstantFunction<dim>(1),
                                                  tmp);
    // Then add the contributions from the boundary to those from the interior
    // of the domain:
    system_rhs += tmp;
    // For assembling the right hand side, we had to use two different vector
    // objects, and later add them together. The reason we had to do so is
    // that the VectorTools::create_right_hand_side and
    // VectorTools::create_boundary_right_hand_side functions
    // first clear the output vector, rather than adding up their results to
    // previous contents. This can reasonably be called a design flaw in the
    // library made in its infancy, but unfortunately things are as they are
    // for some time now and it is difficult to change such things that
    // silently break existing code, so we have to live with that.

    // Now, the linear system is set up, so we can eliminate the one degree of
    // freedom which we constrained to the other DoFs on the boundary for the
    // mean value constraint from matrix and right hand side vector, and solve
    // the system. After that, distribute the constraints again, which in this
    // case means setting the constrained degree of freedom to its proper
    // value
    mean_value_constraints.condense (system_matrix);
    mean_value_constraints.condense (system_rhs);

    solve ();
    mean_value_constraints.distribute (solution);

    // Finally, evaluate what we got as solution. As stated in the
    // introduction, we are interested in the H1 semi-norm of the
    // solution. Here, as well, we have a function in the library that does
    // this, although in a slightly non-obvious way: the
    // VectorTools::integrate_difference function integrates the
    // norm of the difference between a finite element function and a
    // continuous function. If we therefore want the norm of a finite element
    // field, we just put the continuous function to zero. Note that this
    // function, just as so many other ones in the library as well, has at
    // least two versions, one which takes a mapping as argument (which we
    // make us of here), and the one which we have used in previous examples
    // which implicitly uses MappingQ1.  Also note that we take a
    // quadrature formula of one degree higher, in order to avoid
    // superconvergence effects where the solution happens to be especially
    // close to the exact solution at certain points (we don't know whether
    // this might be the case here, but there are cases known of this, and we
    // just want to make sure):
    Vector<float> norm_per_cell (triangulation.n_active_cells());
    VectorTools::integrate_difference (mapping, dof_handler,
                                       solution,
                                       ZeroFunction<dim>(),
                                       norm_per_cell,
                                       QGauss<dim>(gauss_degree+1),
                                       VectorTools::H1_seminorm);
    // Then, the function just called returns its results as a vector of
    // values each of which denotes the norm on one cell. To get the global
    // norm, a simple computation shows that we have to take the l2 norm of
    // the vector:
    const double norm = norm_per_cell.l2_norm();

    // Last task -- generate output:
    output_table.add_value ("cells", triangulation.n_active_cells());
    output_table.add_value ("|u|_1", norm);
    output_table.add_value ("error", std::fabs(norm-std::sqrt(3.14159265358/2)));
  }

  // The following function solving the linear system of equations is copied
  // from step-5 and is explained there in some detail:
  template <int dim>
  void LaplaceProblem<dim>::solve ()
  {
    SolverControl           solver_control (1000, 1e-12);
    SolverCG<>              cg (solver_control);

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

    cg.solve (system_matrix, solution, system_rhs,
              preconditioner);
  }

  // Finally the main function controlling the different steps to be
  // performed. Its content is rather straightforward, generating a
  // triangulation of a circle, associating a boundary to it, and then doing
  // several cycles on subsequently finer grids. Note again that we have put
  // mesh refinement into the loop header; this may be something for a test
  // program, but for real applications you should consider that this implies
  // that the mesh is refined after the loop is executed the last time since
  // the increment clause (the last part of the three-parted loop header) is
  // executed before the comparison part (the second one), which may be rather
  // costly if the mesh is already quite refined. In that case, you should
  // arrange code such that the mesh is not further refined after the last
  // loop run (or you should do it at the beginning of each run except for the
  // first one).
  template <int dim>
  void LaplaceProblem<dim>::run ()
  {
    GridGenerator::hyper_ball (triangulation);
    static const HyperBallBoundary<dim> boundary;
    triangulation.set_boundary (0, boundary);

    for (unsigned int cycle=0; cycle<6; ++cycle, triangulation.refine_global(1))
      {
        setup_system ();
        assemble_and_solve ();
      };

    // After all the data is generated, write a table of results to the
    // screen:
    output_table.set_precision("|u|_1", 6);
    output_table.set_precision("error", 6);
    output_table.write_text (std::cout);
    std::cout << std::endl;
  }
}

// Finally the main function. It's structure is the same as that used in
// several of the previous examples, so probably needs no more explanation.
int main ()
{
  try
    {
      dealii::deallog.depth_console (0);
      std::cout.precision(5);

      // This is the main loop, doing the computations with mappings of linear
      // through cubic mappings. Note that since we need the object of type
      // LaplaceProblem@<2@> only once, we do not even name it,
      // but create an unnamed such object and call the run
      // function of it, subsequent to which it is immediately destroyed
      // again.
      for (unsigned int mapping_degree=1; mapping_degree<=3; ++mapping_degree)
        Step11::LaplaceProblem<2>(mapping_degree).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;
}


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

  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/fix.hpp>
#include <boost/simd/function/fast.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/pi.hpp>
#include <boost/simd/constant/three.hpp>

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

  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(fix(bs::Inf<T>()), bs::Inf<T>());
  STF_EQUAL(fix(bs::Minf<T>()), bs::Minf<T>());
  STF_IEEE_EQUAL(fix(bs::Nan<T>()), bs::Nan<T>());
#endif
  STF_EQUAL(fix(bs::One<T>()), bs::One<T>());
  STF_EQUAL(fix(bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(fix(bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(fix(bs::Pi<T>()), bs::Three<T>());
  STF_EQUAL(fix(T(1.4)), T(1));
  STF_EQUAL(fix(T(1.5)), T(1));
  STF_EQUAL(fix(T(1.6)), T(1));
  STF_EQUAL(fix(T(2.5)), T(2));
  STF_EQUAL(fix(T(-1.4)), T(-1));
  STF_EQUAL(fix(T(-1.5)), T(-1));
  STF_EQUAL(fix(T(-1.6)), T(-1));
  STF_EQUAL(fix(T(-2.5)), T(-2));
} // end of test for floating_

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

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

  // specific values tests
  STF_EQUAL(fix(bs::One<T>()), bs::One<T>());
  STF_EQUAL(fix(bs::Zero<T>()), bs::Zero<T>());
} // end of test for unsigned_int_

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

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

  // specific values tests
  STF_EQUAL(fix(bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(fix(bs::One<T>()), bs::One<T>());
  STF_EQUAL(fix(bs::Zero<T>()), bs::Zero<T>());
} // end of test for signed_int_

STF_CASE_TPL ( "fast fix real",  STF_IEEE_TYPES)
{

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::fix;
  using r_t = decltype(fix(T()));

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

  // specific values tests
  STF_EQUAL(bs::fast_(fix)(bs::One<T>()), bs::One<T>());
  STF_EQUAL(bs::fast_(fix)(bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(bs::fast_(fix)(bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(bs::fast_(fix)(bs::Pi<T>()), bs::Three<T>());
  STF_EQUAL(bs::fast_(fix)(T(1.4)), T(1));
  STF_EQUAL(bs::fast_(fix)(T(1.5)), T(1));
  STF_EQUAL(bs::fast_(fix)(T(1.6)), T(1));
  STF_EQUAL(bs::fast_(fix)(T(2.5)), T(2));
  STF_EQUAL(bs::fast_(fix)(T(-1.4)), T(-1));
  STF_EQUAL(bs::fast_(fix)(T(-1.5)), T(-1));
  STF_EQUAL(bs::fast_(fix)(T(-1.6)), T(-1));
  STF_EQUAL(bs::fast_(fix)(T(-2.5)), T(-2));
} // end of test for floating_


/* autodiff_benchmark.cpp - Program to benchmark different automatic differentiation tools

  Copyright (C) 2014 The University of Reading

  Copying and distribution of this file, with or without modification,
  are permitted in any medium without royalty provided the copyright
  notice and this notice are preserved.  This file is offered as-is,
  without any warranty.
*/

#include <sstream>
#include <iostream>
#include <vector>
#include <cmath>
#include <valarray>

#include "differentiator.h"

#include <adept.h>
using adept::Real;

static
Real
rms(const std::vector<Real>& a, const std::vector<Real>&b)
{
  if (a.size() != b.size()) {
    throw differentiator_exception("Attempt to compute RMS difference between vectors of different size");
  }
  Real sum = 0.0;
  for (size_t i = 0; i < a.size(); i++) {
    sum += (a[i]-b[i])*(a[i]-b[i]);
  }
  return sqrt(sum/a.size());
}

static
void
usage(const char* argv0)
{
  std::cout << "Usage: " << argv0 << " [OPTIONS] where OPTIONS can be\n";
  std::cout << "  -h|--help          Print this message\n";
  std::cout << "  -a|--algorithm  s  Use test algorithms specified by string s which may be\n";
  std::cout << "                     \"all\" or a comma separated list with possible entries\n";
  std::cout << "                     " << test_algorithms() << "\n";
  std::cout << "  -t|--tool       s  Use automatic differentiation tools specified by string\n";
  std::cout << "                     s which may be \"all\" or a comma separated list with\n";
  std::cout << "                     possible entries " << autodiff_tools() << "\n";
  std::cout << "  -r|--repeat     n  Benchmark repeats the simulation n times\n";
  std::cout << "  -j|--jrepeat    n  Repeat the Jacobian simulation n times\n";
  std::cout << "  -n|--timesteps  n  Simulation uses n timesteps\n";
  std::cout << "  --print-result     Print the final output from the simulation(s)\n";
  std::cout << "  --print-adjoint    Print the hand-coded adjoint\n";
  std::cout << "  --print-jacobian   Print the hand-coded Jacobian matrix\n";
  std::cout << "  --no-openmp        Don't use OpenMP to speed up Adept\n";
  std::cout << "  --jacobian-forward Force use of forward-mode Jacobian\n";
  std::cout << "  --jacobian-reverse Force use of reverse-mode Jacobian\n";
  std::cout << "  --tolerance     x  Agreement with hand-coded requires RMS difference < x\n";
  std::cout << "  --verify-only      No benchmark: only verify correctness of results\n";
  std::cout << "Return code: 0 if all automatic differentiation tools produce adjoints and\n"
    "  Jacobians whose RMS difference with the values from hand-coded\n"
    "  differentiation is less than the required tolerance; 1 otherwise.\n";
}

int
main(int argc, char** argv)
{
  int nt = 2000;
  int nr = 100;
  int nr_jacobian = nr/10;
  Real dt = 0.125;
  Real tolerance = 1.0e-5;
  int force_jacobian = 0;

  bool verbose = false;
  bool print_result = false;
  bool print_adjoint = false;
  bool print_jacobian = false;
  bool no_openmp = false;
  bool verify_only = false;

  std::valarray<bool> use_tool(N_AUTODIFF_TOOLS);
  std::valarray<bool> use_algorithm(N_TEST_ALGORITHMS);

  use_tool = true;
  use_algorithm = true;

  int iarg = 1;

  while (iarg < argc) {
    if (std::string("-h") == argv[iarg]
	|| std::string("--help") == argv[iarg]) {
      usage(argv[0]);
      return 0;
    }
    if (std::string("-v") == argv[iarg]
	|| std::string("--verbose") == argv[iarg]) {
      verbose = true;
    }
    else if (std::string("--print-result") == argv[iarg]) {
      print_result = true;
    }
    else if (std::string("--print-adjoint") == argv[iarg]) {
      print_adjoint = true;
    }
    else if (std::string("--print-jacobian") == argv[iarg]) {
      print_jacobian = true;
    }
    else if (std::string("--jacobian-forward") == argv[iarg]) {
      force_jacobian = +1;
    }
    else if (std::string("--jacobian-reverse") == argv[iarg]) {
      force_jacobian = -1;
    }
    else if (std::string("--no-openmp") == argv[iarg]) {
      no_openmp = true;
    }
    else if (std::string("--verify-only") == argv[iarg]) {
      verify_only = true;
    }
    else if (std::string("-a") == argv[iarg]
	     || std::string("--algorithm") == argv[iarg]) {
      if (++iarg < argc) {
	if (std::string(argv[iarg]) != "all") {
	  use_algorithm = false;
	  std::istringstream ss(argv[iarg]);
	  std::string alg;
	  while (std::getline(ss, alg, ',')) {
	    bool found = false;
	    for (int i = 0; i < N_TEST_ALGORITHMS; i++) {
	      if (alg == test_algorithm_string[i]) {
		use_algorithm[i] = true;
		found = true;
		break;
	      }
	    }
	    if (!found) {
	      std::cout << "Test algorithm \""
			<< alg << "\" not available; available algorithms are "
			<< test_algorithms() << "\n";
	    }
	  }
	}
      }
      else {
	std::cout << "Arguments \"-a\" or \"--algorithm\" need to be followed by a string containing a comma-separated list of algorithms\n";
	return 1;
      }
    }
    else if (std::string("-t") == argv[iarg]
	     || std::string("--tool") == argv[iarg]) {
      if (++iarg < argc) {
	if (std::string(argv[iarg]) != "all") {
	  use_tool = false;
	  std::istringstream ss(argv[iarg]);
	  std::string tool;
	  while (std::getline(ss, tool, ',')) {
	    bool found = false;
	    for (int i = 0; i < N_AUTODIFF_TOOLS; i++) {
	      if (tool == autodiff_tool_string[i]) {
		use_tool[i] = true;
		found = true;
		break;
	      }
	    }
	    if (!found) {
	      std::cout << "Automatic differentiation tool \""
			<< tool << "\" not available; available tools are "
			<< autodiff_tools() << "\n";
	    }
	  }
	}
      }
      else {
	std::cout << "Arguments \"-a\" or \"--algorithm\" need to be followed by a string containing a comma-separated list of algorithms\n";
	return 1;
      }
    }
    else if (std::string("-r") == argv[iarg]
	     || std::string("--repeat") == argv[iarg]) {
      if (++iarg < argc) {
	std::stringstream ss(argv[iarg]);
	if (ss >> nr) {
	  if (nr <= 0) {
	    std::cout << "Number of repeats must be greater than zero\n";
	    return 1;
	  }
	}
	else {
	  std::cout << "Failed to read \""
		    << argv[iarg]
		    << "\"as an integer\n";
	  return 1;
	}
      }
      else {
	throw differentiator_exception("Arguments \"-r\" or \"--repeat\" need to be followed by a number");
      }
    }
    else if (std::string("-j") == argv[iarg]
	     || std::string("--jrepeat") == argv[iarg]) {
      if (++iarg < argc) {
	std::stringstream ss(argv[iarg]);
	if (ss >> nr_jacobian) {
	  if (nr <= 0) {
	    throw differentiator_exception("Number of repeats must be greater than zero");
	  }
	}
	else {
	  std::string msg = "Failed to read \"";
	  msg += argv[iarg];
	  msg += "\"as an integer";
	  throw differentiator_exception(msg.c_str());
	}
      }
      else {
	throw differentiator_exception("Arguments \"-j\" or \"--jrepeat\" need to be followed by a number");
      }
    }
    else if (std::string("-n") == argv[iarg]
	     || std::string("--timesteps") == argv[iarg]) {
      if (++iarg < argc) {
	std::stringstream ss(argv[iarg]);
	if (ss >> nt) {
	  if (nt < 0) {
	    throw differentiator_exception("Number of timesteps must be greater than or equal to zero");
	  }
	}
	else {
	  std::string msg = "Failed to read \"";
	  msg += argv[iarg];
	  msg += "\"as an integer";
	  throw differentiator_exception(msg.c_str());
	}
      }
      else {
	throw differentiator_exception("Arguments \"-n\" or \"--timesteps\" need to be followed by a number");
      }
    }
    else if (std::string("--tolerance") == argv[iarg]) {
      if (++iarg < argc) {
	std::stringstream ss(argv[iarg]);
	if (ss >> tolerance) {
	  if (tolerance < 0) {
	    throw differentiator_exception("Tolerance must be greater than or equal to zero");
	  }
	}
	else {
	  std::string msg = "Failed to read \"";
	  msg += argv[iarg];
	  msg += "\"as a Real";
	  throw differentiator_exception(msg.c_str());
	}
      }
      else {
	throw differentiator_exception("Arguments \"-j\" or \"--jrepeat\" need to be followed by a number");
      }
    }
    else {
      std::string msg = "Argument \"";
      msg += argv[iarg];
      msg += "\" not understood\n";
      std::cout << msg;
      usage(argv[0]);
      return 1;
    }
    iarg++;
  }

  Real pi = 4.0*atan(1.0);
  std::vector<Real> q_init(NX);
  std::vector<Real> q(NX);
  std::vector<Real> q_AD(NX);
  std::vector<Real> q_init_AD(NX);
  std::vector<Real> q_init_AD_reference(NX);
  std::vector<Real> jac(NX*NX);
  std::vector<Real> jac_reference(NX*NX);

  int nr_warm_up = nr/10;
  int nr_jacobian_warm_up = nr_jacobian/10;
  if (nr_warm_up < 1) {
    nr_warm_up = 1;
  }
  if (nr_jacobian_warm_up < 1) {
    nr_jacobian_warm_up = 1;
  }

  if (verify_only) {
    nr = 0;
    nr_jacobian = 0;
    nr_warm_up = 1;
    nr_jacobian_warm_up = 1;
  }

  for (int i = 0; i < NX; i++) q_init[i] = (0.5+0.5*sin((i*2.0*pi)/(NX-1.5)))+1;
  for (int i = 0; i < NX; i++) q_AD[i] = 0.1;

  bool verify_error = false;

  Timer timer;

  std::cout << "Automatic differentiation benchmark and verification\n";
  std::cout << "   Automatic differentiation tools = ";
  bool is_first = true;
  for (int i = 0; i < N_AUTODIFF_TOOLS; i++) {
    if (use_tool[i]) {
      if (!is_first) {
	std::cout << ", ";
      }
      else {
	is_first = false;
      }
      std::cout << autodiff_tool_long_string[i];
    }
  }
  std::cout << "\n";

  std::cout << "   Test algorithms = ";
  is_first = true;
  for (int i = 0; i < N_TEST_ALGORITHMS; i++) {
    if (use_algorithm[i]) {
      if (!is_first) {
	std::cout << ", ";
      }
      else {
	is_first = false;
      }
      std::cout << test_algorithm_long_string[i];
    }
  }
  std::cout << "\n";

  std::cout << "   Number of x points = " << NX << "\n";
  std::cout << "   Number of timesteps = " << nt << ", Courant number = " << dt << "\n";
  if (!verify_only) {
    std::cout << "   Algorithm repeats = " << nr << ", warm-up repeats = " << nr_warm_up << "\n";
    std::cout << "   Jacobian repeats = " << nr_jacobian << ", warm-up repeats = " << nr_jacobian_warm_up << "\n";
  }
  else {
    std::cout << "   Verifying results only: no repeats\n";
  }

  std::cout << adept::configuration();

  // Loop through test algorithms
  for (int ialg = 0; ialg < N_TEST_ALGORITHMS; ialg++) {
    if (use_algorithm[ialg]) {

      std::string algorithm_string = test_algorithm_long_string[ialg];
      std::cout << "\nRunning test algorithm \"" << algorithm_string << "\":\n";

      TestAlgorithm ta = static_cast<TestAlgorithm>(ialg);

      std::cout << "   Hand coded (forward-mode Jacobian only)\n";

      HandCodedDifferentiator hand_coded_differentiator(timer, algorithm_string);
      hand_coded_differentiator.initialize(nt, dt);
      for (int i = 0; i < nr_warm_up; i++) {
	hand_coded_differentiator.func(ta, q_init, q);
	hand_coded_differentiator.adjoint(ta, q_init, q, q_AD, q_init_AD_reference);
	hand_coded_differentiator.jacobian(ta, q_init, q, jac_reference);
      }
      hand_coded_differentiator.reset_timings();
      for (int i = 0; i < nr; i++) {
	hand_coded_differentiator.func(ta, q_init, q);
	hand_coded_differentiator.adjoint(ta, q_init, q, q_AD, q_init_AD_reference);
	hand_coded_differentiator.jacobian(ta, q_init, q, jac_reference);
      }

      if (print_result) {
	std::cout << "      result = [" << q[0];
	for (int i = 1; i < NX; i++) {
	  std::cout << ", " << q[i];
	}
	std::cout << "]\n";
      }

      if (print_adjoint) {
	std::cout << "adjoint = [" << q_init_AD_reference[0];
	for (int i = 1; i < NX; i++) {
	  std::cout << ", " << q_init_AD_reference[i];
	}
	std::cout << "]\n";
      }
      if (print_jacobian) {
	Real (&q_K)[NX][NX]
	  = *reinterpret_cast<Real(*)[NX][NX]>(&jac_reference[0]);
	std::cout << "jacobian = [\n";
	for (int i = 0; i < NX; i++) {
	  std::cout << q_K[i][0];
	  for (int j = 1; j < NX; j++) {
	    std::cout << ", " << q_K[i][j];
	}
	  std::cout << "\n";
	}
	std::cout << "]\n";
      }

      Real base_time = timer.timing(hand_coded_differentiator.base_timer_id());

      if (!verify_only) {
	std::cout << "      Time of original algorithm: " << base_time << " seconds\n";
	std::cout << "      Absolute time of adjoint: "
		  << timer.timing(hand_coded_differentiator.adjoint_compute_timer_id())
		  << " s\n";
	std::cout << "      Relative time of adjoint: "
		  << timer.timing(hand_coded_differentiator.adjoint_compute_timer_id())
	  / base_time << "\n";
	std::cout << "      Absolute time of Jacobian: "
		  << timer.timing(hand_coded_differentiator.jacobian_timer_id())
		  << " s\n";
	std::cout << "      Relative time of Jacobian: "
		  << timer.timing(hand_coded_differentiator.jacobian_timer_id())
	  / base_time << "\n";
      }

      for (int itool = 0; itool < N_AUTODIFF_TOOLS; itool++) {
	if (use_tool[itool]) {
	  Differentiator* differentiator
	    = new_differentiator(static_cast<AutoDiffTool>(itool),
				 timer, algorithm_string);
	  if (!differentiator) {
	    if (verbose) std::cout << "Automatic differentiation tool with code " << itool << " not available\n";
	    continue;
	  }

	  differentiator->initialize(nt, dt);
	  if (no_openmp) {
	    differentiator->no_openmp();
	  }

	  std::cout << "   " << differentiator->name() << "\n";

	  for (int i = 0; i < nr_warm_up; i++) {
	    differentiator->adjoint(ta, q_init, q, q_AD, q_init_AD);
	  }
	  Real rms_verify = rms(q_init_AD, q_init_AD_reference);
	  if (rms_verify > tolerance) {
	    std::cout << "      *** Adjoint RMS difference with hand-coded of " << rms_verify << " is greater than tolerance of " << tolerance << " ***\n";
	    if (print_adjoint) {
	      std::cout << "adjoint_auto = [" << q_init_AD[0];
	      for (int i = 1; i < NX; i++) {
		std::cout << ", " << q_init_AD[i];
	      }
	      std::cout << "]\n";
	    }

	    verify_error = true;
	  }
	  else {
	    std::cout << "      Adjoint RMS difference with hand-coded of " << rms_verify << " is within tolerance of " << tolerance << "\n";
	  }

	  for (int i = 0; i < nr_jacobian_warm_up; i++) {
	    differentiator->jacobian(ta, q_init, q, jac, force_jacobian);
	  }
	  rms_verify = rms(jac, jac_reference);
	  if (rms_verify > tolerance) {
	    std::cout << "      *** Jacobian RMS difference with hand-coded of " << rms_verify << " is greater than tolerance of " << tolerance << " ***\n";
	    verify_error = true;
	  }
	  else {
	    std::cout << "      Jacobian RMS difference with hand-coded of " << rms_verify << " is within tolerance of " << tolerance << "\n";
	  }

	  if (!verify_only) {
	    differentiator->reset_timings();
	    for (int i = 0; i < nr; i++) {
	      differentiator->adjoint(ta, q_init, q, q_AD, q_init_AD);
	    }

	    Real relative_record_time = timer.timing(differentiator->base_timer_id())
	      / base_time;
	    Real relative_adjoint_time
	      = timer.timing(differentiator->adjoint_compute_timer_id())
	      / base_time;
	    Real relative_adjoint_prep_time
	      = timer.timing(differentiator->adjoint_prep_timer_id())
	      / base_time;

	    std::cout << "      Absolute time of adjoint: "
		      << timer.timing(differentiator->base_timer_id())
	      + timer.timing(differentiator->adjoint_compute_timer_id())
	      + timer.timing(differentiator->adjoint_prep_timer_id())
		      << " s ("
		      << timer.timing(differentiator->base_timer_id())
		      << " s + ";
	    if (relative_adjoint_prep_time > 0.0) {
	      std::cout << timer.timing(differentiator->adjoint_prep_timer_id())
			<< " s + ";
	    }
	    std::cout <<  timer.timing(differentiator->adjoint_compute_timer_id())
		      << " s)\n";
	    std::cout << "      Relative time of adjoint: "
		      << relative_record_time + relative_adjoint_prep_time
	      + relative_adjoint_time
		      << " (" << relative_record_time << " + ";
	    if (relative_adjoint_prep_time > 0.0) {
	      std::cout << relative_adjoint_prep_time << " + ";
	    }
	    std::cout << relative_adjoint_time << ")\n";
	    differentiator->reset_timings();
	  }

	  for (int i = 0; i < nr_jacobian; i++) {
	    differentiator->jacobian(ta, q_init, q, jac, force_jacobian);
	  }

	  if (print_jacobian) {
	    Real (&q_K)[NX][NX]
	      = *reinterpret_cast<Real(*)[NX][NX]>(&jac[0]);
	    std::cout << "jacobian_auto = [\n";
	    for (int i = 0; i < NX; i++) {
	      std::cout << q_K[i][0];
	      for (int j = 1; j < NX; j++) {
		std::cout << ", " << q_K[i][j];
	      }
	      std::cout << "\n";
	    }
	    std::cout << "]\n";
	  }

	  if (!verify_only) {
	    Real relative_record_time = (nr*timer.timing(differentiator->base_timer_id()))
	      /(nr_jacobian*base_time);
	    Real relative_jacobian_time = (nr*timer.timing(differentiator->jacobian_timer_id()))
	      /(nr_jacobian*base_time);
	    Real relative_adjoint_prep_time = (nr*timer.timing(differentiator->adjoint_prep_timer_id()))
	      /(nr_jacobian*base_time);
	    std::cout << "      Absolute time of Jacobian: "
		      << timer.timing(differentiator->base_timer_id())
	      + timer.timing(differentiator->adjoint_prep_timer_id())
	      + timer.timing(differentiator->jacobian_timer_id())
		      << " s ("
		      << timer.timing(differentiator->base_timer_id())
		      << " s + ";
	    if (relative_adjoint_prep_time > 0.0) {
	      std::cout << timer.timing(differentiator->adjoint_prep_timer_id())
			<< " s + ";
	    }
	    std::cout << timer.timing(differentiator->jacobian_timer_id())
		      << " s)\n";
	    std::cout << "      Relative time of Jacobian: "
		      << relative_record_time + relative_adjoint_prep_time + relative_jacobian_time
		      << " (" << relative_record_time << " + ";
	    if (relative_adjoint_prep_time > 0.0) {
	      std::cout << relative_adjoint_prep_time << " + ";
	    }
	    std::cout << relative_jacobian_time << ")\n";
	  }
	  	  differentiator->print();
	  delete differentiator;
	}
      }
    }
  }
  if (verify_error) {
    std::cout << "\nEXITING WITH ERROR CODE 1: ONE OR MORE OF THE AUTOMATIC DIFFERENTIATION\n"
	      << "TOOLS DID NOT REPRODUCE THE HAND-CODING RESULT\n";
    return 1;
  }
  else {
    std::cout << "\nAll tests were passed within tolerance\n";
    return 1;
  }
}


//          Copyright (C) 2012, Michele Caini.
// 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)

//          Two Graphs Common Spanning Trees Algorithm
//      Based on academic article of Mint, Read and Tarjan
//     Efficient Algorithm for Common Spanning Tree Problem
// Electron. Lett., 28 April 1983, Volume 19, Issue 9, p.346-347

#include <boost/type_traits.hpp>
#include <boost/concept/requires.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/compressed_pair.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/two_graphs_common_spanning_trees.hpp>
#include <vector>

using namespace boost::assign;

namespace boost
{

typedef
boost::adjacency_list
  <
    boost::vecS,         // OutEdgeList
    boost::vecS,         // VertexList
    boost::undirectedS,  // Directed
    boost::no_property,  // VertexProperties
    boost::no_property,  // EdgeProperties
    boost::no_property,  // GraphProperties
    boost::listS         // EdgeList
  >
Graph
;

typedef
boost::graph_traits<Graph>::vertex_descriptor
vertex_descriptor;

typedef
boost::graph_traits<Graph>::edge_descriptor
edge_descriptor;

typedef
boost::graph_traits<Graph>::vertex_iterator
vertex_iterator;

typedef
boost::graph_traits<Graph>::edge_iterator
edge_iterator;

template < typename Coll, typename Seq >
struct check_edge
{
public:
  BOOST_CONCEPT_ASSERT((RandomAccessContainer<Coll>));
  BOOST_CONCEPT_ASSERT((RandomAccessContainer<Seq>));

  typedef typename Coll::value_type coll_value_type;
  typedef typename Seq::value_type seq_value_type;

  BOOST_STATIC_ASSERT((is_same<coll_value_type, Seq>::value));
  BOOST_STATIC_ASSERT((is_same<seq_value_type, bool>::value));

  void operator()(Coll& coll, Seq& seq)
  {
    bool found = false;

    for(typename Coll::iterator iterator = coll.begin();
        !found && iterator != coll.end(); ++iterator)
    {
      Seq& coll_seq = *iterator;

      BOOST_REQUIRE(coll_seq.size() == seq.size());

      found = true;
      for(typename Seq::size_type pos = 0; found && pos < seq.size(); ++pos) {
        found &= coll_seq[pos] == seq[pos];
      }
    }

    BOOST_REQUIRE(found);
  }
};

void two_graphs_common_spanning_trees_test()
{
  Graph iG, vG;
  std::vector< edge_descriptor > iG_o;
  std::vector< edge_descriptor > vG_o;

  iG_o.push_back(boost::add_edge(0, 1, iG).first);
  iG_o.push_back(boost::add_edge(1, 3, iG).first);
  iG_o.push_back(boost::add_edge(3, 2, iG).first);
  iG_o.push_back(boost::add_edge(1, 5, iG).first);
  iG_o.push_back(boost::add_edge(5, 4, iG).first);
  iG_o.push_back(boost::add_edge(5, 6, iG).first);
  iG_o.push_back(boost::add_edge(5, 3, iG).first);
  iG_o.push_back(boost::add_edge(3, 1, iG).first);
  iG_o.push_back(boost::add_edge(1, 3, iG).first);

  vG_o.push_back(boost::add_edge(0, 2, vG).first);
  vG_o.push_back(boost::add_edge(0, 4, vG).first);
  vG_o.push_back(boost::add_edge(0, 5, vG).first);
  vG_o.push_back(boost::add_edge(5, 1, vG).first);
  vG_o.push_back(boost::add_edge(5, 3, vG).first);
  vG_o.push_back(boost::add_edge(5, 6, vG).first);
  vG_o.push_back(boost::add_edge(5, 4, vG).first);
  vG_o.push_back(boost::add_edge(5, 2, vG).first);
  vG_o.push_back(boost::add_edge(2, 6, vG).first);

  std::vector< std::vector<bool> > coll;
  boost::tree_collector<
      std::vector< std::vector<bool> >, std::vector<bool>
    > collector(coll);
  std::vector<bool> inL(iG_o.size(), false);

  boost::two_graphs_common_spanning_trees(iG, iG_o, vG, vG_o, collector, inL);

  check_edge< std::vector< std::vector<bool> >, std::vector<bool> > checker;
  std::vector<bool> check;

  check.clear();
  check += true, true, true, true, true, true, false, false, false;
  checker(coll, check);

  check.clear();
  check += true, true, true, true, true, true, false, false, false;
  checker(coll, check);
}

}

int test_main ( int argc, char** argv )
{
  boost::two_graphs_common_spanning_trees_test();
  return EXIT_SUCCESS;
}


///////////////////////////////////////////////////////////////////////////////
//
// THIS SOFTWARE IS PROVIDED "AS-IS". THERE IS NO WARRANTY OF ANY KIND.
// NEITHER THE AUTHORS NOR THE OHIO STATE UNIVERSITY WILL BE LIABLE
// FOR ANY DAMAGES OF ANY KIND, EVEN IF ADVISED OF SUCH POSSIBILITY.
//
// Copyright (c) 2010 Jyamiti Research Group.
// CS&E Department of the Ohio State University, Columbus, OH.
// All rights reserved.
//
// Author: Sayan Mandal
//
///////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <ctime>
#include <map>
#include <list>
#include <vector>
#include <cmath>
#include <sstream>
#include <unordered_set>
#include <string>
#include <sys/stat.h>
#include <cstddef>

#ifndef INFINITY
#define INFINITY std::numeric_limits< double >::infinity()
#endif // INFINITY

#include <boost/config.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/timer.hpp>
#include <boost/progress.hpp>
#include <boost/geometry.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
//#include <boost/graph/read_dimacs.hpp>
#include <boost/graph/graph_utility.hpp>

#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/reader_utils.h>
#include <gudhi/Bitmap_cubical_complex.h>
#include <gudhi/Persistent_cohomology.h>
// standard stuff

#include<ParseCommand.h>
#include <Legal.h>
#include "Graph.h"

using namespace boost;
using namespace std;
namespace bg = boost::geometry;

//extern vector<int> complexSizes;
//extern vector<int> accumulativeSizes;
//extern int accumulativeSize;
//extern double fThreshold;
//extern vector<double> vecFiltrationScale;
//extern SimplicialTree<bool> domain_complex;

typedef Gudhi::cubical_complex::Bitmap_cubical_complex_base<double> Bitmap_cubical_complex_base;
typedef Gudhi::cubical_complex::Bitmap_cubical_complex<Bitmap_cubical_complex_base> Bitmap_cubical_complex;
typedef Gudhi::persistent_cohomology::Field_Zp Field_Zp;
typedef Gudhi::persistent_cohomology::Persistent_cohomology<Bitmap_cubical_complex, Field_Zp> Persistent_cohomology;

extern map <int,int> threesimplex_to_node; // maps index of tetrahedrons from simplex->first to dual-graph-nodes->second
extern map <int,int> twosimplex_to_tri; // maps index of traingles from simplex->first to dual-graph-edges->second
extern map <int,int> threesimplex_to_node_rev; // maps index of tetrahedrons from dual-graph-nodes->first to simplex->second
extern map <int,int> twosimplex_to_tri_rev; // maps index of traingles from dual-graph-edges->first to simplex->second
extern map<size_t, vector<int>>  sizescale;//0- edge, 1- vertex, for each deathindex, count of vertex and edges
extern vector<double> x;
extern vector<double> y;
extern vector<double> z;
extern map<unsigned long,unsigned long> vertind; // vertex index in cubical complex(first) to actual index in x,y,z, filter

struct cmp_intervals_by_dim_then_length {
    explicit cmp_intervals_by_dim_then_length(Bitmap_cubical_complex * sc)
    : sc_(sc) { }
    template<typename Persistent_interval>
    bool operator()(const Persistent_interval & p1, const Persistent_interval & p2) {
        if (sc_->dimension(get < 0 > (p1)) == sc_->dimension(get < 0 > (p2)))
            return (sc_->filtration(get < 1 > (p1)) - sc_->filtration(get < 0 > (p1))
                    > sc_->filtration(get < 1 > (p2)) - sc_->filtration(get < 0 > (p2)));
        else
            return (sc_->dimension(get < 0 > (p1)) > sc_->dimension(get < 0 > (p2)));
    }
    Bitmap_cubical_complex* sc_;
};

Bitmap_cubical_complex buildComplex(const char* file) {

    bool dbg = false;
    std::ifstream inFiltration;
    inFiltration.open(file);

    unsigned dimensionOfData;
    inFiltration >> dimensionOfData;

    if (dbg) {
        std::cerr << "dimensionOfData : " << dimensionOfData << std::endl;
    }

    std::vector<unsigned> sizes;
    sizes.reserve(dimensionOfData);
    // all dimensions multiplied
    std::size_t dimensions = 1;
    size_t top_dim = 1;
    for (std::size_t i = 0; i != dimensionOfData; ++i) {
        unsigned size_in_this_dimension;
        inFiltration >> size_in_this_dimension;
        sizes.push_back(size_in_this_dimension - 1);
        dimensions *= (size_in_this_dimension);
        top_dim *= (size_in_this_dimension - 1);
        if (dbg) {
            std::cerr << "size_in_this_dimension : " << size_in_this_dimension << std::endl;
        }
    }
    if (dbg) {
        std::cerr << "Top dimension names : " << top_dim << std::endl;
    }

    //Create a blank Cubical Complex
    vector<double> top_dim_cells(top_dim, 0);
    Bitmap_cubical_complex b(sizes, top_dim_cells);
    double filtrationLevel;

    vector<size_t> indices_to_consider;

    //Assign values to vertices
    //Bitmap_cubical_complex::Skeleton_simplex_iterator it(&b, 0);
    for (Bitmap_cubical_complex::Skeleton_simplex_iterator it = b.skeleton_simplex_range(0).begin();
         it != b.skeleton_simplex_range(0).end(); it++) {
        if (!(inFiltration >> filtrationLevel) || (inFiltration.eof())) {
            throw std::ios_base::failure("Bad Perseus file format.");
        }
        if (dbg) {
            std::cerr << "Cell of an index : " << (*it)
            << " and dimension: " << b.get_dimension_of_a_cell(*it)
            << " get the value : " << filtrationLevel << std::endl;
        }
        b.get_cell_data(*it) = filtrationLevel;
        size_t s = (*it);
        indices_to_consider.push_back(s);

    }
    vector<bool> is_this_cell_considered(b.num_simplices(), false);
    while (indices_to_consider.size()) {
        if (dbg) {
            std::cerr << "indices_to_consider in this iteration \n";
            for (std::size_t i = 0; i != indices_to_consider.size(); ++i) {
                std::cout << indices_to_consider[i] << "  ";
            }
        }
        std::vector<std::size_t> new_indices_to_consider;
        for (std::size_t i = 0; i != indices_to_consider.size(); ++i) {
            std::vector<std::size_t> bd = b.get_coboundary_of_a_cell(indices_to_consider[i]);
            for (std::size_t boundaryIt = 0; boundaryIt != bd.size(); ++boundaryIt) {
                if (dbg) {
                    std::cerr << "filtration of a cell : " << bd[boundaryIt] << " is : " << b.filtration(bd[boundaryIt])
                    << " while of a cell: " << indices_to_consider[i] << " is: " << b.filtration(indices_to_consider[i])
                    << std::endl;
                }
                if (b.filtration(bd[boundaryIt]) < b.filtration(indices_to_consider[i])) {
                    b.get_cell_data(bd[boundaryIt]) = b.filtration(indices_to_consider[i]);
                    if (dbg) {
                        std::cerr << "Setting the value of a cell : " << bd[boundaryIt]
                        << " to : " << b.filtration(indices_to_consider[i]) << std::endl;
                    }
                }
                if (is_this_cell_considered[bd[boundaryIt]] == false) {
                    new_indices_to_consider.push_back(bd[boundaryIt]);
//                    if (b.get_dimension_of_a_cell(bd[boundaryIt]) == dimensionOfData - 1) {
//                        edges->push_back(bd[boundaryIt]);
//                    }
                    is_this_cell_considered[bd[boundaryIt]] = true;
                }
            }
        }
        indices_to_consider.swap(new_indices_to_consider);
    }

    b.initialize_simplex_associated_to_key();
//cub = b;
    return b;
}

int main(int argc, char *argv[] )
{
    int dimensions, noPoints, currentEdgeSize = 0, barcode_count = 0, scalecount = 0;
    int filtration = 0, totinsCount = 0, numbars = 0, index = 0, gap = 0;

  string point_file, tr_file, to_file;
  std::vector<bg::model::point<double, 3, bg::cs::cartesian>> vPoint;
  bool verbose;
    ParseCommand(argc, argv,  point_file, numbars, verbose, index, gap);
//  Simplex_tree simplexTree;
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    cout<<"Point_file: "<<point_file; //getchar();
    // THIS LINE GENERATES THE CUBICAL COMPLEX
    Bitmap_cubical_complex cub(point_file.c_str());//buildComplex(point_file.c_str());//(//
//    c = ;
    // Compute the persistence diagram of the complex
    Persistent_cohomology pcoh(cub);

    int p = 2;
    double min_persistence = 0;
    vector<size_t> birthiv, deathiv;
    pcoh.init_coefficients(p);  // initializes the coefficient field for homology
    pcoh.compute_persistent_cohomology(min_persistence);
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ofstream spers(point_file+"_pers");
    ofstream fpers(point_file+"_filtpers");

    cmp_intervals_by_dim_then_length cmp(&cub);
    auto persistent_pairs = pcoh.get_persistent_pairs();
    cout<<" persistent_pairs: "<<persistent_pairs.size()<<"\n";
    if(verbose)
        getchar();
    sort(std::rbegin(persistent_pairs), std::rend(persistent_pairs), cmp);

    for (auto pair : persistent_pairs) {
        if(cub.dimension(get<0>(pair))!=2)
            continue;

        birthiv.push_back(get<0>(pair));
        deathiv.push_back(get<1>(pair));
    }

    if(birthiv.size()<numbars)
    {
        cout<<"Not enough 2-cycles: "<<birthiv.size()<<" "<<numbars<<endl;
        getchar();
        exit(0);
    }
//        birthiv.erase(birthiv.begin(),birthiv.end()-numbars);
//        deathiv.erase(deathiv.begin(),deathiv.end()-numbars);
    if(verbose){
        cout<<"birth size: "<<birthiv.size()<<" "<<deathiv.size()<<" numbers: "<<numbars<<" gaps: "<<gap;
    getchar();
    }
    birthiv.erase(birthiv.begin(),birthiv.end()-numbars-gap);
    deathiv.erase(deathiv.begin(),deathiv.end()-numbars-gap);
    birthiv.erase(birthiv.end()-gap,birthiv.end());
    deathiv.erase(deathiv.end()-gap,deathiv.end());
    buildFullCubicSetting(cub, point_file, deathiv);        // GRAPH BUILDING
    cout<<"Sizes: "<<birthiv.size()<<" "<<deathiv.size()<<" "<<numbars<<" "<<(birthiv.size()-numbars);
    if(verbose)
     getchar();

    for(int gotonumbars=birthiv.size()-1; gotonumbars >= 0; gotonumbars--){

        size_t birthi = birthiv[gotonumbars];
        size_t deathi = deathiv[gotonumbars];
//        cout<<"birthi: "<<birthi<<" deathi:"<<deathi<<endl;
//        cout.setf(ios::fixed);

        spers<<"2 "<<to_string(int(birthi))<<" "<<to_string(int(deathi))<<"\n";
        fpers<<"2 "<<to_string(float(cub.filtration(birthi)))<<" "<<to_string(float(cub.filtration(deathi)))<<"\n";
        if(verbose){
            cout<<"birthi: "<<birthi<<" deathi:"<<deathi<<endl;
            getchar();

        }

        vector<vector<int>> rGraph;
        vector<vector<int>> vectri
        = boostCut( cub, birthi, deathi, verbose);
//        = minCut(adjMatrix, threesimplex_to_node[death], adjMatrix.size()-1, rGraph );
        cout<<" Found Boykov Komolov cut.\n"; //getchar();
        string ind = point_file;

        string dir_path = ind+"loops/";

        boost::filesystem::path dir(dir_path.c_str());
        boost::filesystem::create_directory(dir_path.c_str());
        string ff = ind+"loops/"+to_string(int(birthi))+"_"+to_string(int(deathi))+".off";
        tr_file = ind+"loops/"+to_string(int(birthi))+"_"+to_string(int(deathi))+".txt";
        to_file = ind+"loops/"+to_string(int(birthi))+"_"+to_string(int(deathi))+".off";
        string cu_file = ind+"loops/"+to_string(int(birthi))+"_"+to_string(int(deathi))+"cub.txt";
        cout<<"tr_file:"<<tr_file<<endl;
//        writeToFile( cub, vectri, birthi, deathi, tr_file);//
        writeToFileOFF( cub, vectri, birthi, deathi, to_file);
        cout<<"Written to TEXT: "<<gotonumbars<<" vec: "<<birthiv.size()<<" current index:"<<gotonumbars<<endl; //getchar();

    }

    threesimplex_to_node.clear();
    twosimplex_to_tri.clear();
    threesimplex_to_node_rev.clear();
    twosimplex_to_tri_rev.clear();
    sizescale.clear();
    x.clear();
    y.clear();
    z.clear();
    vertind.clear();

  return EXIT_SUCCESS;
}//end of main


#include "typesjudge.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include "all.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
namespace HT
{
    void isinteger(PASTNode astnode, ParsersHelper& ph)
    {
        validateEach(astnode, ph, "integer?", 1, [](PASTNode an){return true;});
        astnode->type = Simple;
        astnode->token.tokenType = Boolean;
        auto & secondCh = *astnode->ch.rbegin();
        if (secondCh->token.tokenType != Complex)
        {
          astnode->token.info = BooleanType(false);
          return;
        }

        auto cast = boost::get<ComplexType>(secondCh->token.info);

        astnode->token.info = BooleanType(cast.isInt());
        astnode->remove();
    }
    void isrational(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error("rational? can only have one parameter");
        astnode->type = Simple;
        astnode->token.tokenType = Boolean;
        auto & secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex)
        {
          astnode->token.info =BooleanType(false);
          return;
        }

        auto cast = boost::get<ComplexType>(secondCh->token.info);

        astnode->token.info = BooleanType(cast.isRational());
        astnode->remove();
    }

    void isreal(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error("real? can only have one parameter");
        astnode->type = Simple;
        astnode->token.tokenType = Boolean;
        auto & secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex)
        {
            astnode->token.info = BooleanType(false);
            return;
        }

        auto cast = boost::get<ComplexType>(secondCh->token.info);

        astnode->token.info = BooleanType(cast.isReal() && (cast.exact()));
        astnode->remove();
    }
    void iscomplex(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error("Complex? can only have one parameter");
        astnode->type = Simple;
        astnode->token.tokenType = Boolean;
        auto & secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        astnode->token.info =  BooleanType(secondCh->token.tokenType == Complex);

        astnode->remove();
    }

    void isnumber(PASTNode astnode, ParsersHelper& ph)
    {
        iscomplex(astnode,ph);
    }

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ARG_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ARG_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/arg.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/simd/sdk/simd/meta/is_real_convertible.hpp>
#include <boost/simd/include/constants/real.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/functions/simd/is_ltz.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/include/functions/simd/if_allbits_else.hpp>
// No implementation for integer types

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::arg_, tag::cpu_,
                      (A0)(X),
                      ((simd_<floating_<A0>,X>))
                     )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      // a0 >= 0 -> 0, a0 < 0 ->Pi, a0 Nan -> Nan
      return if_nan_else(is_nan(a0), if_else_zero(is_ltz(a0), Pi<result_type>()));
    }
  };
} } }
#endif


#pragma once

#include <iostream>
#include <memory>
#include <Eigen/Dense>
#include <vector>

class HipCommand
{
public:
    Eigen::Matrix<double,6,1> q;
    Eigen::Matrix<double,6,1> qdot;
    Eigen::Matrix<double,6,1> jtrq;
};

class HipSensorData
{
public:
    Eigen::Matrix<double,6,1> q;
    Eigen::Matrix<double,6,1> qdot;
    Eigen::Matrix<double,6,1> jtrq;
};

class HipInterface
{
public:
    HipInterface ();
    virtual ~HipInterface ();

    void getCommand(std::shared_ptr<HipSensorData> data,
                    std::shared_ptr<HipCommand> cmd);
private:
    double mTime;
    int mCount;
    double mInitTime;
    double mServoRate;
    int mNumJoint;
    Eigen::Matrix<double,6,1> mInitQ;

    void _initialize(std::shared_ptr<HipSensorData> data,
                     std::shared_ptr<HipCommand> cmd);
    void _maintainInitialPosition(std::shared_ptr<HipSensorData> data,
                                  std::shared_ptr<HipCommand> cmd);
    void _sinusoidalPosition(std::shared_ptr<HipSensorData> data,
                             std::shared_ptr<HipCommand> cmd);

    Eigen::Matrix<double,6,1> mJPosAct;
    Eigen::Matrix<double,6,1> mJVelAct;
    Eigen::Matrix<double,6,1> mJEffAct;
    Eigen::Matrix<double,6,1> mJPosDes;
    Eigen::Matrix<double,6,1> mJVelDes;
    Eigen::Matrix<double,6,1> mJEffDes;
    Eigen::MatrixXd mA;
    double mGrav;
};


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is 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.
// __END_LICENSE__

// TestImageMath.h
#include <gtest/gtest_VW.h>

#include <vw/config.h>
#include <vw/Core/FundamentalTypes.h>
#include <vw/Image/ImageMath.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/Manipulation.h>
#include <vw/Image/PixelMask.h>

#include <boost/type_traits/is_same.hpp>

using namespace vw;

template <template<class> class TraitT, class T>
static bool bool_trait( T const& /*arg*/ ) {
  return TraitT<T>::value;
}

template <class T1, class T2>
static bool has_pixel_type( T2 ) {
  return boost::is_same<T1,typename T2::pixel_type>::value;
}

// BINARY OPERATION TEST
template <class PixelT>
class ImageBinaryMathTest : public ::testing::Test {
protected:
  ImageBinaryMathTest() {}

  virtual void SetUp() {
    image_a.set_size(2,2);
    image_a(0,0)=PixelT(1); image_a(1,0)=PixelT(2);
    image_a(0,1)=PixelT(3); image_a(1,1)=PixelT(4);
    image_b.set_size(2,2);
    image_b(0,0)=PixelT(0); image_b(1,0)=PixelT(3);
    image_b(0,1)=PixelT(2); image_b(1,1)=PixelT(9);
  }

  ImageView<PixelT> image_a, image_b;
};

#define IMAGE_EXPECT_EQ( image, a, b, c, d ) \
  EXPECT_EQ( image(0,0), a );  \
  EXPECT_EQ( image(1,0), b );  \
  EXPECT_EQ( image(0,1), c );  \
  EXPECT_EQ( image(1,1), d );

TEST( ImageMath, Negation ) {
  ImageView<double> im1(2,2); im1(0,0)=1; im1(1,0)=2; im1(0,1)=3; im1(1,1)=4;

  ImageView<double> im2 = -im1;
  IMAGE_EXPECT_EQ( im2, -1, -2, -3, -4 );

  // Test the traits
  ASSERT_FALSE( bool_trait<IsMultiplyAccessible>( -im1 ) );
}

typedef ImageBinaryMathTest<double> BinaryDoubleMath;
TEST_F( BinaryDoubleMath, Sum ) {
  ImageView<double> im3 = image_a + image_b;
  IMAGE_EXPECT_EQ( im3, 1, 5, 5, 13 );
  im3 = image_a + 2;
  IMAGE_EXPECT_EQ( im3, 3, 4, 5, 6 );
  im3 = 1 + image_a;
  IMAGE_EXPECT_EQ( im3, 2, 3, 4, 5 );
  im3 += image_a;
  IMAGE_EXPECT_EQ( im3, 3, 5, 7, 9 );
  im3 += 2;
  IMAGE_EXPECT_EQ( im3, 5, 7, 9, 11 );
  (im3 += 2 ) += 2;
  IMAGE_EXPECT_EQ( im3, 9, 11, 13, 15 );
  crop(im3,0,1,2,1) += 2;
  IMAGE_EXPECT_EQ( im3, 9, 11, 15, 17 );

  // Test the traits
  ASSERT_FALSE( bool_trait<IsMultiplyAccessible>( image_a + image_b ) );
}

TEST_F( BinaryDoubleMath, Difference ) {
  ImageView<double> im3 = image_a - image_b;
  IMAGE_EXPECT_EQ( im3, 1, -1, 1, -5 );
  im3 = image_a - 2;
  IMAGE_EXPECT_EQ( im3, -1, 0, 1, 2 );
  im3 = 1 - image_a;
  IMAGE_EXPECT_EQ( im3, 0, -1, -2, -3 );
  im3 -= image_a;
  IMAGE_EXPECT_EQ( im3, -1, -3, -5, -7 );
  im3 -= 2;
  IMAGE_EXPECT_EQ( im3, -3, -5, -7, -9 );
  (im3 -= 2 ) -= 2;
  IMAGE_EXPECT_EQ( im3, -7, -9, -11, -13 );

  // Test the traits
  ASSERT_FALSE( bool_trait<IsMultiplyAccessible>( image_a - image_b ) );
}

TEST_F( BinaryDoubleMath, Product ) {
  ImageView<double> im3 = image_a * image_b;
  IMAGE_EXPECT_EQ( im3, 0, 6, 6, 36 );
  im3 = image_a * 2;
  IMAGE_EXPECT_EQ( im3, 2, 4, 6, 8 );
  im3 = 3 * image_a;
  IMAGE_EXPECT_EQ( im3, 3, 6, 9, 12 );
  im3 *= image_a;
  IMAGE_EXPECT_EQ( im3, 3, 12, 27, 48 );
  im3 *= 0.5;
  IMAGE_EXPECT_EQ( im3, 1.5, 6, 13.5, 24 );
  (im3 *= 2 ) *= 2;
  IMAGE_EXPECT_EQ( im3, 6, 24, 54, 96 );

  // Test the traits
  ASSERT_FALSE( bool_trait<IsMultiplyAccessible>( image_a * image_b ) );
}

TEST_F( BinaryDoubleMath, Quotient ) {
  ImageView<double> im3 = image_a / image_b;
  IMAGE_EXPECT_EQ( im3, 0, (2.0/3.0), 1.5, (4.0/9.0) );
  im3 = image_a / 2;
  IMAGE_EXPECT_EQ( im3, 0.5, 1, 1.5, 2 );
  im3 = 2 / image_b;
  IMAGE_EXPECT_EQ( im3, 0, (2.0/3.0), 1, (2.0/9.0) );
  im3 /= image_b;
  IMAGE_EXPECT_EQ( im3, 0, (2.0/9.0), 0.5, (2.0/81) );
  im3 /= 0.5;
  IMAGE_EXPECT_EQ( im3, 0, (4.0/9.0), 1, (4.0/81) );
  (im3 /= 1.0/9 ) /= 1.0/9;
  IMAGE_EXPECT_EQ( im3, 0, 36, 81, 4 );

  // Test the traits
  ASSERT_FALSE( bool_trait<IsMultiplyAccessible>( image_a / image_b ) );
}

typedef ImageBinaryMathTest<PixelMask<double> > BinaryMaskedDoubleMath;
TEST_F( BinaryMaskedDoubleMath, Sum ) {

  ImageView<PixelMask<double> > im3 = image_a + image_b;
  IMAGE_EXPECT_EQ( im3, 1, 5, 5, 13 );
  im3 = image_a + 2;
  IMAGE_EXPECT_EQ( im3, 3, 4, 5, 6 );
  im3 = 1 + image_a;
  IMAGE_EXPECT_EQ( im3, 2, 3, 4, 5 );
  im3 += image_a;
  IMAGE_EXPECT_EQ( im3, 3, 5, 7, 9 );
  im3 += 2;
  IMAGE_EXPECT_EQ( im3, 5, 7, 9, 11 );
  ( im3 += 2 ) -= 3;
  IMAGE_EXPECT_EQ( im3, 4, 6, 8, 10 );

  ASSERT_TRUE( is_valid( im3(0,0) ) );

  im3 += ImageView<PixelMask<double> >(2,2); // All pixel should be invalid.
  ASSERT_FALSE( is_valid( im3(0,0) ) );
}

TEST_F( BinaryMaskedDoubleMath, Quotient ) {
  ImageView<PixelMask<double> > im3 = image_a / image_b;
  IMAGE_EXPECT_EQ( im3, 0, 2.0/3.0, 1.5, 4.0/9.0 );
  im3 = image_a / 2;
  IMAGE_EXPECT_EQ( im3, 0.5, 1, 1.5, 2 );
  im3 = 2 / image_b;
  IMAGE_EXPECT_EQ( im3, 0, 2.0/3.0, 1, 2.0/9.0 );
  im3 /= image_b;
  IMAGE_EXPECT_EQ( im3, 0, 2.0/9.0, 0.5, 2.0/81 );
  im3 /= 0.5;
  IMAGE_EXPECT_EQ( im3, 0, 4.0/9.0, 1, 4.0/81 );
  (im3 /= 1.0/9 ) /= 1.0/9;
  IMAGE_EXPECT_EQ( im3, 0, 36, 81, 4 );

  // Test the traits
  ASSERT_FALSE( bool_trait<IsMultiplyAccessible>( image_a / image_b ) );
}

#define ASSERT_PRESERVED_TYPE( op ) \
  ASSERT_TRUE( has_pixel_type<float>( op(ImageView<float>()) ) ); \
  ASSERT_TRUE( has_pixel_type<double>( op(ImageView<double>()) ) ); \
  ASSERT_TRUE( has_pixel_type<long double>( op(ImageView<long double>()) ) ); \
  ASSERT_TRUE( has_pixel_type<int>( op(ImageView<int>()) ) ); \
  ASSERT_TRUE( has_pixel_type<float>( op(ImageView<std::complex<float> >()) ) ); \
  ASSERT_TRUE( has_pixel_type<double>( op(ImageView<std::complex<double> >()) ) ); \
  ASSERT_TRUE( has_pixel_type<long double>( op(ImageView<std::complex<long double> >()) ) ); \
  ASSERT_TRUE( has_pixel_type<int>( op(ImageView<std::complex<int> >()) ) ); \
  ASSERT_TRUE( has_pixel_type<PixelGray<float> >( op(ImageView<PixelGray<float> >()) ) ); \
  ASSERT_TRUE( has_pixel_type<PixelGray<int> >( op(ImageView<PixelGray<int> >()) ) ); \
  ASSERT_TRUE( has_pixel_type<PixelGray<float> >( op(ImageView<PixelGray<std::complex<float> > >()) ) ); \
  ASSERT_TRUE( has_pixel_type<PixelRGB<float> >( op(ImageView<PixelRGB<float> >()) ) ); \
  ASSERT_TRUE( has_pixel_type<PixelRGB<int> >( op(ImageView<PixelRGB<int> >()) ) ); \
  ASSERT_TRUE( has_pixel_type<PixelRGB<float> >( op(ImageView<PixelRGB<std::complex<float> > >()) ) ); \

TEST( ImageMath, Real ) {
  ImageView<std::complex<double> > im(1,1); im(0,0)=std::complex<double>(1.0,2.0);
  EXPECT_EQ( real(im).cols(), im.cols()  );
  EXPECT_EQ( real(im).rows(), im.rows()  );
  EXPECT_EQ( real(im).planes(), im.planes()  );
  EXPECT_EQ( real(im)(0,0), 1.0 );
  ASSERT_PRESERVED_TYPE( real );
}

TEST( ImageMath, Imag ) {
  ImageView<std::complex<double> > im(1,1);
  im(0,0)=std::complex<double>(1.0,2.0);
  EXPECT_EQ( imag(im).cols(), im.cols()  );
  EXPECT_EQ( imag(im).rows(), im.rows()  );
  EXPECT_EQ( imag(im).planes(), im.planes()  );
  EXPECT_EQ( imag(im)(0,0), 2.0 );
  ASSERT_PRESERVED_TYPE( imag );
}

TEST( ImageMath, Abs ) {
  ImageView<std::complex<double> > im(1,1); im(0,0)=std::complex<double>(1.0,2.0);
  EXPECT_EQ( abs(im).cols(), im.cols()  );
  EXPECT_EQ( abs(im).rows(), im.rows()  );
  EXPECT_EQ( abs(im).planes(), im.planes()  );
  EXPECT_NEAR( abs(im)(0,0), 2.23607, 1e-5 );
  ASSERT_PRESERVED_TYPE( abs );
}

TEST( ImageMath, Conj ) {
  ImageView<std::complex<double> > im(1,1); im(0,0)=std::complex<double>(1.0,2.0);
  EXPECT_EQ( conj(im).cols(), im.cols()  );
  EXPECT_EQ( conj(im).rows(), im.rows()  );
  EXPECT_EQ( conj(im).planes(), im.planes()  );
  EXPECT_EQ( conj(im)(0,0).real(), 1.0 );
  EXPECT_EQ( conj(im)(0,0).imag(), -2.0 );

  ASSERT_TRUE( has_pixel_type<float>( conj(ImageView<float>()) ) );
  ASSERT_TRUE( has_pixel_type<double>( conj(ImageView<double>()) ) );
  ASSERT_TRUE( has_pixel_type<long double>( conj(ImageView<long double>()) ) );
  ASSERT_TRUE( has_pixel_type<int>( conj(ImageView<int>()) ) );
  ASSERT_TRUE( has_pixel_type<std::complex<float> >( conj(ImageView<std::complex<float> >()) ) );
  ASSERT_TRUE( has_pixel_type<std::complex<double> >( conj(ImageView<std::complex<double> >()) ) );
  ASSERT_TRUE( has_pixel_type<std::complex< long double> >( conj(ImageView<std::complex<long double> >()) ) );
  ASSERT_TRUE( has_pixel_type<std::complex<int> >( conj(ImageView<std::complex<int> >()) ) );
}

TEST( ImageMath, Square ) {
  ImageView<double> im(2,1);
  im(0,0) = 3; im(1,0) = -4;
  EXPECT_EQ( square(im).cols(), im.cols() );
  EXPECT_EQ( square(im).rows(), im.rows() );
  EXPECT_EQ( square(im).planes(), im.planes() );
  EXPECT_EQ( square(im)(0,0), 9 );
  EXPECT_EQ( square(im)(1,0), 16 );
  ASSERT_TRUE( has_pixel_type<float>( square(ImageView<float>()) ) );
  ASSERT_TRUE( has_pixel_type<double>( square(ImageView<double>()) ) );
  ASSERT_TRUE( has_pixel_type<int32>( square(ImageView<int32>()) ) );
  ASSERT_TRUE( has_pixel_type<PixelGray<float> >( square(ImageView<PixelGray<float> >()) ) );
  ASSERT_TRUE( has_pixel_type<PixelRGB<float> >( square(ImageView<PixelRGB<float> >()) ) );
  ASSERT_TRUE( has_pixel_type<PixelGray<int32> >( square(ImageView<PixelGray<int32> >()) ) );
  ASSERT_TRUE( has_pixel_type<PixelRGB<int32> >( square(ImageView<PixelRGB<int32> >()) ) );
}

TEST( ImageMath, MaskMath ) {
  ImageView<PixelMask<PixelGray<float> > > image(2,1);
  for ( int i = 0; i < 2; i++ )
    image(i,0) = .5;
  image(1,0).invalidate();

  ASSERT_TRUE( is_valid(image(0,0)) == true );
  ASSERT_TRUE( is_valid(image(1,0)) == false );

  for ( int i = 0; i < 2; i++ )
    ASSERT_TRUE( image(i,0) == .5 );

  image += .25;
  for ( int i = 0; i < 2; i++ )
    ASSERT_TRUE( image(i,0) == .75 );

  image /= 2;
  for ( int i = 0; i < 2; i++ )
    ASSERT_TRUE( image(i,0) == .375 );

  image *= 2;
  for ( int i = 0; i < 2; i++ )
    ASSERT_TRUE( image(i,0) == .75 );

  image -= .5;
  for ( int i = 0; i < 2; i++ )
    ASSERT_TRUE( image(i,0) == .25 );

  for ( int i = 0; i < 2; i++ )
    EXPECT_EQ( image(i,0), .25 );
}

#define TEST_UNARY_MATH_FUNCTION(func,arg,result)                       \
  do {                                                                  \
    ImageView<double> im(1,1); im(0,0)=(arg);                           \
    EXPECT_EQ( func(im).cols(), im.cols() );                     \
    EXPECT_EQ( func(im).rows(), im.rows() );                     \
    EXPECT_EQ( func(im).planes(), im.planes() );                 \
    EXPECT_NEAR( func(im)(0,0), result, 1e-5 );                  \
    ASSERT_TRUE( has_pixel_type<float>( func(ImageView<float>()) ) );     \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<double>()) ) );   \
    ASSERT_TRUE( has_pixel_type<long double>( func(ImageView<long double>()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<int>()) ) );      \
  } while(false)

#define TEST_BINARY_MATH_FUNCTION(func,arg1,arg2,result)                \
  do {                                                                  \
    ImageView<double> im1(1,1); im1(0,0)=(arg1);                        \
    ImageView<double> im2(1,1); im2(0,0)=(arg2);                        \
    EXPECT_EQ( func(im1,im2).cols(), im1.cols() );               \
    EXPECT_EQ( func(im1,im2).rows(), im1.rows() );               \
    EXPECT_EQ( func(im1,im2).planes(), im1.planes() );           \
    EXPECT_NEAR( func(im1,im2)(0,0), result, 1e-5 );             \
    EXPECT_EQ( func(im1,arg2).cols(), im1.cols() );              \
    EXPECT_EQ( func(im1,arg2).rows(), im1.rows() );              \
    EXPECT_EQ( func(im1,arg2).planes(), im1.planes() );          \
    EXPECT_NEAR( func(im1,arg2)(0,0), result, 1e-5 );            \
    EXPECT_EQ( func(arg1,im2).cols(), im1.cols() );              \
    EXPECT_EQ( func(arg1,im2).rows(), im1.rows() );              \
    EXPECT_EQ( func(arg1,im2).planes(), im1.planes() );          \
    EXPECT_NEAR( func(arg1,im2)(0,0), result, 1e-5 );            \
    ASSERT_TRUE( has_pixel_type<float>( func(ImageView<float>(),ImageView<float>()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<double>(),ImageView<double>()) ) ); \
    ASSERT_TRUE( has_pixel_type<long double>( func(ImageView<long double>(),ImageView<long double>()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<int>(),ImageView<int>()) ) );  \
    ASSERT_TRUE( has_pixel_type<float>( func(ImageView<int>(),ImageView<float>()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<float>(),ImageView<double>()) ) ); \
    ASSERT_TRUE( has_pixel_type<long double>( func(ImageView<double>(),ImageView<long double>()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<char>(),ImageView<int>()) ) );  \
    ASSERT_TRUE( has_pixel_type<float>( func(ImageView<float>(),float()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<double>(),double()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<int>(),int()) ) );  \
    ASSERT_TRUE( has_pixel_type<float>( func(ImageView<int>(),float()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<float>(),double()) ) ); \
    ASSERT_TRUE( has_pixel_type<long double>( func(ImageView<long double>(),double()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(ImageView<char>(),int()) ) );  \
    ASSERT_TRUE( has_pixel_type<float>( func(float(),ImageView<float>()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(double(),ImageView<double>()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(int(),ImageView<int>()) ) );  \
    ASSERT_TRUE( has_pixel_type<float>( func(int(),ImageView<float>()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(float(),ImageView<double>()) ) ); \
    ASSERT_TRUE( has_pixel_type<long double>( func(double(),ImageView<long double>()) ) ); \
    ASSERT_TRUE( has_pixel_type<double>( func(char(),ImageView<int>()) ) );  \
  } while(false)

TEST( ImageMath, ACOS ) { TEST_UNARY_MATH_FUNCTION(acos,0.5,1.0472); }
TEST( ImageMath, ASIN ) { TEST_UNARY_MATH_FUNCTION(asin,0.5,0.523599); }
TEST( ImageMath, ATAN ) { TEST_UNARY_MATH_FUNCTION(atan,1.0,0.785398); }
TEST( ImageMath, COS ) { TEST_UNARY_MATH_FUNCTION(cos,1.0,0.540302); }
TEST( ImageMath, SIN ) { TEST_UNARY_MATH_FUNCTION(sin,1.0,0.841471); }
TEST( ImageMath, TAN ) { TEST_UNARY_MATH_FUNCTION(tan,1.0,1.55741); }
TEST( ImageMath, COSH ) { TEST_UNARY_MATH_FUNCTION(cosh,1.0,1.54308); }
TEST( ImageMath, SINH ) { TEST_UNARY_MATH_FUNCTION(sinh,1.0,1.1752); }
TEST( ImageMath, TANH ) { TEST_UNARY_MATH_FUNCTION(tanh,1.0,0.761594); }
TEST( ImageMath, EXP ) { TEST_UNARY_MATH_FUNCTION(exp,1.0,2.718281); }
TEST( ImageMath, LOG ) { TEST_UNARY_MATH_FUNCTION(log,2.0,0.693147); }
TEST( ImageMath, LOG10 ) { TEST_UNARY_MATH_FUNCTION(log10,2.0,0.30103); }
TEST( ImageMath, SQRT ) { TEST_UNARY_MATH_FUNCTION(sqrt,2.0,1.41421); }
TEST( ImageMath, CEIL ) { TEST_UNARY_MATH_FUNCTION(ceil,1.5,2.0);
  TEST_UNARY_MATH_FUNCTION(ceil,-1.5,-1.0); }
TEST( ImageMath, FLOOR ) { TEST_UNARY_MATH_FUNCTION(floor,1.5,1.0);
  TEST_UNARY_MATH_FUNCTION(floor,-1.5,-2.0); }
TEST( ImageMath, ATAN2 ) { TEST_BINARY_MATH_FUNCTION(atan2,2.0,1.0,1.10715); }
TEST( ImageMath, POW ) { TEST_BINARY_MATH_FUNCTION(pow,3.0,2.0,9.0); }

#ifndef WIN32
TEST( ImageMath, ACOSH ) { TEST_UNARY_MATH_FUNCTION(acosh,1.5,0.962424); }
TEST( ImageMath, ASINH ){ TEST_UNARY_MATH_FUNCTION(asinh,1.0,0.881374); }
TEST( ImageMath, ATANH ) { TEST_UNARY_MATH_FUNCTION(atanh,0.5,0.549306); }
TEST( ImageMath, EXP2 ) {
#ifdef VW_HAVE_EXP2
  TEST_UNARY_MATH_FUNCTION(exp2,1.0,2.0);
#endif
}
TEST( ImageMath, EXPM1 ) { TEST_UNARY_MATH_FUNCTION(expm1,1.0,1.718281); }
TEST( ImageMath, LOG2 ) {
#ifdef VW_HAVE_LOG2
  TEST_UNARY_MATH_FUNCTION(log2,2.0,1.0);
#endif
}
TEST( ImageMath, LOG1P ) { TEST_UNARY_MATH_FUNCTION(log1p,1.0,0.693147); }
TEST( ImageMath, CBRT ) { TEST_UNARY_MATH_FUNCTION(cbrt,2.0,1.25992); }
TEST( ImageMath, ERF ) { TEST_UNARY_MATH_FUNCTION(erf,1.0,0.842701); }
TEST( ImageMath, ERFC ) { TEST_UNARY_MATH_FUNCTION(erfc,1.0,0.157299); }
TEST( ImageMath, TGAMMA ) {
#ifdef VW_HAVE_TGAMMA
  TEST_UNARY_MATH_FUNCTION(tgamma,1.5,0.886227);
#endif
}
TEST( ImageMath, LGAMMA ) { TEST_UNARY_MATH_FUNCTION(lgamma,2.5,0.284683); }
TEST( ImageMath, ROUND ) { TEST_UNARY_MATH_FUNCTION(round,1.4,1.0);
  TEST_UNARY_MATH_FUNCTION(round,1.5,2.0); }
TEST( ImageMath, TRUNC ) { TEST_UNARY_MATH_FUNCTION(trunc,1.5,1.0);
  TEST_UNARY_MATH_FUNCTION(trunc,-1.5,-1.0); }

TEST( ImageMath, HYPOT ) { TEST_BINARY_MATH_FUNCTION(hypot,2.0,1.0,2.23607); }
TEST( ImageMath, COPYSIGN ) { TEST_BINARY_MATH_FUNCTION(copysign,3.0,-2.0,-3.0);
  TEST_BINARY_MATH_FUNCTION(copysign,3.0,2.0,3.0); }
TEST( ImageMath, FDIM ) { TEST_BINARY_MATH_FUNCTION(fdim,3.0,2.0,1.0);
  TEST_BINARY_MATH_FUNCTION(fdim,2.0,3.0,0.0); }
#endif


#include <random>
#include <vector>

#include <Eigen/Core>
#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/camera.h>
#include <aslam/cameras/distortion-equidistant.h>
#include <aslam/common/entrypoint.h>
#include <aslam/common/pose-types.h>
#include <eigen-checks/gtest.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <opencv2/core/core.hpp>

#include "aslam/calibration/target-algorithms.h"
#include "aslam/calibration/target-aprilgrid.h"
#include "aslam/calibration/target-observation.h"

class TargetObservationTest : public ::testing::Test {
 protected:
  virtual void SetUp() {
    const aslam::calibration::TargetAprilGrid::TargetConfiguration april_config;
    april_grid = aslam::calibration::TargetAprilGrid::Ptr(
        new aslam::calibration::TargetAprilGrid(april_config));
    camera = aslam::PinholeCamera::ConstPtr(
        aslam::PinholeCamera::createTestCamera<aslam::EquidistantDistortion>());
    setTargetObservation();
  }

  aslam::calibration::TargetBase::Ptr april_grid;
  aslam::Camera::ConstPtr camera;
  aslam::Transformation T_G_C;
  aslam::calibration::TargetObservation::ConstPtr april_grid_observation;

 private:
  void setTargetObservation() {
    CHECK(april_grid);
    CHECK(camera);
    Eigen::Matrix2Xd reprojected_corners;
    Eigen::VectorXi corner_ids;
    getReprojectedCornersAndIds(&reprojected_corners, &corner_ids);
    april_grid_observation = aslam::calibration::TargetObservation::ConstPtr(
        new aslam::calibration::TargetObservation(
            april_grid, camera->imageHeight(), camera->imageWidth(), corner_ids,
            reprojected_corners));
  }

  void getReprojectedCornersAndIds(
      Eigen::Matrix2Xd* reprojected_corners, Eigen::VectorXi* corner_ids) {
    CHECK(april_grid);
    CHECK(camera);
    CHECK_NOTNULL(reprojected_corners);
    CHECK_NOTNULL(corner_ids);
    setTargetTransformation();
    const Eigen::Matrix3Xd corner_points_G = april_grid->points();
    const Eigen::Matrix3Xd corner_points_C =
        T_G_C.inverse().transformVectorized(corner_points_G);
    Eigen::Matrix2Xd corner_points_reprojected;
    std::vector<aslam::ProjectionResult> projection_results;
    Eigen::Matrix2Xd reprojected_corners_without_noise;
    camera->project3Vectorized(
        corner_points_C, &reprojected_corners_without_noise,
        &projection_results);
    CHECK(isReprojectionValid(projection_results));
    corruptWithGaussianNoise(
        reprojected_corners_without_noise, reprojected_corners);
    *corner_ids = Eigen::VectorXi(reprojected_corners->cols());
    for (int index = 0; index < corner_ids->size(); ++index) {
      (*corner_ids)(index) = index;
    }
    CHECK_EQ(reprojected_corners->cols(), corner_ids->size());
  }

  void setTargetTransformation() {
    constexpr double kDistanceFromTargetMeters = 0.5;
    T_G_C = aslam::Transformation(
        aslam::Quaternion(0.0, 1.0, 0.0, 0.0),
        aslam::Position3D(
            0.5 * april_grid->width(), 0.5 * april_grid->height(),
            kDistanceFromTargetMeters));
  }

  bool isReprojectionValid(
      const std::vector<aslam::ProjectionResult>& projection_results) const {
    for (const aslam::ProjectionResult& projection_result :
         projection_results) {
      if (!projection_result) {
        return false;
      }
    }
    return true;
  }

  void corruptWithGaussianNoise(
      const Eigen::Matrix2Xd& image_points,
      Eigen::Matrix2Xd* image_points_corrupted) {
    const size_t num_rows = image_points.rows();
    CHECK_EQ(num_rows, 2u);
    const size_t num_cols = image_points.cols();
    *CHECK_NOTNULL(image_points_corrupted) = image_points;
    std::random_device random_device{};
    std::mt19937 random_generator{random_device()};
    constexpr double kStandardDeviationPixels = 0.5;
    std::normal_distribution<> normal_distribution{0.0,
                                                   kStandardDeviationPixels};
    for (size_t row_index = 0u; row_index < num_rows; ++row_index) {
      for (size_t col_index = 0u; col_index < num_cols; ++col_index) {
        (*image_points_corrupted)(row_index, col_index) +=
            normal_distribution(random_generator);
      }
    }
  }

  void drawCornersIntoImage() {
    cv::Mat out_image(
        camera->imageHeight(), camera->imageWidth(), CV_8UC3,
        cv::Scalar(255, 255, 255));
    april_grid_observation->drawCornersIntoImage(&out_image);
    cv::imwrite("simulated_grid_corners.png", out_image);
  }
};

// Simulates a pinhole camera observing an AprilTag grid. The reprojected
// corners of the target are corrupted with Gaussian noise, and this test
// verifies that the pnp problem achieves a reasonable accuracy when estimating
// the transform of the camera w.r.t. the target origin.
TEST_F(TargetObservationTest, AprilGridPoseEstimation) {
  constexpr double kTolerancePositionMeters = 0.03;
  constexpr double kToleranceRotationRad = 0.03;
  aslam::Transformation T_G_Cest;
  ASSERT_TRUE(
      aslam::calibration::estimateTargetTransformation(
          *april_grid_observation, camera, &T_G_Cest));
  const aslam::Transformation T_C_Cest = T_G_C.inverse() * T_G_Cest;
  const double position_error_meters = T_C_Cest.getPosition().norm();
  EXPECT_LT(position_error_meters, kTolerancePositionMeters);
  VLOG(1) << "Position error of target pose estimation in meters: "
          << position_error_meters;
  const aslam::AngleAxis angle_axis(T_C_Cest.getRotation());
  const double angle_error_rad = angle_axis.angle();
  EXPECT_LT(angle_error_rad, kToleranceRotationRad);
  VLOG(1) << "Angle error of target pose estimation in radians: "
          << angle_error_rad;
}

ASLAM_UNITTEST_ENTRYPOINT


#include "alpha_shapes.hpp"
#include <boost/lexical_cast.hpp>
#include <iostream>

int main(int argc, char* argv[]){
	if(argc != 2){
		std::cout << "usage: " << argv[0] << " <alpha>" << std::endl;
		return 1;
	}
	double alpha = boost::lexical_cast<double>(argv[1]);

	unsigned int num_edges, num_triangles, num_tetrahedra;
	double points[] = {
		.2,.2,.2,
		1,0,0,
		0,1,0,
		0,0,1,
		.7,.7,.7
	};
	double weights[] = {
		1, 1, 1, 1, 1
	};
	unsigned int* edges;
	unsigned int* triangles;
	unsigned int* tetrahedra;

	alpha_shapes(
		5, alpha, points, weights,
		&num_edges, &edges,
		&num_triangles, &triangles,
		&num_tetrahedra, &tetrahedra
	);

	std::cout << "Tetra " << num_tetrahedra << std::endl;
	for(unsigned int i = 0; i < num_tetrahedra*4; i+=4){
		for(unsigned int j = 0; j < 4; ++j){
			std::cout << "\t" << tetrahedra[i+j];
		}
		std::cout << std::endl;
	}

	std::cout << "Tri " << num_triangles << std::endl;
	for(unsigned int i = 0; i < num_triangles*3; i+=3){
		for(unsigned int j = 0; j < 3; ++j){
			std::cout << "\t" << triangles[i+j];
		}
		std::cout << std::endl;
	}

	std::cout << "Edge " << num_edges << std::endl;
	for(unsigned int i = 0; i < num_edges*2; i+=2){
		for(unsigned int j = 0; j < 2; ++j){
			std::cout << "\t" << edges[i+j];
		}
		std::cout << std::endl;
	}

	return 0;
}


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   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 <simd_bench.hpp>
#include <boost/simd/function/simd/cast.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/pack.hpp>

namespace nsb = ns::bench;
namespace bs =  boost::simd;
namespace bd =  boost::dispatch;

struct caster
{
  template<class T> float operator()(const T & a) const
  {
    return bs::cast<T>(a);
  }
};

DEFINE_SIMD_BENCH(simd_caster, caster());
DEFINE_SIMD_BENCH(simd_castf, castf());

DEFINE_BENCH_MAIN()
{
  nsb::for_each<simd_caster, int8_t, int8_t, uint16_t, uint16_t, nt32_t, int64_t, uint32_t, uint64_t, float, double>(-10, 10);
}


#include <tiny_math_types.h>

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

template<typename math_types>
inline void compile_test_math_types()
{
  typedef typename math_types::real_type           real_type;
  typedef typename math_types::vector3_type        vector3_type;
  typedef typename math_types::matrix3x3_type      matrix3x3_type;
  typedef typename math_types::quaternion_type     quaternion_type;
  typedef typename math_types::coordsys_type       coordsys_type;
  typedef typename math_types::value_traits        value_traits;

  vector3_type     v;
  matrix3x3_type   m;
  quaternion_type  q;
  coordsys_type    c;

  real_type s1 = value_traits::zero();
  real_type s2 = value_traits::one();
  real_type s3 = value_traits::two();
  real_type s4 = value_traits::pi();
  real_type s5 = value_traits::pi_2();
  real_type s6 = value_traits::infinity();

  BOOST_CHECK( s1 == value_traits::zero() );
  BOOST_CHECK( s2 == value_traits::one() );
  BOOST_CHECK( s3 == value_traits::two() );
  BOOST_CHECK( s4 == value_traits::pi() );
  BOOST_CHECK( s5 == value_traits::pi_2() );
  BOOST_CHECK( s6 == value_traits::infinity() );
}

BOOST_AUTO_TEST_SUITE(tiny_math_types);

		BOOST_AUTO_TEST_CASE(type_and_member_compile_test)
		{
      //--- Compile testing that we can instantiate most common types and access members
      typedef tiny::MathTypes<float > type1;
      typedef tiny::MathTypes<double > type2;
      void (*ptr1)() = &(compile_test_math_types<type1>);
      void (*ptr2)() = &(compile_test_math_types<type2>);
      ptr1 = 0;
      ptr2 = 0;
    }

BOOST_AUTO_TEST_SUITE_END();


#include "places.h"
#include "xmlrpc_helpers.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

Places::Places(ros::NodeHandle & nh, ros::NodeHandle & nh_priv,
    const std::string & param_name) {

    xh::Array output;
    xh::fetchParam(nh_priv, param_name, output);
    xh::Struct output_i;

    for (int i = 0; i < output.size(); ++i)
    {
        XmlRpc::XmlRpcValue name_list;
        XmlRpc::XmlRpcValue pos_list;
        std::vector<geometry_msgs::Point> pos;

        xh::getArrayItem(output, i, output_i);
        xh::getStructMember(output_i, "boundary", pos_list);
        xh::getStructMember(output_i, "name", name_list);

        for(int j=0; j < pos_list.size(); ++j)
        {
            geometry_msgs::Point pt;

            pt.x = pos_list[j][0];
            pt.y = pos_list[j][1];

            pos.push_back(pt);
        }

        boundary_.push_back(pos);
        places_.push_back(name_list);
    }
}

std::vector<std::string> Places::getPlaces()
{
    return places_;
}

std::vector<std::vector<geometry_msgs::Point> > Places::getBoundary()
{
    return boundary_;
}

std::string Places::where_am_i(geometry_msgs::Point pt)
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;
    typedef boost::geometry::model::polygon<point_type> polygon_type;

    for(int i = 0; i < boundary_.size() ; i++)
    {
        std::string polygon = "POLYGON((";

        for(int j=0; j < boundary_[i].size(); j++)
        {
            polygon.append(std::to_string(boundary_[i][j].y));
            polygon.append(" ");
            polygon.append(std::to_string(boundary_[i][j].x));
            polygon.append(",");
        }

        polygon.append(std::to_string(boundary_[i][0].y));
        polygon.append(" ");
        polygon.append(std::to_string(boundary_[i][0].x));
        polygon.append("))");

        polygon_type poly;
        boost::geometry::read_wkt(polygon, poly);

        point_type p(pt.y, pt.x);

        if(boost::geometry::within(p, poly))
            return places_[i];
    }
    return "";
}