/*=========================================================================
 *
 *  Copyright David Doria 2012 daviddoria@gmail.com
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/

#ifndef CompressedHistogramAcceptanceVisitor_HPP
#define CompressedHistogramAcceptanceVisitor_HPP

#include <boost/graph/graph_traits.hpp>

// Parent class
#include "Visitors/AcceptanceVisitors/AcceptanceVisitorParent.h"

// Custom
#include <Mask/Mask.h>
#include <Mask/MaskOperations.h>
#include <ITKHelpers/ITKHelpers.h>
#include <Utilities/Histogram/Histogram.h>

// ITK
#include "itkImage.h"
#include "itkImageRegion.h"

/**

 */
template <typename TGraph, typename TImage>
struct CompressedHistogramAcceptanceVisitor : public AcceptanceVisitorParent<TGraph>
{
  TImage* Image;
  Mask* MaskImage;

  const unsigned int HalfWidth;

  float DifferenceThreshold;

  typedef typename boost::graph_traits<TGraph>::vertex_descriptor VertexDescriptorType;

  CompressedHistogramAcceptanceVisitor(TImage* const image, Mask* const mask, const unsigned int halfWidth,
                                       const float differenceThreshold = 100.0f) :
  AcceptanceVisitorParent<TGraph>("CompressedHistogramAcceptanceVisitor"),
  Image(image), MaskImage(mask), HalfWidth(halfWidth),
  DifferenceThreshold(differenceThreshold)
  {

  }

  bool AcceptMatch(VertexDescriptorType target, VertexDescriptorType source) const
  {
    float computedEnergy;
    return AcceptMatch(target, source, computedEnergy);
  }

  bool AcceptMatch(VertexDescriptorType target, VertexDescriptorType source, float& computedEnergy) const override
  {
    itk::Index<2> targetPixel = ITKHelpers::CreateIndex(target);
    itk::ImageRegion<2> targetRegion = ITKHelpers::GetRegionInRadiusAroundPixel(targetPixel, HalfWidth);

    itk::Index<2> sourcePixel = ITKHelpers::CreateIndex(source);
    itk::ImageRegion<2> sourceRegion = ITKHelpers::GetRegionInRadiusAroundPixel(sourcePixel, HalfWidth);

    // Get the pixels to use in the target region
    std::vector<typename TImage::PixelType> validPixelsTargetRegion =
             MaskOperations::GetValidPixelsInRegion(Image, MaskImage, targetRegion);

    // Get the pixels to use in the source region
    std::vector<itk::Offset<2> > validOffsets = MaskImage->GetValidOffsetsInRegion(targetRegion);
    std::vector<itk::Index<2> > sourcePatchValidPixelIndices =
           ITKHelpers::OffsetsToIndices(validOffsets, sourceRegion.GetIndex());
    std::vector<typename TImage::PixelType> validPixelsSourceRegion =
           ITKHelpers::GetPixelValues(Image, sourcePatchValidPixelIndices);

    assert(validPixelsSourceRegion.size() == validPixelsTargetRegion.size());

    unsigned int numberOfPixels = validPixelsTargetRegion.size();

    float totalHistogramDifference = 0.0f;

    for(unsigned int component = 0; component < Image->GetNumberOfComponentsPerPixel(); ++component)
    {
      std::vector<float> targetValues(numberOfPixels);
      std::vector<float> sourceValues(numberOfPixels);

      for(unsigned int pixelId = 0; pixelId < numberOfPixels; ++pixelId)
      {
        targetValues[pixelId] = validPixelsTargetRegion[pixelId][component];
        sourceValues[pixelId] = validPixelsSourceRegion[pixelId][component];
      }

      float minTargetValue = *(std::min_element(targetValues.begin(), targetValues.end()));
      float minSourceValue = *(std::min_element(sourceValues.begin(), sourceValues.end()));

      float maxTargetValue = *(std::max_element(targetValues.begin(), targetValues.end()));
      float maxSourceValue = *(std::max_element(sourceValues.begin(), sourceValues.end()));

      float minValue = std::min(minTargetValue, minSourceValue);
      float maxValue = std::max(maxTargetValue, maxSourceValue);

      std::vector<float> targetHistogram = Histogram::ScalarHistogram(targetValues, 20,
                                                                      minValue, maxValue);
      std::vector<float> sourceHistogram = Histogram::ScalarHistogram(sourceValues, 20,
                                                                      minValue, maxValue);

      // We normalize the histograms because the magnitude of the histogram difference should not
      // change based on the number of pixels that were in the valid region of the patches.
      Helpers::NormalizeVector(targetHistogram);
      Helpers::NormalizeVector(sourceHistogram);

      float channelHistogramDifference = Helpers::VectorSumOfAbsoluteDifferences(targetHistogram, sourceHistogram);
      totalHistogramDifference += channelHistogramDifference;
    }

    // Compute the difference
    computedEnergy = totalHistogramDifference;
    std::cout << "HistogramDifferenceAcceptanceVisitor Energy: " << computedEnergy << std::endl;

    if(computedEnergy < DifferenceThreshold)
      {
      std::cout << "HistogramDifferenceAcceptanceVisitor: Match accepted (less than " << DifferenceThreshold
                << ")" << std::endl;
      return true;
      }
    else
      {
      std::cout << "HistogramDifferenceAcceptanceVisitor: Match rejected (greater than " << DifferenceThreshold
                << ")" << std::endl;
      return false;
      }
  }

};

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2010-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <algorithms/test_reverse.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <test_common/test_point.hpp>
#include <test_geometries/all_custom_linestring.hpp>
#include <test_geometries/all_custom_ring.hpp>
#include <test_geometries/wrapped_boost_array.hpp>

template <typename LineString>
void test_linestring()
{
    // Simplex
    test_geometry<LineString >(
        "LINESTRING(0 0,1 1)",
        "LINESTRING(1 1,0 0)");

    // Three points, middle should stay the same
    test_geometry<LineString >(
        "LINESTRING(0 0,1 1,2 2)",
        "LINESTRING(2 2,1 1,0 0)");

    // Four points
    test_geometry<LineString >(
        "LINESTRING(0 0,1 1,2 2,3 3)",
        "LINESTRING(3 3,2 2,1 1,0 0)");
}

template <typename Ring>
void test_ring()
{
    test_geometry<Ring>(
        "POLYGON((4 0,8 2,8 7,4 9,0 7,0 2,2 1,4 0))",
        "POLYGON((4 0,2 1,0 2,0 7,4 9,8 7,8 2,4 0))");
}

template <typename Point>
void test_all()
{
    test_linestring<bg::model::linestring<Point> >();
    test_linestring<all_custom_linestring<Point> >();

    // Polygon with holes
    test_geometry<bg::model::polygon<Point> >(
        "POLYGON((4 0,8 2,8 7,4 9,0 7,0 2,2 1,4 0),(7 3,7 6,1 6,1 3,4 3,7 3))",
        "POLYGON((4 0,2 1,0 2,0 7,4 9,8 7,8 2,4 0),(7 3,4 3,1 3,1 6,7 6,7 3))");

    // Check compilation
    test_geometry<Point>("POINT(0 0)", "POINT(0 0)");

    test_ring<bg::model::ring<Point> >();
    test_ring<all_custom_ring<Point> >();
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<double> >();

#if defined(HAVE_TTMATH)
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


/* Author: Guido Kanschat, Texas A&M University, 2009 */

/*    $Id: step-12.cc 28552 2013-02-25 15:18:15Z kanschat $       */
/*                                                                */
/*    Copyright (C) 2010-2013 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// The first few files have already been covered in previous examples and will
// thus not be further commented on:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/compressed_sparsity_pattern.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/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/fe/fe_values.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/numerics/data_out.h>
#include <deal.II/fe/mapping_q1.h>
// Here the discontinuous finite elements are defined. They are used in the
// same way as all other finite elements, though -- as you have seen in
// previous tutorial programs -- there isn't much user interaction with finite
// element classes at all: they are passed to DoFHandler and
// FEValues objects, and that is about it.
#include <deal.II/fe/fe_dgq.h>
// We are going to use the simplest possible solver, called Richardson
// iteration, that represents a simple defect correction. This, in combination
// with a block SSOR preconditioner (defined in precondition_block.h), that
// uses the special block matrix structure of system matrices arising from DG
// discretizations.
#include <deal.II/lac/solver_richardson.h>
#include <deal.II/lac/precondition_block.h>
// We are going to use gradients as refinement indicator.
#include <deal.II/numerics/derivative_approximation.h>

// Here come the new include files for using the MeshWorker framework. The
// first contains the class MeshWorker::DoFInfo, which provides local
// integrators with a mapping between local and global degrees of freedom. It
// stores the results of local integrals as well in its base class
// Meshworker::LocalResults.  In the second of these files, we find an object
// of type MeshWorker::IntegrationInfo, which is mostly a wrapper around a
// group of FEValues objects. The file <tt>meshworker/simple.h</tt> contains
// classes assembling locally integrated data into a global system containing
// only a single matrix. Finally, we will need the file that runs the loop
// over all mesh cells and faces.
#include <deal.II/meshworker/dof_info.h>
#include <deal.II/meshworker/integration_info.h>
#include <deal.II/meshworker/simple.h>
#include <deal.II/meshworker/loop.h>

// Like in all programs, we finish this section by including the needed C++
// headers and declaring we want to use objects in the dealii namespace
// without prefix.
#include <iostream>
#include <fstream>

namespace Step12
{
  using namespace dealii;

  // @sect3{Equation data}
  //
  // First, we define a class describing the inhomogeneous boundary
  // data. Since only its values are used, we implement value_list(), but
  // leave all other functions of Function undefined.
  template <int dim>
  class BoundaryValues:  public Function<dim>
  {
  public:
    BoundaryValues () {};
    virtual void value_list (const std::vector<Point<dim> > &points,
                             std::vector<double> &values,
                             const unsigned int component=0) const;
  };

  // Given the flow direction, the inflow boundary of the unit square
  // $[0,1]^2$ are the right and the lower boundaries. We prescribe
  // discontinuous boundary values 1 and 0 on the x-axis and value 0 on the
  // right boundary. The values of this function on the outflow boundaries
  // will not be used within the DG scheme.
  template <int dim>
  void BoundaryValues<dim>::value_list(const std::vector<Point<dim> > &points,
                                       std::vector<double> &values,
                                       const unsigned int) const
  {
    Assert(values.size()==points.size(),
           ExcDimensionMismatch(values.size(),points.size()));

    for (unsigned int i=0; i<values.size(); ++i)
      {
        if (points[i](0)<0.5)
          values[i]=1.;
        else
          values[i]=0.;
      }
  }
  // @sect3{The AdvectionProblem class}
  //
  // After this preparations, we proceed with the main class of this program,
  // called AdvectionProblem. It is basically the main class of step-6. We do
  // not have a ConstraintMatrix, because there are no hanging node
  // constraints in DG discretizations.

  // Major differences will only come up in the implementation of the assemble
  // functions, since here, we not only need to cover the flux integrals over
  // faces, we also use the MeshWorker interface to simplify the loops
  // involved.
  template <int dim>
  class AdvectionProblem
  {
  public:
    AdvectionProblem ();
    void run ();

  private:
    void setup_system ();
    void assemble_system ();
    void solve (Vector<double> &solution);
    void refine_grid ();
    void output_results (const unsigned int cycle) const;

    Triangulation<dim>   triangulation;
    const MappingQ1<dim> mapping;

    // Furthermore we want to use DG elements of degree 1 (but this is only
    // specified in the constructor). If you want to use a DG method of a
    // different degree the whole program stays the same, only replace 1 in
    // the constructor by the desired polynomial degree.
    FE_DGQ<dim>          fe;
    DoFHandler<dim>      dof_handler;

    // The next four members represent the linear system to be
    // solved. system_matrix and right_hand_side are
    // generated by assemble_system(), the solution
    // is computed in solve(). The sparsity_pattern
    // is used to determine the location of nonzero elements in
    // system_matrix.
    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double>       solution;
    Vector<double>       right_hand_side;

    // Finally, we have to provide functions that assemble the cell, boundary,
    // and inner face terms. Within the MeshWorker framework, the loop over
    // all cells and much of the setup of operations will be done outside this
    // class, so all we have to provide are these three operations. They will
    // then work on intermediate objects for which first, we here define
    // typedefs to the info objects handed to the local integration functions
    // in order to make our life easier below.
    typedef MeshWorker::DoFInfo<dim> DoFInfo;
    typedef MeshWorker::IntegrationInfo<dim> CellInfo;

    // The following three functions are then the ones that get called inside
    // the generic loop over all cells and faces. They are the ones doing the
    // actual integration.
    //
    // In our code below, these functions do not access member variables of
    // the current class, so we can mark them as static and
    // simply pass pointers to these functions to the MeshWorker
    // framework. If, however, these functions would want to access member
    // variables (or needed additional arguments beyond the ones specified
    // below), we could use the facilities of boost::bind (or std::bind,
    // respectively) to provide the MeshWorker framework with objects that act
    // as if they had the required number and types of arguments, but have in
    // fact other arguments already bound.
    static void integrate_cell_term (DoFInfo &dinfo,
                                     CellInfo &info);
    static void integrate_boundary_term (DoFInfo &dinfo,
                                         CellInfo &info);
    static void integrate_face_term (DoFInfo &dinfo1,
                                     DoFInfo &dinfo2,
                                     CellInfo &info1,
                                     CellInfo &info2);
  };

  // We start with the constructor. The 1 in the constructor call of
  // fe is the polynomial degree.
  template <int dim>
  AdvectionProblem<dim>::AdvectionProblem ()
    :
    mapping (),
    fe (1),
    dof_handler (triangulation)
  {}

  template <int dim>
  void AdvectionProblem<dim>::setup_system ()
  {
    // In the function that sets up the usual finite element data structures,
    // we first need to distribute the DoFs.
    dof_handler.distribute_dofs (fe);

    // We start by generating the sparsity pattern. To this end, we first fill
    // an intermediate object of type CompressedSparsityPattern with the
    // couplings appearing in the system. After building the pattern, this
    // object is copied to sparsity_pattern and can be discarded.

    // To build the sparsity pattern for DG discretizations, we can call the
    // function analogue to DoFTools::make_sparsity_pattern, which is called
    // DoFTools::make_flux_sparsity_pattern:
    CompressedSparsityPattern c_sparsity(dof_handler.n_dofs());
    DoFTools::make_flux_sparsity_pattern (dof_handler, c_sparsity);
    sparsity_pattern.copy_from(c_sparsity);

    // Finally, we set up the structure of all components of the linear
    // system.
    system_matrix.reinit (sparsity_pattern);
    solution.reinit (dof_handler.n_dofs());
    right_hand_side.reinit (dof_handler.n_dofs());
  }

  // @sect4{The assemble_system function}

  // Here we see the major difference to assembling by hand. Instead of
  // writing loops over cells and faces, we leave all this to the MeshWorker
  // framework. In order to do so, we just have to define local integration
  // functions and use one of the classes in namespace MeshWorker::Assembler
  // to build the global system.
  template <int dim>
  void AdvectionProblem<dim>::assemble_system ()
  {
    // This is the magic object, which knows everything about the data
    // structures and local integration.  This is the object doing the work in
    // the function MeshWorker::loop(), which is implicitly called by
    // MeshWorker::integration_loop() below. After the functions to which we
    // provide pointers did the local integration, the
    // MeshWorker::Assembler::SystemSimple object distributes these into the
    // global sparse matrix and the right hand side vector.
    MeshWorker::IntegrationInfoBox<dim> info_box;

    // First, we initialize the quadrature formulae and the update flags in
    // the worker base class. For quadrature, we play safe and use a QGauss
    // formula with number of points one higher than the polynomial degree
    // used. Since the quadratures for cells, boundary and interior faces can
    // be selected independently, we have to hand over this value three times.
    const unsigned int n_gauss_points = dof_handler.get_fe().degree+1;
    info_box.initialize_gauss_quadrature(n_gauss_points,
                                         n_gauss_points,
                                         n_gauss_points);

    // These are the types of values we need for integrating our system. They
    // are added to the flags used on cells, boundary and interior faces, as
    // well as interior neighbor faces, which is forced by the four @p true
    // values.
    info_box.initialize_update_flags();
    UpdateFlags update_flags = update_quadrature_points |
                               update_values            |
                               update_gradients;
    info_box.add_update_flags(update_flags, true, true, true, true);

    // After preparing all data in <tt>info_box</tt>, we initialize the
    // FEValues objects in there.
    info_box.initialize(fe, mapping);

    // The object created so far helps us do the local integration on each
    // cell and face. Now, we need an object which receives the integrated
    // (local) data and forwards them to the assembler.
    MeshWorker::DoFInfo<dim> dof_info(dof_handler);

    // Now, we have to create the assembler object and tell it, where to put
    // the local data. These will be our system matrix and the right hand
    // side.
    MeshWorker::Assembler::SystemSimple<SparseMatrix<double>, Vector<double> >
    assembler;
    assembler.initialize(system_matrix, right_hand_side);

    // Finally, the integration loop over all active cells (determined by the
    // first argument, which is an active iterator).
    //
    // As noted in the discussion when declaring the local integration
    // functions in the class declaration, the arguments expected by the
    // assembling integrator class are not actually function pointers. Rather,
    // they are objects that can be called like functions with a certain
    // number of arguments. Consequently, we could also pass objects with
    // appropriate operator() implementations here, or the result of std::bind
    // if the local integrators were, for example, non-static member
    // functions.
    MeshWorker::loop<dim, dim, MeshWorker::DoFInfo<dim>, MeshWorker::IntegrationInfoBox<dim> >
    (dof_handler.begin_active(), dof_handler.end(),
     dof_info, info_box,
     &AdvectionProblem<dim>::integrate_cell_term,
     &AdvectionProblem<dim>::integrate_boundary_term,
     &AdvectionProblem<dim>::integrate_face_term,
     assembler, true);
  }

  // @sect4{The local integrators}

  // These are the functions given to the MeshWorker::integration_loop()
  // called just above. They compute the local contributions to the system
  // matrix and right hand side on cells and faces.
  template <int dim>
  void AdvectionProblem<dim>::integrate_cell_term (DoFInfo &dinfo,
                                                   CellInfo &info)
  {
    // First, let us retrieve some of the objects used here from @p info. Note
    // that these objects can handle much more complex structures, thus the
    // access here looks more complicated than might seem necessary.
    const FEValuesBase<dim> &fe_v = info.fe_values();
    FullMatrix<double> &local_matrix = dinfo.matrix(0).matrix;
    const std::vector<double> &JxW = fe_v.get_JxW_values ();

    // With these objects, we continue local integration like always. First,
    // we loop over the quadrature points and compute the advection vector in
    // the current point.
    for (unsigned int point=0; point<fe_v.n_quadrature_points; ++point)
      {
        Point<dim> beta;
        beta(0) = -fe_v.quadrature_point(point)(1);
        beta(1) = fe_v.quadrature_point(point)(0);
        beta /= beta.norm();

        // We solve a homogeneous equation, thus no right hand side shows up
        // in the cell term.  What's left is integrating the matrix entries.
        for (unsigned int i=0; i<fe_v.dofs_per_cell; ++i)
          for (unsigned int j=0; j<fe_v.dofs_per_cell; ++j)
            local_matrix(i,j) -= beta*fe_v.shape_grad(i,point)*
                                 fe_v.shape_value(j,point) *
                                 JxW[point];
      }
  }

  // Now the same for the boundary terms. Note that now we use FEValuesBase,
  // the base class for both FEFaceValues and FESubfaceValues, in order to get
  // access to normal vectors.
  template <int dim>
  void AdvectionProblem<dim>::integrate_boundary_term (DoFInfo &dinfo,
                                                       CellInfo &info)
  {
    const FEValuesBase<dim> &fe_v = info.fe_values();
    FullMatrix<double> &local_matrix = dinfo.matrix(0).matrix;
    Vector<double> &local_vector = dinfo.vector(0).block(0);

    const std::vector<double> &JxW = fe_v.get_JxW_values ();
    const std::vector<Point<dim> > &normals = fe_v.get_normal_vectors ();

    std::vector<double> g(fe_v.n_quadrature_points);

    static BoundaryValues<dim> boundary_function;
    boundary_function.value_list (fe_v.get_quadrature_points(), g);

    for (unsigned int point=0; point<fe_v.n_quadrature_points; ++point)
      {
        Point<dim> beta;
        beta(0) = -fe_v.quadrature_point(point)(1);
        beta(1) = fe_v.quadrature_point(point)(0);
        beta /= beta.norm();

        const double beta_n=beta * normals[point];
        if (beta_n>0)
          for (unsigned int i=0; i<fe_v.dofs_per_cell; ++i)
            for (unsigned int j=0; j<fe_v.dofs_per_cell; ++j)
              local_matrix(i,j) += beta_n *
                                   fe_v.shape_value(j,point) *
                                   fe_v.shape_value(i,point) *
                                   JxW[point];
        else
          for (unsigned int i=0; i<fe_v.dofs_per_cell; ++i)
            local_vector(i) -= beta_n *
                               g[point] *
                               fe_v.shape_value(i,point) *
                               JxW[point];
      }
  }

  // Finally, the interior face terms. The difference here is that we receive
  // two info objects, one for each cell adjacent to the face and we assemble
  // four matrices, one for each cell and two for coupling back and forth.
  template <int dim>
  void AdvectionProblem<dim>::integrate_face_term (DoFInfo &dinfo1,
                                                   DoFInfo &dinfo2,
                                                   CellInfo &info1,
                                                   CellInfo &info2)
  {
    // For quadrature points, weights, etc., we use the FEValuesBase object of
    // the first argument.
    const FEValuesBase<dim> &fe_v = info1.fe_values();

    // For additional shape functions, we have to ask the neighbors
    // FEValuesBase.
    const FEValuesBase<dim> &fe_v_neighbor = info2.fe_values();

    // Then we get references to the four local matrices. The letters u and v
    // refer to trial and test functions, respectively. The %numbers indicate
    // the cells provided by info1 and info2. By convention, the two matrices
    // in each info object refer to the test functions on the respective
    // cell. The first matrix contains the interior couplings of that cell,
    // while the second contains the couplings between cells.
    FullMatrix<double> &u1_v1_matrix = dinfo1.matrix(0,false).matrix;
    FullMatrix<double> &u2_v1_matrix = dinfo1.matrix(0,true).matrix;
    FullMatrix<double> &u1_v2_matrix = dinfo2.matrix(0,true).matrix;
    FullMatrix<double> &u2_v2_matrix = dinfo2.matrix(0,false).matrix;

    // Here, following the previous functions, we would have the local right
    // hand side vectors. Fortunately, the interface terms only involve the
    // solution and the right hand side does not receive any contributions.

    const std::vector<double> &JxW = fe_v.get_JxW_values ();
    const std::vector<Point<dim> > &normals = fe_v.get_normal_vectors ();

    for (unsigned int point=0; point<fe_v.n_quadrature_points; ++point)
      {
        Point<dim> beta;
        beta(0) = -fe_v.quadrature_point(point)(1);
        beta(1) = fe_v.quadrature_point(point)(0);
        beta /= beta.norm();

        const double beta_n=beta * normals[point];
        if (beta_n>0)
          {
            // This term we've already seen:
            for (unsigned int i=0; i<fe_v.dofs_per_cell; ++i)
              for (unsigned int j=0; j<fe_v.dofs_per_cell; ++j)
                u1_v1_matrix(i,j) += beta_n *
                                     fe_v.shape_value(j,point) *
                                     fe_v.shape_value(i,point) *
                                     JxW[point];

            // We additionally assemble the term $(\beta\cdot n u,\hat
            // v)_{\partial \kappa_+}$,
            for (unsigned int k=0; k<fe_v_neighbor.dofs_per_cell; ++k)
              for (unsigned int j=0; j<fe_v.dofs_per_cell; ++j)
                u1_v2_matrix(k,j) -= beta_n *
                                     fe_v.shape_value(j,point) *
                                     fe_v_neighbor.shape_value(k,point) *
                                     JxW[point];
          }
        else
          {
            // This one we've already seen, too:
            for (unsigned int i=0; i<fe_v.dofs_per_cell; ++i)
              for (unsigned int l=0; l<fe_v_neighbor.dofs_per_cell; ++l)
                u2_v1_matrix(i,l) += beta_n *
                                     fe_v_neighbor.shape_value(l,point) *
                                     fe_v.shape_value(i,point) *
                                     JxW[point];

            // And this is another new one: $(\beta\cdot n \hat u,\hat
            // v)_{\partial \kappa_-}$:
            for (unsigned int k=0; k<fe_v_neighbor.dofs_per_cell; ++k)
              for (unsigned int l=0; l<fe_v_neighbor.dofs_per_cell; ++l)
                u2_v2_matrix(k,l) -= beta_n *
                                     fe_v_neighbor.shape_value(l,point) *
                                     fe_v_neighbor.shape_value(k,point) *
                                     JxW[point];
          }
      }
  }

  // @sect3{All the rest}
  //
  // For this simple problem we use the simplest possible solver, called
  // Richardson iteration, that represents a simple defect correction. This,
  // in combination with a block SSOR preconditioner, that uses the special
  // block matrix structure of system matrices arising from DG
  // discretizations. The size of these blocks are the number of DoFs per
  // cell. Here, we use a SSOR preconditioning as we have not renumbered the
  // DoFs according to the flow field. If the DoFs are renumbered in the
  // downstream direction of the flow, then a block Gauss-Seidel
  // preconditioner (see the PreconditionBlockSOR class with relaxation=1)
  // does a much better job.
  template <int dim>
  void AdvectionProblem<dim>::solve (Vector<double> &solution)
  {
    SolverControl           solver_control (1000, 1e-12);
    SolverRichardson<>      solver (solver_control);

    // Here we create the preconditioner,
    PreconditionBlockSSOR<SparseMatrix<double> > preconditioner;

    // then assign the matrix to it and set the right block size:
    preconditioner.initialize(system_matrix, fe.dofs_per_cell);

    // After these preparations we are ready to start the linear solver.
    solver.solve (system_matrix, solution, right_hand_side,
                  preconditioner);
  }

  // We refine the grid according to a very simple refinement criterion,
  // namely an approximation to the gradient of the solution. As here we
  // consider the DG(1) method (i.e. we use piecewise bilinear shape
  // functions) we could simply compute the gradients on each cell. But we do
  // not want to base our refinement indicator on the gradients on each cell
  // only, but want to base them also on jumps of the discontinuous solution
  // function over faces between neighboring cells. The simplest way of doing
  // that is to compute approximative gradients by difference quotients
  // including the cell under consideration and its neighbors. This is done by
  // the DerivativeApproximation class that computes the
  // approximate gradients in a way similar to the
  // GradientEstimation described in step-9 of this tutorial. In
  // fact, the DerivativeApproximation class was developed
  // following the GradientEstimation class of step-9. Relating
  // to the discussion in step-9, here we consider $h^{1+d/2}|\nabla_h
  // u_h|$. Furthermore we note that we do not consider approximate second
  // derivatives because solutions to the linear advection equation are in
  // general not in $H^2$ but in $H^1$ (to be more precise, in $H^1_\beta$)
  // only.
  template <int dim>
  void AdvectionProblem<dim>::refine_grid ()
  {
    // The DerivativeApproximation class computes the gradients
    // to float precision. This is sufficient as they are approximate and
    // serve as refinement indicators only.
    Vector<float> gradient_indicator (triangulation.n_active_cells());

    // Now the approximate gradients are computed
    DerivativeApproximation::approximate_gradient (mapping,
                                                   dof_handler,
                                                   solution,
                                                   gradient_indicator);

    // and they are cell-wise scaled by the factor $h^{1+d/2}$
    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();
    for (unsigned int cell_no=0; cell!=endc; ++cell, ++cell_no)
      gradient_indicator(cell_no)*=std::pow(cell->diameter(), 1+1.0*dim/2);

    // Finally they serve as refinement indicator.
    GridRefinement::refine_and_coarsen_fixed_number (triangulation,
                                                     gradient_indicator,
                                                     0.3, 0.1);

    triangulation.execute_coarsening_and_refinement ();
  }

  // The output of this program consists of eps-files of the adaptively
  // refined grids and the numerical solutions given in gnuplot format. This
  // was covered in previous examples and will not be further commented on.
  template <int dim>
  void AdvectionProblem<dim>::output_results (const unsigned int cycle) const
  {
    // Write the grid in eps format.
    std::string filename = "grid-";
    filename += ('0' + cycle);
    Assert (cycle < 10, ExcInternalError());

    filename += ".eps";
    deallog << "Writing grid to <" << filename << ">" << std::endl;
    std::ofstream eps_output (filename.c_str());

    GridOut grid_out;
    grid_out.write_eps (triangulation, eps_output);

    // Output of the solution in gnuplot format.
    filename = "sol-";
    filename += ('0' + cycle);
    Assert (cycle < 10, ExcInternalError());

    filename += ".gnuplot";
    deallog << "Writing solution to <" << filename << ">" << std::endl;
    std::ofstream gnuplot_output (filename.c_str());

    DataOut<dim> data_out;
    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution, "u");

    data_out.build_patches ();

    data_out.write_gnuplot(gnuplot_output);
  }

  // The following run function is similar to previous examples.
  template <int dim>
  void AdvectionProblem<dim>::run ()
  {
    for (unsigned int cycle=0; cycle<6; ++cycle)
      {
        deallog << "Cycle " << cycle << std::endl;

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

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

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

        setup_system ();

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

        assemble_system ();
        solve (solution);

        output_results (cycle);
      }
  }
}

// The following main function is similar to previous examples as
// well, and need not be commented on.
int main ()
{
  try
    {
      Step12::AdvectionProblem<2> dgmethod;
      dgmethod.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;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file    timeiSAM2Chain.cpp
 * @brief   Times each iteration of a long chain in iSAM2, to measure asymptotic performance
 * @author  Richard Roberts
 */

#include <gtsam/base/timing.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h>

#include <fstream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;

typedef Pose2 Pose;

typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim());

int main(int argc, char *argv[]) {

  const size_t steps = 50000;

  cout << "Playing forward " << steps << " time steps..." << endl;

  ISAM2 isam2;

  // Times
  vector<double> times(steps);

  for(size_t step=0; step < steps; ++step) {

    Values newVariables;
    NonlinearFactorGraph newFactors;

    // Collect measurements and new variables for the current step
    gttic_(Create_measurements);
    if(step == 0) {
      // Add prior
      newFactors.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
      newVariables.insert(0, Pose());
    } else {
      Vector eta = Vector::Random(Pose::Dim()) * 0.1;
      Pose2 between = Pose().retract(eta);
      // Add between
      newFactors.add(BetweenFactor<Pose>(step-1, step, between, model));
      newVariables.insert(step, isam2.calculateEstimate<Pose>(step-1) * between);
    }

    gttoc_(Create_measurements);

    // Update iSAM2
    gttic_(Update_ISAM2);
    isam2.update(newFactors, newVariables);
    gttoc_(Update_ISAM2);

    tictoc_finishedIteration_();

    tictoc_getNode(node, Update_ISAM2);
    times[step] = node->time();

    if(step % 1000 == 0) {
      cout << "Step " << step << endl;
      tictoc_print_();
    }
  }

  tictoc_print_();

  // Write times
  ofstream timesFile("times.txt");
  BOOST_FOREACH(double t, times) {
    timesFile << t << "\n"; }

  return 0;
}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to 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)

#ifndef BOOST_MATH_TOOLS_SOLVE_ROOT_HPP
#define BOOST_MATH_TOOLS_SOLVE_ROOT_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/precision.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/cstdint.hpp>
#include <limits>

#ifdef BOOST_MATH_LOG_ROOT_ITERATIONS
#  define BOOST_MATH_LOGGER_INCLUDE <boost/math/tools/iteration_logger.hpp>
#  include BOOST_MATH_LOGGER_INCLUDE
#  undef BOOST_MATH_LOGGER_INCLUDE
#else
#  define BOOST_MATH_LOG_COUNT(count)
#endif

namespace boost{ namespace math{ namespace tools{

template <class T>
class eps_tolerance
{
public:
   eps_tolerance()
   {
      eps = 4 * tools::epsilon<T>();
   }
   eps_tolerance(unsigned bits)
   {
      BOOST_MATH_STD_USING
      eps = (std::max)(T(ldexp(1.0F, 1-bits)), T(4 * tools::epsilon<T>()));
   }
   bool operator()(const T& a, const T& b)
   {
      BOOST_MATH_STD_USING
      return fabs(a - b) <= (eps * (std::min)(fabs(a), fabs(b)));
   }
private:
   T eps;
};

struct equal_floor
{
   equal_floor(){}
   template <class T>
   bool operator()(const T& a, const T& b)
   {
      BOOST_MATH_STD_USING
      return floor(a) == floor(b);
   }
};

struct equal_ceil
{
   equal_ceil(){}
   template <class T>
   bool operator()(const T& a, const T& b)
   {
      BOOST_MATH_STD_USING
      return ceil(a) == ceil(b);
   }
};

struct equal_nearest_integer
{
   equal_nearest_integer(){}
   template <class T>
   bool operator()(const T& a, const T& b)
   {
      BOOST_MATH_STD_USING
      return floor(a + 0.5f) == floor(b + 0.5f);
   }
};

namespace detail{

template <class F, class T>
void bracket(F f, T& a, T& b, T c, T& fa, T& fb, T& d, T& fd)
{
   //
   // Given a point c inside the existing enclosing interval
   // [a, b] sets a = c if f(c) == 0, otherwise finds the new
   // enclosing interval: either [a, c] or [c, b] and sets
   // d and fd to the point that has just been removed from
   // the interval.  In other words d is the third best guess
   // to the root.
   //
   BOOST_MATH_STD_USING  // For ADL of std math functions
   T tol = tools::epsilon<T>() * 2;
   //
   // If the interval [a,b] is very small, or if c is too close
   // to one end of the interval then we need to adjust the
   // location of c accordingly:
   //
   if((b - a) < 2 * tol * a)
   {
      c = a + (b - a) / 2;
   }
   else if(c <= a + fabs(a) * tol)
   {
      c = a + fabs(a) * tol;
   }
   else if(c >= b - fabs(b) * tol)
   {
      c = b - fabs(b) * tol;
   }
   //
   // OK, lets invoke f(c):
   //
   T fc = f(c);
   //
   // if we have a zero then we have an exact solution to the root:
   //
   if(fc == 0)
   {
      a = c;
      fa = 0;
      d = 0;
      fd = 0;
      return;
   }
   //
   // Non-zero fc, update the interval:
   //
   if(boost::math::sign(fa) * boost::math::sign(fc) < 0)
   {
      d = b;
      fd = fb;
      b = c;
      fb = fc;
   }
   else
   {
      d = a;
      fd = fa;
      a = c;
      fa= fc;
   }
}

template <class T>
inline T safe_div(T num, T denom, T r)
{
   //
   // return num / denom without overflow,
   // return r if overflow would occur.
   //
   BOOST_MATH_STD_USING  // For ADL of std math functions

   if(fabs(denom) < 1)
   {
      if(fabs(denom * tools::max_value<T>()) <= fabs(num))
         return r;
   }
   return num / denom;
}

template <class T>
inline T secant_interpolate(const T& a, const T& b, const T& fa, const T& fb)
{
   //
   // Performs standard secant interpolation of [a,b] given
   // function evaluations f(a) and f(b).  Performs a bisection
   // if secant interpolation would leave us very close to either
   // a or b.  Rationale: we only call this function when at least
   // one other form of interpolation has already failed, so we know
   // that the function is unlikely to be smooth with a root very
   // close to a or b.
   //
   BOOST_MATH_STD_USING  // For ADL of std math functions

   T tol = tools::epsilon<T>() * 5;
   T c = a - (fa / (fb - fa)) * (b - a);
   if((c <= a + fabs(a) * tol) || (c >= b - fabs(b) * tol))
      return (a + b) / 2;
   return c;
}

template <class T>
T quadratic_interpolate(const T& a, const T& b, T const& d,
                           const T& fa, const T& fb, T const& fd,
                           unsigned count)
{
   //
   // Performs quadratic interpolation to determine the next point,
   // takes count Newton steps to find the location of the
   // quadratic polynomial.
   //
   // Point d must lie outside of the interval [a,b], it is the third
   // best approximation to the root, after a and b.
   //
   // Note: this does not guarentee to find a root
   // inside [a, b], so we fall back to a secant step should
   // the result be out of range.
   //
   // Start by obtaining the coefficients of the quadratic polynomial:
   //
   T B = safe_div(T(fb - fa), T(b - a), tools::max_value<T>());
   T A = safe_div(T(fd - fb), T(d - b), tools::max_value<T>());
   A = safe_div(T(A - B), T(d - a), T(0));

   if(A == 0)
   {
      // failure to determine coefficients, try a secant step:
      return secant_interpolate(a, b, fa, fb);
   }
   //
   // Determine the starting point of the Newton steps:
   //
   T c;
   if(boost::math::sign(A) * boost::math::sign(fa) > 0)
   {
      c = a;
   }
   else
   {
      c = b;
   }
   //
   // Take the Newton steps:
   //
   for(unsigned i = 1; i <= count; ++i)
   {
      //c -= safe_div(B * c, (B + A * (2 * c - a - b)), 1 + c - a);
      c -= safe_div(T(fa+(B+A*(c-b))*(c-a)), T(B + A * (2 * c - a - b)), T(1 + c - a));
   }
   if((c <= a) || (c >= b))
   {
      // Oops, failure, try a secant step:
      c = secant_interpolate(a, b, fa, fb);
   }
   return c;
}

template <class T>
T cubic_interpolate(const T& a, const T& b, const T& d,
                    const T& e, const T& fa, const T& fb,
                    const T& fd, const T& fe)
{
   //
   // Uses inverse cubic interpolation of f(x) at points
   // [a,b,d,e] to obtain an approximate root of f(x).
   // Points d and e lie outside the interval [a,b]
   // and are the third and forth best approximations
   // to the root that we have found so far.
   //
   // Note: this does not guarentee to find a root
   // inside [a, b], so we fall back to quadratic
   // interpolation in case of an erroneous result.
   //
   BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b
      << " d = " << d << " e = " << e << " fa = " << fa << " fb = " << fb
      << " fd = " << fd << " fe = " << fe);
   T q11 = (d - e) * fd / (fe - fd);
   T q21 = (b - d) * fb / (fd - fb);
   T q31 = (a - b) * fa / (fb - fa);
   T d21 = (b - d) * fd / (fd - fb);
   T d31 = (a - b) * fb / (fb - fa);
   BOOST_MATH_INSTRUMENT_CODE(
      "q11 = " << q11 << " q21 = " << q21 << " q31 = " << q31
      << " d21 = " << d21 << " d31 = " << d31);
   T q22 = (d21 - q11) * fb / (fe - fb);
   T q32 = (d31 - q21) * fa / (fd - fa);
   T d32 = (d31 - q21) * fd / (fd - fa);
   T q33 = (d32 - q22) * fa / (fe - fa);
   T c = q31 + q32 + q33 + a;
   BOOST_MATH_INSTRUMENT_CODE(
      "q22 = " << q22 << " q32 = " << q32 << " d32 = " << d32
      << " q33 = " << q33 << " c = " << c);

   if((c <= a) || (c >= b))
   {
      // Out of bounds step, fall back to quadratic interpolation:
      c = quadratic_interpolate(a, b, d, fa, fb, fd, 3);
   BOOST_MATH_INSTRUMENT_CODE(
      "Out of bounds interpolation, falling back to quadratic interpolation. c = " << c);
   }

   return c;
}

} // namespace detail

template <class F, class T, class Tol, class Policy>
std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, const T& fax, const T& fbx, Tol tol, boost::uintmax_t& max_iter, const Policy& pol)
{
   //
   // Main entry point and logic for Toms Algorithm 748
   // root finder.
   //
   BOOST_MATH_STD_USING  // For ADL of std math functions

   static const char* function = "boost::math::tools::toms748_solve<%1%>";

   boost::uintmax_t count = max_iter;
   T a, b, fa, fb, c, u, fu, a0, b0, d, fd, e, fe;
   static const T mu = 0.5f;

   // initialise a, b and fa, fb:
   a = ax;
   b = bx;
   if(a >= b)
      return boost::math::detail::pair_from_single(policies::raise_domain_error(
         function,
         "Parameters a and b out of order: a=%1%", a, pol));
   fa = fax;
   fb = fbx;

   if(tol(a, b) || (fa == 0) || (fb == 0))
   {
      max_iter = 0;
      if(fa == 0)
         b = a;
      else if(fb == 0)
         a = b;
      return std::make_pair(a, b);
   }

   if(boost::math::sign(fa) * boost::math::sign(fb) > 0)
      return boost::math::detail::pair_from_single(policies::raise_domain_error(
         function,
         "Parameters a and b do not bracket the root: a=%1%", a, pol));
   // dummy value for fd, e and fe:
   fe = e = fd = 1e5F;

   if(fa != 0)
   {
      //
      // On the first step we take a secant step:
      //
      c = detail::secant_interpolate(a, b, fa, fb);
      detail::bracket(f, a, b, c, fa, fb, d, fd);
      --count;
      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);

      if(count && (fa != 0) && !tol(a, b))
      {
         //
         // On the second step we take a quadratic interpolation:
         //
         c = detail::quadratic_interpolate(a, b, d, fa, fb, fd, 2);
         e = d;
         fe = fd;
         detail::bracket(f, a, b, c, fa, fb, d, fd);
         --count;
         BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
      }
   }

   while(count && (fa != 0) && !tol(a, b))
   {
      // save our brackets:
      a0 = a;
      b0 = b;
      //
      // Starting with the third step taken
      // we can use either quadratic or cubic interpolation.
      // Cubic interpolation requires that all four function values
      // fa, fb, fd, and fe are distinct, should that not be the case
      // then variable prof will get set to true, and we'll end up
      // taking a quadratic step instead.
      //
      T min_diff = tools::min_value<T>() * 32;
      bool prof = (fabs(fa - fb) < min_diff) || (fabs(fa - fd) < min_diff) || (fabs(fa - fe) < min_diff) || (fabs(fb - fd) < min_diff) || (fabs(fb - fe) < min_diff) || (fabs(fd - fe) < min_diff);
      if(prof)
      {
         c = detail::quadratic_interpolate(a, b, d, fa, fb, fd, 2);
         BOOST_MATH_INSTRUMENT_CODE("Can't take cubic step!!!!");
      }
      else
      {
         c = detail::cubic_interpolate(a, b, d, e, fa, fb, fd, fe);
      }
      //
      // re-bracket, and check for termination:
      //
      e = d;
      fe = fd;
      detail::bracket(f, a, b, c, fa, fb, d, fd);
      if((0 == --count) || (fa == 0) || tol(a, b))
         break;
      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
      //
      // Now another interpolated step:
      //
      prof = (fabs(fa - fb) < min_diff) || (fabs(fa - fd) < min_diff) || (fabs(fa - fe) < min_diff) || (fabs(fb - fd) < min_diff) || (fabs(fb - fe) < min_diff) || (fabs(fd - fe) < min_diff);
      if(prof)
      {
         c = detail::quadratic_interpolate(a, b, d, fa, fb, fd, 3);
         BOOST_MATH_INSTRUMENT_CODE("Can't take cubic step!!!!");
      }
      else
      {
         c = detail::cubic_interpolate(a, b, d, e, fa, fb, fd, fe);
      }
      //
      // Bracket again, and check termination condition, update e:
      //
      detail::bracket(f, a, b, c, fa, fb, d, fd);
      if((0 == --count) || (fa == 0) || tol(a, b))
         break;
      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
      //
      // Now we take a double-length secant step:
      //
      if(fabs(fa) < fabs(fb))
      {
         u = a;
         fu = fa;
      }
      else
      {
         u = b;
         fu = fb;
      }
      c = u - 2 * (fu / (fb - fa)) * (b - a);
      if(fabs(c - u) > (b - a) / 2)
      {
         c = a + (b - a) / 2;
      }
      //
      // Bracket again, and check termination condition:
      //
      e = d;
      fe = fd;
      detail::bracket(f, a, b, c, fa, fb, d, fd);
      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
      BOOST_MATH_INSTRUMENT_CODE(" tol = " << T((fabs(a) - fabs(b)) / fabs(a)));
      if((0 == --count) || (fa == 0) || tol(a, b))
         break;
      //
      // And finally... check to see if an additional bisection step is
      // to be taken, we do this if we're not converging fast enough:
      //
      if((b - a) < mu * (b0 - a0))
         continue;
      //
      // bracket again on a bisection:
      //
      e = d;
      fe = fd;
      detail::bracket(f, a, b, T(a + (b - a) / 2), fa, fb, d, fd);
      --count;
      BOOST_MATH_INSTRUMENT_CODE("Not converging: Taking a bisection!!!!");
      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
   } // while loop

   max_iter -= count;
   if(fa == 0)
   {
      b = a;
   }
   else if(fb == 0)
   {
      a = b;
   }
   BOOST_MATH_LOG_COUNT(max_iter)
   return std::make_pair(a, b);
}

template <class F, class T, class Tol>
inline std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, const T& fax, const T& fbx, Tol tol, boost::uintmax_t& max_iter)
{
   return toms748_solve(f, ax, bx, fax, fbx, tol, max_iter, policies::policy<>());
}

template <class F, class T, class Tol, class Policy>
inline std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, Tol tol, boost::uintmax_t& max_iter, const Policy& pol)
{
   max_iter -= 2;
   std::pair<T, T> r = toms748_solve(f, ax, bx, f(ax), f(bx), tol, max_iter, pol);
   max_iter += 2;
   return r;
}

template <class F, class T, class Tol>
inline std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, Tol tol, boost::uintmax_t& max_iter)
{
   return toms748_solve(f, ax, bx, tol, max_iter, policies::policy<>());
}

template <class F, class T, class Tol, class Policy>
std::pair<T, T> bracket_and_solve_root(F f, const T& guess, T factor, bool rising, Tol tol, boost::uintmax_t& max_iter, const Policy& pol)
{
   BOOST_MATH_STD_USING
   static const char* function = "boost::math::tools::bracket_and_solve_root<%1%>";
   //
   // Set up inital brackets:
   //
   T a = guess;
   T b = a;
   T fa = f(a);
   T fb = fa;
   //
   // Set up invocation count:
   //
   boost::uintmax_t count = max_iter - 1;

   int step = 32;

   if((fa < 0) == (guess < 0 ? !rising : rising))
   {
      //
      // Zero is to the right of b, so walk upwards
      // until we find it:
      //
      while((boost::math::sign)(fb) == (boost::math::sign)(fa))
      {
         if(count == 0)
            return boost::math::detail::pair_from_single(policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", b, pol));
         //
         // Heuristic: normally it's best not to increase the step sizes as we'll just end up
         // with a really wide range to search for the root.  However, if the initial guess was *really*
         // bad then we need to speed up the search otherwise we'll take forever if we're orders of
         // magnitude out.  This happens most often if the guess is a small value (say 1) and the result
         // we're looking for is close to std::numeric_limits<T>::min().
         //
         if((max_iter - count) % step == 0)
         {
            factor *= 2;
            if(step > 1) step /= 2;
         }
         //
         // Now go ahead and move our guess by "factor":
         //
         a = b;
         fa = fb;
         b *= factor;
         fb = f(b);
         --count;
         BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
      }
   }
   else
   {
      //
      // Zero is to the left of a, so walk downwards
      // until we find it:
      //
      while((boost::math::sign)(fb) == (boost::math::sign)(fa))
      {
         if(fabs(a) < tools::min_value<T>())
         {
            // Escape route just in case the answer is zero!
            max_iter -= count;
            max_iter += 1;
            return a > 0 ? std::make_pair(T(0), T(a)) : std::make_pair(T(a), T(0));
         }
         if(count == 0)
            return boost::math::detail::pair_from_single(policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", a, pol));
         //
         // Heuristic: normally it's best not to increase the step sizes as we'll just end up
         // with a really wide range to search for the root.  However, if the initial guess was *really*
         // bad then we need to speed up the search otherwise we'll take forever if we're orders of
         // magnitude out.  This happens most often if the guess is a small value (say 1) and the result
         // we're looking for is close to std::numeric_limits<T>::min().
         //
         if((max_iter - count) % step == 0)
         {
            factor *= 2;
            if(step > 1) step /= 2;
         }
         //
         // Now go ahead and move are guess by "factor":
         //
         b = a;
         fb = fa;
         a /= factor;
         fa = f(a);
         --count;
         BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
      }
   }
   max_iter -= count;
   max_iter += 1;
   std::pair<T, T> r = toms748_solve(
      f,
      (a < 0 ? b : a),
      (a < 0 ? a : b),
      (a < 0 ? fb : fa),
      (a < 0 ? fa : fb),
      tol,
      count,
      pol);
   max_iter += count;
   BOOST_MATH_INSTRUMENT_CODE("max_iter = " << max_iter << " count = " << count);
   BOOST_MATH_LOG_COUNT(max_iter)
   return r;
}

template <class F, class T, class Tol>
inline std::pair<T, T> bracket_and_solve_root(F f, const T& guess, const T& factor, bool rising, Tol tol, boost::uintmax_t& max_iter)
{
   return bracket_and_solve_root(f, guess, factor, rising, tol, max_iter, policies::policy<>());
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_SOLVE_ROOT_HPP


/*
 *
 *encoder_to_wrist.cpp
 *
 *  Created on: Nov 16 2015
 *      Author: Camilo P
 */

// input/output management
#include <iostream>
// file input/output management
#include <fstream>
#include <ros/ros.h>
#include <stdio.h>
#include <math.h>
#include <sensor_msgs/JointState.h>
//#include <sensor_msgs/Range.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
//#include <Eigen/Geometry>
//#include <Eigen/Dense>
#include "std_srvs/Empty.h"

#define PI        3.14159265358979323
#define CLOSE_GRASP 1.0
#define OPEN_GRASP  2.0
#define CLOSE_SPREAD  4.0
#define OPEN_SPREAD  8.0

//Eigen::VectorXf joint_position(7);

float roll_encoder_value, pitch_encoder_value, yaw_encoder_value, roll_rad_value, pitch_rad_value,yaw_rad_value, button_press;

//Service Clients
ros::ServiceClient grasp_open_srv;
ros::ServiceClient grasp_close_srv;
ros::ServiceClient spread_open_srv;
ros::ServiceClient spread_close_srv;
bool open_grasp, close_grasp;
bool open_spread, close_spread;
float open_grasp_st, close_grasp_st;
float open_spread_st, close_spread_st;
std_srvs::Empty empty;
/*void wamJointStatesCallback(const sensor_msgs::JointState::ConstPtr& jointMessage)
{
  joint_position[0] = jointMessage->position[0];
  joint_position[1] = jointMessage->position[1];
  joint_position[2] = jointMessage->position[2];
  joint_position[3] = jointMessage->position[3];
  joint_position[4] = jointMessage->position[4];
  joint_position[5] = jointMessage->position[5];
  joint_position[6] = jointMessage->position[6];
  }*/

void wristCallback(const geometry_msgs::Quaternion::ConstPtr& wristMessage)
{
  open_grasp = close_grasp = open_spread = close_spread = false;
  roll_encoder_value=wristMessage->x;
  pitch_encoder_value=wristMessage->y;
  yaw_encoder_value=wristMessage->z;
  button_press=wristMessage->w;

  //limit roll_joint
  if(roll_encoder_value>=0 && roll_encoder_value<1024)
    {
      roll_rad_value=(roll_encoder_value)*2*PI/4096;
    }
  else if(roll_encoder_value>=3072 && roll_encoder_value<4096)
    {
      roll_encoder_value=roll_encoder_value-3072;
      roll_encoder_value=1024-roll_encoder_value;
      roll_rad_value=-(roll_encoder_value)*2*PI/4096;
    }

  //limit pitch_joint
  pitch_encoder_value=(pitch_encoder_value-1410)*2*PI/4096;

  if(pitch_encoder_value<1.50 && pitch_encoder_value>-1.50)
    {
      pitch_rad_value=-pitch_encoder_value;
    }

  /////////////////////button_State_logic
 // Gripper Grasp Command
    // Checking to see if gripper open button is being pressed
  if(button_press== OPEN_GRASP && (open_grasp_st != OPEN_GRASP)) {
        open_grasp = true; // set grasp publish state
    }
    //Checking to see if gripper close button is being pressed
  else if(button_press == CLOSE_GRASP && (close_grasp_st != CLOSE_GRASP)) {
        close_grasp = true; // set grasp publish state
    }
    open_grasp_st = button_press;
    close_grasp_st = button_press;

    // Gripper Spread Command
    if (button_press==OPEN_SPREAD && (open_spread_st != OPEN_SPREAD)) {
        open_spread = true;
    } else if (button_press==CLOSE_SPREAD && (close_spread_st != CLOSE_SPREAD)) {
        close_spread = true;
    }
    open_spread_st = button_press;
    close_spread_st = button_press;

  //////////////////
}

void update()
{

  if (open_grasp) {
        ROS_INFO("Grasp OPEN");
        grasp_open_srv.call(empty);
    } else if (close_grasp) {
        ROS_INFO("Grasp CLOSE");
        grasp_close_srv.call(empty);
    } else if (open_spread) {
        ROS_INFO("Spread OPEN");
        spread_open_srv.call(empty);
    } else if (close_spread) {
        ROS_INFO("Spread CLOSE");
        spread_close_srv.call(empty);
    }

}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "encoder_to_wrist");
  ros::NodeHandle nh;
//Subscribers
//  ros::Subscriber jointSates_sub = nh.subscribe("/zeus/wam/joint_states", 1, wamJointStatesCallback);
  ros::Subscriber wrist_sub = nh.subscribe("/wrist", 1, wristCallback);

//Publishers

  ros::Publisher joint_pub = nh.advertise<sensor_msgs::JointState>("/zeus/wam/jnt_hand_tool_cmd", 1);

 //Service Clients

  grasp_open_srv = nh.serviceClient<std_srvs::Empty>("zeus/bhand/open_grasp");
  grasp_close_srv = nh.serviceClient<std_srvs::Empty>("zeus/bhand/close_grasp");
  spread_open_srv = nh.serviceClient<std_srvs::Empty>("zeus/bhand/open_spread");
  spread_close_srv = nh.serviceClient<std_srvs::Empty>("zeus/bhand/close_spread");

  ros::Rate loop_rate(80);
  ROS_INFO("Main loop");

  //sensor_msgs::JointState send_joints;
  //send_joints.position.resize(7);
  while(ros::ok())
    {
       sensor_msgs::JointState send_joints;
      //send_joints.position.resize(7);
      send_joints.position.push_back(0.0);
      send_joints.position.push_back(0.0);
      send_joints.position.push_back(0.0);
      send_joints.position.push_back(0.0);
      send_joints.position.push_back(0.0);
      send_joints.position.push_back(0.0);
      // send_joints.position.push_back(pitch_rad_value);
      send_joints.position.push_back(0.0);
      // send_joints.position.push_back(roll_rad_value);
      joint_pub.publish(send_joints);
      update();
      ros::spinOnce();
      loop_rate.sleep();
    }
}


///////////////////////////////////////////////////////////////////////////////
// acv0.hpp                                                                  //
//                                                                           //
//  Copyright 2008 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ACCUMULATORS_STATISTICS_ACV0_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_ACV0_HPP_ER_2008_04
#include <cmath>

#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>

#include <boost/call_traits.hpp>
//#include <boost/assert.hpp>
#include <boost/array.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/type_traits/add_const.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

#include <boost/accumulators/statistics/acvf.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ////////////////////////////////////////////////////////////////////////////
    // acv0_impl
    template<typename T,typename I>
    class acv0_impl
      : public accumulator_base
    {
    public:
        typedef T result_type;
        acv0_impl(dont_care)
        {}

        template<typename Args>
        void operator()(Args const &args)
        {
            val = (*begin(acvf<I>(args[accumulator])));
        }

        result_type result(dont_care) const
        {
            return val;
        }
    private:
        T val;
    };

} // namespace impl
///////////////////////////////////////////////////////////////////////////////
// tag::acv0
//

namespace tag
{
    template <typename I = default_delay_discriminator>
    struct acv0
      : depends_on<acvf<I> >
    {
        /// INTERNAL ONLY
      typedef
        accumulators::impl::acv0_impl<
            mpl::_1,I> impl;

    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::acv0
//

namespace extract
{

  template<typename I,typename AccumulatorSet>
  typename mpl::apply<
    AccumulatorSet,tag::acv0<I>
    >::type::result_type
  acv0(AccumulatorSet const& acc){
    typedef tag::acv0<I> the_tag;
    return extract_result<the_tag>(acc);
  }

}

using extract::acv0;

}} // namespace boost::accumulators

#endif


#include "../../include/waveO1/observer.hpp"
#include "../../include/waveO1/utilities.hpp"
#include "../../include/mymfem/utilities.hpp"

#include <iostream>
#include <Eigen/Core>

//! Computes DG jumps on time-like face at a specified time
double WaveO1Observer
:: eval_dgFtimeJump (std::shared_ptr<GridFunction>& p,
                     std::shared_ptr<GridFunction>& v) const
{
    double jumpp=0;
    double jumpv=0;

    // assumes that all elements have the same geometry
    auto faceElGeomType = p->FESpace()->GetMesh()
                ->GetFaceGeometryType(0);

    // jump in pressure
    // set integration rules
    IntegrationRules rule1{};
    const IntegrationRule *ir1;
    int order1 = 2*p->FESpace()->GetFE(0)->GetOrder()+2;
    ir1 = &rule1.Get(faceElGeomType, order1);
    m_assembleDgJumps->SetIntegrationRule(ir1);
    jumpp = m_assembleDgJumps->FtimeScalar(p.get());

    // jump in velocity
    // set integration rules
    IntegrationRules rule2{};
    const IntegrationRule *ir2;
    int order2 = 2*v->FESpace()->GetFE(0)->GetOrder()+2;
    ir2 = &rule2.Get(faceElGeomType, order2);
    m_assembleDgJumps->SetIntegrationRule(ir2);
    jumpv = m_assembleDgJumps->FtimeVector(v.get());

    return (jumpp + jumpv);
}

//! Computes DG jump for a space-like face
//! at a specified time
double WaveO1Observer
:: eval_dgFspaceJump (int degp, int degv, Mesh* mesh,
                      double t, std::shared_ptr
                      <WaveO1InvSqMediumCoeff>& invSqMed)
const
{
    double tracep=0;
    double tracev=0;

    // assumes that all elements have the same geometry
    auto elGeomType = mesh->GetElementBaseGeometry(0);

    IntegrationRules rule1{}, rule2{};
    const IntegrationRule *ir1, *ir2;

    // pressure
    int order1 = 2*degp+4;
    ir1 = &rule1.Get(elGeomType, order1);
    m_assembleDgJumps->SetIntegrationRule(ir1);
    m_pE_coeff->SetTime(t);
    tracep = m_assembleDgJumps->FspaceScalar
            (mesh, m_pE_coeff.get(), invSqMed.get());

    // velocity
    int order2 = 2*degv+4;
    ir2 = &rule2.Get(elGeomType, order2);
    m_assembleDgJumps->SetIntegrationRule(ir2);
    tracev = m_assembleDgJumps->FspaceVector
            (mesh, m_vE_coeff.get());

    return (tracep + tracev);
}

double WaveO1Observer
:: eval_dgFspaceJump (std::shared_ptr<GridFunction>& p,
                      std::shared_ptr<GridFunction>& v,
                      std::shared_ptr
                      <WaveO1InvSqMediumCoeff>& invSqMed)
const
{
    double jumpp=0;
    double jumpv=0;

    // assumes that all elements have the same geometry
    auto elGeomType = p->FESpace()
                ->GetFE(0)->GetGeomType();

    IntegrationRules rule1{}, rule2{};
    const IntegrationRule *ir1, *ir2;

    // pressure
    int order1 = 2*p->FESpace()->GetFE(0)->GetOrder()+2;
    ir1 = &rule1.Get(elGeomType, order1);
    m_assembleDgJumps->SetIntegrationRule(ir1);
    jumpp = m_assembleDgJumps->FspaceScalar
            (p.get(), invSqMed.get());

    // velocity
    int order2 = 2*v->FESpace()->GetFE(0)->GetOrder()+2;
    ir2 = &rule2.Get(elGeomType, order2);
    m_assembleDgJumps->SetIntegrationRule(ir2);
    jumpv = m_assembleDgJumps->FspaceVector(v.get());

    return (jumpp + jumpv);
}

double WaveO1Observer
:: eval_dgFspaceJump (std::shared_ptr<GridFunction>& p1,
                      std::shared_ptr<GridFunction>& p2,
                      std::shared_ptr<GridFunction>& v1,
                      std::shared_ptr<GridFunction>& v2,
                      std::shared_ptr
                      <WaveO1InvSqMediumCoeff>& invSqMed)
const
{
    double jumpp=0;
    double jumpv=0;

    // assumes that all elements have the same geometry
    auto elGeomType = p1->FESpace()
                ->GetFE(0)->GetGeomType();

    IntegrationRules rule1{}, rule2{};
    const IntegrationRule *ir1, *ir2;

    // pressure
    int order1 = 2*p1->FESpace()->GetFE(0)->GetOrder()+2;
    ir1 = &rule1.Get(elGeomType, order1);
    m_assembleDgJumps->SetIntegrationRule(ir1);
    jumpp = m_assembleDgJumps->FspaceScalar
            (p1.get(), p2.get(), invSqMed.get());

    // velocity
    int order2 = 2*v1->FESpace()->GetFE(0)->GetOrder()+2;
    ir2 = &rule2.Get(elGeomType, order2);
    m_assembleDgJumps->SetIntegrationRule(ir2);
    jumpv = m_assembleDgJumps->FspaceVector
            (v1.get(), v2.get());

    return (jumpp + jumpv);
}

// Computes energy
Vector WaveO1Observer
:: eval_energy(BlockVector& W) const
{
    int Nt = m_tWspace->GetNE();
    int xdimW1 = m_xW1space->GetTrueVSize();
    int xdimW2 = m_xW2space->GetTrueVSize();

    Vector energy(Nt+1);

    Vector& p = W.GetBlock(0);
    Vector& v = W.GetBlock(1);

    Vector pSol1(xdimW1), pSol2(xdimW1);
    auto pGSol1 = std::make_shared<GridFunction>
            (m_xW1space, pSol1);
    auto pGSol2 = std::make_shared<GridFunction>
            (m_xW1space, pSol2);

    Vector vSol1(xdimW2), vSol2(xdimW2);
    auto vGSol1 = std::make_shared<GridFunction>
            (m_xW2space, vSol1);
    auto vGSol2 = std::make_shared<GridFunction>
            (m_xW2space, vSol2);

    // medium
    auto invSqMed = std::make_shared
            <WaveO1InvSqMediumCoeff>(m_testCase);

    // energy @t=0
    int degp = pGSol1->FESpace()->GetFE(0)->GetOrder();
    int degv = vGSol1->FESpace()->GetFE(0)->GetOrder();
    double initEnergy = 0.5*eval_dgFspaceJump
            (degp, degv, pGSol1->FESpace()->GetMesh(),
             0, invSqMed);

    ElementTransformation *tTrans = nullptr;
    const FiniteElement *tFe = nullptr;
    Vector tShape;
    Array<int> tVdofs;

    // for time-like faces
    Eigen::VectorXd bufEnergyFtime(Nt);
    bufEnergyFtime.setZero();

    double jumpFtime;
    for (int n=0; n<Nt; n++)
    {
        m_tWspace->GetElementVDofs(n, tVdofs);
        tTrans = m_tWspace->GetElementTransformation(n);

        tFe = m_tWspace->GetFE(n);
        int tNdofs = tFe->GetDof();
        tShape.SetSize(tNdofs);

        int order = 2*tFe->GetOrder()+2;
        const IntegrationRule *ir
                = &IntRules.Get(tFe->GetGeomType(), order);

        for (int i = 0; i < ir->GetNPoints(); i++)
        {
            const IntegrationPoint &ip = ir->IntPoint(i);
            tTrans->SetIntPoint(&ip);
            tFe->CalcShape(ip, tShape);
            build_xSol_FG(p, tShape, tVdofs, pSol1);
            build_xSol_FG(v, tShape, tVdofs, vSol1);

            jumpFtime = eval_dgFtimeJump(pGSol1, vGSol1);

            double w = ip.weight*tTrans->Weight();
            bufEnergyFtime(n) += w*jumpFtime;
        }
    }

    // for space-like faces
    Eigen::VectorXd bufEnergyFspace(Nt-1);
    bufEnergyFspace.setZero();

    IntegrationPoint ip0, ip1;
    ip0.Set1w(0, 1);
    ip1.Set1w(1, 1);

    // for 0 < t < T
    for (int n=1; n<Nt; n++)
    {
        int nm = n-1;
        int np = n;

        // build solution at tn^{-}
        m_tWspace->GetElementVDofs(nm, tVdofs);
        tTrans = m_tWspace->GetElementTransformation(nm);
        tFe = m_tWspace->GetFE(nm);
        int tNdofs = tFe->GetDof();
        tShape.SetSize(tNdofs);

        tTrans->SetIntPoint(&ip1);
        tFe->CalcShape(ip1, tShape);
        build_xSol_FG(p, tShape, tVdofs, pSol1);
        build_xSol_FG(v, tShape, tVdofs, vSol1);

        // build solution at tn^{+}
        m_tWspace->GetElementVDofs(np, tVdofs);
        tTrans = m_tWspace->GetElementTransformation(np);
        tFe = m_tWspace->GetFE(np);
        tNdofs = tFe->GetDof();
        tShape.SetSize(tNdofs);

        tTrans->SetIntPoint(&ip0);
        tFe->CalcShape(ip0, tShape);
        build_xSol_FG(p, tShape, tVdofs, pSol2);
        build_xSol_FG(v, tShape, tVdofs, vSol2);

        bufEnergyFspace(n-1) = 0.5*eval_dgFspaceJump
                (pGSol1, pGSol2, vGSol1, vGSol2, invSqMed);
    }

    energy(0) = initEnergy;
    energy(1) = energy(0)
            - bufEnergyFtime(0);
    for (int n=2; n<=Nt; n++) {
        energy(n) = energy(n-1)
                - bufEnergyFtime(n-1) - bufEnergyFspace(n-2);
    }

    return energy;
}

// End of file


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

  @copyright 2017 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)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ALGORITHM_REDUCE_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_REDUCE_HPP_INCLUDED

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/function/sum.hpp>
#include <boost/simd/pack.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Computes the sum over elements in the given Contiguous Range @range{first,last} and
    the initial value @c init.

    \notebox{The summation order can be different from the order of a sequential summation
            , thus leading to different results.
            }

    @par Example:
    @snippet reduce.simple.cpp reduce-simple
    Possible output:
    @code
    SIMD reduce     : 45
    STD  accumulate : 45
    @endcode

    @param first  Beginning of the range of elements to sum
    @param last   End of the range of elements to sum
    @param init   Initial value of the sum

    @return The sum of the given value and elements in the given range.
  **/
  template<typename T> T reduce(T const* first, T const* last, T init)
  {
    pack<T> acc(0);
    auto pr = segmented_aligned_range(first,last);

    for( T        e : pr.head ) init += e;
    for( pack<T>  e : pr.body ) acc  += e;
    for( T        e : pr.tail ) init += e;

    return init + sum(acc);
  }

  /*!
    @ingroup group-algo

     Reduces the range [first; last), possibly permuted and aggregated in unspecified manner,
     along with the initial value init over binary_op.

    \notebox{The summation order can be different from the order of a sequential summation
            , thus leading to different results.
            }

    @par Example:
    @snippet reduce.phases.cpp reduce-phases
    Possible output:
    @code
    SIMD reduce     : 285
    @endcode

    @param first    Beginning of the range of elements to sum
    @param last     End of the range of elements to sum
    @param init     Initial value of the reduction
    @param binop    Binary function object that will be applied in unspecified order to the
                    result of dereferencing the input pointers, the results of other @c binop
                    and @ init.

    @return The generalized sum of the given init value and elements in the given range over @ binop.
  **/
  template<typename T, typename F>
  T reduce( T const* first, T const* last, T init, F binop )
  {
    auto pr = segmented_aligned_range(first,last);
    for( T v : pr.head ) init = binop(init, v);
    auto b = pr.body.begin();
    auto e = pr.body.end();
    if (b !=  e){
      pack<T> acc(*b++);
      for( ; b != e; b++) acc  = binop(acc, pack<T>(*b));
      for( T     x : acc) init = binop(init, x);
    }
    for( T v : pr.tail ) init = binop(init, v);
    return init;
  }

} }

#endif


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include <boost/test/unit_test.hpp>

#include <numeric>
#include <vector>

#include "Search_UnitTestHelpers.hpp"
// clang-format off
#include "ArborXTest_TreeTypeTraits.hpp"
// clang-format on

BOOST_AUTO_TEST_SUITE(Callbacks)

namespace tt = boost::test_tools;

template <typename DeviceType>
struct CustomInlineCallback
{
  using tag = ArborX::Details::InlineCallbackTag;
  Kokkos::View<ArborX::Point *, DeviceType> points;
  ArborX::Point const origin = {{0., 0., 0.}};
  template <typename Query, typename Insert>
  KOKKOS_FUNCTION void operator()(Query const &, int index,
                                  Insert const &insert) const
  {
    float const distance_to_origin =
        ArborX::Details::distance(points(index), origin);
    insert({index, distance_to_origin});
  }
};

template <typename DeviceType>
struct CustomPostCallback
{
  using tag = ArborX::Details::PostCallbackTag;
  Kokkos::View<ArborX::Point *, DeviceType> points;
  ArborX::Point const origin = {{0., 0., 0.}};
  template <typename Predicates, typename InOutView, typename InView,
            typename OutView>
  void operator()(Predicates const &, InOutView &offset, InView in,
                  OutView &out) const
  {
    using ExecutionSpace = typename DeviceType::execution_space;
    using ArborX::Details::distance;
    auto const n = offset.extent(0) - 1;
    ArborX::reallocWithoutInitializing(out, in.extent(0));
    // NOTE woraround to avoid implicit capture of *this
    auto const &points_ = points;
    auto const &origin_ = origin;
    Kokkos::parallel_for(
        Kokkos::RangePolicy<ExecutionSpace>(0, n), KOKKOS_LAMBDA(int i) {
          for (int j = offset(i); j < offset(i + 1); ++j)
          {
            out(j) = {in(j), (float)distance(points_(in(j)), origin_)};
          }
        });
  }
};

BOOST_AUTO_TEST_CASE_TEMPLATE(callback_spatial_predicate, TreeTypeTraits,
                              TreeTypeTraitsList)
{
  using Tree = typename TreeTypeTraits::type;
  using ExecutionSpace = typename TreeTypeTraits::execution_space;
  using DeviceType = typename TreeTypeTraits::device_type;

  int const n = 10;
  Kokkos::View<ArborX::Point *, DeviceType> points(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "points"), n);
  ArborX::Point const origin = {{0., 0., 0.}};
  Kokkos::parallel_for(Kokkos::RangePolicy<ExecutionSpace>(0, n),
                       KOKKOS_LAMBDA(int i) {
                         points(i) = {{(double)i, (double)i, (double)i}};
                       });
  auto points_host =
      Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, points);

  std::vector<Kokkos::pair<int, float>> values;
  values.reserve(n);
  for (int i = 0; i < n; ++i)
    values.emplace_back(i, ArborX::Details::distance(points_host(i), origin));
  std::vector<int> offsets = {0, n};

  Tree const tree(ExecutionSpace{}, points);

  ARBORX_TEST_QUERY_TREE_CALLBACK(ExecutionSpace{}, tree,
                                  makeIntersectsBoxQueries<DeviceType>({
                                      static_cast<ArborX::Box>(tree.bounds()),
                                  }),
                                  CustomInlineCallback<DeviceType>{points},
                                  make_compressed_storage(offsets, values));

  ARBORX_TEST_QUERY_TREE_CALLBACK(ExecutionSpace{}, tree,
                                  makeIntersectsBoxQueries<DeviceType>({
                                      static_cast<ArborX::Box>(tree.bounds()),
                                  }),
                                  CustomPostCallback<DeviceType>{points},
                                  make_compressed_storage(offsets, values));
}

#ifndef ARBORX_TEST_DISABLE_NEAREST_QUERY
BOOST_AUTO_TEST_CASE_TEMPLATE(callback_nearest_predicate, TreeTypeTraits,
                              TreeTypeTraitsList)
{
  using Tree = typename TreeTypeTraits::type;
  using ExecutionSpace = typename TreeTypeTraits::execution_space;
  using DeviceType = typename TreeTypeTraits::device_type;

  int const n = 10;
  Kokkos::View<ArborX::Point *, DeviceType> points(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "points"), n);
  ArborX::Point const origin = {{0., 0., 0.}};
  Kokkos::parallel_for(Kokkos::RangePolicy<ExecutionSpace>(0, n),
                       KOKKOS_LAMBDA(int i) {
                         points(i) = {{(double)i, (double)i, (double)i}};
                       });
  auto points_host =
      Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, points);

  std::vector<Kokkos::pair<int, float>> values;
  values.reserve(n);
  for (int i = 0; i < n; ++i)
    values.emplace_back(i, ArborX::Details::distance(points_host(i), origin));
  std::vector<int> offsets = {0, n};

  Tree const tree(ExecutionSpace{}, points);

  ARBORX_TEST_QUERY_TREE_CALLBACK(ExecutionSpace{}, tree,
                                  makeNearestQueries<DeviceType>({
                                      {origin, n},
                                  }),
                                  CustomInlineCallback<DeviceType>{points},
                                  make_compressed_storage(offsets, values));

  ARBORX_TEST_QUERY_TREE_CALLBACK(ExecutionSpace{}, tree,
                                  makeNearestQueries<DeviceType>({
                                      {origin, n},
                                  }),
                                  CustomPostCallback<DeviceType>{points},
                                  make_compressed_storage(offsets, values));
}
#endif

#ifndef ARBORX_TEST_DISABLE_CALLBACK_EARLY_EXIT
template <class DeviceType>
struct Experimental_CustomCallbackEarlyExit
{
  Kokkos::View<int *, DeviceType, Kokkos::MemoryTraits<Kokkos::Atomic>> counts;
  template <class Predicate>
  KOKKOS_FUNCTION auto operator()(Predicate const &predicate, int) const
  {
    int i = getData(predicate);

    if (counts(i)++ < i)
    {
      return ArborX::CallbackTreeTraversalControl::normal_continuation;
    }

    return ArborX::CallbackTreeTraversalControl::early_exit;
  }
};

BOOST_AUTO_TEST_CASE_TEMPLATE(callback_early_exit, TreeTypeTraits,
                              TreeTypeTraitsList)
{
  using Tree = typename TreeTypeTraits::type;
  using ExecutionSpace = typename TreeTypeTraits::execution_space;
  using DeviceType = typename TreeTypeTraits::device_type;

  auto const tree =
      make<Tree>(ExecutionSpace{}, {
                                       {{{0., 0., 0.}}, {{0., 0., 0.}}},
                                       {{{1., 1., 1.}}, {{1., 1., 1.}}},
                                       {{{2., 2., 2.}}, {{2., 2., 2.}}},
                                       {{{3., 3., 3.}}, {{3., 3., 3.}}},
                                   });

  Kokkos::View<int *, DeviceType> counts("counts", 4);

  std::vector<int> counts_ref(4);
  std::iota(counts_ref.begin(), counts_ref.end(), 1);

  auto b = static_cast<ArborX::Box>(tree.bounds());
  auto predicates = makeIntersectsBoxWithAttachmentQueries<DeviceType, int>(
      {b, b, b, b}, {0, 1, 2, 3});

  tree.query(ExecutionSpace{}, predicates,
             Experimental_CustomCallbackEarlyExit<DeviceType>{counts});

  auto counts_host =
      Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, counts);

  BOOST_TEST(counts_host == counts_ref, tt::per_element());
}
#endif

template <typename DeviceType>
struct CustomInlineCallbackWithAttachment
{
  using tag = ArborX::Details::InlineCallbackTag;
  Kokkos::View<ArborX::Point *, DeviceType> points;
  ArborX::Point const origin = {{0., 0., 0.}};
  template <typename Query, typename Insert>
  KOKKOS_FUNCTION void operator()(Query const &query, int index,
                                  Insert const &insert) const
  {
    float const distance_to_origin =
        ArborX::Details::distance(points(index), origin);

    auto data = ArborX::getData(query);
    insert({index, data + distance_to_origin});
  }
};
template <typename DeviceType>
struct CustomPostCallbackWithAttachment
{
  using tag = ArborX::Details::PostCallbackTag;
  Kokkos::View<ArborX::Point *, DeviceType> points;
  ArborX::Point const origin = {{0., 0., 0.}};
  template <typename Predicates, typename InOutView, typename InView,
            typename OutView>
  void operator()(Predicates const &queries, InOutView &offset, InView in,
                  OutView &out) const
  {
    using ExecutionSpace = typename DeviceType::execution_space;
    using ArborX::Details::distance;
    auto const n = offset.extent(0) - 1;
    ArborX::reallocWithoutInitializing(out, in.extent(0));
    // NOTE workaround to avoid implicit capture of *this
    auto const &points_ = points;
    auto const &origin_ = origin;
    Kokkos::parallel_for(
        Kokkos::RangePolicy<ExecutionSpace>(0, n), KOKKOS_LAMBDA(int i) {
          auto data_2 = ArborX::getData(queries(i));
          auto data = data_2[1];
          for (int j = offset(i); j < offset(i + 1); ++j)
          {
            out(j) = {in(j), data + (float)distance(points_(in(j)), origin_)};
          }
        });
  }
};

BOOST_AUTO_TEST_CASE_TEMPLATE(callback_with_attachment_spatial_predicate,
                              TreeTypeTraits, TreeTypeTraitsList)
{
  using Tree = typename TreeTypeTraits::type;
  using ExecutionSpace = typename TreeTypeTraits::execution_space;
  using DeviceType = typename TreeTypeTraits::device_type;

  int const n = 10;
  Kokkos::View<ArborX::Point *, DeviceType> points(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "points"), n);
  ArborX::Point const origin = {{0., 0., 0.}};
  Kokkos::parallel_for(Kokkos::RangePolicy<ExecutionSpace>(0, n),
                       KOKKOS_LAMBDA(int i) {
                         points(i) = {{(double)i, (double)i, (double)i}};
                       });
  auto points_host =
      Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, points);

  std::vector<Kokkos::pair<int, float>> values;
  values.reserve(n);
  float const delta = 5.f;
  for (int i = 0; i < n; ++i)
    values.emplace_back(
        i, delta + ArborX::Details::distance(points_host(i), origin));
  std::vector<int> offsets = {0, n};

  Tree const tree(ExecutionSpace{}, points);

  ARBORX_TEST_QUERY_TREE_CALLBACK(
      ExecutionSpace{}, tree,
      (makeIntersectsBoxWithAttachmentQueries<DeviceType, float>(
          {static_cast<ArborX::Box>(tree.bounds())}, {delta})),
      CustomInlineCallbackWithAttachment<DeviceType>{points},
      make_compressed_storage(offsets, values));

  ARBORX_TEST_QUERY_TREE_CALLBACK(
      ExecutionSpace{}, tree,
      (makeIntersectsBoxWithAttachmentQueries<DeviceType,
                                              Kokkos::Array<float, 2>>(
          {static_cast<ArborX::Box>(tree.bounds())}, {{0., delta}})),
      CustomPostCallbackWithAttachment<DeviceType>{points},
      make_compressed_storage(offsets, values));
}

#ifndef ARBORX_TEST_DISABLE_NEAREST_QUERY
BOOST_AUTO_TEST_CASE_TEMPLATE(callback_with_attachment_nearest_predicate,
                              TreeTypeTraits, TreeTypeTraitsList)
{
  using Tree = typename TreeTypeTraits::type;
  using ExecutionSpace = typename TreeTypeTraits::execution_space;
  using DeviceType = typename TreeTypeTraits::device_type;

  int const n = 10;
  Kokkos::View<ArborX::Point *, DeviceType> points(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "points"), n);
  ArborX::Point const origin = {{0., 0., 0.}};
  Kokkos::parallel_for(Kokkos::RangePolicy<ExecutionSpace>(0, n),
                       KOKKOS_LAMBDA(int i) {
                         points(i) = {{(double)i, (double)i, (double)i}};
                       });
  auto points_host =
      Kokkos::create_mirror_view_and_copy(Kokkos::HostSpace{}, points);

  std::vector<Kokkos::pair<int, float>> values;
  values.reserve(n);
  float const delta = 5.f;
  for (int i = 0; i < n; ++i)
    values.emplace_back(
        i, delta + ArborX::Details::distance(points_host(i), origin));
  std::vector<int> offsets = {0, n};

  Tree const tree(ExecutionSpace{}, points);

  ARBORX_TEST_QUERY_TREE_CALLBACK(
      ExecutionSpace{}, tree,
      (makeNearestWithAttachmentQueries<DeviceType, float>({{origin, n}},
                                                           {delta})),
      CustomInlineCallbackWithAttachment<DeviceType>{points},
      make_compressed_storage(offsets, values));

  ARBORX_TEST_QUERY_TREE_CALLBACK(
      ExecutionSpace{}, tree,
      (makeNearestWithAttachmentQueries<DeviceType, Kokkos::Array<float, 2>>(
          {{origin, n}}, {{0, delta}})),
      CustomPostCallbackWithAttachment<DeviceType>{points},
      make_compressed_storage(offsets, values));
}
#endif

BOOST_AUTO_TEST_SUITE_END()


#include <boost/accumulators/statistics/weighted_skewness.hpp>


//  Copyright 2018 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/special_functions.hpp>
#include <boost/chrono.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>

template <class Clock>
struct stopwatch
{
   typedef typename Clock::duration duration;
   stopwatch()
   {
      m_start = Clock::now();
   }
   duration elapsed()
   {
      return Clock::now() - m_start;
   }
   void reset()
   {
      m_start = Clock::now();
   }

 private:
   typename Clock::time_point m_start;
};

template <class T>
T generate_random()
{
   typedef int                   e_type;
   static boost::random::mt19937 gen;
   T                             val      = gen();
   T                             prev_val = -1;
   while (val != prev_val)
   {
      val *= (gen.max)();
      prev_val = val;
      val += gen();
   }
   e_type e;
   val = frexp(val, &e);

   static boost::random::uniform_int_distribution<e_type> ui(-20, 20);
   return ldexp(val, ui(gen));
}

template <typename T>
double my_convert_to_double(const T& x)
{
   double ret = 0;
   if (isfinite(x))
   {
      if (x.backend().exponent() >= -1023 - 52 && x != 0)
      {
         if (x.backend().exponent() <= 1023)
         {
            int     e  = x.backend().exponent();
            T       y  = ldexp(abs(x), 55 - e);
            T       t  = trunc(y);
            int64_t ti = t.template convert_to<int64_t>();
            if ((ti & 1) == 0)
            {
               if (t < y)
                  ti |= 1;
            }
            if (e >= -1023 + 1)
            {
               ret = ldexp(double(ti), e - 55);
            }
            else
            {
               // subnormal
               typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float<128, boost::multiprecision::backends::digit_base_2> > cpp_bin_float128_t;
               cpp_bin_float128_t                                                                                sx = ldexp(cpp_bin_float128_t(ti), e - 55);
               sx += DBL_MIN;
               e                     = -1023 + 1;
               cpp_bin_float128_t sy = ldexp(sx, 55 - e);
               cpp_bin_float128_t st = trunc(sy);
               ti                    = st.convert_to<int64_t>();
               if ((ti & 1) == 0)
               {
                  if (st < sy)
                     ti |= 1;
               }
               ret = ldexp(double(ti), e - 55) - DBL_MIN;
            }
         }
         else
         {
            // overflow
            ret = HUGE_VAL;
         }
      }
   }
   else
   {
      if (isnan(x))
         return nan("");
      // inf
      ret = HUGE_VAL;
   }
   return x.backend().sign() ? -ret : ret;
}

template <class T>
void test_conversion_time(const char* name)
{
   std::cout << "Testing times for type: " << name << "\n";
   std::vector<T> values;

   for (unsigned i = 0; i < 10000000; ++i)
   {
      values.push_back(generate_random<T>());
   }

   boost::chrono::duration<double>                 time;
   stopwatch<boost::chrono::high_resolution_clock> c;

   double total = 0;

   for (typename std::vector<T>::const_iterator i = values.begin(); i != values.end(); ++i)
   {
      total += my_convert_to_double(*i);
   }

   time = c.elapsed();
   std::cout << std::setprecision(3) << std::fixed;
   std::cout << "Reference time: " << std::setw(7) << std::right << time << " (total sum = " << total << ")" << std::endl;

   c.reset();

   total = 0;

   for (typename std::vector<T>::const_iterator i = values.begin(); i != values.end(); ++i)
   {
      total += i->template convert_to<double>();
   }

   time = c.elapsed();
   std::cout << "Boost time:     " << std::setw(7) << std::right << time << " (total sum = " << total << ")" << std::endl;
}

int main()
{
   using namespace boost::multiprecision;

   test_conversion_time<cpp_bin_float_double>("cpp_bin_float_double");
   test_conversion_time<cpp_bin_float_quad>("cpp_bin_float_quad");
   test_conversion_time<cpp_bin_float_oct>("cpp_bin_float_oct");
   test_conversion_time<cpp_bin_float_50>("cpp_bin_float_50");
   test_conversion_time<cpp_bin_float_100>("cpp_bin_float_100");

   return 0;
}