/* Author: Moritz Allmaras, Texas A&M University, 2007 */

/* $Id: step-29.cc 27661 2012-11-21 14:38:52Z bangerth $ */
/*    Copyright (C) 2007-2008, 2010-2012 by the deal.II authors and M. Allmaras   */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the text and       */
/*    further information on this license.                        */

// @sect3{Include files}

// The following header files are unchanged from step-7 and have been
// discussed before:

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/tria_boundary_lib.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/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

#include <fstream>

// This header file contains the necessary declarations for the
// ParameterHandler class that we will use to read our parameters from a
// configuration file:
#include <deal.II/base/parameter_handler.h>

// For solving the linear system, we'll use the sparse LU-decomposition
// provided by UMFPACK (see the SparseDirectUMFPACK class), for which the
// following header file is needed.  Note that in order to compile this
// tutorial program, the deal.II-library needs to be built with UMFPACK
// support, which can be most easily achieved by giving the
// --with-umfpack switch when configuring the library:
#include <deal.II/lac/sparse_direct.h>

// The FESystem class allows us to stack several FE-objects to one compound,
// vector-valued finite element field. The necessary declarations for this
// class are provided in this header file:
#include <deal.II/fe/fe_system.h>

// Finally, include the header file that declares the Timer class that we will
// use to determine how much time each of the operations of our program takes:
#include <deal.II/base/timer.h>

// As the last step at the beginning of this program, we put everything that
// is in this program into its namespace and, within it, make everything that
// is in the deal.II namespace globally available, without the need to prefix
// everything with dealii:::
namespace Step29
{
  using namespace dealii;

  // @sect3{The DirichletBoundaryValues class}

  // First we define a class for the function representing the Dirichlet
  // boundary values. This has been done many times before and therefore does
  // not need much explanation.
  //
  // Since there are two values $v$ and $w$ that need to be prescribed at the
  // boundary, we have to tell the base class that this is a vector-valued
  // function with two components, and the vector_value function
  // and its cousin vector_value_list must return vectors with
  // two entries. In our case the function is very simple, it just returns 1
  // for the real part $v$ and 0 for the imaginary part $w$ regardless of the
  // point where it is evaluated.
  template <int dim>
  class DirichletBoundaryValues : public Function<dim>
  {
  public:
    DirichletBoundaryValues() : Function<dim> (2) {};

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

    virtual void vector_value_list (const std::vector<Point<dim> > &points,
                                    std::vector<Vector<double> >   &value_list) const;
  };

  template <int dim>
  inline
  void DirichletBoundaryValues<dim>::vector_value (const Point<dim> & /*p*/,
                                                   Vector<double>   &values) const
  {
    Assert (values.size() == 2, ExcDimensionMismatch (values.size(), 2));

    values(0) = 1;
    values(1) = 0;
  }

  template <int dim>
  void DirichletBoundaryValues<dim>::vector_value_list (const std::vector<Point<dim> > &points,
                                                        std::vector<Vector<double> >   &value_list) const
  {
    Assert (value_list.size() == points.size(),
            ExcDimensionMismatch (value_list.size(), points.size()));

    for (unsigned int p=0; p<points.size(); ++p)
      DirichletBoundaryValues<dim>::vector_value (points[p], value_list[p]);
  }

  // @sect3{The ParameterReader class}

  // The next class is responsible for preparing the ParameterHandler object
  // and reading parameters from an input file.  It includes a function
  // declare_parameters that declares all the necessary
  // parameters and a read_parameters function that is called
  // from outside to initiate the parameter reading process.
  class ParameterReader : public Subscriptor
  {
  public:
    ParameterReader(ParameterHandler &);
    void read_parameters(const std::string);

  private:
    void declare_parameters();
    ParameterHandler &prm;
  };

  // The constructor stores a reference to the ParameterHandler object that is
  // passed to it:
  ParameterReader::ParameterReader(ParameterHandler &paramhandler)
    :
    prm(paramhandler)
  {}

  // @sect4{ParameterReader::declare_parameters}

  // The declare_parameters function declares all the parameters
  // that our ParameterHandler object will be able to read from input files,
  // along with their types, range conditions and the subsections they appear
  // in. We will wrap all the entries that go into a section in a pair of
  // braces to force the editor to indent them by one level, making it simpler
  // to read which entries together form a section:
  void ParameterReader::declare_parameters()
  {
    // Parameters for mesh and geometry include the number of global
    // refinement steps that are applied to the initial coarse mesh and the
    // focal distance $d$ of the transducer lens. For the number of refinement
    // steps, we allow integer values in the range $[0,\infty)$, where the
    // omitted second argument to the Patterns::Integer object denotes the
    // half-open interval.  For the focal distance any number greater than
    // zero is accepted:
    prm.enter_subsection ("Mesh & geometry parameters");
    {
      prm.declare_entry("Number of refinements", "6",
                        Patterns::Integer(0),
                        "Number of global mesh refinement steps "
                        "applied to initial coarse grid");

      prm.declare_entry("Focal distance", "0.3",
                        Patterns::Double(0),
                        "Distance of the focal point of the lens "
                        "to the x-axis");
    }
    prm.leave_subsection ();

    // The next subsection is devoted to the physical parameters appearing in
    // the equation, which are the frequency $\omega$ and wave speed
    // $c$. Again, both need to lie in the half-open interval $[0,\infty)$
    // represented by calling the Patterns::Double class with only the left
    // end-point as argument:
    prm.enter_subsection ("Physical constants");
    {
      prm.declare_entry("c", "1.5e5",
                        Patterns::Double(0),
                        "Wave speed");

      prm.declare_entry("omega", "5.0e7",
                        Patterns::Double(0),
                        "Frequency");
    }
    prm.leave_subsection ();

    // Last but not least we would like to be able to change some properties
    // of the output, like filename and format, through entries in the
    // configuration file, which is the purpose of the last subsection:
    prm.enter_subsection ("Output parameters");
    {
      prm.declare_entry("Output file", "solution",
                        Patterns::Anything(),
                        "Name of the output file (without extension)");

      // Since different output formats may require different parameters for
      // generating output (like for example, postscript output needs
      // viewpoint angles, line widths, colors etc), it would be cumbersome if
      // we had to declare all these parameters by hand for every possible
      // output format supported in the library. Instead, each output format
      // has a FormatFlags::declare_parameters function, which
      // declares all the parameters specific to that format in an own
      // subsection. The following call of
      // DataOutInterface<1>::declare_parameters executes
      // declare_parameters for all available output formats, so
      // that for each format an own subsection will be created with
      // parameters declared for that particular output format. (The actual
      // value of the template parameter in the call, @<1@>
      // above, does not matter here: the function does the same work
      // independent of the dimension, but happens to be in a
      // template-parameter-dependent class.)  To find out what parameters
      // there are for which output format, you can either consult the
      // documentation of the DataOutBase class, or simply run this program
      // without a parameter file present. It will then create a file with all
      // declared parameters set to their default values, which can
      // conveniently serve as a starting point for setting the parameters to
      // the values you desire.
      DataOutInterface<1>::declare_parameters (prm);
    }
    prm.leave_subsection ();
  }

  // @sect4{ParameterReader::read_parameters}

  // This is the main function in the ParameterReader class.  It gets called
  // from outside, first declares all the parameters, and then reads them from
  // the input file whose filename is provided by the caller. After the call
  // to this function is complete, the prm object can be used to
  // retrieve the values of the parameters read in from the file:
  void ParameterReader::read_parameters (const std::string parameter_file)
  {
    declare_parameters();

    prm.read_input (parameter_file);
  }

  // @sect3{The ComputeIntensity class}

  // As mentioned in the introduction, the quantity that we are really after
  // is the spatial distribution of the intensity of the ultrasound wave,
  // which corresponds to $|u|=\sqrt{v^2+w^2}$. Now we could just be content
  // with having $v$ and $w$ in our output, and use a suitable visualization
  // or postprocessing tool to derive $|u|$ from the solution we
  // computed. However, there is also a way to output data derived from the
  // solution in deal.II, and we are going to make use of this mechanism here.

  // So far we have always used the DataOut::add_data_vector function to add
  // vectors containing output data to a DataOut object.  There is a special
  // version of this function that in addition to the data vector has an
  // additional argument of type DataPostprocessor. What happens when this
  // function is used for output is that at each point where output data is to
  // be generated, the DataPostprocessor::compute_derived_quantities_scalar or
  // DataPostprocessor::compute_derived_quantities_vector function of the
  // specified DataPostprocessor object is invoked to compute the output
  // quantities from the values, the gradients and the second derivatives of
  // the finite element function represented by the data vector (in the case
  // of face related data, normal vectors are available as well). Hence, this
  // allows us to output any quantity that can locally be derived from the
  // values of the solution and its derivatives.  Of course, the ultrasound
  // intensity $|u|$ is such a quantity and its computation doesn't even
  // involve any derivatives of $v$ or $w$.

  // In practice, the DataPostprocessor class only provides an interface to
  // this functionality, and we need to derive our own class from it in order
  // to implement the functions specified by the interface. In the most
  // general case one has to implement several member functions but if the
  // output quantity is a single scalar then some of this boilerplate code can
  // be handled by a more specialized class, DataPostprocessorScalar and we
  // can derive from that one instead. This is what the
  // ComputeIntensity class does:
  template <int dim>
  class ComputeIntensity : public DataPostprocessorScalar<dim>
  {
  public:
    ComputeIntensity ();

    virtual
    void
    compute_derived_quantities_vector (const std::vector< Vector< double > > &uh,
                                       const std::vector< std::vector< Tensor< 1, dim > > > &duh,
                                       const std::vector< std::vector< Tensor< 2, dim > > > &dduh,
                                       const std::vector< Point< dim > > &normals,
                                       const std::vector<Point<dim> > &evaluation_points,
                                       std::vector< Vector< double > > &computed_quantities) const;
  };

  // In the constructor, we need to call the constructor of the base class
  // with two arguments. The first denotes the name by which the single scalar
  // quantity computed by this class should be represented in output files. In
  // our case, the postprocessor has $|u|$ as output, so we use "Intensity".
  //
  // The second argument is a set of flags that indicate which data is needed
  // by the postprocessor in order to compute the output quantities.  This can
  // be any subset of update_values, update_gradients and update_hessians
  // (and, in the case of face data, also update_normal_vectors), which are
  // documented in UpdateFlags.  Of course, computation of the derivatives
  // requires additional resources, so only the flags for data that is really
  // needed should be given here, just as we do when we use FEValues objects.
  // In our case, only the function values of $v$ and $w$ are needed to
  // compute $|u|$, so we're good with the update_values flag.
  template <int dim>
  ComputeIntensity<dim>::ComputeIntensity ()
    :
    DataPostprocessorScalar<dim> ("Intensity",
                                  update_values)
  {}

  // The actual prostprocessing happens in the following function.  Its inputs
  // are a vector representing values of the function (which is here
  // vector-valued) representing the data vector given to
  // DataOut::add_data_vector, evaluated at all evaluation points where we
  // generate output, and some tensor objects representing derivatives (that
  // we don't use here since $|u|$ is computed from just $v$ and $w$, and for
  // which we assign no name to the corresponding function argument).  The
  // derived quantities are returned in the computed_quantities
  // vector.  Remember that this function may only use data for which the
  // respective update flag is specified by
  // get_needed_update_flags. For example, we may not use the
  // derivatives here, since our implementation of
  // get_needed_update_flags requests that only function values
  // are provided.
  template <int dim>
  void
  ComputeIntensity<dim>::compute_derived_quantities_vector (
    const std::vector< Vector< double > >                  &uh,
    const std::vector< std::vector< Tensor< 1, dim > > >  & /*duh*/,
    const std::vector< std::vector< Tensor< 2, dim > > >  & /*dduh*/,
    const std::vector< Point< dim > >                     & /*normals*/,
    const std::vector<Point<dim> >                        & /*evaluation_points*/,
    std::vector< Vector< double > >                        &computed_quantities
  ) const
  {
    Assert(computed_quantities.size() == uh.size(),
           ExcDimensionMismatch (computed_quantities.size(), uh.size()));

    // The computation itself is straightforward: We iterate over each entry
    // in the output vector and compute $|u|$ from the corresponding values of
    // $v$ and $w$:
    for (unsigned int i=0; i<computed_quantities.size(); i++)
      {
        Assert(computed_quantities[i].size() == 1,
               ExcDimensionMismatch (computed_quantities[i].size(), 1));
        Assert(uh[i].size() == 2, ExcDimensionMismatch (uh[i].size(), 2));

        computed_quantities[i](0) = sqrt(uh[i](0)*uh[i](0) + uh[i](1)*uh[i](1));
      }
  }

  // @sect3{The UltrasoundProblem class}

  // Finally here is the main class of this program.  It's member functions
  // are very similar to the previous examples, in particular step-4, and the
  // list of member variables does not contain any major surprises either.
  // The ParameterHandler object that is passed to the constructor is stored
  // as a reference to allow easy access to the parameters from all functions
  // of the class.  Since we are working with vector valued finite elements,
  // the FE object we are using is of type FESystem.
  template <int dim>
  class UltrasoundProblem
  {
  public:
    UltrasoundProblem (ParameterHandler &);
    ~UltrasoundProblem ();
    void run ();

  private:
    void make_grid ();
    void setup_system ();
    void assemble_system ();
    void solve ();
    void output_results () const;

    ParameterHandler      &prm;

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

    SparsityPattern        sparsity_pattern;
    SparseMatrix<double>   system_matrix;
    Vector<double>         solution, system_rhs;
  };

  // The constructor takes the ParameterHandler object and stores it in a
  // reference. It also initializes the DoF-Handler and the finite element
  // system, which consists of two copies of the scalar Q1 field, one for $v$
  // and one for $w$:
  template <int dim>
  UltrasoundProblem<dim>::UltrasoundProblem (ParameterHandler &param)
    :
    prm(param),
    dof_handler(triangulation),
    fe(FE_Q<dim>(1), 2)
  {}

  template <int dim>
  UltrasoundProblem<dim>::~UltrasoundProblem ()
  {
    dof_handler.clear();
  }

  // @sect4{UltrasoundProblem::make_grid}

  // Here we setup the grid for our domain.  As mentioned in the exposition,
  // the geometry is just a unit square (in 2d) with the part of the boundary
  // that represents the transducer lens replaced by a sector of a circle.
  template <int dim>
  void UltrasoundProblem<dim>::make_grid ()
  {
    // First we generate some logging output and start a timer so we can
    // compute execution time when this function is done:
    deallog << "Generating grid... ";
    Timer timer;
    timer.start ();

    // Then we query the values for the focal distance of the transducer lens
    // and the number of mesh refinement steps from our ParameterHandler
    // object:
    prm.enter_subsection ("Mesh & geometry parameters");

    const double                focal_distance = prm.get_double("Focal distance");
    const unsigned int  n_refinements  = prm.get_integer("Number of refinements");

    prm.leave_subsection ();

    // Next, two points are defined for position and focal point of the
    // transducer lens, which is the center of the circle whose segment will
    // form the transducer part of the boundary. We compute the radius of this
    // circle in such a way that the segment fits in the interval [0.4,0.6] on
    // the x-axis.  Notice that this is the only point in the program where
    // things are slightly different in 2D and 3D.  Even though this tutorial
    // only deals with the 2D case, the necessary additions to make this
    // program functional in 3D are so minimal that we opt for including them:
    const Point<dim>    transducer = (dim == 2) ?
                                     Point<dim> (0.5, 0.0) :
                                     Point<dim> (0.5, 0.5, 0.0),
                                     focal_point = (dim == 2) ?
                                                   Point<dim> (0.5, focal_distance) :
                                                   Point<dim> (0.5, 0.5, focal_distance);

    const double radius = std::sqrt( (focal_point.distance(transducer) *
                                      focal_point.distance(transducer)) +
                                     ((dim==2) ? 0.01 : 0.02));

    // As initial coarse grid we take a simple unit square with 5 subdivisions
    // in each direction. The number of subdivisions is chosen so that the
    // line segment $[0.4,0.6]$ that we want to designate as the transducer
    // boundary is spanned by a single face. Then we step through all cells to
    // find the faces where the transducer is to be located, which in fact is
    // just the single edge from 0.4 to 0.6 on the x-axis. This is where we
    // want the refinements to be made according to a circle shaped boundary,
    // so we mark this edge with a different boundary indicator.
    GridGenerator::subdivided_hyper_cube (triangulation, 5, 0, 1);

    typename Triangulation<dim>::cell_iterator
    cell = triangulation.begin (),
    endc = triangulation.end();

    for (; cell!=endc; ++cell)
      for (unsigned int face=0; face<GeometryInfo<dim>::faces_per_cell; ++face)
        if ( cell->face(face)->at_boundary() &&
             ((cell->face(face)->center() - transducer).square() < 0.01) )

          cell->face(face)->set_boundary_indicator (1);

    // For the circle part of the transducer lens, a hyper-ball object is used
    // (which, of course, in 2D just represents a circle), with radius and
    // center as computed above. By marking this object as
    // static, we ensure that it lives until the end of the
    // program and thereby longer than the triangulation object we will
    // associated with it. We then assign this boundary-object to the part of
    // the boundary with boundary indicator 1:
    static const HyperBallBoundary<dim> boundary(focal_point, radius);
    triangulation.set_boundary(1, boundary);

    // Now global refinement is executed. Cells near the transducer location
    // will be automatically refined according to the circle shaped boundary
    // of the transducer lens:
    triangulation.refine_global (n_refinements);

    // Lastly, we generate some more logging output. We stop the timer and
    // query the number of CPU seconds elapsed since the beginning of the
    // function:
    timer.stop ();
    deallog << "done ("
            << timer()
            << "s)"
            << std::endl;

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

  // @sect4{UltrasoundProblem::setup_system}
  //
  // Initialization of the system matrix, sparsity patterns and vectors are
  // the same as in previous examples and therefore do not need further
  // comment. As in the previous function, we also output the run time of what
  // we do here:
  template <int dim>
  void UltrasoundProblem<dim>::setup_system ()
  {
    deallog << "Setting up system... ";
    Timer timer;
    timer.start();

    dof_handler.distribute_dofs (fe);

    sparsity_pattern.reinit (dof_handler.n_dofs(),
                             dof_handler.n_dofs(),
                             dof_handler.max_couplings_between_dofs());

    DoFTools::make_sparsity_pattern (dof_handler, sparsity_pattern);
    sparsity_pattern.compress();

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

    timer.stop ();
    deallog << "done ("
            << timer()
            << "s)"
            << std::endl;

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

  // @sect4{UltrasoundProblem::assemble_system}

  // As before, this function takes care of assembling the system matrix and
  // right hand side vector:
  template <int dim>
  void UltrasoundProblem<dim>::assemble_system ()
  {
    deallog << "Assembling system matrix... ";
    Timer timer;
    timer.start ();

    // First we query wavespeed and frequency from the ParameterHandler object
    // and store them in local variables, as they will be used frequently
    // throughout this function.

    prm.enter_subsection ("Physical constants");

    const double omega = prm.get_double("omega"),
                 c     = prm.get_double("c");

    prm.leave_subsection ();

    // As usual, for computing integrals ordinary Gauss quadrature rule is
    // used. Since our bilinear form involves boundary integrals on
    // $\Gamma_2$, we also need a quadrature rule for surface integration on
    // the faces, which are $dim-1$ dimensional:
    QGauss<dim>    quadrature_formula(2);
    QGauss<dim-1>  face_quadrature_formula(2);

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

    // The FEValues objects will evaluate the shape functions for us.  For the
    // part of the bilinear form that involves integration on $\Omega$, we'll
    // need the values and gradients of the shape functions, and of course the
    // quadrature weights.  For the terms involving the boundary integrals,
    // only shape function values and the quadrature weights are necessary.
    FEValues<dim>  fe_values (fe, quadrature_formula,
                              update_values | update_gradients |
                              update_JxW_values);

    FEFaceValues<dim> fe_face_values (fe, face_quadrature_formula,
                                      update_values | update_JxW_values);

    // As usual, the system matrix is assembled cell by cell, and we need a
    // matrix for storing the local cell contributions as well as an index
    // vector to transfer the cell contributions to the appropriate location
    // in the global system matrix after.
    FullMatrix<double> cell_matrix (dofs_per_cell, dofs_per_cell);
    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();

    for (; cell!=endc; ++cell)
      {

        // On each cell, we first need to reset the local contribution matrix
        // and request the FEValues object to compute the shape functions for
        // the current cell:
        cell_matrix = 0;
        fe_values.reinit (cell);

        for (unsigned int i=0; i<dofs_per_cell; ++i)
          {
            for (unsigned int j=0; j<dofs_per_cell; ++j)
              {

                // At this point, it is important to keep in mind that we are
                // dealing with a finite element system with two
                // components. Due to the way we constructed this FESystem,
                // namely as the cartesian product of two scalar finite
                // element fields, each shape function has only a single
                // nonzero component (they are, in deal.II lingo, @ref
                // GlossPrimitive "primitive").  Hence, each shape function
                // can be viewed as one of the $\phi$'s or $\psi$'s from the
                // introduction, and similarly the corresponding degrees of
                // freedom can be attributed to either $\alpha$ or $\beta$.
                // As we iterate through all the degrees of freedom on the
                // current cell however, they do not come in any particular
                // order, and so we cannot decide right away whether the DoFs
                // with index $i$ and $j$ belong to the real or imaginary part
                // of our solution.  On the other hand, if you look at the
                // form of the system matrix in the introduction, this
                // distinction is crucial since it will determine to which
                // block in the system matrix the contribution of the current
                // pair of DoFs will go and hence which quantity we need to
                // compute from the given two shape functions.  Fortunately,
                // the FESystem object can provide us with this information,
                // namely it has a function
                // FESystem::system_to_component_index, that for each local
                // DoF index returns a pair of integers of which the first
                // indicates to which component of the system the DoF
                // belongs. The second integer of the pair indicates which
                // index the DoF has in the scalar base finite element field,
                // but this information is not relevant here. If you want to
                // know more about this function and the underlying scheme
                // behind primitive vector valued elements, take a look at
                // step-8 or the @ref vector_valued module, where these topics
                // are explained in depth.
                if (fe.system_to_component_index(i).first ==
                    fe.system_to_component_index(j).first)
                  {

                    // If both DoFs $i$ and $j$ belong to same component,
                    // i.e. their shape functions are both $\phi$'s or both
                    // $\psi$'s, the contribution will end up in one of the
                    // diagonal blocks in our system matrix, and since the
                    // corresponding entries are computed by the same formula,
                    // we do not bother if they actually are $\phi$ or $\psi$
                    // shape functions. We can simply compute the entry by
                    // iterating over all quadrature points and adding up
                    // their contributions, where values and gradients of the
                    // shape functions are supplied by our FEValues object.

                    for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
                      cell_matrix(i,j) += (((fe_values.shape_value(i,q_point) *
                                             fe_values.shape_value(j,q_point)) *
                                            (- omega * omega)
                                            +
                                            (fe_values.shape_grad(i,q_point) *
                                             fe_values.shape_grad(j,q_point)) *
                                            c * c) *
                                           fe_values.JxW(q_point));

                    // You might think that we would have to specify which
                    // component of the shape function we'd like to evaluate
                    // when requesting shape function values or gradients from
                    // the FEValues object. However, as the shape functions
                    // are primitive, they have only one nonzero component,
                    // and the FEValues class is smart enough to figure out
                    // that we are definitely interested in this one nonzero
                    // component.
                  }
              }
          }

        // We also have to add contributions due to boundary terms. To this
        // end, we loop over all faces of the current cell and see if first it
        // is at the boundary, and second has the correct boundary indicator
        // associated with $\Gamma_2$, the part of the boundary where we have
        // absorbing boundary conditions:
        for (unsigned int face=0; face<GeometryInfo<dim>::faces_per_cell; ++face)
          if (cell->face(face)->at_boundary() &&
              (cell->face(face)->boundary_indicator() == 0) )
            {

              // These faces will certainly contribute to the off-diagonal
              // blocks of the system matrix, so we ask the FEFaceValues
              // object to provide us with the shape function values on this
              // face:
              fe_face_values.reinit (cell, face);

              // Next, we loop through all DoFs of the current cell to find
              // pairs that belong to different components and both have
              // support on the current face:
              for (unsigned int i=0; i<dofs_per_cell; ++i)
                for (unsigned int j=0; j<dofs_per_cell; ++j)
                  if ((fe.system_to_component_index(i).first !=
                       fe.system_to_component_index(j).first) &&
                      fe.has_support_on_face(i, face) &&
                      fe.has_support_on_face(j, face))
                    // The check whether shape functions have support on a
                    // face is not strictly necessary: if we don't check for
                    // it we would simply add up terms to the local cell
                    // matrix that happen to be zero because at least one of
                    // the shape functions happens to be zero. However, we can
                    // save that work by adding the checks above.

                    // In either case, these DoFs will contribute to the
                    // boundary integrals in the off-diagonal blocks of the
                    // system matrix. To compute the integral, we loop over
                    // all the quadrature points on the face and sum up the
                    // contribution weighted with the quadrature weights that
                    // the face quadrature rule provides.  In contrast to the
                    // entries on the diagonal blocks, here it does matter
                    // which one of the shape functions is a $\psi$ and which
                    // one is a $\phi$, since that will determine the sign of
                    // the entry.  We account for this by a simple conditional
                    // statement that determines the correct sign. Since we
                    // already checked that DoF $i$ and $j$ belong to
                    // different components, it suffices here to test for one
                    // of them to which component it belongs.
                    for (unsigned int q_point=0; q_point<n_face_q_points; ++q_point)
                      cell_matrix(i,j) += ((fe.system_to_component_index(i).first == 0) ? -1 : 1) *
                                          fe_face_values.shape_value(i,q_point) *
                                          fe_face_values.shape_value(j,q_point) *
                                          c *
                                          omega *
                                          fe_face_values.JxW(q_point);
            }

        // Now we are done with this cell and have to transfer its
        // contributions from the local to the global system matrix. To this
        // end, we first get a list of the global indices of the this cells
        // DoFs...
        cell->get_dof_indices (local_dof_indices);

        // ...and then add the entries to the system matrix one by one:
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          for (unsigned int j=0; j<dofs_per_cell; ++j)
            system_matrix.add (local_dof_indices[i],
                               local_dof_indices[j],
                               cell_matrix(i,j));
      }

    // The only thing left are the Dirichlet boundary values on $\Gamma_1$,
    // which is characterized by the boundary indicator 1. The Dirichlet
    // values are provided by the DirichletBoundaryValues class
    // we defined above:
    std::map<unsigned int,double> boundary_values;
    VectorTools::interpolate_boundary_values (dof_handler,
                                              1,
                                              DirichletBoundaryValues<dim>(),
                                              boundary_values);

    MatrixTools::apply_boundary_values (boundary_values,
                                        system_matrix,
                                        solution,
                                        system_rhs);

    timer.stop ();
    deallog << "done ("
            << timer()
            << "s)"
            << std::endl;
  }

  // @sect4{UltrasoundProblem::solve}

  // As already mentioned in the introduction, the system matrix is neither
  // symmetric nor definite, and so it is not quite obvious how to come up
  // with an iterative solver and a preconditioner that do a good job on this
  // matrix.  We chose instead to go a different way and solve the linear
  // system with the sparse LU decomposition provided by UMFPACK. This is
  // often a good first choice for 2D problems and works reasonably well even
  // for a large number of DoFs.  The deal.II interface to UMFPACK is given by
  // the SparseDirectUMFPACK class, which is very easy to use and allows us to
  // solve our linear system with just 3 lines of code.

  // Note again that for compiling this example program, you need to have the
  // deal.II library built with UMFPACK support, which can be achieved by
  // providing the  --with-umfpack switch to the configure script
  // prior to compilation of the library.
  template <int dim>
  void UltrasoundProblem<dim>::solve ()
  {
    deallog << "Solving linear system... ";
    Timer timer;
    timer.start ();

    // The code to solve the linear system is short: First, we allocate an
    // object of the right type. The following initialize call
    // provides the matrix that we would like to invert to the
    // SparseDirectUMFPACK object, and at the same time kicks off the
    // LU-decomposition. Hence, this is also the point where most of the
    // computational work in this program happens.
    SparseDirectUMFPACK  A_direct;
    A_direct.initialize(system_matrix);

    // After the decomposition, we can use A_direct like a matrix
    // representing the inverse of our system matrix, so to compute the
    // solution we just have to multiply with the right hand side vector:
    A_direct.vmult (solution, system_rhs);

    timer.stop ();
    deallog << "done ("
            << timer ()
            << "s)"
            << std::endl;
  }

  // @sect4{UltrasoundProblem::output_results}

  // Here we output our solution $v$ and $w$ as well as the derived quantity
  // $|u|$ in the format specified in the parameter file. Most of the work for
  // deriving $|u|$ from $v$ and $w$ was already done in the implementation of
  // the ComputeIntensity class, so that the output routine is
  // rather straightforward and very similar to what is done in the previous
  // tutorials.
  template <int dim>
  void UltrasoundProblem<dim>::output_results () const
  {
    deallog << "Generating output... ";
    Timer timer;
    timer.start ();

    // Define objects of our ComputeIntensity class and a DataOut
    // object:
    ComputeIntensity<dim> intensities;
    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);

    // Next we query the output-related parameters from the ParameterHandler.
    // The DataOut::parse_parameters call acts as a counterpart to the
    // DataOutInterface<1>::declare_parameters call in
    // ParameterReader::declare_parameters. It collects all the
    // output format related parameters from the ParameterHandler and sets the
    // corresponding properties of the DataOut object accordingly.
    prm.enter_subsection("Output parameters");

    const std::string output_file    = prm.get("Output file");
    data_out.parse_parameters(prm);

    prm.leave_subsection ();

    // Now we put together the filename from the base name provided by the
    // ParameterHandler and the suffix which is provided by the DataOut class
    // (the default suffix is set to the right type that matches the one set
    // in the .prm file through parse_parameters()):
    const std::string filename = output_file +
                                 data_out.default_suffix();

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

    // The solution vectors $v$ and $w$ are added to the DataOut object in the
    // usual way:
    std::vector<std::string> solution_names;
    solution_names.push_back ("Re_u");
    solution_names.push_back ("Im_u");

    data_out.add_data_vector (solution, solution_names);

    // For the intensity, we just call add_data_vector again, but
    // this with our ComputeIntensity object as the second
    // argument, which effectively adds $|u|$ to the output data:
    data_out.add_data_vector (solution, intensities);

    // The last steps are as before. Note that the actual output format is now
    // determined by what is stated in the input file, i.e. one can change the
    // output format without having to re-compile this program:
    data_out.build_patches ();
    data_out.write (output);

    timer.stop ();
    deallog << "done ("
            << timer()
            << "s)"
            << std::endl;
  }

  // @sect4{UltrasoundProblem::run}

  // Here we simply execute our functions one after the other:
  template <int dim>
  void UltrasoundProblem<dim>::run ()
  {
    make_grid ();
    setup_system ();
    assemble_system ();
    solve ();
    output_results ();
  }
}

// @sect4{The main function}

// Finally the main function of the program. It has the same
// structure as in almost all of the other tutorial programs. The only
// exception is that we define ParameterHandler and
// ParameterReader objects, and let the latter read in the
// parameter values from a textfile called step-29.prm. The
// values so read are then handed over to an instance of the UltrasoundProblem
// class:
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step29;

      ParameterHandler  prm;
      ParameterReader   param(prm);
      param.read_parameters("step-29.prm");

      UltrasoundProblem<2>  ultrasound_problem (prm);
      ultrasound_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


/**
 * @file  CableVelocityFactor.h
 * @brief Cable velocity factor: relates cable velocity (or acceleration), two
 * mounting points, and resultant mounting point velocities
 * @author Frank Dellaert
 * @author Gerry Chen
 */

#include "CableVelocityFactor.h"

#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose3.h>

#include <boost/optional.hpp>
#include <iostream>
#include <string>

using namespace gtsam;

namespace gtdynamics {

double CableVelocityFactor::computeLdot(const Pose3 &wTx, const Vector6 &Vx,
                                        boost::optional<Matrix &> H_wTx,
                                        boost::optional<Matrix &> H_Vx) const {
  // Jacobians: cable direction
  Matrix13 H_dir;
  Matrix33 dir_H_wPb;
  Matrix36 wPb_H_wTx;
  // Jacobians: End-effector Mounting point velocity (in world coords)
  Matrix13 H_wPDOTb;
  Matrix33 wPDOTb_H_wRx;
  Matrix33 wPDOTb_H_xPDOTb;
  Matrix36 xPDOTb_H_Vx;
  Matrix33 cross_H_omega;

  // cable direction
  Point3 wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : 0);
  Vector3 dir = normalize(wPb - wPa_, H_wTx ? &dir_H_wPb : 0);

  // velocity aka pdot
  // TODO(gerry): use Adjoint
  Vector3 eePDOTem =
      Vx.tail<3>() + cross(Vx.head<3>(), xPb_, H_Vx ? &cross_H_omega : 0);
  if (H_Vx) xPDOTb_H_Vx << cross_H_omega, I_3x3;
  Vector3 wPDOTem = wTx.rotation().rotate(eePDOTem,  //
                                          H_wTx ? &wPDOTb_H_wRx : 0,
                                          H_Vx ? &wPDOTb_H_xPDOTb : 0);

  // ldot = (cable direction) dot (velocity aka pdot)
  double ldot = dot(dir, wPDOTem,        //
                    H_wTx ? &H_dir : 0,  //
                    H_Vx ? &H_wPDOTb : 0);

  // jacobians
  if (H_wTx) {
    *H_wTx = H_dir * dir_H_wPb * wPb_H_wTx;  //
    H_wTx->leftCols<3>() += H_wPDOTb * wPDOTb_H_wRx;
  }
  if (H_Vx) *H_Vx = H_wPDOTb * wPDOTb_H_xPDOTb * xPDOTb_H_Vx;

  return ldot;
}

}  // namespace gtdynamics


/*
@copyright Louis Dionne 2015
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/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/zero.hpp>
namespace hana = boost::hana;

BOOST_HANA_CONSTANT_CHECK(hana::zero<hana::integral_constant_tag<int>>() == hana::int_c<0>);
static_assert(hana::zero<long>() == 0l, "");

int main() { }


/// \file   test_quantity.cpp
///
/// \brief
///
/// \authors    Maarten P. Scholl
/// \date       2019-08-15
/// \copyright  Copyright 2017-2019 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             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.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE quantity

#include <boost/test/included/unit_test.hpp>

#include <esl/quantity.hpp>

BOOST_AUTO_TEST_SUITE(ESL)

BOOST_AUTO_TEST_CASE(quantity_constructor)
{
    {
        esl::quantity q_default_;

        BOOST_CHECK_EQUAL(q_default_.amount, 0);
        BOOST_CHECK_EQUAL(q_default_.basis, 1);
    }

    BOOST_CHECK_THROW(esl::quantity(13, 0), std::logic_error);

    {
        esl::quantity one_(1);

        BOOST_CHECK_EQUAL(one_.amount, 1);
        BOOST_CHECK_EQUAL(one_.basis, 1);
    }

    {
        esl::quantity threefifty_(350, 100);

        BOOST_CHECK_EQUAL(threefifty_.amount, 350);
        BOOST_CHECK_EQUAL(threefifty_.basis, 100);

        esl::quantity again_(threefifty_);

        BOOST_CHECK_EQUAL(again_.amount, 350);
        BOOST_CHECK_EQUAL(again_.basis, 100);
    }
}

BOOST_AUTO_TEST_CASE(quantity_arithmetic_guards)
{
    // test exception on adding incompatible quantities
    esl::quantity threefifty_(350, 100);
#if EXCEPTION_ON_BASIS_CHANGE
    BOOST_CHECK_THROW(threefifty_ + esl::quantity(1, 42), std::logic_error);
#else
    BOOST_CHECK_EQUAL(threefifty_ + esl::quantity(1, 42),
                      esl::quantity(148, 42));
#endif

    // test exception on subtracting with negative result
    BOOST_CHECK_THROW(threefifty_ - esl::quantity(351, 100), std::logic_error);
    BOOST_CHECK_THROW(threefifty_ - esl::quantity(176, 50), std::logic_error);
    BOOST_CHECK_THROW(threefifty_ - esl::quantity(701, 200), std::logic_error);

    // test conversion on subtraction
#if EXCEPTION_ON_BASIS_CHANGE
    BOOST_CHECK_THROW(threefifty_ - esl::quantity(1, 42), std::logic_error);
#else
    BOOST_CHECK_EQUAL(threefifty_ - esl::quantity(1, 42),
                      esl::quantity(146, 42));

    BOOST_CHECK_EQUAL(threefifty_ - esl::quantity(3, 1),
                      esl::quantity(50, 100));
#endif

#if EXCEPTION_ON_BASIS_CHANGE
    {
        esl::quantity again_(threefifty_);
        BOOST_CHECK_THROW(again_ += esl::quantity(1, 42), std::logic_error);
    }
    {
        esl::quantity again_(threefifty_);
        BOOST_CHECK_THROW(again_ -= esl::quantity(1, 42), std::logic_error);
    }
#endif
}

BOOST_AUTO_TEST_CASE(quantity_arithmetic)
{
    esl::quantity threefifty_(350, 100);

    // .divide integer
    {
        auto constexpr_d_ = threefifty_.divide<4>();
        BOOST_CHECK_EQUAL(constexpr_d_[0], esl::quantity(88, 100));
        BOOST_CHECK_EQUAL(constexpr_d_[1], esl::quantity(88, 100));
        BOOST_CHECK_EQUAL(constexpr_d_[2], esl::quantity(87, 100));
        BOOST_CHECK_EQUAL(constexpr_d_[3], esl::quantity(87, 100));
    }

    // / integer
    {
        auto dynamic_d_ = threefifty_ / 3;
        auto q_117_     = esl::quantity(117, 100);
        auto q_116_     = esl::quantity(116, 100);
        BOOST_CHECK_EQUAL(dynamic_d_[0], q_117_);
        BOOST_CHECK_EQUAL(dynamic_d_[1], q_117_);
        BOOST_CHECK_EQUAL(dynamic_d_[2], q_116_);
    }

    // +
    BOOST_CHECK_EQUAL(threefifty_ + esl::quantity(123, 100),
                      esl::quantity(473, 100));
    BOOST_CHECK_EQUAL(threefifty_ + esl::quantity(0, 100),
                      esl::quantity(350, 100));
    // -
    BOOST_CHECK_EQUAL(threefifty_ - esl::quantity(123, 100),
                      esl::quantity(227, 100));
    // * integer
    BOOST_CHECK_EQUAL(threefifty_ * 3, esl::quantity(1050, 100));
    // * quantity
    BOOST_CHECK_EQUAL(threefifty_ * esl::quantity(50, 100),
                      esl::quantity(175, 100));
    BOOST_CHECK_EQUAL(threefifty_ * esl::quantity(200, 100),
                      esl::quantity(700, 100));
    BOOST_CHECK_EQUAL(threefifty_ * esl::quantity(0, 100),
                      esl::quantity(0, 100));
    BOOST_CHECK_EQUAL(threefifty_ * esl::quantity(100, 100),
                      esl::quantity(350, 100));
    BOOST_CHECK_EQUAL(threefifty_ * esl::quantity(10, 100),
                      esl::quantity(35, 100));
}

BOOST_AUTO_TEST_CASE(quantity_logic)
{
    esl::quantity threefifty_(350, 100);

    BOOST_CHECK_EQUAL(threefifty_, esl::quantity(350, 100));
    BOOST_CHECK_NE(threefifty_, esl::quantity(123, 100));

    BOOST_CHECK_LE(threefifty_, esl::quantity(456, 100));
    BOOST_CHECK_GE(threefifty_, esl::quantity(123, 100));

    BOOST_CHECK_EQUAL(threefifty_, 3.5);

    BOOST_CHECK_EQUAL(esl::quantity(142'857'142'857, 100'000'000'000),
                      10.0 / 7);
}

BOOST_AUTO_TEST_SUITE_END()  // ESL


// XXX needs fixing
#if 0
/*
	Copyright (C) 2012-2014 by Kristina Simpson <sweet.kristas@gmail.com>

	This software is provided 'as-is', without any express or implied
	warranty. In no event will the authors be held liable for any damages
	arising from the use of this software.

	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:

	   1. The origin of this software must not be misrepresented; you must not
	   claim that you wrote the original software. If you use this software
	   in a product, an acknowledgement in the product documentation would be
	   appreciated but is not required.

	   2. Altered source versions must be plainly marked as such, and must not be
	   misrepresented as being the original software.

	   3. This notice may not be removed or altered from any source
	   distribution.
*/
#include <boost/array.hpp>
#include <boost/shared_array.hpp>
#include "intrusive_ptr.hpp"

#include <assert.h>

#include <algorithm>
#include <cmath>
#include <deque>
#include <numeric>
#include <map>
#include <vector>

#if !defined(WINRT)
#include "SDL2/SDL.h"
#else
#include "SDL.h"
#endif

#include "asserts.hpp"
#include "border_widget.hpp"
#include "button.hpp"
#include "checkbox.hpp"
#include "color_picker.hpp"
#include "filesystem.hpp"
#include "formatter.hpp"
#include "grid_widget.hpp"
#include "gui_section.hpp"
#include "image_widget.hpp"
#include "input.hpp"
#include "json_parser.hpp"
#include "label.hpp"
#include "level_runner.hpp"
#include "module.hpp"
#include "preferences.hpp"
#include "slider.hpp"
#include "surface_utils.hpp"
#include "unit_test.hpp"

namespace
{
	struct rgb
	{
		uint8_t r, g, b;
	};

	struct hsv
	{
		uint8_t h, s, v;
	};

	hsv rgb_to_hsv(uint8_t r, uint8_t g, uint8_t b)
	{
		hsv out;
		uint8_t min_color, max_color, delta;

		min_color = std::min(r, std::min(g, b));
		max_color = std::max(r, std::max(g, b));

		delta = max_color - min_color;
		out.v = max_color;
		if(out.v == 0) {
			out.s = 0;
			out.h = 0;
			return out;
		}

		out.s = uint8_t(255.0 * delta / out.v);
		if(out.s == 0) {
			out.h = 0;
			return out;
		}

		if(r == max_color) {
			out.h = uint8_t(43.0 * (g-b)/delta);
		} else if(g == max_color) {
			out.h = 85 + uint8_t(43.0 * (b-r)/delta);
		} else {
			out.h = 171 + uint8_t(43.0 * (r-g)/delta);
		}
		return out;
	}

	rgb hsv_to_rgb(uint8_t h, uint8_t s, uint8_t v)
	{
		rgb out;
		uint8_t region, remainder, p, q, t;

		if(s == 0) {
			out.r = out.g = out.b = v;
		} else {
			region = h / 43;
			remainder = (h - (region * 43)) * 6;

			p = (v * (255 - s)) >> 8;
			q = (v * (255 - ((s * remainder) >> 8))) >> 8;
			t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;

			switch(region)
			{
				case 0:  out.r = v; out.g = t; out.b = p; break;
				case 1:  out.r = q; out.g = v; out.b = p; break;
				case 2:  out.r = p; out.g = v; out.b = t; break;
				case 3:  out.r = p; out.g = q; out.b = v; break;
				case 4:  out.r = t; out.g = p; out.b = v; break;
				default: out.r = v; out.g = p; out.b = q; break;
			}
		}
		return out;
	}

	template<typename T>
	uint8_t clamp_u8(T a)
	{
		if(a < 0) {
			return 0;
		} else if(a > T(255)) {
			return 255;
		}
		return uint8_t(a);
	}

	void calculate_normal(SDL_Surface* s, double gs_param[3])
	{
		ASSERT_LOG(s != nullptr, "FATAL: Invalid surface");

		graphics::setAlpha_for_transparent_colors_in_rgba_surface(s, 0);

		// convert to array of values (from hsv)
		const int sz = s->w * s->h;
		std::vector<hsv> hsv_ary;
		hsv_ary.resize(sz);
		uint8_t* pixels = reinterpret_cast<uint8_t*>(s->pixels);
		for(int n = 0; n != sz; ++n) {
			if(pixels[3] != 0) {
				hsv_ary[n] = rgb_to_hsv(pixels[0], pixels[1], pixels[2]);
			}
			pixels += 4;
		}

		// convert to grayscale
		std::vector<uint8_t> gs_ary;
		gs_ary.resize(sz);
		pixels = reinterpret_cast<uint8_t*>(s->pixels);
		for(int n = 0; n != sz; ++n) {
			if(pixels[3] != 0) {
				gs_ary[n] = clamp_u8(gs_param[0]*pixels[0]+gs_param[1]*pixels[1]+gs_param[2]*pixels[2]);
			}
			pixels += 4;
		}

		// run sobel convolution on data, writing data into vert/horz
		std::vector<uint8_t> vert(sz,0);
		std::vector<uint8_t> horz(sz,0);
		/*for(int m = 1; m != s->h-1; ++m) {
			for(int n = 1; n != s->w-1; ++n) {
				int ndx = m*s->w + n;
				vert[ndx] = clamp_u8(gs_ary[(m-1)*s->w+n-1]*1 + gs_ary[(m-1)*s->w+n]*2 + gs_ary[(m-1)*s->w+n+1]*1
					- gs_ary[(m+1)*s->w+n-1]*1 - gs_ary[(m+1)*s->w+n]*2 - gs_ary[(m+1)*s->w+n+1]*1);
				horz[ndx] = clamp_u8(gs_ary[(m-1)*s->w+n-1]*1 - gs_ary[(m-1)*s->w+n+1]*1
					+ gs_ary[(m)*s->w+n-1]*2 - gs_ary[(m)*s->w+n+1]*2
					+ gs_ary[(m+1)*s->w+n-1]*1 - gs_ary[(m+1)*s->w+n+1]*1);
			}
		}*/
		// emboss [1,1,1][0,0,0][-1,-1,-1]
		const int bias = 128;
		for(int m = 1; m != s->h-1; ++m) {
			for(int n = 1; n != s->w-1; ++n) {
				int ndx = m*s->w + n;
				//vert[ndx] = clamp_u8(gs_ary[(m-1)*s->w+n-1]*2 + gs_ary[ndx]*-1 + gs_ary[(m+1)*s->w+n+1]*-1);
				//horz[ndx] = clamp_u8(gs_ary[(m-1)*s->w+n+1]*2 + gs_ary[ndx]*-1 + gs_ary[(m+1)*s->w+n-1]*-1);
				vert[ndx] = clamp_u8(gs_ary[(m-1)*s->w+n-1]*-1 + gs_ary[(m-1)*s->w+n]*-1 + gs_ary[(m-1)*s->w+n+1]*-1
					- gs_ary[(m+1)*s->w+n-1]*1 - gs_ary[(m+1)*s->w+n]*1 - gs_ary[(m+1)*s->w+n+1]*1 + bias);
				horz[ndx] = clamp_u8(gs_ary[(m-1)*s->w+n-1]*-1 + gs_ary[(m-1)*s->w+n+1]*1
					+ gs_ary[(m)*s->w+n-1]*-1 + gs_ary[(m)*s->w+n+1]*1
					+ gs_ary[(m+1)*s->w+n-1]*-1 + gs_ary[(m+1)*s->w+n+1]*1 + bias);
			}
		}

		pixels = reinterpret_cast<uint8_t*>(s->pixels);
		for(int n = 0; n != sz; ++n) {
			if(pixels[3] != 0) {
				pixels[0] = horz[n];
				pixels[1] = vert[n];
				pixels[2] = clamp_u8(hsv_ary[n].v+bias);
			}
			pixels += 4;
		}
	}

	graphics::surface make_grayscale(const graphics::surface& src, double* gs_param)
	{
		double gs_p[3];
		if(gs_param == nullptr) {
			gs_p[0] = 0.21;
			gs_p[1] = 0.71;
			gs_p[2] = 0.07;
		} else {
			gs_p[0] = gs_param[0];
			gs_p[1] = gs_param[1];
			gs_p[2] = gs_param[2];
		}
		graphics::surface dst = src.clone();
		const uint8_t* s_pixels = reinterpret_cast<const uint8_t*>(src.get()->pixels);
		uint8_t* d_pixels = reinterpret_cast<uint8_t*>(dst.get()->pixels);
		const int sz = src.get()->w * src.get()->h;
		for(int n = 0; n != sz; ++n) {
			if(s_pixels[3] != 0) {
				uint8_t pv = clamp_u8(gs_p[0]*s_pixels[0]+gs_p[1]*s_pixels[1]+gs_p[2]*s_pixels[2]);
				d_pixels[0] = d_pixels[1] = d_pixels[2] = pv;
			}
			s_pixels += 4;
			d_pixels += 4;
		}
		return dst;
	}

	graphics::surface calculate_normal2(const graphics::surface& horz, const graphics::surface& vert)
	{
		ASSERT_LOG(horz.get()->w == vert.get()->w && horz.get()->h == vert.get()->h, "FATAL: NORMAL: Widths or heights don't match");
		graphics::surface res = horz.clone();

		const int sz = horz.get()->w * horz.get()->h;
		uint8_t* pixels = reinterpret_cast<uint8_t*>(res.get()->pixels);
		for(int n = 0; n != sz; ++n) {
			if(pixels[3] != 0) {
				pixels[0] = reinterpret_cast<uint8_t*>(horz.get()->pixels)[n*4];
				pixels[1] = reinterpret_cast<uint8_t*>(vert.get()->pixels)[n*4];
				pixels[2] = clamp_u8(128);
				pixels[3] = 255;
			}
			pixels += 4;
		}
		return res;
	}

	// Generic convolution filter
	// bias is added to all pixels in the output.
	// divisor is the factor to divide the computing pixel value by.
	// for pixels outside of the input range
	//     if clamp == false: use default_color
    //	   else: use closest pixel value
	// if preserve_alpha: don't saturate the alpha value.
	graphics::surface convolution_filter(const graphics::surface& surf,
		const std::vector<std::vector<double>>& matrix,
		int mx,
		int my,
		double divisor,
		double bias,
		bool preserve_alpha,
		const graphics::color& default_color,
		bool clamp)
	{
		graphics::surface res = surf.clone();
		const uint8_t* pixels = reinterpret_cast<const uint8_t*>(surf.get()->pixels);
		uint8_t* res_pixels = reinterpret_cast<uint8_t*>(res.get()->pixels);
		for(int m = 0; m != res->h; ++m) {
			for(int n = 0; n != res->w; ++n) {
				const int ndx = (n + m * res->w) * 4;
				float sum_r = bias;
				float sum_g = bias;
				float sum_b = bias;
				float sum_a = bias;
				for(int j = 0; j != my; ++j) {
					for(int i = 0; i != mx; ++i) {
						int xoffs = i-mx/2;
						int yoffs = j-my/2;

						int quadrant = 0;
						if(n+xoffs < 0) {
							quadrant |= 1;
						}
						if(n+xoffs >= res->w) {
							quadrant |= 2;
						}
						if(m+yoffs < 0) {
							quadrant |= 4;
						}
						if(m+yoffs >= res->h) {
							quadrant |= 8;
						}
						if(quadrant != 0) {
							if(clamp) {
								sum_r += float(default_color.r()) * matrix[j][i];
								sum_g += float(default_color.g()) * matrix[j][i];
								sum_b += float(default_color.b()) * matrix[j][i];
								sum_a += float(default_color.a()) * matrix[j][i];
								continue;
							}
							if(quadrant & 1) {
								xoffs = 0;
							}
							if(quadrant & 2) {
								xoffs = 0;
							}
							if(quadrant & 4) {
								yoffs = 0;
							}
							if(quadrant & 8) {
								yoffs = 0;
							}
						}
						const int offs = ndx + (xoffs + yoffs*res->w) * 4;

						sum_r += float(pixels[offs+0]) * matrix[j][i];
						sum_g += float(pixels[offs+1]) * matrix[j][i];
						sum_b += float(pixels[offs+2]) * matrix[j][i];
						sum_a += float(pixels[offs+3]) * matrix[j][i];
					}
				}
				res_pixels[ndx+0] = clamp_u8(sum_r / divisor);
				res_pixels[ndx+1] = clamp_u8(sum_g / divisor);
				res_pixels[ndx+2] = clamp_u8(sum_b / divisor);
				if(!preserve_alpha) {
					res_pixels[ndx+3] = clamp_u8(sum_a / divisor);
				}
			}
		}
		return res;
	}

	graphics::surface blur_filter(const graphics::surface& s, int blur_x, int blur_y, int passes)
	{
		// blur filter is special case of a convolution filter with 1.0 co-efficients.
		std::vector<std::vector<double>> blur_matrix;
		blur_matrix.resize(blur_y);
		for(int n = 0; n != blur_y; ++n) {
			blur_matrix[n].resize(blur_x, 1.0);
		}

		graphics::surface res = s.clone();
		for(int ps = 0; ps != passes; ++ps) {
			res = convolution_filter(res, blur_matrix, blur_x, blur_y, double(blur_x)*blur_y, 0, true, graphics::color(0,0,0), true);
		}
		return res;
	}

	graphics::surface alpha_clip(const graphics::surface& src, const graphics::surface& src_with_alpha)
	{
		graphics::surface res = src_with_alpha.clone();
		const int sz = res->w * res->h;
		const uint8_t* s_pixels = reinterpret_cast<const uint8_t*>(src.get()->pixels);
		uint8_t* pixels = reinterpret_cast<uint8_t*>(res.get()->pixels);
		for(int n = 0; n != sz; ++n) {
			if(pixels[3] != 0) {
				pixels[0] = s_pixels[0];
				pixels[1] = s_pixels[1];
				pixels[2] = s_pixels[2];
			}
			pixels += 4;
			s_pixels += 4;
		}
		return res;
	}

	graphics::surface extract_alpha_mask(const graphics::surface& s,
		const graphics::color& shadow_color,
		const graphics::color& non_shadow_color)
	{
		graphics::surface mask = s.clone();
		const int sz = mask->w * mask->h;
		uint8_t* pixels = reinterpret_cast<uint8_t*>(mask->pixels);
		for(int n = 0; n != sz; ++n) {
			if(pixels[3] != 255) {
				pixels[0] = shadow_color.r();
				pixels[1] = shadow_color.g();
				pixels[2] = shadow_color.b();
			} else {
				pixels[0] = non_shadow_color.r();
				pixels[1] = non_shadow_color.g();
				pixels[2] = non_shadow_color.b();
			}
			pixels[3] = 255;
			pixels += 4;
		}
		return mask;
	}

	graphics::surface drop_shadow_filter(const graphics::surface& s,
		const graphics::color& shadow_color,
		int blur_x,
		int blur_y,
		double angle,
		double distance,
		double strength,
		bool inner_shadow,
		bool knockout,
		int passes)
	{
		// 1) clone the new surface and create a mask with a color value of black if there is alpha != 255
		//    else white.
		graphics::surface mask = extract_alpha_mask(s, shadow_color, graphics::color(255,255,255));
		// 2) Scale the surface
		//SDL_Surface* scaled = SDL_CreateRGBSurface(0, mask->w, mask->h, 32, SURFACE_MASK);
		//SDL_FillRect(scaled, nullptr, shadow_color.value());
		//SDL_Rect scaled_rect = {
		//	int(distance/2), int(distance/2), int(mask->w-distance/2), int(mask->h-distance/2)
		//};
		//SDL_BlitScaled(mask, nullptr, scaled, &scaled_rect);

		mask = blur_filter(mask, blur_x, blur_y, passes);

		const int sz = s->w * s->h;
		uint8_t* spixels = reinterpret_cast<uint8_t*>(s->pixels);
		uint8_t* pixels = reinterpret_cast<uint8_t*>(mask->pixels);
		for(int n = 0; n != sz; ++n) {
			if(spixels[3] != 255) {
				pixels[0] = shadow_color.r();
				pixels[1] = shadow_color.g();
				pixels[2] = shadow_color.b();
			}
			pixels += 4;
			spixels += 4;
		}

		// move the image by angle/distance
		if(distance > 0) {
			// XXX
		}

		return graphics::surface(mask);
	}

	// angle in radians, returns a new surface with the given sobel operator applied
	graphics::surface sobel(const graphics::surface& s, double angle)
	{
		std::vector<std::vector<double>> matrix;
		matrix.resize(3);
		for(int n = 0; n != 3; ++n) {
			matrix[n].resize(3);
		}
		double delta_r = M_PI/4.0;
		matrix[0][0] = cos(angle + 3.0*delta_r);
		matrix[0][1] = cos(angle + 2.0*delta_r);
		matrix[0][2] = cos(angle + delta_r);

		matrix[1][0] = cos(delta_r + 4.0*delta_r);
		matrix[1][1] = 0;
		matrix[1][2] = cos(angle);

		matrix[2][0] = cos(angle + 5.0*delta_r);
		matrix[2][1] = cos(angle + 6.0*delta_r);
		matrix[2][2] = cos(angle + 7.0*delta_r);

		return convolution_filter(s, matrix, 3, 3, 1.0, 128.0, true, graphics::color(255,255,255), true);
	}

	// angle in radians, returns a new surface with the given embossing.
	graphics::surface emboss(const graphics::surface& s, double angle)
	{
		std::vector<std::vector<double>> matrix;
		matrix.resize(3);
		for(int n = 0; n != 3; ++n) {
			matrix[n].resize(3);
		}
		double delta_r = M_PI/4.0;
		matrix[0][0] = cos(angle + delta_r);
		matrix[0][1] = cos(angle + 2.0*delta_r);
		matrix[0][2] = cos(angle + 3.0*delta_r);

		matrix[1][0] = cos(angle + delta_r);
		matrix[1][1] = 0;
		matrix[1][2] = cos(angle + 4.0*delta_r);

		matrix[2][0] = cos(angle - delta_r);
		matrix[2][1] = cos(angle - 2.0*delta_r);
		matrix[2][2] = cos(angle - 3.0*delta_r);

		return convolution_filter(s, matrix, 3, 3, 1.0, 128.0, true, graphics::color(255,255,255), false);
	}

	// a is the base layer, b is the top layer.
	graphics::surface blend_overlay(const graphics::surface& src_a, const graphics::surface& src_b, double opacity)
	{
		graphics::surface res = src_a.clone();
		const int sz = res->w * res->h;
		const uint8_t* a_pixels = reinterpret_cast<const uint8_t*>(src_a.get()->pixels);
		const uint8_t* b_pixels = reinterpret_cast<const uint8_t*>(src_b.get()->pixels);
		uint8_t* pixels = reinterpret_cast<uint8_t*>(res.get()->pixels);
		for(int n = 0; n != sz; ++n) {
			hsv a_hsv = rgb_to_hsv(a_pixels[0], a_pixels[1], a_pixels[2]);
			if(a_hsv.v < 0.5) {
				pixels[0] = clamp_u8(2*a_pixels[0]*b_pixels[0]);
				pixels[1] = clamp_u8(2*a_pixels[1]*b_pixels[1]);
				pixels[2] = clamp_u8(2*a_pixels[2]*b_pixels[2]);
				pixels[3] = clamp_u8(2*a_pixels[3]*b_pixels[3]);
			} else {
				pixels[0] = 255-clamp_u8(2*(255-a_pixels[0])*(255-b_pixels[0]));
				pixels[1] = 255-clamp_u8(2*(255-a_pixels[1])*(255-b_pixels[1]));
				pixels[2] = 255-clamp_u8(2*(255-a_pixels[2])*(255-b_pixels[2]));
				pixels[3] = 255-clamp_u8(2*(255-a_pixels[3])*(255-b_pixels[3]));
			}

			pixels += 4;
			a_pixels += 4;
			b_pixels += 4;
		}
		return res;
	}

	struct manager
	{
		manager(gles2::program_ptr shader)
		{
			glGetIntegerv(GL_CURRENT_PROGRAM, &old_program);
			glUseProgram(shader->get());
		}
		~manager()
		{
			glUseProgram(old_program);
		}
		GLint old_program;
	};

	class ImageWidget_lighted : public gui::ImageWidget
	{
	public:
		explicit ImageWidget_lighted(graphics::texture tex, graphics::texture tex_normal, int w, int h)
			: ImageWidget(tex, w, h), tex_normal_(tex_normal) {
			shader_ = gles2::shader_program::get_global("texture_2d_lighted")->shader();
			u_mvp_matrix_ = shader_->get_fixed_uniform("mvp_matrix");

			u_tex_map_ = shader_->get_fixed_uniform("tex_map");
			u_normal_map_ = shader_->get_fixed_uniform("normal_map");

			u_resolution_ = shader_->get_fixed_uniform("resolution");

			lighting_.reset(new graphics::lighting(shader_));
			lighting_->set_light_position(0, glm::vec3(0.5f, 0.5f, 0.07f));
			lighting_->set_ambient_color(0, glm::vec3(1.0f, 1.0f, 1.0f));
			lighting_->set_light_color(0, glm::vec3(1.0f, 0.8f, 0.8f));
			lighting_->set_ambient_intensity(0, 0.2f);
			lighting_->enable_light_source(0, true);

			set_uniforms();

			setFrameSet("empty_window");
		}
		virtual ~ImageWidget_lighted() {
		}
		void set_uniforms() const {
			manager m(shader_);
			glUniform1i(u_tex_map_, 0);
			glUniform1i(u_normal_map_, 1);
			glUniform2f(u_resolution_, GLfloat(width()), GLfloat(height()));
		}
	protected:
		virtual void handleDraw() const {
			manager m(shader_);

			GLint cur_id = graphics::texture::get_currentTexture();

			if(lighting_) {
				lighting_->set_all_uniforms();
			}

			glActiveTexture(GL_TEXTURE1);
			glBindTexture(GL_TEXTURE_2D, tex_normal_.getId());
			glActiveTexture(GL_TEXTURE0);
			glBindTexture(GL_TEXTURE_2D, tex().getId());

			const int w_odd = width() % 2;
			const int h_odd = height() % 2;
			const int w = width() / 2;
			const int h = height() / 2;

			glPushMatrix();
			glTranslatef((x()+w)&preferences::xypos_draw_mask, (y()+h)&preferences::xypos_draw_mask, 0.0f);
			glUniformMatrix4fv(u_mvp_matrix_, 1, GL_FALSE, glm::value_ptr(gles2::get_mvp_matrix()));

			const GLfloat varray[] = {
				(GLfloat)-w, (GLfloat)-h,
				(GLfloat)-w, (GLfloat)h+h_odd,
				(GLfloat)w+w_odd, (GLfloat)-h,
				(GLfloat)w+w_odd, (GLfloat)h+h_odd
			};

			GLfloat tcx, tcy, tcx2, tcy2;
			if(area().w() == 0) {
				tcx = graphics::texture::get_coord_x(0.0);
				tcy = graphics::texture::get_coord_y(0.0);
				tcx2 = graphics::texture::get_coord_x(1.0);
				tcy2 = graphics::texture::get_coord_y(1.0);
			} else {
				tcx = GLfloat(area().x())/tex().width();
				tcy = GLfloat(area().y())/tex().height();
				tcx2 = GLfloat(area().x2())/tex().width();
				tcy2 = GLfloat(area().y2())/tex().height();
			}

			const GLfloat tcarray[] = {
				tcx, tcy,
				tcx, tcy2,
				tcx2, tcy,
				tcx2, tcy2,
			};
			shader_->vertex_array(2, GL_FLOAT, 0, 0, varray);
			shader_->texture_array(2, GL_FLOAT, 0, 0, tcarray);
			glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);

			glBindTexture(GL_TEXTURE_2D, cur_id);

			glPopMatrix();
		}

		virtual bool handleEvent(const SDL_Event& event, bool claimed) {
			if(event.type == SDL_MOUSEMOTION) {
				const SDL_MouseMotionEvent& e = event.motion;
				if(inWidget(e.x, e.y) && lighting_) {
					lighting_->set_light_position(0, glm::vec3(float(e.x-x()), float(e.y-y()), 0.07f));
					claimed = true;
				}
			}
			return claimed;
		}
	private:
		graphics::texture tex_normal_;
		//GLfloat varray_[8];
		gles2::program_ptr shader_;

		GLuint u_resolution_;

		GLuint u_mvp_matrix_;

		GLuint u_tex_map_;
		GLuint u_normal_map_;

		graphics::lighting_ptr lighting_;
	};

	typedef ffl::IntrusivePtr<ImageWidget_lighted> ImageWidget_lighted_ptr;

	class normal_viewer : public gui::dialog
	{
	public:
		explicit normal_viewer(const rect& r, const std::string& fname);
		virtual ~normal_viewer();

		void init();
	private:
		std::string fname_;
		rect area_;

		gui::ImageWidgetPtr img_;
		gui::ImageWidgetPtr normal_;
		gui::ImageWidgetPtr aux_;
		ImageWidget_lighted_ptr output_img_;

		normal_viewer();
		normal_viewer(const normal_viewer&);
	};

	normal_viewer::normal_viewer(const rect& r, const std::string& fname)
		: dialog(r.x(), r.y(), r.w(), r.h()), area_(r), fname_(fname)
	{
		init();
	}

	normal_viewer::~normal_viewer()
	{
	}

	void normal_viewer::init()
	{
		using namespace graphics;
		clear();

		const int sidebar_padding = 0;//(width()*15)/100;
		const int between_padding = 10;

		const int widget_width = width()/2 - sidebar_padding - between_padding*2;
		const int widget_height = height()/2 - between_padding;

		surface s = surface_cache::get_no_cache(fname_);
		texture tex = texture::get_no_cache(s);

		img_.reset(new gui::ImageWidget(tex, widget_width, widget_height));
		addWidget(img_, 0, 0);

		surface s_sobel = s.clone();
		double gs_param[3] = {0.21, 0.71, 0.07};
		calculate_normal(s_sobel.get(), gs_param);
		texture s_tex = texture::get_no_cache(s_sobel);

		graphics::setAlpha_for_transparent_colors_in_rgba_surface(s, 0);
		surface s_aux = drop_shadow_filter(s, graphics::color(0,0,0), 3, 3, 0.0, 3, 1.0, true, false, 3);
		//graphics::surface s_aux = extract_alpha_mask(s, graphics::color(0,0,0), graphics::color(255,255,255));
		//surface s_emb_h = emboss(s_aux, 0);
		s_aux = make_grayscale(s_aux, nullptr);
		surface s_norm = calculate_normal2(emboss(s_aux, 0), emboss(s_aux, 90));
		s_norm = alpha_clip(s_norm, s);
		//surface s_emb = s_aux;
		aux_.reset(new gui::ImageWidget(texture::get_no_cache(s_norm), widget_width, widget_height));
		addWidget(aux_, 0, widget_height + between_padding);

		normal_.reset(new gui::ImageWidget(s_tex, widget_width, widget_height));
		addWidget(normal_, widget_width + between_padding, 0);

		output_img_.reset(new ImageWidget_lighted(tex, texture::get_no_cache(s_norm), widget_width, widget_height));
		addWidget(output_img_, widget_width + between_padding, widget_height + between_padding);
	}

}

UTILITY(calculate_normal_map)
{
	std::deque<std::string> arguments(args.begin(), args.end());

	ASSERT_LOG(arguments.size() <= 1, "Unexpected arguments");

	std::string fname;
	if(arguments.empty() == false) {
		fname = module::map_file(arguments.front());
	}

	ffl::IntrusivePtr<normal_viewer> editor(new normal_viewer(rect(0, 0, preferences::actual_screen_width(), preferences::actual_screen_height()), fname));
	editor->showModal();
}

#endif


#include <ros/ros.h>
#include <vector>
#include <Eigen/Geometry>

#include <moveit_visual_tools/moveit_visual_tools.h>

//#include "arf_moveit_wrapper/moveit_wrapper.h"
#include "arf_moveit_wrapper/redundant_robot.h"

namespace rvt = rviz_visual_tools;

class Rviz
{
public:
    moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
  Rviz()
  {
    visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("world","/demo_visuals"));
  }

  void plotPose(Eigen::Affine3d pose)
  {
    Eigen::Isometry3d pose_temp;
    pose_temp = pose.matrix();
    visual_tools_->publishAxis(pose_temp, rvt::LARGE);
    visual_tools_->trigger();
  }

  void clear()
  {
    visual_tools_->deleteAllMarkers();
    visual_tools_->trigger();
  }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_moveit_wrapper");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    arf::RedundantRobot robot;
    Rviz rviz;
    rviz.clear();

    // calculate forward kinematics to get reachable pose
    std::vector<double> q1 = {0.0, 0.0, 0, 0, 0, 0, 0, 0};
    auto fk_pose = robot.fk(q1, "tool_tip");

    // move the fk_pose closer to robot
    Eigen::Vector3d p1(0.3, 0.5, -0.3);
    fk_pose.translate(p1);

    // visualize fk_pose
    Eigen::Isometry3d pose_temp(fk_pose.matrix());
    rviz.visual_tools_->publishAxisLabeled(pose_temp, "fk_pose");
    rviz.visual_tools_->trigger();

    ros::Duration(1.0).sleep();

    // set values for fixed joints = y- and z-rail
    std::vector<double> q_fixed = {0, 0};

    //auto ik_sol = robot.redundantIk(fk_pose, q_fixed);
    auto ik_sol = robot.ikGridSamples(fk_pose);

    // show ik solutions
    if (ik_sol.size() > 0)
    {
        for (auto q_sol : ik_sol)
        {
            robot.plot(rviz.visual_tools_, q_sol);
            ros::Duration(0.4).sleep();
        }
    }
    else
    {
        ROS_INFO("No ik solutions found");
    }

    ros::Duration(1.0).sleep();

    ros::shutdown();
    return 0;
}


#include "calculator/cell/total_aggregated_fission_source.h"

#include <deal.II/base/utilities.h>

#include "system/moments/spherical_harmonic_i.h"
#include "domain/definition_i.h"
#include "calculator/cell/integrated_fission_source_i.h"

namespace bart {

namespace calculator {

namespace cell {

template<int dim>
double TotalAggregatedFissionSource<dim>::AggregatedFissionSource(
    system::moments::SphericalHarmonicI *system_moments_ptr) const {

  auto cells = domain_ptr_->Cells();
  double fission_source = 0;

  for (auto& cell : cells) {
    fission_source += cell_fission_source_ptr_->CellValue(cell, system_moments_ptr);
  }

  return dealii::Utilities::MPI::sum(fission_source, MPI_COMM_WORLD);
}

template class TotalAggregatedFissionSource<1>;
template class TotalAggregatedFissionSource<2>;
template class TotalAggregatedFissionSource<3>;

} // namespace cell

} // namespace calculator

} // namespace bart


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

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/table.hpp>
#include <nt2/include/constants/i.hpp>
#include <nt2/include/functions/splat.hpp>

NT2_TEST_CASE_TPL ( pow3,  NT2_SIMD_REAL_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<cT,ext_t>                vcT;

  nt2::table<T> a = nt2::ones(3, 3, nt2::meta::as_<T>());
  nt2::table<T> b = nt2::ones(3, 3, nt2::meta::as_<T>());
  // specific values tests
 for(int i=1; i <= 3; i++)
 {
   NT2_TEST_ULP_EQUAL(nt2::pow(nt2::splat<vcT>(a(i)), nt2::splat<vcT>(b(i))), nt2::splat<vcT>(a(i)), 10.0);
 }
}

NT2_TEST_CASE_TPL ( pow4,  NT2_SIMD_REAL_TYPES)
{
  using nt2::pow;
  using nt2::tag::pow_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<cT,ext_t>                vcT;
  nt2::table<cT> a = nt2::ones(1, 3, nt2::meta::as_<T>());
  for(int i=1; i <= 3; i++) a(i) =  cT(i, i);
  nt2::table<cT> b = nt2::ones(1, 3, nt2::meta::as_<T>());
  for(int i=1; i <= 3; i++) b(i) = cT(i-1);
  nt2::table<cT> r(nt2::of_size(1, 3));
  r(1) = cT(1,0); r(2) = cT(2,2); r(3) = cT(0,18);

  // specific values tests
 for(int i=1; i <= 3; i++)
 {
   NT2_TEST_ULP_EQUAL(nt2::pow(nt2::splat<vcT>(a(i)), nt2::splat<vcT>(b(i))), nt2::splat<vcT>(r(i)), 10.0);
 }

}


#ifndef __RANK_FILTER__
#define __RANK_FILTER__

#include <deque>
#include <cassert>
#include <functional>
#include <iostream>
#include <iterator>

#include <boost/array.hpp>
#include <boost/container/set.hpp>
#include <boost/container/node_allocator.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits.hpp>

namespace rank_filter
{

template<class I1,
         class I2>
inline void lineRankOrderFilter1D(const I1& src_begin, const I1& src_end,
        I2& dest_begin, I2& dest_end,
        size_t half_length, double rank)
{
    // Types in use.
    typedef typename std::iterator_traits<I1>::value_type T1;
    typedef typename std::iterator_traits<I2>::value_type T2;

    // Establish common types to work with source and destination values.
    BOOST_STATIC_ASSERT((boost::is_same<T1, T2>::value));
    typedef T1 T;

    // Define container types that will be used.
    typedef boost::container::multiset< T,
            std::less<T>,
            boost::container::node_allocator<T>,
            boost::container::tree_assoc_options< boost::container::tree_type<boost::container::scapegoat_tree> >::type> multiset;
    typedef std::deque< typename multiset::iterator > deque;

    // Precompute some window lengths.
    const size_t half_length_add_1 = half_length + 1;
    const size_t length_sub_1 = 2 * half_length;
    const size_t length = length_sub_1 + 1;

    // Ensure the result will fit.
    assert(std::distance(src_begin, src_end) <= std::distance(dest_begin, dest_end));

    // Window length cannot exceed input data with reflection.
    assert(half_length_add_1 <= std::distance(src_begin, src_end));

    // Rank must be in the range 0 to 1.
    assert((0 <= rank) && (rank <= 1));

    // Track values in window both in sorted and sequential order.
    multiset sorted_window;
    deque window_iters(length);

    // Iterators in source and destination.
    I1 src_pos = src_begin;
    I1 dest_pos = dest_begin;

    // Get the initial window in sequential order with reflection.
    // Insert elements in order to the multiset for sorting.
    // Store all multiset iterators into the deque in sequential order.
    {
        std::deque<T> window_init(half_length_add_1);

        window_init[half_length] = *src_pos;
        for (size_t j = half_length; j > 0;)
        {
            window_init[--j] = *(++src_pos);
        }
        ++src_pos;

        for (size_t j = 0; j < half_length; ++j)
        {
            window_iters[j] = sorted_window.insert(window_init[j]);
        }
        window_iters[half_length] = sorted_window.insert(
            window_init[half_length]
        );
        for (size_t j = half_length_add_1; j < length; ++j)
        {
            window_init.pop_back();
            window_iters[j] = sorted_window.insert(window_init.back());
        }
        window_init.pop_back();
    }

    // Window position corresponding to this rank.
    const size_t rank_pos = static_cast<size_t>(boost::math::round(rank * length_sub_1));
    typename multiset::iterator rank_point = sorted_window.begin();
    std::advance(rank_point, rank_pos);

    // Roll window forward one value at a time.
    typename multiset::iterator prev_iter;
    T prev_value;
    T rank_value = *rank_point;
    *dest_pos = rank_value;
    T next_value;
    size_t window_reflect_pos = length_sub_1;
    bool src_not_empty = ( src_pos != src_end );
    while ( src_not_empty || ( window_reflect_pos > 0 ) )
    {
        // Get last value to remove from window.
        prev_iter = window_iters.front();
        prev_value = *prev_iter;
        window_iters.pop_front();

        // Determine next value to add to window.
        // Handle special cases like reflection at the end.
        if ( src_not_empty )
        {
            next_value = *src_pos;
            ++src_pos;
            src_not_empty = ( src_pos != src_end );
        }
        else
        {
            window_reflect_pos -= 2;
            next_value = *(window_iters[window_reflect_pos]);
        }

        // Remove old value and add new value to the window.
        // Handle special cases where `rank_pos` may have an adjusted position
        // due to where the old and new values are inserted.
        if ( ( rank_value < prev_value ) && ( rank_value <= next_value ) )
        {
            sorted_window.erase(prev_iter);
            window_iters.push_back(sorted_window.insert(next_value));
        }
        else if ( ( rank_value >= prev_value ) && ( rank_value > next_value ) )
        {
            if ( rank_point == prev_iter )
            {
                window_iters.push_back(sorted_window.insert(next_value));
                --rank_point;
                sorted_window.erase(prev_iter);
            }
            else
            {
                sorted_window.erase(prev_iter);
                window_iters.push_back(sorted_window.insert(next_value));
            }
        }
        else if ( ( rank_value < prev_value ) && ( rank_value > next_value ) )
        {
            sorted_window.erase(prev_iter);
            window_iters.push_back(sorted_window.insert(next_value));
            --rank_point;
        }
        else if ( ( rank_value >= prev_value ) && ( rank_value <= next_value ) )
        {
            if (rank_point == prev_iter)
            {
                window_iters.push_back(sorted_window.insert(next_value));
                ++rank_point;
                sorted_window.erase(prev_iter);
            }
            else
            {
                sorted_window.erase(prev_iter);
                window_iters.push_back(sorted_window.insert(next_value));
                ++rank_point;
            }
        }

        // Store rank value in the destination.
        *(++dest_pos) = rank_value = *rank_point;
    }
}

}

namespace std
{
    template <class T, size_t N>
    ostream& operator<<(ostream& out, const boost::array<T, N>& that)
    {
        out << "{ ";
        for (unsigned int i = 0; i < (N - 1); ++i)
        {
            out << that[i] << ", ";
        }
        out << that[N - 1] << " }";

        return(out);
    }
}

#endif //__RANK_FILTER__


/*
@copyright Louis Dionne 2014
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/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto negative = [](auto x) {
        return x < 0;
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto negate = [](auto x) {
        return -x;
    };

    BOOST_HANA_CONSTEXPR_ASSERT(
        adjust(negative, negate, list(-3, -2, -1, 0, 1, 2, 3))
        ==
        list(3, 2, 1, 0, 1, 2, 3)
    );
    //! [main]
}