/**
 * @file parallel_sgd_test.cpp
 * @author Shikhar Bhardwaj
 *
 * Test file for Parallel SGD.
 *
 * 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/parallel_sgd/decay_policies/constant_step.hpp>
#include <mlpack/core/optimizers/parallel_sgd/decay_policies/exponential_backoff.hpp>
#include <mlpack/core/optimizers/parallel_sgd/sparse_test_function.hpp>
#include <mlpack/core/optimizers/lbfgs/test_functions.hpp>
// We need some thorough testing
#define private public
#include <mlpack/core/optimizers/parallel_sgd/parallel_sgd.hpp>
#undef private

#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(ParallelSGDTest);

/**
 * Test the correctness of the Parallel SGD implementation using a specified
 * sparse test function, with guaranteed disjoint updates between different
 * threads.
 */
BOOST_AUTO_TEST_CASE(SimpleParallelSGDTest)
{
  SparseTestFunction f;

  ConstantStep decayPolicy(0.4);

  // The batch size for this test should be chosen according to the threads
  // available on the system. If the update does not touch each datapoint, the
  // test will fail.

  size_t threadsAvailable = omp_get_max_threads();

  for (size_t i = threadsAvailable; i > 0; --i)
  {
    omp_set_num_threads(i);

    size_t batchSize = std::ceil((float) f.NumFunctions() / i);

    ParallelSGD<ConstantStep> s(10000, batchSize, 1e-5, true, decayPolicy);

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

    // The final value of the objective function should be close to the optimal
    // value, that is the sum of values at the vertices of the parabolas.
    BOOST_REQUIRE_CLOSE(result, 123.75, 0.01);

    // The co-ordinates should be the vertices of the parabolas.
    BOOST_REQUIRE_CLOSE(coordinates[0], 2, 0.02);
    BOOST_REQUIRE_CLOSE(coordinates[1], 1, 0.02);
    BOOST_REQUIRE_CLOSE(coordinates[2], 1.5, 0.02);
    BOOST_REQUIRE_CLOSE(coordinates[3], 4, 0.02);
  }
}

/**
 * When run with a single thread, parallel SGD should be identical to normal
 * SGD.
 */
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);

    ConstantStep decayPolicy(0.001);

    ParallelSGD<ConstantStep> s(0, f.NumFunctions(), 1e-12, true, decayPolicy);

    arma::mat coordinates = f.GetInitialPoint();

    omp_set_num_threads(1);
    double result = s.Optimize(f, coordinates);

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

/**
 * Test the correctness of the Exponential backoff stepsize decay policy.
 */
BOOST_AUTO_TEST_CASE(ExponentialBackoffDecayTest)
{
  ExponentialBackoff decayPolicy(100, 100, 0.9);

  // At the first iteration, stepsize should be unchanged
  BOOST_REQUIRE_EQUAL(decayPolicy.StepSize(1), 100);
  // At the 99th iteration, stepsize should be unchanged
  BOOST_REQUIRE_EQUAL(decayPolicy.StepSize(99), 100);
  // At the 100th iteration, stepsize should be changed
  BOOST_REQUIRE_EQUAL(decayPolicy.StepSize(100), 90);
  // At the 210th iteration, stepsize should be unchanged
  BOOST_REQUIRE_EQUAL(decayPolicy.StepSize(210), 90);
  // At the 211th iteration, stepsize should be changed
  BOOST_REQUIRE_EQUAL(decayPolicy.StepSize(211), 81);
}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/graph/undirected_graph.hpp>
#include <iostream>
using namespace boost;
using namespace std;
int main( )
{
    undirected_graph<>  g;
    //A vertex descriptor corresponds to a unique vertex in an abstract graph instance.
    undirected_graph<>::vertex_descriptor u = g.add_vertex();
    undirected_graph<>::vertex_descriptor v = g.add_vertex();
    undirected_graph<>::vertex_descriptor w = g.add_vertex();
    undirected_graph<>::vertex_descriptor x = g.add_vertex();

    //Adds edge (u,v) to the graph and returns the edge descriptor for the new edge.
    add_edge(u, v, g);
    add_edge(u, w, g);
    add_edge(v, x, g);

    cout << "Degree of u: " << degree(u, g)<<"\n";

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/trigonometric/include/functions/rem_2pi.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/twopi.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/ten.hpp>
#include <nt2/include/constants/eps.hpp>

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

NT2_TEST_CASE_TPL ( rem_2pi_real__1_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::rem_2pi;
  using nt2::tag::rem_2pi_;

  {
    T res = rem_2pi(nt2::Zero<T>());
    NT2_TEST_ULP_EQUAL( res, nt2::Zero<T>(), 1.5);
    res = rem_2pi(nt2::Pi<T>()-nt2::Ten<T>()*nt2::Eps<T>());
    NT2_TEST_ULP_EQUAL( res, nt2::Pi<T>()-nt2::Ten<T>()*nt2::Eps<T>(), 1.5);
    res = rem_2pi(nt2::Pi<T>()+nt2::Ten<T>()*nt2::Eps<T>());
    NT2_TEST_ULP_EQUAL( res, nt2::Ten<T>()*nt2::Eps<T>()-nt2::Pi<T>(), 1.5);
    res = rem_2pi(nt2::Twopi<T>());
    NT2_TEST_ULP_EQUAL( res, nt2::Zero<T>(), 1.5);
    res = rem_2pi(nt2::Pio_2<T>());
    NT2_TEST_ULP_EQUAL( res, nt2::Pio_2<T>(), 1.5);
  }
} // end of test for floating_

NT2_TEST_CASE_TPL ( rem_2pi_targeted,  NT2_SIMD_REAL_TYPES)
{

  using nt2::rem_2pi;
  using nt2::tag::rem_2pi_;

  T x = nt2::Twopi<T>(), xr;
  xr = rem_2pi(x, nt2::meta::as_<nt2::big_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<T>(), 1.5);

  xr = rem_2pi(x, nt2::meta::as_<nt2::medium_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<T>(), 1.5);

  xr = rem_2pi(x, nt2::meta::as_<nt2::small_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<T>(), 1.5);

  xr = rem_2pi(x, nt2::meta::as_<nt2::very_small_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<T>(), 1.5);

}


//
// 	Copyright (c) 2018-2020, Cem Bassoy, cem.bassoy@gmail.com
// 	Copyright (c) 2019-2020, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include "utility.hpp"
#include <boost/test/unit_test.hpp>

#include <functional>

BOOST_AUTO_TEST_SUITE(test_tensor_static_expression)

using test_types = zip<int,float,std::complex<float>>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

struct fixture
{
  template<std::size_t N1,size_t... N>
  using extents_type = boost::numeric::ublas::extents<N1,N...>;

  std::tuple<
    extents_type<1,1>, 		// 1
    extents_type<2,3>, 		// 2
    extents_type<4,1,3>, 	// 3
    extents_type<4,2,3>, 	// 4
    extents_type<4,2,3,5>  	// 5
    > extents;
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_static_expression_retrieve_extents, value,  test_types, fixture)
{
  namespace ublas  = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    auto uplus1 = [](auto const& a){return a + value_type(1);};
    auto uplus2 = [](auto const& a){return value_type(2) + a;};
    auto bplus  = std::plus <value_type>{};
    auto bminus = std::minus<value_type>{};

    for_each_in_tuple(extents, [&](auto const& /*unused*/, auto& e){
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type,extents_type,layout_type>;

        auto t = tensor_type();
        auto v = value_type{};
        for(auto& tt: t){ tt = v; v+=value_type{1}; }

        BOOST_CHECK( ublas::detail::retrieve_extents( t ) == e );

        // uexpr1 = t+1
        // uexpr2 = 2+t
        auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type>( t, uplus1 );
        auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type>( t, uplus2 );

        BOOST_CHECK( ublas::detail::retrieve_extents( uexpr1 ) == e );
        BOOST_CHECK( ublas::detail::retrieve_extents( uexpr2 ) == e );

        // bexpr_uexpr = (t+1) + (2+t)
        auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( uexpr1, uexpr2, bplus );

        BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_uexpr ) == e );

        // bexpr_bexpr_uexpr = ((t+1) + (2+t)) - t
        auto bexpr_bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( bexpr_uexpr, t, bminus );

        BOOST_CHECK( ublas::detail::retrieve_extents( bexpr_bexpr_uexpr ) == e );

    });

    for_each_in_tuple(extents, [&](auto I, auto& e1){

        if ( I >= std::tuple_size_v<decltype(extents)> - 1){
            return;
        }

        using extents_type1 = std::decay_t<decltype(e1)>;
        using tensor_type1 = ublas::tensor_static<value_type, extents_type1, layout_type>;

        for_each_in_tuple(extents, [&](auto J, auto& e2){

            if( J != I + 1 ){
                return;
            }

            using extents_type2 = std::decay_t<decltype(e2)>;
            using tensor_type2 = ublas::tensor_static<value_type, extents_type2, layout_type>;

            auto v = value_type{};

            tensor_type1 t1;
            for(auto& tt: t1){ tt = v; v+=value_type{1}; }

            tensor_type2 t2;
            for(auto& tt: t2){ tt = v; v+=value_type{2}; }

            BOOST_CHECK( ublas::detail::retrieve_extents( t1 ) != ublas::detail::retrieve_extents( t2 ) );

            // uexpr1 = t1+1
            // uexpr2 = 2+t2
            auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type1>( t1, uplus1 );
            auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type2>( t2, uplus2 );

            BOOST_CHECK( ublas::detail::retrieve_extents( t1 )     == ublas::detail::retrieve_extents( uexpr1 ) );
            BOOST_CHECK( ublas::detail::retrieve_extents( t2 )     == ublas::detail::retrieve_extents( uexpr2 ) );
            BOOST_CHECK( ublas::detail::retrieve_extents( uexpr1 ) != ublas::detail::retrieve_extents( uexpr2 ) );

        });
    });
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_static_expression_all_extents_equal, value,  test_types, fixture)
{
  namespace ublas  = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;

    auto uplus1 = [](auto const& a){return a + value_type(1);};
    auto uplus2 = [](auto const& a){return value_type(2) + a;};
    auto bplus  = std::plus <value_type>{};
    auto bminus = std::minus<value_type>{};

    for_each_in_tuple(extents, [&](auto const& /*unused*/, auto& e){
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type,extents_type,layout_type>;

        auto t = tensor_type{};
        auto v = value_type{};
        for(auto& tt: t){ tt = v; v+=value_type{1}; }

        BOOST_CHECK( ublas::detail::all_extents_equal( t , e ) );

        // uexpr1 = t+1
        // uexpr2 = 2+t
        auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type>( t, uplus1 );
        auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type>( t, uplus2 );

        BOOST_CHECK( ublas::detail::all_extents_equal( uexpr1, e ) );
        BOOST_CHECK( ublas::detail::all_extents_equal( uexpr2, e ) );

        // bexpr_uexpr = (t+1) + (2+t)
        auto bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( uexpr1, uexpr2, bplus );

        BOOST_CHECK( ublas::detail::all_extents_equal( bexpr_uexpr, e ) );

        // bexpr_bexpr_uexpr = ((t+1) + (2+t)) - t
        auto bexpr_bexpr_uexpr = ublas::detail::make_binary_tensor_expression<tensor_type>( bexpr_uexpr, t, bminus );

        BOOST_CHECK( ublas::detail::all_extents_equal( bexpr_bexpr_uexpr , e ) );

    });

    for_each_in_tuple(extents, [&](auto I, auto& e1){

        if ( I >= std::tuple_size_v<decltype(extents)> - 1){
            return;
        }

        using extents_type1 = std::decay_t<decltype(e1)>;
        using tensor_type1 = ublas::tensor_static<value_type, extents_type1, layout_type>;

        for_each_in_tuple(extents, [&](auto J, auto& e2){

            if( J != I + 1 ){
                return;
            }

            using extents_type2 = std::decay_t<decltype(e2)>;
            using tensor_type2 = ublas::tensor_static<value_type, extents_type2, layout_type>;

            auto v = value_type{};

            tensor_type1 t1;
            for(auto& tt: t1){ tt = v; v+=value_type{1}; }

            tensor_type2 t2;
            for(auto& tt: t2){ tt = v; v+=value_type{2}; }

            BOOST_CHECK( ublas::detail::all_extents_equal( t1, ublas::detail::retrieve_extents(t1) ) );
            BOOST_CHECK( ublas::detail::all_extents_equal( t2, ublas::detail::retrieve_extents(t2) ) );

            // uexpr1 = t1+1
            // uexpr2 = 2+t2
            auto uexpr1 = ublas::detail::make_unary_tensor_expression<tensor_type1>( t1, uplus1 );
            auto uexpr2 = ublas::detail::make_unary_tensor_expression<tensor_type2>( t2, uplus2 );

            BOOST_CHECK( ublas::detail::all_extents_equal( uexpr1, ublas::detail::retrieve_extents(uexpr1) ) );
            BOOST_CHECK( ublas::detail::all_extents_equal( uexpr2, ublas::detail::retrieve_extents(uexpr2) ) );

        });
    });

}

BOOST_AUTO_TEST_SUITE_END()


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, round) {
  using stan::math::round;
  EXPECT_FLOAT_EQ(-27, round(-27.3239));
  EXPECT_FLOAT_EQ(-1, round(-0.5));
  EXPECT_FLOAT_EQ(0, round(0));
  EXPECT_FLOAT_EQ(0, round(0.0));
  EXPECT_FLOAT_EQ(1, round(0.5));
  EXPECT_FLOAT_EQ(27, round(27.3239));
}

TEST(MathFunctions, roundNaN) {
  double nan = std::numeric_limits<double>::quiet_NaN();
  EXPECT_PRED1(boost::math::isnan<double>, stan::math::round(nan));
}


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

#ifndef NETKET_CONVLAYER_HH
#define NETKET_CONVLAYER_HH

#include <time.h>
#include <Eigen/Dense>
#include <algorithm>
#include <complex>
#include <fstream>
#include <random>
#include <vector>

#include "Graph/graph.hpp"
#include "Lookup/lookup.hpp"
#include "Utils/all_utils.hpp"
#include "abstract_layer.hpp"

namespace netket {
/** Convolutional layer with spin 1/2 hidden units.
 Important: In order for this to work correctly, VectorType and MatrixType must
 be column major.
 */
template <typename Activation, typename T>
class Convolutional : public AbstractLayer<T> {
  using VectorType = typename AbstractLayer<T>::VectorType;

  using MatrixType = typename AbstractLayer<T>::MatrixType;
  static_assert(!MatrixType::IsRowMajor, "MatrixType must be column-major");

  Activation activation_;  // activation function class

  bool usebias_;  // boolean to turn or off bias

  int nv_;            // number of visible units in the full network
  int in_channels_;   // number of input channels
  int in_size_;       // input size: should be multiple of no. of sites
  int out_channels_;  // number of output channels
  int out_size_;      // output size: should be multiple of no. of sites
  int npar_;          // number of parameters in layer

  int dist_;         // Distance to include in one convolutional image
  int kernel_size_;  // Size of convolutional kernel (depends on dist_)
  std::vector<std::vector<int>>
      neighbours_;  // list of neighbours for each site
  std::vector<std::vector<int>>
      flipped_neighbours_;  // list of reverse neighbours for each site
  MatrixType kernels_;      // Weight parameters, W(in_size x out_size)
  VectorType bias_;         // Bias parameters, b(out_size x 1)

  // Note that input of this layer is also the output of
  // previous layer

  MatrixType lowered_image_;
  MatrixType lowered_image2_;
  MatrixType lowered_der_;
  MatrixType flipped_kernels_;

 public:
  using StateType = typename AbstractLayer<T>::StateType;
  using LookupType = typename AbstractLayer<T>::LookupType;

  /// Constructor
  Convolutional(const Graph &graph, const int input_channel,
                const int output_channel, const int dist = 1,
                const bool use_bias = true)
      : activation_(),
        usebias_(use_bias),
        nv_(graph.Nsites()),
        in_channels_(input_channel),
        out_channels_(output_channel),
        dist_(dist) {
    in_size_ = in_channels_ * nv_;
    out_size_ = out_channels_ * nv_;

    Init(graph);
  }

  explicit Convolutional(const Graph &graph, const json &pars)
      : activation_(), nv_(graph.Nsites()) {
    in_channels_ = FieldVal(pars, "InputChannels");
    in_size_ = in_channels_ * nv_;

    out_channels_ = FieldVal(pars, "OutputChannels");
    out_size_ = out_channels_ * nv_;

    dist_ = FieldVal(pars, "Distance");

    usebias_ = FieldOrDefaultVal(pars, "UseBias", true);

    Init(graph);
  }

  void Init(const Graph &graph) {
    // Construct neighbourhood of all nodes with distance of at most dist_ from
    // each node i kernel(k) will act on neighbours_[i][k]
    for (int i = 0; i < nv_; ++i) {
      std::vector<int> neigh;
      graph.BreadthFirstSearch(i, dist_, [&neigh](int node, int /*depth*/) {
        neigh.push_back(node);
      });
      neighbours_.push_back(neigh);
    }

    // Check that all sites have same number of neighbours
    int check;
    kernel_size_ = neighbours_[0].size();
    for (int i = 1; i < nv_; i++) {
      check = neighbours_[i].size();
      if (check != kernel_size_) {
        throw InvalidInputError(
            "number of neighbours of each site is not the same for chosen "
            "lattice");
      }
    }
    // Construct flipped_neighbours_
    // flipped_neighbours_[i][k] should be acted on by kernel(k)
    // Let neighbours_[i][k] = l
    // i.e. look for the direction k' where neighbours_[l][k'] = i
    // then neighbours_[i][k'] will be acted on by kernel(k)
    // so flipped_neighbours_[i][k] = neighbours_[i][k']
    for (int i = 0; i < nv_; ++i) {
      std::vector<int> flippedneigh;
      for (int k = 0; k < kernel_size_; ++k) {
        int l = neighbours_[i][k];
        for (int kp = 0; kp < kernel_size_; ++kp) {
          if (neighbours_[l][kp] == i) {
            flippedneigh.push_back(neighbours_[i][kp]);
          }
        }
      }
      flipped_neighbours_.push_back(flippedneigh);
    }

    for (int i = 1; i < nv_; i++) {
      check = flipped_neighbours_[i].size();
      if (check != kernel_size_) {
        throw InvalidInputError(
            "number of neighbours of each site is not the same for chosen "
            "lattice");
      }
    }

    kernels_.resize(in_channels_ * kernel_size_, out_channels_);
    bias_.resize(out_channels_);

    lowered_image_.resize(in_channels_ * kernel_size_, nv_);
    lowered_image2_.resize(nv_, in_channels_ * kernel_size_);
    lowered_der_.resize(kernel_size_ * out_channels_, nv_);
    flipped_kernels_.resize(kernel_size_ * out_channels_, in_channels_);

    npar_ = in_channels_ * kernel_size_ * out_channels_;

    if (usebias_) {
      npar_ += out_channels_;
    } else {
      bias_.setZero();
    }

    std::string buffer = "";

    InfoMessage(buffer) << "Convolutional Layer: " << in_size_ << " --> "
                        << out_size_ << std::endl;
    InfoMessage(buffer) << "# # InputChannels = " << in_channels_ << std::endl;
    InfoMessage(buffer) << "# # OutputChannels = " << out_channels_
                        << std::endl;
    InfoMessage(buffer) << "# # Filter Distance = " << dist_ << std::endl;
    InfoMessage(buffer) << "# # Filter Size = " << kernel_size_ << std::endl;
    InfoMessage(buffer) << "# # UseBias = " << usebias_ << std::endl;
  }

  void InitRandomPars(int seed, double sigma) override {
    VectorType par(npar_);

    netket::RandomGaussian(par, seed, sigma);

    SetParameters(par, 0);
  }

  int Npar() const override { return npar_; }

  int Ninput() const override { return in_size_; }

  int Noutput() const override { return out_size_; }

  void GetParameters(VectorType &pars, int start_idx) const override {
    int k = start_idx;

    if (usebias_) {
      for (int i = 0; i < out_channels_; ++i) {
        pars(k) = bias_(i);
        ++k;
      }
    }

    for (int j = 0; j < out_channels_; ++j) {
      for (int i = 0; i < in_channels_ * kernel_size_; ++i) {
        pars(k) = kernels_(i, j);
        ++k;
      }
    }
  }

  void SetParameters(const VectorType &pars, int start_idx) override {
    int k = start_idx;

    if (usebias_) {
      for (int i = 0; i < out_channels_; ++i) {
        bias_(i) = pars(k);
        ++k;
      }
    }

    for (int j = 0; j < out_channels_; ++j) {
      for (int i = 0; i < in_channels_ * kernel_size_; ++i) {
        kernels_(i, j) = pars(k);
        ++k;
      }
    }
  }

  void InitLookup(const VectorType &v, LookupType &lt,
                  VectorType &output) override {
    lt.resize(1);
    lt[0].resize(out_size_);

    Forward(v, lt, output);
  }

  void UpdateLookup(const VectorType &input,
                    const std::vector<int> &input_changes,
                    const VectorType &new_input, LookupType &theta,
                    const VectorType & /*output*/,
                    std::vector<int> &output_changes,
                    VectorType &new_output) override {
    // At the moment the light cone structure of the convolution is not
    // exploited. To do so we would to change the part
    // else if (num_of_changes >0) {...}
    const int num_of_changes = input_changes.size();
    if (num_of_changes == in_size_) {
      output_changes.resize(out_size_);
      new_output.resize(out_size_);
      Forward(new_input, theta, new_output);
    } else if (num_of_changes > 0) {
      UpdateTheta(input, input_changes, new_input, theta);
      output_changes.resize(out_size_);
      new_output.resize(out_size_);
      Forward(theta, new_output);
    } else {
      output_changes.resize(0);
      new_output.resize(0);
    }
  }

  void UpdateLookup(const Eigen::VectorXd &input,
                    const std::vector<int> &tochange,
                    const std::vector<double> &newconf, LookupType &theta,
                    const VectorType & /*output*/,
                    std::vector<int> &output_changes,
                    VectorType &new_output) override {
    const int num_of_changes = tochange.size();
    if (num_of_changes > 0) {
      UpdateTheta(input, tochange, newconf, theta);
      output_changes.resize(out_size_);
      new_output.resize(out_size_);
      Forward(theta, new_output);
    } else {
      output_changes.resize(0);
      new_output.resize(0);
    }
  }

  // Feedforward
  void Forward(const VectorType &prev_layer_output, LookupType &theta,
               VectorType &output) override {
    LinearTransformation(prev_layer_output, theta);
    NonLinearTransformation(theta, output);
  }

  // Feedforward Using lookup
  void Forward(const LookupType &theta, VectorType &output) override {
    // Apply activation function
    NonLinearTransformation(theta, output);
  }

  // performs the convolution of the kernel onto the image and writes into z
  inline void Convolve(const VectorType &image, VectorType &z) {
    // im2col method
    for (int i = 0; i < nv_; ++i) {
      int j = 0;
      for (auto n : neighbours_[i]) {
        for (int in = 0; in < in_channels_; ++in) {
          lowered_image_(in * kernel_size_ + j, i) = image(in * nv_ + n);
        }
        j++;
      }
    }
    Eigen::Map<MatrixType> output_image(z.data(), nv_, out_channels_);
    output_image.noalias() = lowered_image_.transpose() * kernels_;
  }

  // Performs the linear transformation for the layer.
  inline void LinearTransformation(const VectorType &input, LookupType &theta) {
    Convolve(input, theta[0]);

    if (usebias_) {
      int k = 0;
      for (int out = 0; out < out_channels_; ++out) {
        for (int i = 0; i < nv_; ++i) {
          theta[0](k) += bias_(out);
          ++k;
        }
      }
    }
  }

  // Performs the nonlinear transformation for the layer.
  inline void NonLinearTransformation(const LookupType &theta,
                                      VectorType &output) {
    activation_(theta[0], output);
  }

  inline void UpdateTheta(const VectorType &v,
                          const std::vector<int> &input_changes,
                          const VectorType &new_input, LookupType &theta) {
    const int num_of_changes = input_changes.size();
    for (int s = 0; s < num_of_changes; ++s) {
      const int sf = input_changes[s];
      int kout = 0;
      for (int out = 0; out < out_channels_; ++out) {
        for (int k = 0; k < kernel_size_; ++k) {
          theta[0](flipped_neighbours_[sf][k] + kout) +=
              kernels_(k, out) * (new_input(s) - v(sf));
        }
        kout += nv_;
      }
    }
  }

  inline void UpdateTheta(const VectorType &prev_input,
                          const std::vector<int> &tochange,
                          const std::vector<double> &newconf,
                          LookupType &theta) {
    const int num_of_changes = tochange.size();
    for (int s = 0; s < num_of_changes; ++s) {
      const int sf = tochange[s];
      int kout = 0;
      for (int out = 0; out < out_channels_; ++out) {
        for (int k = 0; k < kernel_size_; ++k) {
          theta[0](flipped_neighbours_[sf][k] + kout) +=
              kernels_(k, out) * (newconf[s] - prev_input(sf));
        }
        kout += nv_;
      }
    }
  }

  void Backprop(const VectorType &prev_layer_output,
                const VectorType &this_layer_output,
                const LookupType &this_layer_theta, const VectorType &dout,
                VectorType &din, VectorType &der, int start_idx) override {
    // Compute dL/dz
    VectorType dLz(out_size_);
    activation_.ApplyJacobian(this_layer_theta[0], this_layer_output, dout,
                              dLz);

    int kd = start_idx;

    // Derivative for bias, d(L) / d(b) = d(L) / d(z)
    if (usebias_) {
      int k = 0;
      for (int out = 0; out < out_channels_; ++out) {
        der(kd) = 0;
        for (int i = 0; i < nv_; ++i) {
          der(kd) += dLz(k);
          ++k;
        }
        ++kd;
      }
    }

    // Derivative for weights, d(L) / d(W) = [d(L) / d(z)] * in'
    // Reshape dLdZ
    Eigen::Map<MatrixType> dLz_reshaped(dLz.data(), nv_, out_channels_);

    // Reshape image
    for (int in = 0; in < in_channels_; ++in) {
      for (int k = 0; k < kernel_size_; ++k) {
        for (int i = 0; i < nv_; ++i) {
          lowered_image2_(i, k + in * kernel_size_) =
              prev_layer_output(in * nv_ + neighbours_[i][k]);
        }
      }
    }
    Eigen::Map<MatrixType> der_w(der.data() + kd, in_channels_ * kernel_size_,
                                 out_channels_);
    der_w.noalias() = lowered_image2_.transpose() * dLz_reshaped;

    // Compute d(L) / d_in = W * [d(L) / d(z)]
    int kout = 0;
    for (int out = 0; out < out_channels_; ++out) {
      for (int in = 0; in < in_channels_; ++in) {
        for (int k = 0; k < kernel_size_; ++k) {
          flipped_kernels_(k + kout, in) = kernels_(k + in * kernel_size_, out);
        }
      }
      kout += kernel_size_;
    }

    for (int i = 0; i < nv_; i++) {
      int j = 0;
      for (auto n : flipped_neighbours_[i]) {
        for (int out = 0; out < out_channels_; ++out) {
          lowered_der_(out * kernel_size_ + j, i) = dLz(out * nv_ + n);
        }
        j++;
      }
    }

    din.resize(in_size_);
    Eigen::Map<MatrixType> der_in(din.data(), nv_, in_channels_);
    der_in.noalias() = lowered_der_.transpose() * flipped_kernels_;
  }

  void to_json(json &pars) const override {
    json layerpar;
    layerpar["Name"] = "Convolutional";
    layerpar["UseBias"] = usebias_;
    layerpar["Inputs"] = in_size_;
    layerpar["Outputs"] = out_size_;
    layerpar["InputChannels"] = in_channels_;
    layerpar["OutputChannels"] = out_channels_;
    layerpar["Bias"] = bias_;
    layerpar["Kernels"] = kernels_;

    pars["Machine"]["Layers"].push_back(layerpar);
  }

  void from_json(const json &pars) override {
    if (FieldExists(pars, "Kernels")) {
      kernels_ = pars["Kernels"];
    } else {
      kernels_.setZero();
    }
    if (FieldExists(pars, "Bias")) {
      bias_ = pars["Bias"];
    } else {
      bias_.setZero();
    }
  }
};
}  // namespace netket

#endif


#include <mass.h>

#include <algorithm> // needed for std::min and std::max
#include <cmath>     // needed for std::fabs

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

template<typename T>
class BoxMesh
: public mass::FaceCallbackInterface<T>
{
public:

  T m_x1[4];  T m_y1[4];  T m_z1[4];
  T m_x2[4];  T m_y2[4];  T m_z2[4];
  T m_x3[4];  T m_y3[4];  T m_z3[4];
  T m_x4[4];  T m_y4[4];  T m_z4[4];
  T m_x5[4];  T m_y5[4];  T m_z5[4];
  T m_x6[4];  T m_y6[4];  T m_z6[4];

  T * m_x[6];  T * m_y[6];  T * m_z[6];

public:

  size_t get_number_of_vertices(size_t const & ) const
  {
    return 4u;
  }

  void get_x_coords(size_t const & face_no, double * coords) const
  {
    coords[0] = m_x[face_no][0];
    coords[1] = m_x[face_no][1];
    coords[2] = m_x[face_no][2];
    coords[3] = m_x[face_no][3];
  }

  void get_y_coords(size_t const & face_no, double * coords) const
  {
    coords[0] = m_y[face_no][0];
    coords[1] = m_y[face_no][1];
    coords[2] = m_y[face_no][2];
    coords[3] = m_y[face_no][3];
  }

  void get_z_coords(size_t const & face_no, double * coords) const
  {
    coords[0] = m_z[face_no][0];
    coords[1] = m_z[face_no][1];
    coords[2] = m_z[face_no][2];
    coords[3] = m_z[face_no][3];
  }

public:

  BoxMesh()
  {
    this->set_box( 2.0, 2.0, 2.0, 2.0, 2.0  );
  }

  BoxMesh(
          T const & top_width
          , T const & top_depth
          , T const & bottom_width
          , T const & bottom_depth
          , T const & height
          )
  {
    this->set_box( top_width, top_depth, bottom_width, bottom_depth, height  );
  }

protected:

  void set_box(
               T const & top_width
               , T const & top_depth
               , T const & bottom_width
               , T const & bottom_depth
               , T const & height)
  {
    T const H     = height       * 0.5;
    T const top_W = top_width    * 0.5;
    T const top_D = top_depth    * 0.5;
    T const bot_W = bottom_width * 0.5;
    T const bot_D = bottom_depth * 0.5;

    // front face
    m_x1[0] = -bot_W; m_y1[0] = -H; m_z1[0] =  bot_D;
    m_x1[1] =  bot_W; m_y1[1] = -H; m_z1[1] =  bot_D;
    m_x1[2] =  top_W; m_y1[2] =  H; m_z1[2] =  top_D;
    m_x1[3] = -top_W; m_y1[3] =  H; m_z1[3] =  top_D;

    // back face
    m_x2[0] = -top_W; m_y2[0] =  H; m_z2[0] = -top_D;
    m_x2[1] =  top_W; m_y2[1] =  H; m_z2[1] = -top_D;
    m_x2[2] =  bot_W; m_y2[2] = -H; m_z2[2] = -bot_D;
    m_x2[3] = -bot_W; m_y2[3] = -H; m_z2[3] = -bot_D;

    // top face
    m_x3[0] = -top_W; m_y3[0] =  H; m_z3[0] =  top_D;
    m_x3[1] =  top_W; m_y3[1] =  H; m_z3[1] =  top_D;
    m_x3[2] =  top_W; m_y3[2] =  H; m_z3[2] = -top_D;
    m_x3[3] = -top_W; m_y3[3] =  H; m_z3[3] = -top_D;

    // bottom face
    m_x4[0] = -bot_W; m_y4[0] = -H; m_z4[0] = -bot_D;
    m_x4[1] =  bot_W; m_y4[1] = -H; m_z4[1] = -bot_D;
    m_x4[2] =  bot_W; m_y4[2] = -H; m_z4[2] =  bot_D;
    m_x4[3] = -bot_W; m_y4[3] = -H; m_z4[3] =  bot_D;

    // left face
    m_x5[0] = -bot_W; m_y5[0] = -H; m_z5[0] = -bot_D;
    m_x5[1] = -bot_W; m_y5[1] = -H; m_z5[1] =  bot_D;
    m_x5[2] = -top_W; m_y5[2] =  H; m_z5[2] =  top_D;
    m_x5[3] = -top_W; m_y5[3] =  H; m_z5[3] = -top_D;

    // right face
    m_x6[0] =  bot_W; m_y6[0] = -H; m_z6[0] =  bot_D;
    m_x6[1] =  bot_W; m_y6[1] = -H; m_z6[1] = -bot_D;
    m_x6[2] =  top_W; m_y6[2] =  H; m_z6[2] = -top_D;
    m_x6[3] =  top_W; m_y6[3] =  H; m_z6[3] =  top_D;

    m_x[0] = &m_x1[0]; m_x[1] = &m_x2[0]; m_x[2] = &m_x3[0]; m_x[3] = &m_x4[0]; m_x[4] = &m_x5[0]; m_x[5] = &m_x6[0];
    m_y[0] = &m_y1[0]; m_y[1] = &m_y2[0]; m_y[2] = &m_y3[0]; m_y[3] = &m_y4[0]; m_y[4] = &m_y5[0]; m_y[5] = &m_y6[0];
    m_z[0] = &m_z1[0]; m_z[1] = &m_z2[0]; m_z[2] = &m_z3[0]; m_z[3] = &m_z4[0]; m_z[4] = &m_z5[0]; m_z[5] = &m_z6[0];
  }

};

BOOST_AUTO_TEST_SUITE(mass);

BOOST_AUTO_TEST_CASE(box_mesh)
{
  BoxMesh<double> const callback;

  double const rho = 1.0;

  mass::Properties<double> Ibox = compute_box( rho, 1.0, 1.0, 1.0 );  // Compute ground truth value using analytical solution

  // Compute numerical solution
  mass::Properties<double> Imesh = compute_mesh( rho, 6, &callback);

  // Compare numerical solution to ground truth solution -- they should be the same
  BOOST_CHECK( Imesh.m_m   > 0.0 );
  BOOST_CHECK( Imesh.m_Ixx > 0.0 );
  BOOST_CHECK( Imesh.m_Iyy > 0.0 );
  BOOST_CHECK( Imesh.m_Izz > 0.0 );
  BOOST_CHECK_CLOSE( Imesh.m_Ixx, Imesh.m_Iyy, 0.01 );
  BOOST_CHECK_CLOSE( Imesh.m_Iyy, Imesh.m_Izz, 0.01 );
  BOOST_CHECK_CLOSE(   Imesh.m_x, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(   Imesh.m_y, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(   Imesh.m_z, 0.0, 0.01 );
  BOOST_CHECK_CLOSE(   Imesh.m_m, Ibox.m_m, 0.01 );
  BOOST_CHECK_CLOSE( Imesh.m_Ixx, Ibox.m_Ixx, 0.01 );
  BOOST_CHECK_CLOSE( Imesh.m_Iyy, Ibox.m_Iyy, 0.01 );
  BOOST_CHECK_CLOSE( Imesh.m_Izz, Ibox.m_Izz, 0.01 );
  BOOST_CHECK_CLOSE( Imesh.m_Ixy, 0.0, 0.01 );
  BOOST_CHECK_CLOSE( Imesh.m_Ixz, 0.0, 0.01 );
  BOOST_CHECK_CLOSE( Imesh.m_Iyz, 0.0, 0.01 );

  BOOST_CHECK( Imesh.is_body_space() );
  BOOST_CHECK( !Imesh.is_model_space() );
}

BOOST_AUTO_TEST_CASE(cuboid_mesh)
{
  using std::min;
  using std::max;
  using std::fabs;

  double const rho = 1.0;

  double tw = 1.0;
  double td = 1.0;
  double bw = 1.0;
  double bd = 1.0;
  double h  = 1.0;

  double old_y = 0.0;

  for(size_t i=0;i<10u;++i)
  {
    BoxMesh<double> const callback = BoxMesh<double>(tw,td,bw,bd,h);

    tw *= 0.1;
    td *= 0.1;

    double iw = min(tw,bw)*0.5;
    double id = min(td,bd)*0.5;
    mass::Properties<double> I = compute_box( rho, iw, h, id );

    double ow = max(tw,bw)*0.5;
    double od = max(td,bd)*0.5;
    mass::Properties<double> O = compute_box( rho, ow, h, od );

    // Compute numerical solution
    mass::Properties<double> M = compute_mesh( rho, 6, &callback);

    BOOST_CHECK( M.m_m   >= I.m_m );
    BOOST_CHECK( M.m_m   <= O.m_m );

    BOOST_CHECK( M.m_Ixx   >= I.m_Ixx );
    BOOST_CHECK( M.m_Ixx   <= O.m_Ixx );

    BOOST_CHECK( M.m_Iyy   >= I.m_Iyy );
    BOOST_CHECK( M.m_Iyy   <= O.m_Iyy );

    BOOST_CHECK( M.m_Izz   >= I.m_Izz );
    BOOST_CHECK( M.m_Izz   <= O.m_Izz );

    BOOST_CHECK( fabs( M.m_Ixy ) < 10e-10 );
    BOOST_CHECK( fabs( M.m_Ixz ) < 10e-10 );
    BOOST_CHECK( fabs( M.m_Iyz ) < 10e-10 );

    BOOST_CHECK( fabs( M.m_x ) < 10e-10 );
    BOOST_CHECK( fabs( M.m_z ) < 10e-10 );

    BOOST_CHECK( M.m_y   <= old_y );

    old_y = M.m_y;
  }
}

BOOST_AUTO_TEST_SUITE_END();


//
// Created by huangkun on 2020/9/18.
//
#include <Eigen/Eigen>
#include <opencv2/core/eigen.hpp>
#include <algorithm>
#include <limits>

#include <cv_calib.hpp>
#include <dbscan.h>
#include <opengv2/utility/utility.hpp>
#include <opengv2/event_camera_calib/CirclesEventFrame.hpp>
#include <opengv2/event_camera_calib/CalibCircle.hpp>
#include <opengv2/sensor/PinholeCamera.hpp>

opengv2::CirclesEventFrame::CirclesEventFrame(EventContainer::Ptr container, const std::pair<double, double> &duration,
                                              CirclePatternParameters::Ptr pattern, Params params) :
        EventFrame(container, duration), pattern_(pattern), params_(params) {
    auto camera = dynamic_cast<CameraBase *>(sensor_.get());
    if (pattern_->isAsymmetric) {
        circleRadiusThreshold_ = std::min(
                std::max(camera->size()[0], camera->size()[1]) /
                std::max(pattern_->rows, 2 * pattern_->cols),
                std::min(camera->size()[0], camera->size()[1]) /
                std::min(pattern_->rows, 2 * pattern_->cols)) / pattern->squareSize * pattern->circleRadius * 1.5;
    } else {
        circleRadiusThreshold_ = std::min(
                std::max(camera->size()[0], camera->size()[1]) /
                std::max(pattern_->rows, pattern_->cols),
                std::min(camera->size()[0], camera->size()[1]) /
                std::min(pattern_->rows, pattern_->cols)) / pattern->squareSize * pattern->circleRadius * 1.5;
    }
}

opengv2::CirclesEventFrame::Params::Params() {
    dbscan_eps = 4;
    dbscan_startMinSample = 2;
    clusterMinSample = 5;
    knn_num = 3;
}

opengv2::CirclesEventFrame::Params::Params(const cv::FileStorage &node) {
    node["dbscan_eps"] >> dbscan_eps;
    node["dbscan_startMinSample"] >> dbscan_startMinSample;
    node["clusterMinSample"] >> clusterMinSample;
    node["knn_num"] >> knn_num;
    node["fitCircle"] >> fitCircle;
}

struct CircleErr {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CircleErr(double fitErr, double radius, const Eigen::Ref<const Eigen::Vector2d> &center) :
            fitErr(fitErr), radius(radius), center(center) {}

    double fitErr;
    double radius;
    Eigen::Vector2d center;
};

bool opengv2::CirclesEventFrame::extractFeatures() {
    if (positiveEvents_.empty() || negativeEvents_.empty()) {
        return false;
    }

    auto dbscan = DBSCAN<Eigen::Vector2d, double>();
    dbscan.Run(&positiveEvents_, 2, params_.dbscan_eps, params_.dbscan_startMinSample);
    auto p_noise = std::move(dbscan.Noise);
    auto p_clusters = std::move(dbscan.Clusters);
    dbscan.Run(&negativeEvents_, 2, params_.dbscan_eps, params_.dbscan_startMinSample);
    auto n_noise = std::move(dbscan.Noise);
    auto n_clusters = std::move(dbscan.Clusters);

    /** Draw the result and filter data **/
    auto camera = dynamic_cast<CameraBase *>(sensor_.get());
    image_ = cv::Mat(camera->size()[1], camera->size()[0], CV_8UC3, cv::Vec3b(0, 0, 0));

    //debug
    eventImage = image_.clone();
    for (const Eigen::Vector2d &p: positiveEvents_) {
        cv::Point loc(p[0], p[1]);
        eventImage.at<cv::Vec3b>(loc) = cv::Vec3b(0, 0, 255);
    }
    for (const Eigen::Vector2d &p: negativeEvents_) {
        cv::Point loc(p[0], p[1]);
        eventImage.at<cv::Vec3b>(loc) = cv::Vec3b(0, 255, 0);
    }

    for (auto itr = p_clusters.begin(); itr != p_clusters.end();) {
        // remove cluster with too few samples
        if (itr->size() < params_.clusterMinSample) {
            itr = p_clusters.erase(itr);
        } else {
            unsigned int color = 20 * (itr - p_clusters.begin());
            for (unsigned int idx : *itr) {
                cv::Point loc(positiveEvents_[idx][0], positiveEvents_[idx][1]);
                image_.at<cv::Vec3b>(loc) = cv::Vec3b(color / 256, color % 256, 200);
            }

            itr++;
        }
    }

    for (auto itr = n_clusters.begin(); itr != n_clusters.end();) {
        // remove cluster with too few samples
        if (itr->size() < params_.clusterMinSample) {
            itr = n_clusters.erase(itr);
        } else {
            unsigned int color = 20 * (itr - n_clusters.begin());
            for (unsigned int idx : *itr) {
                cv::Point loc(negativeEvents_[idx][0], negativeEvents_[idx][1]);
                image_.at<cv::Vec3b>(loc) = cv::Vec3b(color / 256, color % 256, 100);
            }

            itr++;
        }
    }

    // store
    nClusters_ = n_clusters;
    pClusters_ = p_clusters;

    // debug
    clusterImage = image_.clone();

    // too few events
    if (p_clusters.size() < pattern_->rows * pattern_->cols || n_clusters.size() < pattern_->rows * pattern_->cols) {
        return false;
    }

    /** find candidate circles **/
    std::vector<std::pair<size_t, size_t>> candidates;
    vectorofEigenMatrix<Eigen::Vector2d> candidateCenters;
    std::vector<double> candidatesRadius;

    // calculate each cluster center (median)
    std::vector<uint> p_centers, n_centers;
    auto p_compare_fun = [&](uint lhs, uint rhs) { return positiveEvents_[lhs].norm() < positiveEvents_[rhs].norm(); };
    auto n_compare_fun = [&](uint lhs, uint rhs) { return negativeEvents_[lhs].norm() < negativeEvents_[rhs].norm(); };
    for (auto &pCluster: p_clusters) {
        std::nth_element(pCluster.begin(), pCluster.begin() + pCluster.size() / 2, pCluster.end(), p_compare_fun);
        p_centers.push_back(pCluster[pCluster.size() / 2]);
    }
    for (auto &nCluster: n_clusters) {
        std::nth_element(nCluster.begin(), nCluster.begin() + nCluster.size() / 2, nCluster.end(), n_compare_fun);
        n_centers.push_back(nCluster[nCluster.size() / 2]);
    }

    /*// draw cluster centers
    for (auto center: p_centers) {
        cv::Point loc(positiveEvents_[center][0], positiveEvents_[center][1]);
        cv::circle(image_, loc, 2, cv::Vec3b(255, 255, 255));
    }
    for (auto center: n_centers) {
        cv::Point loc(negativeEvents_[center][0], negativeEvents_[center][1]);
        cv::circle(image_, loc, 2, cv::Vec3b(255, 255, 255));
    }*/

    // building k-d tree
    vectorofEigenMatrix<Eigen::Vector2d> pCenters(p_centers.size()), nCenters(n_centers.size());
    for (int i = 0; i < p_centers.size(); ++i) {
        pCenters[i] = positiveEvents_[p_centers[i]];
    }
    for (int i = 0; i < n_centers.size(); ++i) {
        nCenters[i] = negativeEvents_[n_centers[i]];
    }
    KDTreeVectorOfVectorsAdaptor<vectorofEigenMatrix<Eigen::Vector2d>, double, 2, nanoflann::metric_L2_Simple>
            p_kdTree(2, pCenters, 10), n_kdTree(2, nCenters, 10);

    // Store fitError for Reusing
    std::unordered_map<std::pair<size_t, size_t>, CircleErr, pair_hash> fitErrandRadiusMap;

    const size_t num_results = params_.fitCircle ? params_.knn_num : 1;
    std::vector<size_t> n_idx(num_results);
    std::vector<size_t> p_idx(num_results);
    std::vector<double> out_dists_sqr(num_results);
    vectorofEigenMatrix<Eigen::Vector2d> centers(num_results);
    std::vector<double> radius(num_results);
    std::vector<double> fitErrs;
    if (params_.fitCircle) {
        for (size_t pi = 0; pi < p_centers.size(); ++pi) {
            size_t realNum_results = num_results;
            // search knn in opposite polarity cluster
            n_kdTree.query(positiveEvents_[p_centers[pi]].data(), num_results, n_idx.data(), out_dists_sqr.data());

            // remove cluster too far away
            for (int oi = 0; oi < out_dists_sqr.size(); ++oi) {
                if (out_dists_sqr[oi] > out_dists_sqr[0] * 4 ||
                    out_dists_sqr[oi] > 4 * circleRadiusThreshold_ * circleRadiusThreshold_) {
                    realNum_results = oi;
                    break;
                }
            }
            if (realNum_results == 0)
                continue;

            // connect two centers as diameter, verify circle fitting error
            fitErrs.assign(realNum_results, 0);
            for (int j = 0; j < realNum_results; ++j) {
                auto itr = fitErrandRadiusMap.find(std::pair<size_t, size_t>(pi, n_idx[j]));
                if (itr == fitErrandRadiusMap.end()) {
                    fitCircle(p_clusters[pi], n_clusters[n_idx[j]], centers[j], radius[j]);

                    double approxRadius =
                            (positiveEvents_[p_centers[pi]] - negativeEvents_[n_centers[n_idx[j]]]).norm() / 2;
                    if (radius[j] > circleRadiusThreshold_ || radius[j] > 2 * approxRadius) {
                        fitErrs[j] = std::numeric_limits<double>::max();
                    } else {
                        for (auto pEvent: p_clusters[pi]) {
                            fitErrs[j] += std::abs((positiveEvents_[pEvent] - centers[j]).norm() - radius[j]);
                        }
                        for (auto nEvent: n_clusters[n_idx[j]]) {
                            fitErrs[j] += std::abs((negativeEvents_[nEvent] - centers[j]).norm() - radius[j]);
                        }

                        fitErrs[j] /= (p_clusters[pi].size() + n_clusters[n_idx[j]].size()) * radius[j];
                    }
                    fitErrandRadiusMap.emplace(std::pair<size_t, size_t>(pi, n_idx[j]),
                                               CircleErr(fitErrs[j], radius[j], centers[j]));
                } else {
                    fitErrs[j] = itr->second.fitErr;
                    radius[j] = itr->second.radius;
                    centers[j] = itr->second.center;
                }
            }
            int n_minIdx = std::min_element(fitErrs.begin(), fitErrs.end()) - fitErrs.begin();

            // Do circle check for the minimum, if the fitting error greater than Expectation(assume gaussian noise)
            const double gaussianNoise = 2 / radius[n_minIdx]; // TODO: considering ellipse
            if (fitErrs[n_minIdx] < gaussianNoise) {
                /* Double Direction Check */
                realNum_results = num_results;
                p_kdTree.query(negativeEvents_[n_centers[n_idx[n_minIdx]]].data(), num_results, p_idx.data(),
                               out_dists_sqr.data());
                for (int oi = 0; oi < out_dists_sqr.size(); ++oi) {
                    if (out_dists_sqr[oi] > out_dists_sqr[0] * 4 ||
                        out_dists_sqr[oi] > 4 * circleRadiusThreshold_ * circleRadiusThreshold_) {
                        realNum_results = oi;
                        break;
                    }
                }
                if (realNum_results == 0)
                    continue;
                fitErrs.assign(realNum_results, 0);
                for (int i = 0; i < realNum_results; ++i) {
                    auto itr = fitErrandRadiusMap.find(std::pair<size_t, size_t>(p_idx[i], n_idx[n_minIdx]));
                    if (itr == fitErrandRadiusMap.end()) {
                        fitCircle(p_clusters[p_idx[i]], n_clusters[n_idx[n_minIdx]], centers[i], radius[i]);

                        double approxRadius = (positiveEvents_[p_centers[p_idx[i]]] -
                                               negativeEvents_[n_centers[n_idx[n_minIdx]]]).norm() / 2;
                        if (radius[i] > circleRadiusThreshold_ || radius[i] > 2 * approxRadius) {
                            fitErrs[i] = std::numeric_limits<double>::max();
                        } else {
                            for (auto pEvent: p_clusters[p_idx[i]]) {
                                fitErrs[i] += std::abs((positiveEvents_[pEvent] - centers[i]).norm() - radius[i]);
                            }
                            for (auto nEvent: n_clusters[n_idx[n_minIdx]]) {
                                fitErrs[i] += std::abs((negativeEvents_[nEvent] - centers[i]).norm() - radius[i]);
                            }
                            fitErrs[i] /=
                                    (p_clusters[p_idx[i]].size() + n_clusters[n_idx[n_minIdx]].size()) * radius[i];
                        }
                        fitErrandRadiusMap.emplace(std::pair<size_t, size_t>(p_idx[i], n_idx[n_minIdx]),
                                                   CircleErr(fitErrs[i], radius[i], centers[i]));
                    } else {
                        fitErrs[i] = itr->second.fitErr;
                        radius[i] = itr->second.radius;
                        centers[i] = itr->second.center;
                    }
                }
                int p_minIdx = std::min_element(fitErrs.begin(), fitErrs.end()) - fitErrs.begin();

                // if the same, add to candidate
                if (p_idx[p_minIdx] == pi) {
                    candidates.emplace_back(pi, n_idx[n_minIdx]);
                    candidateCenters.push_back(centers[p_minIdx]);
                    candidatesRadius.push_back(radius[p_minIdx]);
                }
            }
        }
    } else {
        for (size_t pi = 0; pi < p_centers.size(); ++pi) {
            n_kdTree.query(positiveEvents_[p_centers[pi]].data(), num_results, n_idx.data(), out_dists_sqr.data());
            // remove cluster too far away
            if (out_dists_sqr[0] > 4 * circleRadiusThreshold_ * circleRadiusThreshold_) {
                continue;
            }
            p_kdTree.query(negativeEvents_[n_centers[n_idx[0]]].data(), num_results, p_idx.data(),
                           out_dists_sqr.data());
            if (p_idx[0] == pi) {
                // check circle
                Eigen::Vector2d center =
                        (positiveEvents_[p_centers[p_idx[0]]] + negativeEvents_[n_centers[n_idx[0]]]) / 2;
                double r = (positiveEvents_[p_centers[p_idx[0]]] - negativeEvents_[n_centers[n_idx[0]]]).norm() / 2;
                double fitErr = 0;
                for (auto pEvent: p_clusters[p_idx[0]]) {
                    fitErr += std::abs((positiveEvents_[pEvent] - center).norm() - r);
                }
                for (auto nEvent: n_clusters[n_idx[0]]) {
                    fitErr += std::abs((negativeEvents_[nEvent] - center).norm() - r);
                }
                fitErr /= (p_clusters[p_idx[0]].size() + n_clusters[n_idx[0]].size()) * r;

                if (fitErr < 10 / r) {
                    candidates.emplace_back(pi, n_idx[0]);
                    candidateCenters.push_back(center);
                    candidatesRadius.push_back(r);
                }
            }
        }
    }

    // draw candidate circle
    for (int i = 0; i < candidates.size(); ++i) {
        cv::Point loc(candidateCenters[i][0], candidateCenters[i][1]);
        cv::circle(image_, loc, candidatesRadius[i], cv::Vec3b(0, 255, 0));
    }

    /** use prior pattern to match the candidate **/
    std::vector<cv::Point2f> points, outCenters;
    for (const Eigen::Vector2d &center: candidateCenters) {
        points.emplace_back(center[0], center[1]);
    }
    // TODO: 1. improve the sensitivity to additional nearby outlier
    //  2. we should assume there are absence,
    //  (even wrong pairs inside the pattern (seems too hard, maybe solved previously by more samples)),
    //  also the centers are not accurate.
    //  (solution) maybe optimization liangzu;
    //  or maybe solved by mul-spline segment(just ignore the error segment) and ignore with go on;
    //  user interaction (note: find sample in raw data)
    bool isFound = cv::findCirclesGrid(points, cv::Size(pattern_->cols, pattern_->rows), outCenters,
                                       cv::CALIB_CB_ASYMMETRIC_GRID);
    if (!isFound)
        isFound = cv::findCirclesGrid(points, cv::Size(pattern_->cols, pattern_->rows), outCenters,
                                      cv::CALIB_CB_ASYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING);
    /*if (isFound)
        circleExtractionImage = image_.clone();*/
    drawChessboardCorners(image_, cv::Size(pattern_->cols, pattern_->rows), cv::Mat(outCenters), isFound);
    if (isFound) {
        KDTreeVectorOfVectorsAdaptor<vectorofEigenMatrix<Eigen::Vector2d>, double, 2, nanoflann::metric_L2_Simple>
                kdTree(2, candidateCenters, 10);
        std::vector<size_t> orderIdxs(pattern_->rows * pattern_->cols);
        std::vector<double> out_dist_sqr(1);
        for (int i = 0; i < outCenters.size(); ++i) {
            Eigen::Vector2d c(outCenters[i].x, outCenters[i].y);
            kdTree.query(c.data(), 1, orderIdxs.data() + i, out_dist_sqr.data());
        }

        // features_
        for (auto idx: orderIdxs) {
            features_.push_back(std::make_shared<CalibCircle>(candidateCenters[idx], candidatesRadius[idx]));
        }

        //detectionImage = image_.clone();
    }

    return isFound;
}

void opengv2::CirclesEventFrame::fitCircle(const std::vector<uint> &pSet,
                                           const std::vector<uint> &nSet,
                                           Eigen::Ref<Eigen::Vector2d> center, double &radius) {
    double sum_x = 0, sum_y = 0;
    double sum_xx = 0, sum_yy = 0, sum_xy = 0;
    double sum_xxx = 0, sum_yyy = 0, sum_xyy = 0, sum_xxy = 0;
    for (auto p_idx: pSet) {
        const Eigen::Vector2d &sample = positiveEvents_[p_idx];

        sum_x += sample[0];
        sum_y += sample[1];

        double xx = sample[0] * sample[0];
        double yy = sample[1] * sample[1];
        double xy = sample[0] * sample[1];

        sum_xx += xx;
        sum_yy += yy;
        sum_xy += xy;

        sum_xxx += xx * sample[0];
        sum_yyy += yy * sample[1];
        sum_xyy += xy * sample[1];
        sum_xxy += sample[0] * xy;
    }
    for (auto n_idx: nSet) {
        const Eigen::Vector2d &sample = negativeEvents_[n_idx];

        sum_x += sample[0];
        sum_y += sample[1];

        double xx = sample[0] * sample[0];
        double yy = sample[1] * sample[1];
        double xy = sample[0] * sample[1];

        sum_xx += xx;
        sum_yy += yy;
        sum_xy += xy;

        sum_xxx += xx * sample[0];
        sum_yyy += yy * sample[1];
        sum_xyy += xy * sample[1];
        sum_xxy += sample[0] * xy;
    }

    Eigen::Matrix3d A;
    A << 2 * sum_x, 2 * sum_y, pSet.size() + nSet.size(),
            2 * sum_xx, 2 * sum_xy, sum_x,
            2 * sum_xy, 2 * sum_yy, sum_y;
    Eigen::Vector3d b(sum_xx + sum_yy, sum_xxx + sum_xyy, sum_xxy + sum_yyy);

    Eigen::Vector3d x = A.lu().solve(b);
    center = x.block<2, 1>(0, 0);
    radius = std::sqrt(x[0] * x[0] + x[1] * x[1] + x[2]);
}

bool
opengv2::CirclesEventFrame::rectifyFeatures(const std::unordered_set<int> &outlierIdxs,
                                            const Eigen::Ref<const Eigen::Matrix3d> &Rcw,
                                            const Eigen::Ref<const Eigen::Vector3d> &tcw) {
    KDTreeVectorOfVectorsAdaptor<vectorofEigenMatrix<Eigen::Vector2d>, double, 2, nanoflann::metric_L2_Simple>
            p_kdTree(2, positiveEvents_, 10), n_kdTree(2, negativeEvents_, 10);

    Eigen::Matrix3d Rsw = Rcw;
    Eigen::Vector3d tsw = tcw;
    auto camera = dynamic_cast<PinholeCamera *>(sensor_.get());
    cv::Mat tvec, rvec, Rvec, distCoeffs, cameraMatrix;
    cv::eigen2cv(Rsw, Rvec);
    cv::Rodrigues(Rvec, rvec);
    cv::eigen2cv(tsw, tvec);
    cv::eigen2cv(camera->distCoeffs(), distCoeffs);
    cv::eigen2cv(camera->K(), cameraMatrix);
    for (int kIdx = 0; kIdx < features_.size(); kIdx++) {
        auto f = dynamic_cast<CalibCircle *>(features_[kIdx].get());
        auto lm = f->landmark();
        Eigen::Vector3d center = lm->position();

        std::vector<cv::Point3f> objectPoints;
        objectPoints.reserve(5);
        objectPoints.emplace_back(center[0], center[1], center[2]);
        double skewR = pattern_->circleRadius / std::sqrt(2);
        // Four quadrant, considering circle distortion
        objectPoints.emplace_back(center[0] + skewR, center[1] + skewR, center[2]);
        objectPoints.emplace_back(center[0] + skewR, center[1] - skewR, center[2]);
        objectPoints.emplace_back(center[0] - skewR, center[1] - skewR, center[2]);
        objectPoints.emplace_back(center[0] - skewR, center[1] + skewR, center[2]);

        std::vector<cv::Point2f> imagePoints;
        cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);
        vectorofEigenMatrix<Eigen::Vector2d> eigenImagePoints;
        eigenImagePoints.reserve(5);
        for (const auto &p: imagePoints) {
            eigenImagePoints.emplace_back(p.x, p.y);
        }

        // if center exceed image bound, delete feature
        if (imagePoints[0].x >= camera->size()[0] || imagePoints[0].y >= camera->size()[1] ||
            imagePoints[0].x < 0 || imagePoints[0].y < 0) {
            features_[kIdx] = nullptr;
            continue;
        }

        std::vector<double> radius;
        double maxRadius = 0;
        radius.reserve(4);
        for (int i = 1; i < 5; ++i) {
            radius.push_back((eigenImagePoints[i] - eigenImagePoints[0]).norm());
            if (radius.back() > maxRadius) {
                maxRadius = radius.back();
            }
        }

        double inlierThreshold = 3; // pixel unit
        std::vector<std::pair<size_t, double>> p_IndicesDists, n_IndicesDists;
        p_kdTree.index->radiusSearch(eigenImagePoints[0].data(), std::pow(maxRadius + inlierThreshold, 2),
                                     p_IndicesDists,
                                     nanoflann::SearchParams(32, 0, false));
        n_kdTree.index->radiusSearch(eigenImagePoints[0].data(), std::pow(maxRadius + inlierThreshold, 2),
                                     n_IndicesDists,
                                     nanoflann::SearchParams(32, 0, false));

        // collect inliers
        std::pair<std::vector<uint>, std::vector<uint>> patternCircle;
        for (const auto &pair: p_IndicesDists) {
            Eigen::Vector2d direction = positiveEvents_[pair.first] - eigenImagePoints[0];
            double distance = std::sqrt(pair.second);
            int idx = 0;
            if (direction[0] >= 0 && direction[1] >= 0) {
                idx = 0;
            } else if (direction[0] >= 0 && direction[1] <= 0) {
                idx = 1;
            } else if (direction[0] <= 0 && direction[1] <= 0) {
                idx = 2;
            } else if (direction[0] <= 0 && direction[1] >= 0) {
                idx = 3;
            }

            if (std::abs(distance - radius[idx]) <= inlierThreshold) {
                patternCircle.first.push_back(pair.first);
            }
        }
        for (const auto &pair: n_IndicesDists) {
            Eigen::Vector2d direction = negativeEvents_[pair.first] - eigenImagePoints[0];
            double distance = std::sqrt(pair.second);
            int idx = 0;
            if (direction[0] >= 0 && direction[1] >= 0) {
                idx = 0;
            } else if (direction[0] >= 0 && direction[1] <= 0) {
                idx = 1;
            } else if (direction[0] <= 0 && direction[1] <= 0) {
                idx = 2;
            } else if (direction[0] <= 0 && direction[1] >= 0) {
                idx = 3;
            }

            if (std::abs(distance - radius[idx]) <= inlierThreshold) {
                patternCircle.second.push_back(pair.first);
            }
        }

        // expand sample set by clustering
        std::set<uint> pClusterSet, nClusterSet;
        std::unordered_map<uint, uint> pSample2Set, nSample2Set;
        for (int i = 0; i < pClusters_.size(); ++i) {
            for (int j = 0; j < pClusters_[i].size(); ++j) {
                pSample2Set.emplace(pClusters_[i][j], i);
            }
        }
        for (int i = 0; i < nClusters_.size(); ++i) {
            for (int j = 0; j < nClusters_[i].size(); ++j) {
                nSample2Set.emplace(nClusters_[i][j], i);
            }
        }
        for (const auto &p : patternCircle.first) {
            if (pSample2Set.find(p) != pSample2Set.end())
                pClusterSet.insert(pSample2Set[p]);
        }
        for (const auto &n : patternCircle.second) {
            if (nSample2Set.find(n) != nSample2Set.end())
                nClusterSet.insert(nSample2Set[n]);
        }
        patternCircle.first.clear();
        for (const auto &pCluster: pClusterSet) {
            for (const auto &p: pClusters_[pCluster]) {
                patternCircle.first.push_back(p);
            }
        }
        patternCircle.second.clear();
        for (const auto &nCluster: nClusterSet) {
            for (const auto &n: nClusters_[nCluster]) {
                patternCircle.second.push_back(n);
            }
        }

        // remove feature with too few measurements
        if (patternCircle.first.size() < 5 || patternCircle.second.size() < 5) {
            features_[kIdx] = nullptr;
            continue;
        }

        Eigen::Vector2d rectifiedCenter;
        double rectifiedRadius;
        fitCircle(patternCircle.first, patternCircle.second, rectifiedCenter,
                  rectifiedRadius);

        std::nth_element(radius.begin(), radius.begin() + radius.size() / 2, radius.end());
        // if fit result not good, delete feature
        if ((rectifiedCenter - eigenImagePoints[0]).norm() > 2 * inlierThreshold ||
            std::abs(rectifiedRadius - radius[radius.size() / 2]) > 1.5 * inlierThreshold) {
            features_[kIdx] = nullptr;
            continue;
        }

        f->setLocation(rectifiedCenter);
        f->radius = rectifiedRadius;
    }

    // release no longer used member
    positiveEvents_.clear();
    negativeEvents_.clear();

    // outlier features in the edges are more important
    std::vector<std::unordered_set<int>> edgePattern(4);
    std::vector<int> score(4, 0);
    for (int i = 0; i < pattern_->cols; ++i) // 1st row
        edgePattern[0].insert(i);
    for (int i = (pattern_->rows - 1) * pattern_->cols; i < (pattern_->rows * pattern_->cols); ++i) // last row
        edgePattern[1].insert(i);
    for (int i = 0;
         i < (pattern_->rows * pattern_->cols); i += (pattern_->isAsymmetric ? 2 : 1) * pattern_->cols) // 1st col
        edgePattern[2].insert(i);
    for (int i = pattern_->isAsymmetric ? (2 * pattern_->cols - 1) : (pattern_->cols - 1);
         i < (pattern_->rows * pattern_->cols); i += (pattern_->isAsymmetric ? 2 : 1) * pattern_->cols) // last col
        edgePattern[3].insert(i);

    int counter = 0, index = 0;
    for (auto itr = features_.begin(); itr != features_.end(); index++) {
        if (*itr == nullptr) {
            itr = features_.erase(itr);
            for (int i = 0; i < 4; ++i) {
                if (edgePattern[i].find(index) != edgePattern[i].end())
                    score[i]++;
            }
            counter++;
        } else {
            itr++;
        }
    }

    // check features on edge
    if (!params_.fitCircle) {
        for (int i = 0; i < 4; ++i) {
            if (score[i] >= (int) (edgePattern[i].size() - 1))
                return false;
        }
    }

    // remove frame with too many features gone
    if (counter >= 0.2 * (pattern_->cols * pattern_->rows)) {
        return false;
    }

    for (auto fb: features_) {
        auto f = dynamic_cast<CalibCircle *>(fb.get());
        cv::Point loc(f->location()[0], f->location()[1]);
        cv::circle(image_, loc, f->radius, cv::Vec3b(255, 255, 255));
    }

    // Building KD-Tree for neighbor searching on features_, used for establish corresponds for given event.
    circles_.reserve(features_.size());
    for (const auto &f: features_) {
        circles_.push_back(f->location());
    }
    circleKdTree_ = std::make_shared<KDTreeVectorOfVectorsAdaptor<vectorofEigenMatrix<Eigen::Vector2d>, double, 2>>
            (2, circles_, 10);

    return true;
}


#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <message_filters/subscriber.h>
#include <boost/math/distributions/normal.hpp>
#include <boost/random.hpp>
#include <tf/transform_listener.h>

namespace
{
using namespace std;
using namespace geometry_msgs;
using namespace std_msgs;

static const double MEAN = 0.0;
static const double STD_DEV = 2.0;

class ImuGaussianNoise
{
private:
    //! Node handle
    ros::NodeHandle nh;

    //! Private nh
    ros::NodeHandle pnh;

    //! IMU subscription
    auto_ptr<message_filters::Subscriber<sensor_msgs::Imu> > imuSub;

    //! Publisher for the filterered IMU
    ros::Publisher filteredPub;
    boost::normal_distribution<double> dist;

    boost::mt19937 engine;

public:
    ImuGaussianNoise() :
        pnh("~"), dist(MEAN, STD_DEV)
    {
        imuSub.reset(new message_filters::Subscriber<sensor_msgs::Imu>(nh, "/in", 1));
        imuSub->registerCallback(boost::bind(&ImuGaussianNoise::imuCallback, this, _1));

        engine.seed(0);
        filteredPub = nh.advertise<sensor_msgs::Imu>("/out", 1);
    }

private:

    void imuCallback(sensor_msgs::ImuConstPtr data)
    {
        ROS_DEBUG("Unfiltered angular velocity [%f] [%f] [%f]", data->angular_velocity.x, data->angular_velocity.y, data->angular_velocity.z);

        // Copy over all fields
        sensor_msgs::ImuPtr output(new sensor_msgs::Imu(*data));

        // Compute random noise on xyz velocity
        boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > randNormal(engine, dist);

        double x = randNormal();
        double y = randNormal();
        double z = randNormal();

        ROS_DEBUG("Generated random parameters for x [%f], y [%f], z [%f]", x, y, z);
        output->angular_velocity.x += x;
        output->angular_velocity.y += y;
        output->angular_velocity.z += z;

        // Compute random noise on position
        double r = randNormal() / 20.0;
        double p = randNormal() / 20.0;
        double yaw = randNormal() / 20.0;
        ROS_INFO("Generated random parameters for r [%f], p [%f], yaw [%f]", r, p, yaw);

        tf::Quaternion update = tf::createQuaternionFromRPY(r, p, yaw);
        tf::Quaternion curr;
        tf::quaternionMsgToTF(data->orientation, curr);
        tf::quaternionTFToMsg((curr + update).normalized(), output->orientation);

        filteredPub.publish(output);
    }
};
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "imu_gaussian_noise");

    ImuGaussianNoise imuFilter;
    ros::spin();
}


/**
 *  test_dyn_longitudinal.cpp
 *
 *  The longitudinal kinematics of DC9-30.
 *
 *  Created by Yinan Li on Mar.27, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <boost/numeric/odeint.hpp>

#include "src/system.hpp"
#include "src/csolver.h"
#include "src/matlabio.h"

typedef std::array<double, 3> state_type;

/* Parameters of the model */
double mg = 60000.0*9.81;
double mi = 1.0/60000; /* weight inverse: 1/m */

/* ODE of the longitudinal equation of motions for DC9-30 */
struct eomlong {
    static const int n = 3;  // state dimension
    static const int m = 2;  // control dimension

    /**
     * Constructors:
     * @param[out] dx (dV, dgamma, dh)
     * @param[in] x (V,gamma,h).
     * @param[in] u (T, alpha) control input (thrust, angle of attack)
     */
    template<typename S>
    eomlong(S *dx, const S *x, rocs::Rn u) {
	double c = 1.25+4.2*u[1];
	dx[0] = mi*(u[0]*cos(u[1])-(2.7+3.08*c*c)*x[0]*x[0]-mg*sin(x[1]));
	dx[1] = (1.0/(60000*x[0]))*(u[0]*sin(u[1])+68.6*c*x[0]*x[0]-mg*cos(x[1]));
	dx[2] = x[0]*sin(x[1]);
    }

};

int main(int argc, char *argv[])
{
    /**
     * Define the control system
     **/
    /* Set sampling time and disturbance */
    double tau = 0.25;
    double delta = 10;
    /* Set parameters for computation */
    int kmax = 5;
    double tol = 0.01;
    double alpha = 0.5;
    double beta = 2;
    rocs::params controlparams(kmax, tol, alpha, beta);
    rocs::CTCntlSys<eomlong> aircraft("landing", tau, eomlong::n,
				      eomlong::m, delta, &controlparams);

    /* set the state space */
    double xlb[] = {58, -3*M_PI/180, 0};
    double xub[] = {83, 0, 56};
    double ulb[] = {0,0};
    double uub[] = {32000, 8*M_PI/180};
    double mu[] = {32000, 9.0/8.0*M_PI/180};
    aircraft.init_workspace(xlb, xub);
    aircraft.init_inputset(mu, ulb, uub);
    // std::cout << "# of u= " << aircraft._ugrid._nv
    // 	      << ", dimension= " << aircraft._ugrid._dim << '\n';
    // for(int i = 0; i < aircraft._ugrid._nv; ++i) {
    // 	std::cout << aircraft._ugrid._data[i][0] << ','
    // 		  << aircraft._ugrid._data[i][1] << '\n';
    // }
    aircraft.allocate_flows();

    /* test the reachable set */
    // rocs::ivec x= {rocs::interval(60,60.05),
    // 		   rocs::interval(-M_PI/180,-21/22*M_PI/180),
    // 		   rocs::interval(16,16.2)};
    rocs::ivec x= {rocs::interval(80.0,81.0),
		   rocs::interval(-0.0196,-0.0131),
		   rocs::interval(54.25,56.00)};
    // rocs::Rn u = {0,0};
    std::vector<rocs::ivec> y(aircraft._ugrid._nv, rocs::ivec(3));
    std::cout << "The initial interval: " << x << '\n';
    aircraft.get_reach_set(y, x);
    std::cout << "The reachable set of x: \n";
    // std::cout << y[12] << '\n';
    for (int i = 0; i < y.size(); ++i)
    	std::cout << y[i] <<'\n';

    aircraft.release_flows();
    return 0;
}


#include <boost/math/special_functions/sinhc.hpp>


#include <wav_utils.hxx>

#include <iostream>

///stats.hpp>
//#include <boost/accumulators/statistics/mean.hpp>
//#include <boost/accumulators/statistics/variance.hpp>

// this program is a lot like basic_convolution.cxx in ../fast_fourier.
// in this one, I read two wav files and find the point of maximum correlation.

// I then FFT both, multiple the results FFT1 * complex_conjugate(FFT2) at each position.

// last, I inverse FFT that and save to channels 3/4 (real/imaginary)

// ffmpeg -i sample_video/htc/VIDEO0635.mp4 -map 0:a -ac 1 tmp/htc.wav
// ffmpeg -i sample_video/s5/VID_20170608_152423.mp4 -map 0:a tmp/s5.wav
// find . | xargs -I file ffmpeg -i file -map 0:a -ac 1 -ar 10000 -y ../tmp10k/file

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

  if (argc < 3) {
    std::cerr << "need two wav filenames" << std::endl;
    exit(-1);
  }

  bool include_cc = false;
  if (argc > 3) {
    if (std::string(argv[3]) == "true") {
      include_cc = true;
    } else {
      printf("unknown third option. expecting true, got %s\n", argv[3]);
      exit(-1);
    }
  }

  wav_samples ws1;
  ws1.filename = argv[1];
  read_wav(ws1);
  wav_samples ws2;
  ws2.filename = argv[2];
  read_wav(ws2);

  // I want the fftsize to be twice the sample length. Why?
  // If ws2 preceeds ws1, then the peak will be in the second
  // half of the cross-correlation.
  // What if ws1 preceeds ws2 by more than half of the sample
  // length? In that case, the peak will also be in the second
  // half.
  // By using twice the length, that second case will have a peak
  // in the first half. A peak in the second half will only be
  // possible with case 1.
  int fftsize = std::max(ws1.frames, ws2.frames)*2;
  std::cout << "need fft size of " << fftsize << std::endl;

  add_fft(ws1, fftsize);
  add_fft(ws2, fftsize);

  correlation_data cd = correlate_wavs(ws1, ws2, fftsize, ws1.samplerate, include_cc);

  std::cout << "offset is " << double(cd.offset)/double(ws1.samplerate) << std::endl;

  const int channels = (include_cc ? 4 : 2);

  // have to divide by two since we multiplied above.
  const int numaligned = fftsize/2+std::abs(cd.offset);

  std::vector<double> samples(numaligned*channels,0);

  for(int i=0; i<numaligned; i++) {
    samples[channels*i] = 0;
    samples[channels*i+1] = 0;
  }

  // in correlation_data struct, wss1 will be earlier in the timeline.
  std::cout << cd.wss1->filename << " is ahead of " << cd.wss2->filename
            << " by " << double(cd.offset)/cd.wss1->samplerate << std::endl;

  for(int i=0; i<cd.wss1->frames; i++) {
    samples[channels*i] =               cd.wss1->samples[cd.wss1->channels*i];
  }
  for(int i=0; i<cd.wss2->frames; i++) {
    // the +1 is because we want the second channel.
    samples[channels*(i+cd.offset)+1] = cd.wss2->samples[cd.wss2->channels*i];
  }

  if (include_cc) {
    double maxval = 0;

    for(int i=0; i<fftsize/2; i++) {
      maxval = std::max(maxval, std::abs(cd.cross_correlation[i]));
    }
    for(int i=0; i<fftsize/2; i++) {
      samples[channels*i+2] =           cd.cross_correlation[i].real()/maxval;
      samples[channels*i+3] =           cd.cross_correlation[i].imag()/maxval;
    }
  }

  write_wav(samples, numaligned, cd.wss1->samplerate, channels, std::string("aligned.wav"));

}


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <iterator>
#include <iomanip>
#include <Eigen/Dense>
#include <cmath>
#include <random>
#include "constants.h"
#include "simulationbox.h"
#include "fileinput.h"
#include "fileoutput.h"
#include "thermostats.h"

using namespace Eigen;

class LennardJones
{
    private:
         Vector3d cutoffadjust;
        double sigma;
        double epsilon;
        double cutoff;
    public:
        LennardJones(double, double, double);
        Vector3d getForce(Vector3d);
};

LennardJones::LennardJones(double insigma, double inepsilon, double incutoff=10000) {
    // Constructs the Lennard Jones class, converting to atomic units as we go
    // We expect the sigma to be in angstrom,
    // The epsilon to be in Hartrees
    // and the cutoff to also be in angstrom

    sigma = insigma * constants::bohrAng;
    epsilon = inepsilon * constants::boltzHar;
    cutoff = incutoff * constants::bohrAng;
    double cutoff_pair_virial = 24 * epsilon / pow(cutoff, 2);
    double cutoff_sigma_6 = pow(sigma, 6) / pow(cutoff, 6);
    double cutoff_sigma_12 = pow(cutoff_sigma_6, 2);
    double cutoffadjust_value = cutoff_pair_virial * (2 * cutoff_sigma_12 - cutoff_sigma_6);
    cutoffadjust(0) = cutoffadjust_value;
    cutoffadjust(1) = cutoffadjust_value;
    cutoffadjust(2) = cutoffadjust_value;
}

Vector3d LennardJones::getForce(Vector3d distance)
{
    /* Class function for the Lennard Jones model that takes in
     * a radial distance and outputs the force vector,
     * and the energy in relation to that */
    double radiusSquared=distance.squaredNorm();
    if (radiusSquared > pow(cutoff,2)){
        // We're outside the range we wanted to look at
        // so just return no force and carry on
        Vector3d force(0.0, 0.0, 0.0);
        return force;
    }
    double sigma6 = pow(sigma, 6) / pow(radiusSquared, 3);
    double sigma12 = pow(sigma6, 2);
    //Allen and Tildesey eq 2.59-2.63, then eq 5.3
    double pair_virial = 24.0 * epsilon / radiusSquared;
    Vector3d force = (distance * pair_virial * (2 * sigma12 - sigma6)) + cutoffadjust;
    return force;
}

void implement_boundary_conditions(ArrayXXd &positions, const Box &inputBox) {
    // Mutates the input array to fix any atoms that are outside the
    // boundary conditions
    // Fails when an atom is more than twice the boundary away,
    // but something else has gone wrong then

    for (int atom=0; atom < positions.rows(); ++atom) {
        for (int dim=0; dim < positions.cols(); ++dim) {
            if (positions(atom, dim) < 0.0) {
                positions(atom, dim) += inputBox.sides(dim);
            }
            else if (positions(atom, dim) > inputBox.sides(dim)) {
                positions(atom, dim) -= inputBox.sides(dim);
            }
        }
    }
}

ArrayXXd populate_velocities(const Box &input_box, const ArrayXd &masses, double temperature){
    // Populates the velocity array with velocities sampled
    // randomly from a Gaussian distribution with mean 0 and
    // variance 1.

    // This is then rescaled to match the temperature before
    // it is used

    std::random_device device;
    std::mt19937 mt_rand(device());

    // This is the normal distribution generator
    double mean = 0.0;
    double var = 1.0;
    std::normal_distribution<double> gaussian(mean, var);

    ArrayXXd velocities = ArrayXXd::Zero(masses.rows(), input_box.dimensions);
    // Use the number of rows in the masses as a proxy for
    // number of atoms
    for (int atom=0; atom < masses.rows(); ++atom) {
        for (int dim=0; dim < input_box.dimensions; ++dim) {
            velocities(atom, dim) = gaussian(mt_rand);
        }
    }

    auto rescaler = VelocityRescale();
    rescaler.apply(velocities, masses, temperature);
    return velocities;
}

ArrayXXd populate_positions(const Box &inputBox, int num_x, int num_y, int num_z) {
    /* Fills the position array with atoms evenly spaced in the box
     * starting from (0, 0) */
    ArrayXXd positions(num_x * num_y * num_z, inputBox.dimensions);
    double x_increment = inputBox.sides(0) / static_cast<double>(num_x);
    double y_increment = inputBox.sides(1) / static_cast<double>(num_y);
    double z_increment = inputBox.sides(2) / static_cast<double>(num_z);
    int counter = 0;
    for (int row=0; row < num_x; ++row) {
        for (int col=0; col < num_y; ++col) {
            for (int plane=0; plane < num_z; ++plane) {
                positions(counter, 0) = row * x_increment;
                positions(counter, 1) = col * y_increment;
                positions(counter, 2) = plane * z_increment;
                ++counter;
            }
        }
    }
    return positions;
}

Vector3d implement_minimum_image(const Vector3d distance, const Box &box) {
    auto new_distance = distance;
    for (int dim=0; dim < box.dimensions; ++dim) {
        if (distance(dim) <= -0.5 * box.sides(dim)) {
            new_distance(dim) += box.sides(dim);
        } else if (distance(dim) > 0.5 * box.sides(dim)) {
            new_distance(dim) -= box.sides(dim);
        }
    }
    return new_distance;
}

ArrayXXd calculate_forces(ArrayXXd &atoms_array, const Box &box, LennardJones &potential_model) {
    /* Calculates the force according to a Lennard-Jones potential
     * from one atom to its nearest image, and returns
     * an array of these forces */
    ArrayXXd forces = ArrayXXd::Zero(atoms_array.rows(), atoms_array.cols());
    for (int i=0; i < atoms_array.rows(); ++i) {
        for (int j=0; j<i; ++j) {
            Vector3d distance = atoms_array.row(i) - atoms_array.row(j);
            distance = implement_minimum_image(distance, box);
            Vector3d instant_force(potential_model.getForce(distance));

            for (int dim=0; dim < box.dimensions; ++dim) {
                forces(i, dim) += instant_force(dim);
                forces(j, dim) -= instant_force(dim);
            }
        }
    }
    return forces;
}

void velocityVerlet(ArrayXXd &positions, ArrayXXd &velocities, double timestep, ArrayXd &masses,
                    const Box &box, LennardJones &potentialModel) {
    ArrayXXd accelerations = calculate_forces(positions, box, potentialModel).colwise() / masses;
    ArrayXXd positions_next = positions
                              + (velocities * timestep)
                              + (0.5 * accelerations * pow(timestep, 2));
    ArrayXXd accelerations_next = calculate_forces(positions_next, box, potentialModel).colwise() / masses;
    ArrayXXd velocities_next = velocities
                               + 0.5 * ( accelerations + accelerations_next) * timestep;
    velocities = velocities_next;
    implement_boundary_conditions(positions_next, box);
    positions = positions_next;
}

int main() {
    auto parameter_map = read_in_from_file("input.inpt");
    LennardJones potentialModel(std::stod(parameter_map["LJ_sigma"]),
                                std::stod(parameter_map["LJ_epsilon"]),
                                std::stod(parameter_map["LJ_cutoff"]));
    Box simulation_box(std::stod(parameter_map["length_x"]),
                       std::stod(parameter_map["length_y"]),
                       std::stod(parameter_map["length_z"]));
    int num_x = stoi(parameter_map["num_x"]);
    int num_y = stoi(parameter_map["num_y"]);
    int num_z = stoi(parameter_map["num_z"]);
    double temperature = std::stod(parameter_map["temperature"]);
    double timestep = std::stod(parameter_map["timestep"]);
    int rescaleStep=std::stoi(parameter_map["num_rescale"]);
    int outputStep=std::stoi(parameter_map["num_output"]);
    int numSteps=std::stoi(parameter_map["num_steps"]);

    ArrayXXd atoms = populate_positions(simulation_box, num_x, num_y, num_z);
    ArrayXd masses = ArrayXd::Zero(atoms.rows(), 1);
    masses += std::stod(parameter_map["mass"]);

    ArrayXXd velocities = populate_velocities(simulation_box, masses, temperature);

    clearOutputFiles(parameter_map["position_file"]);

    for (int step=0; step < numSteps; ++step) {
        velocityVerlet(atoms, velocities, timestep, masses, simulation_box, potentialModel);
        if (step % rescaleStep == 0) {
            rescale_velocities(velocities, masses, temperature);
        }

        if (step % outputStep == 0) {
            dumpToFile(atoms, parameter_map["position_file"], step);
            std::cout << "Step " << step << std::endl;
        }
    }
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_FRECHET_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_FRECHET_RNG_HPP

#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <boost/random/weibull_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    frechet_rng(double alpha,
                double sigma,
                RNG& rng) {
      using boost::variate_generator;
      using boost::random::weibull_distribution;

      static const char* function("frechet_rng");

      check_finite(function, "Shape parameter", alpha);
      check_positive(function, "Shape parameter", alpha);
      check_not_nan(function, "Scale parameter", sigma);
      check_positive(function, "Scale parameter", sigma);

      variate_generator<RNG&, weibull_distribution<> >
        weibull_rng(rng, weibull_distribution<>(alpha, 1.0/sigma));
      return 1.0 / weibull_rng();
    }

  }
}
#endif


#include <boost/math/special_functions/modf.hpp>


#pragma once
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>

#include <tbb/tbb.h>

#include "Utilities/Utility.hpp"
#include "Observables/Energy.hpp"
#include "./utils.hpp"

namespace yannq
{
template<typename Machine, typename Hamiltonian>
class SRMat;
} //namespace yannq

namespace Eigen { //namespace Eigen
namespace internal {
	template<typename Machine, typename Hamiltonian>
	struct traits<yannq::SRMat<Machine, Hamiltonian> >
		:  public Eigen::internal::traits<Eigen::SparseMatrix<typename Machine::Scalar> > {};
}
}// namespace Eigen;

namespace yannq
{
//! \addtogroup GroundState

//! \ingroup GroundState
//! This class that generates the quantum Fisher matrix for the stochastic reconfiguration (SR) method.
template<typename Machine, typename Hamiltonian>
class SRMat
	: public Eigen::EigenBase<SRMat<Machine, Hamiltonian> >
{
public:
	using Scalar = typename Machine::Scalar;
	using RealScalar = typename remove_complex<Scalar>::type;

	using Matrix = typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
	using Vector = typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

	using VectorConstRef = typename Eigen::Ref<const Vector>;
	using RealMatrix = typename Eigen::Matrix<RealScalar, Eigen::Dynamic, Eigen::Dynamic>;
	using RealVector = typename Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;

	using StorageIndex = uint32_t;

	enum {
		ColsAtCompileTime = Eigen::Dynamic,
		MaxColsAtCompileTime = Eigen::Dynamic,
		IsRowMajor = false
	};

private:
	uint32_t n_;

	const Machine& qs_;
	const Hamiltonian& ham_;

	Energy<Scalar, Hamiltonian> energy_;

	RealScalar shift_;

	Matrix deltas_;
	Vector deltaMean_;

	Vector energyGrad_;
	Vector weights_;

public:
	//! \param qs Machine that describes quantum states
	//! \param ham Hamiltonian for SR
	SRMat(const Machine& qs, const Hamiltonian& ham)
	  : n_{qs.getN()}, qs_(qs), ham_(ham), energy_(ham)
	{
	}

	Eigen::Index rows() const { return qs_.getDim(); }
	Eigen::Index cols() const { return qs_.getDim(); }

	template<typename Rhs>
	Eigen::Product<SRMat<Machine, Hamiltonian>, Rhs, Eigen::AliasFreeProduct>
			operator*(const Eigen::MatrixBase<Rhs>& x) const {
	  return Eigen::Product<SRMat<Machine, Hamiltonian>, Rhs, Eigen::AliasFreeProduct>(*this, x.derived());
	}

	//! \param rs Sampling results obtained from samplers.
	template<class SamplingResult>
	void constructFromSamples(SamplingResult&& sr)
	{
		weights_.resize(0);
		int nsmp = sr.size();

		constructDelta(qs_, sr, deltas_);
		constructObs(qs_, sr, energy_);

		deltaMean_ = deltas_.colwise().mean();
		deltas_ = deltas_.rowwise() - deltaMean_.transpose();

		energyGrad_ =  deltas_.adjoint() * energy_.elocs();
		energyGrad_ /= nsmp;
	}

	template<class SamplingResult>
	void constructFromWeightSamples(const Eigen::Ref<const RealVector>& weights, SamplingResult&& sr)
	{
		weights_ = weights;

		constructDelta(qs_, sr, deltas_);
		constructObsWeights(qs_, std::forward<SamplingResult>(sr), weights, energy_);

		deltaMean_ = weights.transpose()*deltas_;
		deltas_ = deltas_.rowwise() - deltaMean_.transpose();

		energyGrad_ =  deltas_.adjoint() * weights.asDiagonal()
			* energy_.elocs();
	}

	void setShift(RealScalar shift)
	{
		shift_ = shift;
	}

	RealScalar getShift() const
	{
		return shift_;
	}

	//! return \f$\langle \nabla_{\theta} \psi_\theta(\sigma) \rangle\f$
	const Vector& oloc() const&
	{
		return deltaMean_;
	}

	Vector oloc() &&
	{
		return deltaMean_;
	}

	Matrix corrMat()
	{
		int nsmp = deltas_.rows();
		if(weights_.size() == 0)
			return (deltas_.adjoint() * deltas_)/nsmp;
		else
			return (deltas_.adjoint() * weights_.asDiagonal() * deltas_);
	}

	RealScalar eloc() const
	{
		return std::real(energy_.eloc());
	}

	Scalar elocVar() const
	{
		return std::real(energy_.elocVar());
	}

	const Vector& energyGrad() const&
	{
		return energyGrad_;
	}
	Vector energyGrad() &&
	{
		return energyGrad_;
	}

	Vector apply(const Vector& rhs) const
	{
		assert(rhs.size() == qs_.getDim());
		Vector r = deltas_*rhs;

		Vector res;

		if(weights_.size() == 0)
		{
			r /= r.rows();
			res = deltas_.adjoint()*r;
		}
		else
		{
			r.array() *= weights_.array();
			res = deltas_.adjoint() * r;
		}

		return res + Scalar(shift_)*rhs;
	}

	/*! \brief Use conjugate gradient solover to solve the optimizing vector.
	 *
	 * We solve \f$ (S + \epsilon \mathbb{1})v = f \f$ where \f$ f \f$ is the gradient of the energy expectation values.
	 * \param shift \f$ \epsilon \f$ that controls regularization
	 * \param tol tolerance for CG solver
	 */
	Vector solveCG(RealScalar shift, RealScalar tol = 1e-4)
	{
		setShift(shift);
		Eigen::ConjugateGradient<
			SRMat<Machine, Hamiltonian>,
			Eigen::Lower|Eigen::Upper,
			Eigen::IdentityPreconditioner> cg;
		cg.compute(*this);
		cg.setTolerance(tol);
		return cg.solve(energyGrad_);

	}

	/**
	 * Solve S^{-1}vec using conjugate gradient method
	 */
	Vector solveCG(const VectorConstRef& vec, double shift, double tol = 1e-4)
	{
		setShift(shift);
		Eigen::ConjugateGradient<
			SRMat<Machine, Hamiltonian>,
			Eigen::Lower|Eigen::Upper,
			Eigen::IdentityPreconditioner> cg;
		cg.compute(*this);
		cg.setTolerance(tol);
		return cg.solve(vec);
	}

	/*! \brief Solve the optimizing vector by solving the linear equation exactly..
	 *
	 * We solve \f$ (S + \epsilon \mathbb{1})v = f \f$ where \f$ f \f$ is the gradient of the energy expectation values.
	 * \param shift \f$ \epsilon \f$ that controls regularization
	 */
	Vector solveExact(RealScalar shift)
	{
		Matrix mat = corrMat();
		mat += shift*Matrix::Identity(mat.rows(),mat.cols());
		Eigen::LLT<Matrix> llt{mat};
		return llt.solve(energyGrad_);
	}

};
} //namespace yannq

namespace Eigen {
namespace internal {
	template<typename Rhs, typename Machine, typename Hamiltonian>
	struct generic_product_impl<yannq::SRMat<Machine, Hamiltonian>, Rhs, SparseShape, DenseShape, GemvProduct> // GEMV stands for matrix-vector
	: generic_product_impl_base<yannq::SRMat<Machine, Hamiltonian>, Rhs, generic_product_impl<yannq::SRMat<Machine, Hamiltonian>, Rhs> >
	{
		typedef typename Product<yannq::SRMat<Machine, Hamiltonian>, Rhs>::Scalar Scalar;
		template<typename Dest>
		static void scaleAndAddTo(Dest& dst, const yannq::SRMat<Machine, Hamiltonian>& lhs, const Rhs& rhs, const Scalar& alpha)
		{
			// This method should implement "dst += alpha * lhs * rhs" inplace,
			// however, for iterative solvers, alpha is always equal to 1, so let's not bother about it.
			assert(alpha==Scalar(1) && "scaling is not implemented");
			EIGEN_ONLY_USED_FOR_DEBUG(alpha);

			dst += lhs.apply(rhs);
		}
	};
} //namespace internal
} //namespace Eigen