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

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

 * See LICENSE for the license information

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

/**
 *  @file   testGaussianFactorGraphUnordered.cpp
 *  @brief  Unit tests for Linear Factor Graph
 *  @author Christian Potthast
 *  @author Frank Dellaert
 *  @author Luca Carlone
 *  @author Richard Roberts
 **/

#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/VerticalBlockMatrix.h>

#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp>  // for operator +=
using namespace boost::assign;

#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>

using namespace std;
using namespace gtsam;

typedef std::tuple<size_t, size_t, double> SparseTriplet;
bool triplet_equal(SparseTriplet a, SparseTriplet b) {
  if (get<0>(a) == get<0>(b) && get<1>(a) == get<1>(b) &&
      get<2>(a) == get<2>(b)) return true;

  cout << "not equal:" << endl;
  cout << "\texpected: "
      "(" << get<0>(a) << ", " << get<1>(a) << ") = " << get<2>(a) << endl;
  cout << "\tactual:   "
      "(" << get<0>(b) << ", " << get<1>(b) << ") = " << get<2>(b) << endl;
  return false;
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, initialization) {
  // Create empty graph
  GaussianFactorGraph fg;
  SharedDiagonal unit2 = noiseModel::Unit::Create(2);

  fg +=
    JacobianFactor(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2),
    JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2),
    JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2),
    JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);

  EXPECT_LONGS_EQUAL(4, (long)fg.size());

  // Test sparse, which takes a vector and returns a matrix, used in MATLAB
  // Note that this the augmented vector and the RHS is in column 7
  Matrix expectedIJS =
      (Matrix(3, 21) <<
      1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 6., 7., 8., 7., 8., 7., 8.,
      1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 3., 4., 5., 6., 7., 7.,
      10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5.,
        1., -5., -5., 5., 5., -1., 1.5).finished();
  Matrix actualIJS = fg.sparseJacobian_();
  EQUALITY(expectedIJS, actualIJS);
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, sparseJacobian) {
  // Create factor graph:
  // x1 x2 x3 x4 x5  b
  //  1  2  3  0  0  4
  //  5  6  7  0  0  8
  //  9 10  0 11 12 13
  //  0  0  0 14 15 16

  // Expected
  Matrix expected = (Matrix(16, 3) <<
      1., 1., 2.,
      1., 2., 4.,
      1., 3., 6.,
      2., 1.,10.,
      2., 2.,12.,
      2., 3.,14.,
      1., 6., 8.,
      2., 6.,16.,
      3., 1.,18.,
      3., 2.,20.,
      3., 4.,22.,
      3., 5.,24.,
      4., 4.,28.,
      4., 5.,30.,
      3., 6.,26.,
      4., 6.,32.).finished();

  // expected: in matlab format - NOTE the transpose!)
  Matrix expectedMatlab = expected.transpose();

  GaussianFactorGraph gfg;
  SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
  const Key x123 = 0, x45 = 1;
  gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(),
          Vector2(4, 8), model);
  gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(),
          x45,  (Matrix(2, 2) << 11, 12, 14, 15.).finished(),
          Vector2(13, 16), model);

  Matrix actual = gfg.sparseJacobian_();

  EXPECT(assert_equal(expectedMatlab, actual));

  // SparseTriplets
  auto boostActual = gfg.sparseJacobian();
  // check the triplets size...
  EXPECT_LONGS_EQUAL(16, boostActual.size());
  // check content
  for (int i = 0; i < 16; i++) {
    EXPECT(triplet_equal(
        SparseTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)),
        boostActual.at(i)));
  }
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, matrices) {
  // Create factor graph:
  // x1 x2 x3 x4 x5  b
  //  1  2  3  0  0  4
  //  5  6  7  0  0  8
  //  9 10  0 11 12 13
  //  0  0  0 14 15 16

  Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished();
  Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished();
  Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15).finished();

  GaussianFactorGraph gfg;
  SharedDiagonal model = noiseModel::Unit::Create(2);
  gfg.add(0, A00, Vector2(4., 8.), model);
  gfg.add(0, A10, 1, A11, Vector2(13., 16.), model);

  Matrix Ab(4, 6);
  Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16;

  // augmented versions
  EXPECT(assert_equal(Ab, gfg.augmentedJacobian()));
  EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian()));

  // jacobian
  Matrix A = Ab.leftCols(Ab.cols() - 1);
  Vector b = Ab.col(Ab.cols() - 1);
  Matrix actualA;
  Vector actualb;
  boost::tie(actualA, actualb) = gfg.jacobian();
  EXPECT(assert_equal(A, actualA));
  EXPECT(assert_equal(b, actualb));

  // hessian
  Matrix L = A.transpose() * A;
  Vector eta = A.transpose() * b;
  Matrix actualL;
  Vector actualeta;
  boost::tie(actualL, actualeta) = gfg.hessian();
  EXPECT(assert_equal(L, actualL));
  EXPECT(assert_equal(eta, actualeta));

  // hessianBlockDiagonal
  VectorValues expectLdiagonal;  // Make explicit that diagonal is sum-squares of columns
  expectLdiagonal.insert(0, Vector3(1 + 25 + 81, 4 + 36 + 100, 9 + 49));
  expectLdiagonal.insert(1, Vector2(121 + 196, 144 + 225));
  EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));

  // hessianBlockDiagonal
  map<Key, Matrix> actualBD = gfg.hessianBlockDiagonal();
  LONGS_EQUAL(2, actualBD.size());
  EXPECT(assert_equal(A00.transpose() * A00 + A10.transpose() * A10, actualBD[0]));
  EXPECT(assert_equal(A11.transpose() * A11, actualBD[1]));
}

/* ************************************************************************* */
/// Factor graph with 2 2D factors on 3 2D variables
static GaussianFactorGraph createSimpleGaussianFactorGraph() {
  GaussianFactorGraph fg;
  Key x1 = 2, x2 = 0, l1 = 1;
  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
  // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
  fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
  // odometry between x1 and x2: x2-x1=[0.2;-0.1]
  fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2);
  // measurement between x1 and l1: l1-x1=[0.0;0.2]
  fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
  // measurement between x2 and l1: l1-x2=[-0.2;0.3]
  fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
  return fg;
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, gradient) {
  GaussianFactorGraph fg = createSimpleGaussianFactorGraph();

  // Construct expected gradient
  // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 +
  // 25*(l1-x2-[-0.2;0.3])^2
  // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
  VectorValues expected = map_list_of<Key, Vector>(1, Vector2(5.0, -12.5))(2, Vector2(30.0, 5.0))(
      0, Vector2(-25.0, 17.5));

  // Check the gradient at delta=0
  VectorValues zero = VectorValues::Zero(expected);
  VectorValues actual = fg.gradient(zero);
  EXPECT(assert_equal(expected, actual));
  EXPECT(assert_equal(expected, fg.gradientAtZero()));

  // Check the gradient at the solution (should be zero)
  VectorValues solution = fg.optimize();
  VectorValues actual2 = fg.gradient(solution);
  EXPECT(assert_equal(VectorValues::Zero(solution), actual2));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, transposeMultiplication) {
  GaussianFactorGraph A = createSimpleGaussianFactorGraph();

  Errors e;
  e += Vector2(0.0, 0.0), Vector2(15.0, 0.0), Vector2(0.0, -5.0), Vector2(-7.5, -5.0);

  VectorValues expected;
  expected.insert(1, Vector2(-37.5, -50.0));
  expected.insert(2, Vector2(-150.0, 25.0));
  expected.insert(0, Vector2(187.5, 25.0));

  VectorValues actual = A.transposeMultiply(e);
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminate_empty) {
  // eliminate an empty factor
  GaussianFactorGraph gfg;
  gfg.add(JacobianFactor());
  GaussianBayesNet::shared_ptr actualBN;
  GaussianFactorGraph::shared_ptr remainingGFG;
  boost::tie(actualBN, remainingGFG) = gfg.eliminatePartialSequential(Ordering());

  // expected Bayes net is empty
  GaussianBayesNet expectedBN;

  // expected remaining graph should be the same as the original, still containing the empty factor
  GaussianFactorGraph expectedLF = gfg;

  // check if the result matches
  EXPECT(assert_equal(*actualBN, expectedBN));
  EXPECT(assert_equal(*remainingGFG, expectedLF));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, matrices2) {
  GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
  Matrix A;
  Vector b;
  boost::tie(A, b) = gfg.jacobian();
  Matrix AtA;
  Vector eta;
  boost::tie(AtA, eta) = gfg.hessian();
  EXPECT(assert_equal(A.transpose() * A, AtA));
  EXPECT(assert_equal(A.transpose() * b, eta));
  Matrix expectedAtA(6, 6);
  expectedAtA << 125, 0, -25, 0, -100, 0,  //
      0, 125, 0, -25, 0, -100,             //
      -25, 0, 50, 0, -25, 0,               //
      0, -25, 0, 50, 0, -25,               //
      -100, 0, -25, 0, 225, 0,             //
      0, -100, 0, -25, 0, 225;
  EXPECT(assert_equal(expectedAtA, AtA));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, multiplyHessianAdd) {
  GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();

  VectorValues x = map_list_of<Key, Vector>(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6));

  VectorValues expected;
  expected.insert(0, Vector2(-450, -450));
  expected.insert(1, Vector2(0, 0));
  expected.insert(2, Vector2(950, 1050));

  VectorValues actual;
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(expected, actual));

  // now, do it with non-zero y
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(2 * expected, actual));
}

/* ************************************************************************* */
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
  GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
  gfg += HessianFactor(1, 2, 100*I_2x2, Z_2x2,   Vector2(0.0, 1.0),
                                           400*I_2x2, Vector2(1.0, 1.0), 3.0);
  return gfg;
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, multiplyHessianAdd2) {
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();

  // brute force
  Matrix AtA;
  Vector eta;
  boost::tie(AtA, eta) = gfg.hessian();
  Vector X(6);
  X << 1, 2, 3, 4, 5, 6;
  Vector Y(6);
  Y << -450, -450, 300, 400, 2950, 3450;
  EXPECT(assert_equal(Y, AtA * X));

  VectorValues x = map_list_of<Key, Vector>(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6));

  VectorValues expected;
  expected.insert(0, Vector2(-450, -450));
  expected.insert(1, Vector2(300, 400));
  expected.insert(2, Vector2(2950, 3450));

  VectorValues actual;
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(expected, actual));

  // now, do it with non-zero y
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(2 * expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, matricesMixed) {
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
  Matrix A;
  Vector b;
  boost::tie(A, b) = gfg.jacobian();  // incorrect !
  Matrix AtA;
  Vector eta;
  boost::tie(AtA, eta) = gfg.hessian();  // correct
  EXPECT(assert_equal(A.transpose() * A, AtA));
  Vector expected = -(Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished();
  EXPECT(assert_equal(expected, eta));
  EXPECT(assert_equal(A.transpose() * b, eta));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, gradientAtZero) {
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
  VectorValues expected;
  VectorValues actual = gfg.gradientAtZero();
  expected.insert(0, Vector2(-25, 17.5));
  expected.insert(1, Vector2(5, -13.5));
  expected.insert(2, Vector2(29, 4));
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, clone) {
  // 2 variables, frontal has dim=4
  VerticalBlockMatrix blockMatrix(list_of(4)(2)(1), 4);
  blockMatrix.matrix() << 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0,
      0.0, 3.0, 0.0, 4.0, 0.0, 0.3, 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4;
  GaussianConditional cg(list_of(1)(2), 1, blockMatrix);

  GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor();
  init_graph.push_back(GaussianFactor::shared_ptr());  /// Add null factor
  init_graph.push_back(GaussianConditional(cg));

  GaussianFactorGraph exp_graph =
      createGaussianFactorGraphWithHessianFactor();   // Created separately
  exp_graph.push_back(GaussianFactor::shared_ptr());  /// Add null factor
  exp_graph.push_back(GaussianConditional(cg));

  GaussianFactorGraph actCloned = init_graph.clone();
  EXPECT(assert_equal(init_graph, actCloned));  // Same as the original version

  // Apply an in-place change to init_graph and compare
  JacobianFactor::shared_ptr jacFactor0 =
      boost::dynamic_pointer_cast<JacobianFactor>(init_graph.at(0));
  CHECK(jacFactor0);
  jacFactor0->getA(jacFactor0->begin()) *= 7.;
  EXPECT(assert_inequal(init_graph, exp_graph));
  EXPECT(assert_equal(exp_graph, actCloned));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, negate) {
  GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor();
  init_graph.push_back(GaussianFactor::shared_ptr());  /// Add null factor
  GaussianFactorGraph actNegation = init_graph.negate();
  GaussianFactorGraph expNegation;
  expNegation.push_back(init_graph.at(0)->negate());
  expNegation.push_back(init_graph.at(1)->negate());
  expNegation.push_back(init_graph.at(2)->negate());
  expNegation.push_back(init_graph.at(3)->negate());
  expNegation.push_back(init_graph.at(4)->negate());
  expNegation.push_back(GaussianFactor::shared_ptr());
  EXPECT(assert_equal(expNegation, actNegation));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, hessianDiagonal) {
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
  VectorValues expected;
  Matrix infoMatrix = gfg.hessian().first;
  Vector d = infoMatrix.diagonal();

  VectorValues actual = gfg.hessianDiagonal();
  expected.insert(0, d.segment<2>(0));
  expected.insert(1, d.segment<2>(2));
  expected.insert(2, d.segment<2>(4));
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, DenseSolve) {
  GaussianFactorGraph fg = createSimpleGaussianFactorGraph();
  VectorValues expected = fg.optimize();
  VectorValues actual = fg.optimizeDensely();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, ProbPrime) {
  GaussianFactorGraph gfg;
  gfg.emplace_shared<JacobianFactor>(1, I_1x1, Z_1x1,
                                     noiseModel::Isotropic::Sigma(1, 1.0));

  VectorValues values;
  values.insert(1, I_1x1);

  // We are testing the normal distribution PDF where info matrix Σ = 1,
  // mean mu = 0  and x = 1.
  // Therefore factor squared error: y = 0.5 * (Σ*x - mu)^2 =
  // 0.5 * (1.0 - 0)^2 = 0.5
  // NOTE the 0.5 constant is a part of the factor error.
  EXPECT_DOUBLES_EQUAL(0.5, gfg.error(values), 1e-12);

  // The gaussian PDF value is: exp^(-0.5 * (Σ*x - mu)^2) / sqrt(2 * PI)
  // Ignore the denominator and we get: exp^(-0.5 * (1.0)^2) = exp^(-0.5)
  double expected = exp(-0.5);
  EXPECT_DOUBLES_EQUAL(expected, gfg.probPrime(values), 1e-12);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// Copyright 2005 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_PYTHON_GENERATORS_HPP
#define BOOST_GRAPH_PYTHON_GENERATORS_HPP

#include <memory>
#include <boost/python.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/python/graph.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/plod_generator.hpp>
#include <boost/graph/small_world_generator.hpp>

namespace boost { namespace graph { namespace python {

template<typename Graph>
boost::python::object
erdos_renyi_graph(typename graph_traits<Graph>::vertices_size_type n,
                  double prob, bool allow_self_loops = false, int seed = 1)
{
  typedef erdos_renyi_iterator<minstd_rand, Graph> iterator;
  minstd_rand gen(seed);
  return Graph::pyconstruct(iterator(gen, n, prob, allow_self_loops),
                            iterator(), n);
}

template<typename Graph>
boost::python::object
plod_graph(typename graph_traits<Graph>::vertices_size_type n,
           double alpha, double beta, bool allow_self_loops = false,
           int seed = 1)
{
  typedef plod_iterator<minstd_rand, Graph> iterator;
  minstd_rand gen(seed);
  return Graph::pyconstruct(iterator(gen, n, alpha, beta, allow_self_loops),
                            iterator(), n);
}

template<typename Graph>
boost::python::object
small_world_graph(typename graph_traits<Graph>::vertices_size_type n,
                  typename graph_traits<Graph>::vertices_size_type k,
                  double prob, bool allow_self_loops = false,
		  bool allow_multiple_edges = true, int seed = 1)
{
  typedef small_world_iterator<minstd_rand, Graph> iterator;
  minstd_rand gen(seed);
  return Graph::pyconstruct(iterator(gen, n, k, prob,
				     allow_self_loops, allow_multiple_edges),
                            iterator(), n);
}

} } } // end namespace boost::graph::python

#endif // BOOST_GRAPH_PYTHON_GENERATORS_HPP


//=======================================================================
// Copyright 1997-2001 University of Notre Dame.
// Authors: Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <iostream>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/transpose_graph.hpp>

/* Output:

  distances from start vertex:
  distance(a) = 0
  distance(b) = 3
  distance(c) = 1
  distance(d) = 3
  distance(e) = 3

  min-max paths tree
  a --> c
  b -->
  c --> d
  d --> e
  e --> b

*/

int
main(int , char* [])
{
  using namespace boost;

  typedef adjacency_list<listS, vecS, directedS,
    no_property, property<edge_weight_t, int> > Graph;
  typedef graph_traits<Graph>::vertex_descriptor Vertex;

  typedef std::pair<int,int> E;

  const char name[] = "abcdef";

  const int num_nodes = 6;
  E edges[] = { E(0,2), E(1,1), E(1,3), E(1,4), E(2,1), E(2,3),
                E(3,4), E(4,0), E(4,1) };
  int weights[] = { 1, 2, 1, 2, 7, 3, 1, 1, 1};
  const int n_edges = sizeof(edges)/sizeof(E);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ can't handle iterator constructors
  Graph G(num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, G);
  for (std::size_t j = 0; j < sizeof(edges) / sizeof(E); ++j) {
    graph_traits<Graph>::edge_descriptor e; bool inserted;
    tie(e, inserted) = add_edge(edges[j].first, edges[j].second, G);
    weightmap[e] = weights[j];
  }
#else
  Graph G(edges, edges + n_edges, weights, num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, G);
#endif

  std::vector<Vertex> p(num_vertices(G));
  std::vector<int> d(num_vertices(G));

  Vertex s = *(vertices(G).first);

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  dijkstra_shortest_paths
    (G, s, &p[0], &d[0], weightmap, get(vertex_index, G),
     std::greater<int>(), closed_plus<int>(), std::numeric_limits<int>::max(), 0,
     default_dijkstra_visitor());
#else
  dijkstra_shortest_paths
    (G, s, distance_map(&d[0]).
     predecessor_map(&p[0]).
     distance_compare(std::greater<int>()));
#endif

  std::cout << "distances from start vertex:" << std::endl;
  graph_traits<Graph>::vertex_iterator vi, vend;
  for(tie(vi,vend) = vertices(G); vi != vend; ++vi)
    std::cout << "distance(" << name[*vi] << ") = " << d[*vi] << std::endl;
  std::cout << std::endl;

  std::cout << "min-max paths tree" << std::endl;
  adjacency_list<> tree(num_nodes);

  for(tie(vi,vend) = vertices(G); vi != vend; ++vi)
    if (*vi != p[*vi])
      add_edge(p[*vi], *vi, tree);

  print_graph(tree, name);

  return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/
#pragma once

#include "algorithms/GraphPlayUtils.hpp"
#include "algorithms/util/RTPGHI.hpp"
#include "algorithms/public/STFT.hpp"
#include "algorithms/util/AlgorithmUtils.hpp"
#include "algorithms/util/FluidEigenMappings.hpp"
#include "data/FluidDataSet.hpp"
#include "data/TensorTypes.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <fstream>
#include <random>
#include <vector>

namespace fluid {
namespace algorithm {

class GraphGrain {

public:
  using MatrixXd = Eigen::MatrixXd;
  using VectorXd = Eigen::VectorXd;
  using DataSet = FluidDataSet<std::string, double, 1>;

  void init(RealVectorView audio, index sampleRate, index windowSize,
            index fftSize, index hopSize, index numBands, index distance,
            double threshold, index nClusters, RealVectorView output) {
    using namespace Eigen;
    using namespace _impl;
    using namespace std;
    mWindowSize = windowSize;
    mFFTSize = fftSize;
    mHopSize = hopSize;
    mFrameSize = (mFFTSize / 2) + 1;
    mThreshold = threshold;
    STFT stft = STFT(mWindowSize, mFFTSize, mHopSize);
    mLength = std::floor((audio.size() + mHopSize) / mHopSize);
    mSpectrogram = ComplexMatrix(mLength, mFrameSize);
    stft.process(audio, mSpectrogram);
    mMagnitude = RealMatrix(mLength, mFrameSize);
    stft.magnitude(mSpectrogram, mMagnitude);
    mDM = mUtils.computeDM(mMagnitude, numBands, sampleRate, windowSize,
                           fftSize, distance);
    mDM.diagonal().setZero();
    mForbidden = ArrayXXd::Ones(mDM.rows(), mDM.cols());
    mVisited = ArrayXXd::Zero(mDM.rows(), mDM.cols());
    ArrayXd odf = mDM.diagonal(1).array();
    mClusters = FluidTensor<index, 1>(mLength);
    mUtils.onsetDetection(odf, mForbidden);
    if (nClusters != 1) {
      mClusters = mUtils.spectralClustering(mDM, nClusters);
      for (index i = 0; i < mLength; i++) {
        for (index j = 0; j < mLength; j++) {
          if (mClusters(i) != mClusters(j)) {
            mForbidden(i, j) = 0;
            mForbidden(j, i) = 0;
          }
        }
      }
    }
    mRP = (mDM.array() < threshold).cast<double>();
    mRP = mRP.array() * mForbidden.array();
    mRTPGHI.init(fftSize);
    mPrevMag = RealVector(mFrameSize);
    mPrevMag = mMagnitude.row(0);
    mInitialized = true;
  }

  index selectProb() {
    index nNeighbors = mRP.col(mPos).sum();
    if (nNeighbors == 0)
      return nextInCluster(mPos);
    std::vector<index> candidates(nNeighbors);
    std::vector<double> acumProbs(nNeighbors);
    index nCandidates = 0;
    double prob = 0;
    for (index i = 0; i < mRP.rows(); i++) {
      if (mRP(mPos, i) > 0 && mVisited(mPos, i) <= 0) {
        prob = prob + (1 - mDM(mPos, i));
        acumProbs[nCandidates] = prob;
        candidates[nCandidates++] = i;
      }
    }
    double rnd = prob * mUtils.rand();
    index selected = 0;
    while (acumProbs[selected] < rnd)
      selected++;
    return candidates[selected];
  }

  index nextInCluster(index current) {
    for (index i = current + 1; i < mLength + current; i++) {
      index pos = i % mLength;
      if (mClusters(pos) == mClusters(current))
        return pos;
    }
  }

  index selectRand(double randomness) {
    index nNeighbors = mRP.col(mPos).sum();
    if (nNeighbors == 0) {
      mVisited.row(mPos).setZero();
      return nextInCluster(mPos);
    }
    std::vector<index> candidates(nNeighbors);
    index nCandidates = 0;
    for (index i = 0; i < mRP.rows(); i++)
      if (i != mPos && mRP(mPos, i) > 0 && mVisited(mPos, i) <= 0) {
        candidates[nCandidates++] = i;
      }
    std::sort(candidates.begin(), candidates.end(),
              [this](index a, index b) { return mDM(mPos, a) < mDM(mPos, b); });
    if (randomness == 0)
      return candidates[0];
    index k = lrint(randomness * nCandidates);
    index next = mUtils.randInt(k);
    return candidates[next];
  }

  index selectNearest() {
    index nNeighbors = mRP.col(mPos).sum();
    if (nNeighbors == 0) {
      return nextInCluster(mPos);
    }
    double minDist = infinity;
    index selected = 0;
    for (index i = 0; i < mRP.rows(); i++) {
      if ((i != mPos) && (mRP(mPos, i) > 0) && (mDM(mPos, i) < minDist) &&
          (mVisited(mPos, i) == 0)) {
        selected = i;
        minDist = mDM(mPos, i);
      }
    }
    if (selected == 0)
      return nextInCluster(mPos);
    return selected;
  }

  void processFrame(ComplexVectorView out, double start, double threshold,
                    index forget, double rand, index phaseGen,
                    RealVectorView output) {
    using namespace Eigen;
    using namespace _impl;

    if (mThreshold != threshold) {
      mRP = (mDM.array() < threshold).cast<double>();
      mRP = mRP.array() * mForbidden.array();
      mThreshold = threshold;
    }

    index next = (mPos + 1) % mSpectrogram.rows();
    mVisited = (mVisited.array() - 1).cwiseMax(0);
    index startFrame = lrint(start * (mSpectrogram.rows() - 1));
    if (startFrame != mStartFrame) {
      mStartFrame = startFrame;
      mPos = mStartFrame;
      index nNeighbors = mRP.col(mPos).sum();
      while (nNeighbors == 0) {
        nNeighbors = mRP.col(mPos).sum();
        mPos = (mPos + 1) % mSpectrogram.rows();
      }
      mCount = 0;
    } else {
      index prevPos = mPos;
      mPos = selectRand(rand);
      mVisited(prevPos, mPos) = forget;
    }
    RealVectorView frame = mMagnitude.row(mPos);
    if (phaseGen > 0)
      mRTPGHI.processFrame(frame, out, mWindowSize, mFFTSize, mHopSize, 1e-5);
    else
      out = mSpectrogram.row(mPos);
    output(0) = mPos;
    output(1) = mClusters(mPos);
  }

  bool initialized() { return mInitialized; }

  index mWindowSize;
  index mHopSize;
  index mFFTSize;

private:
  GraphPlayUtils mUtils;
  ComplexMatrix mSpectrogram;
  RealMatrix mMagnitude;
  RealVector mPrevMag;
  index mFrameSize;
  MatrixXd mDM;
  MatrixXd mRP;
  MatrixXd mForbidden;
  MatrixXd mVisited;
  VectorXd mDeg;
  bool mInitialized{false};
  int mPos{0};
  index mLength;
  index mStartFrame{-1};
  index mEndFrame;
  double mThreshold;
  index mCount{0};
  RTPGHI mRTPGHI;
  FluidTensor<index, 1> mClusters;
  double mPrevGain{0};
};
} // namespace algorithm
} // namespace fluid


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "tudat/basics/utilities.h"
#include "tudat/basics/testMacros.h"
#include "tudat/basics/basicTypedefs.h"
#include "tudat/io/basicInputOutput.h"
#include "tudat/io/matrixTextFileReader.h"

#include "tudat/math/statistics/basicStatistics.h"
#include "tudat/math/filters/extendedKalmanFilter.h"
#include "tudat/math/integrators/createNumericalIntegrator.h"

#include "tudat/math/filters/controlClass.h"

namespace tudat
{

namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_extended_kalman_filter )

// Functions for extended Kalman filter.
Eigen::Vector2d stateFunction1( const double time, const Eigen::Vector2d& state, const Eigen::Vector2d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Vector2d stateDerivative;
    stateDerivative[ 0 ] = state[ 1 ] * std::pow( std::cos( state[ 0 ] ), 3 );
    stateDerivative[ 1 ] = std::sin( state[ 0 ] );
    return stateDerivative;
}
Eigen::Vector1d measurementFunction1( const double time, const Eigen::Vector2d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    Eigen::Vector1d measurement;
    measurement[ 0 ] = std::pow( state[ 0 ], 3 );
    return measurement;
}
Eigen::Matrix2d stateJacobianFunction1( const double time, const Eigen::Vector2d& state, const Eigen::Vector2d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Matrix2d stateJacobian = Eigen::Matrix2d::Zero( );
    stateJacobian( 0, 0 ) = - 3.0 * state[ 1 ] * std::pow( std::cos( state[ 0 ] ), 2 ) * std::sin( state[ 0 ] );
    stateJacobian( 0, 1 ) = std::pow( std::cos( state[ 0 ] ), 3 );
    stateJacobian( 1, 0 ) = std::cos( state[ 0 ] );
    return stateJacobian;
}
Eigen::RowVector2d measurementJacobianFunction1( const double time, const Eigen::Vector2d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    Eigen::RowVector2d measurementJacobian;
    measurementJacobian[ 0 ] = 3.0 * std::pow( state[ 0 ], 2 );
    return measurementJacobian;
}

// Test implementation of extended Kalman filter class.
BOOST_AUTO_TEST_CASE( testExtendedKalmanFilterFirstCase )
{
    using namespace tudat::filters;

    // Set initial conditions
    const double initialTime = 0.0;
    const double timeStep = 0.01;
    const unsigned int numberOfTimeSteps = 1000;

    Eigen::Vector2d initialStateVector;
    initialStateVector[ 0 ] = 3.0;
    initialStateVector[ 1 ] = -0.3;

    Eigen::Vector2d initialEstimatedStateVector;
    initialEstimatedStateVector[ 0 ] = 10.0;
    initialEstimatedStateVector[ 1 ] = -3.0;

    Eigen::Matrix2d initialEstimatedStateCovarianceMatrix = Eigen::Matrix2d::Zero( );
    initialEstimatedStateCovarianceMatrix( 0, 0 ) = 100.0;
    initialEstimatedStateCovarianceMatrix( 1, 1 ) = 100.0;

    // Set system and measurement uncertainty
    Eigen::Matrix2d systemUncertainty = Eigen::Matrix2d::Zero( );
    Eigen::Vector1d measurementUncertainty = Eigen::Vector1d::Zero( );
    systemUncertainty( 0, 0 ) = 100.0;
    systemUncertainty( 1, 1 ) = 100.0;
    measurementUncertainty[ 0 ] = 100.0;

    // Set integrator settings
    std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< > > (
                numerical_integrators::euler, initialTime, timeStep );

    // Create control class
    std::shared_ptr< ControlWrapper< double, double, 2 > > control =
            std::make_shared< ControlWrapper< double, double, 2 > >(
                [ & ]( const double, const Eigen::Vector2d& ){ return Eigen::Vector2d::Zero( ); } );

    // Create extended Kalman filter object
    KalmanFilterDoublePointer extendedFilter = std::make_shared< ExtendedKalmanFilterDouble >(
                std::bind( &stateFunction1, std::placeholders::_1, std::placeholders::_2,
                             std::bind( &ControlWrapper< double, double, 2 >::getCurrentControlVector, control ) ),
                std::bind( &measurementFunction1, std::placeholders::_1, std::placeholders::_2 ),
                std::bind( &stateJacobianFunction1, std::placeholders::_1, std::placeholders::_2,
                             std::bind( &ControlWrapper< double, double, 2 >::getCurrentControlVector, control ) ),
                [ & ]( const double, const Eigen::Vector2d& ){ return Eigen::Matrix2d::Identity( ); },
                std::bind( &measurementJacobianFunction1, std::placeholders::_1, std::placeholders::_2 ),
                [ & ]( const double, const Eigen::Vector2d& ){ return Eigen::Vector1d::Identity( ); },
                systemUncertainty, measurementUncertainty, timeStep,
                initialTime, initialEstimatedStateVector, initialEstimatedStateCovarianceMatrix,
                integratorSettings );

    // Load noise from file
    Eigen::MatrixXd systemNoise = input_output::readMatrixFromFile( tudat::paths::getTudatTestDataPath( ) +
                                                                    "/ekfSystemNoise1.dat" );
    Eigen::MatrixXd measurementNoise = input_output::readMatrixFromFile( tudat::paths::getTudatTestDataPath( ) +
                                                                         "/ekfMeasurementNoise1.dat" );

    // Loop over each time step
    const bool showProgress = false;
    double currentTime = initialTime;
    Eigen::Vector2d currentActualStateVector = initialStateVector;
    Eigen::Vector2d currentControlVector = Eigen::Vector2d::Zero( );
    Eigen::Vector1d currentMeasurementVector;
    std::map< double, Eigen::Vector2d > actualStateVectorHistory;
    std::map< double, Eigen::Vector1d > measurementVectorHistory;
    actualStateVectorHistory[ initialTime ] = initialStateVector;
    for ( unsigned int i = 0; i < numberOfTimeSteps; i++ )
    {
        // Compute actual values and perturb them
        currentActualStateVector += ( stateFunction1( currentTime, currentActualStateVector, currentControlVector ) +
                                      systemNoise.col( i ) ) * timeStep;
        currentMeasurementVector = measurementFunction1( currentTime, currentActualStateVector ) + measurementNoise.col( i );

        // Update control class
        control->setCurrentControlVector( currentTime, extendedFilter->getCurrentStateEstimate( ) );

        // Update filter
        extendedFilter->updateFilter( currentMeasurementVector );
        currentTime = extendedFilter->getCurrentTime( );

        // Store values
        actualStateVectorHistory[ currentTime ] = currentActualStateVector;
        measurementVectorHistory[ currentTime ] = currentMeasurementVector;

        // Print progress
        if ( showProgress )
        {
            std::cout << "Time: " << currentTime << std::endl
                      << "Measurement: " << currentMeasurementVector.transpose( ) << std::endl
                      << "Estimated State: " << extendedFilter->getCurrentStateEstimate( ).transpose( ) << std::endl << std::endl;
        }
    }

    // Check that final state is as expected
    Eigen::Vector2d expectedFinalState;
    expectedFinalState << 4.972005968275564, -18.516511373319734;
    for ( int i = 0; i < expectedFinalState.rows( ); i++ )
    {
        BOOST_CHECK_SMALL( extendedFilter->getCurrentStateEstimate( )[ i ] - expectedFinalState[ i ], 1.0e-10 );
    }
}

// Constant parameters for example
const double gravitationalParameter = 32.2;

// Functions for extended Kalman filter.
Eigen::Vector3d stateFunction2( const double time, const Eigen::Vector3d& state, const Eigen::Vector3d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Vector3d stateDerivative = Eigen::Vector3d::Zero( );
    stateDerivative[ 0 ] = state[ 1 ];
    stateDerivative[ 1 ] = 0.0034 * gravitationalParameter * std::exp( - state[ 0 ] / 22000.0 ) *
            std::pow( state[ 1 ], 2 ) / ( 2.0 * state[ 2 ] ) - gravitationalParameter;
    return stateDerivative;
}
Eigen::Vector1d measurementFunction2( const double time, const Eigen::Vector3d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    Eigen::Vector1d measurement;
    measurement[ 0 ] = state[ 0 ];
    return measurement;
}
Eigen::Matrix3d stateJacobianFunction2( const double time, const Eigen::Vector3d& state, const Eigen::Vector3d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Matrix3d stateJacobian = Eigen::Matrix3d::Zero( );
    stateJacobian( 0, 1 ) = 1.0;
    stateJacobian( 1, 0 ) = - 0.0034 * gravitationalParameter * std::exp( - state[ 0 ] / 22000.0 ) *
            std::pow( state[ 1 ], 2 ) / ( 44000.0 * state[ 2 ] );
    stateJacobian( 1, 1 ) = 0.0034 * gravitationalParameter * std::exp( - state[ 0 ] / 22000.0 ) *
            state[ 1 ] / state[ 2 ];
    stateJacobian( 1, 2 ) = - 0.0034 * gravitationalParameter * std::exp( - state[ 0 ] / 22000.0 ) *
            std::pow( state[ 1 ], 2 ) / ( 2.0 * std::pow( state[ 2 ], 2 ) );
    return stateJacobian;
}
Eigen::RowVector3d measurementJacobianFunction2( const double time, const Eigen::Vector3d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( state );
    Eigen::RowVector3d measurementJacobian = Eigen::RowVector3d::Zero( );
    measurementJacobian[ 0 ] = 1.0;
    return measurementJacobian;
}

// Test implementation of extended Kalman filter class.
BOOST_AUTO_TEST_CASE( testExtendedKalmanFilterSecondCase )
{
    using namespace tudat::filters;

    // Set initial conditions
    const double initialTime = 0.0;
    const double timeStep = 0.1;
    const unsigned int numberOfTimeSteps = 300;

    Eigen::Vector3d initialStateVector;
    initialStateVector[ 0 ] = 200000.0;
    initialStateVector[ 1 ] = -6000.0;
    initialStateVector[ 2 ] = 500.0;

    Eigen::Vector3d initialEstimatedStateVector;
    initialEstimatedStateVector[ 0 ] = 200025.0;
    initialEstimatedStateVector[ 1 ] = -6150.0;
    initialEstimatedStateVector[ 2 ] = 800.0;

    Eigen::Matrix3d initialEstimatedStateCovarianceMatrix = Eigen::Matrix3d::Zero( );
    initialEstimatedStateCovarianceMatrix( 0, 0 ) = std::pow( 1000.0, 2 );
    initialEstimatedStateCovarianceMatrix( 1, 1 ) = 20000.0;
    initialEstimatedStateCovarianceMatrix( 2, 2 ) = std::pow( 300.0, 2 );

    // Set system and measurement uncertainty
    Eigen::Matrix3d systemUncertainty = Eigen::Matrix3d::Zero( );
    Eigen::Vector1d measurementUncertainty = Eigen::Vector1d::Zero( );
    systemUncertainty( 0, 0 ) = std::pow( 100.0, 2 );
    systemUncertainty( 1, 1 ) = std::pow( 10.0, 2 );
    systemUncertainty( 2, 2 ) = std::pow( 1.0, 2 );
    measurementUncertainty[ 0 ] = std::pow( 25.0, 2 );

    // Set integrator settings
    std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< > > (
                numerical_integrators::euler, initialTime, timeStep );

    // Create control class
    std::shared_ptr< ControlWrapper< double, double, 3 > > control =
            std::make_shared< ControlWrapper< double, double, 3 > >(
                [ & ]( const double, const Eigen::Vector3d& ){ return Eigen::Vector3d::Zero( ); } );

    // Create extended Kalman filter object
    KalmanFilterDoublePointer extendedFilter = std::make_shared< ExtendedKalmanFilterDouble >(
                std::bind( &stateFunction2, std::placeholders::_1, std::placeholders::_2,
                             std::bind( &ControlWrapper< double, double, 3 >::getCurrentControlVector, control ) ),
                std::bind( &measurementFunction2, std::placeholders::_1, std::placeholders::_2 ),
                std::bind( &stateJacobianFunction2, std::placeholders::_1, std::placeholders::_2,
                             std::bind( &ControlWrapper< double, double, 3 >::getCurrentControlVector, control ) ),
                [ & ]( const double, const Eigen::Vector3d& ){ return Eigen::Matrix3d::Identity( ); },
                std::bind( &measurementJacobianFunction2, std::placeholders::_1, std::placeholders::_2 ),
                [ & ]( const double, const Eigen::Vector3d& ){ return Eigen::Vector1d::Identity( ); },
                systemUncertainty, measurementUncertainty, timeStep,
                initialTime, initialEstimatedStateVector, initialEstimatedStateCovarianceMatrix,
                integratorSettings );

    // Load noise from file
    Eigen::MatrixXd systemNoise = input_output::readMatrixFromFile( tudat::paths::getTudatTestDataPath( ) +
                                                                    "/ekfSystemNoise2.dat" );
    Eigen::MatrixXd measurementNoise = input_output::readMatrixFromFile( tudat::paths::getTudatTestDataPath( ) +
                                                                         "/ekfMeasurementNoise2.dat" );

    // Loop over each time step
    const bool showProgress = false;
    double currentTime = initialTime;
    Eigen::Vector3d currentActualStateVector = initialStateVector;
    Eigen::Vector3d currentControlVector = Eigen::Vector3d::Zero( );
    Eigen::Vector1d currentMeasurementVector;
    std::map< double, Eigen::Vector3d > actualStateVectorHistory;
    std::map< double, Eigen::Vector1d > measurementVectorHistory;
    actualStateVectorHistory[ initialTime ] = initialStateVector;
    for ( unsigned int i = 0; i < numberOfTimeSteps; i++ )
    {
        // Compute actual values and perturb them
        currentActualStateVector += ( stateFunction2( currentTime, currentActualStateVector, currentControlVector ) +
                                      systemNoise.col( i ) ) * timeStep;
        currentMeasurementVector = measurementFunction2( currentTime, currentActualStateVector ) + measurementNoise.col( i );

        // Update control class
        control->setCurrentControlVector( currentTime, extendedFilter->getCurrentStateEstimate( ) );

        // Update filter
        extendedFilter->updateFilter( currentMeasurementVector );
        currentTime = extendedFilter->getCurrentTime( );

        // Store values
        actualStateVectorHistory[ currentTime ] = currentActualStateVector;
        measurementVectorHistory[ currentTime ] = currentMeasurementVector;

        // Print progress
        if ( showProgress )
        {
            std::cout << "Time: " << currentTime << std::endl
                      << "Measurement: " << currentMeasurementVector.transpose( ) << std::endl
                      << "Estimated State: " << extendedFilter->getCurrentStateEstimate( ).transpose( ) << std::endl << std::endl;
        }
    }

    // Check that final state is as expected
    Eigen::Vector3d expectedFinalState = Eigen::Vector3d::Zero( );
    expectedFinalState << 25202.174591778028, -3327.344292984541, 498.97471055815663;
    for ( int i = 0; i < expectedFinalState.rows( ); i++ )
    {
        BOOST_CHECK_SMALL( extendedFilter->getCurrentStateEstimate( )[ i ] - expectedFinalState[ i ], 1.0e-10 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


//==================================================================================================
/*!
  @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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ACSCD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACSCD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse cosecant in degree:
    \f$(180/\pi) \arcsin(1/x)\f$.

    @par Header <boost/simd/function/acscd.hpp>

    @see acsc, accscpi

    @par Example:

      @snippet acscd.cpp acscd

    @par Possible output:

      @snippet acscd.txt acscd

  **/
  IEEEValue acscd(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/acscd.hpp>
#include <boost/simd/function/simd/acscd.hpp>

#endif


#include "SMP/HElib.hpp"
#include <HElib/FHEContext.h>
#include <HElib/EncryptedArray.h>
#include <HElib/PAlgebra.h>
#include <NTL/ZZX.h>
#include <NTL/lzz_pX.h>

#include <algorithm>
void rawEncode(NTL::zz_pX &out,
               std::vector<NTL::zz_pX> const& slots,
               FHEcontext const& context)
{
    const auto &encoder = context.alMod.getDerived(PA_zz_p());
    encoder.CRT_reconstruct(out, const_cast<std::vector<NTL::zz_pX> &>(slots));
}

void rawEncode(NTL::ZZX &out,
               std::vector<NTL::zz_pX> const& slots,
               FHEcontext const& context)
{
    NTL::zz_pX tmp;
    rawEncode(tmp, slots, context);
    NTL::conv(out, tmp);
}

void rawDecode(std::vector<NTL::zz_pX> &out,
               NTL::zz_pX const& poly,
               FHEcontext const& context)
{
    const auto &decoder = context.alMod.getDerived(PA_zz_p());
    decoder.CRT_decompose(out, poly);
}

void rawDecode(std::vector<NTL::zz_pX> &out,
               NTL::ZZX const& poly,
               FHEcontext const& context)
{
    NTL::zz_pX tmp;
    NTL::conv(tmp, poly);
    rawDecode(out, tmp, context);
}

void rawDecode(std::vector<NTL::ZZX> &out,
               NTL::ZZX const& poly,
               FHEcontext const& context)
{
    NTL::zz_pX tmp;
    std::vector<NTL::zz_pX> tmp2;
    rawDecode(tmp2, tmp, context);
    out.resize(tmp2.size());
    for (size_t i = 0; i < tmp2.size(); i++)
        NTL::conv(out[i], tmp2[i]);
}

static GMMPrecompTable precompute_gmm_table(long beta, long p, long slots)
{
    GMMPrecompTable tbl;
    tbl.beta_powers.resize(slots);
    /// (-beta)^k mod p for 0 <= k < l
    for (long i = 0; i < slots; i++)
        tbl.beta_powers[i] = NTL::PowerMod(i & 1 ? p - beta : beta, i, p);
    return tbl;
}

static bool is_valid_for_GMM(NTL::ZZX const& factor)
{
    for (long d = 1; d < NTL::deg(factor); d++) {
        if (NTL::coeff(factor, d) != 0)
            return false;
    }
    return true;
}

std::vector<GMMPrecompTable> precompute_gmm_tables(FHEcontext const& context)
{
    long p = context.alMod.getPPowR();
    long l = context.ea->size();
    std::vector<GMMPrecompTable> tbls;
    tbls.reserve(l);
    NTL::mulmod_t inv_p = NTL::PrepMulMod(p);
    for (const auto & factor : context.alMod.getFactorsOverZZ()) {
        assert(is_valid_for_GMM(factor));
        long beta = NTL::to_long(factor[0]);
        tbls.push_back(precompute_gmm_table(beta, p, l)) ;
        tbls.back().inv_p = inv_p;
    }
    return tbls;
}

long extract_inner_product(NTL::ZZX const& poly,
                           GMMPrecompTable const& tbl,
                           FHEcontext const& context)
{
    long d = context.ea->getDegree();
    long l = context.ea->size();
    long p = context.alMod.getPPowR();
    long phim = context.zMStar.getPhiM();
    long ret = 0;
    for (long i = 0; i < l; i++) {
        long coeff_loc = (i + 1) * d - 1;
        assert(coeff_loc < phim);
        long coeff = NTL::to_long(NTL::coeff(poly, coeff_loc));
        coeff = NTL::MulMod(coeff, tbl.beta_powers.at(i), p, tbl.inv_p);
        ret = NTL::AddMod(ret, coeff, p);
    }
    return ret;
}

long extract_inner_product(NTL::Vec<long> const& poly,
                           GMMPrecompTable const& tbl,
                           FHEcontext const& context)
{
    long d = context.ea->getDegree();
    long l = context.ea->size();
    long p = context.alMod.getPPowR();
    long phim = context.zMStar.getPhiM();
    assert(poly.length() == phim);
    long ret = 0;
    for (long i = 0; i < l; i++) {
        long coeff_loc = (i + 1) * d - 1;
        assert(coeff_loc < phim);
        long coeff = poly.at(coeff_loc) % p;
        coeff = NTL::MulMod(coeff, tbl.beta_powers.at(i), p, tbl.inv_p);
        ret = NTL::AddMod(ret, coeff, p);
    }
    return ret;
}

void extract_inner_products(std::vector<long> &out,
                            NTL::ZZX const& poly,
                            std::vector<GMMPrecompTable> const& tables,
                            FHEcontext const& context)
{
    out.clear();
    out.reserve(context.ea->size());
    for (const auto &tbl : tables) {
        out.push_back(extract_inner_product(poly, tbl, context));
    }
}

void extract_inner_products(std::vector<long> &out,
                            NTL::Vec<long> const& poly,
                            std::vector<GMMPrecompTable> const& tables,
                            FHEcontext const& context)
{
    out.clear();
    out.reserve(context.ea->size());
    for (const auto &tbl : tables) {
        out.push_back(extract_inner_product(poly, tbl, context));
    }
}

void faster_decrypt(NTL::Vec<long> &out,
                    FHESecKey const& sk,
                    Ctxt const& ctx)
{
    const FHEcontext& context = sk.getContext();
    assert(ctx.getPrimeSet().card() == 1);
    DoubleCRT dcrt(context, ctx.getPrimeSet()); // Set to zero
    sk.Decrypt(dcrt, ctx);
    dcrt.getOneRow(out, 0);
}


#ifdef BMO_ENABLE_ARMA_WRAPPERS
    #ifndef ARMA_DONT_USE_WRAPPER
        #define ARMA_DONT_USE_WRAPPER
    #endif

    #include "armadillo"

    using Mat_t = arma::mat;
    using ColVec_t = arma::vec;
    using RowVec_t = arma::rowvec;
    using ColVecInt_t = arma::uvec;
#endif

//

#ifdef BMO_ENABLE_EIGEN_WRAPPERS
    #include <iostream>
    #include <random>
    #include <Eigen/Dense>

    template<typename eT, int iTr, int iTc>
    using EigenMat = Eigen::Matrix<eT,iTr,iTc>;

    using Mat_t = Eigen::MatrixXd;
    using ColVec_t = Eigen::VectorXd;
    using RowVec_t = Eigen::Matrix<double,1,Eigen::Dynamic>;
    using ColVecInt_t = Eigen::VectorXi;
#endif

#include "BaseMatrixOps.hpp"


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file scattered-interpolation.hpp
 * @author Jakub Cerveny <jakub.cerveny@ext.citationtech.net>
 * @author Matyas Hollmann <matyas.hollmann@melowntech.com>
 *
 * Interpolation on 2D scattered data.
 */

#ifndef imgproc_scattered_interpolation_hpp_included_
#define imgproc_scattered_interpolation_hpp_included_

#include <vector>
#include <Eigen/Sparse>
#include <opencv2/core/core.hpp>

#include "dbglog/dbglog.hpp"
#include "utility/gccversion.hpp"
#include "rastermask.hpp"

namespace imgproc {

#if !defined(IMGPROC_HAS_OPENCV) || !defined(IMGPROC_HAS_EIGEN3)
    UTILITY_FUNCTION_ERROR("Laplace interpolation is available only when "
                           "compiled with both OpenCV and Eigen3 libraries.")
#endif

/** Solves the boundary value problem -\Delta u = 0 on elements in the matrix
 *  'data' that correspond to unset elements in 'mask'. Elements corresponding
 *  to set positions in 'mask' are regarded as given data.
 *
 *  The method is described in section 3.8 "Laplace Interpolation" of
 *  Numerical Recipes in C, Third Edition.
 *
 *  typename T_OPT:  floating-point numeric type in which Eigen solves the problem,
 *                   float(faster), double(more precise)
 *  typename T_DATA: numeric type of (per-channel) elements of 'data' matrix,
 *                   if it's an integral type results are rounded before storing
 *  int nChan:       number of channels of 'data' matrix
 *
 *  Example usage: for 'data' matrix of type CV_32FC2 use <float, 2, T_OPT>
 *                 for 'data' matrix of type  CV_8UC3 use <unsigned char, 3, T_OPT>
 */
template<typename T_DATA, int nChan, typename T_OPT = float>
void laplaceInterpolate(cv::Mat &data, const imgproc::RasterMask &mask, double tol = 1e-12)
{
    static_assert(std::is_floating_point<T_OPT>::value,
                  "Floating-point numeric type expected.");

    assert(sizeof(T_DATA) == data.elemSize1() && data.channels() == nChan);

    // round results if 'data' matrix elements are of integral type
    constexpr bool doRound = std::is_integral<T_DATA>::value;

    using SparseMatrix = Eigen::SparseMatrix<T_OPT>;
    using Triplet = Eigen::Triplet<T_OPT>;
    // used to access individual entries in the 'data' matrix
    using cvVec = cv::Vec<T_DATA, nChan>;
    // used to access individual entris in the 'rhs' vector,
    // should be a vector of floating-point types
    using rhsVecT = cv::Vec<T_OPT, nChan>;

    int w = data.cols, h = data.rows;
    // # of variables
    int n = 0;

    auto linearizeID([w](int x, int y) -> int
    {
        return y * w + x;
    });

    auto isInside([w, h](const cv::Point2i& pt) -> bool
    {
        return std::min(pt.x, pt.y) >= 0 && pt.x < w && pt.y < h;
    });

    // optimize only free points, given points should not be a part of the
    // optimization
    std::vector<int> pixelIDs(w * h, -1);
    for (int y = 0; y < h; ++y)
    for (int x = 0; x < w; ++x)
    {
        if (!mask.get(x, y)) // free point
        {
            // assing ID and increase value of the counter
            pixelIDs[linearizeID(x, y)] = n++;
        }
    }

    if (!n) // nothing to do here
    {
        LOG(debug) << "All points are given, nothing to do.";
        return;
    }

    // assemble the linear system
    LOG(debug) << "Assembling " << n << "x" << n
               << " sparse system. # of channels: " << nChan;

    std::vector<Triplet> coefs;
    // default constructor = initialize with zeros
    std::vector<rhsVecT> rhsVec(n, rhsVecT());
    coefs.reserve(5 * n);

    const static std::array<cv::Point2i, 4> dirs = {{{1, 0}, {-1, 0},
                                                     {0, 1}, {0, -1}}};

    for (int y = 0; y < h; ++y)
    for (int x = 0; x < w; ++x)
    {
        // index of the unknown
        int k = pixelIDs[linearizeID(x, y)];

        // given point
        if (k < 0) { continue; }

        cv::Point2i cur{x, y};
        // # of neighbors
        int nNeighs = 0;
        for (const auto& dir : dirs)
        {
            cv::Point2i tmp = cur + dir;
            if (!isInside(tmp)) { continue; }
            ++nNeighs;

            // index of the neighboring unknown
            int t = pixelIDs[linearizeID(tmp.x, tmp.y)];
            if (t < 0) // neighbor is a given point
            {
                // convert 'data' to rhsVecT
                rhsVec[k] += rhsVecT(data.at<cvVec>(tmp));
            }
            else // neighbor is a free point
            {
                coefs.emplace_back(k, t, -1.0);
            }
        }
        coefs.emplace_back(k, k, nNeighs);
    }

    SparseMatrix mat(n, n);
    mat.setFromTriplets(coefs.begin(), coefs.end());

    LOG(debug) << "Matrix constructed.";

    using Precond = Eigen::DiagonalPreconditioner<T_OPT>;
    using Solver = Eigen::BiCGSTAB<SparseMatrix, Precond>;
    using EigVecX = Eigen::Matrix<T_OPT, Eigen::Dynamic, 1>;

    // solve the system
    Solver solver(mat);
    solver.setTolerance(tol);

    EigVecX rhs(n), sln(n);
    for (int i = 0; i < nChan; ++i)
    {
        for (int j = 0; j < n; ++j) {
            rhs(j) = rhsVec[j](i);
        }

        LOG(debug) << "Solving system with rhs = channel " << (i + 1)
                   << " out of " << nChan;

        sln = solver.solve(rhs);

        LOG(debug) << "#iterations: " << solver.iterations();
        LOG(debug) << "estimated error: " << solver.error();
        LOG(debug) << "min: " << sln.minCoeff() << ", max: " << sln.maxCoeff();

        // for integral types we should round the results
        // TODO: in C++17 use if constexpr
        if (doRound)
        {
            for (int j = 0; j < n; ++j) {
               sln(j) = std::round(sln(j));
            }
        }

        // store solution of the i-th channel
        for (int y = 0; y < h; ++y)
        for (int x = 0; x < w; ++x)
        {
            int id = pixelIDs[linearizeID(x, y)];
            if (id >= 0) // free point
            {
                data.at<cvVec>(y, x)(i) = sln(id);
            }
        }
    }
}

} // imgproc

#endif // imgproc_scattered_interpolation_hpp_included_


/*******************************************************************\

Module:

Author: Daniel Kroening, kroening@kroening.com

\*******************************************************************/

#include <util/arith_tools.h>
#include <util/fixedbv.h>
#include <util/std_types.h>

fixedbv_spect::fixedbv_spect(const fixedbv_typet &type)
{
  integer_bits = type.get_integer_bits();
  width = type.get_width();
}

fixedbv_spect::fixedbv_spect(const fixedbv_type2tc &type)
{
  integer_bits = type->integer_bits;
  width = type->get_width();
}

const fixedbv_type2tc fixedbv_spect::get_type() const
{
  return fixedbv_type2tc(width, integer_bits);
}

fixedbvt::fixedbvt() : v(0)
{
}

fixedbvt::fixedbvt(const fixedbv_spect &s) : spec(s), v(0)
{
}

fixedbvt::fixedbvt(const constant_exprt &expr)
{
  from_expr(expr);
}

void fixedbvt::from_expr(const constant_exprt &expr)
{
  spec = to_fixedbv_type(expr.type());
  v = binary2integer(id2string(expr.get_value()), true);
}

void fixedbvt::from_integer(const BigInt &i)
{
  v = i * power(2, spec.get_fraction_bits());
}

BigInt fixedbvt::to_integer() const
{
  // this rounds to zero, i.e., we just divide
  return v / power(2, spec.get_fraction_bits());
}

constant_exprt fixedbvt::to_expr() const
{
  fixedbv_typet type;
  type.set_width(spec.width);
  type.set_integer_bits(spec.integer_bits);
  constant_exprt expr(type);
  assert(spec.width != 0);
  expr.set_value(integer2binary(v, spec.width));
  return expr;
}

void fixedbvt::round(const fixedbv_spect &dest_spec)
{
  unsigned old_fraction_bits = spec.width - spec.integer_bits;
  unsigned new_fraction_bits = dest_spec.width - dest_spec.integer_bits;

  BigInt result = v;

  if(new_fraction_bits > old_fraction_bits)
    result = v * power(2, new_fraction_bits - old_fraction_bits);
  else if(new_fraction_bits < old_fraction_bits)
  {
    // may need to round
    BigInt p = power(2, old_fraction_bits - new_fraction_bits);
    BigInt div = v / p;
    BigInt rem = v % p;
    if(rem < 0)
      rem = -rem;

    if(rem * 2 >= p)
    {
      if(v < 0)
        --div;
      else
        ++div;
    }

    result = div;
  }

  unsigned old_integer_bits = spec.integer_bits;
  unsigned new_integer_bits = dest_spec.integer_bits;

  if(old_integer_bits > new_integer_bits)
  {
    // Need to cut off some higher bits.
    fixedbvt tmp;
    tmp.spec = dest_spec;

    // Make a number that's 2^integer_bits
    BigInt aval(2);
    aval = power(aval, new_integer_bits);
    tmp.from_integer(aval);

    // Now modulus that up.
    result = result % tmp.v;
  }

  // Increasing integer bits requires no additional changes to representation.

  v = result;
  spec = dest_spec;
}

void fixedbvt::negate()
{
  v = -v;
}

fixedbvt &fixedbvt::operator*=(const fixedbvt &o)
{
  v *= o.v;

  fixedbv_spect old_spec = spec;

  spec.width += o.spec.width;
  spec.integer_bits += o.spec.integer_bits;

  round(old_spec);

  return *this;
}

fixedbvt &fixedbvt::operator/=(const fixedbvt &o)
{
  v *= power(2, o.spec.get_fraction_bits());
  v /= o.v;

  return *this;
}

bool fixedbvt::operator==(int i) const
{
  return v == power(2, spec.get_fraction_bits()) * i;
}

std::string fixedbvt::format(const format_spect &format_spec) const
{
  std::string dest;
  unsigned fraction_bits = spec.get_fraction_bits();

  BigInt int_value = v;
  BigInt factor = power(2, fraction_bits); //BigInt(1)<<fraction_bits;

  if(int_value.is_negative())
  {
    dest += '-';
    int_value.negate();
  }

  std::string base_10_string =
    integer2string(int_value * power(10, fraction_bits) / factor);

  while(base_10_string.size() <= fraction_bits)
    base_10_string = "0" + base_10_string;

  std::string integer_part =
    std::string(base_10_string, 0, base_10_string.size() - fraction_bits);

  std::string fraction_part =
    std::string(base_10_string, base_10_string.size() - fraction_bits);

  dest += integer_part;

  // strip trailing zeros
  while(!fraction_part.empty() &&
        fraction_part[fraction_part.size() - 1] == '0')
    fraction_part.resize(fraction_part.size() - 1);

  if(!fraction_part.empty())
    dest += "." + fraction_part;

  while(dest.size() < format_spec.min_width)
    dest = " " + dest;

  return dest;
}

fixedbvt &fixedbvt::operator+=(const fixedbvt &o)
{
  v += o.v;

  // No need to change the spec.
  round(spec);

  return *this;
}

fixedbvt &fixedbvt::operator-=(const fixedbvt &o)
{
  v -= o.v;

  // No need to change the spec.
  round(spec);

  return *this;
}

fixedbvt &fixedbvt::operator!()
{
  this->negate();
  return (*this);
}

bool operator>(const fixedbvt &a, int i)
{
  fixedbvt other;
  other.spec = a.spec;
  other.from_integer(i);
  return a > other;
}

bool operator<(const fixedbvt &a, int i)
{
  fixedbvt other;
  other.spec = a.spec;
  other.from_integer(i);
  return a < other;
}

bool operator>=(const fixedbvt &a, int i)
{
  fixedbvt other;
  other.spec = a.spec;
  other.from_integer(i);
  return a >= other;
}

bool operator<=(const fixedbvt &a, int i)
{
  fixedbvt other;
  other.spec = a.spec;
  other.from_integer(i);
  return a <= other;
}

#ifdef WITH_PYTHON
#include <boost/python/class.hpp>

void build_fixedbv_python_class()
{
  using namespace boost::python;

  init<unsigned, unsigned> fbv_spec_init;
  class_<fixedbv_spect>("fixedbv_spec", fbv_spec_init)
    .def_readwrite("width", &fixedbv_spect::width)
    .def_readwrite("integer_bits", &fixedbv_spect::integer_bits)
    .def("get_fraction_bits", &fixedbv_spect::get_fraction_bits);

  // Only default inits
  class_<fixedbvt>("fixedbv")
    .def_readwrite("spec", &fixedbvt::spec)
    .add_property(
      "value",
      make_function(
        &fixedbvt::get_value, return_value_policy<return_by_value>()),
      make_function(
        &fixedbvt::set_value, return_value_policy<return_by_value>()))
    .def("from_integer", &fixedbvt::from_integer)
    .def("to_integer", &fixedbvt::to_integer)
    .def("round", &fixedbvt::round);
}

#endif


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>
#include <cstdint>
#include <vector>
#include <algorithm>
#include <range/v3/algorithm.hpp>
bool istriangle(std::vector<int> v) noexcept
{
  if (v[0] + v[1] <= v[2]) {
    return false;
  }
  for (int i = 0; i < 2; ++i) {
    std::rotate(v.begin(), v.begin() + 1, v.end());
    if (v[0] + v[1] <= v[2]) {
      return false;
    }
  }
  return true;
}

int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    std::string s;
    int n = 0;
    while (std::getline(ifs, s)) {
      std::vector<int> v;
      int line;
      std::istringstream iss(s);

      while (iss >> line) {
        v.push_back(line);
      }
      if (istriangle(v)) {
        ++n;
      }
    }
    fmt::print("triangle count:{}", n);
  }
}


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/triangulation/triangulation.h"

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <Eigen/SVD>
// #include <glog/logging.h>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/math/util.h"
#include "theia/sfm/pose/essential_matrix_utils.h"
#include "theia/sfm/pose/fundamental_matrix_util.h"
#include "theia/sfm/pose/util.h"

namespace theia {
namespace {

using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

// Given either a fundamental or essential matrix and two corresponding images
// points such that ematrix * point2 produces a line in the first image,
// this method finds corrected image points such that
// corrected_point1^t * ematrix * corrected_point2 = 0.
void FindOptimalImagePoints(const Matrix3d& ematrix,
                            const Vector2d& point1,
                            const Vector2d& point2,
                            Vector2d* corrected_point1,
                            Vector2d* corrected_point2) {
  const Vector3d point1_homog = point1.homogeneous();
  const Vector3d point2_homog = point2.homogeneous();

  // A helper matrix to isolate certain coordinates.
  Matrix<double, 2, 3> s_matrix;
  s_matrix << 1, 0, 0, 0, 1, 0;

  const Eigen::Matrix2d e_submatrix = ematrix.topLeftCorner<2, 2>();

  // The epipolar line from one image point in the other image.
  Vector2d epipolar_line1 = s_matrix * ematrix * point2_homog;
  Vector2d epipolar_line2 = s_matrix * ematrix.transpose() * point1_homog;

  const double a = epipolar_line1.transpose() * e_submatrix * epipolar_line2;
  const double b =
      (epipolar_line1.squaredNorm() + epipolar_line2.squaredNorm()) / 2.0;
  const double c = point1_homog.transpose() * ematrix * point2_homog;

  const double d = sqrt(b * b - a * c);

  double lambda = c / (b + d);
  epipolar_line1 -= e_submatrix * lambda * epipolar_line1;
  epipolar_line2 -= e_submatrix.transpose() * lambda * epipolar_line2;

  lambda *=
      (2.0 * d) / (epipolar_line1.squaredNorm() + epipolar_line2.squaredNorm());

  *corrected_point1 =
      (point1_homog - s_matrix.transpose() * lambda * epipolar_line1)
          .hnormalized();
  *corrected_point2 =
      (point2_homog - s_matrix.transpose() * lambda * epipolar_line2)
          .hnormalized();
}

}  // namespace

// Triangulates 2 posed views
bool Triangulate(const Matrix3x4d& pose1,
                 const Matrix3x4d& pose2,
                 const Vector2d& point1,
                 const Vector2d& point2,
                 Vector4d* triangulated_point) {
  Eigen::Matrix3d ematrix;
  EssentialMatrixFromTwoProjectionMatrices(pose1, pose2, &ematrix);

  Vector2d corrected_point1, corrected_point2;
  FindOptimalImagePoints(
      ematrix, point1, point2, &corrected_point1, &corrected_point2);

  // Now the two points are guaranteed to intersect. We can use the DLT method
  // since it is easy to construct.
  return TriangulateDLT(
      pose1, pose2, corrected_point1, corrected_point2, triangulated_point);
}

// Triangulates a 3D point by determining the closest point between the two
// rays. This method is known to be suboptimal in terms of reprojection error
// but it is extremely fast.
bool TriangulateMidpoint(const std::vector<Vector3d>& ray_origin,
                         const std::vector<Vector3d>& ray_direction,
                         Eigen::Vector4d* triangulated_point) {
  assert(triangulated_point != nullptr);
  assert(ray_origin.size() >= 2);
  assert(ray_origin.size() == ray_direction.size());

  Eigen::Matrix4d A;
  A.setZero();
  Eigen::Vector4d b;
  b.setZero();
  for (int i = 0; i < ray_origin.size(); i++) {
    const Eigen::Vector4d ray_direction_homog(
        ray_direction[i].x(), ray_direction[i].y(), ray_direction[i].z(), 0);
    const Eigen::Matrix4d A_term =
        Eigen::Matrix4d::Identity() -
        ray_direction_homog * ray_direction_homog.transpose();
    A += A_term;
    b += A_term * ray_origin[i].homogeneous();
  }

  Eigen::LLT<Eigen::Matrix4d> linear_solver(A);
  if (linear_solver.info() != Eigen::Success) {
    return false;
  }
  *triangulated_point = linear_solver.solve(b);
  return linear_solver.info() == Eigen::Success;
}

// Triangulates 2 posed views
bool TriangulateDLT(const Matrix3x4d& pose1,
                    const Matrix3x4d& pose2,
                    const Vector2d& point1,
                    const Vector2d& point2,
                    Vector4d* triangulated_point) {
  Matrix4d design_matrix;
  design_matrix.row(0) = point1[0] * pose1.row(2) - pose1.row(0);
  design_matrix.row(1) = point1[1] * pose1.row(2) - pose1.row(1);
  design_matrix.row(2) = point2[0] * pose2.row(2) - pose2.row(0);
  design_matrix.row(3) = point2[1] * pose2.row(2) - pose2.row(1);

  // Extract nullspace.
  *triangulated_point =
      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
  return true;
}

// Triangulates N views by computing SVD that minimizes the error.
bool TriangulateNViewSVD(const std::vector<Matrix3x4d>& poses,
                         const std::vector<Vector2d>& points,
                         Vector4d* triangulated_point) {
  assert(poses.size() == points.size());

  MatrixXd design_matrix(3 * points.size(), 4 + points.size());

  for (int i = 0; i < points.size(); i++) {
    design_matrix.block<3, 4>(3 * i, 0) = -poses[i].matrix();
    design_matrix.block<3, 1>(3 * i, 4 + i) = points[i].homogeneous();
  }

  *triangulated_point = design_matrix.jacobiSvd(Eigen::ComputeFullV)
                            .matrixV()
                            .rightCols<1>()
                            .head(4);
  return true;
}

bool TriangulateNView(const std::vector<Matrix3x4d>& poses,
                      const std::vector<Vector2d>& points,
                      Vector4d* triangulated_point) {
  assert(poses.size() == points.size());

  Matrix4d design_matrix = Matrix4d::Zero();
  for (int i = 0; i < points.size(); i++) {
    const Vector3d norm_point = points[i].homogeneous().normalized();
    const Eigen::Matrix<double, 3, 4> cost_term =
        poses[i].matrix() -
        norm_point * norm_point.transpose() * poses[i].matrix();
    design_matrix = design_matrix + cost_term.transpose() * cost_term;
  }

  Eigen::SelfAdjointEigenSolver<Matrix4d> eigen_solver(design_matrix);
  *triangulated_point = eigen_solver.eigenvectors().col(0);
  return eigen_solver.info() == Eigen::Success;
}

bool IsTriangulatedPointInFrontOfCameras(
    const FeatureCorrespondence& correspondence,
    const Matrix3d& rotation,
    const Vector3d& position) {
  const Vector3d dir1 = correspondence.feature1.homogeneous();
  const Vector3d dir2 =
      rotation.transpose() * correspondence.feature2.homogeneous();

  const double dir1_sq = dir1.squaredNorm();
  const double dir2_sq = dir2.squaredNorm();
  const double dir1_dir2 = dir1.dot(dir2);
  const double dir1_pos = dir1.dot(position);
  const double dir2_pos = dir2.dot(position);

  return (dir2_sq * dir1_pos - dir1_dir2 * dir2_pos > 0 &&
          dir1_dir2 * dir1_pos - dir1_sq * dir2_pos > 0);
}

// Returns true if the triangulation angle between any two observations is
// sufficient.
bool SufficientTriangulationAngle(
    const std::vector<Eigen::Vector3d>& ray_directions,
    const double min_triangulation_angle_degrees) {
  // Test that the angle between the rays is sufficient.
  const double cos_of_min_angle =
      cos(DegToRad(min_triangulation_angle_degrees));
  for (int i = 0; i < ray_directions.size(); i++) {
    for (int j = i + 1; j < ray_directions.size(); j++) {
      if (ray_directions[i].dot(ray_directions[j]) < cos_of_min_angle) {
        return true;
      }
    }
  }
  return false;
}

}  // namespace theia


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


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_ATANH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_ATANH_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/bitofsign.hpp>
#include <boost/simd/function/scalar/bitwise_xor.hpp>
#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/scalar/log1p.hpp>
#include <boost/simd/function/scalar/oneminus.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( atanh_
                          , (typename A0)
                             , bd::cpu_
                            , bd::scalar_< bd::floating_<A0> >
                            )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      A0 absa0 = bs::abs(a0);
      A0 t =  absa0+absa0;
      A0 z1 = oneminus(absa0);
      return bitwise_xor(bitofsign(a0),
                         Half<A0>()*log1p((absa0 < Half<A0>())
                                          ? fma(t, absa0/z1, t)
                                          : t/z1)
                        );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( atanh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bs::std_tag
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( A0  a0, std_tag const&) const BOOST_NOEXCEPT
    {
      return std::atanh(a0);
    }
  };
} } }

#endif


// faster alternative to ExpPlusPPTrajectory.eval() in Matlab
// Michael Kaess, June 2013

#include <mex.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
#include <iostream>

using namespace Eigen;
using namespace std;

// Initialization:
//   obj = ExpPlusPPTrajectoryEvalmex(breaks,K,A,alpha,gamma)
// Evaluation:
//   [y,jj] = ExpPlusPPTrajectoryEvalmex(obj, t)
//
// t:      m      vector of evaluation points
//
// breaks: n+1
// K:      d x a
// A:      a x a
// alpha:  a x n
// gamma:  dxn x p - reshape!
//
// y:      p x m
// jj:     m

class Eval {

private:

  const VectorXd m_breaks;
  const MatrixXd m_K, m_A, m_alpha, m_gamma;
  const int m_n, m_d, m_p;

public:

  MatrixXd expm(const MatrixXd& A) {
    MatrixXd F;
    MatrixExponential<MatrixXd>(A).compute(F);
    return F;
  }

  Eval(const VectorXd& breaks, const MatrixXd& K, const MatrixXd& A, const MatrixXd& alpha, const MatrixXd& gamma) : m_breaks(breaks), m_K(K), m_A(A), m_alpha(alpha), m_gamma(gamma), m_n(alpha.cols()), m_d(K.rows()), m_p(gamma.cols()) {}

  int dim() {
    return m_d;
  }

  VectorXd term(int j, double trel) {
    VectorXd trels(m_p);
    trels(0) = (trel<0)?-1.:1.;
    for (int i=1; i<m_p; i++) {
      trels(i) = trels(i-1) * trel;
    }
    const MatrixXd gammaj = m_gamma.block(j*m_d,0,m_d,m_p);
    return m_K*expm(m_A*trel)*m_alpha.col(j) + gammaj*trels;
  }

  void compute(const VectorXd& t, Map<MatrixXd>& y, Map<MatrixXd>& jj) {
    int m = t.rows();
    for(int k=0; k<m; k++) {
      double tk = t(k);
      // find the right interval
      int j = 0;
      for (int b=1; b<m_n; b++) { // todo: binary search
        if (tk >= m_breaks(b)) j=b;
      }
      double trel = tk - m_breaks(j);
      y.col(k) = term(j,trel);
      jj(k) = j+1; // convert to Matlab convention
    }
  }

};

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if (nrhs == 5 && nlhs == 1) {

    // create object
    Map<VectorXd> breaks(mxGetPr(prhs[0]), mxGetNumberOfElements(prhs[0]));
    Map<MatrixXd>      K(mxGetPr(prhs[1]), mxGetM(prhs[1]), mxGetN(prhs[1]));
    Map<MatrixXd>      A(mxGetPr(prhs[2]), mxGetM(prhs[2]), mxGetN(prhs[2]));
    Map<MatrixXd>  alpha(mxGetPr(prhs[3]), mxGetM(prhs[3]), mxGetN(prhs[3]));
    Map<MatrixXd>  gamma(mxGetPr(prhs[4]), mxGetM(prhs[4]), mxGetN(prhs[4]));

    Eval *eval = new Eval(breaks, K, A, alpha, gamma);
    mxClassID cid;
    if (sizeof(eval)==4) cid = mxUINT32_CLASS;
    else if (sizeof(eval)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("Drake:ExpPlusPPTmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
    memcpy(mxGetData(plhs[0]),&eval,sizeof(eval));

    //    mexPrintf("constructor\n"); mexCallMATLAB(0,NULL,0,NULL,"drawnow");

  } else {

    // retrieve object
    Eval *eval = NULL;
    if (nrhs==0 || !mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
      mexErrMsgIdAndTxt("Drake:ExpPlusPPTmex:BadInputs","the first argument should be the mex_ptr");
    memcpy(&eval,mxGetData(prhs[0]),sizeof(eval));

    if (nrhs == 1) {
      //      mexPrintf("delete\n"); mexCallMATLAB(0,NULL,0,NULL,"drawnow");

      // delete object
      if (eval)
	delete eval;

    } else {

      //      mexPrintf("eval\n"); mexCallMATLAB(0,NULL,0,NULL,"drawnow");

      // eval() function call
      if (nrhs != 2 || nlhs != 2) {
        mexErrMsgIdAndTxt("Drake:ExpPlusPPTmex:WrongNumberOfInputs","Usage obj = ExpPlusPPTmex(breaks,K,A,alpha,gamma) or [y,jj] = ExpPlusPPTmex(obj,t)");
      }

      Map<VectorXd> t(mxGetPr(prhs[1]), mxGetNumberOfElements(prhs[1]));

      int m = t.rows();
      int d = eval->dim();

      plhs[0] = mxCreateDoubleMatrix(d,m,mxREAL);
      Map<MatrixXd> y(mxGetPr(plhs[0]),d,m);
      plhs[1] = mxCreateDoubleMatrix(m,1,mxREAL);
      Map<MatrixXd> jj(mxGetPr(plhs[1]),m,1);

      eval->compute(t, y, jj);

    }

  }

}


//  Copyright (c) 2014 Anton Bikineev
//  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)
//
//  Appends negative test cases to the *.ipp files.
//  Takes the next parameters:
//  -f <file> file where the negative values will be appended;
//  -x add minus to existing x values and append result;
//  -v, -xv like previous option.
//  Usage example:
//  ./bessel_derivative_append_negative -f "bessel_y_derivative_large_data.ipp" -x -v -xv

#include <fstream>
#include <utility>
#include <functional>
#include <map>
#include <vector>
#include <iterator>
#include <algorithm>

#include <boost/multiprecision/mpfr.hpp>
#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>

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

template <class T>
T bessel_j_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_j(v, x) - boost::math::cyl_bessel_j(v+1, x);
}

template <class T>
T bessel_y_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_neumann(v, x) - boost::math::cyl_neumann(v+1, x);
}

template <class T>
T bessel_i_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_i(v, x) + boost::math::cyl_bessel_i(v+1, x);
}

template <class T>
T bessel_k_derivative_bare(T v, T x)
{
   return (v / x) * boost::math::cyl_bessel_k(v, x) - boost::math::cyl_bessel_k(v+1, x);
}

template <class T>
T sph_bessel_j_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_bessel(1, x);
   return boost::math::sph_bessel(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_bessel(itrunc(v), x);
}

template <class T>
T sph_bessel_y_derivative_bare(T v, T x)
{
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   if(v == 0)
      return -boost::math::sph_neumann(1, x);
   return boost::math::sph_neumann(itrunc(v-1), x) - ((v + 1) / x) * boost::math::sph_neumann(itrunc(v), x);
}

namespace opt = boost::program_options;
using FloatType = boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<200u> >;
using Function = FloatType(*)(FloatType, FloatType);
using Lines = std::vector<std::string>;

enum class Negate: char
{
   x,
   v,
   xv
};

namespace
{

const unsigned kSignificand = 50u;

std::map<std::string, Function> kFileMapper = {
   {"bessel_j_derivative_data.ipp", ::bessel_j_derivative_bare},
   {"bessel_j_derivative_int_data.ipp", ::bessel_j_derivative_bare},
   {"bessel_j_derivative_large_data.ipp", ::bessel_j_derivative_bare},
   {"bessel_y01_derivative_data.ipp", ::bessel_y_derivative_bare},
   {"bessel_yn_derivative_data.ipp", ::bessel_y_derivative_bare},
   {"bessel_yv_derivative_data.ipp", ::bessel_y_derivative_bare},
   {"bessel_i_derivative_data.ipp", ::bessel_i_derivative_bare},
   {"bessel_i_derivative_int_data.ipp", ::bessel_i_derivative_bare},
   {"bessel_k_derivative_data.ipp", ::bessel_k_derivative_bare},
   {"bessel_k_derivative_int_data.ipp", ::bessel_k_derivative_bare},
   {"sph_bessel_derivative_data.ipp", ::sph_bessel_j_derivative_bare},
   {"sph_neumann_derivative_data.ipp", ::sph_bessel_y_derivative_bare}
};

Function fp = ::bessel_j_derivative_bare;

Lines getSourcePartOfFile(std::fstream& file)
{
   file.seekg(std::ios::beg);

   Lines lines;
   while (true)
   {
      auto line = std::string{};
      std::getline(file, line);
      if (line.find("}};") != std::string::npos)
         break;
      lines.push_back(line);
   }
   file.seekg(std::ios::beg);
   return lines;
}

std::pair<std::string, std::string::iterator> parseValue(std::string::iterator& iter)
{
   using std::isdigit;

   auto value = std::string{};
   auto iterator = std::string::iterator{};

   while (!isdigit(*iter) && *iter != '-')
      ++iter;
   iterator = iter;
   while (isdigit(*iter) || *iter == '.' || *iter == 'e' || *iter == '-' || *iter == '+')
   {
      value.push_back(*iter);
      ++iter;
   }
   return {value, iterator};
}

void addMinusToValue(std::string& line, Negate which)
{
   using std::isdigit;

   auto iter = line.begin();
   switch (which)
   {
      case Negate::x:
      {
         ::parseValue(iter);
         auto value_begin = ::parseValue(iter).second;
         if (*value_begin != '-')
            line.insert(value_begin, '-');
         break;
      }
      case Negate::v:
      {
         auto value_begin = ::parseValue(iter).second;
         if (*value_begin != '-')
            line.insert(value_begin, '-');
         break;
      }
      case Negate::xv:
      {
         auto v_value_begin = ::parseValue(iter).second;
         if (*v_value_begin != '-')
            line.insert(v_value_begin, '-');
         // iterator could get invalid
         iter = line.begin();
         ::parseValue(iter);
         auto x_value_begin = ::parseValue(iter).second;
         if (*x_value_begin != '-')
            line.insert(x_value_begin, '-');
         break;
      }
   }
}

void replaceResultInLine(std::string& line)
{
   using std::isdigit;

   auto iter = line.begin();

   // parse v and x values from line and convert them to FloatType
   auto v = FloatType{::parseValue(iter).first};
   auto x = FloatType{::parseValue(iter).first};
   auto result = fp(v, x).str(kSignificand);

   while (!isdigit(*iter) && *iter != '-')
      ++iter;
   const auto where_to_write = iter;
   while (isdigit(*iter) || *iter == '.' || *iter == 'e' || *iter == '-' || *iter == '+')
      line.erase(iter);

   line.insert(where_to_write, result.begin(), result.end());
}

Lines processValues(const Lines& source_lines, Negate which)
{
   using std::placeholders::_1;

   auto processed_lines = source_lines;
   std::for_each(std::begin(processed_lines), std::end(processed_lines), std::bind(&addMinusToValue, _1, which));
   std::for_each(std::begin(processed_lines), std::end(processed_lines), &replaceResultInLine);

   return processed_lines;
}

void updateTestCount(Lines& source_lines, std::size_t mult)
{
   using std::isdigit;

   const auto where = std::find_if(std::begin(source_lines), std::end(source_lines),
      [](const std::string& str){ return str.find("boost::array") != std::string::npos; });
   auto& str = *where;
   const auto pos = str.find(">, ") + 3;
   auto digits_length = 0;

   auto k = pos;
   while (isdigit(str[k++]))
      ++digits_length;

   const auto new_value = mult * boost::lexical_cast<std::size_t>(str.substr(pos, digits_length));
   str.replace(pos, digits_length, boost::lexical_cast<std::string>(new_value));
}

} // namespace

int main(int argc, char*argv [])
{
   auto desc = opt::options_description{"All options"};
   desc.add_options()
      ("help", "produce help message")
      ("file", opt::value<std::string>()->default_value("bessel_j_derivative_data.ipp"))
      ("x", "append negative x")
      ("v", "append negative v")
      ("xv", "append negative x and v");
   opt::variables_map vm;
   opt::store(opt::command_line_parser(argc, argv).options(desc)
         .style(opt::command_line_style::default_style |
         opt::command_line_style::allow_long_disguise)
      .run(),vm);
   opt::notify(vm);

   if (vm.count("help"))
   {
      std::cout << desc;
      return 0;
   }

   auto filename = vm["file"].as<std::string>();
   fp = kFileMapper[filename];

   std::fstream file{filename.c_str()};
   if (!file.is_open())
      return -1;
   auto source_part = ::getSourcePartOfFile(file);
   source_part.back().push_back(',');

   auto cases_lines = Lines{};
   for (const auto& str: source_part)
   {
      if (str.find("SC_") != std::string::npos)
         cases_lines.push_back(str);
   }

   auto new_lines = Lines{};
   new_lines.reserve(cases_lines.size());

   std::size_t mult = 1;
   if (vm.count("x"))
   {
      std::cout << "process x..." << std::endl;
      const auto x_lines = ::processValues(cases_lines, Negate::x);
      new_lines.insert(std::end(new_lines), std::begin(x_lines), std::end(x_lines));
      ++mult;
   }
   if (vm.count("v"))
   {
      std::cout << "process v..." << std::endl;
      const auto v_lines = ::processValues(cases_lines, Negate::v);
      new_lines.insert(std::end(new_lines), std::begin(v_lines), std::end(v_lines));
      ++mult;
   }
   if (vm.count("xv"))
   {
      std::cout << "process xv..." << std::endl;
      const auto xv_lines = ::processValues(cases_lines, Negate::xv);
      new_lines.insert(std::end(new_lines), std::begin(xv_lines), std::end(xv_lines));
      ++mult;
   }

   source_part.insert(std::end(source_part), std::begin(new_lines), std::end(new_lines));
   ::updateTestCount(source_part, mult);

   file.close();
   file.open(filename, std::ios::out | std::ios::trunc);
   std::for_each(std::begin(source_part), std::end(source_part), [&file](const std::string& str)
      { file << str << std::endl; });
   file << "   }};";

   std::cout << "processed, ok\n";
   return 0;
}


#include <Rcpp.h>
#include <dlib/pixel.h>
#include <dlib/array2d.h>
#include <dlib/matrix.h>
//#include <dlib/image_loader/image_loader.h>
#include <dlib/image_transforms/fhog.h>
using namespace dlib;

// [[Rcpp::export]]
Rcpp::List dlib_fhog(std::vector<int> x, int rows, int cols,
                     const int cell_size = 8,
                     const int filter_rows_padding = 1,
                     const int filter_cols_padding = 1) {
  // Import from file
  //array2d<rgb_pixel> img;
  //load_bmp(img, file.c_str());
  array2d<rgb_pixel> img;
  img.set_size(rows, cols);
  for(int r = 0; r < rows; r++){
    for(int c = 0; c < cols; c++){
      int index = c*3 + r*cols*3;
      assign_pixel(img[r][c], rgb_pixel(x[index], x[index + 1], x[index + 2]));
    }
  }
  // Compute HOG features
  array2d<matrix<float,31,1> > hog;
  extract_fhog_features(img, hog, cell_size, filter_rows_padding, filter_cols_padding);

  Rcpp::NumericVector fhog(hog.nr() * hog.nc() * 31);
  int i = 0;
  for(int feat = 0; feat < 31; feat++){
    for(int x_i = 0; x_i < hog.nc(); x_i++){ // x is hog_width
      for(int y_i = 0; y_i < hog.nr(); y_i++){ // y is hog_height
        fhog[i] = hog[y_i][x_i](feat, 0);
        i = i + 1;
      }
    }
  }

  return Rcpp::List::create(Rcpp::Named("hog_height") = hog.nr(),
                            Rcpp::Named("hog_width") = hog.nc(),
                            Rcpp::Named("fhog") = fhog,
                            Rcpp::Named("hog_cell_size") = cell_size,
                            Rcpp::Named("filter_rows_padding") = filter_rows_padding,
                            Rcpp::Named("filter_cols_padding") = filter_cols_padding);
}


#include <armadillo>

int main() {

    arma::cube c(3, 3, 5);
    for (size_t s = 0; s < c.n_slices; s++)
        c.slice(s).fill(s);

    c.print("c");

    arma::sum(c, 2).print("c sum 2");

    return 0;

}


#define BOOST_TEST_MODULE unary
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "exprtest.hpp"

EXPRTEST(unary1, "-(2)",  -2)
EXPRTEST(unary2, "-(-2)",  2)
EXPRTEST(unary3, "+(-2)", -2)
EXPRTEST(unary4, "+(+2)",  2)