#pragma once

#include <cmath>
#include <vector>
#include <stdexcept>
#include <string.h>
#include <Eigen/Core>

#include "traits.hh"
#include "../util/assert.hh"

namespace bold
{
  template<typename T>
  class MovingAverage
  {
  public:
    MovingAverage(unsigned short windowSize)
    : d_items(new T[windowSize]),
      d_length(0),
      d_nextPointer(0),
      d_sum(),
      d_windowSize(windowSize)
    {
      if (windowSize == 0)
        throw new std::runtime_error("Cannot have zero window size.");
      AveragingTraits::zero(d_sum);
    }

    MovingAverage(MovingAverage const& other)
    {
      copy(other);
    }

    ~MovingAverage()
    {
      destroy();
    }

    MovingAverage& operator=(MovingAverage const& other)
    {
      destroy();
      copy(other);
      return *this;
    }

    int count() const { return d_length; }
    int getWindowSize() const { return d_windowSize; }
    bool isMature() const { return d_length == d_windowSize; }

    T next(T value)
    {
      if (d_length == d_windowSize)
      {
        d_sum -= d_items[d_nextPointer];
      }
      else
      {
        d_length++;
      }

      d_items[d_nextPointer] = value;
      d_sum += value;
      d_nextPointer = (d_nextPointer + 1) % d_windowSize;

      d_avg = d_sum / int{d_length};

      return d_avg;
    }

    void reset()
    {
      d_length = 0;
      d_nextPointer = 0;
      AveragingTraits::zero(d_sum);
    }

    T getAverage() const { return d_avg; }

    T calculateStdDev()
    {
      // TODO unit test this
      T sum;
      AveragingTraits::zero(sum);
      for (int i = 0; i < d_length; i++)
      {
        int index = (d_nextPointer - i - 1) % d_windowSize;
        if (index < 0)
          index += d_windowSize;
        ASSERT(index >= 0 && index < d_length);
        T diff = d_items[index] - d_avg;
        sum += diff * diff;
      }
      return sqrt(sum / d_length);
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  private:
    T* d_items;
    unsigned short d_length;
    int d_nextPointer;
    T d_sum;
    T d_avg;
    unsigned short d_windowSize;

    void destroy()
    {
      delete[] d_items;
    }

    void copy(MovingAverage const& other)
    {
      d_windowSize = other.d_windowSize;
      d_items = new T[other.d_windowSize];
      for (unsigned i = 0; i < d_windowSize; ++i)
        d_items[i] = other.d_items[i];

      d_length = other.d_length;
      d_nextPointer = other.d_nextPointer;
      d_sum = other.d_sum;
      d_avg = other.d_avg;
    }
  };

  template <>
  inline Eigen::Vector3d MovingAverage<Eigen::Vector3d>::calculateStdDev()
  {
    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    for (int i = 0; i < d_length; i++)
    {
      int index = (d_nextPointer - i - 1) % d_windowSize;
      if (index < 0)
        index += d_windowSize;
      ASSERT(index >= 0 && index < d_length);
      Eigen::Vector3d diff = d_items[index] - d_avg;
      sum += diff * diff.transpose();
    }
    Eigen::Vector3d r = sum.diagonal();
    for (int i = 0; i < r.size(); i++)
      r[i] = sqrt(r[i]);
    return r / d_length;
  }

  template <>
  inline Eigen::Vector2d MovingAverage<Eigen::Vector2d>::calculateStdDev()
  {
//     std::cout << "------------------------" << std::endl;
//     std::cout << "d_avg = " << d_avg.transpose() << std::endl;
    Eigen::Matrix2d sum = Eigen::Matrix2d::Zero();
    for (int i = 0; i < d_length; i++)
    {
      int index = (d_nextPointer - i - 1) % d_windowSize;
      if (index < 0)
        index += d_windowSize;
      ASSERT(index >= 0 && index < d_length);
      Eigen::Vector2d diff = d_items[index] - d_avg;
      sum += diff * diff.transpose();
//       std::cout << "---- index " << index << std::endl
//           << "diff " << diff.transpose() << std::endl
//           << "sum" << std::endl << sum << std::endl;
    }
    Eigen::Vector2d r = sum.diagonal();
//     std::cout << "r = " << r.transpose() << std::endl;
    for (int i = 0; i < r.size(); i++)
      r[i] = sqrt(r[i]);
//     std::cout << "r_squared = " << r.transpose() << std::endl;
//     std::cout << "stddev = " << (r/d_length).transpose() << std::endl;
    return r / d_length;
  }
}


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/bits.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_positive.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/true.hpp>
#include <boost/simd/function/is_negative.hpp>
#include <boost/simd/function/is_positive.hpp>

template <typename T, int N, typename Env>
void test(Env& $)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  T a1[N], b[N], a2[N], c[N];
  for(int i = 0; i < N; ++i)
  {
    a1[i] = ((i%2) ? T(2*i) : T(-2*i))/T(3);
    a2[i] = T(2*i+1)/T(2);
    b[i] = bs::nearbyint(a1[i]) ;
    c[i] = bs::nearbyint(a2[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t aa2(&a2[0], &a2[0]+N);
  p_t bb (&b[0], &b[0]+N);
  p_t cc (&c[0], &c[0]+N);
  STF_IEEE_EQUAL(bs::nearbyint(aa1), bb);
  STF_IEEE_EQUAL(bs::nearbyint(aa2), cc);
}

STF_CASE_TPL("Check nearbyint on pack" , STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL("Check nearbyint on halfs" , STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  p_t a(1.5);
  p_t b(2.5);
  for(int i = 1; i <= 9; i+= 1)
  {
    STF_IEEE_EQUAL(bs::nearbyint(p_t(i+T(0.5))), p_t(i%2 ? i+1 : i ));
    STF_IEEE_EQUAL(bs::nearbyint(p_t(-i+T(0.5))), p_t(i%2 ? -i+1 : -i ));
  }
  STF_EXPECT(bs::is_negative(nearbyint(bs::Mzero<T>())));
  STF_EXPECT(bs::is_positive(nearbyint(bs::Zero<T>())));
}

STF_CASE_TPL ( "nearbyint real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::nearbyint;
  using p_t = bs::pack<T>;
  using r_t = decltype(nearbyint(p_t()));

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

  // specific values tests
  STF_IEEE_EQUAL(nearbyint(p_t(1.4)), p_t(1));
  STF_IEEE_EQUAL(nearbyint(p_t(1.5)), p_t(2));
  STF_IEEE_EQUAL(nearbyint(p_t(1.6)), p_t(2));
  STF_IEEE_EQUAL(nearbyint(p_t(2.5)), p_t(2));
  STF_IEEE_EQUAL(nearbyint(bs::Half<p_t>()), bs::Zero<r_t>());
  STF_IEEE_EQUAL(nearbyint(bs::Inf<p_t>()), bs::Inf<r_t>());
  STF_IEEE_EQUAL(nearbyint(bs::Mhalf<p_t>()), bs::Zero<r_t>());
  STF_IEEE_EQUAL(nearbyint(bs::Minf<p_t>()), bs::Minf<r_t>());
  STF_IEEE_EQUAL(nearbyint(bs::Mone<p_t>()), bs::Mone<r_t>());
  STF_IEEE_EQUAL(nearbyint(bs::Nan<p_t>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(nearbyint(bs::One<p_t>()), bs::One<r_t>());
  STF_IEEE_EQUAL(nearbyint(bs::Zero<p_t>()), bs::Zero<r_t>());
  STF_IEEE_EQUAL(nearbyint(bs::Nan<p_t>()), bs::Nan<r_t>());
  STF_EQUAL(bs::is_negative(nearbyint(bs::Mzero<p_t>())), bs::True<r_t>());
  STF_EQUAL(bs::is_positive(nearbyint(bs::Zero<p_t>())), bs::True<r_t>());
} // end of test for floating_


// Copyright 2018 Hans Dembinski
//
// 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)

//[ guide_custom_2d_axis

#include <boost/histogram.hpp>
#include <cassert>

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

  // axis which returns 1 if the input falls inside the unit circle and zero otherwise
  struct circle_axis {
    // accepts a 2D point in form of a std::tuple
    axis::index_type index(const std::tuple<double, double>& point) const {
      const auto x = std::get<0>(point);
      const auto y = std::get<1>(point);
      return x * x + y * y <= 1.0;
    }

    axis::index_type size() const { return 2; }
  };

  auto h1 = make_histogram(circle_axis());

  // fill looks normal for a histogram which has only one Nd-axis
  h1(0, 0);   // in
  h1(0, -1);  // in
  h1(0, 1);   // in
  h1(-1, 0);  // in
  h1(1, 0);   // in
  h1(1, 1);   // out
  h1(-1, -1); // out

  // 2D histogram, but only 1D index
  assert(h1.at(0) == 2); // out
  assert(h1.at(1) == 5); // in

  // other axes can be combined with a Nd-axis
  auto h2 = make_histogram(circle_axis(), axis::category<std::string>({"red", "blue"}));

  // now we need to pass arguments for Nd-axis explicitly as std::tuple
  h2(std::make_tuple(0, 0), "red");
  h2(std::make_tuple(1, 1), "blue");

  // 3D histogram, but only 2D index
  assert(h2.at(0, 0) == 0); // out, red
  assert(h2.at(0, 1) == 1); // out, blue
  assert(h2.at(1, 0) == 1); // in, red
  assert(h2.at(1, 1) == 0); // in, blue
}

//]


/**
 * \file boost/numeric/ublasx/test/relational_opts.cpp
 *
 * \brief Test suite for matrix/vector relational operators.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublasx/detail/debug.hpp>
#include <boost/numeric/ublasx/operation/relational_ops.hpp>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

BOOST_UBLASX_TEST_DEF( equal_real_vector )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Vector - Equality");

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;

	const ::std::size_t n(4);

	vector_type u;
	vector_type v;

	bool res;
	bool expect;

	u = ublas::scalar_vector<value_type>(n, 1);
	v = ublas::scalar_vector<value_type>(n, 1);
	res = u == v;
	expect = true;
	BOOST_UBLASX_DEBUG_TRACE("u=" << u);
	BOOST_UBLASX_DEBUG_TRACE("v=" << v);
	BOOST_UBLASX_DEBUG_TRACE("u==v? " << std::boolalpha << res);
	BOOST_UBLASX_TEST_CHECK( res == expect );

	u = ublas::scalar_vector<value_type>(n, 1);
	v = 2*ublas::scalar_vector<value_type>(n, 1);
	res = u == v;
	expect = false;
	BOOST_UBLASX_DEBUG_TRACE("u=" << u);
	BOOST_UBLASX_DEBUG_TRACE("v=" << v);
	BOOST_UBLASX_DEBUG_TRACE("u==v? " << std::boolalpha << res);
	BOOST_UBLASX_TEST_CHECK( res == expect );
}

BOOST_UBLASX_TEST_DEF( not_equal_real_vector )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Vector - Inequality");

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;

	const ::std::size_t n(4);

	vector_type u;
	vector_type v;

	bool res;
	bool expect;

	u = ublas::scalar_vector<value_type>(n, 1);
	v = 2*ublas::scalar_vector<value_type>(n, 1);
	res = u != v;
	expect = true;
	BOOST_UBLASX_DEBUG_TRACE("u=" << u);
	BOOST_UBLASX_DEBUG_TRACE("v=" << v);
	BOOST_UBLASX_DEBUG_TRACE("u!=v? " << std::boolalpha << res);
	BOOST_UBLASX_TEST_CHECK( res == expect );

	u = ublas::scalar_vector<value_type>(n, 1);
	v = ublas::scalar_vector<value_type>(n, 1);
	res = u != v;
	expect = false;
	BOOST_UBLASX_DEBUG_TRACE("u=" << u);
	BOOST_UBLASX_DEBUG_TRACE("v=" << v);
	BOOST_UBLASX_DEBUG_TRACE("u!=v? " << std::boolalpha << res);
	BOOST_UBLASX_TEST_CHECK( res == expect );
}

BOOST_UBLASX_TEST_DEF( equal_real_matrix )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Matrix - Equality");

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(4);

	matrix_type A;
	matrix_type B;

	bool res;
	bool expect;

	A = ublas::identity_matrix<value_type>(nr,nc);
	B = ublas::identity_matrix<value_type>(nr,nc);
	res = A == B;
	expect = true;
	BOOST_UBLASX_DEBUG_TRACE("A=" << A);
	BOOST_UBLASX_DEBUG_TRACE("B=" << B);
	BOOST_UBLASX_DEBUG_TRACE("A==B? " << std::boolalpha << res);
	BOOST_UBLASX_TEST_CHECK( res == expect );

	A = ublas::identity_matrix<value_type>(nr,nc);
	B = 2*ublas::identity_matrix<value_type>(nr,nc);
	res = A == B;
	expect = false;
	BOOST_UBLASX_DEBUG_TRACE("A=" << A);
	BOOST_UBLASX_DEBUG_TRACE("B=" << B);
	BOOST_UBLASX_DEBUG_TRACE("A==B? " << std::boolalpha << res);
	BOOST_UBLASX_TEST_CHECK( res == expect );
}

BOOST_UBLASX_TEST_DEF( not_equal_real_matrix )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Real Matrix - Inequality");

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(4);

	matrix_type A;
	matrix_type B;

	bool res;
	bool expect;

	A = ublas::identity_matrix<value_type>(nr,nc);
	B = 2*ublas::identity_matrix<value_type>(nr,nc);
	res = A != B;
	expect = true;
	BOOST_UBLASX_DEBUG_TRACE("A=" << A);
	BOOST_UBLASX_DEBUG_TRACE("B=" << B);
	BOOST_UBLASX_DEBUG_TRACE("A!=B? " << std::boolalpha << res);
	BOOST_UBLASX_TEST_CHECK( res == expect );

	A = ublas::identity_matrix<value_type>(nr,nc);
	B = ublas::identity_matrix<value_type>(nr,nc);
	res = A != B;
	expect = false;
	BOOST_UBLASX_DEBUG_TRACE("A=" << A);
	BOOST_UBLASX_DEBUG_TRACE("B=" << B);
	BOOST_UBLASX_DEBUG_TRACE("A!=B? " << std::boolalpha << res);
	BOOST_UBLASX_TEST_CHECK( res == expect );
}

int main()
{
	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( equal_real_vector );
	BOOST_UBLASX_TEST_DO( not_equal_real_vector );
	BOOST_UBLASX_TEST_DO( equal_real_matrix );
	BOOST_UBLASX_TEST_DO( not_equal_real_matrix );

	BOOST_UBLASX_TEST_END();
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2015 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-2
 */

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>

#include <fstream>

using namespace dealii;

void
show_statistics(const SparsityPattern &sparsity_pattern)
{
  Vector<int> v;
  double      sum = 0.0;
  for (unsigned int i = 0; i < sparsity_pattern.n_cols(); ++i)
    {
      sum += sparsity_pattern.row_length(i);
    }

  std::cout << "Statistics for the current sparsity pattern:"
            << "\n"
            << "bandwidth: " << sparsity_pattern.bandwidth() << "\t"
            << " number of unknowns : " << sparsity_pattern.n_cols() << "\t"
            << "average number of of entries per row"
            << sum / sparsity_pattern.n_cols() << "\n";
}

void
make_grid(Triangulation<2> &triangulation)
{
  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 5);

  static const SphericalManifold<2> manifold_description(center);
  triangulation.set_all_manifold_ids(0);
  triangulation.set_manifold(0, manifold_description);

  for (unsigned int step = 0; step < 3; ++step)
    {
      Triangulation<2>::active_cell_iterator cell =
                                               triangulation.begin_active(),
                                             endc = triangulation.end();

      for (; cell != endc; ++cell)
        for (unsigned int v = 0; v < GeometryInfo<2>::vertices_per_cell; ++v)
          {
            const double distance_from_center =
              center.distance(cell->vertex(v));

            if (std::fabs(distance_from_center - inner_radius) < 1e-10)
              {
                cell->set_refine_flag();
                break;
              }
          }

      triangulation.execute_coarsening_and_refinement();
    }
}

void
distribute_dofs(DoFHandler<2> &dof_handler)
{
  static const FE_Q<2> finite_element(
    1); // degree of poly 1: bilinear element, 2:bi-quadratic element, ...
  // std::cout << "Value at (1,1) "
  //           << finite_element.shape_value(2, Point<2>(1, 1)) << "\n";

  dof_handler.distribute_dofs(
    finite_element); // we have associated a degree of freedom with a global
                     // number to each vertex

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());

  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);
  std::cout << "Number of entries per row in the sparsity pattern"
            << "\n";
  // SparseMatrix<double> system_matrix;
  // system_matrix.reinit(sparsity_pattern);

  for (unsigned int i = 0; i < dof_handler.n_dofs(); ++i)
    {
      std::cout << "Length of " << i << "-th row"
                << "\t" << sparsity_pattern.row_length(i) << "\n";
    }

  show_statistics(sparsity_pattern);
  std::ofstream out("sparsity_pattern1.svg");
  sparsity_pattern.print_svg(out);

  std::cout
    << "Printing all the non-zero entries for row 42 before renumbering: "
    << "\n";

  auto start_row_42 = sparsity_pattern.begin(42);
  auto end_row_42   = sparsity_pattern.end(42);
  for (auto it = start_row_42; it != end_row_42; ++it)
    {
      std::cout << (*it).column() << "\n";
    }
}

void
renumber_dofs(DoFHandler<2> &dof_handler)
{
  DoFRenumbering::Cuthill_McKee(dof_handler);

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

  std::cout << "Row length for Line 42: " << sparsity_pattern.row_length(42)
            << "\n";

  show_statistics(sparsity_pattern); // Bonus
  std::ofstream out("sparsity_pattern2.svg");
  sparsity_pattern.print_svg(out);

  std::cout
    << "Printing all the non-zero entries for row 42 after renumbering: "
    << "\n";
  for (auto it = sparsity_pattern.begin(42); it != sparsity_pattern.end(42);
       ++it)
    {
      std::cout << (*it).column() << "\n";
    }
}

void
make_grid_square(Triangulation<2> &square)
{
  GridGenerator::hyper_cube(square, -1, 1);
  square.refine_global(3);
}

int
main()
{
  Triangulation<2> triangulation;
  make_grid(triangulation);

  DoFHandler<2> dof_handler(triangulation);

  distribute_dofs(dof_handler);
  renumber_dofs(dof_handler);

  // //Test with a square
  // Triangulation<2> square;
  // make_grid_square(square);

  // DoFHandler<2> dof_handler_square(square);

  // distribute_dofs(dof_handler_square);
  // renumber_dofs(dof_handler_square);
}


#include <tuple>
#include <vector>
#include <string>
#include <cmath>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <boost/optional/optional_io.hpp>

#include "CommonSetting.h"
#include "SamplingGrid.h"

using cgal = CGAL::Exact_predicates_inexact_constructions_kernel;

pl::SamplingGrid::SamplingGrid() : width(0), height(0) {}

pl::SamplingGrid::SamplingGrid(int w, int h) : width(w), height(h) {}

pl::SamplingGrid::SamplingGrid(CommonSetting common_setting) : SamplingGrid() {
  std::string sampling_grid = common_setting.val;

  auto t = pl::split(sampling_grid, 'x');
  width = std::stoi(t[0]);
  height = std::stoi(t[1]);
}

pl::SamplingGrid &pl::SamplingGrid::operator=(const SamplingGrid &cs) {
  width = cs.width;
  height = cs.height;
  return *this;
}

bool pl::SamplingGrid::isOutOfArea(const cgal::Point_2 p) const {
  return p.x() < 0 || width < p.x() || p.y() < 0 || height < p.y();
}

bool pl::SamplingGrid::isInsideOfArea(const cgal::Point_2 p) const {
  return (0 - 1.0e-10) <= p.x() && p.x() <= (width + 1.0e-10) && (0 - 1.0e-10) <= p.y() && p.y() <= (height + 1.0e-10);
}

std::tuple<cgal::Point_2, cgal::Point_2> pl::SamplingGrid::intersection(const cgal::Line_2 &line) const {
  std::vector<cgal::Segment_2> border = {
    {{width, height}, {0, height}},
    {{width, 0}, {width, height}},
    {{0, 0}, {width, 0}},
    {{0, height}, {0, 0}}
  };

  std::vector<cgal::Point_2> intersections;

  for (auto segment : border)
    if (const auto point = CGAL::intersection(cgal::Line_2{segment}, line); point && isInsideOfArea(boost::get<cgal::Point_2>(*point)))
      intersections.push_back(boost::get<cgal::Point_2>(*point));

  if (intersections.size() < 2 || 2 < intersections.size()) {
    std::cout << "intersection with border does have more then 2 or less then 2 intersections" << "\n";
    std::exit(-1);
  }

  return {intersections[0], intersections[1]};
}

cgal::Point_2 pl::SamplingGrid::center() const {
  cgal::Point_2 p(0, 0), q(width, 0), r(0, height);
  const auto circle = cgal::Circle_2(p, q, r);
  return circle.center();
}

std::vector<cgal::Point_2> pl::SamplingGrid::points() const {
  return {{0,0},{0,height},{width,height},{width,0}};
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2012 Desire Nuentsa Wakam <desire.nuentsa_wakam@inria.fr>
// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
#include "sparse.h"
#include <Eigen/SparseQR>

template<typename MatrixType,typename DenseMat>
int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 150)
{
  eigen_assert(maxRows >= maxCols);
  typedef typename MatrixType::Scalar Scalar;
  int rows = internal::random<int>(1,maxRows);
  int cols = internal::random<int>(1,maxCols);
  double density = (std::max)(8./(rows*cols), 0.01);

  A.resize(rows,cols);
  dA.resize(rows,cols);
  initSparse<Scalar>(density, dA, A,ForceNonZeroDiag);
  A.makeCompressed();
  int nop = internal::random<int>(0, internal::random<double>(0,1) > 0.5 ? cols/2 : 0);
  for(int k=0; k<nop; ++k)
  {
    int j0 = internal::random<int>(0,cols-1);
    int j1 = internal::random<int>(0,cols-1);
    Scalar s = internal::random<Scalar>();
    A.col(j0)  = s * A.col(j1);
    dA.col(j0) = s * dA.col(j1);
  }

//   if(rows<cols) {
//     A.conservativeResize(cols,cols);
//     dA.conservativeResize(cols,cols);
//     dA.bottomRows(cols-rows).setZero();
//   }

  return rows;
}

template<typename Scalar> void test_sparseqr_scalar()
{
  typedef SparseMatrix<Scalar,ColMajor> MatrixType;
  typedef Matrix<Scalar,Dynamic,Dynamic> DenseMat;
  typedef Matrix<Scalar,Dynamic,1> DenseVector;
  MatrixType A;
  DenseMat dA;
  DenseVector refX,x,b;
  SparseQR<MatrixType, COLAMDOrdering<int> > solver;
  generate_sparse_rectangular_problem(A,dA);

  b = dA * DenseVector::Random(A.cols());
  solver.compute(A);
  if(internal::random<float>(0,1)>0.5f)
    solver.factorize(A);  // this checks that calling analyzePattern is not needed if the pattern do not change.
  if (solver.info() != MySuccess)
  {
    std::cerr << "sparse QR factorization failed\n";
    exit(0);
    return;
  }
  x = solver.solve(b);
  if (solver.info() != MySuccess)
  {
    std::cerr << "sparse QR factorization failed\n";
    exit(0);
    return;
  }

  VERIFY_IS_APPROX(A * x, b);

  //Compare with a dense QR solver
  ColPivHouseholderQR<DenseMat> dqr(dA);
  refX = dqr.solve(b);

  VERIFY_IS_EQUAL(dqr.rank(), solver.rank());
  if(solver.rank()==A.cols()) // full rank
    VERIFY_IS_APPROX(x, refX);
//   else
//     VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() );

  // Compute explicitly the matrix Q
  MatrixType Q, QtQ, idM;
  Q = solver.matrixQ();
  //Check  ||Q' * Q - I ||
  QtQ = Q * Q.adjoint();
  idM.resize(Q.rows(), Q.rows()); idM.setIdentity();
  VERIFY(idM.isApprox(QtQ));

  // Q to dense
  DenseMat dQ;
  dQ = solver.matrixQ();
  VERIFY_IS_APPROX(Q, dQ);
}
void test_sparseqr()
{
  for(int i=0; i<g_repeat; ++i)
  {
    CALL_SUBTEST_1(test_sparseqr_scalar<double>());
    CALL_SUBTEST_2(test_sparseqr_scalar<std::complex<double> >());
  }
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <boost/math/quaternion.hpp>

#include "DataStructures/DataVector.hpp"
#include "Domain/FunctionsOfTime/QuaternionHelpers.hpp"
#include "Utilities/Gsl.hpp"

SPECTRE_TEST_CASE("Unit.Domain.FunctionsOfTime.QuaternionHelpers",
                  "[Unit][Domain]") {
  using quaternion = boost::math::quaternion<double>;

  quaternion quat1(1.0, 2.0, 3.0, -4.0);
  DataVector dv{1.0, 2.0, 3.0, -4.0};
  DataVector dv_short{9.9, 9.8, 9.7};

  DataVector dv2 = quaternion_to_datavector(quat1);
  CHECK(dv2 == dv);

  quaternion quat2 = datavector_to_quaternion(dv2);
  CHECK(quat2 == quat1);

  quaternion quat3 = datavector_to_quaternion(dv_short);
  CHECK(quat3 == quaternion{0.0, 9.9, 9.8, 9.7});

  normalize_quaternion(make_not_null(&quat2));
  CHECK(norm(quat2) == approx(1.0));
  CHECK(abs(quat2) == approx(1.0));
}


/**
 * @file momentum_sgd_test.cpp
 * @author Ryan Curtin
 *
 * Test file for MomentumSGD (stochastic gradient descent with momentum updates).
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/sgd/sgd.hpp>
#include <mlpack/core/optimizers/sgd/update_policies/gradient_clipping.hpp>
#include <mlpack/core/optimizers/sgd/update_policies/momentum_update.hpp>
#include <mlpack/core/optimizers/problems/generalized_rosenbrock_function.hpp>
#include <mlpack/core/optimizers/problems/sgd_test_function.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace std;
using namespace arma;
using namespace mlpack;
using namespace mlpack::optimization;
using namespace mlpack::optimization::test;

BOOST_AUTO_TEST_SUITE(MomentumSGDTest);

BOOST_AUTO_TEST_CASE(MomentumSGDSpeedUpTestFunction)
{
  SGDTestFunction f;
  MomentumUpdate momentumUpdate(0.7);
  MomentumSGD s(0.0003, 1, 2500000, 1e-9, true, momentumUpdate);

  arma::mat coordinates = f.GetInitialPoint();
  double result = s.Optimize(f, coordinates);

  BOOST_REQUIRE_CLOSE(result, -1.0, 0.15);
  BOOST_REQUIRE_SMALL(coordinates[0], 1e-3);
  BOOST_REQUIRE_SMALL(coordinates[1], 1e-7);
  BOOST_REQUIRE_SMALL(coordinates[2], 1e-7);

  // Compare with SGD with vanilla update.
  SGDTestFunction f1;
  StandardSGD s1(0.0003, 1, 2500000, 1e-9, true);

  arma::mat coordinates1 = f.GetInitialPoint();
  double result1 = s1.Optimize(f1, coordinates1);

  // Result doesn't converge in 2500000 iterations.
  BOOST_REQUIRE_GT(result1 + 1.0, 0.05);
  BOOST_REQUIRE_GE(coordinates1[0], 1e-3);
  BOOST_REQUIRE_SMALL(coordinates1[1], 1e-7);
  BOOST_REQUIRE_SMALL(coordinates1[2], 1e-7);

  BOOST_REQUIRE_LE(result, result1);
}

BOOST_AUTO_TEST_CASE(GeneralizedRosenbrockTest)
{
  // Loop over several variants.
  for (size_t i = 10; i < 50; i += 5)
  {
    // Create the generalized Rosenbrock function.
    GeneralizedRosenbrockFunction f(i);
    MomentumUpdate momentumUpdate(0.4);
    MomentumSGD s(0.0008, 1, 0, 1e-15, true, momentumUpdate);

    arma::mat coordinates = f.GetInitialPoint();
    double result = s.Optimize(f, coordinates);

    BOOST_REQUIRE_SMALL(result, 1e-4);
    for (size_t j = 0; j < i; ++j)
      BOOST_REQUIRE_CLOSE(coordinates[j], (double) 1.0, 1e-3);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef SEQUENTIAL_LINE_SEARCH_ACQUISITION_FUNCTION_HPP
#define SEQUENTIAL_LINE_SEARCH_ACQUISITION_FUNCTION_HPP

#include <Eigen/Core>
#include <memory>
#include <sequential-line-search/regressor.hpp>
#include <vector>

namespace sequential_line_search
{
    enum class AcquisitionFuncType
    {
        ExpectedImprovement,
        GaussianProcessUpperConfidenceBound,
    };

    namespace acquisition_func
    {
        /// \brief Calculate the value of the acquisition function value.
        ///
        /// \param function_type Type of the acquisition function.
        ///
        /// \param gaussian_process_upper_confidence_bound_hyperparam The hyperparameter in the GP-UCB algorithm, which
        /// controls the trade-off of exploration and exploitation. If the acquisition function is not GP-UCB, this
        /// value will not be used.
        double CalcAcquisitionValue(const Regressor&          regressor,
                                    const Eigen::VectorXd&    x,
                                    const AcquisitionFuncType func_type,
                                    const double              gaussian_process_upper_confidence_bound_hyperparam = 1.0);

        /// \param gaussian_process_upper_confidence_bound_hyperparam The hyperparameter in the GP-UCB algorithm, which
        /// controls the trade-off of exploration and exploitation. If the acquisition function is not GP-UCB, this
        /// value will not be used.
        Eigen::VectorXd
        CalcAcquisitionValueDerivative(const Regressor&          regressor,
                                       const Eigen::VectorXd&    x,
                                       const AcquisitionFuncType func_type,
                                       const double gaussian_process_upper_confidence_bound_hyperparam = 1.0);

        /// \param num_global_search_iters The number of trials of acquisition value maximization. Specifying a large
        /// number is helpful for finding the global maximizer while it increases the computational cost proportional to
        /// it.
        ///
        /// \param gaussian_process_upper_confidence_bound_hyperparam The hyperparameter in the GP-UCB algorithm, which
        /// controls the trade-off of exploration and exploitation. If the acquisition function is not GP-UCB, this
        /// value will not be used.
        Eigen::VectorXd FindNextPoint(const Regressor&          regressor,
                                      const unsigned            num_global_search_iters = 100,
                                      const unsigned            num_local_search_iters  = 50,
                                      const AcquisitionFuncType func_type = AcquisitionFuncType::ExpectedImprovement,
                                      const double gaussian_process_upper_confidence_bound_hyperparam = 1.0);

        /// \brief Find the next n sampled points that should be observed.
        ///
        /// \details The points will be determined by Schonlau et al.'s method [1998]. This method determines the points
        /// one by one sequentially by maximizing the acquisition function. In each maximization, the variance of the
        /// surrogate function (or the covariance matrix of the underlying GP model) is updated using the newly sampled
        /// point, by which it avoids sampling similar points.
        ///
        /// Matthias Schonlau, William J. Welch, and Donald R. Jones. 1998. Global versus local search in constrained
        /// optimization of computer models. Institute of Mathematical Statistics Lecture Notes - Monograph Series,
        /// 1998: 11-25 (1998). DOI: https://doi.org/10.1214/lnms/1215456182
        ///
        /// \param num_points The number of the sampled points.
        ///
        /// \param num_global_search_iters The number of trials of acquisition value maximization. Specifying a large
        /// number is helpful for finding the global maximizer while it increases the computational cost proportional to
        /// it.
        ///
        /// \param gaussian_process_upper_confidence_bound_hyperparam The hyperparameter in the GP-UCB algorithm, which
        /// controls the trade-off of exploration and exploitation. If the acquisition function is not GP-UCB, this
        /// value will not be used.
        std::vector<Eigen::VectorXd>
        FindNextPoints(const Regressor&          regressor,
                       const unsigned            num_points,
                       const unsigned            num_global_search_iters = 100,
                       const unsigned            num_local_search_iters  = 50,
                       const AcquisitionFuncType func_type               = AcquisitionFuncType::ExpectedImprovement,
                       const double              gaussian_process_upper_confidence_bound_hyperparam = 1.0);
    } // namespace acquisition_func
} // namespace sequential_line_search

#endif // SEQUENTIAL_LINE_SEARCH_ACQUISITION_FUNCTION_HPP


/*
 * NativeFilterMatrixOps.cpp
 *
 *  Created on: Sep 5, 2018
 *      Author: Georg Wiedebach
 */

#include <jni.h>
#include <Eigen/Dense>
#include "us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper.h"

using Eigen::MatrixXd;

typedef Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> JMatrixMap;

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_computeABAt(JNIEnv *env, jobject thisObj, jdoubleArray result,
      jdoubleArray aData, jdoubleArray bData, jint n, jint m)
{
   jdouble *aDataArray = (jdouble*) env->GetPrimitiveArrayCritical(aData, NULL);
   jdouble *bDataArray = (jdouble*) env->GetPrimitiveArrayCritical(bData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap A(aDataArray, n, m);
   JMatrixMap B(bDataArray, m, m);
   JMatrixMap ABAt(resultDataArray, n, n);

   ABAt.noalias() = A * B * A.transpose();

   env->ReleasePrimitiveArrayCritical(aData, aDataArray, 0);
   env->ReleasePrimitiveArrayCritical(bData, bDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 0);
}

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_predictErrorCovariance
  (JNIEnv *env, jobject thisObj, jdoubleArray result, jdoubleArray fData, jdoubleArray pData, jdoubleArray qData, jint n)
{
   jdouble *fDataArray = (jdouble*) env->GetPrimitiveArrayCritical(fData, NULL);
   jdouble *pDataArray = (jdouble*) env->GetPrimitiveArrayCritical(pData, NULL);
   jdouble *qDataArray = (jdouble*) env->GetPrimitiveArrayCritical(qData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap F(fDataArray, n, n);
   JMatrixMap P(pDataArray, n, n);
   JMatrixMap Q(qDataArray, n, n);
   JMatrixMap errorCovariance(resultDataArray, n, n);

   MatrixXd Qdiag = Q.diagonal().asDiagonal();
   errorCovariance.noalias() = F * P.selfadjointView<Eigen::Upper>() * F.transpose() + Qdiag;

   env->ReleasePrimitiveArrayCritical(fData, fDataArray, 0);
   env->ReleasePrimitiveArrayCritical(pData, pDataArray, 0);
   env->ReleasePrimitiveArrayCritical(qData, qDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 0);
}

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_updateErrorCovariance
  (JNIEnv *env, jobject thisObj, jdoubleArray result, jdoubleArray kData, jdoubleArray hData, jdoubleArray pData, jint n, jint m)
{
   jdouble *kDataArray = (jdouble*) env->GetPrimitiveArrayCritical(kData, NULL);
   jdouble *hDataArray = (jdouble*) env->GetPrimitiveArrayCritical(hData, NULL);
   jdouble *pDataArray = (jdouble*) env->GetPrimitiveArrayCritical(pData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap K(kDataArray, m, n);
   JMatrixMap H(hDataArray, n, m);
   JMatrixMap P(pDataArray, m, m);
   JMatrixMap errorCovariance(resultDataArray, m, m);

   errorCovariance.noalias() = (MatrixXd::Identity(m, m) - K * H) * P.selfadjointView<Eigen::Upper>();

   env->ReleasePrimitiveArrayCritical(kData, kDataArray, 0);
   env->ReleasePrimitiveArrayCritical(hData, hDataArray, 0);
   env->ReleasePrimitiveArrayCritical(pData, pDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 0);
}

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_computeKalmanGain
  (JNIEnv *env, jobject thisObj, jdoubleArray result, jdoubleArray pData, jdoubleArray hData, jdoubleArray rData, jint n, jint m)
{
   jdouble *pDataArray = (jdouble*) env->GetPrimitiveArrayCritical(pData, NULL);
   jdouble *hDataArray = (jdouble*) env->GetPrimitiveArrayCritical(hData, NULL);
   jdouble *rDataArray = (jdouble*) env->GetPrimitiveArrayCritical(rData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap P(pDataArray, m, m);
   JMatrixMap H(hDataArray, n, m);
   JMatrixMap R(rDataArray, n, n);
   JMatrixMap gain(resultDataArray, m, n);

   MatrixXd PHt = P.selfadjointView<Eigen::Upper>() * H.transpose();
   MatrixXd Rdiag = R.diagonal().asDiagonal();
   MatrixXd toInvert = H * PHt + Rdiag;
   gain.noalias() = PHt * toInvert.inverse();

   env->ReleasePrimitiveArrayCritical(pData, pDataArray, 0);
   env->ReleasePrimitiveArrayCritical(hData, hDataArray, 0);
   env->ReleasePrimitiveArrayCritical(rData, rDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 0);
}

JNIEXPORT void JNICALL Java_us_ihmc_ekf_filter_NativeFilterMatrixOpsWrapper_updateState
  (JNIEnv *env, jobject thisObj, jdoubleArray result, jdoubleArray xData, jdoubleArray kData, jdoubleArray rData, jint n, jint m)
{
   jdouble *xDataArray = (jdouble*) env->GetPrimitiveArrayCritical(xData, NULL);
   jdouble *kDataArray = (jdouble*) env->GetPrimitiveArrayCritical(kData, NULL);
   jdouble *rDataArray = (jdouble*) env->GetPrimitiveArrayCritical(rData, NULL);
   jdouble *resultDataArray = (jdouble*) env->GetPrimitiveArrayCritical(result, NULL);

   JMatrixMap x(xDataArray, n, 1);
   JMatrixMap K(kDataArray, n, m);
   JMatrixMap r(rDataArray, m, 1);
   JMatrixMap state(resultDataArray, n, 1);

   state.noalias() = x + K * r;

   env->ReleasePrimitiveArrayCritical(xData, xDataArray, 0);
   env->ReleasePrimitiveArrayCritical(kData, kDataArray, 0);
   env->ReleasePrimitiveArrayCritical(rData, rDataArray, 0);
   env->ReleasePrimitiveArrayCritical(result, resultDataArray, 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)

#include <pch.hpp>

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"

#if !defined(TEST_FLOAT) && !defined(TEST_DOUBLE) && !defined(TEST_LDOUBLE) && !defined(TEST_REAL_CONCEPT)
#  define TEST_FLOAT
#  define TEST_DOUBLE
#  define TEST_LDOUBLE
#  define TEST_REAL_CONCEPT
#endif

//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the incomplete gamma function inverses
// gamma_p_inva and gamma_q_inva. There are two sets of tests:
// 2) TODO: Accuracy tests use values generated with NTL::RR at
// 1000-bit precision and our generic versions of these functions.
// 3) Round trip sanity checks, use the test data for the forward
// functions, and verify that we can get (approximately) back
// where we started.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double";
   }
   else
   {
      largest_type = "long double";
   }
#else
   largest_type = "(long\\s+)?double";
#endif
   //
   // Linux:
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "linux",                          // platform
      largest_type,                     // test type(s)
      "[^|]*",                          // test data group
      "[^|]*", 800, 200);               // test function

   //
   // Catch all cases come last:
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*",                          // test data group
      "[^|]*", 3000, 1000);             // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*",                          // test data group
      "[^|]*", 300, 100);               // test function
   // this one has to come last in case double *is* the widest
   // float type:
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      "float|double",                   // test type(s)
      "[^|]*",                          // test data group
      "[^|]*", 10, 5);                 // test function
   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

#define BOOST_CHECK_CLOSE_EX(a, b, prec, i) \
   {\
      unsigned int failures = boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed;\
      BOOST_CHECK_CLOSE(a, b, prec); \
      if(failures != boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed)\
      {\
         std::cerr << "Failure was at row " << i << std::endl;\
         std::cerr << std::setprecision(35); \
         std::cerr << "{ " << data[i][0] << " , " << data[i][1] << " , " << data[i][2];\
         std::cerr << " , " << data[i][3] << " , " << data[i][4] << " , " << data[i][5] << " } " << std::endl;\
      }\
   }

template <class T>
void do_test_gamma_2(const T& data, const char* type_name, const char* test_name)
{
   //
   // test gamma_p_inva(T, T) against data:
   //
   using namespace std;
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   std::cout << test_name << " with type " << type_name << std::endl;

   //
   // These sanity checks test for a round trip accuracy of one half
   // of the bits in T, unless T is type float, in which case we check
   // for just one decimal digit.  The problem here is the sensitivity
   // of the functions, not their accuracy.  This test data was generated
   // for the forward functions, which means that when it is used as
   // the input to the inverses then it is necessarily inexact.  This rounding
   // of the input is what makes the data unsuitable for use as an accuracy check,
   // and also demonstrates that you can't in general round-trip these functions.
   // It is however a useful sanity check.
   //
   value_type precision = static_cast<value_type>(ldexp(1.0, 1-boost::math::policies::digits<value_type, boost::math::policies::policy<> >()/2)) * 100;
   if(boost::math::policies::digits<value_type, boost::math::policies::policy<> >() < 50)
      precision = 1;   // 1% or two decimal digits, all we can hope for when the input is truncated to float

   for(unsigned i = 0; i < data.size(); ++i)
   {
      //
      // These inverse tests are thrown off if the output of the
      // incomplete gamma is too close to 1: basically there is insuffient
      // information left in the value we're using as input to the inverse
      // to be able to get back to the original value.
      //
      if(data[i][5] == 0)
         BOOST_CHECK_EQUAL(boost::math::gamma_p_inva(data[i][1], data[i][5]), boost::math::tools::max_value<value_type>());
      else if((1 - data[i][5] > 0.001) && (fabs(data[i][5]) > 2 * boost::math::tools::min_value<value_type>()))
      {
         value_type inv = boost::math::gamma_p_inva(data[i][1], data[i][5]);
         BOOST_CHECK_CLOSE_EX(data[i][0], inv, precision, i);
      }
      else if(1 == data[i][5])
         BOOST_CHECK_EQUAL(boost::math::gamma_p_inva(data[i][1], data[i][5]), boost::math::tools::min_value<value_type>());
      else if(data[i][5] > 2 * boost::math::tools::min_value<value_type>())
      {
         // not enough bits in our input to get back to x, but we should be in
         // the same ball park:
         value_type inv = boost::math::gamma_p_inva(data[i][1], data[i][5]);
         BOOST_CHECK_CLOSE_EX(data[i][0], inv, 100, i);
      }

      if(data[i][3] == 0)
         BOOST_CHECK_EQUAL(boost::math::gamma_q_inva(data[i][1], data[i][3]), boost::math::tools::min_value<value_type>());
      else if((1 - data[i][3] > 0.001)
         && (fabs(data[i][3]) > 2 * boost::math::tools::min_value<value_type>())
         && (fabs(data[i][3]) > 2 * boost::math::tools::min_value<double>()))
      {
         value_type inv = boost::math::gamma_q_inva(data[i][1], data[i][3]);
         BOOST_CHECK_CLOSE_EX(data[i][0], inv, precision, i);
      }
      else if(1 == data[i][3])
         BOOST_CHECK_EQUAL(boost::math::gamma_q_inva(data[i][1], data[i][3]), boost::math::tools::max_value<value_type>());
      else if(data[i][3] > 2 * boost::math::tools::min_value<value_type>())
      {
         // not enough bits in our input to get back to x, but we should be in
         // the same ball park:
         value_type inv = boost::math::gamma_q_inva(data[i][1], data[i][3]);
         BOOST_CHECK_CLOSE_EX(data[i][0], inv, 100, i);
      }
   }
   std::cout << std::endl;
}

template <class T>
void do_test_gamma_inva(const T& data, const char* type_name, const char* test_name)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   typedef value_type (*pg)(value_type, value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::gamma_p_inva<value_type, value_type>;
#else
   pg funcp = boost::math::gamma_p_inva;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test gamma_p_inva(T, T) against data:
   //
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0, 1),
      extract_result(2));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::gamma_p_inva", test_name);
   //
   // test gamma_q_inva(T, T) against data:
   //
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = boost::math::gamma_q_inva<value_type, value_type>;
#else
   funcp = boost::math::gamma_q_inva;
#endif
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0, 1),
      extract_result(3));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::gamma_q_inva", test_name);
}

template <class T>
void test_gamma(T, const char* name)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // First the data for the incomplete gamma function, each
   // row has the following 6 entries:
   // Parameter a, parameter z,
   // Expected tgamma(a, z), Expected gamma_q(a, z)
   // Expected tgamma_lower(a, z), Expected gamma_p(a, z)
   //
#  include "igamma_med_data.ipp"

   do_test_gamma_2(igamma_med_data, name, "Running round trip sanity checks on incomplete gamma medium sized values");

#  include "igamma_small_data.ipp"

   do_test_gamma_2(igamma_small_data, name, "Running round trip sanity checks on incomplete gamma small values");

#  include "igamma_big_data.ipp"

   do_test_gamma_2(igamma_big_data, name, "Running round trip sanity checks on incomplete gamma large values");

#  include "igamma_inva_data.ipp"

   do_test_gamma_inva(igamma_inva_data, name, "Incomplete gamma inverses.");
}

int test_main(int, char* [])
{
   expected_results();
   BOOST_MATH_CONTROL_FP;

#ifndef BOOST_MATH_BUGGY_LARGE_FLOAT_CONSTANTS
#ifdef TEST_FLOAT
   test_gamma(0.1F, "float");
#endif
#endif
#ifdef TEST_DOUBLE
   test_gamma(0.1, "double");
#endif
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#ifdef TEST_LDOUBLE
   test_gamma(0.1L, "long double");
#endif
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
#ifdef TEST_REAL_CONCEPT
   test_gamma(boost::math::concepts::real_concept(0.1), "real_concept");
#endif
#endif
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif
   return 0;
}


#include <benchmark/benchmark.h>

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

#include "src/utils/distributions.h"
#include "utils.h"

Eigen::VectorXd lpdf_cov(const Eigen::MatrixXd &x, const Eigen::VectorXd &mean,
                         const Eigen::MatrixXd &cov) {
  Eigen::VectorXd out(x.rows());
  for (int i = 0; i < x.rows(); i++) {
    out(i) = stan::math::multi_normal_lpdf(x.row(i), mean, cov);
  }
  return out;
}

Eigen::VectorXd lpdf_prec(const Eigen::MatrixXd &x,
                          const Eigen::VectorXd &mean,
                          const Eigen::MatrixXd &prec) {
  Eigen::VectorXd out(x.rows());
  for (int i = 0; i < x.rows(); i++) {
    out(i) = stan::math::multi_normal_prec_lpdf(x.row(i), mean, prec);
  }
  return out;
}

Eigen::VectorXd lpdf_naive(const Eigen::MatrixXd &x,
                           const Eigen::VectorXd &mean,
                           const Eigen::MatrixXd &prec_chol,
                           double prec_logdet) {
  Eigen::VectorXd out(x.rows());
  for (int i = 0; i < x.rows(); i++) {
    out(i) = bayesmix::multi_normal_prec_lpdf(x.row(i), mean, prec_chol,
                                              prec_logdet);
  }
  return out;
}

Eigen::VectorXd lpdf_fully_optimized(const Eigen::MatrixXd &x,
                                     const Eigen::VectorXd &mean,
                                     const Eigen::MatrixXd &prec_chol,
                                     double prec_logdet) {
  using stan::math::NEG_LOG_SQRT_TWO_PI;
  Eigen::VectorXd exp =
      ((x.rowwise() - mean.transpose()) * prec_chol).rowwise().squaredNorm();
  Eigen::VectorXd base = Eigen::ArrayXd::Ones(x.rows()) * prec_logdet +
                         NEG_LOG_SQRT_TWO_PI * x.cols();
  return (base - exp) * 0.5;
}

static void BM_gauss_lpdf_cov(benchmark::State &state) {
  int dim = state.range(0);
  Eigen::VectorXd mean = Eigen::VectorXd::Zero(dim);
  Eigen::MatrixXd cov = get_spd_matrix(dim);
  Eigen::MatrixXd x = Eigen::MatrixXd::Ones(200, dim);
  for (auto _ : state) {
    lpdf_cov(x, mean, cov);
  }
}

static void BM_gauss_lpdf_prec(benchmark::State &state) {
  int dim = state.range(0);
  Eigen::VectorXd mean = Eigen::VectorXd::Zero(dim);
  Eigen::MatrixXd prec = get_spd_matrix(dim);
  Eigen::MatrixXd x = Eigen::MatrixXd::Ones(200, dim);
  for (auto _ : state) {
    lpdf_prec(x, mean, prec);
  }
}

static void BM_gauss_lpdf_naive(benchmark::State &state) {
  int dim = state.range(0);
  Eigen::VectorXd mean = Eigen::VectorXd::Zero(dim);
  Eigen::MatrixXd prec = get_spd_matrix(dim);
  Eigen::MatrixXd prec_chol = Eigen::LLT<Eigen::MatrixXd>(prec).matrixU();
  Eigen::VectorXd diag = prec_chol.diagonal();
  double prec_logdet = 2 * log(diag.array()).sum();
  Eigen::MatrixXd x = Eigen::MatrixXd::Ones(200, dim);

  for (auto _ : state) {
    lpdf_naive(x, mean, prec_chol, prec_logdet);
  }
}

static void BM_gauss_lpdf_fully_optimized(benchmark::State &state) {
  int dim = state.range(0);
  Eigen::VectorXd mean = Eigen::VectorXd::Zero(dim);
  Eigen::MatrixXd prec = get_spd_matrix(dim);
  Eigen::MatrixXd prec_chol = Eigen::LLT<Eigen::MatrixXd>(prec).matrixU();
  Eigen::VectorXd diag = prec_chol.diagonal();
  double prec_logdet = 2 * log(diag.array()).sum();
  Eigen::MatrixXd x = Eigen::MatrixXd::Ones(200, dim);

  for (auto _ : state) {
    lpdf_fully_optimized(x, mean, prec_chol, prec_logdet);
  }
}

BENCHMARK(BM_gauss_lpdf_cov)->RangeMultiplier(2)->Range(2, 2 << 4);
BENCHMARK(BM_gauss_lpdf_cov)->RangeMultiplier(2)->Range(2, 2 << 4);
BENCHMARK(BM_gauss_lpdf_naive)->RangeMultiplier(2)->Range(2, 2 << 4);
BENCHMARK(BM_gauss_lpdf_fully_optimized)->RangeMultiplier(2)->Range(2, 2 << 4);


//  (C) Copyright Nick Thompson 2018.
//  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_DIFFERENTIATION_FINITE_DIFFERENCE_HPP
#define BOOST_MATH_DIFFERENTIATION_FINITE_DIFFERENCE_HPP

/*
 * Performs numerical differentiation by finite-differences.
 *
 * All numerical differentiation using finite-differences are ill-conditioned, and these routines are no exception.
 * A simple argument demonstrates that the error is unbounded as h->0.
 * Take the one sides finite difference formula f'(x) = (f(x+h)-f(x))/h.
 * The evaluation of f induces an error as well as the error from the finite-difference approximation, giving
 * |f'(x) - (f(x+h) -f(x))/h| < h|f''(x)|/2 + (|f(x)|+|f(x+h)|)eps/h =: g(h), where eps is the unit roundoff for the type.
 * It is reasonable to choose h in a way that minimizes the maximum error bound g(h).
 * The value of h that minimizes g is h = sqrt(2eps(|f(x)| + |f(x+h)|)/|f''(x)|), and for this value of h the error bound is
 * sqrt(2eps(|f(x+h) +f(x)||f''(x)|)).
 * In fact it is not necessary to compute the ratio (|f(x+h)| + |f(x)|)/|f''(x)|; the error bound of ~\sqrt{\epsilon} still holds if we set it to one.
 *
 *
 * For more details on this method of analysis, see
 *
 * http://www.uio.no/studier/emner/matnat/math/MAT-INF1100/h08/kompendiet/diffint.pdf
 * http://web.archive.org/web/20150420195907/http://www.uio.no/studier/emner/matnat/math/MAT-INF1100/h08/kompendiet/diffint.pdf
 *
 *
 * It can be shown on general grounds that when choosing the optimal h, the maximum error in f'(x) is ~(|f(x)|eps)^k/k+1|f^(k-1)(x)|^1/k+1.
 * From this we can see that full precision can be recovered in the limit k->infinity.
 *
 * References:
 *
 * 1) Fornberg, Bengt. "Generation of finite difference formulas on arbitrarily spaced grids." Mathematics of computation 51.184 (1988): 699-706.
 *
 *
 * The second algorithm, the complex step derivative, is not ill-conditioned.
 * However, it requires that your function can be evaluated at complex arguments.
 * The idea is that f(x+ih) = f(x) +ihf'(x) - h^2f''(x) + ... so f'(x) \approx Im[f(x+ih)]/h.
 * No subtractive cancellation occurs. The error is ~ eps|f'(x)| + eps^2|f'''(x)|/6; hard to beat that.
 *
 * References:
 *
 * 1) Squire, William, and George Trapp. "Using complex variables to estimate derivatives of real functions." Siam Review 40.1 (1998): 110-112.
 */

#include <complex>
#include <boost/math/special_functions/next.hpp>

namespace boost{ namespace math{ namespace differentiation {

namespace detail {
    template<class Real>
    Real make_xph_representable(Real x, Real h)
    {
        using std::numeric_limits;
        // Redefine h so that x + h is representable. Not using this trick leads to large error.
        // The compiler flag -ffast-math evaporates these operations . . .
        Real temp = x + h;
        h = temp - x;
        // Handle the case x + h == x:
        if (h == 0)
        {
            h = boost::math::nextafter(x, (numeric_limits<Real>::max)()) - x;
        }
        return h;
    }
}

template<class F, class Real>
Real complex_step_derivative(const F f, Real x)
{
    // Is it really this easy? Yes.
    // Note that some authors recommend taking the stepsize h to be smaller than epsilon(), some recommending use of the min().
    // This idea was tested over a few billion test cases and found the make the error *much* worse.
    // Even 2eps and eps/2 made the error worse, which was surprising.
    using std::complex;
    using std::numeric_limits;
    constexpr const Real step = (numeric_limits<Real>::epsilon)();
    constexpr const Real inv_step = 1/(numeric_limits<Real>::epsilon)();
    return f(complex<Real>(x, step)).imag()*inv_step;
}

namespace detail {

   template <unsigned>
   struct fd_tag {};

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<1>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^1/2
      // Note that this estimate of h differs from the best estimate by a factor of sqrt((|f(x)| + |f(x+h)|)/|f''(x)|).
      // Since this factor is invariant under the scaling f -> kf, then we are somewhat justified in approximating it by 1.
      // This approximation will get better as we move to higher orders of accuracy.
      Real h = 2 * sqrt(eps);
      h = detail::make_xph_representable(x, h);

      Real yh = f(x + h);
      Real y0 = f(x);
      Real diff = yh - y0;
      if (error)
      {
         Real ym = f(x - h);
         Real ypph = abs(yh - 2 * y0 + ym) / h;
         // h*|f''(x)|*0.5 + (|f(x+h)+|f(x)|)*eps/h
         *error = ypph / 2 + (abs(yh) + abs(y0))*eps / h;
      }
      return diff / h;
   }

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<2>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^2/3
      // See the previous discussion to understand determination of h and the error bound.
      // Series[(f[x+h] - f[x-h])/(2*h), {h, 0, 4}]
      Real h = pow(3 * eps, static_cast<Real>(1) / static_cast<Real>(3));
      h = detail::make_xph_representable(x, h);

      Real yh = f(x + h);
      Real ymh = f(x - h);
      Real diff = yh - ymh;
      if (error)
      {
         Real yth = f(x + 2 * h);
         Real ymth = f(x - 2 * h);
         *error = eps * (abs(yh) + abs(ymh)) / (2 * h) + abs((yth - ymth) / 2 - diff) / (6 * h);
      }

      return diff / (2 * h);
   }

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<4>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^4/5
      Real h = pow(11.25*eps, (Real)1 / (Real)5);
      h = detail::make_xph_representable(x, h);
      Real ymth = f(x - 2 * h);
      Real yth = f(x + 2 * h);
      Real yh = f(x + h);
      Real ymh = f(x - h);
      Real y2 = ymth - yth;
      Real y1 = yh - ymh;
      if (error)
      {
         // Mathematica code to extract the remainder:
         // Series[(f[x-2*h]+ 8*f[x+h] - 8*f[x-h] - f[x+2*h])/(12*h), {h, 0, 7}]
         Real y_three_h = f(x + 3 * h);
         Real y_m_three_h = f(x - 3 * h);
         // Error from fifth derivative:
         *error = abs((y_three_h - y_m_three_h) / 2 + 2 * (ymth - yth) + 5 * (yh - ymh) / 2) / (30 * h);
         // Error from function evaluation:
         *error += eps * (abs(yth) + abs(ymth) + 8 * (abs(ymh) + abs(yh))) / (12 * h);
      }
      return (y2 + 8 * y1) / (12 * h);
   }

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<6>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^6/7
      // Error: h^6f^(7)(x)/140 + 5|f(x)|eps/h
      Real h = pow(eps / 168, (Real)1 / (Real)7);
      h = detail::make_xph_representable(x, h);

      Real yh = f(x + h);
      Real ymh = f(x - h);
      Real y1 = yh - ymh;
      Real y2 = f(x - 2 * h) - f(x + 2 * h);
      Real y3 = f(x + 3 * h) - f(x - 3 * h);

      if (error)
      {
         // Mathematica code to generate fd scheme for 7th derivative:
         // Sum[(-1)^i*Binomial[7, i]*(f[x+(3-i)*h] + f[x+(4-i)*h])/2, {i, 0, 7}]
         // Mathematica to demonstrate that this is a finite difference formula for 7th derivative:
         // Series[(f[x+4*h]-f[x-4*h] + 6*(f[x-3*h] - f[x+3*h]) + 14*(f[x-h] - f[x+h] + f[x+2*h] - f[x-2*h]))/2, {h, 0, 15}]
         Real y7 = (f(x + 4 * h) - f(x - 4 * h) - 6 * y3 - 14 * y1 - 14 * y2) / 2;
         *error = abs(y7) / (140 * h) + 5 * (abs(yh) + abs(ymh))*eps / h;
      }
      return (y3 + 9 * y2 + 45 * y1) / (60 * h);
   }

   template<class F, class Real>
   Real finite_difference_derivative(const F f, Real x, Real* error, const fd_tag<8>&)
   {
      using std::sqrt;
      using std::pow;
      using std::abs;
      using std::numeric_limits;

      const Real eps = (numeric_limits<Real>::epsilon)();
      // Error bound ~eps^8/9.
      // In double precision, we only expect to lose two digits of precision while using this formula, at the cost of 8 function evaluations.
      // Error: h^8|f^(9)(x)|/630 + 7|f(x)|eps/h assuming 7 unstabilized additions.
      // Mathematica code to get the error:
      // Series[(f[x+h]-f[x-h])*(4/5) + (1/5)*(f[x-2*h] - f[x+2*h]) + (4/105)*(f[x+3*h] - f[x-3*h]) + (1/280)*(f[x-4*h] - f[x+4*h]), {h, 0, 9}]
      // If we used Kahan summation, we could get the max error down to h^8|f^(9)(x)|/630 + |f(x)|eps/h.
      Real h = pow(551.25*eps, (Real)1 / (Real)9);
      h = detail::make_xph_representable(x, h);

      Real yh = f(x + h);
      Real ymh = f(x - h);
      Real y1 = yh - ymh;
      Real y2 = f(x - 2 * h) - f(x + 2 * h);
      Real y3 = f(x + 3 * h) - f(x - 3 * h);
      Real y4 = f(x - 4 * h) - f(x + 4 * h);

      Real tmp1 = 3 * y4 / 8 + 4 * y3;
      Real tmp2 = 21 * y2 + 84 * y1;

      if (error)
      {
         // Mathematica code to generate fd scheme for 7th derivative:
         // Sum[(-1)^i*Binomial[9, i]*(f[x+(4-i)*h] + f[x+(5-i)*h])/2, {i, 0, 9}]
         // Mathematica to demonstrate that this is a finite difference formula for 7th derivative:
         // Series[(f[x+5*h]-f[x- 5*h])/2 + 4*(f[x-4*h] - f[x+4*h]) + 27*(f[x+3*h] - f[x-3*h])/2 + 24*(f[x-2*h]  - f[x+2*h]) + 21*(f[x+h] - f[x-h]), {h, 0, 15}]
         Real f9 = (f(x + 5 * h) - f(x - 5 * h)) / 2 + 4 * y4 + 27 * y3 / 2 + 24 * y2 + 21 * y1;
         *error = abs(f9) / (630 * h) + 7 * (abs(yh) + abs(ymh))*eps / h;
      }
      return (tmp1 + tmp2) / (105 * h);
   }

   template<class F, class Real, class tag>
   Real finite_difference_derivative(const F, Real, Real*, const tag&)
   {
      // Always fails, but condition is template-arg-dependent so only evaluated if we get instantiated.
      static_assert(sizeof(Real) == 0, "Finite difference not implemented for this order: try 1, 2, 4, 6 or 8");
   }

}

template<class F, class Real, size_t order=6>
inline Real finite_difference_derivative(const F f, Real x, Real* error = nullptr)
{
   return detail::finite_difference_derivative(f, x, error, detail::fd_tag<order>());
}

}}}  // namespaces
#endif


/* test_poisson.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/discrete_distribution.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/exception/diagnostic_information.hpp>
#include <vector>
#include <iostream>
#include <numeric>

#include "chi_squared_test.hpp"

bool do_test(int n, long long max) {
    std::cout << "running discrete(p0, p1, ..., p" << n-1 << ")" << " " << max << " times: " << std::flush;

    std::vector<double> expected;
    {
        boost::mt19937 egen;
        for(int i = 0; i < n; ++i) {
            expected.push_back(egen());
        }
        double sum = std::accumulate(expected.begin(), expected.end(), 0.0);
        for(std::vector<double>::iterator iter = expected.begin(), end = expected.end(); iter != end; ++iter) {
            *iter /= sum;
        }
    }

    boost::random::discrete_distribution<> dist(expected);
    boost::mt19937 gen;
    std::vector<long long> results(expected.size());
    for(long long i = 0; i < max; ++i) {
        ++results[dist(gen)];
    }

    long long sum = std::accumulate(results.begin(), results.end(), 0ll);
    if(sum != max) {
        std::cout << "*** Failed: incorrect total: " << sum << " ***" << std::endl;
        return false;
    }
    double chsqr = chi_squared_test(results, expected, max);

    bool result = chsqr < 0.99;
    const char* err = result? "" : "*";
    std::cout << std::setprecision(17) << chsqr << err << std::endl;

    std::cout << std::setprecision(6);

    return result;
}

bool do_tests(int repeat, int max_n, long long trials) {
    boost::mt19937 gen;
    boost::uniform_int<> idist(1, max_n);
    int errors = 0;
    for(int i = 0; i < repeat; ++i) {
        if(!do_test(idist(gen), trials)) {
            ++errors;
        }
    }
    if(errors != 0) {
        std::cout << "*** " << errors << " errors detected ***" << std::endl;
    }
    return errors == 0;
}

int usage() {
    std::cerr << "Usage: test_discrete -r <repeat> -n <max n> -t <trials>" << std::endl;
    return 2;
}

template<class T>
bool handle_option(int& argc, char**& argv, char opt, T& value) {
    if(argv[0][1] == opt && argc > 1) {
        --argc;
        ++argv;
        value = boost::lexical_cast<T>(argv[0]);
        return true;
    } else {
        return false;
    }
}

int main(int argc, char** argv) {
    int repeat = 10;
    int max_n = 100000;
    long long trials = 1000000ll;

    if(argc > 0) {
        --argc;
        ++argv;
    }
    while(argc > 0) {
        if(argv[0][0] != '-') return usage();
        else if(!handle_option(argc, argv, 'r', repeat)
             && !handle_option(argc, argv, 'n', max_n)
             && !handle_option(argc, argv, 't', trials)) {
            return usage();
        }
        --argc;
        ++argv;
    }

    try {
        if(do_tests(repeat, max_n, trials)) {
            return 0;
        } else {
            return EXIT_FAILURE;
        }
    } catch(...) {
        std::cerr << boost::current_exception_diagnostic_information() << std::endl;
        return EXIT_FAILURE;
    }
}


#include "drake/math/saturate.h"

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/common/autodiff.h"
#include "drake/common/symbolic.h"

namespace drake {

using symbolic::Environment;
using symbolic::Expression;

namespace math {
namespace {

// Tests that saturate works with double types.
GTEST_TEST(SaturateTest, DoubleTest) {
  const double kLow{5};
  const double kHigh{10};

  const double kTooLow{kLow - 1e-10};
  const double kMiddle{kLow + kHigh / 2.0};
  const double kTooHigh{kHigh + 1e-10};
  EXPECT_EQ(saturate(kTooLow, kLow, kHigh), kLow);
  EXPECT_EQ(saturate(kMiddle, kLow, kHigh), kMiddle);
  EXPECT_EQ(saturate(kTooHigh, kLow, kHigh), kHigh);
}

// Tests that saturate works with AutoDiff types.
GTEST_TEST(SaturateTest, AutoDiffXdTest) {
  const AutoDiffXd lowerbound{5.0, Vector1d(1.5)};
  const AutoDiffXd upperbound{10.0, Vector1d(-1.5)};
  const AutoDiffXd withinBoundsValue{7.5, Vector1d(6.38)};

  EXPECT_EQ(
      saturate(AutoDiffXd{1.0, Vector1d(10.2)}, lowerbound, upperbound),
      lowerbound);
  EXPECT_EQ(
      saturate(withinBoundsValue, lowerbound, upperbound),
      withinBoundsValue);
  EXPECT_EQ(
      saturate(AutoDiffXd{100.3, Vector1d(-9)}, lowerbound, upperbound),
      upperbound);

  // Tests a mixed-type scenario. Adding or subtracting 1 from an AutoDiffXd
  // does not result in an AutoDiffXd, but rather some intermediate type that
  // implicitly converts to AutoDiffXd.
  EXPECT_EQ(
      saturate(AutoDiffXd{1.0, Vector1d(10.2)}, lowerbound - 1, upperbound + 1),
      lowerbound - 1);
}

// Tests that saturate() works with symbolic::Expression types.
GTEST_TEST(SaturateTest, SymbolicExpressionTest) {
  Expression result;
  result = saturate(Expression{1.5}, Expression::One(), Expression::Pi());
  EXPECT_EQ(result.to_string(), "1.5");
  result = saturate(Expression::Zero(), Expression::One(), Expression::Pi());
  EXPECT_EQ(result.to_string(), "1");
  result = saturate(Expression{5.6}, Expression::One(), Expression::Pi());
  const std::string kPi{"3.14"};
  EXPECT_EQ(result.to_string().compare(0, kPi.length(), kPi), 0);

  symbolic::Variable x{"x"};
  symbolic::Variable lo{"lo"};
  symbolic::Variable hi{"hi"};
  auto saturate_expression = saturate(
      Expression{x}, Expression{lo}, Expression{hi});
  EXPECT_EQ(saturate_expression.Evaluate(
      Environment{{x, 0}, {lo, 3}, {hi, 10}}), 3);
  EXPECT_EQ(saturate_expression.Evaluate(
      Environment{{x, 5}, {lo, 3}, {hi, 10}}), 5);
  EXPECT_EQ(saturate_expression.Evaluate(
      Environment{{x, 12.334}, {lo, 3}, {hi, 10}}), 10);
}

}  // namespace
}  // namespace math
}  // namespace drake


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/integral.hpp>

#include <boost/hana/detail/assert.hpp>
using namespace boost::hana;

int main() {
    // Integral < Integral
    BOOST_HANA_CONSTANT_ASSERT(less(int_<0>, int_<1>));
    BOOST_HANA_CONSTANT_ASSERT(not_(less(int_<0>, int_<0>)));
    BOOST_HANA_CONSTANT_ASSERT(not_(less(int_<1>, int_<0>)));

    BOOST_HANA_CONSTANT_ASSERT(less(int_<0>, long_<1>));
    BOOST_HANA_CONSTANT_ASSERT(not_(less(int_<0>, long_<0>)));
    BOOST_HANA_CONSTANT_ASSERT(not_(less(int_<1>, long_<0>)));

    // Integral < other
    BOOST_HANA_CONSTEXPR_ASSERT(less(int_<0>, int{1}));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int_<0>, int{0})));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int_<1>, int{0})));

    BOOST_HANA_CONSTEXPR_ASSERT(less(int_<0>, long{1}));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int_<0>, long{0})));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int_<1>, long{0})));

    BOOST_HANA_CONSTEXPR_ASSERT(less(int_<0>, float{1}));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int_<0>, float{0})));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int_<1>, float{0})));

    // other < Integral
    BOOST_HANA_CONSTEXPR_ASSERT(less(int{0}, int_<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int{0}, int_<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int{1}, int_<0>)));

    BOOST_HANA_CONSTEXPR_ASSERT(less(int{0}, long_<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int{0}, long_<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(int{1}, long_<0>)));

    BOOST_HANA_CONSTEXPR_ASSERT(less(float{0}, long_<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(float{0}, long_<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(not_(less(float{1}, long_<0>)));
}


//  Copyright (c) 2015 John Maddock
//  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_ELLINT_JZ_HPP
#define BOOST_MATH_ELLINT_JZ_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_rj.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/workaround.hpp>

// Elliptic integral the Jacobi Zeta function.

namespace boost { namespace math {

namespace detail{

// Elliptic integral - Jacobi Zeta
template <typename T, typename Policy>
T jacobi_zeta_imp(T phi, T k, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math::tools;
    using namespace boost::math::constants;

    bool invert = false;
    if(phi < 0)
    {
       phi = fabs(phi);
       invert = true;
    }

    T result;
    T sinp = sin(phi);
    T cosp = cos(phi);
    T s2 = sinp * sinp;
    T k2 = k * k;
    T kp = 1 - k2;
    if(k == 1)
       result = sinp * (boost::math::sign)(cosp);  // We get here by simplifying JacobiZeta[w, 1] in Mathematica, and the fact that 0 <= phi.
    else
       result = k2 * sinp * cosp * sqrt(1 - k2 * s2) * ellint_rj_imp(T(0), kp, T(1), T(1 - k2 * s2), pol) / (3 * ellint_k_imp(k, pol));
    return invert ? T(-result) : result;
}

} // detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type jacobi_zeta(T1 k, T2 phi, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::jacobi_zeta_imp(static_cast<value_type>(phi), static_cast<value_type>(k), pol), "boost::math::jacobi_zeta<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type jacobi_zeta(T1 k, T2 phi)
{
   return boost::math::jacobi_zeta(k, phi, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_D_HPP


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/trunc.hpp>
#include <boost/simd/function/fast.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/three.hpp>

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

  STF_TYPE_IS(r_t, T);

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

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

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

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

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

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

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

STF_CASE_TPL ( "fast trunc real",  STF_IEEE_TYPES)
{

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

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

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


#include <iostream>
#include <Eigen/Dense>
#include "../include/layer.h"

int main()
{
    using std::cout;
    using std::endl;
    using namespace MyDL;
    using namespace Eigen;

    int row = 4;
    int col = 5;
    double dropout_ratio = 0.5;
    Dropout dropout(row, col, dropout_ratio);

    MatrixXd X = MatrixXd::Random(row, col);
    MatrixXd dout = MatrixXd::Ones(row, col);
    vector<MatrixXd> inputs, outputs, douts, grads;
    inputs.push_back(X);
    douts.push_back(dout);

    cout << "input: " << endl;
    cout << X << endl;

    cout << "Dropout forward -train mode- :" << endl;
    outputs = dropout.forward(inputs);

    cout << outputs[0] << endl;

    Config::getInstance().set_flag(false);

    cout << "Dropout forward -inference mode- : " << endl;
    outputs = dropout.forward(inputs);

    cout << outputs[0] << endl;

    cout << "Dropout backward: " << endl;
    grads = dropout.backward(douts);

    cout << grads[0] << endl;

    return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Example illustrating the use of GCoptimization.cpp
//
/////////////////////////////////////////////////////////////////////////////
//
//  Optimization problem:
//  is a set of sites (pixels) of width 10 and hight 5. Thus number of pixels is 50
//  grid neighborhood: each pixel has its left, right, up, and bottom pixels as neighbors
//  7 labels
//  Data costs: D(pixel,label) = 0 if pixel < 25 and label = 0
//            : D(pixel,label) = 10 if pixel < 25 and label is not  0
//            : D(pixel,label) = 0 if pixel >= 25 and label = 5
//            : D(pixel,label) = 10 if pixel >= 25 and label is not  5
// Smoothness costs: V(p1,p2,l1,l2) = min( (l1-l2)*(l1-l2) , 4 )
// Below in the main program, we illustrate different ways of setting data and smoothness costs
// that our interface allow and solve this optimizaiton problem

// For most of the examples, we use no spatially varying pixel dependent terms.
// For some examples, to demonstrate spatially varying terms we use
// V(p1,p2,l1,l2) = w_{p1,p2}*[min((l1-l2)*(l1-l2),4)], with
// w_{p1,p2} = p1+p2 if |p1-p2| == 1 and w_{p1,p2} = p1*p2 if |p1-p2| is not 1

#include <ctype.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <string>
#include <time.h>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/ximgproc.hpp>
#include <boost/log/core.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/expressions.hpp>
#include <boost/filesystem.hpp>
#include <limits>

//#include <opencv/contrib/contrib.hpp>
#include <iostream>
#include <fstream>

using namespace cv;
using namespace std;
using namespace ximgproc;
//using namespace GCoptimization;

#include "GCoptimization.h"
#include "IcgBench.h"
#include "CImg.h"

using namespace cimg_library;

struct ForDataFn {
	int numLab;
	int *data;
};

struct Result {
	int *labels;
	long long e;
};

int delta = 5;
int desvio = 1.2;

int smoothFn(int p1, int p2, int l1, int l2) {

	//return exp( pow(p1 - p2, 2)/(2*d*d) );

	if (l1 == l2) {
		return 0;
	} else {
		//return delta * abs(p1 - p2);//pow(p1 - p2, 2);
		return delta * exp(-pow(p1 - p2, 2) / (2 * desvio * desvio));
	}

	//return a * abs(p1 - p2);
	//if ((l1 - l2) * (l1 - l2) <= 4)
	// return ((l1 - l2) * (l1 - l2));
	// else
	// return (4);
}

int dataFn(int p, int l, void *data) {
	ForDataFn *myData = (ForDataFn *) data;
	int numLab = myData->numLab;

	return (myData->data[p * numLab + l]);
}

int loadImage(std::string path, Mat& mask, Mat &labelSuperpixels) {

	//Load Image with opencv
	Mat image;

	image = imread(path, CV_LOAD_IMAGE_COLOR);   // Read the file

	if (!image.data)                              // Check for invalid input
	{
		printf("Could not open or find the image\n");
		return 0;
	}

	//cvtColor(image, gray, CV_RGB2GRAY);

	int num_iterations = 4;
	int prior = 2;
	bool double_step = false;
	int num_superpixels = 100;
	int num_levels = 4;
	int num_histogram_bins = 5;

	Mat result;
	Ptr<SuperpixelSEEDS> seeds;
	int width, height;

	width = image.size().width;
	height = image.size().height;

	seeds = createSuperpixelSEEDS(width, height, image.channels(),
			num_superpixels, num_levels, prior, num_histogram_bins,
			double_step);

	Mat converted;
	cvtColor(image, converted, COLOR_BGR2HSV);

	double t = (double) getTickCount();

	Ptr<SuperpixelSLIC> slic = createSuperpixelSLIC(converted,SLIC,50,float(100));

	//seeds->iterate(converted, num_iterations);
	slic->iterate(num_iterations);
	result = image;

	t = ((double) getTickCount() - t) / getTickFrequency();
	//printf("SEEDS segmentation took %i ms with %3i superpixels\n",
	//		(int) (t * 1000), seeds->getNumberOfSuperpixels());

	/* retrieve the segmentation result */
	//Mat labels;
	//seeds->getLabels(labels);
	//slic->getLabels(labels);
	/* get the contours for displaying */
	slic->getLabelContourMask(mask, false);
	result.setTo(Scalar(50, 50, 255), mask);

	//seeds->getLabels()

	slic->getLabels(labelSuperpixels);

	//cout << labelSuperpixels.size() << endl;

	//abort();
	//imshow("Superpixel", result);

	//namedWindow("Display window", WINDOW_AUTOSIZE); // Create a window for display.
	//imshow("Display window", superPixel);           // Show our image inside it.
	//imshow("Gray image", gray);

	//waitKey(0);

	// Wait for a keystroke in the window

	return slic->getNumberOfSuperpixels();

}

////////////////////////////////////////////////////////////////////////////////
// in this version, set data and smoothness terms using arrays
// grid neighborhood structure is assumed
//
Result * GridGraph_DMAT(const Mat& I, int num_labels, int f_labels[],
		std::vector<IcgBench::Seed> seeds, int countSeed[], CImg<float> *dataTermCuda,
		vector<vector<IcgBench::Seed>> &super) {

	int width = I.cols;
	int height = I.rows;
	int num_pixels = width * height;
	long long e = 0;

	int *result = new int[num_pixels];   // stores result of optimization

	vector<Mat> labelsSeed(num_labels);
	int countInstSeed[num_labels];

	for (int i = 0; i < num_labels; ++i) {
		Mat l(3, countSeed[i], CV_8SC1);
		labelsSeed[i] = l;
		countInstSeed[i] = 0;
	}

	for (unsigned int i = 0; i < seeds.size(); ++i) {
		IcgBench::Seed seed = seeds[i];
		unsigned int p = I.at<unsigned char>(seed.y, seed.x);
		//printf("%u", p);
		(labelsSeed[seed.label]).at<unsigned char>(0, countInstSeed[seed.label]) =
				p;
		(labelsSeed[seed.label]).at<unsigned char>(1, countInstSeed[seed.label]) =
				seed.y;
		(labelsSeed[seed.label]).at<unsigned char>(2, countInstSeed[seed.label]) =
				seed.x;

		countInstSeed[seed.label]++;
	}

	Mat r = I.reshape(1, num_pixels);
	//cout << r.depth() << ", " << r.channels() << endl;
	//printf("%d", r.);
	// first set up the array for data costs
	int sizeData = num_pixels * num_labels;
	double *data = new double[sizeData];
	int *dataInt = new int[sizeData];
	double min = std::numeric_limits<double>::max();
	double max = std::numeric_limits<double>::min();
	for (int i = 0; i < num_pixels; i++) {
		for (int l = 0; l < num_labels; l++) {
			//printf("Teste %d\n", r.at<unsigned char>(i));
			//data[i * num_labels + l] = pow(
			//		f_labels[l] - r.at<unsigned char>(0, i), 2);
			dataInt[i * num_labels + l] = abs(
					f_labels[l] - r.at<unsigned char>(0, i));

			/*int x = i/I.size().width;
			 int y = i%I.size().width;
			 Mat t1(1,countSeed[l],CV_32F);
			 Mat t2(1,countSeed[l],CV_32F);
			 Mat rootSquare;
			 pow(x - labelsSeed[l].col(2), 2, t1);
			 pow(y - labelsSeed[l].col(1), 2, t2);
			 sqrt(t1 + t2, rootSquare);*/

			//cout << labelsSeed[l].col(0) << endl;
			//abort();
			Mat c = labelsSeed[l].col(0);
			int diff = 0;
			for (int j = 0; j < c.size().height; j++) {
				diff += abs(r.at<unsigned char>(0, i) - c.at<unsigned char>(j, 0));
				//printf("%d", diff);
			}
			double p = diff / (1.0 * countSeed[l]);

			data[i * num_labels + l] = p;

			if (p < min) {
				min = p;
				//printf("%lf\n", min);
			}
			if (p > max) {
				max = p;
				//printf("%lf\n", min);
			}

			/*	double p = sum((r.at<unsigned char>(0, i) - labelsSeed[l].col(0)))[0]/countSeed[l];
			 //printf("%lf\n", -log());
			 data[i * num_labels + l] = p;
			 if ( p < min){
			 min = p;
			 //printf("%lf\n", min);
			 }
			 if ( p > max){
			 max = p;
			 //printf("%lf\n", min);
			 }*/

		}
	}

	//min = abs(min);
	double sumData = 0;
	for (int i = 0; i < sizeData; ++i) {
		data[i] = (data[i] - min)/(max - min);
		//printf("%lf\n", data[i]);
		//sumData += data[i];
	}

	//int *dataInt = new int[sizeData];

	for (int i = 0; i < sizeData; ++i) {
		//printf("%lf\n", -log(data[i]));
		data[i] = abs(-1* -log(data[i]));
		//dataInt[i] = data[i];
		//printf("%d\n", dataInt[i]);
	}

	for (int i = 0; i < I.size().height; i++) {
			for (int j = 0; j < I.size().width; j++) {
				for (int l = 0; l < num_labels; l++) {

					dataInt[(i * I.size().width + j) * num_labels + l] = (*dataTermCuda)(j,i, 0, l);
				}
			}
		}

	/*for (int i = 0; i < num_pixels; i++) {
		for (int l = 0; l < num_labels; l++) {

			dataInt[i * num_labels + l] = (*dataTermCuda)(i/num_pixels, i%num_pixels, 0, l);//(*dataTermCuda)();
		}
	}*/

	try {
		GCoptimizationGridGraph *gc = new GCoptimizationGridGraph(width, height,
				num_labels);

// set up the needed data to pass to function for the data costs
		ForDataFn toFn;
		toFn.data = dataInt;
		toFn.numLab = num_labels;

		gc->setDataCost(&dataFn, &toFn);

// smoothness comes from function pointer
		gc->setSmoothCost(&smoothFn);

//gc->setLabelCost()
		//gc->setLabelOrder(true);

		//gc->setLabelOrder(true);
		//gc->dynamic_programming();

		int *ordem = new int(num_labels);
		for (int i = 0; i < num_labels; ++i) {
			ordem[i] = num_labels - i - 1;
		}
		gc->setLabelOrder(ordem, num_labels);

//printf("\nBefore optimization energy is %lld", gc->compute_energy());
//for(int i =0; i < 100; i+=10){
		//gc->expansion(1); // run expansion for 2 iterations. For swap use gc->swap(num_iterations);
		srand(time(NULL));
//gc->swap(10);

		gc->dynamic_programming(super, super.size(), dataTermCuda);
		//gc->expansion(1);

		//printf("\n%lld", gc->compute_energy());

		for (int i = 0; i < num_labels; ++i) {
			ordem[i] = i;
		}
		gc->setLabelOrder(ordem, num_labels);

		//gc->expansion(1);

		//gc->setLabel(0,0);

		//printf("\n%lld", gc->compute_energy());

//printf(" Dynamic Programming ");
//gc->dynamic_programming();
//printf("\n%lld", gc->compute_energy());
//}
//gc->expansion(100); // run expansion for 2 iterations. For swap use gc->swap(num_iterations);
//printf("\nAfter optimization energy is %lld", gc->compute_energy());
		e = gc->compute_energy();
//printf("\n%lld", e);

		for (int i = 0; i < num_pixels; i++)
			result[i] = gc->whatLabel(i);

		delete gc;

	} catch (GCException e) {
		e.Report();
	}

	//delete[] result;
	//delete[] smooth;
	delete[] data;

	Result *rEnd = new Result;

	rEnd->labels = result;
	rEnd->e = e;

	return rEnd;

}

void createImageLabelsShow(int *labels, int k, Mat &img) {

	unsigned char pv[k];

	for (int i = 0, j = 0; i < 256; i += (256 / k)) {
		pv[j] = i;
		j++;
	}

	for (int i = 0; i < img.rows; i++) {
		for (int j = 0; j < img.cols; j++) {
			img.at<unsigned char>(i, j) = pv[labels[i * img.cols + j]];
		}
	}

}

void createImageLabels(int *labels, int k, Mat &img) {

	unsigned char pv[k];

	for (int i = 0, j = 0; i < 256; i += (256 / k)) {
		pv[j] = i;
		j++;
	}

	for (int i = 0; i < img.rows; i++) {
		for (int j = 0; j < img.cols; j++) {
			img.at<unsigned char>(i, j) = labels[i * img.cols + j];
		}
	}

}

void writeCSV(string filename, cv::Mat m) {
//cv::Formatter const * c_formatter(cv::Formatter::get(cv::Formatter::FMT_CSV));
	ofstream myfile;
	myfile.open(filename.c_str(), ios::out);
	myfile << cv::format(m, cv::Formatter::FMT_CSV);//c_formatter->format(m);
	myfile.close();
}

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

	boost::log::core::get()->set_logging_enabled(false);

	int iflag = 0, oflag = 0, gflag = 0, dataflag = 0;
	char *cvalue = NULL;
	char *gvalue = NULL;
	char *outValue = NULL;
	char *dataValue = NULL;

	int index;
	int c;

	opterr = 0;

	while ((c = getopt(argc, argv, "o:g:i:d:")) != -1) {
		switch (c) {
		case 'i':
			cvalue = optarg;
			iflag = 1;
			break;
		case 'g':
			gvalue = optarg;
			gflag = 1;
			break;
		case 'o':
			outValue = optarg;
			oflag = 1;
			break;
		case 'd':
			dataValue = optarg;
			dataflag = 1;
			break;
		case '?':
			if (optopt == 'i')
				fprintf(stderr, "Option -i requires an argument.\n");
			else if (optopt == 'o')
				fprintf(stderr, "Option -o requires an argument.\n");
			else if (optopt == 'g')
				fprintf(stderr, "Option -g requires an argument.\n");
			else if (isprint(optopt))
				fprintf(stderr, "Unknown option `-%c'.\n", optopt);
			else
				fprintf(stderr, "Unknown option character `\\x%x'.\n", optopt);
			return 1;
		default:
			abort();
		}
	}

	//printf("aflag = %d, bflag = %d, cvalue = %s\n", aflag, bflag, cvalue);

	for (index = optind; index < argc; index++) {
		printf("Non-option argument %s\n", argv[index]);
	}

	if (!iflag) {
		printf("-i require");
		abort();
	} else if (!oflag) {
		printf("-o require");
		abort();
	}
	if (!gflag) {
		printf("-g require");
		abort();
	}
	if (!dataflag) {
			printf("-d require");
			abort();
		}

	//BOOST_LOG_TRIVIAL(trace)<< "A trace severity message";
	//BOOST_LOG_TRIVIAL(debug)<< "A debug severity message";
	//BOOST_LOG_TRIVIAL(info)<< "An informational severity message";
	//BOOST_LOG_TRIVIAL(warning)<< "A warning severity message";
	//BOOST_LOG_TRIVIAL(error)<< "An error severity message";
	//BOOST_LOG_TRIVIAL(fatal)<< "A fatal severity message";

	//Load Data Term
	boost::filesystem::path dataTermPath(dataValue);
	boost::filesystem::path dataTermName("dataEnergy.cimg");
	boost::filesystem::path fullDataTermPath = dataTermPath / dataTermName;

	CImg<float> dataTerm(fullDataTermPath.string().c_str());

	//printf("\nData Term %f\n", dataTerm(0,0,0,0));

	string fileNameGroundTruth(gvalue);
	BOOST_LOG_TRIVIAL(info)<< "Load groundtruth " << fileNameGroundTruth;
	IcgBench::IcgBenchFileIO groudTruth(fileNameGroundTruth);

	boost::filesystem::path fileImagePath(cvalue);
	boost::filesystem::path fileImage(groudTruth.getFileName());
	boost::filesystem::path fullImagePath = fileImagePath / fileImage;
	BOOST_LOG_TRIVIAL(info)<< "Load Image " << fullImagePath;
	Mat grayImage, labelSuperPixels;
	int numSuperPixels = loadImage(fullImagePath.string(), grayImage, labelSuperPixels);
	if (numSuperPixels) {

		int k = groudTruth.getNumLabels();
		Result *r = NULL;
		int s_labels[k];
		int c_labels[k];
		for (int i = 0; i < k; ++i) {
			c_labels[i] = 0;
			s_labels[i] = 0;
		}

		vector<vector<IcgBench::Seed>> super(numSuperPixels);
		for (int i = 0; i < super.size(); ++i) {
			vector<IcgBench::Seed> s;
			super[i] = s;
		}

		for(int i = 0; i < labelSuperPixels.size().height; i++){
			for(int j = 0; j < labelSuperPixels.size().width; j++){
				IcgBench::Seed s;
				s.x = i;
				s.y = j;
				s.label = i * grayImage.cols + j;//labelSuperPixels.at<unsigned int>(i,j);
				super[labelSuperPixels.at<unsigned int>(i,j)].push_back(s);
			}
		}

		BOOST_LOG_TRIVIAL(info)<< "Define seeds using ground truth ";
		std::vector<IcgBench::Seed> seeds = groudTruth.getSeeds();
		for (unsigned int i = 0; i < seeds.size(); ++i) {
			IcgBench::Seed seed = seeds[i];
			//if ( seed.x <= grayImage.cols &&  seed.y <= grayImage.rows)
			unsigned int p = grayImage.at<unsigned char>(seed.y, seed.x);
			s_labels[seed.label] += p;
			c_labels[seed.label]++;
		}

		for (int i = 0; i < k; ++i) {
			s_labels[i] /= c_labels[i];
		}

		BOOST_LOG_TRIVIAL(info)<< "Execute segmentation";
		double t = (double) getTickCount();

		r = GridGraph_DMAT(grayImage, k, s_labels, seeds, c_labels, &dataTerm, super);

		t = ((double) getTickCount() - t) / getTickFrequency();

		BOOST_LOG_TRIVIAL(info)<< "Show result";

		Mat seg(grayImage.rows, grayImage.cols, CV_8SC1);
		createImageLabelsShow(r->labels, k - 1, seg);

		//imshow("Seg", seg);
		//waitKey(0);

		BOOST_LOG_TRIVIAL(info)<< "Save Label and Seed path";

		Mat labelsMat(grayImage.rows, grayImage.cols, CV_8SC1);
		Mat seedsMat = Mat::ones(grayImage.rows, grayImage.cols, CV_8SC1) * -1;

		boost::filesystem::path fileNameGroundTruthPath(fileNameGroundTruth);
		boost::filesystem::path fileOutPath(outValue);
		boost::filesystem::path fullOutPath = fileOutPath
				/ fileNameGroundTruthPath.stem();
		boost::filesystem::create_directory(fullOutPath);
		boost::filesystem::path extensionLabel(".label");
		boost::filesystem::path extensionSeed(".seed");

		boost::filesystem::path fullOutLabelPath("gd.label");
		//fullOutLabelPath.replace_extension(extensionLabel);
		fullOutLabelPath = fullOutPath / fullOutLabelPath;
		boost::filesystem::path fullOutSeedPath("gd.seed");// = fullOutPath;
		fullOutSeedPath = fullOutPath / fullOutSeedPath;
		//fullOutSeedPath.replace_extension(extensionSeed);

		createImageLabels(r->labels, k - 1, labelsMat);

//Label unsigned
		int sizeLabels = grayImage.rows * grayImage.cols;
		unsigned int labels[grayImage.rows * grayImage.cols];
		for (int i = 0; i < sizeLabels; ++i) {
			labels[i] = r->labels[i];
		}

		IcgBench::LabelImage labelImageIcg(labels, grayImage.cols,
				grayImage.rows);

		IcgBench::LabelImage* a = groudTruth.getLabels();
		IcgBench::LabelImage* b = &labelImageIcg;

//printf("%d\t%d\t%d\t%d\n", a->height(), b->height(), a->width(), b->width());

		double score = IcgBench::computeDiceScore(*a, *b);

		printf("%lld\t%.6lf\t%d\t%d\t%3i\n", r->e, score, numSuperPixels, k, (int) (t * 1000));

		writeCSV(fullOutLabelPath.string(), labelsMat);
		writeCSV(fullOutSeedPath.string(), seedsMat);

		BOOST_LOG_TRIVIAL(info)<< "End";

		return 0;

	} else {
		fprintf(stderr, "Not load image\n");
		abort();
	}

	//Load Image
	/*Mat gray;
	 int *labels = NULL;
	 Result *r;
	 int k  = 3;
	 if (loadImage(cvalue, gray)) {
	 //imshow("Gray image", gray);

	 long long eBefore = 0;
	 int l = 20;

	 for (k = 4; k < 5; k++) {

	 l = k;
	 int f_labels[l];

	 for (int i = 0, j = 0; i < 256; i += (256 / l)) {
	 f_labels[j] = i;
	 j++;
	 }

	 r = GridGraph_DMAT(gray, l, f_labels);

	 if ( k == 2 ){
	 eBefore = r->e;
	 labels = r->labels;

	 }

	 if (r->e >= eBefore ){
	 labels = r->labels;
	 break;
	 }

	 cout <<"E=" << r->e << " k = " << k << endl;

	 eBefore = r->e;

	 delete[] r->labels;
	 delete r;

	 }

	 //
	 waitKey(0);

	 }

	 Mat seg(gray.rows, gray.cols, CV_8SC1);

	 createImageLabelsShow(labels, k-1, seg);

	 Mat labelsMat(gray.rows, gray.cols, CV_8SC1);
	 Mat seedsMat = Mat::ones(gray.rows, gray.cols, CV_8SC1)*-1;

	 createImageLabels(labels, k-1, labelsMat);

	 cout << labelsMat << endl;

	 writeCSV("teste.label", labelsMat);
	 writeCSV("teste.seed", seedsMat);

	 /*for (int i = 0; i < gray.rows; i++) {
	 for (int j = 0; j < gray.cols; j++) {
	 printf("%d ", labels[i * gray.cols + j]);

	 }
	 printf("\n");
	 }*/

	//Mat seg(gray.rows, gray.cols, CV_32SC1, labels);
	//Mat cm_img0;
	// Apply the colormap:
	//applyColorMap(seg, cm_img0, COLORMAP_HOT);
	// Show the result:
	//imshow("Seg", seg);
	//waitKey(0);
	//return 0;
	/*int width = 10;
	 int height = 5;
	 int num_pixels = width*height;
	 int num_labels = 7;

	 // smoothness and data costs are set up one by one, individually
	 GridGraph_Individually(width,height,num_pixels,num_labels);

	 // smoothness and data costs are set up using arrays
	 GridGraph_DArraySArray(width,height,num_pixels,num_labels);

	 // smoothness and data costs are set up using functions
	 GridGraph_DfnSfn(width,height,num_pixels,num_labels);

	 // smoothness and data costs are set up using arrays.
	 // spatially varying terms are present
	 GridGraph_DArraySArraySpatVarying(width,height,num_pixels,num_labels);

	 //Will pretend our graph is
	 //general, and set up a neighborhood system
	 // which actually is a grid
	 GeneralGraph_DArraySArray(width,height,num_pixels,num_labels);

	 //Will pretend our graph is general, and set up a neighborhood system
	 // which actually is a grid. Also uses spatially varying terms
	 GeneralGraph_DArraySArraySpatVarying(width,height,num_pixels,num_labels);

	 printf("\n  Finished %d (%d) clock per sec %d",clock()/CLOCKS_PER_SEC,clock(),CLOCKS_PER_SEC);
	 */

}