// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// Peter Sonneveld and Martin B. van Gijzen, IDR(s): a family of simple and fast algorithms for solving large nonsymmetric linear systems.
// SIAM J. Sci. Comput. Vol. 31, No. 2, pp. 1035-1062 (2008). (copyright SIAM)

#ifndef ITL_IDR_S_INCLUDE
#define ITL_IDR_S_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/operation/random.hpp>
#include <boost/numeric/mtl/operation/orth.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/matrix/strict_upper.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/krylov/base_solver.hpp>

namespace itl {

/// Induced Dimension Reduction on s dimensions (IDR(s))
template < typename LinearOperator, typename Vector,
	   typename LeftPreconditioner, typename RightPreconditioner,
	   typename Iteration >
int idr_s(const LinearOperator &A, Vector &x, const Vector &b,
	  const LeftPreconditioner &, const RightPreconditioner &,
	  Iteration& iter, size_t s)
{
    mtl::vampir_trace<7010> tracer;
    using mtl::size; using mtl::iall; using mtl::mat::strict_upper;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    typedef typename mtl::Collection<Vector>::size_type  Size;

    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");
    if (s < 1) s= 1;

    const Scalar                zero= math::zero(Scalar());
    Scalar                      omega(zero);
    Vector                      x0(x), y(resource(x)), v(resource(x)), t(resource(x)), q(resource(x)), r(b - A * x);
    mtl::mat::multi_vector<Vector>   dR(Vector(resource(x), zero), s), dX(Vector(resource(x), zero), s), P(Vector(resource(x), zero), s);
    mtl::dense_vector<Scalar>   m(s), c(s), dm(s);   // replicated in distributed solvers
    mtl::mat::dense2D<Scalar>        M(s, s);             // dito

    random(P);
    P.vector(0)= r;
    orth(P);

    for (size_t k= 0; k < s; k++) {
	v= A * r;
	omega= dot(v, r) / dot(v, v);
	dX.vector(k)= omega * r;
	dR.vector(k)= -omega * v;
	x+= dX.vector(k);
	r+= dR.vector(k);
	if ((++iter).finished(r)) return iter;
	M[iall][k]= trans(P) * dR.vector(k);
    }

    Size oldest= 0;
    m= trans(P) * r;

    while (! iter.finished(r)) {

	for (size_t k= 0; k < s; k++) {
	    c= lu_solve(M, m);
	    q= dR * -c;
	    v= r + q;
	    if (k == 0) {
		t= A * v;
		omega= dot(t, v) / dot(t, t);
		dR.vector(oldest)= q - omega * t;
		dX.vector(oldest)= omega * v - dX * c;
	    } else {
		dX.vector(oldest)= omega * v - dX * c;
		dR.vector(oldest)= A * -dX.vector(oldest);
	    }
	    r+= dR.vector(oldest);
	    x+= dX.vector(oldest);

	    if ((++iter).finished(r))
		return iter;

	    dm= trans(P) * dR.vector(oldest);
	    M[iall][oldest]= dm;
	    m+= dm;
	    oldest= (oldest + 1) % s;
	}
    }
    return iter;
}

/// Solver class for IDR(s) method; right preconditioner ignored (prints warning if not identity)
/** Methods inherited from \ref base_solver. **/
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class idr_s_solver
  : public base_solver< idr_s_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator >
{
    typedef base_solver< idr_s_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator > base;
  public:
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit idr_s_solver(const LinearOperator& A, size_t s= 8) : base(A), s(s), L(A), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    idr_s_solver(const LinearOperator& A, size_t s, const Preconditioner& L) : base(A), s(s), L(L), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    idr_s_solver(const LinearOperator& A, size_t s, const Preconditioner& L, const RightPreconditioner& R)
      : base(A), s(s), L(L), R(R) {}

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceX, typename HilbertSpaceB, typename Iteration >
    int solve(HilbertSpaceX& x, const HilbertSpaceB& b, Iteration& iter) const
    {
	return idr_s(this->A, x, b, L, R, iter, s);
    }

  private:
    size_t                s;
    Preconditioner        L;
    RightPreconditioner   R;
};

} // namespace itl

#endif // ITL_IDR_S_INCLUDE


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

 * 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    testGaussianBayesNet.cpp
 * @brief   Unit tests for GaussianBayesNet
 * @author  Frank Dellaert
 */

#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>

#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/bind/bind.hpp>

// STL/C++
#include <iostream>
#include <sstream>

using namespace boost::assign;
using namespace boost::placeholders;
using namespace std;
using namespace gtsam;

static const Key _x_ = 11, _y_ = 22, _z_ = 33;

static GaussianBayesNet smallBayesNet =
    list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1))(
        GaussianConditional(_y_, Vector1::Constant(5), I_1x1));

static GaussianBayesNet noisyBayesNet =
    list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1,
                                noiseModel::Isotropic::Sigma(1, 2.0)))(
        GaussianConditional(_y_, Vector1::Constant(5), I_1x1,
                            noiseModel::Isotropic::Sigma(1, 3.0)));

/* ************************************************************************* */
TEST( GaussianBayesNet, Matrix )
{
  Matrix R; Vector d;
  boost::tie(R,d) = smallBayesNet.matrix(); // find matrix and RHS

  Matrix R1 = (Matrix2() <<
          1.0, 1.0,
          0.0, 1.0
    ).finished();
  Vector d1 = Vector2(9.0, 5.0);

  EXPECT(assert_equal(R,R1));
  EXPECT(assert_equal(d,d1));
}

/* ************************************************************************* */
TEST( GaussianBayesNet, NoisyMatrix )
{
  Matrix R; Vector d;
  boost::tie(R,d) = noisyBayesNet.matrix(); // find matrix and RHS

  Matrix R1 = (Matrix2() <<
          0.5, 0.5,
          0.0, 1./3.
    ).finished();
  Vector d1 = Vector2(9./2., 5./3.);

  EXPECT(assert_equal(R,R1));
  EXPECT(assert_equal(d,d1));
}

/* ************************************************************************* */
TEST(GaussianBayesNet, Optimize) {
  VectorValues expected =
      map_list_of<Key, Vector>(_x_, Vector1::Constant(4))(_y_, Vector1::Constant(5));
  VectorValues actual = smallBayesNet.optimize();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianBayesNet, NoisyOptimize) {
  Matrix R;
  Vector d;
  boost::tie(R, d) = noisyBayesNet.matrix();  // find matrix and RHS
  const Vector x = R.inverse() * d;
  VectorValues expected = map_list_of<Key, Vector>(_x_, x.head(1))(_y_, x.tail(1));

  VectorValues actual = noisyBayesNet.optimize();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( GaussianBayesNet, optimizeIncomplete )
{
  static GaussianBayesNet incompleteBayesNet = list_of
    (GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1));

  VectorValues solutionForMissing = map_list_of<Key, Vector>
    (_y_, Vector1::Constant(5));

  VectorValues actual = incompleteBayesNet.optimize(solutionForMissing);

  VectorValues expected = map_list_of<Key, Vector>
    (_x_, Vector1::Constant(4))
    (_y_, Vector1::Constant(5));

  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( GaussianBayesNet, optimize3 )
{
  // y = R*x, x=inv(R)*y
  // 4 = 1 1   -1
  // 5     1    5
  // NOTE: we are supplying a new RHS here

  VectorValues expected = map_list_of<Key, Vector>
    (_x_, Vector1::Constant(-1))
    (_y_, Vector1::Constant(5));

  // Test different RHS version
  VectorValues gx = map_list_of<Key, Vector>
    (_x_, Vector1::Constant(4))
    (_y_, Vector1::Constant(5));
  VectorValues actual = smallBayesNet.backSubstitute(gx);
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianBayesNet, ordering)
{
  Ordering expected;
  expected += _x_, _y_;
  const auto actual = noisyBayesNet.ordering();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( GaussianBayesNet, MatrixStress )
{
  GaussianBayesNet bn;
  using GC = GaussianConditional;
  bn.emplace_shared<GC>(_x_, Vector2(1, 2), 1 * I_2x2, _y_, 2 * I_2x2, _z_, 3 * I_2x2);
  bn.emplace_shared<GC>(_y_, Vector2(3, 4), 4 * I_2x2, _z_, 5 * I_2x2);
  bn.emplace_shared<GC>(_z_, Vector2(5, 6), 6 * I_2x2);

  const VectorValues expected = bn.optimize();
  for (const auto& keys :
       {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}),
        KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}),
        KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) {
    const Ordering ordering(keys);
    Matrix R;
    Vector d;
    boost::tie(R, d) = bn.matrix(ordering);
    EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d));
  }
}

/* ************************************************************************* */
TEST( GaussianBayesNet, backSubstituteTranspose )
{
  // x=R'*y, expected=inv(R')*x
  // 2 = 1    2
  // 5   1 1  3
  VectorValues
    x = map_list_of<Key, Vector>
      (_x_, Vector1::Constant(2))
      (_y_, Vector1::Constant(5)),
    expected = map_list_of<Key, Vector>
      (_x_, Vector1::Constant(2))
      (_y_, Vector1::Constant(3));

  VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
  EXPECT(assert_equal(expected, actual));

  const auto ordering = noisyBayesNet.ordering();
  const Matrix R = smallBayesNet.matrix(ordering).first;
  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
}

/* ************************************************************************* */
TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
{
  // x=R'*y, expected=inv(R')*x
  // 2 = 1    2
  // 5   1 1  3
  VectorValues
    x = map_list_of<Key, Vector>
      (_x_, Vector1::Constant(2))
      (_y_, Vector1::Constant(5)),
    expected = map_list_of<Key, Vector>
      (_x_, Vector1::Constant(4))
      (_y_, Vector1::Constant(9));

  VectorValues actual = noisyBayesNet.backSubstituteTranspose(x);
  EXPECT(assert_equal(expected, actual));

  const auto ordering = noisyBayesNet.ordering();
  const Matrix R = noisyBayesNet.matrix(ordering).first;
  const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
  EXPECT(assert_equal(expected_vector, actual.vector(ordering)));
}

/* ************************************************************************* */
// Tests computing Determinant
TEST( GaussianBayesNet, DeterminantTest )
{
  GaussianBayesNet cbn;
  cbn += GaussianConditional(
          0, Vector2(3.0, 4.0), (Matrix2() << 1.0, 3.0, 0.0, 4.0).finished(),
          1, (Matrix2() << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));

  cbn += GaussianConditional(
          1, Vector2(5.0, 6.0), (Matrix2() << 1.0, 1.0, 0.0, 3.0).finished(),
          2, (Matrix2() << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));

  cbn += GaussianConditional(
      3, Vector2(7.0, 8.0), (Matrix2() << 1.0, 1.0, 0.0, 5.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));

  double expectedDeterminant = 60.0 / 64.0;
  double actualDeterminant = cbn.determinant();

  EXPECT_DOUBLES_EQUAL( expectedDeterminant, actualDeterminant, 1e-9);
}

/* ************************************************************************* */
namespace {
  double computeError(const GaussianBayesNet& gbn, const Vector10& values)
  {
    pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian();
    return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
  }
}

/* ************************************************************************* */
TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {

  // Create an arbitrary Bayes Net
  GaussianBayesNet gbn;
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
    0, Vector2(1.0,2.0), (Matrix2() << 3.0,4.0,0.0,6.0).finished(),
    3, (Matrix2() << 7.0,8.0,9.0,10.0).finished(),
    4, (Matrix2() << 11.0,12.0,13.0,14.0).finished()));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
    1, Vector2(15.0,16.0), (Matrix2() << 17.0,18.0,0.0,20.0).finished(),
    2, (Matrix2() << 21.0,22.0,23.0,24.0).finished(),
    4, (Matrix2() << 25.0,26.0,27.0,28.0).finished()));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
    2, Vector2(29.0,30.0), (Matrix2() << 31.0,32.0,0.0,34.0).finished(),
    3, (Matrix2() << 35.0,36.0,37.0,38.0).finished()));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
    3, Vector2(39.0,40.0), (Matrix2() << 41.0,42.0,0.0,44.0).finished(),
    4, (Matrix2() << 45.0,46.0,47.0,48.0).finished()));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
    4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished()));

  // Compute the Hessian numerically
  Matrix hessian = numericalHessian<Vector10>(
      boost::bind(&computeError, gbn, _1), Vector10::Zero());

  // Compute the gradient numerically
  Vector gradient = numericalGradient<Vector10>(
      boost::bind(&computeError, gbn, _1), Vector10::Zero());

  // Compute the gradient using dense matrices
  Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
  LONGS_EQUAL(11, (long)augmentedHessian.cols());
  Vector denseMatrixGradient = -augmentedHessian.col(10).segment(0,10);
  EXPECT(assert_equal(gradient, denseMatrixGradient, 1e-5));

  // Compute the steepest descent point
  double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
  Vector expected = gradient * step;

  // Compute the steepest descent point with the dogleg function
  VectorValues actual = gbn.optimizeGradientSearch();

  // Check that points agree
  KeyVector keys {0, 1, 2, 3, 4};
  Vector actualAsVector = actual.vector(keys);
  EXPECT(assert_equal(expected, actualAsVector, 1e-5));

  // Check that point causes a decrease in error
  double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(actual));
  double newError = GaussianFactorGraph(gbn).error(actual);
  EXPECT(newError < origError);
}

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


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include "print_values.hpp"

#include <mpllibs/metamonad/list.hpp>
#include <mpllibs/metamonad/do_c.hpp>
#include <mpllibs/metamonad/name.hpp>
#include <mpllibs/metamonad/guard.hpp>
#include <mpllibs/metamonad/pair.hpp>

#include <boost/mpl/list_c.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/equal_to.hpp>

#include <iostream>

int main()
{
  using mpllibs::metamonad::do_c;
  using mpllibs::metamonad::list_tag;
  using mpllibs::metamonad::set;
  using mpllibs::metamonad::do_return;
  using mpllibs::metamonad::guard;
  using mpllibs::metamonad::pair;

  using boost::mpl::list_c;
  using boost::mpl::range_c;
  using boost::mpl::equal_to;

  using namespace mpllibs::metamonad::name;

  /*
    List comprehension syntax in Haskell: [(i,j) | i <- [1,2], j <- [1..4]]

    Do syntax in Haskell:
      do i <- [1,2]
         j <- [1..4]
         return (i,j)

    Result in Haskell: [(1,1),(1,2),(1,3),(1,4),(2,1),(2,2),(2,3),(2,4)]
   */

  typedef
    do_c<list_tag,
      set<i, list_c<int, 1, 2> >,
      set<j, range_c<int, 1, 5> >,
      do_return<pair<i, j> >
    >
    result_of_list_comprehension;

  print_values<result_of_list_comprehension>();
  std::cout << std::endl;

  /*
    List comprehension syntax in Haskell:
      [(i,j) | i <- [1,2], j <- [1..4], i == j]

    Do syntax in Haskell:
      do i <- [1,2]
         j <- [1..4]
         Control.Monad.guard $ i == j
         return (i,j)

    Result in Haskell: [(1,1),(2,2)]
   */

  typedef
    do_c<list_tag,
      set<i, list_c<int, 1, 2> >,
      set<j, range_c<int, 1, 5> >,
      guard<list_tag, equal_to<i, j> >,
      do_return<pair<i, j> >
    >
    result_of_list_comprehension_with_guard;

  print_values<result_of_list_comprehension_with_guard>();
  std::cout << std::endl;
}


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

#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <iomanip>
#include <vector>
#include <iterator>

//[IE1

/*`
In this simple example, we'll import/export the bits of a cpp_int
to a vector of 8-bit unsigned values:
*/
/*=
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <iomanip>
#include <vector>
#include <iterator>
*/

int main()
{
   using boost::multiprecision::cpp_int;
   // Create a cpp_int with just a couple of bits set:
   cpp_int i;
   bit_set(i, 5000); // set the 5000'th bit
   bit_set(i, 200);
   bit_set(i, 50);
   // export into 8-bit unsigned values, most significant bit first:
   std::vector<unsigned char> v;
   export_bits(i, std::back_inserter(v), 8);
   // import back again, and check for equality:
   cpp_int j;
   import_bits(j, v.begin(), v.end());
   assert(i == j);
}

//]


#pragma once

#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

/**
 * @brief Calcola l'insieme di facce adiancenti a ogni vertice.
 *
 * Una faccia e' adiacente a un vertice se la faccia punta ad esso,
 * ovvero se esso e' uno dei suoi corner.
 *
 * @param V I vertici della mesh. Per ogni riga della matrice V, la posizione
 *          del vertice e' costituita dalle coordinate x,y,z memorizzate nelle
 *          3 colonne della riga.
 * @param F I triangoli della mesh. Per ogni riga della matrice F, il triangolo
 *          e' descritto dagli indici i,j,k memorizzati nelle 3 colonne della
 *          riga. Gli indici i,j,k si riferiscono ai 3 vertici A, B, C del
 *          triangolo, memorizzati in V.row(i), V.row(j) e V.row(k),
 *          rispettivamente. NOTA: per ogni triangolo, i 3 vertici sono da
 *          considerarsi indicati in senso antiorario.
 * @param VF Array di array di indici. Ogni elemento dell'array esterno
 *           corrisponde all'elenco di indici delle facce adiacenti al
 *           al vertice corrispondente. L'array esterno ha quindi V.rows() elementi.
 * @param VFi Array di array di indici. L'array esterno ha V.rows() elementi. VFi[i]
 *            ha lo stesso numero di elementi del corrispondente elemento in VF[i].
 *            VFi[i][j] corrisponde al corner di Vf[i][j] che incide sul vertice i.
 */
void vertex_face_adjacency(
    MatrixXd const &V,
    MatrixXi const &F,
    std::vector<std::vector<int>> &VF,
    std::vector<std::vector<int>> &VFi)
{
    VF.clear();
    VFi.clear();
    VF.resize(V.rows());
    VFi.resize(V.rows());

    for (int f = 0; f < F.rows(); ++f) {
        for (int p = 0; p < F.cols(); ++p) {
            int v = F(f, p);
            VF[v].push_back(f);
            VFi[v].push_back(p);
        }
    }
}

/**
 * @brief Calcola la lista di facce adiacenti ad ogni faccia.
 *
 * Una faccia e' adiacente a un'altra se queste condividono un lato, ovvero se
 * puntano agli stessi due vertici, ma in ordine inverso:
 * F.row(i) || F.row(j) <=> F(i,p) == F(j,q) &&
 *                          F(i,(p+1)%F.cols()) == F(j, (F.cols()+q-1)%F.cols())
 *
 * Questa funzione necessita dell'adiacenza vertice->facce, risultato della funzione
 * vertex_face_adjacency().
 *
 * @param V I vertici della mesh. Per ogni riga della matrice V, la posizione
 *          del vertice e' costituita dalle coordinate x,y,z memorizzate nelle
 *          3 colonne della riga.
 * @param F I triangoli della mesh. Per ogni riga della matrice F, il triangolo
 *          e' descritto dagli indici i,j,k memorizzati nelle 3 colonne della
 *          riga. Gli indici i,j,k si riferiscono ai 3 vertici A, B, C del
 *          triangolo, memorizzati in V.row(i), V.row(j) e V.row(k),
 *          rispettivamente. NOTA: per ogni triangolo, i 3 vertici sono da
 *          considerarsi indicati in senso antiorario.
 * @param VF Array di array di indici. Ogni elemento dell'array esterno
 *           corrisponde all'elenco di indici delle facce adiacenti al
 *           al vertice corrispondente. L'array esterno ha quindi V.rows() elementi.
 * @param VFi Array di array di indici. L'array esterno ha V.rows() elementi. VFi[i]
 *            ha lo stesso numero di elementi del corrispondente elemento in VF[i].
 *            VFi[i][j] corrisponde al corner di Vf[i][j] che incide sul vertice i.
 * @param FF Lista di indici di facce adiacenti per ogni faccia. Ogni riga corrisponde
 *           alla lista di adiacenza di una faccia. Ogni faccia ha F.cols() lati,
 *           per cui avra' F.cols() facce adiacenti (alcune potrebbero essere
 *           nulle in presenza di buchi - indice -1).
 * @param FFi Lista di indici per ogni faccia. Stessa dimensionalita' di FF. Ogni
 *            riga corrisponde a indici per una faccia, un indice per ogni faccia
 *            adiacente. Alla riga i, colonna j, l'indice corrisponde al lato della
 *            j-esima faccia adiacente alla i-esima, condiviso con il lato j-esimo
 *            della faccia i-esima.
 */
void face_face_adjacency(
    MatrixXd const &V,
    MatrixXi const &F,
    std::vector<std::vector<int>> const& VF,
    std::vector<std::vector<int>> const& VFi,
    MatrixXi &FF,
    MatrixXi &FFi)
{
    FF.resize(F.rows(), F.cols());
    FFi.resize(F.rows(), F.cols());
    FF.setConstant(-1);
    FFi.setConstant(-1);

    for (int f = 0; f < F.rows(); ++f) {
        for (int p = 0; p < F.cols(); ++p) {
            int v = F(f, p);
            std::vector<int> const& VF_adj = VF[v];
            std::vector<int> const& VFi_adj = VFi[v];

            int v_next = F(f, (p + 1) % F.cols());

            for (std::size_t i = 0; i < VF_adj.size(); ++i) {
                int f_adj = VF_adj[i];

                if (f_adj == f) {
                    continue;
                }

                int fi_adj = VFi_adj[i];
                int fi_prev = (F.cols() + fi_adj - 1) % F.cols();

                if (F(f_adj, fi_prev) == v_next) {
                    // trovato!
                    FF(f, p) = f_adj;
                    FFi(f, p) = fi_prev;
                }
            }
        }
    }
}


#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include "../../BenchTimer.h"
using namespace Eigen;

#ifndef SCALAR
#error SCALAR must be defined
#endif

typedef SCALAR Scalar;

template<typename MatA, typename MatB, typename MatC>
inline void lazy_gemm(const MatA &A, const MatB &B, MatC &C)
{
  escape((void*)A.data());
  escape((void*)B.data());
  C.noalias() += A.lazyProduct(B);
  escape((void*)C.data());
}

template<int m, int n, int k, int TA>
EIGEN_DONT_INLINE
double bench()
{
  typedef Matrix<Scalar,m,k,TA> MatA;
  typedef Matrix<Scalar,k,n> MatB;
  typedef Matrix<Scalar,m,n> MatC;

  MatA A(m,k);
  MatB B(k,n);
  MatC C(m,n);
  A.setRandom();
  B.setRandom();
  C.setZero();

  BenchTimer t;

  double up = 1e7*4/sizeof(Scalar);
  double tm0 = 10, tm1 = 20;

  double flops = 2. * m * n * k;
  long rep = std::max(10., std::min(10000., up/flops) );
  long tries = std::max(tm0, std::min(tm1, up/flops) );

  BENCH(t, tries, rep, lazy_gemm(A,B,C));

  return 1e-9 * rep * flops / t.best();
}

template<int m, int n, int k>
double bench_t(int t)
{
  if(t)
    return bench<m,n,k,RowMajor>();
  else
    return bench<m,n,k,0>();
}

EIGEN_DONT_INLINE
double bench_mnk(int m, int n, int k, int t)
{
  int id = m*10000 + n*100 + k;
  switch(id) {
    case  10101 : return bench_t< 1, 1, 1>(t); break;
    case  20202 : return bench_t< 2, 2, 2>(t); break;
    case  30303 : return bench_t< 3, 3, 3>(t); break;
    case  40404 : return bench_t< 4, 4, 4>(t); break;
    case  50505 : return bench_t< 5, 5, 5>(t); break;
    case  60606 : return bench_t< 6, 6, 6>(t); break;
    case  70707 : return bench_t< 7, 7, 7>(t); break;
    case  80808 : return bench_t< 8, 8, 8>(t); break;
    case  90909 : return bench_t< 9, 9, 9>(t); break;
    case 101010 : return bench_t<10,10,10>(t); break;
    case 111111 : return bench_t<11,11,11>(t); break;
    case 121212 : return bench_t<12,12,12>(t); break;
  }
  return 0;
}

int main(int argc, char **argv)
{
  std::vector<double> results;

  std::ifstream settings("lazy_gemm_settings.txt");
  long m, n, k, t;
  while(settings >> m >> n >> k >> t)
  {
    //std::cerr << "  Testing " << m << " " << n << " " << k << std::endl;
    results.push_back( bench_mnk(m, n, k, t) );
  }

  std::cout << RowVectorXd::Map(results.data(), results.size());

  return 0;
}


#ifndef CANNON_ML_ADAM_H
#define CANNON_ML_ADAM_H

/*!
 * \file cannon/ml/adam.hpp
 * \brief File containing AdamOptimizer class definition.
 */

/*!
 * \namespace cannon::ml;
 * \brief Namespace containing various optimization and learning algorithms.
 *
 * ML, or Machine Learning, is being used as an umbrella term here to suggest that
 * everything in this namespace has some relationship to optimizing function
 * approximations using data, but many of these methods are not, strictly
 * speaking, machine learning methods.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace ml {

    /*!
     * \brief Class representing an Adam optimizer. Adam is a stochastic
     * optimization method which produces modified stochastic gradients by
     * estimating the moments of the distribution of stochastic gradients. See
     * https://arxiv.org/abs/1412.6980
     */
    class AdamOptimizer {
      public:
        AdamOptimizer() = delete;

        /*!
         * \brief Constructor taking the number of rows and columns of the
         * parameter matrix to be optimized, as well as a learning rate for
         * gradient-based updates.
         */
        AdamOptimizer(unsigned int rows, unsigned int cols,
                      double learning_rate)
            : cols_(cols), rows_(rows), learning_rate_(learning_rate),
              first_moments_(MatrixXd::Zero(rows_, cols_)),
              second_moments_(MatrixXd::Zero(rows_, cols_)) {}

        /*!
         * \brief Apply a stochastic gradient update by modifying the moment
         * estimates of this optimizer with the input gradient.
         *
         * \param params Current parameter values
         * \param gradient Stochastic gradient
         *
         * \returns New parameters, adjusted using Adam.
         */
        MatrixXd apply_update(const MatrixXd& params, const MatrixXd& gradient);

        /*!
         * \brief Get modified Adam update given an input stochastic gradient.
         * Also modifies the moment estimates of this optimizer.
         *
         * \param gradient Stochastic gradient.
         *
         * \returns Adam update
         */
        MatrixXd get_update(const MatrixXd& gradient);

        /*!
         * \brief Set the learning rate for this optimizer.
         *
         * \param learning_rate The new learning rate.
         */
        void set_learning_rate(double learning_rate);

      private:
        // Parameters
        unsigned int cols_; //!< Number of columns in parameter matrix
        unsigned int rows_; //!< Number of rows in parameter matrix
        double learning_rate_; //!< Learning rate for parameter updates

        // Hyperparameters
        double beta_1_ = 0.9; //!< Exponential averaging parameter for first-order moments
        double beta_2_ = 0.999; //!< Exponential averaging parameter for second-order moments
        double epsilon_ = 1e-8; //!< Small positive number to prevent division by zero

        unsigned int t_ = 0; //!< Number of stochastic gradients seen so far

        // Matrices
        MatrixXd first_moments_; //!< First-order moments of stochastic gradient distribution
        MatrixXd second_moments_; //!< Exponential averaging parameter for first-order moments
    };

  } // namespace ml
} // namespace cannon

#endif /* ifndef CANNON_ML_ADAM_H */


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010-2011 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "common.h"
#include <Eigen/LU>

// computes an LU factorization of a general M-by-N matrix A using partial pivoting with row interchanges
EIGEN_LAPACK_FUNC(getrf,(int *m, int *n, RealScalar *pa, int *lda, int *ipiv, int *info))
{
  *info = 0;
        if(*m<0)                  *info = -1;
  else  if(*n<0)                  *info = -2;
  else  if(*lda<std::max(1,*m))   *info = -4;
  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"GETRF", &e, 6);
  }

  if(*m==0 || *n==0)
    return 0;

  Scalar* a = reinterpret_cast<Scalar*>(pa);
  int nb_transpositions;
  int ret = int(Eigen::internal::partial_lu_impl<Scalar,ColMajor,int>
                     ::blocked_lu(*m, *n, a, *lda, ipiv, nb_transpositions));

  for(int i=0; i<std::min(*m,*n); ++i)
    ipiv[i]++;

  if(ret>=0)
    *info = ret+1;

  return 0;
}

//GETRS solves a system of linear equations
//    A * X = B  or  A' * X = B
//  with a general N-by-N matrix A using the LU factorization computed  by GETRF
EIGEN_LAPACK_FUNC(getrs,(char *trans, int *n, int *nrhs, RealScalar *pa, int *lda, int *ipiv, RealScalar *pb, int *ldb, int *info))
{
  *info = 0;
        if(OP(*trans)==INVALID)  *info = -1;
  else  if(*n<0)                 *info = -2;
  else  if(*nrhs<0)              *info = -3;
  else  if(*lda<std::max(1,*n))  *info = -5;
  else  if(*ldb<std::max(1,*n))  *info = -8;
  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"GETRS", &e, 6);
  }

  Scalar* a = reinterpret_cast<Scalar*>(pa);
  Scalar* b = reinterpret_cast<Scalar*>(pb);
  MatrixType lu(a,*n,*n,*lda);
  MatrixType B(b,*n,*nrhs,*ldb);

  for(int i=0; i<*n; ++i)
    ipiv[i]--;
  if(OP(*trans)==NOTR)
  {
    B = PivotsType(ipiv,*n) * B;
    lu.triangularView<UnitLower>().solveInPlace(B);
    lu.triangularView<Upper>().solveInPlace(B);
  }
  else if(OP(*trans)==TR)
  {
    lu.triangularView<Upper>().transpose().solveInPlace(B);
    lu.triangularView<UnitLower>().transpose().solveInPlace(B);
    B = PivotsType(ipiv,*n).transpose() * B;
  }
  else if(OP(*trans)==ADJ)
  {
    lu.triangularView<Upper>().adjoint().solveInPlace(B);
    lu.triangularView<UnitLower>().adjoint().solveInPlace(B);
    B = PivotsType(ipiv,*n).transpose() * B;
  }
  for(int i=0; i<*n; ++i)
    ipiv[i]++;

  return 0;
}


#pragma once
#include <cmath>
#include <limits>
#include <algorithm>
#include <boost/unordered_map.hpp>
#include "types.hh"

using Bounds = std::pair<double, bool>;
static Bounds operator+ (Bounds a,Bounds b) {
  return Bounds(a.first + b.first, a.second && b.second);
}
static inline std::ostream& operator << (std::ostream& os, const Bounds& b) {
  os << "(" << b.first << ", " << b.second << ")";
  return os;
}

#include <eigen3/Eigen/Core>
//! @TODO configure include directory for eigen

struct Zone {
  using Variables = char;
  using Tuple = std::tuple<std::vector<Bounds>,Bounds>;
  Eigen::Matrix<Bounds, Eigen::Dynamic, Eigen::Dynamic> value;
  Bounds M;

  inline std::size_t getNumOfVar() const {
    return value.cols() - 1;
  }

  inline void cutVars (std::shared_ptr<Zone> &out,std::size_t from,std::size_t to) {
    out = std::make_shared<Zone>();
    out->value.resize(to - from + 2, to - from + 2);
    out->value.block(0,0,1,1) << Bounds(0,true);
    out->value.block(1, 1, to - from + 1, to - from + 1) = value.block(from + 1, from + 1, to - from + 1,to - from + 1);
    out->value.block(1, 0, to - from + 1, 1) = value.block(from + 1, 0, to - from + 1, 1);
    out->value.block(0, 1, 1, to - from + 1) = value.block(0, from + 1, 1, to - from + 1);
    out->M = M;
  }

  static Zone zero(int size) {
    static Zone zeroZone;
    zeroZone.value.resize(size, size);
    zeroZone.value.fill(Bounds(0, true));
    return zeroZone;
  }

  std::tuple<std::vector<Bounds>,Bounds> toTuple() const {
    // omit (0,0)
    return std::tuple<std::vector<Bounds>,Bounds>(std::vector<Bounds>(value.data() + 1, value.data() + value.size()),M);
  }

  //! @brief add the constraint x - y \le (c,s)
  void tighten(Variables x, Variables y, Bounds c) {
    x++;
    y++;
    value(x,y) = std::min(value(x, y), c);
    close1(x);
    close1(y);
  }

  void close1(Variables x) {
    for (int i = 0; i < value.rows(); i++) {
      for (int j = 0; j < value.cols(); j++) {
        value(i, j) = std::min(value(i, j), value(i, x) + value(x, j));
      }
    }
  }

  // The reset value is always (0, \le)
  void reset(Variables x) {
    // 0 is the special varibale here
    x++;
    value(0,x) = Bounds(0, true);
    value(x,0) = Bounds(0, true);
    value.col(x).tail(value.rows() - 1) = value.col(0).tail(value.rows() - 1);
    value.row(x).tail(value.cols() - 1) = value.row(0).tail(value.cols() - 1);
  }

  void elapse() {
    static const Bounds infinity = Bounds(std::numeric_limits<double>::infinity(), false);
    value.col(0).fill(infinity);
    for (int i = 0; i < value.row(0).size(); ++i) {
      value.row(0)[i].second = false;
    }
  }

  void canonize() {
    for (int k = 0; k < value.cols(); k++) {
      close1(k);
    }
  }

  bool isSatisfiable() {
    canonize();
    return (value + value.transpose()).minCoeff() >= Bounds(0.0,true);
  }

  void abstractize() {
    static const Bounds infinity = Bounds(std::numeric_limits<double>::infinity(), false);
    for (auto it = value.data(); it < value.data() + value.size(); it++) {
      if (*it >= M) {
        *it = infinity;
      }
    }
  }

  bool operator== (Zone z) const {
    z.value(0,0) = value(0,0);
    return value == z.value;
  }
};

struct ZoneAutomaton : public AbstractionAutomaton<Zone> {
  struct TAEdge {
    State source;
    State target;
    Alphabet c;
    std::vector<Alphabet> resetVars;
    std::vector<Constraint> guard;
  };

  boost::unordered_map<std::tuple<State, State, Alphabet>, TAEdge> edgeMap;
  boost::unordered_map<std::pair<TAState, typename Zone::Tuple>, RAState> zones_in_za;
  int numOfVariables;
};

static inline std::ostream& operator << (std::ostream& os, const Zone& z) {
  for (int i = 0; i < z.value.rows();i++) {
    for (int j = 0; j < z.value.cols();j++) {
      os << z.value(i,j);
    }
    os << "\n";
  }
  os << std::endl;
  return os;
}


#include "Vector.hpp"
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include "fhe/EncryptedArray.h"
#include <cmath>
#include <cassert>
#include <algorithm>
namespace MDL {
template<typename T>
double Vector<T>::L2() const {
    double norm = 0.0;

    for (auto e : *this) {
        auto de = static_cast<double>(e);
        norm += de * de;
    }
    return std::sqrt(norm);
}

template<typename T>
Vector<double> Vector<T>::reduce(double factor) const {
    Vector<double> vec(dimension());
    std::transform(this->begin(), this->end(), vec.begin(),
                   [&factor](const T &v) { return v / factor; });
    return vec;
}

template<>
Vector<double> Vector<NTL::ZZX>::reduce(double factor) const {
    Vector<double> vec(dimension());
    std::transform(this->begin(), this->end(), vec.begin(),
                   [&factor](const NTL::ZZX &v) {
                   return NTL::to_long(v[0]) / factor;
                   });
    return vec;
}

template<>
Vector<double> Vector<NTL::ZZ>::reduce(double factor) const {
    Vector<double> vec(dimension());
    std::transform(this->begin(), this->end(), vec.begin(),
                   [&factor](const NTL::ZZ &v) {
                   double d;
                   NTL::conv(d, v);
                   return d / factor;
                   });
    return vec;
}

template<>
double Vector<NTL::ZZX>::L2() const {
    NTL::ZZ summation(0);

    for (auto& e : *this) {
		if (e.rep.length() > 0) summation += e[0] * e[0];
    }
    return std::sqrt(std::exp(log(summation)));
}

template<>
double Vector<NTL::ZZ>::L2() const {
    NTL::ZZ summation(0);

    for (auto& e : *this) {
		summation += e * e;
    }
    return std::sqrt(std::exp(log(summation)));
}

template<typename T>
T Vector<T>::dot(const Vector<T>& oth) const {
    assert(dimension() == oth.dimension());
    T sum(0);

    for (size_t i = 0; i < dimension(); i++) {
        sum += this->at(i) * oth[i];
    }
    return sum;
}

template<typename T>
Vector<T> Vector<T>::subvector(long startIndex, long endIndex) const
{
    while (startIndex < 0) { startIndex += dimension(); }
    while (endIndex < 0) { endIndex += dimension(); }
    if (endIndex < startIndex || endIndex >= this->size()) {
        std::cerr << "Invalid subvector arguments" << std::endl;
        return *this;
    }
    Vector<T> sub(endIndex - startIndex + 1);
    auto start = this->begin();
    auto end = start;
    std::advance(start, startIndex);
    std::advance(end, endIndex + 1);
    std::copy(start, end, sub.begin());
    return sub;
}

template<typename T>
Vector<long> Vector<T>::div(long factor) const
{
    Vector<long> vec(dimension());
    std::transform(this->begin(), this->end(), vec.begin(), [&factor](T e) { return std::lround(e / factor); });
    return vec;
}

template<>
Vector<long> Vector<NTL::ZZX>::div(long factor) const = delete;

template<>
NTL::ZZX Vector<long>::encode(const EncryptedArray &ea) const
{
    assert(this->size() <= ea.size());
    NTL::ZZX encoded;
    if (this->size() < ea.size()) {
        auto tmp(*this);
        tmp.resize(ea.size());
        ea.encode(encoded, tmp);
    } else {
        ea.encode(encoded, *this);
    }
    return encoded;
}

template<typename T>
Vector<T>& Vector<T>::operator*=(const T& val)
{
    for (auto &ele : *this) {
        ele *= val;
    }
    return *this;
}

template<typename T>
Vector<T>& Vector<T>::operator-=(const Vector<T> &oth)
{
    auto dd = std::min(dimension(), oth.dimension());
    for (size_t d = 0; d < dd; d++) {
        this->at(d) -= oth[d];
    }
    return *this;
}

template<typename T>
Vector<T>& Vector<T>::operator+=(const Vector<T> &oth)
{
    auto dd = std::min(dimension(), oth.dimension());
    for (size_t d = 0; d < dd; d++) {
        this->at(d) += oth[d];
    }
    return *this;
}

template<>
void Vector<long>::random(const long &domain)
{
    for (size_t i = 0; i < size(); i++) {
        this->at(i) = NTL::RandomBnd(domain);
    }
}

template class Vector<long>;
template class Vector<double>;
template class Vector<NTL::ZZX>;
} // namespace MDL


#include <boost/test/unit_test.hpp>

#include "math/algo/interval_scheduler_maximize.h" // header to test
#include "che/sequence_interval.h"

using namespace biosim;

BOOST_AUTO_TEST_SUITE(suite_interval_scheduler_maximize)

BOOST_AUTO_TEST_CASE(interval_scheduler_maximize_interval) {
  std::set<math::interval<int>> intervals;
  math::algo::interval_scheduler_maximize<math::interval<int>> s;
  BOOST_CHECK(s.schedule(intervals).empty());

  math::interval<int> iv1(1, 5), iv2(2, 4), iv3(2, 6), iv4(5, 10), iv5(6, 10), iv6(7, 10);

  intervals.insert(iv1);
  BOOST_CHECK(s.schedule(intervals).size() == 1);

  intervals.insert(iv2);
  intervals.insert(iv3);
  BOOST_CHECK(s.schedule(intervals).size() == 1);

  intervals.insert(iv4);
  BOOST_CHECK(s.schedule(intervals).size() == 2);

  intervals.insert(iv5);
  intervals.insert(iv6);
  BOOST_CHECK(s.schedule(intervals).size() == 2);
}

BOOST_AUTO_TEST_CASE(interval_scheduler_maximize_sequence_interval) {
  std::set<che::cchb_dssp_interval> intervals;
  math::algo::interval_scheduler_maximize<che::cchb_dssp_interval> s;
  BOOST_CHECK(s.schedule(intervals).empty());

  che::cchb_dssp_interval iv1(1, 5, che::cchb_dssp('H')), iv2(2, 4, che::cchb_dssp('H')),
      iv3(4, 6, che::cchb_dssp('E'));
  intervals.insert(iv1);
  intervals.insert(iv2);
  BOOST_CHECK(s.schedule(intervals).size() == 1);

  intervals.insert(iv3);
  BOOST_CHECK(s.schedule(intervals).size() == 1);
}

BOOST_AUTO_TEST_SUITE_END()


#include "RBGL.hpp"
#include "Basic2DMatrix.hpp"
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/dag_shortest_paths.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>

extern "C"
{
    SEXP BGL_dijkstra_shortest_paths_D (SEXP num_verts_in,
                                        SEXP num_edges_in, SEXP R_edges_in,
                                        SEXP R_weights_in, SEXP init_ind)
    {
        using namespace boost;

        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        int N = num_vertices(g);
        std::vector<Vertex> p(N);
        std::vector<double> d(N);

        dijkstra_shortest_paths(g, vertex((int)INTEGER(init_ind)[0], g),
                                predecessor_map(&p[0]).distance_map(&d[0]));

        SEXP dists, pens, ansList;
        PROTECT(dists = allocVector(REALSXP,N));
        PROTECT(pens = allocVector(INTSXP,N));
        graph_traits < Graph_dd >::vertex_iterator vi, vend;
        for (tie(vi, vend) = vertices(g); vi != vend; ++vi) {
            REAL(dists)[*vi] = d[*vi];
            INTEGER(pens)[*vi] = p[*vi];
        }
        PROTECT(ansList = allocVector(VECSXP,2));
        SET_VECTOR_ELT(ansList,0,dists);
        SET_VECTOR_ELT(ansList,1,pens);

        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_johnson_all_pairs_shortest_paths_D(SEXP num_verts_in,
            SEXP num_edges_in, SEXP R_edges_in,
            SEXP R_weights_in)
    {
        using namespace boost;
        int nv = INTEGER(num_verts_in)[0];
        SEXP out;

        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        Basic2DMatrix<double> D(nv, nv);

        johnson_all_pairs_shortest_paths(g, D);

        PROTECT(out = NEW_NUMERIC(nv*nv));
        int k = 0;
        for (int i = 0 ; i < nv ; i++)
            for (int j = 0; j < nv; j++ )
            {
                REAL(out)[k] = D[i][j];
                k++;
            }
        UNPROTECT(1);
        return out;
    }

    SEXP BGL_bellman_ford_shortest_paths(SEXP num_verts_in,
                                         SEXP num_edges_in, SEXP R_edges_in,
                                         SEXP R_weights_in, SEXP init_ind)
    {
        using namespace boost;

        typedef adjacency_list < vecS, vecS, directedS,
        no_property, property < edge_weight_t, double> > EdgeGraph;

        int i;
        int NE = (int)INTEGER(num_edges_in)[0];
        int N = (int)INTEGER(num_verts_in)[0];
        int s = (int)INTEGER(init_ind)[0];

        EdgeGraph g(N);

        int* edges_in = INTEGER(R_edges_in);
        for ( i = 0; i < NE; i++, edges_in += 2 )
            add_edge(*edges_in, *(edges_in+1), g);

        std::vector<std::size_t> p(N);
        for ( i = 0; i < N; i++ ) p[i] = i;

        std::vector<double> d(N, std::numeric_limits<double>::max());
        d[s] = 0;

        property_map<EdgeGraph, edge_weight_t>::type w = get(edge_weight, g);

        int* weight_i = (isReal(R_weights_in)) ? 0 : INTEGER(R_weights_in);
        double* weight_d = (isReal(R_weights_in)) ? REAL(R_weights_in) : 0;

        graph_traits< EdgeGraph >::edge_iterator ei, ei_end;
        for ( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei )
            w[*ei] = weight_i ? (*weight_i++) : (*weight_d++);

        bool r = bellman_ford_shortest_paths(g, N,
                 weight_map(w).predecessor_map(&p[0]).distance_map(&d[0]));

        SEXP conn, dList, pList, ansList;
        PROTECT(ansList = allocVector(VECSXP,3));
        PROTECT(conn = NEW_LOGICAL(1));
        PROTECT(dList = allocVector(REALSXP,N));
        PROTECT(pList = allocVector(INTSXP,N));

        LOGICAL(conn)[0] = r;

        for (i = 0; i < N; i++)
        {
            INTEGER(pList)[i] = p[i];
            REAL(dList)[i] = d[i];
        }

        SET_VECTOR_ELT(ansList,0,conn);
        SET_VECTOR_ELT(ansList,1,dList);
        SET_VECTOR_ELT(ansList,2,pList);

        UNPROTECT(4);
        return(ansList);
    }

    SEXP BGL_dag_shortest_paths(SEXP num_verts_in,
                                SEXP num_edges_in, SEXP R_edges_in,
                                SEXP R_weights_in, SEXP init_ind)
    {
        using namespace boost;

        typedef graph_traits < Graph_dd >::edge_descriptor Edge;
        typedef graph_traits < Graph_dd >::vertex_descriptor Vertex;
        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        int N = num_vertices(g);
        std::vector<Vertex> p(N);
        std::vector<double> d(N);

        dag_shortest_paths(g, vertex((int)INTEGER(init_ind)[0], g),
                           predecessor_map(&p[0]).distance_map(&d[0]));

        SEXP dists, pens, ansList;
        PROTECT(dists = allocVector(REALSXP,N));
        PROTECT(pens = allocVector(INTSXP,N));
        graph_traits < Graph_dd >::vertex_iterator vi, vend;
        for (tie(vi, vend) = vertices(g); vi != vend; ++vi) {
            if ( int(d[*vi]) == std::numeric_limits<int>::max() )
            {
                REAL(dists)[*vi] = R_NaN;
                INTEGER(pens)[*vi] = *vi;
            }
            else
            {
                REAL(dists)[*vi] = d[*vi];
                INTEGER(pens)[*vi] = p[*vi];
            }
        }
        PROTECT(ansList = allocVector(VECSXP,2));
        SET_VECTOR_ELT(ansList,0,dists);
        SET_VECTOR_ELT(ansList,1,pens);

        UNPROTECT(3);
        return(ansList);
    }

    SEXP BGL_floyd_warshall_all_pairs_shortest_paths_D(SEXP num_verts_in,
            SEXP num_edges_in, SEXP R_edges_in,
            SEXP R_weights_in)
    {
        using namespace boost;
        typedef adjacency_list<vecS, vecS, directedS, no_property,
        property< edge_weight_t, double, property< edge_weight2_t, double > > > Graph;
        int nv = INTEGER(num_verts_in)[0];
        SEXP out;
        typedef std::pair < int, int >Edge;

        Graph_dd g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        Basic2DMatrix<double> D(nv, nv);

        floyd_warshall_all_pairs_shortest_paths(g, D);

        PROTECT(out = NEW_NUMERIC(nv*nv));
        int k = 0;
        for (int i = 0 ; i < nv ; i++)
            for (int j = 0; j < nv; j++ )
            {
                REAL(out)[k] = D[i][j];
                k++;
            }
        UNPROTECT(1);
        return out;
    }
}


// std includes
#include <iostream> // cout, endl
#include <memory> // shared_ptr
#include <tuple>
#include <vector>
// thirdparties includes
#include <Eigen/Dense>
// lib includes
// // s0s
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/point.h"
// // sa0
#include "sa0/active.h"
#include "sa0/actuator/point.h"
// simple includes
#include "flow.h"

using TypeScalar = double;
template<typename ...Args>
using TypeContainer = std::vector<Args...>;
// Space
template<int StateSize>
using TypeState = Eigen::Matrix<TypeScalar, StateSize, 1>;
constexpr unsigned int DIM = 3;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Point
using TypeStepPoint = sl0::StepPoint<TypeState, DIM, TypeRef, TypeView, Flow>;
// Choose passive
using TypeStepPassive = TypeStepPoint;
// Active
using TypeStepActuator = sl0::sa0::StepActuator<TypeStepPassive::TypeStateStatic, TypeRef, TypeStepPassive>;
using TypeStepPointSwim = sl0::sa0::StepPointSwim<TypeStepPassive::TypeStateStatic, TypeRef, TypeStepPassive, TypeVector>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg;

int main () {
    TypeVector us = TypeVector::Constant(0.0);
    us[0] = 1.0;
    TypeVector x0 = TypeVector::Constant(1.0);
    double t0 = 0.0;
    double dt = 1e-0;
    double tEnd = 1.0;
    unsigned int nt = std::round((tEnd - t0) / dt);
    // Create activePoint
    sl0::sa0::ObjectActive<TypeState, TypeRef, TypeView, TypeStepPassive, TypeStepActuator, TypeSolver> activePoint(TypeStepPassive(std::make_shared<Flow>()));
    std::shared_ptr<TypeStepPointSwim> sStepPointSwim = std::make_shared<TypeStepPointSwim>(us);
    activePoint.sStep->register_actuator(sStepPointSwim);
    // Set initial state
    activePoint.sStep->x(activePoint.state) = x0;
    activePoint.t = t0;
    // Computation
    for(std::size_t i = 0; i < nt; i++) {
        activePoint.update(dt);
    }
    // out
    std::cout << "\n";
    std::cout << "activePoint advected and swimming in an exponential flow, t = " << activePoint.t << "\n";
    std::cout << "\n";
    std::cout << "activePoint position : " << "\n" << activePoint.sStep->x(activePoint.state) << "\n";
    std::cout << std::endl;
}


//
//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//
//  And we acknowledge the support from all contributors.

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

#include <algorithm>
#include <iostream>

#include "utility.hpp"

BOOST_AUTO_TEST_SUITE ( test_einstein_notation/*,
                      *boost::unit_test::depends_on("test_multi_index") */)

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

//using test_types = zip<int>::with_t<boost::numeric::ublas::layout::first_order>;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_einstein_multiplication, value,  test_types )
{
  namespace ublas = boost::numeric::ublas;

  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;
  using tensor_t  = ublas::tensor_dynamic<value_t,layout_t>;
  // NOLINTNEXTLINE(google-build-using-namespace)
  using namespace boost::numeric::ublas::index;

  {
    auto A = tensor_t(5,3);
    auto B = tensor_t{3,4};
    //  auto C = tensor_t{4,5,6};

    for(auto j = 0u; j < A.extents().at(1); ++j){
      for(auto i = 0u; i < A.extents().at(0); ++i){
        A.at( i,j ) = value_t( static_cast< inner_type_t<value_t> >(i+1) );
      }
    }

    for(auto j = 0u; j < B.extents().at(1); ++j){
      for(auto i = 0u; i < B.extents().at(0); ++i){
        B.at( i,j ) = value_t( static_cast< inner_type_t<value_t> >(i+1) );
      }
    }

    auto AB = A(_,_e) * B(_e,_);

    //  std::cout << "A = " << A << std::endl;
    //  std::cout << "B = " << B << std::endl;
    //  std::cout << "AB = " << AB << std::endl;

    for(auto j = 0u; j < AB.extents().at(1); ++j){
      for(auto i = 0u; i < AB.extents().at(0); ++i){
        auto e0   = B.extents().at(0);
        auto sum  = std::div(e0*(e0+1),2);
        auto quot = value_t(sum.quot);
        BOOST_CHECK_EQUAL( AB.at(i,j) , A.at(i,0)*quot );
      }
    }
  }

  {
    auto A = tensor_t{4,5,3};
    auto B = tensor_t{3,4,2};

    for(auto k = 0u; k < A.extents().at(2); ++k){
      for(auto j = 0u; j < A.extents().at(1); ++j){
        for(auto i = 0u; i < A.extents().at(0); ++i){
          A.at( i,j,k ) = value_t( static_cast< inner_type_t<value_t> >(i+1) );
        }
      }
    }

    for(auto k = 0u; k < B.extents().at(2); ++k){
      for(auto j = 0u; j < B.extents().at(1); ++j){
        for(auto i = 0u; i < B.extents().at(0); ++i){
          B.at( i,j,k ) = value_t( static_cast< inner_type_t<value_t> >(i+1) );
        }
      }
    }

    auto AB = A(_d,_,_f) * B(_f,_d,_);

    //  std::cout << "A = " << A << std::endl;
    //  std::cout << "B = " << B << std::endl;
    //  std::cout << "AB = " << AB << std::endl;
    // n*(n+1)/2;
    auto const nf = ( B.extents().at(0) * (B.extents().at(0)+1) / 2 );
    auto const nd = ( A.extents().at(0) * (A.extents().at(0)+1) / 2 );

    for(auto j = 0u; j < AB.extents().at(1); ++j){
      for(auto i = 0u; i < AB.extents().at(0); ++i){
        BOOST_CHECK_EQUAL( AB.at( i,j ) ,  value_t( static_cast< inner_type_t<value_t> >(nf * nd) ) );
      }
    }
  }

  {
    auto A = tensor_t{{4,3}};
    auto B = tensor_t{3,4,2};

    for(auto j = 0u; j < A.extents().at(1); ++j){
      for(auto i = 0u; i < A.extents().at(0); ++i){
        A.at( i,j ) = value_t( static_cast< inner_type_t<value_t> >(i+1) );
      }
    }

    for(auto k = 0u; k < B.extents().at(2); ++k){
      for(auto j = 0u; j < B.extents().at(1); ++j){
        for(auto i = 0u; i < B.extents().at(0); ++i){
          B.at( i,j,k ) = value_t( static_cast< inner_type_t<value_t> >(i+1) );
        }
      }
    }

    auto AB = A(_d,_f) * B(_f,_d,_);

    // n*(n+1)/2;
    auto const nf = ( B.extents().at(0) * (B.extents().at(0)+1) / 2 );
    auto const nd = ( A.extents().at(0) * (A.extents().at(0)+1) / 2 );

    for(auto i = 0u; i < AB.extents().at(0); ++i){
      BOOST_CHECK_EQUAL ( AB.at( i  ) ,  value_t( static_cast< inner_type_t<value_t> >(nf * nd) ) );
    }

  }
}

BOOST_AUTO_TEST_SUITE_END()


#include <cstddef>
#include <cstdint>

#include <vector>
#include <string>
#include <iterator>
#include <iostream>
#include <unordered_map>

#include <boost/graph/compressed_sparse_row_graph.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>

#include <boost/range/algorithm/for_each.hpp>

#include <z3++.h>

using Graph = boost::compressed_sparse_row_graph<>;
using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
using Edge = typename boost::graph_traits<Graph>::edge_descriptor;

Graph makeGraph() {
  const auto tag = boost::edges_are_unsorted_multi_pass;
  using VertexPair = std::pair<std::size_t, std::size_t>;

  /*
     1
    / \
   0 - 3
    \ /
     2
  */
  const std::size_t numVertices{4};
  std::vector<VertexPair> edges{{0, 1}, {1, 0}, {0, 2}, {2, 0}, {1, 3}, {3, 1}, {2, 3}, {3, 2}, {0, 3}, {3, 0}};

  return {tag, begin(edges), end(edges), numVertices};
}

void makeMaxFlow(const Graph& graph, const Vertex s, const Vertex t) {
  std::vector<std::uint8_t> capacities(num_edges(graph), 0);
  std::vector<std::uint8_t> residuals(num_edges(graph), 0);
  std::vector<Edge> reverse(num_edges(graph));

  auto capacityMap = boost::make_iterator_property_map(begin(capacities), get(boost::edge_index, graph));
  auto residualMap = boost::make_iterator_property_map(begin(residuals), get(boost::edge_index, graph));
  auto reverseMap = boost::make_iterator_property_map(begin(reverse), get(boost::edge_index, graph));

  for_each(edges(graph), [&](const auto edge) {
    const auto reverseEdge = lookup_edge(target(edge, graph), source(edge, graph), graph).first;
    put(reverseMap, edge, reverseEdge);

    put(capacityMap, edge, 1);
    put(residualMap, edge, 1);
  });

  const auto flow = edmonds_karp_max_flow(graph, s, t, capacity_map(capacityMap) //
                                                           .residual_capacity_map(residualMap)
                                                           .reverse_edge_map(reverseMap));

  std::cout << flow << std::endl;
}

void makeMaxFlowSolver(const Graph& graph, const Vertex s, const Vertex t) {
  z3::context context;
  z3::optimize solver{context};

  std::unordered_map<std::string, z3::expr> symbols;
  std::vector<z3::expr> constraints;

  const z3::expr zero{context.int_val(0)};

  const auto symbolize = [&](const auto edge) {
    const auto from = source(edge, graph);
    const auto to = target(edge, graph);
    return "x_" + std::to_string(from) + "_" + std::to_string(to);
  };

  boost::for_each(edges(graph), [&](const auto edge) {
    const auto edgeSym = symbolize(edge);
    symbols.emplace(edgeSym, context.int_const(edgeSym.c_str()));
  });

  boost::for_each(edges(graph), [&](const auto edge) {
    const auto edgeSym = symbolize(edge);
    constraints.emplace_back(symbols.at(edgeSym) >= 0);
    constraints.emplace_back(symbols.at(edgeSym) <= 1);
  });

  boost::for_each(vertices(graph), [&](const auto vertex) {
    if (vertex == s or vertex == t)
      return;

    z3::expr outgoing{zero}, incoming{zero};
    boost::for_each(out_edges(vertex, graph), [&](const auto edge) { outgoing = outgoing + symbols.at(symbolize(edge)); });

    std::vector<Edge> inEdges;
    boost::for_each(edges(graph), [&](const auto edge) {
      const auto to = target(edge, graph);
      if (to == vertex)
        inEdges.push_back(edge);
    });

    boost::for_each(inEdges, [&](const auto edge) { incoming = incoming + symbols.at(symbolize(edge)); });

    constraints.emplace_back(outgoing - incoming == 0);
  });

  z3::expr outgoing{zero}, incoming{zero};
  boost::for_each(out_edges(s, graph), [&](const auto edge) { outgoing = outgoing + symbols.at(symbolize(edge)); });

  std::vector<Edge> inEdges;
  boost::for_each(edges(graph), [&](const auto edge) {
    const auto to = target(edge, graph);
    if (to == s)
      inEdges.push_back(edge);
  });

  boost::for_each(inEdges, [&](const auto edge) { incoming = incoming + symbols.at(symbolize(edge)); });

  boost::for_each(constraints, [&](const auto& constraint) { std::cout << constraint << std::endl; });

  solver.maximize(outgoing - incoming);

  if (solver.check() not_eq z3::sat)
    return;

  const auto model = solver.get_model();
  std::cout << model << std::endl;
}

int main() {
  const auto graph = makeGraph();
  const Vertex source{0}, target{3};

  //makeMaxFlow(graph, source, target);
  makeMaxFlowSolver(graph, source, target);
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests for the trigonometric sine function of (fixed_point) for a small digit range.

#include <cmath>

#define BOOST_TEST_MODULE test_negatable_func_sine_small
#define BOOST_LIB_DIAGNOSTIC

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

BOOST_AUTO_TEST_CASE(test_negatable_func_sine_small)
{
  typedef boost::fixed_point::negatable<7, -24> fixed_point_type;
  typedef fixed_point_type::float_type          float_point_type;

  const fixed_point_type tol = ldexp(fixed_point_type(1), fixed_point_type::resolution + 7);

  using std::sin;

  // Check positive arguments.
  for(int i = 1; i < 64; ++i)
  {
    const fixed_point_type x = sin(fixed_point_type(i) / 10);
    const float_point_type y = sin(float_point_type(i) / 10);

    BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
  }

  // Check negative arguments.
  for(int i = 1; i < 64; ++i)
  {
    const fixed_point_type x = sin(fixed_point_type(-i) / 10);
    const float_point_type y = sin(float_point_type(-i) / 10);

    BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
  }

  const fixed_point_type local_pi_half = boost::fixed_point::negatable_constants<fixed_point_type>::pi_half();

  BOOST_CHECK_EQUAL(sin(+local_pi_half), fixed_point_type(+1));
  BOOST_CHECK_EQUAL(sin(-local_pi_half), fixed_point_type(-1));

  BOOST_CHECK_EQUAL(sin(fixed_point_type(0)), fixed_point_type(0));
}


#define BOOST_TEST_MODULE "test_flexible_local_dihedral_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/check_potential.hpp>
#include <mjolnir/forcefield/FLP/FlexibleLocalDihedralPotential.hpp>
#include <mjolnir/math/constants.hpp>

BOOST_AUTO_TEST_CASE(FlexibleLocalDihedral_double)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-5;
    constexpr real_type tol = 1e-5;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();

    const real_type k  = 1.0;
    const std::array<real_type, 7> term{{
        2.2056, 0.2183, -0.0795, 0.0451, -0.3169, 0.0165, -0.1375
    }};

    mjolnir::FlexibleLocalDihedralPotential<real_type> pot(k, term);

    const real_type x_min = -pi;
    const real_type x_max =  pi;

    mjolnir::test::check_potential(pot, x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(FlexibleLocalDihedral_float)
{
    using real_type = float;
    constexpr std::size_t N   = 100;
    constexpr real_type   h   = 1e-2;
    constexpr real_type   tol = 1e-2;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();

    const real_type k  = 1.0;
    const std::array<real_type, 7> term{{
        2.2056, 0.2183, -0.0795, 0.0451, -0.3169, 0.0165, -0.1375
    }};

    mjolnir::FlexibleLocalDihedralPotential<real_type> pot(k, term);

    const real_type x_min = -pi;
    const real_type x_max =  pi;

    mjolnir::test::check_potential(pot, x_min, x_max, tol, h, N);
}


#include "drake/common/test_utilities/eigen_geometry_compare.h"

#include <Eigen/Dense>

#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/common/text_logging.h"

namespace drake {

::testing::AssertionResult ExpectRotMat(const Eigen::Matrix3d& R,
                                        double tolerance) {
  // Don't have access to common EXPECT_NEAR low-level macros :(
  const double det = R.determinant();
  const double det_err = fabs(det - 1);
  if (det_err > tolerance) {
    return ::testing::AssertionFailure()
        << "Determinant of R = " << det << " != 1 by an error of "
        << det_err << "\nR = " << R;
  }
  return CompareMatrices(Eigen::Matrix3d::Identity(), R.transpose() * R,
                         tolerance)
      << "Rotation matrix is non-orthonormal";
}

::testing::AssertionResult CompareTransforms(
    const Eigen::Isometry3d &X_expected, const Eigen::Isometry3d &X_actual,
    double tolerance) {
  ::testing::AssertionResult check_R_expected =
      ExpectRotMat(X_expected.rotation(), tolerance);
  if (!check_R_expected) {
    return check_R_expected << "(X_expected)";
  }
  ::testing::AssertionResult check_R_actual =
      ExpectRotMat(X_actual.rotation(), tolerance);
  if (!check_R_actual) {
    return check_R_actual << "(X_actual)";
  }
  return CompareMatrices(X_expected.matrix(), X_actual.matrix(), tolerance);
}

}   // namespace drake


#include <NTL/GF2E.h>

#include <NTL/new.h>

NTL_START_IMPL

GF2EInfoT::GF2EInfoT(const GF2X& NewP)
{
   ref_count = 1;

   build(p, NewP);

   if (p.size == 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         KarCross = 4;
      else
         KarCross = 8;
   }
   else if (p.size == 2)
      KarCross = 8;
   else if (p.size <= 5)
      KarCross = 4;
   else if (p.size == 6)
      KarCross = 3;
   else
      KarCross = 2;

   if (p.size <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         ModCross = 20;
      else
         ModCross = 40;
   }
   else if (p.size <= 2)
      ModCross = 75;
   else if (p.size <= 4)
      ModCross = 50;
   else
      ModCross = 25;

   if (p.size == 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         DivCross = 100;
      else
         DivCross = 200;
   }
   else if (p.size == 2)
      DivCross = 400;
   else if (p.size <= 4)
      DivCross = 200;
   else if (p.size == 5)
      DivCross = 150;
   else if (p.size <= 13)
      DivCross = 100;
   else
      DivCross = 75;

   _card_init = 0;
   _card_exp = p.n;
}

const ZZ& GF2E::cardinality()
{
   if (!GF2EInfo) Error("GF2E::cardinality: undefined modulus");

   if (!GF2EInfo->_card_init) {
      power(GF2EInfo->_card, 2, GF2EInfo->_card_exp);
      GF2EInfo->_card_init = 1;
   }

   return GF2EInfo->_card;
}

GF2EInfoT *GF2EInfo = 0;

typedef GF2EInfoT *GF2EInfoPtr;

static
void CopyPointer(GF2EInfoPtr& dst, GF2EInfoPtr src)
{
   if (src == dst) return;

   if (dst) {
      dst->ref_count--;

      if (dst->ref_count < 0)
         Error("internal error: negative GF2EContext ref_count");

      if (dst->ref_count == 0) delete dst;
   }

   if (src) {
      if (src->ref_count == NTL_MAX_LONG)
         Error("internal error: GF2EContext ref_count overflow");

      src->ref_count++;

   }

   dst = src;
}

void GF2E::init(const GF2X& p)
{
   GF2EContext c(p);
   c.restore();
}

GF2EContext::GF2EContext(const GF2X& p)
{
   ptr = NTL_NEW_OP GF2EInfoT(p);
}

GF2EContext::GF2EContext(const GF2EContext& a)
{
   ptr = 0;
   CopyPointer(ptr, a.ptr);
}

GF2EContext& GF2EContext::operator=(const GF2EContext& a)
{
   CopyPointer(ptr, a.ptr);
   return *this;
}

GF2EContext::~GF2EContext()
{
   CopyPointer(ptr, 0);
}

void GF2EContext::save()
{
   CopyPointer(ptr, GF2EInfo);
}

void GF2EContext::restore() const
{
   CopyPointer(GF2EInfo, ptr);
}

GF2EBak::~GF2EBak()
{
   if (MustRestore)
      CopyPointer(GF2EInfo, ptr);

   CopyPointer(ptr, 0);
}

void GF2EBak::save()
{
   MustRestore = 1;
   CopyPointer(ptr, GF2EInfo);
}

void GF2EBak::restore()
{
   MustRestore = 0;
   CopyPointer(GF2EInfo, ptr);
}

const GF2E& GF2E::zero()
{
   static GF2E z(GF2E_NoAlloc);
   return z;
}

istream& operator>>(istream& s, GF2E& x)
{
   GF2X y;

   s >> y;
   conv(x, y);

   return s;
}

void div(GF2E& x, const GF2E& a, const GF2E& b)
{
   GF2E t;

   inv(t, b);
   mul(x, a, t);
}

void div(GF2E& x, GF2 a, const GF2E& b)
{
   inv(x, b);
   mul(x, x, a);
}

void div(GF2E& x, long a, const GF2E& b)
{
   inv(x, b);
   mul(x, x, a);
}

void inv(GF2E& x, const GF2E& a)
{
   InvMod(x._GF2E__rep, a._GF2E__rep, GF2E::modulus());
}

NTL_END_IMPL


#pragma once
/**

   @file Leadline.hpp
   @brief Basic leadline response.

*/

#include <atomic>
#include <chrono>
#include <cinttypes>
#include <memory>

#include <boost/statechart/detail/memory.hpp>
#include <boost/statechart/fifo_scheduler.hpp>
#include <boost/statechart/event_base.hpp>

#include <yaml-cpp/yaml.h>

#ifdef _MSC_VER
#pragma warning(push, 0)
#endif
#include <dds/pub/Publisher.hpp>
#include <dds/sub/Subscriber.hpp>
#ifdef _MSC_VER
#pragma warning(pop)
#endif

#include <mimir/IAlgorithm.hpp>

namespace mimir
{
  namespace algorithm
  {

    /**
       @brief Leadline predicted response.

       \rst

       This algorithm calculates an expected depth response of the leadline based on the
       provided input parameters. The output is the solution to the initial value problem
       over a prediction horizon of ``prediction_horizon_sec`` seconds, with
       discretization step equal to ``time_step_ms`` milliseconds.

       Let :math:`x(t) \in \mathbb{R}` be the depth in meters at a given time
       :math:`t`. Let :math:`x_d \in \mathbb{R}` and :math:`\tau \in \mathbb{R}_{>0}` be a
       setpoint depth [m] and time constant [s], respectively. Suppose :math:`t_f>0` is
       the prediction horizon and :math:`\delta t` is a descretization the step
       size. Define the set of discretized time points as :math:`\mathcal{T} := \{ t : t =
       k\delta t\, \forall k \in \mathbb{Z}_{\geq 0}, t \in [0, t_f] \}`. The solution to the
       initial value problem

       .. math::
          :nowrap:

          \begin{align}
          \dat x(t) &= \frac{x(t)-x_d}{\tau} \\
          x(0) &= 0
          \end{align}

       is :math:`x(t)` and the solution set :math:`\mathcal{X} := \{ z : z = x(t)\, \forall t \in \mathcal{T} \}`.

       The algorithm solution provides :math:`(\mathcal{T},\mathcal{X})` given :math:`(x_d,\tau)`.

       \endrst

    */
    class Leadline : public IAlgorithm
    {
    public:
      /**
         @brief Leadline constructor.

         The constructor parses the specification from the given YAML
         node. It establishes data structures and sets up DDS readers
         and writers according to the input/output scheme of the
         algorithm. The following YAML code block shows the expected
         layout of the ``Leadline`` map of a input config file.  The
         inputs, outputs, and initial conditions are communicated with
         DDS communication. Common is their DDS topic and DDS
         identifier. The specification is deemed self-explanatory.

         \rst

         .. code-block:: yaml

             time_step_ms: 200
             prediction_horizon_sec: 1000
             inputs:
               parameters:                   # DDS type: fkin::IdVec2d
                 topic: leadline_parameters
                 id: Leadline
                 default: [350, 160]
             outputs:                        # DDS type: fkin::BatchIdVec1d
               depth:
                 topic: leadline_response
                 id: Leadline

         \endrst

         @param [in] config YAML configuration from input file.
         @param [in] scheduler State machine scheduler, needed to post events to state machine.
         @param [in] machine State machine processor handle, needed to post events to state machine.
         @param publisher DDS data writer to send data.
         @param subscriber DDS data reader to receive data.
      */
      explicit Leadline(
          const YAML::Node& config,
          boost::statechart::fifo_scheduler<>& scheduler,
          boost::statechart::fifo_scheduler<>::processor_handle machine,
          dds::pub::Publisher publisher,
          dds::sub::Subscriber subscriber);
      /// Destructor.
      virtual ~Leadline();
      /// See base class.
      virtual void solve(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void initialize(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void timer(const std::atomic<bool>& cancel_token);
      /// Name identifier of algorithm.
      virtual inline const char* name() { return "Leadline"; }
      void event(boost::statechart::event_base * const event);

    private:
      /// March simulation time one time step ahead.
      inline void step_time() { m_next_step += m_time_step; m_now += m_time_step; }
      Leadline() = delete;
      class Impl;
      std::unique_ptr<Impl> m_impl; ///< Holds the implementation of the algorithm.
      boost::statechart::fifo_scheduler<> & m_scheduler; ///< Members for state machine.
      boost::statechart::fifo_scheduler<>::processor_handle m_stateMachine; ///< Member for state machine.
      const std::chrono::milliseconds m_time_step; ///< Discrete time step.
      std::chrono::steady_clock::time_point m_next_step; ///< Simulation time point.
      std::chrono::system_clock::time_point m_now;
      const YAML::Node m_config; ///< YAML configuration for algorithm.
    };

  }
}


// Copyright (C) 2015 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: Benjamin Nuernberger (bnuernberger@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_dominant_plane_from_points.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <limits>
#include <memory>
#include <vector>

#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/pose/util.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Vector3d;

// An estimator for computing a dominant plane from a set of 3D points.
class DominantPlaneEstimator : public Estimator<Vector3d, Plane> {
 public:
  DominantPlaneEstimator() {}

  // 3 non-collinear points are needed to determine a plane.
  double SampleSize() const { return 3; }

  // Estimates candidate dominant planes from three 3D points.
  bool EstimateModel(const std::vector<Vector3d>& points,
                     std::vector<Plane>* planes) const {
    // If the points are collinear, there are no possible solutions.
    static const double kTolerance = 1e-6;
    const Vector3d a = points[1] - points[0];
    const Vector3d b = points[2] - points[0];
    const Vector3d cross = a.cross(b);
    if (cross.squaredNorm() < kTolerance) {
      VLOG(3) << "The 3 world points are collinear! No solution for a plane "
                 "exists.";
      return false;
    }
    Plane plane;
    plane.point = points[0];
    plane.unit_normal = cross.normalized();

    planes->emplace_back(plane);
    return true;
  }

  // The error for a point given a plane model is the point-to-plane distance.
  double Error(const Vector3d& point, const Plane& plane) const {
    return std::abs(plane.unit_normal.dot(point - plane.point));
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(DominantPlaneEstimator);
};

}  // namespace

bool EstimateDominantPlaneFromPoints(const RansacParameters& ransac_params,
                                     const RansacType& ransac_type,
                                     const std::vector<Vector3d>& points,
                                     Plane* plane,
                                     RansacSummary* ransac_summary) {
  DominantPlaneEstimator dominant_plane_estimator;
  std::unique_ptr<SampleConsensusEstimator<DominantPlaneEstimator> > ransac =
      CreateAndInitializeRansacVariant(
          ransac_type, ransac_params, dominant_plane_estimator);
  // Estimate the dominant plane.
  return ransac->Estimate(points, plane, ransac_summary);
}

}  // namespace theia


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include "calc/geometry.hpp"

using namespace calc;

BOOST_AUTO_TEST_SUITE( geometryTest )

BOOST_AUTO_TEST_CASE(FailTest)
{
  BOOST_CHECK_NE(5, geometry::sqr(2));
}

BOOST_AUTO_TEST_CASE(PassTest)
{
  BOOST_CHECK_EQUAL(4, geometry::sqr(2));
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

//____________________________________________________________________________//

BOOST_AUTO_TEST_CASE( test )
{
    double v1 = 1.23456e28;
    double v2 = 1.23457e28;

    BOOST_REQUIRE_CLOSE( v1, v2, 0.001 );
    // Absolute value of difference between these two values is 1e+23.
    // But we are interested only that it does not exeed 0.001% of a values compared
    // And this test will pass.
}

//____________________________________________________________________________//


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "ViewerCore.h"
#include "gl.h"
#include "../quat_to_mat.h"
#include "../snap_to_fixed_up.h"
#include "../look_at.h"
#include "../frustum.h"
#include "../ortho.h"
#include "../massmatrix.h"
#include "../barycenter.h"
#include "../PI.h"
#include <Eigen/Geometry>
#include <iostream>

IGL_INLINE void igl::opengl::ViewerCore::align_camera_center(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F)
{
  if(V.rows() == 0)
    return;

  get_scale_and_shift_to_fit_mesh(V,F,model_zoom,model_translation);
  // Rather than crash on empty mesh...
  if(V.size() > 0)
  {
    object_scale = (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm();
  }
}

IGL_INLINE void igl::opengl::ViewerCore::get_scale_and_shift_to_fit_mesh(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F,
  float& zoom,
  Eigen::Vector3f& shift)
{
  if (V.rows() == 0)
    return;

  Eigen::MatrixXd BC;
  if (F.rows() <= 1)
  {
    BC = V;
  } else
  {
    igl::barycenter(V,F,BC);
  }
  return get_scale_and_shift_to_fit_mesh(BC,zoom,shift);
}

IGL_INLINE void igl::opengl::ViewerCore::align_camera_center(
  const Eigen::MatrixXd& V)
{
  if(V.rows() == 0)
    return;

  get_scale_and_shift_to_fit_mesh(V,model_zoom,model_translation);
  // Rather than crash on empty mesh...
  if(V.size() > 0)
  {
    object_scale = (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm();
  }
}

IGL_INLINE void igl::opengl::ViewerCore::get_scale_and_shift_to_fit_mesh(
  const Eigen::MatrixXd& V,
  float& zoom,
  Eigen::Vector3f& shift)
{
  if (V.rows() == 0)
    return;

  auto min_point = V.colwise().minCoeff();
  auto max_point = V.colwise().maxCoeff();
  auto centroid  = (0.5*(min_point + max_point)).eval();
  shift.setConstant(0);
  shift.head(centroid.size()) = -centroid.cast<float>();
  zoom = 2.0 / (max_point-min_point).array().abs().maxCoeff();
}

IGL_INLINE void igl::opengl::ViewerCore::clear_framebuffers()
{
  glClearColor(background_color[0],
               background_color[1],
               background_color[2],
               1.0f);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}

IGL_INLINE void igl::opengl::ViewerCore::draw(
  ViewerData& data,
  bool update_matrices)
{
  using namespace std;
  using namespace Eigen;

  if (depth_test)
    glEnable(GL_DEPTH_TEST);
  else
    glDisable(GL_DEPTH_TEST);

  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  /* Bind and potentially refresh mesh/line/point data */
  if (data.dirty)
  {
    data.updateGL(data, data.invert_normals,data.meshgl);
    data.dirty = MeshGL::DIRTY_NONE;
  }
  data.meshgl.bind_mesh();

  // Initialize uniform
  glViewport(viewport(0), viewport(1), viewport(2), viewport(3));

  if(update_matrices)
  {
    model = Eigen::Matrix4f::Identity();
    view  = Eigen::Matrix4f::Identity();
    proj  = Eigen::Matrix4f::Identity();

    // Set view
    look_at( camera_eye, camera_center, camera_up, view);

    float width  = viewport(2);
    float height = viewport(3);

    // Set projection
    if (orthographic)
    {
      float length = (camera_eye - camera_center).norm();
      float h = tan(camera_view_angle/360.0 * igl::PI) * (length);
      ortho(-h*width/height, h*width/height, -h, h, camera_dnear, camera_dfar,proj);
    }
    else
    {
      float fH = tan(camera_view_angle / 360.0 * igl::PI) * camera_dnear;
      float fW = fH * (double)width/(double)height;
      frustum(-fW, fW, -fH, fH, camera_dnear, camera_dfar,proj);
    }
    // end projection

    // Set model transformation
    float mat[16];
    igl::quat_to_mat(trackball_angle.coeffs().data(), mat);

    for (unsigned i=0;i<4;++i)
      for (unsigned j=0;j<4;++j)
        model(i,j) = mat[i+4*j];

    // Why not just use Eigen::Transform<double,3,Projective> for model...?
    model.topLeftCorner(3,3)*=camera_zoom;
    model.topLeftCorner(3,3)*=model_zoom;
    model.col(3).head(3) += model.topLeftCorner(3,3)*model_translation;
  }

  // Send transformations to the GPU
  GLint modeli = glGetUniformLocation(data.meshgl.shader_mesh,"model");
  GLint viewi  = glGetUniformLocation(data.meshgl.shader_mesh,"view");
  GLint proji  = glGetUniformLocation(data.meshgl.shader_mesh,"proj");
  glUniformMatrix4fv(modeli, 1, GL_FALSE, model.data());
  glUniformMatrix4fv(viewi, 1, GL_FALSE, view.data());
  glUniformMatrix4fv(proji, 1, GL_FALSE, proj.data());

  // Light parameters
  GLint specular_exponenti    = glGetUniformLocation(data.meshgl.shader_mesh,"specular_exponent");
  GLint light_position_worldi = glGetUniformLocation(data.meshgl.shader_mesh,"light_position_world");
  GLint lighting_factori      = glGetUniformLocation(data.meshgl.shader_mesh,"lighting_factor");
  GLint fixed_colori          = glGetUniformLocation(data.meshgl.shader_mesh,"fixed_color");
  GLint texture_factori       = glGetUniformLocation(data.meshgl.shader_mesh,"texture_factor");

  glUniform1f(specular_exponenti, data.shininess);
  Vector3f rev_light = -1.*light_position;
  glUniform3fv(light_position_worldi, 1, rev_light.data());
  glUniform1f(lighting_factori, lighting_factor); // enables lighting
  glUniform4f(fixed_colori, 0.0, 0.0, 0.0, 0.0);

  if (data.V.rows()>0)
  {
    // Render fill
    if (data.show_faces)
    {
      // Texture
      glUniform1f(texture_factori, data.show_texture ? 1.0f : 0.0f);
      data.meshgl.draw_mesh(true);
      glUniform1f(texture_factori, 0.0f);
    }

    // Render wireframe
    if (data.show_lines)
    {
      glLineWidth(data.line_width);
      glUniform4f(fixed_colori,
        data.line_color[0],
        data.line_color[1],
        data.line_color[2], 1.0f);
      data.meshgl.draw_mesh(false);
      glUniform4f(fixed_colori, 0.0f, 0.0f, 0.0f, 0.0f);
    }
  }

  if (data.show_overlay)
  {
    if (data.show_overlay_depth)
      glEnable(GL_DEPTH_TEST);
    else
      glDisable(GL_DEPTH_TEST);

    if (data.lines.rows() > 0)
    {
      data.meshgl.bind_overlay_lines();
      modeli = glGetUniformLocation(data.meshgl.shader_overlay_lines,"model");
      viewi  = glGetUniformLocation(data.meshgl.shader_overlay_lines,"view");
      proji  = glGetUniformLocation(data.meshgl.shader_overlay_lines,"proj");

      glUniformMatrix4fv(modeli, 1, GL_FALSE, model.data());
      glUniformMatrix4fv(viewi, 1, GL_FALSE, view.data());
      glUniformMatrix4fv(proji, 1, GL_FALSE, proj.data());
      // This must be enabled, otherwise glLineWidth has no effect
      glEnable(GL_LINE_SMOOTH);
      glLineWidth(data.line_width);

      data.meshgl.draw_overlay_lines();
    }

    if (data.points.rows() > 0)
    {
      data.meshgl.bind_overlay_points();
      modeli = glGetUniformLocation(data.meshgl.shader_overlay_points,"model");
      viewi  = glGetUniformLocation(data.meshgl.shader_overlay_points,"view");
      proji  = glGetUniformLocation(data.meshgl.shader_overlay_points,"proj");

      glUniformMatrix4fv(modeli, 1, GL_FALSE, model.data());
      glUniformMatrix4fv(viewi, 1, GL_FALSE, view.data());
      glUniformMatrix4fv(proji, 1, GL_FALSE, proj.data());
      glPointSize(data.point_size);

      data.meshgl.draw_overlay_points();
    }

    glEnable(GL_DEPTH_TEST);
  }

}

IGL_INLINE void igl::opengl::ViewerCore::draw_buffer(ViewerData& data,
  bool update_matrices,
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& R,
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& G,
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& B,
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& A)
{
  assert(R.rows() == G.rows() && G.rows() == B.rows() && B.rows() == A.rows());
  assert(R.cols() == G.cols() && G.cols() == B.cols() && B.cols() == A.cols());

  unsigned x = R.rows();
  unsigned y = R.cols();

  // Create frame buffer
  GLuint frameBuffer;
  glGenFramebuffers(1, &frameBuffer);
  glBindFramebuffer(GL_FRAMEBUFFER, frameBuffer);

  // Create texture to hold color buffer
  GLuint texColorBuffer;
  glGenTextures(1, &texColorBuffer);
  glBindTexture(GL_TEXTURE_2D, texColorBuffer);

  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, x, y, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);

  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

  glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, texColorBuffer, 0);

  // Create Renderbuffer Object to hold depth and stencil buffers
  GLuint rboDepthStencil;
  glGenRenderbuffers(1, &rboDepthStencil);
  glBindRenderbuffer(GL_RENDERBUFFER, rboDepthStencil);
  glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH24_STENCIL8, x, y);
  glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_STENCIL_ATTACHMENT, GL_RENDERBUFFER, rboDepthStencil);

  assert(glCheckFramebufferStatus(GL_FRAMEBUFFER) == GL_FRAMEBUFFER_COMPLETE);

  glBindFramebuffer(GL_FRAMEBUFFER, frameBuffer);

  // Clear the buffer
  glClearColor(background_color(0), background_color(1), background_color(2), 0.f);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  // Save old viewport
  Eigen::Vector4f viewport_ori = viewport;
  viewport << 0,0,x,y;

  // Draw
  draw(data,update_matrices);

  // Restore viewport
  viewport = viewport_ori;

  // Copy back in the given Eigen matrices
  GLubyte* pixels = (GLubyte*)calloc(x*y*4,sizeof(GLubyte));
  glReadPixels
  (
   0, 0,
   x, y,
   GL_RGBA, GL_UNSIGNED_BYTE, pixels
   );

  int count = 0;
  for (unsigned j=0; j<y; ++j)
  {
    for (unsigned i=0; i<x; ++i)
    {
      R(i,j) = pixels[count*4+0];
      G(i,j) = pixels[count*4+1];
      B(i,j) = pixels[count*4+2];
      A(i,j) = pixels[count*4+3];
      ++count;
    }
  }

  // Clean up
  free(pixels);
  glBindFramebuffer(GL_FRAMEBUFFER, 0);
  glDeleteRenderbuffers(1, &rboDepthStencil);
  glDeleteTextures(1, &texColorBuffer);
  glDeleteFramebuffers(1, &frameBuffer);
}

IGL_INLINE void igl::opengl::ViewerCore::set_rotation_type(
  const igl::opengl::ViewerCore::RotationType & value)
{
  using namespace Eigen;
  using namespace std;
  const RotationType old_rotation_type = rotation_type;
  rotation_type = value;
  if(rotation_type == ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP &&
    old_rotation_type != ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP)
  {
    snap_to_fixed_up(Quaternionf(trackball_angle),trackball_angle);
  }
}

IGL_INLINE igl::opengl::ViewerCore::ViewerCore()
{
  // Default colors
  background_color << 0.3f, 0.3f, 0.5f, 1.0f;

  // Default lights settings
  light_position << 0.0f, -0.30f, -5.0f;
  lighting_factor = 1.0f; //on

  // Default trackball
  trackball_angle = Eigen::Quaternionf::Identity();
  set_rotation_type(ViewerCore::ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP);

  // Defalut model viewing parameters
  model_zoom = 1.0f;
  model_translation << 0,0,0;

  // Camera parameters
  camera_zoom = 1.0f;
  orthographic = false;
  camera_view_angle = 45.0;
  camera_dnear = 1.0;
  camera_dfar = 100.0;
  camera_eye << 0, 0, 5;
  camera_center << 0, 0, 0;
  camera_up << 0, 1, 0;

  depth_test = true;

  is_animating = false;
  animation_max_fps = 30.;

  viewport.setZero();
}

IGL_INLINE void igl::opengl::ViewerCore::init()
{
}

IGL_INLINE void igl::opengl::ViewerCore::shut()
{
}


#pragma once
#ifndef CANNON_MATH_LATTICE_POINTS_H
#define CANNON_MATH_LATTICE_POINTS_H

/*!
 * \file cannon/math/lattice_points.hpp
 * \brief File containing utilities for working with points making up a lattice
 * in arbitrary real vector spaces.
 */

#include <queue>

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace math {

      using VectorXu = Matrix<unsigned int, Dynamic, 1>;

      /*!
       * \brief Generate all lattice points on an integer-valued grid.
       *
       * \param dim Which dimension of the lattice to generate.
       * \param sizes Number of elements in each dimension of the grid.
       *
       * \return Lattice points for dimensions 0 through dim-1
       */
      std::vector<VectorXu> make_lattice_points(unsigned int dim, const VectorXu& sizes) {
        std::vector<VectorXu> ret_vec;

        if (dim == 1) {
          for (unsigned int i = 0; i < sizes[dim-1]; i++) {
            VectorXu tmp = VectorXu::Zero(dim);
            tmp[0] = i;
            ret_vec.push_back(tmp);
          }
        } else {
          for (unsigned int i = 0; i < sizes[dim-1]; i++) {
            auto smaller_vec = make_lattice_points(dim-1, sizes);
            for (const auto& coord : smaller_vec) {
              VectorXu tmp = VectorXu::Zero(dim);
              tmp.head(dim-1) = coord;
              tmp[dim-1] = i;

              ret_vec.push_back(tmp);
            }
          }
        }

        return ret_vec;
      }

      /*!
       * \brief Generate all lattice points on an integer-valued grid.
       *
       * \param sizes Number of elements in each dimension of the grid.
       *
       * \return Lattice points for the grid.
       */
      std::vector<VectorXu> make_lattice_points(const VectorXu& sizes) {
        return make_lattice_points(sizes.size(), sizes);
      }

  } // namespace math
} // namespace cannon

#endif /* ifndef CANNON_MATH_LATTICE_POINTS_H */


/* test_exponential_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id: test_exponential_distribution.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/exponential_distribution.hpp>

#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::exponential_distribution<>
#define BOOST_RANDOM_ARG1 lambda
#define BOOST_RANDOM_ARG1_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0.0

#define BOOST_RANDOM_TEST2_PARAMS (1000.0)
#define BOOST_RANDOM_TEST2_MIN 0.0

#include "test_distribution.ipp"


#ifndef STAN_MATH_PRIM_SCAL_FUN_CHOOSE_HPP
#define STAN_MATH_PRIM_SCAL_FUN_CHOOSE_HPP

#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <limits>
#include <cmath>

namespace stan {
namespace math {

/**
 * Return the binomial coefficient for the specified integer
 * arguments.
 *
 * The binomial coefficient, \f${n \choose k}\f$, read "n choose k", is
 * defined for \f$0 \leq k \leq n\f$ (otherwise return 0) by
 *
 * \f${n \choose k} = \frac{n!}{k! (n-k)!}\f$.
 *
 * @param n total number of objects
 * @param k number of objects chosen
 * @return n choose k or 0 iff k > n
 * @throw std::domain_error if either argument is negative or the
 * result will not fit in an int type
 */
inline int choose(int n, int k) {
  check_nonnegative("choose", "n", n);
  check_nonnegative("choose", "k", k);
  if (k > n)
    return 0;
  const double choices = boost::math::binomial_coefficient<double>(n, k);
  check_less_or_equal("choose", "n choose k", choices,
                      std::numeric_limits<int>::max());
  return static_cast<int>(std::round(choices));
}

}  // namespace math
}  // namespace stan
#endif


#include <NTL/ZZ.h>
#include "filereader.h"
#include "generalhelpers.h"
#include "hashing.h"
#include "signing.h"
#include <fstream>
#include <sstream>

int ProcessSinging(const CryptoHelpers::SCommandLineOptions& options);
int ProcessChecking(const CryptoHelpers::SCommandLineOptions& options);

int main(int argc, char *argv[])
{
    CryptoHelpers::SCommandLineOptions options;

    options.m_filename = argv[2];
    if(strcmp(argv[1], "-sign")==0)
    {
        options.m_mode = CryptoHelpers::Signing;
    } else if(strcmp(argv[1], "-check") == 0)
    {
        if(argc<3)
        {
            return 0;
        }
        options.m_mode = CryptoHelpers::Checking;
        options.m_fileWithSignature = argv[3];
    }

    if(options.m_mode==CryptoHelpers::Signing)
    {
        ProcessSinging(options);
    }
    else
    {
        ProcessChecking(options);
    }
}

int ProcessSinging(const CryptoHelpers::SCommandLineOptions& options)
{
    std::vector<char> fileData;
    std::ofstream outputFile(options.m_filename + ".sig");
    outputFile << "------------------------------\n";
    outputFile << options.m_filename << std::endl;
    if (!CryptoHelpers::ReadFile(options.m_filename, fileData))
    {
        return 1;
    }

    unsigned long long hashValue = 0;

    if (fileData.size() != 0)
    {
        std::vector<unsigned long long> data;
        CryptoHelpers::ConvertByteToLong(fileData, data);
        CryptoHelpers::Hash(data, hashValue);
    }
    outputFile << "H = " << std::hex << hashValue << '\n';

    NTL::ZZ signature(0);
    NTL::ZZ y(0);
    NTL::ZZ k(0);
    NTL::ZZ g(0);
    NTL::ZZ z(0);
    NTL::ZZ u(0);
    NTL::ZZ x(0);
    CryptoHelpers::Sign(hashValue,signature,y,k,g,u,z,x);
    std::string tmp;
    CryptoHelpers::ConvertLongToString(y, tmp);
    outputFile << "Y = " << tmp << std::endl;
    CryptoHelpers::ConvertLongToString(k, tmp);
    outputFile << "K = " << tmp << std::endl;
    CryptoHelpers::ConvertLongToString(signature, tmp);
    outputFile << "S = " << tmp << std::endl;
    outputFile << "------------------------------\n";

    std::ofstream outputadd(options.m_filename + ".sig.add");
    outputadd << "------------------------------\n";
    outputadd << options.m_filename << std::endl;
    CryptoHelpers::ConvertLongToString(u, tmp);
    outputadd << "U = " << tmp << std::endl;
    CryptoHelpers::ConvertLongToString(z, tmp);
    outputadd << "Z = " << tmp << std::endl;
    CryptoHelpers::ConvertLongToString(g, tmp);
    outputadd << "G = " << tmp << std::endl;
    outputadd << "------------------------------\n";

    return 0;
}

int ProcessChecking(const CryptoHelpers::SCommandLineOptions& options)
{
    std::vector<char> fileData;
    if (!CryptoHelpers::ReadFile(options.m_filename, fileData))
    {
        return 1;
    }

    unsigned long long hashValue = 0;

    if (fileData.size() != 0)
    {
        std::vector<unsigned long long> data;
        CryptoHelpers::ConvertByteToLong(fileData, data);
        CryptoHelpers::Hash(data, hashValue);
    }

    NTL::ZZ signature(0);
    NTL::ZZ y(0);
    NTL::ZZ k(0);
    NTL::ZZ g(0);
    NTL::ZZ z(0);
    NTL::ZZ u(0);
    NTL::ZZ x(0);
    CryptoHelpers::Sign(hashValue, signature, y, k, g, u, z, x);

    std::ifstream fileWithSignature(options.m_fileWithSignature);
    std::string _tmp;
    std::getline(fileWithSignature, _tmp);
    std::getline(fileWithSignature, _tmp);
    std::getline(fileWithSignature, _tmp);
    _tmp = std::move(_tmp.substr(4, _tmp.size() - 4));
    size_t fileHash = 0;
    std::stringstream sstream;
    sstream << std::hex << hashValue;
    std::string _hashStr = sstream.str();

    if (_tmp.compare(_hashStr)!=0)
    {
        std::cout << "Wrong hash value!\n";
        return 2;
    }

    std::getline(fileWithSignature, _tmp);
    _tmp = std::move(_tmp.substr(4, _tmp.size() - 4));
    CryptoHelpers::ConvertHexStringToLong(_tmp, y);

    std::getline(fileWithSignature, _tmp);
    _tmp = std::move(_tmp.substr(4, _tmp.size() - 4));
    CryptoHelpers::ConvertHexStringToLong(_tmp, k);

    std::getline(fileWithSignature, _tmp);
    _tmp = std::move(_tmp.substr(4, _tmp.size() - 4));
    CryptoHelpers::ConvertHexStringToLong(_tmp, signature);

    if(CryptoHelpers::CheckSignature(hashValue,signature,y,k))
    {
        std::cout << "Signature is correct!\n";
    }
    else
    {
        std::cout << "Signature is wrong!\n";
    }

    return 0;
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int n, a, b; cin >> n >> a >> b;
    if ((b - a) % 2 == 0) cout << (b - a) / 2 << endl;
    else cout << min((a - 1), (n - b)) + 1 + (b - a - 1) / 2 << endl;
}