#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <array>
#include <utility>

#include "hexad.hpp"

using ternary::Hexad;
namespace bdata = boost::unit_test::data;

struct HexadFixture
{
    HexadFixture() = default;
    ~HexadFixture() = default;

    Hexad smallPositive { 10 };
    Hexad smallNegative { -4 };

    Hexad largePositive { 255 };
    Hexad largeNegative { -321 };

    // Identifiable pattern: +0-+0-
    Hexad tritPattern { 224 };
};

// Note that these are low to high
constexpr std::array<int, 6> tritsInPattern { -1, 0, 1, -1, 0, 1 };

BOOST_FIXTURE_TEST_SUITE(hexad, HexadFixture)

    BOOST_AUTO_TEST_SUITE(hexad_basic_functions)

    BOOST_AUTO_TEST_CASE(default_constructor)
    {
        Hexad h;
        BOOST_TEST(h.get() == 0);
        BOOST_TEST(h.value_string() == "00");
    }

    BOOST_AUTO_TEST_CASE(constructor_with_value)
    {
        Hexad h { 123 };
        BOOST_TEST(h.get() == 123);
    }

    BOOST_AUTO_TEST_CASE(trit_string)
    {
        auto a { smallPositive.trit_string() };
        BOOST_TEST(a.length() == 6);
        BOOST_TEST(a == "000+0+");
    }

    BOOST_AUTO_TEST_CASE(trit_array)
    {
        auto a { smallNegative.trits() };
        BOOST_TEST(a.size() == 6);
        BOOST_TEST(a[0] == -1);
        BOOST_TEST(a[1] == -1);
        BOOST_TEST(a[2] == 0);
    }

    BOOST_AUTO_TEST_CASE(value_string)
    {
        BOOST_TEST(smallPositive.value_string() == "0J");
        BOOST_TEST(smallNegative.value_string() == "0q");
        BOOST_TEST(largePositive.value_string() == "IL");
        BOOST_TEST(largeNegative.value_string() == "yC");
    }

    BOOST_AUTO_TEST_SUITE_END()

    BOOST_AUTO_TEST_SUITE(hexad_operations_arithmetic)

    BOOST_AUTO_TEST_CASE(operation_add)
    {
        BOOST_TEST(add(smallPositive, largePositive).get() == smallPositive.get() + largePositive.get());
        BOOST_TEST(add(smallNegative, largeNegative).get() == smallNegative.get() + largeNegative.get());
        BOOST_TEST(add(smallPositive, smallNegative).get() == smallPositive.get() + smallNegative.get());
        BOOST_TEST(add(smallNegative, largePositive).get() == smallNegative.get() + largePositive.get());
    }

    BOOST_AUTO_TEST_CASE(operation_subtract)
    {
        BOOST_TEST(subtract(smallPositive, largePositive).get() == smallPositive.get() - largePositive.get());
        BOOST_TEST(subtract(smallNegative, largeNegative).get() == smallNegative.get() - largeNegative.get());
        BOOST_TEST(subtract(smallPositive, smallNegative).get() == smallPositive.get() - smallNegative.get());
        BOOST_TEST(subtract(smallNegative, largePositive).get() == smallNegative.get() - largePositive.get());
    }

    BOOST_AUTO_TEST_CASE(operation_add_with_carry)
    {
        auto r { Hexad::range };
        BOOST_TEST(add_with_carry(smallPositive, smallNegative).first.get() == smallPositive.get() + smallNegative.get());
        BOOST_TEST(add_with_carry(largePositive, largePositive).second == 1);
        BOOST_TEST(add_with_carry(largeNegative, largeNegative).first.get() - r == largeNegative.get() + largeNegative.get());
    }

    BOOST_AUTO_TEST_CASE(operation_subtract_with_carry)
    {
        BOOST_TEST(subtract_with_carry(smallPositive, smallNegative).first.get() == smallPositive.get() - smallNegative.get());
        BOOST_TEST(subtract_with_carry(largePositive, largeNegative).second == 1);
    }

    BOOST_AUTO_TEST_CASE(operation_multiply)
    {
        BOOST_TEST(multiply(smallPositive, smallNegative).first.get() == smallPositive.get() * smallNegative.get());

        auto mp { multiply(largePositive, smallPositive) };
        auto r { Hexad::range };
        BOOST_TEST(mp.second.get() * r + mp.first.get() == largePositive.get() * smallPositive.get());
    }

    BOOST_AUTO_TEST_CASE(operation_divide_single_precision)
    {
        auto d1 { divide(largePositive, smallPositive) };
        BOOST_TEST(d1.first.get() == largePositive.get() / smallPositive.get());
        BOOST_TEST(d1.second.get() == largePositive.get() % smallPositive.get());

        auto d2 { divide(largePositive, smallNegative) };
        BOOST_TEST(d2.first.get() == largePositive.get() / smallNegative.get());
        BOOST_TEST(d2.second.get() == largePositive.get() % smallNegative.get());

        auto d3 { divide(largeNegative, smallPositive) };
        BOOST_TEST(d3.first.get() == largeNegative.get() / smallPositive.get());
        BOOST_TEST(d3.second.get() == largeNegative.get() % smallPositive.get());

        auto d4 { divide(largeNegative, smallNegative) };
        BOOST_TEST(d4.first.get() == largeNegative.get() / smallNegative.get());
        BOOST_TEST(d4.second.get() == largeNegative.get() % smallNegative.get());
    }

    BOOST_AUTO_TEST_CASE(operation_divide_double_precision)
    {
        std::pair<Hexad, Hexad> hh { {0}, {1} };
        auto hhvalue { hh.second.get() * Hexad::range + hh.first.get() };

        BOOST_TEST(divide(hh, smallPositive).first.get() == hhvalue / smallPositive.get());
        BOOST_TEST(divide(hh, smallNegative).first.get() == hhvalue / smallNegative.get());
    }

    BOOST_AUTO_TEST_SUITE_END()

    BOOST_AUTO_TEST_SUITE(hexad_operations_logical)

    BOOST_AUTO_TEST_CASE(operation_left_shift_single)
    {
        auto r { Hexad::range };
        BOOST_TEST(left_shift(smallPositive).get() == smallPositive.get() * 3);
        BOOST_TEST(left_shift(largePositive).get() == largePositive.get() * 3 - r);
        BOOST_TEST(left_shift(largeNegative).get() == largeNegative.get() * 3 + r);
    }

    BOOST_AUTO_TEST_CASE(operation_right_shift_single)
    {
        BOOST_TEST(right_shift(smallPositive).get() == smallPositive.get() / 3);
        BOOST_TEST(right_shift(largePositive).get() == largePositive.get() / 3);
        BOOST_TEST(right_shift(largeNegative).get() == largeNegative.get() / 3);
    }

    BOOST_AUTO_TEST_CASE(operation_left_shift_multiple)
    {
        for (auto i = 1u; i <= Hexad::width; ++i)
        {
            BOOST_TEST(left_shift(tritPattern, i).second == tritsInPattern[Hexad::width - i]);
        }
    }

    BOOST_AUTO_TEST_CASE(operation_right_shift_multiple)
    {
        for (auto i = 1u; i <= Hexad::width; ++i)
        {
            BOOST_TEST(right_shift(tritPattern, i).second == tritsInPattern[i - 1]);
        }
    }

    BOOST_AUTO_TEST_CASE(operation_left_rotate)
    {
        for (auto i = 1u; i <= Hexad::width; ++i)
        {
            BOOST_TEST(rotate_left(tritPattern, i).trits()[0] == tritsInPattern[Hexad::width - i]);
        }
    }

    BOOST_AUTO_TEST_CASE(operation_right_rotate)
    {
        for (auto i = 1u; i <= Hexad::width; ++i)
        {
            BOOST_TEST(rotate_right(tritPattern, i).trits()[0] == tritsInPattern[i % Hexad::width]);
        }
    }

    BOOST_AUTO_TEST_CASE(operation_left_rotate_carry)
    {
        for (auto i = 1u; i <= Hexad::width; ++i)
        {
            BOOST_TEST(rotate_left_carry(tritPattern, 0, i).second == tritsInPattern[Hexad::width - i]);

        }
    }

    BOOST_AUTO_TEST_CASE(operation_right_rotate_carry)
    {
        for (auto i = 1u; i <= Hexad::width; ++i)
        {
            BOOST_TEST(rotate_right_carry(tritPattern, 0, i).second == tritsInPattern[i - 1]);

        }
    }

    BOOST_AUTO_TEST_CASE(operation_simple_invert)
    {
        BOOST_TEST(invert(smallPositive).get() == -smallPositive.get());
        BOOST_TEST(invert(largeNegative).get() == -largeNegative.get());
    }

    BOOST_AUTO_TEST_CASE(operation_positive_invert)
    {
        auto inv1 { positive_invert(smallPositive) };
        auto inv2 { positive_invert(largeNegative) };

        for (auto i = 0u; i < Hexad::width; ++i)
        {
            if (smallPositive.trits()[i] == 1)
            {
                BOOST_TEST(inv1.trits()[i] == -1);
            }
            else
            {
                BOOST_TEST(inv1.trits()[i] == 1);
            }

            if (largeNegative.trits()[i] == 1)
            {
                BOOST_TEST(inv2.trits()[i] == -1);
            }
            else
            {
                BOOST_TEST(inv2.trits()[i] == 1);
            }
        }
    }

    BOOST_AUTO_TEST_CASE(operation_negative_invert)
    {
        auto inv1 { negative_invert(smallPositive) };
        auto inv2 { negative_invert(largeNegative) };

        for (auto i = 0u; i < Hexad::width; ++i)
        {
            if (smallPositive.trits()[i] == -1)
            {
                BOOST_TEST(inv1.trits()[i] == -1);
            }
            else
            {
                BOOST_TEST(inv1.trits()[i] == -1);
            }

            if (largeNegative.trits()[i] == -1)
            {
                BOOST_TEST(inv2.trits()[i] == 1);
            }
            else
            {
                BOOST_TEST(inv2.trits()[i] == -1);
            }
        }
    }

    BOOST_AUTO_TEST_CASE(operation_forward_diode)
    {
        auto d1 { forward_diode(smallPositive) };
        auto d2 { forward_diode(largeNegative) };

        for (auto i = 0u; i < Hexad::width; ++i)
        {
            if (smallPositive.trits()[i] == 1)
            {
                BOOST_TEST(d1.trits()[i] == 1);
            }
            else
            {
                BOOST_TEST(d1.trits()[i] == 0);
            }

            if (largeNegative.trits()[i] == 1)
            {
                BOOST_TEST(d2.trits()[i] == 1);
            }
            else
            {
                BOOST_TEST(d2.trits()[i] == 0);
            }
        }
    }

    BOOST_AUTO_TEST_CASE(operation_reverse_diode)
    {
        auto d1 { reverse_diode(smallPositive) };
        auto d2 { reverse_diode(largeNegative) };

        for (auto i = 0u; i < Hexad::width; ++i)
        {
            if (smallPositive.trits()[i] == -1)
            {
                BOOST_TEST(d1.trits()[i] == -1);
            }
            else
            {
                BOOST_TEST(d1.trits()[i] == 0);
            }

            if (largeNegative.trits()[i] == -1)
            {
                BOOST_TEST(d2.trits()[i] == -1);
            }
            else
            {
                BOOST_TEST(d2.trits()[i] == 0);
            }
        }
    }

    BOOST_AUTO_TEST_CASE(operation_trit_maximum)
    {
        auto result1 { trit_maximum(smallPositive, smallNegative) };
        auto result2 { trit_maximum(largePositive, largeNegative) };

        for (auto i = 0u; i < Hexad::width; ++i)
        {
            BOOST_TEST(result1.trits()[i] ==
                std::max(smallPositive.trits()[i], smallNegative.trits()[i])
            );

            BOOST_TEST(result2.trits()[i] ==
                std::max(largePositive.trits()[i], largeNegative.trits()[i])
            );
        }
    }

    BOOST_AUTO_TEST_CASE(operation_trit_minimum)
    {
        auto result1 { trit_minimum(smallPositive, smallNegative) };
        auto result2 { trit_minimum(largePositive, largeNegative) };

        for (auto i = 0u; i < Hexad::width; ++i)
        {
            BOOST_TEST(result1.trits()[i] ==
                std::min(smallPositive.trits()[i], smallNegative.trits()[i])
            );

            BOOST_TEST(result2.trits()[i] ==
                std::min(largePositive.trits()[i], largeNegative.trits()[i])
            );
        }
    }

    BOOST_AUTO_TEST_CASE(operation_logical_equality)
    {
        auto result1 { logical_equality(smallPositive, smallNegative) };
        auto result2 { logical_equality(largePositive, largeNegative) };

        for (auto i = 0u; i < Hexad::width; ++i)
        {
            BOOST_TEST(result1.trits()[i] ==
                (smallPositive.trits()[i] == smallNegative.trits()[i] ? 1 : -1)
            );

            BOOST_TEST(result2.trits()[i] ==
                (largePositive.trits()[i] == largeNegative.trits()[i] ? 1 : -1)
            );
        }
    }

    BOOST_AUTO_TEST_CASE(operation_logical_multiply)
    {
        auto result1 { logical_multiply(smallPositive, smallNegative) };
        auto result2 { logical_multiply(largePositive, largeNegative) };

        for (auto i = 0u; i < Hexad::width; ++i)
        {
            BOOST_TEST(result1.trits()[i] ==
                (smallPositive.trits()[i] * smallNegative.trits()[i])
            );

            BOOST_TEST(result2.trits()[i] ==
                (largePositive.trits()[i] * largeNegative.trits()[i])
            );
        }
    }

    BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE test_onecellcad

#include <optional>

#include <boost/test/unit_test.hpp>

#include <carl/core/MultivariatePolynomial.h>
#include <carl/core/Variable.h>
#include <carl/ran/RealAlgebraicPoint.h>

#include <smtrat-mcsat/explanations/onecellcad/OneCellCAD.h>

/**
  * References:
  * [1] Christopher W. Brown and Marek Košta. 2015. Constructing a single cell
  * in cylindrical algebraic decomposition. J. Symb. Comput. 70, C (September
  * 2015), 14-48. DOI=http://dx.doi.org/10.1016/j.jsc.2014.09.024
  */

namespace {
  using std::cout;
  using std::endl;
  using std::optional;
  using std::nullopt;

  using smtrat::Rational;
  using namespace smtrat::mcsat::onecellcad;
  using carl::Variable;
  using Poly = carl::MultivariatePolynomial<Rational>;
	using RAN = carl::RealAlgebraicNumber<Rational>;
	using RANPoint = carl::RealAlgebraicPoint<Rational>;

struct VariableFixture {
  Variable x = carl::freshRealVariable("x");
  Variable y = carl::freshRealVariable("y");
  Variable z = carl::freshRealVariable("z");
  std::vector<Variable> varOrder {x};
  std::vector<Variable> varOrder2 {x,y};
  std::vector<Variable> varOrder3 {x,y,z};
};

BOOST_FIXTURE_TEST_CASE(polylevel, VariableFixture) {
  BOOST_TEST_MESSAGE("Test polyLevel");

  BOOST_CHECK(levelOf(varOrder3, Poly(1)) == nullopt);
  BOOST_CHECK(levelOf(varOrder3, Poly(x) * Rational(0)) == nullopt);
  BOOST_CHECK(levelOf(varOrder3, Poly(x * y) * Rational(0)) == nullopt);
  BOOST_CHECK(*levelOf(varOrder3, Poly(x)) == 0);
  BOOST_CHECK(*levelOf(varOrder3, Poly(y)) == 1);
  BOOST_CHECK(*levelOf(varOrder3, Poly(x * y)) == 1);
  BOOST_CHECK(*levelOf(varOrder3, Poly(z)) == 2);
  BOOST_CHECK(*levelOf(varOrder3, Poly(x * z)) == 2);
  BOOST_CHECK(*levelOf(varOrder3, Poly(x * y * z)) == 2);
}

BOOST_FIXTURE_TEST_CASE(cell2d, VariableFixture) {
  BOOST_TEST_MESSAGE("Test 2D example from [1]");
  Poly p = Poly(x*x) + Poly(y*y) - Rational(1) ;
  Poly q = Poly(y*y)*Rational(2) - Poly(x*x) * (Poly(x)*Rational(2) + Rational(3)) ;
  Poly r = Poly(y) + Poly(x)*Rational(0.5) - Rational(0.5) ;
  std::vector<Poly> polys {p,q,r};

  RANPoint point { RAN(Rational(-1)/3), RAN(Rational(1)/3) };

  optional<CADCell> c = OneCellCAD(varOrder3,point).createCADCellAroundPoint(polys);

  BOOST_CHECK(c);
}

} // namespace


#include "aikido/distance/SE2.hpp"

#include <boost/math/constants/constants.hpp>

namespace aikido {
namespace distance {

//=============================================================================
SE2::SE2(std::shared_ptr<statespace::SE2> _space)
  : mStateSpace(std::move(_space))
{
  if (mStateSpace == nullptr)
    throw std::invalid_argument("_space is nullptr.");
}

//=============================================================================
statespace::ConstStateSpacePtr SE2::getStateSpace() const
{
  return mStateSpace;
}

//=============================================================================
double SE2::distance(
    const aikido::statespace::StateSpace::State* _state1,
    const aikido::statespace::StateSpace::State* _state2) const
{
  Eigen::VectorXd tangent1;
  mStateSpace->logMap(
      static_cast<const statespace::SE2::State*>(_state1), tangent1);

  Eigen::VectorXd tangent2;
  mStateSpace->logMap(
      static_cast<const statespace::SE2::State*>(_state2), tangent2);

  Eigen::Vector3d diff;

  // Difference between R^2 positions
  diff.head<2>() = tangent1.head<2>() - tangent2.head<2>();

  // Difference between angles
  double angleDiff = tangent1(2) - tangent2(2);
  angleDiff = std::fmod(std::abs(angleDiff), 2.0 * M_PI);
  if (angleDiff > M_PI)
    angleDiff -= 2.0 * M_PI;
  diff[2] = angleDiff;

  return diff.norm();
}

} // namespace distance
} // namespace aikido


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/WhiteNoiseAcceleration.h>
#include <BayesFilters/utils.h>

#include <cmath>
#include <utility>

#include <Eigen/Cholesky>

using namespace bfl;
using namespace Eigen;

struct WhiteNoiseAcceleration::ImplData
{
    ImplData
    (
        const Dim dim,
        const double sampling_interval,
        const double tilde_q,
        const unsigned int seed
    ) :
        T_(sampling_interval),
        tilde_q_(tilde_q),
        seed_(seed),
        generator_(std::mt19937_64(seed_)),
        distribution_(std::normal_distribution<double>(0.0, 1.0)),
        gauss_rnd_sample_([&] { return (distribution_) (generator_); })
    {
        double q11 = 1.0 / 3.0 * std::pow(T_, 3.0);
        double q2 = 1.0 / 2.0 * std::pow(T_, 2.0);

        Matrix2d F;
        F << 1.0, T_,
             0.0, 1.0;

        Matrix2d Q;
        Q << q11, q2,
              q2, T_;

        switch (dim)
        {
            case Dim::OneD:
            {
                F_ = F;

                Q_ = Q;

                break;
            }

            case Dim::TwoD:
            {
                F_ = MatrixXd(4, 4);
                F_ << F, Matrix2d::Zero(),
                      Matrix2d::Zero(), F;

                Q_ = MatrixXd(4, 4);
                Q_ << Q, Matrix2d::Zero(),
                      Matrix2d::Zero(), Q;

                break;
            }

            case Dim::ThreeD:
            {
                F_ = MatrixXd(6, 6);
                F_ << F, Matrix2d::Zero(), Matrix2d::Zero(),
                      Matrix2d::Zero(), F, Matrix2d::Zero(),
                      Matrix2d::Zero(), Matrix2d::Zero(), F;

                Q_ = MatrixXd(6, 6);
                Q_ << Q, Matrix2d::Zero(), Matrix2d::Zero(),
                      Matrix2d::Zero(), Q, Matrix2d::Zero(),
                      Matrix2d::Zero(), Matrix2d::Zero(), Q;

                break;
            }
        }

        Q_ *= tilde_q_;

        LDLT<MatrixXd> chol_ldlt(Q_);
        sqrt_Q_ = (chol_ldlt.transpositionsP() * MatrixXd::Identity(Q_.rows(), Q_.cols())).transpose() * chol_ldlt.matrixL() * chol_ldlt.vectorD().real().cwiseSqrt().asDiagonal();
    }

    /**
     * Sampling interval in [time].
     */
    double T_;

    /**
     * Power spectral density [length]^2/[time]^3.
     */
    double tilde_q_;

    /**
     * State transition matrix.
     */
    Eigen::MatrixXd F_;

    /**
     * Convariance matrix of the additive white noise of the state model.
     */
    Eigen::MatrixXd Q_;

    /**
     * Square root matrix of R_.
     */
    Eigen::MatrixXd sqrt_Q_;

    /**
     * Seed of the random number generator.
     */
    unsigned int seed_;

    /**
     * Random number generator.
     */
    std::mt19937_64 generator_;

    /**
     * Normal distribution for random number generation functions.
     */
    std::normal_distribution<double> distribution_;

    /**
     * Random number generator function from a Normal distribution.
     * A call to `gauss_rnd_sample_()` returns a double-precision floating point random number.
     */
    std::function<double()> gauss_rnd_sample_;
};

WhiteNoiseAcceleration::WhiteNoiseAcceleration
(
    const Dim dim,
    const double sampling_interval,
    const double tilde_q
) noexcept :
    WhiteNoiseAcceleration(dim, sampling_interval, tilde_q, 1)
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration
(
    const Dim dim,
    const double sampling_interval,
    const double tilde_q,
    const unsigned int seed
) noexcept :
    pimpl_(utils::make_unique<ImplData>(dim, sampling_interval, tilde_q, seed))
{ }

WhiteNoiseAcceleration::WhiteNoiseAcceleration(WhiteNoiseAcceleration&& state_model) noexcept = default;

WhiteNoiseAcceleration& WhiteNoiseAcceleration::operator=(WhiteNoiseAcceleration&& state_model) noexcept = default;

WhiteNoiseAcceleration::~WhiteNoiseAcceleration() noexcept = default;

bool WhiteNoiseAcceleration::setProperty(const std::string& property)
{
    return false;
}

std::pair<std::size_t, std::size_t> WhiteNoiseAcceleration::getOutputSize() const
{
    return std::make_pair(4, 0);
}

MatrixXd WhiteNoiseAcceleration::getNoiseSample(const std::size_t num)
{
    MatrixXd rand_vectors(4, num);
    for (int i = 0; i < rand_vectors.size(); i++)
        *(rand_vectors.data() + i) = pimpl_->gauss_rnd_sample_();

    return pimpl_->sqrt_Q_ * rand_vectors;
}

MatrixXd WhiteNoiseAcceleration::getNoiseCovarianceMatrix()
{
    return pimpl_->Q_;
}

MatrixXd WhiteNoiseAcceleration::getStateTransitionMatrix()
{
    return pimpl_->F_;
}

VectorXd WhiteNoiseAcceleration::getTransitionProbability(const Ref<const MatrixXd>& prev_states, const Ref<const MatrixXd>& cur_states)
{
    return utils::multivariate_gaussian_density(prev_states, prev_states.col(0), pimpl_->Q_);
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/optimal_bounding_box.h>
#include <CGAL/Polygon_mesh_processing/IO/polygon_mesh_io.h>

#include <boost/property_map/function_property_map.hpp>

#include <fstream>
#include <iostream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel    K;
typedef K::Point_3                                             Point;
typedef K::Aff_transformation_3                                Aff_transformation;

typedef CGAL::Surface_mesh<Point>                              Surface_mesh;
typedef boost::graph_traits<Surface_mesh>::vertex_descriptor   vertex_descriptor;

struct Aff_tr_fct
{
  Aff_tr_fct() : m_at(nullptr), m_sm(nullptr) { }
  Aff_tr_fct(const Aff_transformation& at, const Surface_mesh& sm) : m_at(&at), m_sm(&sm) { }

  Point operator()(const vertex_descriptor v) const { return m_at->transform(m_sm->point(v)); }

private:
  const Aff_transformation* m_at;
  const Surface_mesh* m_sm;
};

int main(int argc, char** argv)
{
  const char* filename = (argc > 1) ? argv[1] : "data/pig.off";

  Surface_mesh sm;
  if(!CGAL::Polygon_mesh_processing::read_polygon_mesh(filename, sm) || sm.is_empty())
  {
    std::cerr << "Invalid input file." << std::endl;
    return EXIT_FAILURE;
  }

  // get the transformation that yields the optimal bounding box
  Aff_transformation at;
  CGAL::oriented_bounding_box(sm, at);

  // functor to apply the affine transformation to a vertex of the mesh
  Aff_tr_fct aff_tr_fct(at, sm);
  auto aff_tr_vpm = boost::make_function_property_map<vertex_descriptor>(aff_tr_fct);

  // rotated AABB tree
  typedef CGAL::AABB_face_graph_triangle_primitive<Surface_mesh, decltype(aff_tr_vpm)> AABB_face_graph_primitive;
  typedef CGAL::AABB_traits<K, AABB_face_graph_primitive>                              AABB_face_graph_traits;

  CGAL::AABB_tree<AABB_face_graph_traits> tree(faces(sm).begin(), faces(sm).end(), sm, aff_tr_vpm);

  return EXIT_SUCCESS;
}


#include <igl/per_vertex_normals.h>
#include <igl/principal_curvature.h>
#include <igl/avg_edge_length.h>
#include <igl/massmatrix.h>
#include <igl/adjacency_list.h>
#include <igl/per_face_normals.h>
#include <igl/barycenter.h>
#include <igl/pinv.h>
#include <igl/edges.h>
#include <Eigen/SparseCore>
#include <igl/adjacency_list.h>
#include <igl/adjacency_matrix.h>
#include <igl/per_face_normals.h>
#include <igl/per_vertex_normals.h>
#include <igl/avg_edge_length.h>
#include <igl/edge_flaps.h>
#include <igl/unique_edge_map.h>
#include <igl/vertex_triangle_adjacency.h>
#include <igl/principal_curvature.h>
#include <igl/collapse_edge.h>
#include <igl/point_mesh_squared_distance.h>
#include <igl/C_STR.h>
#include <igl/flip_edge.h>
#include <igl/remove_duplicate_vertices.h>
using namespace std;

void tangential_relaxation(Eigen::MatrixXd & V,Eigen::MatrixXi & F, Eigen::VectorXi & feature,
        Eigen::MatrixXd & V0 ,Eigen::MatrixXi & F0, Eigen::VectorXd & lambda){
    using namespace Eigen;
        MatrixXd Q,P,N,V_projected,V_fixed;
        VectorXd dblA,sqrD;
        VectorXi sqrI;
        std::vector<std::vector<int>> A;
        Matrix3d I, NN;
        I.setIdentity();
        Eigen::MatrixXd SV;
        Eigen::MatrixXi SVI,SVJ;

        V_fixed = V;

        int n = V.rows();
        int m = F.rows();

        //igl::doublearea(V,F,dblA);

        //std::vector<double> vertex_areas;
        //vertex_areas.setZero(m);

        //for (int j = 0; j < m; j++) {
        //    vertex_areas[F(j,0)] = vertex_areas[F(j,0)] + (abs(dblA(j))/6);
        //    vertex_areas[F(j,1)] = vertex_areas[F(j,1)] + (abs(dblA(j))/6);
        //    vertex_areas[F(j,2)] = vertex_areas[F(j,2)] + (abs(dblA(j))/6);
        //}

        Eigen::MatrixXd N_before,N_after;
        igl::adjacency_list(F,A);

        int num_feat = feature.size();
        std::vector<bool> is_feature_vertex;
        is_feature_vertex.resize(n);

        for (int s = 0; s < num_feat; s++) {
            is_feature_vertex[feature(s)] = true;
        }

        Q.resize(n,3);
        P.resize(n,3);
        //           Eigen::MatrixXd N;
        igl::per_vertex_normals(V,F,N);

        for(int i = 0; i < n; i++){
            bool is_feature = is_feature_vertex[i];
            if (!is_feature) {

            Eigen::RowVector3d q,p;
            q.setZero();
            p.setZero();
            double denominator = 0.0;
            for(int j = 0; j < A[i].size(); j++){
                q = q + (V.row(A[i][j])/A[i].size());
                // q = q + (V.row(A[i][j])*vertex_areas[A[i][j]]);
                // std::cout << q << std::endl;
                // denominator = denominator + vertex_areas[A[i][j]];
                } // q is )( barycenter?
            // q = q/denominator;
            // N.row(i) = N.row(i)/N.row(i).norm();
            NN = lambda(i)*N.row(i).transpose()*(N.row(i));
             p = (q.transpose()+(NN*(V.row(i).transpose() - q.transpose()))).transpose();
            // p = q;
             // std::cout << N.row(i) << std::endl;

            V.row(i) = p;

            // igl::per_face_normals(V_projected,F,Eigen::Vector3d(0,0,0),N_after);
    //            for (int j = 0; j < m ; j++) {
    //                if (N_before.row(j).dot(N_after.row(j)) < 0) {
    //                    // std::cout << "Avoided face flipping, I think." << std::endl;
    //                    V.row(i) = V_fixed.row(i);
    //                }
    //            }

            }
        }
//        igl::remove_duplicate_vertices(V,0,SV,SVI,SVJ);
//        std::cout << V.rows()-SV.rows() << std::endl;
        igl::point_mesh_squared_distance(V,V0,F0,sqrD,sqrI,V_projected);

    V = V_projected;
//    igl::remove_duplicate_vertices(V,0,SV,SVI,SVJ);
//    std::cout << V.rows()-SV.rows() << std::endl;
 //   std::cout << "not projecting!" << std::endl;
}

// g++ -I/usr/local/libigl/external/eigen -I/usr/local/libigl/include -std=c++11 -framework Accelerate main.cpp remesh_botsch.cpp -o main


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

#ifndef BOOST_MATH_CCMATH_MODF_HPP
#define BOOST_MATH_CCMATH_MODF_HPP

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/trunc.hpp>

namespace boost::math::ccmath {

namespace detail {

template <typename Real>
inline constexpr Real modf_error_impl(Real x, Real* iptr)
{
    *iptr = x;
    return boost::math::ccmath::abs(x) == Real(0) ? x :
           x > Real(0) ? Real(0) : -Real(0);
}

template <typename Real>
inline constexpr Real modf_nan_impl(Real x, Real* iptr)
{
    *iptr = x;
    return x;
}

template <typename Real>
inline constexpr Real modf_impl(Real x, Real* iptr)
{
    *iptr = boost::math::ccmath::trunc(x);
    return (x - *iptr);
}

} // Namespace detail

template <typename Real>
inline constexpr Real modf(Real x, Real* iptr)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        return boost::math::ccmath::abs(x) == Real(0) ? detail::modf_error_impl(x, iptr) :
               boost::math::ccmath::isinf(x) ? detail::modf_error_impl(x, iptr) :
               boost::math::ccmath::isnan(x) ? detail::modf_nan_impl(x, iptr) :
               boost::math::ccmath::detail::modf_impl(x, iptr);
    }
    else
    {
        using std::modf;
        return modf(x, iptr);
    }
}

inline constexpr float modff(float x, float* iptr)
{
    return boost::math::ccmath::modf(x, iptr);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double modfl(long double x, long double* iptr)
{
    return boost::math::ccmath::modf(x, iptr);
}
#endif

} // Namespaces

#endif // BOOST_MATH_CCMATH_MODF_HPP


#include <iostream>
#include <fstream>
#include <boost/program_options.hpp>
#include <random>
#include <iomanip>
#include <chrono>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_blocked.hpp>
#include "matrix.h"

namespace po = boost::program_options;
using namespace std;
std::uniform_real_distribution<double> urd;
std::default_random_engine re;

Tmat RandomMatrix(int m, int n) {
  Tmat mat = newMat(m, n);
  for (int i = 0; i < m; i += 1) {
    for (int j = 0; j < n; j += 1) {
      mat[j][i] = urd(re);
    }
  }
  return mat;
}
void prikaz1(Tmat &mat) {
  for (auto &vrstica : mat) {
    std::cout << "|";
    for (auto &element : vrstica) {
      std::cout << std::setw(10) << element;
    }
    std::cout << "|" << std::endl;
  }
  std::cout << std::endl;
}
boost::numeric::ublas::matrix<double> TmatToBlas(Tmat mat) {
  int m1 = mat.size();
  int n1 = mat[0].size();
  boost::numeric::ublas::matrix<double> bmat(m1, n1);
  for (int i = 0; i < m1; i += 1) {
    for (int j = 0; j < n1; j += 1) {
      bmat(i, j) = mat[i][j];
    }
  }
  return bmat;
}

boost::numeric::ublas::matrix<double> MultiplicationBlas(boost::numeric::ublas::matrix<double> M1,
                                                         boost::numeric::ublas::matrix<double> M2) {
  return boost::numeric::ublas::block_prod<boost::numeric::ublas::matrix<double>, 32>(M1, M2);
}

enum Method {
  CLASSIC, CLASSIC_T, RECURSIVE, RECURSIVE_T, SUBCUBIC, BLAS //STRASSEN, BLAS
};
static map<string, Method> methodMap{
    {"classic",              CLASSIC},
    {"classic_transposed",   CLASSIC_T},
    {"recursive",            RECURSIVE},
    {"recursive_transposed", RECURSIVE_T},
    {"subcubic",             SUBCUBIC},
    //{"strassen",             STRASSEN},
    {"blas",                 BLAS}
};

int main(int ac, const char **av) {
  int a, b, c;
  int repeat;
  int seed = 2; //totaly random
  int max_time;
  Method method;
  try {
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "Display this message")
        ("method,m", po::value<string>()->default_value("subcubic"), "Method to benchmark \n"
            "Available methods:\n"
            "  classic\n"
            "  classic_transposed\n"
            "  recursive\n"
            "  recursive_transposed\n"
            "  subcubic \n"
            //"  strassen \n"
            "  blas \n"
        )
        ("a,a", po::value<int>()->default_value(512), "A in (A,B)x(B,C) mutiplication  \n ")
        ("b,b", po::value<int>()->default_value(512), "B in (A,B)x(B,C) mutiplication  \n ")
        ("c,c", po::value<int>()->default_value(512), "C in (A,B)x(B,C) mutiplication  \n ")
        ("repeat,r", po::value<int>()->default_value(1), "Number of tests to run")
        ("time,t", po::value<int>()->default_value(0), "Repeat test until arg ms elapsed, overrides --repeat \n ");

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << desc << "\n";
      return 1;
    }

    if (vm.count("method")) {
      string m = vm["method"].as<string>();
      if (methodMap.count(m) > 0) {
        //cout << "testing with method " << m << '\n';
        method = methodMap[m];
      } else {
        cout << m << " is not a valid method, use --help for a list of available methods.";
        return 1;
      }
    } else {
      cout << "method was not set!\n";
      return 1;
    }

    if (vm.count("a")) {
      a = vm["a"].as<int>();
      //cout << "a = " << a << '\n';

    } else {
      cout << "a was not set!\n";
      return 1;
    }
    if (vm.count("b")) {
      b = vm["b"].as<int>();
      //cout << "b = " << b << '\n';

    } else {
      cout << "b was not set!\n";
      return 1;
    }
    if (vm.count("c")) {
      c = vm["c"].as<int>();
      // cout << "c = " << c << '\n';

    } else {
      cout << "c was not set!\n";
      return 1;
    }

    if (vm.count("time")) {
      max_time = vm["time"].as<int>();
    }

    repeat = vm["repeat"].as<int>();
    if (max_time == 0) {
      //cout << "Number of test: " << repeat << endl;
    } else {
      //cout << "Testing for " << max_time << " ms" << endl;
    }

  } catch (std::exception &e) {
    cerr << e.what();
  }

  //Execute the tests
  urd = std::uniform_real_distribution<double>(0, 1);
  re = std::default_random_engine(seed);
  Tmat m1 = RandomMatrix(b, a);
  Tmat m2 = RandomMatrix(c, b);
  boost::numeric::ublas::matrix<double> bm1, bm2;

  std::function<Tmat(Tmat &, Tmat &)> f;
  auto blas_f = MultiplicationBlas;

  switch (method) {
    case CLASSIC:f = MultiplicationClassic;
      break;
    case CLASSIC_T:f = MultiplicationClassicTransposed;
      break;
    case RECURSIVE:f = MultiplicationRecursive;
      break;
    case RECURSIVE_T:f = MultiplicationRecursiveTransposed;
      break;
    case SUBCUBIC:f = MultiplicationSubcubic;
      break;
    //case STRASSEN: f = strassen_mul;
    //  break;
    case BLAS :bm1 = TmatToBlas(m1);
      bm2 = TmatToBlas(m2);
  }
  if (max_time == 1) {
    //placeholder for testing
    int n = 512;
    Tmat mat1 = newMat(n, n);
    Tmat mat2 = newMat(n, n);
    double konst = 100000000.000002;
    for (int i = 0; i < n; i += 1) {
      for (int j = 0; j < n; j += 1) {
        mat1[i][j] = konst;
        mat2[i][j] = konst;
      }
    }
    Tmat mat3 = f(mat1, mat2);
    //prikaz1(mat3);
    cout << setprecision(24);
    cout << konst * konst * n << endl << mat3[0][0] << endl;
    cout << mat3[n / 2][n / 2] - konst * konst * n << endl;
  } else {

    if (max_time > 0) {
      int count = 0;
      std::chrono::time_point<std::chrono::steady_clock> time_start, time_end;
      auto time_total = std::chrono::milliseconds{0};
      while (true) {
        time_start = std::chrono::steady_clock::now();
        if (method == BLAS) {
          auto bm3 = blas_f(bm1, bm2);
        } else {
          Tmat m3 = f(m1, m2);
        }
        time_end = std::chrono::steady_clock::now();
        auto time_of_test = std::chrono::duration_cast<std::chrono::milliseconds>(time_end - time_start);
//cout << time_of_test.count() << endl;
        time_total +=
            time_of_test;
        count++;
        if (time_total.
            count()
            > max_time || count > 10000) {
//cout << "Total time:" << time_total.count() << "ms" << endl;
//cout << "Iteration time:" << time_total.count() / count << "ms" << endl;
          cout << time_total.
              count()
              / count <<
               endl;
          break;
        }
      }
    } else {

      std::chrono::time_point<std::chrono::steady_clock> time_start, time_end;
      time_start = std::chrono::steady_clock::now();
      if (method == BLAS) {
        for (
            int i = 0;
            i < repeat;
            ++i) {
          auto bm3 = blas_f(bm1, bm2);
        }
      } else {
        for (
            int i = 0;
            i < repeat;
            ++i) {
          Tmat m3 = f(m1, m2);
        }
      }

      time_end = std::chrono::steady_clock::now();
      auto time_total = std::chrono::duration_cast<std::chrono::milliseconds>(time_end - time_start);
      std::cout << time_total.count() / repeat << endl;
    }
  }
}


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

 * 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   testAHRSFactor.cpp
 * @brief  Unit test for AHRSFactor
 * @author Krunal Chande
 * @author Luca Carlone
 * @author Frank Dellaert
 * @author Varun Agrawal
 */

#include <gtsam/navigation/AHRSFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/debug.h>
#include <CppUnitLite/TestHarness.h>

#include <boost/bind/bind.hpp>
#include <list>

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

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;

Vector3 kZeroOmegaCoriolis(0,0,0);

// Define covariance matrices
double accNoiseVar = 0.01;
const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;

//******************************************************************************
namespace {
Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
    const Rot3 rot_j, const Vector3& bias) {
  return factor.evaluateError(rot_i, rot_j, bias);
}

Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
    const Rot3 rot_j, const Vector3& bias) {
  return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
}

AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
    const Vector3& bias, const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3::Zero()) {
  AHRSFactor::PreintegratedMeasurements result(bias, I_3x3);

  list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
  list<double>::const_iterator itDeltaT = deltaTs.begin();
  for (; itOmega != measuredOmegas.end(); ++itOmega, ++itDeltaT) {
    result.integrateMeasurement(*itOmega, *itDeltaT);
  }

  return result;
}

Rot3 evaluatePreintegratedMeasurementsRotation(
    const Vector3& bias, const list<Vector3>& measuredOmegas,
    const list<double>& deltaTs,
    const Vector3& initialRotationRate = Vector3::Zero()) {
  return Rot3(
      evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
          initialRotationRate).deltaRij());
}

Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
    const double deltaT) {
  return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
}

Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
  return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
}

}
//******************************************************************************
TEST( AHRSFactor, PreintegratedMeasurements ) {
  // Linearization point
  Vector3 bias(0,0,0); ///< Current estimate of angular rate bias

  // Measurements
  Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
  double deltaT = 0.5;

  // Expected preintegrated values
  Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
  double expectedDeltaT1(0.5);

  // Actual preintegrated values
  AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3);
  actual1.integrateMeasurement(measuredOmega, deltaT);

  EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
  DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);

  // Integrate again
  Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
  double expectedDeltaT2(1);

  // Actual preintegrated values
  AHRSFactor::PreintegratedMeasurements actual2 = actual1;
  actual2.integrateMeasurement(measuredOmega, deltaT);

  EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
  DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
}

//******************************************************************************
TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
  Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4;
  Vector3 omegaCoriolis(0.1, 0.5, 0.9);
  PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis);
  Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias
  Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
  double deltaTij = 0.02;
  Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5;
  Matrix3 preintMeasCov = Matrix3::Ones()*0.2;
  PreintegratedAhrsMeasurements actualPim(
    boost::make_shared<PreintegratedRotationParams>(params),
    bias,
    deltaTij,
    deltaRij,
    delRdelBiasOmega,
    preintMeasCov);
  EXPECT(assert_equal(gyroscopeCovariance,
      actualPim.p().getGyroscopeCovariance(), 1e-6));
  EXPECT(assert_equal(omegaCoriolis,
      actualPim.p().getOmegaCoriolis().get(), 1e-6));
  EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6));
  DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6);
  EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6));
  EXPECT(assert_equal(delRdelBiasOmega, actualPim.delRdelBiasOmega(), 1e-6));
  EXPECT(assert_equal(preintMeasCov, actualPim.preintMeasCov(), 1e-6));
}

/* ************************************************************************* */
TEST(AHRSFactor, Error) {
  // Linearization point
  Vector3 bias(0.,0.,0.); // Bias
  Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
  Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << M_PI / 100, 0, 0;
  double deltaT = 1.0;
  AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3);
  pim.integrateMeasurement(measuredOmega, deltaT);

  // Create factor
  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, boost::none);

  Vector3 errorActual = factor.evaluateError(x1, x2, bias);

  // Expected error
  Vector3 errorExpected(3);
  errorExpected << 0, 0, 0;
  EXPECT(assert_equal(Vector(errorExpected), Vector(errorActual), 1e-6));

  // Expected Jacobians
  Matrix H1e = numericalDerivative11<Vector3, Rot3>(
      boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
  Matrix H2e = numericalDerivative11<Vector3, Rot3>(
      boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
  Matrix H3e = numericalDerivative11<Vector3, Vector3>(
      boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);

  // Check rotation Jacobians
  Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
      boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
  Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
      boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);

  // Actual Jacobians
  Matrix H1a, H2a, H3a;
  (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);

  // rotations
  EXPECT(assert_equal(RH1e, H1a, 1e-5));
  // 1e-5 needs to be added only when using quaternions for rotations

  EXPECT(assert_equal(H2e, H2a, 1e-5));

  // rotations
  EXPECT(assert_equal(RH2e, H2a, 1e-5));
  // 1e-5 needs to be added only when using quaternions for rotations

  EXPECT(assert_equal(H3e, H3a, 1e-5));
  // 1e-5 needs to be added only when using quaternions for rotations
}

/* ************************************************************************* */
TEST(AHRSFactor, ErrorWithBiases) {
  // Linearization point

  Vector3 bias(0, 0, 0.3);
  Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
  Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
  double deltaT = 1.0;

  AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0),
      Z_3x3);
  pim.integrateMeasurement(measuredOmega, deltaT);

  // Create factor
  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);

  Vector errorActual = factor.evaluateError(x1, x2, bias);

  // Expected error
  Vector errorExpected(3);
  errorExpected << 0, 0, 0;
  EXPECT(assert_equal(errorExpected, errorActual, 1e-6));

  // Expected Jacobians
  Matrix H1e = numericalDerivative11<Vector, Rot3>(
      boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
  Matrix H2e = numericalDerivative11<Vector, Rot3>(
      boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
  Matrix H3e = numericalDerivative11<Vector, Vector3>(
      boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);

  // Check rotation Jacobians
  Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
      boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
  Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
      boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
  Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
      boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);

  // Actual Jacobians
  Matrix H1a, H2a, H3a;
  (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);

  EXPECT(assert_equal(H1e, H1a));
  EXPECT(assert_equal(H2e, H2a));
  EXPECT(assert_equal(H3e, H3a));
}

//******************************************************************************
TEST( AHRSFactor, PartialDerivativeExpmap ) {
  // Linearization point
  Vector3 biasOmega(0,0,0);

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << 0.1, 0, 0;
  double deltaT = 0.5;

  // Compute numerical derivatives
  Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
      boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega);

  const Matrix3 Jr = Rot3::ExpmapDerivative(
      (measuredOmega - biasOmega) * deltaT);

  Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign

  // Compare Jacobians
  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));
  // 1e-3 needs to be added only when using quaternions for rotations

}

//******************************************************************************
TEST( AHRSFactor, PartialDerivativeLogmap ) {
  // Linearization point
  Vector3 thetahat;
  thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias

  // Measurements
  Vector3 deltatheta;
  deltatheta << 0, 0, 0;

  // Compute numerical derivatives
  Matrix expectedDelFdeltheta = numericalDerivative11<Vector3, Vector3>(
      boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);

  const Vector3 x = thetahat; // parametrization of so(3)
  const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
  double normx = x.norm();
  const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X
      + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
          * X;

  // Compare Jacobians
  EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));

}

//******************************************************************************
TEST( AHRSFactor, fistOrderExponential ) {
  // Linearization point
  Vector3 biasOmega(0,0,0);

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << 0.1, 0, 0;
  double deltaT = 1.0;

  // change w.r.t. linearization point
  double alpha = 0.0;
  Vector3 deltabiasOmega;
  deltabiasOmega << alpha, alpha, alpha;

  const Matrix3 Jr = Rot3::ExpmapDerivative(
      (measuredOmega - biasOmega) * deltaT);

  Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign

  const Matrix expectedRot = Rot3::Expmap(
      (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();

  const Matrix3 hatRot =
      Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
  const Matrix3 actualRot = hatRot
      * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();

  // Compare Jacobians
  EXPECT(assert_equal(expectedRot, actualRot));
}

//******************************************************************************
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
  // Linearization point
  Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias

  Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));

  // Measurements
  list<Vector3> measuredOmegas;
  list<double> deltaTs;
  measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
  deltaTs.push_back(0.01);
  measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
  deltaTs.push_back(0.01);
  for (int i = 1; i < 100; i++) {
    measuredOmegas.push_back(
        Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
    deltaTs.push_back(0.01);
  }

  // Actual preintegrated values
  AHRSFactor::PreintegratedMeasurements preintegrated =
      evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
          Vector3(M_PI / 100.0, 0.0, 0.0));

  // Compute numerical derivatives
  Matrix expectedDelRdelBias =
      numericalDerivative11<Rot3, Vector3>(
          boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
              measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
  Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);

  // Compare Jacobians
  EXPECT(
      assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3));
  // 1e-3 needs to be added only when using quaternions for rotations
}

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

//******************************************************************************
TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {

  Vector3 bias(0, 0, 0.3);
  Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
  Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));

  // Measurements
  Vector3 omegaCoriolis;
  omegaCoriolis << 0, 0.1, 0.1;
  Vector3 measuredOmega;
  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
  double deltaT = 1.0;

  const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
      Point3(1, 0, 0));

  AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);

  pim.integrateMeasurement(measuredOmega, deltaT);

  // Check preintegrated covariance
  EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov()));

  // Create factor
  AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis);

  // Expected Jacobians
  Matrix H1e = numericalDerivative11<Vector, Rot3>(
      boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
  Matrix H2e = numericalDerivative11<Vector, Rot3>(
      boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
  Matrix H3e = numericalDerivative11<Vector, Vector3>(
      boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);

  // Check rotation Jacobians
  Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
      boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
  Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
      boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
  Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
      boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);

  // Actual Jacobians
  Matrix H1a, H2a, H3a;
  (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);

  EXPECT(assert_equal(H1e, H1a));
  EXPECT(assert_equal(H2e, H2a));
  EXPECT(assert_equal(H3e, H3a));
}
//******************************************************************************
TEST (AHRSFactor, predictTest) {
  Vector3 bias(0,0,0);

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << 0, 0, M_PI / 10.0;
  double deltaT = 0.2;
  AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance);
  for (int i = 0; i < 1000; ++i) {
    pim.integrateMeasurement(measuredOmega, deltaT);
  }
  // Check preintegrated covariance
  Matrix expectedMeasCov(3,3);
  expectedMeasCov = 200*kMeasuredAccCovariance;
  EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov()));

  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);

  // Predict
  Rot3 x;
  Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0);
  Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
  EXPECT(assert_equal(expectedRot, actualRot, 1e-6));

  // AHRSFactor::PreintegratedMeasurements::predict
  Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
      boost::bind(&AHRSFactor::PreintegratedMeasurements::predict,
          &pim, _1, boost::none), bias);

  // Actual Jacobians
  Matrix H;
  (void) pim.predict(bias,H);
  EXPECT(assert_equal(expectedH, H, 1e-8));
}
//******************************************************************************
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h>

TEST (AHRSFactor, graphTest) {
  // linearization point
  Rot3 x1(Rot3::RzRyRx(0, 0, 0));
  Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0));
  Vector3 bias(0,0,0);

  // PreIntegrator
  Vector3 biasHat(0, 0, 0);
  AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance);

  // Pre-integrate measurements
  Vector3 measuredOmega(0, M_PI / 20, 0);
  double deltaT = 1;

  // Create Factor
  noiseModel::Base::shared_ptr model = //
      noiseModel::Gaussian::Covariance(pim.preintMeasCov());
  NonlinearFactorGraph graph;
  Values values;
  for (size_t i = 0; i < 5; ++i) {
    pim.integrateMeasurement(measuredOmega, deltaT);
  }

  // pim.print("Pre integrated measurementes");
  AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
  values.insert(X(1), x1);
  values.insert(X(2), x2);
  values.insert(B(1), bias);
  graph.push_back(factor);
  LevenbergMarquardtOptimizer optimizer(graph, values);
  Values result = optimizer.optimize();
  Rot3 expectedRot(Rot3::RzRyRx(0, M_PI / 4, 0));
  EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
}

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


/* test_discrete_distribution.cpp
 *
 * Copyright Steven Watanabe 2010
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id: test_discrete_distribution.cpp 83381 2013-03-09 22:55:05Z eric_niebler $
 *
 */

#include <boost/random/discrete_distribution.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/assign/list_of.hpp>
#include <sstream>
#include <vector>
#include "concepts.hpp"

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

using boost::random::test::RandomNumberDistribution;
using boost::random::discrete_distribution;
BOOST_CONCEPT_ASSERT((RandomNumberDistribution< discrete_distribution<> >));

struct gen {
    double operator()(double arg) {
        if(arg < 100) return 100;
        else if(arg < 103) return 1;
        else if(arg < 107) return 2;
        else if(arg < 111) return 1;
        else if(arg < 114) return 4;
        else return 100;
    }
};

#define CHECK_PROBABILITIES(actual, expected)       \
    do {                                            \
        std::vector<double> _actual = (actual);     \
        std::vector<double> _expected = (expected); \
        BOOST_CHECK_EQUAL_COLLECTIONS(              \
            _actual.begin(), _actual.end(),         \
            _expected.begin(), _expected.end());    \
    } while(false)

using boost::assign::list_of;

BOOST_AUTO_TEST_CASE(test_constructors) {
    boost::random::discrete_distribution<> dist;
    CHECK_PROBABILITIES(dist.probabilities(), list_of(1.0));

#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    boost::random::discrete_distribution<> dist_il = { 1, 2, 1, 4 };
    CHECK_PROBABILITIES(dist_il.probabilities(), list_of(.125)(.25)(.125)(.5));
#endif
    std::vector<double> probs = boost::assign::list_of(1.0)(2.0)(1.0)(4.0);

    boost::random::discrete_distribution<> dist_r(probs);
    CHECK_PROBABILITIES(dist_r.probabilities(), list_of(.125)(.25)(.125)(.5));

    boost::random::discrete_distribution<> dist_it(probs.begin(), probs.end());
    CHECK_PROBABILITIES(dist_it.probabilities(), list_of(.125)(.25)(.125)(.5));

    boost::random::discrete_distribution<> dist_fun(4, 99, 115, gen());
    CHECK_PROBABILITIES(dist_fun.probabilities(), list_of(.125)(.25)(.125)(.5));

    boost::random::discrete_distribution<> copy(dist);
    BOOST_CHECK_EQUAL(dist, copy);
    boost::random::discrete_distribution<> copy_r(dist_r);
    BOOST_CHECK_EQUAL(dist_r, copy_r);

    boost::random::discrete_distribution<> notpow2(3, 99, 111, gen());
    BOOST_REQUIRE_EQUAL(notpow2.probabilities().size(), 3u);
    BOOST_CHECK_CLOSE_FRACTION(notpow2.probabilities()[0], 0.25, 0.00000000001);
    BOOST_CHECK_CLOSE_FRACTION(notpow2.probabilities()[1], 0.50, 0.00000000001);
    BOOST_CHECK_CLOSE_FRACTION(notpow2.probabilities()[2], 0.25, 0.00000000001);
    boost::random::discrete_distribution<> copy_notpow2(notpow2);
    BOOST_CHECK_EQUAL(notpow2, copy_notpow2);
}

BOOST_AUTO_TEST_CASE(test_param) {
    std::vector<double> probs = boost::assign::list_of(1.0)(2.0)(1.0)(4.0);
    boost::random::discrete_distribution<> dist(probs);
    boost::random::discrete_distribution<>::param_type param = dist.param();
    CHECK_PROBABILITIES(param.probabilities(), list_of(.125)(.25)(.125)(.5));
    boost::random::discrete_distribution<> copy1(param);
    BOOST_CHECK_EQUAL(dist, copy1);
    boost::random::discrete_distribution<> copy2;
    copy2.param(param);
    BOOST_CHECK_EQUAL(dist, copy2);

    boost::random::discrete_distribution<>::param_type param_copy = param;
    BOOST_CHECK_EQUAL(param, param_copy);
    BOOST_CHECK(param == param_copy);
    BOOST_CHECK(!(param != param_copy));
    boost::random::discrete_distribution<>::param_type param_default;
    CHECK_PROBABILITIES(param_default.probabilities(), list_of(1.0));
    BOOST_CHECK(param != param_default);
    BOOST_CHECK(!(param == param_default));

#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    boost::random::discrete_distribution<>::param_type
        parm_il = { 1, 2, 1, 4 };
    CHECK_PROBABILITIES(parm_il.probabilities(), list_of(.125)(.25)(.125)(.5));
#endif

    boost::random::discrete_distribution<>::param_type parm_r(probs);
    CHECK_PROBABILITIES(parm_r.probabilities(), list_of(.125)(.25)(.125)(.5));

    boost::random::discrete_distribution<>::param_type
        parm_it(probs.begin(), probs.end());
    CHECK_PROBABILITIES(parm_it.probabilities(), list_of(.125)(.25)(.125)(.5));

    boost::random::discrete_distribution<>::param_type
        parm_fun(4, 99, 115, gen());
    CHECK_PROBABILITIES(parm_fun.probabilities(), list_of(.125)(.25)(.125)(.5));
}

BOOST_AUTO_TEST_CASE(test_min_max) {
    std::vector<double> probs = boost::assign::list_of(1.0)(2.0)(1.0);
    boost::random::discrete_distribution<> dist;
    BOOST_CHECK_EQUAL((dist.min)(), 0);
    BOOST_CHECK_EQUAL((dist.max)(), 0);
    boost::random::discrete_distribution<> dist_r(probs);
    BOOST_CHECK_EQUAL((dist_r.min)(), 0);
    BOOST_CHECK_EQUAL((dist_r.max)(), 2);
}

BOOST_AUTO_TEST_CASE(test_comparison) {
    std::vector<double> probs = boost::assign::list_of(1.0)(2.0)(1.0)(4.0);
    boost::random::discrete_distribution<> dist;
    boost::random::discrete_distribution<> dist_copy(dist);
    boost::random::discrete_distribution<> dist_r(probs);
    boost::random::discrete_distribution<> dist_r_copy(dist_r);
    BOOST_CHECK(dist == dist_copy);
    BOOST_CHECK(!(dist != dist_copy));
    BOOST_CHECK(dist_r == dist_r_copy);
    BOOST_CHECK(!(dist_r != dist_r_copy));
    BOOST_CHECK(dist != dist_r);
    BOOST_CHECK(!(dist == dist_r));
}

BOOST_AUTO_TEST_CASE(test_streaming) {
    std::vector<double> probs = boost::assign::list_of(1.0)(2.0)(1.0)(4.0);
    boost::random::discrete_distribution<> dist(probs);
    std::stringstream stream;
    stream << dist;
    boost::random::discrete_distribution<> restored_dist;
    stream >> restored_dist;
    BOOST_CHECK_EQUAL(dist, restored_dist);
}

BOOST_AUTO_TEST_CASE(test_generation) {
    std::vector<double> probs = boost::assign::list_of(0.0)(1.0);
    boost::minstd_rand0 gen;
    boost::random::discrete_distribution<> dist;
    boost::random::discrete_distribution<> dist_r(probs);
    for(int i = 0; i < 10; ++i) {
        int value = dist(gen);
        BOOST_CHECK_EQUAL(value, 0);
        int value_r = dist_r(gen);
        BOOST_CHECK_EQUAL(value_r, 1);
        int value_param = dist_r(gen, dist.param());
        BOOST_CHECK_EQUAL(value_param, 0);
        int value_r_param = dist(gen, dist_r.param());
        BOOST_CHECK_EQUAL(value_r_param, 1);
    }
}


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

#ifndef BOOST_MPL11_FWD_MONOID_HPP
#define BOOST_MPL11_FWD_MONOID_HPP

#include <boost/mpl11/fwd/bool.hpp>

namespace boost { namespace mpl11 {
    /*!
     * @ingroup typeclasses
     * @defgroup Monoid Monoid
     *
     * The `Monoid` typeclass is used for datatypes with an associative binary
     * operation that has an identity.
     *
     *
     * Instances of `Monoid` must satisfy the following laws:
     *
        @code
            plus zero x == x
            plus x zero == x
            plus x (plus y z) == plus (plus x y) z
        @endcode
     *
     * The method names refer to the monoid of numbers under addition, but
     * there are many other instances such as sequences under concatenation.
     * Some datatypes can be viewed as a monoid in more than one way, e.g.
     * both addition and multiplication on numbers.
     *
     *
     * ### Methods
     * `plus` and `zero`
     *
     * ### Minimal complete definition
     * All the methods.
     *
     * @{
     */
    template <typename Left, typename Right = Left, typename = true_>
    struct Monoid;

    /*!
     * Associative operation on a `Monoid`.
     *
     * `plus` can be invoked with more than two arguments. Specifically,
     * `plus<x1, x2, xn...>` is equivalent to `plus<plus<x1, x2>, xn...>`.
     */
    template <typename x1, typename x2, typename ...xn>
    struct plus;

    //! Additive identity for the given `Datatype`.
    template <typename Datatype>
    struct zero;
    //! @}
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_FWD_MONOID_HPP


#include "declat.h"

#include <boost/dynamic_bitset.hpp>

#include "itemsets.h"
#include "verticaldatabase.h"

using boost::dynamic_bitset;
using std::vector;

struct Candidate {
    int last_item;
    vector<int> diffset;
    unsigned int support;
};

void declat_recursive(const vector<int>& prefix,
                      const vector<Candidate>& p,
                      unsigned int minsup,
                      FrequentItemsets& result) {
    auto size = p.size();
    for (unsigned int i = 0; i < size; ++i) {
        // take candidate a from p
        const Candidate& a = p[i];
        vector<int> itemset_a = prefix;
        itemset_a.push_back(a.last_item);
        result[itemset_a] = a.support;

        // look for frequent itemsets with candidate a as prefix
        vector<Candidate> pa;
        for (unsigned int j = i+1; j < size; ++j) {
            // take candidate b from p
            const Candidate& b = p[j];

            // new candidate ab
            Candidate ab;
            ab.last_item = b.last_item;
            ab.diffset = itemset_without(b.diffset, a.diffset);
            ab.support = a.support - ab.diffset.size();
            if (ab.support >= minsup)
                pa.emplace_back(ab);
        }

        // recurse!
        if ( ! pa.empty())
            declat_recursive(itemset_a, pa, minsup, result);
    }
}

FrequentItemsets declat(const VerticalDatabase& d, unsigned int minsup) {
    // initial candidates: frequent single-item itemsets
    vector<Candidate> p;
    int nItems = d.nItems();
    for (int i = 0; i < nItems; ++i) {
        dynamic_bitset<> b = d.bs[i];
        unsigned int support = b.count();
        if (support >= minsup)
            p.push_back({i, diffset_for_single_item(b), support});
    }

    // run algorithm
    FrequentItemsets result;
    vector<int> prefix;
    declat_recursive(prefix, p, minsup, result);

    return result;
}


/*
 *
 * Copyright (c) Kresimir Fresl Toon Knapen 2003
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Authors assume no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_PPSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_PPSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/numeric/bindings/traits/detail/symm_herm_traits.hpp>
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A symmetric or Hermitian positive definite matrix
    // stored in packed format
    //
    /////////////////////////////////////////////////////////////////////

    /*
     * ppsv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N symmetric or Hermitian positive
     * definite matrix stored in packed format and X and B are N-by-NRHS
     * matrices.
     *
     * The Cholesky decomposition is used to factor A as
     *   A = U^T * U or A = U^H * U,  if UPLO = 'U',
     *   A = L * L^T or A = L * L^H,  if UPLO = 'L',
     * where U is an upper triangular matrix and L is a lower triangular
     * matrix. The factored form of A is then used to solve the system of
     * equations A * X = B.
     *
     * Only upper or lower triangle of the symmetric matrix A is stored,
     * packed columnwise in a linear array AP.
     */

    namespace detail {

      inline
      void ppsv (char const uplo, int const n, int const nrhs,
                 float* ap, float* b, int const ldb, int* info)
      {
        LAPACK_SPPSV (&uplo, &n, &nrhs, ap, b, &ldb, info);
      }

      inline
      void ppsv (char const uplo, int const n, int const nrhs,
                 double* ap, double* b, int const ldb, int* info)
      {
        LAPACK_DPPSV (&uplo, &n, &nrhs, ap, b, &ldb, info);
      }

      inline
      void ppsv (char const uplo, int const n, int const nrhs,
                 traits::complex_f* ap, traits::complex_f* b, int const ldb,
                 int* info)
      {
        LAPACK_CPPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap),
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void ppsv (char const uplo, int const n, int const nrhs,
                 traits::complex_d* ap, traits::complex_d* b, int const ldb,
                 int* info)
      {
        LAPACK_ZPPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap),
                      traits::complex_ptr (b), &ldb, info);
      }

    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int ppsv (SymmMatrA& a, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatrA>::matrix_structure,
        typename traits::detail::symm_herm_pack_t<
          typename traits::matrix_traits<SymmMatrA>::value_type
        >::type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::ppsv (uplo, n, traits::matrix_size2 (b),
                    traits::matrix_storage (a),
                    traits::matrix_storage (b),
                    traits::leading_dimension (b),
                    &info);
      return info;
    }

    /*
     * pptrf() computes the Cholesky factorization of a symmetric
     * or Hermitian positive definite matrix A in packed storage.
     * The factorization has the form
     *   A = U^T * U or A = U^H * U,  if UPLO = 'U',
     *   A = L * L^T or A = L * L^H,  if UPLO = 'L',
     * where U is an upper triangular matrix and L is lower triangular.
     */

    namespace detail {

      inline
      void pptrf (char const uplo, int const n, float* ap, int* info) {
        LAPACK_SPPTRF (&uplo, &n, ap, info);
      }

      inline
      void pptrf (char const uplo, int const n, double* ap, int* info) {
        LAPACK_DPPTRF (&uplo, &n, ap, info);
      }

      inline
      void pptrf (char const uplo, int const n,
                  traits::complex_f* ap, int* info)
      {
        LAPACK_CPPTRF (&uplo, &n, traits::complex_ptr (ap), info);
      }

      inline
      void pptrf (char const uplo, int const n,
                  traits::complex_d* ap, int* info)
      {
        LAPACK_ZPPTRF (&uplo, &n, traits::complex_ptr (ap), info);
      }

    }

    template <typename SymmMatrA>
    inline
    int pptrf (SymmMatrA& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatrA>::matrix_structure,
        typename traits::detail::symm_herm_pack_t<
          typename traits::matrix_traits<SymmMatrA>::value_type
        >::type
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::pptrf (uplo, n, traits::matrix_storage (a), &info);
      return info;
    }

    /*
     * pptrs() solves a system of linear equations A*X = B with
     * a symmetric or Hermitian positive definite matrix A in packed
     * storage using the Cholesky factorization computed by pptrf().
     */

    namespace detail {

      inline
      void pptrs (char const uplo, int const n, int const nrhs,
                  float const* ap, float* b, int const ldb, int* info)
      {
        LAPACK_SPPTRS (&uplo, &n, &nrhs, ap, b, &ldb, info);
      }

      inline
      void pptrs (char const uplo, int const n, int const nrhs,
                  double const* ap, double* b, int const ldb, int* info)
      {
        LAPACK_DPPTRS (&uplo, &n, &nrhs, ap, b, &ldb, info);
      }

      inline
      void pptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_f const* ap,
                  traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CPPTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (ap),
                       traits::complex_ptr (b), &ldb, info);
      }

      inline
      void pptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_d const* ap,
                  traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZPPTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (ap),
                       traits::complex_ptr (b), &ldb, info);
      }

    }

    template <typename SymmMatrA, typename MatrB>
    inline
    int pptrs (SymmMatrA const& a, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmMatrA>::matrix_structure,
        typename traits::detail::symm_herm_pack_t<
          typename traits::matrix_traits<SymmMatrA>::value_type
        >::type
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::pptrs (uplo, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::matrix_storage (a),
#else
                     traits::matrix_storage_const (a),
#endif
                     traits::matrix_storage (b),
                     traits::leading_dimension (b),
                     &info);
      return info;
    }

    // TO DO: pptri()

  }

}}}

#endif


#include <gtest/gtest.h>
#include <stan/math/prim/mat.hpp>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <test/unit/math/prim/scal/prob/util.hpp>
#include <limits>
#include <vector>

using stan::math::bernoulli_logit_glm_rng;

TEST(ProbDistributionsBernoulliLogitGlm, vectorized) {
  //  Test scalar/vector combinations.
  boost::random::mt19937 rng;

  Eigen::MatrixXd x(2, 3);
  x << 3.5, -1.5, 0.0, 2.0, 1.0, 3.0;

  double alpha_scalar = 1.0;
  std::vector<double> alpha{1.0, 3.0};
  Eigen::VectorXd alpha_vector(2);
  alpha_vector << 1.0, 3.0;
  Eigen::RowVectorXd alpha_vector_t(2);
  alpha_vector_t = alpha_vector;

  double beta_scalar = 2.0;
  std::vector<double> beta{2.0, 4.5, -1.0};
  Eigen::VectorXd beta_vector(3);
  beta_vector << 2.0, 4.5, -1.0;
  Eigen::RowVectorXd beta_vector_t(3);
  beta_vector_t = beta_vector;

  // Can't use VectorRNGTestRig since length(alpha) != length(beta) in general.

  EXPECT_NO_THROW(stan::math::bernoulli_logit_glm_rng(x, alpha, beta, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_scalar, beta, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_vector, beta, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_vector_t, beta, rng));

  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha, beta_scalar, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_scalar, beta_scalar, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_vector, beta_scalar, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_vector_t, beta_scalar, rng));

  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha, beta_vector, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_scalar, beta_vector, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_vector, beta_vector, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_vector_t, beta_vector, rng));

  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha, beta_vector_t, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_scalar, beta_vector_t, rng));
  EXPECT_NO_THROW(
      stan::math::bernoulli_logit_glm_rng(x, alpha_vector, beta_vector_t, rng));
  EXPECT_NO_THROW(stan::math::bernoulli_logit_glm_rng(x, alpha_vector_t,
                                                      beta_vector_t, rng));
}

TEST(ProbDistributionsBernoulliLogitGlm, errorCheck) {
  // Check errors for nonfinite and wrong sizes.
  boost::random::mt19937 rng;

  int N = 3;
  int M = 2;
  int W = 4;

  Eigen::MatrixXd x = Eigen::MatrixXd::Random(N, M);
  Eigen::MatrixXd xw1 = Eigen::MatrixXd::Random(W, M);
  Eigen::MatrixXd xw2 = Eigen::MatrixXd::Random(N, W);
  Eigen::MatrixXd xw3 = Eigen::MatrixXd::Random(N, M) * NAN;
  Eigen::VectorXd alpha = Eigen::VectorXd::Random(N, 1);
  Eigen::VectorXd alphaw1 = Eigen::VectorXd::Random(W, 1);
  Eigen::VectorXd alphaw2 = Eigen::VectorXd::Random(N, 1) * NAN;
  Eigen::VectorXd beta = Eigen::VectorXd::Random(M, 1);
  Eigen::VectorXd betaw1 = Eigen::VectorXd::Random(W, 1);
  Eigen::VectorXd betaw2 = Eigen::VectorXd::Random(M, 1) * NAN;

  EXPECT_NO_THROW(stan::math::bernoulli_logit_glm_rng(x, alpha, beta, rng));
  EXPECT_THROW(stan::math::bernoulli_logit_glm_rng(xw1, alpha, beta, rng),
               std::invalid_argument);
  EXPECT_THROW(stan::math::bernoulli_logit_glm_rng(xw2, alpha, beta, rng),
               std::invalid_argument);
  EXPECT_THROW(stan::math::bernoulli_logit_glm_rng(xw3, alpha, beta, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::bernoulli_logit_glm_rng(x, alphaw1, beta, rng),
               std::invalid_argument);
  EXPECT_THROW(stan::math::bernoulli_logit_glm_rng(x, alphaw2, beta, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::bernoulli_logit_glm_rng(x, alpha, betaw1, rng),
               std::invalid_argument);
  EXPECT_THROW(stan::math::bernoulli_logit_glm_rng(x, alpha, betaw2, rng),
               std::domain_error);
}

TEST(ProbDistributionsBernoulliLogitGlm, marginalChiSquareGoodnessFitTest) {
  // Check distribution of result.
  boost::random::mt19937 rng;
  Eigen::MatrixXd x(2, 2);
  x << 3.5, -1.5, 2.0, -1.2;
  std::vector<double> alpha{2.0, 1.0};
  std::vector<double> beta{2.0, 4.5};

  //  sage: x = matrix([[3.5, -1.5], [2.0, -1.2]])
  //  sage: alpha = matrix([[2.0], [1.0]])
  //  sage: beta = matrix([[2.0], [4.5]])
  //  sage: z = alpha + x * beta
  //  sage: z
  //
  //  [  2.25000000000000]
  //  [-0.399999999999999]
  //  sage: p1 = 1 / (1 + e**(-z[0][0]))
  //  sage: p2 = 1 / (1 + e**(-z[1][0]))
  //  sage: p1
  //  0.904650535100891
  //  sage: p2
  //  0.401312339887548

  double p1 = 0.904650535100891;
  double p2 = 0.401312339887548;

  int N = 10000;

  // First bin is failures, second is successes. Take N samples, take
  // their first component. Should be (1 - p1) * N failures in the
  // first bin, or thereabouts. Now take their second
  // component. Should be (1 - p2) * N failures in the first bin.
  std::vector<double> bin_boundaries{0.1, 1.1};
  std::vector<double> proportions1{(1 - p1), p1};
  std::vector<double> proportions2{(1 - p2), p2};

  std::vector<double> samples1;
  std::vector<double> samples2;
  for (int i = 0; i < N; ++i) {
    std::vector<int> sample
        = stan::math::bernoulli_logit_glm_rng(x, alpha, beta, rng);
    samples1.push_back(sample[0]);
    samples2.push_back(sample[1]);
  }

  assert_matches_bins(samples1, bin_boundaries, proportions1, 1e-6);
  assert_matches_bins(samples2, bin_boundaries, proportions2, 1e-6);
}


/**
 * @file nmf_test.cpp
 * @author Mohan Rajendran
 *
 * Test file for NMF class.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/amf/amf.hpp>
#include <mlpack/methods/amf/init_rules/random_acol_init.hpp>
#include <mlpack/methods/amf/update_rules/nmf_mult_div.hpp>
#include <mlpack/methods/amf/update_rules/nmf_als.hpp>
#include <mlpack/methods/amf/update_rules/nmf_mult_dist.hpp>

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

BOOST_AUTO_TEST_SUITE(NMFTest);

using namespace std;
using namespace arma;
using namespace mlpack;
using namespace mlpack::amf;

/**
 * Check the if the product of the calculated factorization is close to the
 * input matrix. Default case.
 */
BOOST_AUTO_TEST_CASE(NMFDefaultTest)
{
  mat w = randu<mat>(20, 12);
  mat h = randu<mat>(12, 20);
  mat v = w * h;
  size_t r = 12;

  AMF<> nmf;
  nmf.Apply(v, r, w, h);

  mat wh = w * h;

  // Make sure reconstruction error is not too high.  5.0% tolerance.
  BOOST_REQUIRE_SMALL(arma::norm(v - wh, "fro") / arma::norm(v, "fro"),
      0.05);
}

/**
 * Check the if the product of the calculated factorization is close to the
 * input matrix. Random Acol initialization distance minimization update.
 */
BOOST_AUTO_TEST_CASE(NMFAcolDistTest)
{
  mat w = randu<mat>(20, 12);
  mat h = randu<mat>(12, 20);
  mat v = w * h;
  const size_t r = 12;

  SimpleResidueTermination srt(1e-7, 10000);
  AMF<SimpleResidueTermination,RandomAcolInitialization<> >
        nmf(srt);
  nmf.Apply(v, r, w, h);

  mat wh = w * h;

  BOOST_REQUIRE_SMALL(arma::norm(v - wh, "fro") / arma::norm(v, "fro"),
      0.015);
}

/**
 * Check the if the product of the calculated factorization is close to the
 * input matrix. Random initialization divergence minimization update.
 */
BOOST_AUTO_TEST_CASE(NMFRandomDivTest)
{
  mat w = randu<mat>(20, 12);
  mat h = randu<mat>(12, 20);
  mat v = w * h;
  size_t r = 12;

  // Custom tighter tolerance.
  SimpleResidueTermination srt(1e-8, 10000);
  AMF<SimpleResidueTermination,
      RandomInitialization,
      NMFMultiplicativeDivergenceUpdate> nmf(srt);
  nmf.Apply(v, r, w, h);

  mat wh = w * h;

  // Make sure reconstruction error is not too high.  1.5% tolerance.
  BOOST_REQUIRE_SMALL(arma::norm(v - wh, "fro") / arma::norm(v, "fro"),
      0.015);
}

/**
 * Check that the product of the calculated factorization is close to the
 * input matrix.  This uses the random initialization and alternating least
 * squares update rule.
 */
BOOST_AUTO_TEST_CASE(NMFALSTest)
{
  mat w = randu<mat>(20, 12);
  mat h = randu<mat>(12, 20);
  mat v = w * h;
  size_t r = 12;

  SimpleResidueTermination srt(1e-12, 50000);
  AMF<SimpleResidueTermination, RandomAcolInitialization<>, NMFALSUpdate>
        nmf(srt);
  nmf.Apply(v, r, w, h);

  const mat wh = w * h;

  // Make sure reconstruction error is not too high.  8% tolerance.  It seems
  // like ALS doesn't converge to results that are as good.  It also seems to be
  // particularly sensitive to initial conditions.
  BOOST_REQUIRE_SMALL(arma::norm(v - wh, "fro") / arma::norm(v, "fro"),
      0.08);
}

/**
 * Check the if the product of the calculated factorization is close to the
 * input matrix, with a sparse input matrix. Random Acol initialization,
 * distance minimization update.
 */
BOOST_AUTO_TEST_CASE(SparseNMFAcolDistTest)
{
  // We have to ensure that the residues aren't NaNs.  This can happen when a
  // matrix is created with all zeros in a column or row.
  double denseResidue = std::numeric_limits<double>::quiet_NaN();
  double sparseResidue = std::numeric_limits<double>::quiet_NaN();

  mat vp, dvp; // Resulting matrices.

  while (sparseResidue != sparseResidue && denseResidue != denseResidue)
  {
    mlpack::math::RandomSeed(std::time(NULL));
    mat w, h;
    sp_mat v;
    v.sprandu(20, 20, 0.3);
    // Ensure there is at least one nonzero element in every row and column.
    for (size_t i = 0; i < 20; ++i)
      v(i, i) += 1e-5;
    mat dv(v); // Make a dense copy.
    mat dw, dh;
    size_t r = 15;

    SimpleResidueTermination srt(1e-10, 10000);
    AMF<SimpleResidueTermination, RandomAcolInitialization<> > nmf(srt);
    const size_t seed = mlpack::math::RandInt(1000000);
    mlpack::math::RandomSeed(seed); // Set random seed so results are the same.
    nmf.Apply(v, r, w, h);
    mlpack::math::RandomSeed(seed);
    nmf.Apply(dv, r, dw, dh);

    // Reconstruct matrices.
    vp = w * h;
    dvp = dw * dh;

    denseResidue = arma::norm(v - vp, "fro");
    sparseResidue = arma::norm(dv - dvp, "fro");
  }

  // Make sure the results are about equal for the W and H matrices.
  BOOST_REQUIRE_SMALL(arma::norm(vp - dvp, "fro") / arma::norm(vp, "fro"),
      1e-5);
}

/**
 * Check that the product of the calculated factorization is close to the
 * input matrix, with a sparse input matrix.  This uses the random
 * initialization and alternating least squares update rule.
 */
BOOST_AUTO_TEST_CASE(SparseNMFALSTest)
{
  // We have to ensure that the residues aren't NaNs.  This can happen when a
  // matrix is created with all zeros in a column or row.
  double denseResidue = std::numeric_limits<double>::quiet_NaN();
  double sparseResidue = std::numeric_limits<double>::quiet_NaN();

  mat vp, dvp; // Resulting matrices.

  while (sparseResidue != sparseResidue && denseResidue != denseResidue)
  {
    mlpack::math::RandomSeed(std::time(NULL));
    mat w, h;
    sp_mat v;
    v.sprandu(10, 10, 0.3);
    // Ensure there is at least one nonzero element in every row and column.
    for (size_t i = 0; i < 10; ++i)
      v(i, i) += 1e-5;
    mat dv(v); // Make a dense copy.
    mat dw, dh;
    size_t r = 5;

    SimpleResidueTermination srt(1e-10, 10000);
    AMF<SimpleResidueTermination, RandomInitialization, NMFALSUpdate> nmf(srt);
    const size_t seed = mlpack::math::RandInt(1000000);
    mlpack::math::RandomSeed(seed);
    nmf.Apply(v, r, w, h);
    mlpack::math::RandomSeed(seed);
    nmf.Apply(dv, r, dw, dh);

    // Reconstruct matrices.
    vp = w * h; // In general vp won't be sparse.
    dvp = dw * dh;

    denseResidue = arma::norm(v - vp, "fro");
    sparseResidue = arma::norm(dv - dvp, "fro");
  }

  // Make sure the results are about equal for the W and H matrices.
  BOOST_REQUIRE_SMALL(arma::norm(vp - dvp, "fro") / arma::norm(vp, "fro"),
      1e-5);
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once

#include "Base/TQuaternion.h"

#include <boost/bind.hpp>

#include "vector.hpp"
#include "gmres.hpp"
#include "integrate.hpp"

template<int DIMX, int DIMUC>
class Model{
public:
	static const int DimX = DIMX;
	static const int DimU = DIMUC;

	using x_t = vector_t<DIMX>;		///< Vector for a state vector
    using u_t =  vector_t<DIMUC>;	///< Vector for a control input
	struct xu_t : vector_t<DIMX + DIMUC>{	///< Integrated structure for x and u
	x_t& x(){ return *(x_t*)this; }
	u_t& u(){ return *(u_t*)((double*)this + DIMX); }
	const x_t& x()const{ return *(const x_t*)this; }
	const u_t& u()const{ return *(const u_t*)((const double*)this + DIMX); }
	};

	x_t    x0;		///< Initial state
	u_t    u0;		///< Initial input

	x_t    x;		///< Current state
	x_t    x1;		///< State in next step
	u_t    u;		///< Control input

	/*-------------- dPhi/dx -------------- */
	virtual void phix(double t, const x_t& x, x_t& phx1) = 0;

	/*-------------- State Equation -------------- */
	virtual void xpfunc(double t, const x_t& x, const u_t& u, x_t& xprime) = 0;

	/*-------------- Costate Equation -------------- */
	virtual void lpfunc(double t, const x_t& lmd, const xu_t& linp, x_t& lprime) = 0;

	/*-------------- Error in Optimality Condition, Hu -------------- */
	virtual void hufunc(double t, const x_t& x, const x_t& lmd, const u_t& u, u_t& hui) = 0;
};

template<class MODEL, int N, int KMAX>
class Controller{
public:
	static const int DimX = MODEL::DimX;	//< Dimension of state vector
	static const int DimU = MODEL::DimU;	//< Dimension of control input
	static const int dv   = N;				//< Number of steps in prediction horizon
	static const int kmax = KMAX;			//< Iteration number of FDGMRES

	using controller_t = Controller<MODEL, N, KMAX>;
	using model_t = MODEL;
    using x_t = typename MODEL::x_t;
    using u_t = typename MODEL::u_t;
    using xu_t = typename MODEL::xu_t;

	/// State sequence
	struct X_t : vector_t<DimX * (N+1)>{
		template<class T> X_t& operator=(const T& v){ assign(v); return *this; }
		x_t& elem(int i){ return ((x_t*)this)[i]; }
		const x_t& elem(int i)const{ return ((const x_t*)this)[i]; }
	};
	/// Control input sequence
	struct U_t : vector_t<DimU * N>{
		template<class T> U_t& operator=(const T& v){ this->assign(v); return *this; }
		u_t& elem(int i){ return ((u_t*)this)[i]; }
		const u_t& elem(int i)const { return ((const u_t*)this)[i]; }
	};

	double tf;		//< Prediction time
	double ht;		//< Control period
	double alpha;	//< Time constant in case of time-dependent horizon(?)
	double zeta;	//< Feedback gain in Eq.9
	double hdir;	//< h in Eq.11
	double rtol;	//< Tolerance on r for calculating the initial control input
	int    dstep;	//< ?

	double htau;	//< Period in prediction horizon (=tf/dv)
	double onemhdir;	//< 1-hdir
	double hdirbht;		//< hdir/ht
	double onemzetahdir; 	//< 1-zeta*hdir
	double ts;		//< t+hdir
	double tauf;	//< Compensated prediction time in prediction horizon

	x_t x1s;
	x_t	lmd0;
	u_t hu0;
	X_t xtau;
	X_t xtau1;
	X_t ltau;
	U_t duvec;
	U_t utau;
	U_t utau1;
	U_t hutau;
	U_t hutau1;
	U_t hutau2;
	U_t bvec;
	//U_t dutmp;
	U_t utautmp;
	//U_t ptau; //< PrARXモデルのモード出力用　@2018.07.13

	vector_t<dv  +1> tau;
	vector_t<kmax+1> errvec;

	model_t*	model;

public:
	// Constructor
	Controller(){}
	Controller(
               double in_ht,
               double in_zeta,
               double in_hdir,
               double in_tf,
               double in_alpha,
               double in_rtol,
               int in_dstep):
            ht(in_ht), tf(in_tf), alpha(in_alpha),
            zeta(in_zeta), hdir(in_hdir),
            rtol(in_rtol), dstep(in_dstep) {}

	/*-------------- Initial Conditions -------------- */
	/*struct dhu0func{
		void operator()(const u_t& du0, u_t& dhu){
			u_t u;
			u_t hu;

			u = *(u_t*)u0 + hdir * du0;
			hufunc(tsim0, x0, lmd0, u, hu);
			dhu = (hu - hu0) / hdir;
		}
	};*/

	void init(model_t* m){
		model = m;

		onemhdir     = 1 - hdir / ht;
		hdirbht      = hdir / ht;
		onemzetahdir = 1 - zeta * hdir;

		int i;
		//double r;
		vector_t<DimU>   b;
		vector_t<DimU>   du0;
		vector_t<DimU+1> erru0;
		model->x = model->x0;
		// phi_x(x, t)
		model->phix(tsim0, model->x0, lmd0);
		// Hu(
		model->hufunc(tsim0, model->x0, lmd0, model->u0, hu0);
		/*
		du0.clear();
		r = hu0.norm();
		i = 0;

		// 0ステップの最適化問題を解き，最適入力u0を求める
		while( r > rtol && i < 100 ){
			b = -hu0;
			nfgmres<dhu0func, u_t, DIMUC>(b, du0, erru0);
			*(u_t*)u0 += du0;
			hufunc(tsim0, x0, lmd0, u0, hu0);
			r = hu0.norm();
			i++;
		}
		*/
		// 全時刻のuをu0で埋める
		model->u = model->u0;
		for(i=0; i<dv; i++){
			utau .elem(i) = model->u;
			hutau.elem(i) = hu0;
		}
		duvec.clear();
	}

	/*-------------- Control Update -------------- */

	// F(U,x,t)
	void errfunc(double t, const x_t& x, const U_t& u, U_t& hu)
	{
		int i;
		double taut;
		xu_t linp;

		// Time period in prediction horizon
		//tauf = tf * (1.0 - exp(-alpha * t));
		tauf = tf;
		htau = tauf / dv;

		// Fill the initial state with x given
		xtau.elem(0) = x;

		// Calculate state sequence by recursive forward difference calculation(Eq.(1)）
		x_t xd;
		//u_t pd;
		for(taut = t, i=0; i < dv; taut += htau, i++){
			model->xpfunc(taut, xtau.elem(i), u.elem(i), xd);
			//model->xpfunc(taut, xtau.elem(i), u.elem(i), xd, pd);
			xtau.elem(i+1) = xtau.elem(i) + htau * xd;
			tau[i] = taut;
			//ptau.elem(i) = pd;
		}
		tau[i] = taut;

		// Final condition on the costate lambda（Eq.(7)）
		model->phix(taut, xtau.elem(dv), ltau.elem(dv));

		// Calculate costate sequence by recursive backward difference calculation（Eq.(6)）
		x_t ld;
		for(i = dv-1; i >= 0; i--){
			linp.x() = xtau.elem(i);
			linp.u() = u.elem(i);
			model->lpfunc(taut, ltau.elem(i+1), linp, ld);
			ltau.elem(i) = ltau.elem(i+1) - htau * ld;
			taut -= htau;
			// Calculate Hu from predicted x and lambda（Eq.(5)）
			model->hufunc(taut, xtau.elem(i), ltau.elem(i+1), ((U_t&)u).elem(i), hu.elem(i));
		}
	}

	void adufunc(const U_t& du, U_t& adu){
		// U + h dU
		utau1 = utau + du * hdir;
		// F(U + hdU, x + h xd, t + h)
		errfunc(ts, x1s, utau1, hutau2);
		// F_U dU approx [F(U + h dU, x + h xd, t + h) - F(U, x + h xd, t + h)] / h
		adu = (hutau2 - hutau1) / hdir;
	}
	struct call_adufunc{
		controller_t* ctrl;
		void operator()(const U_t& du, U_t& adu){
			ctrl->adufunc(du, adu);
		}
		call_adufunc(controller_t* c):ctrl(c){}
	};

	void unew(double t, const x_t& x, const x_t& x1, u_t& u){
		// t + h
		ts = t + hdir;

		 //前進差分計算のためにxの時間微分xdが必要だが，実際の計算ではxdを求めないため，ここで差分で近似している
		 //xd = (x1 - x) / dt
		 //x1s = x + h xd = x + (x1 - x) * (h/dt) = (h/dt) x1 + (1 - h/dt) x
		x1s = hdirbht * x1 + x * onemhdir;

		// F(t, x, u)
		errfunc(t, x, utau, hutau);

		// F(t + h, x + h xd, u)
		errfunc(ts, x1s, utau, hutau1);

		////< x1s = x1 + h xd = x1 + (x1 - x) * (h/dt) = -(h/dt)x + (1 + h/dt)x1
		//x1s = -hdirbht * x + x1 * onephdir;

		//// F(t, x1, u)
		//errfunc(ts, x1, utau, hutau);

		//// F(t + h, x1 + h xd, u)
		//errfunc(t, x1s, utau, hutau1);

		// b on Eq.(11)
		// b = As F(U,x,t) - DhF(U,x,t:0,xd,1),
		// From DhF(U,x,t:0,xd,1) = (F(U, x + h xd, t+h) - F(U,x,t))/h,
		// b = [(1 + h As) F(U,x,t) - F(U, x + h xd, t+h)] / h
		bvec = (hutau * onemzetahdir - hutau1) / hdir;

		// Solve F_U Ud = b by GMRES
		// adufunc is a function that gives A*dU while A is F_U here.
		nfgmres<call_adufunc, U_t, kmax>(call_adufunc(this), bvec, duvec, errvec);

		// U += dU * ht;
		utau += ht * duvec;

		// Limit the control inputs with upper and lower bound
		for(int i = 0; i < N; i++)for(int j = 0; j < model->NCar; j++){
			if(utau.elem(i)[j] > model->umax){
					utau.elem(i)[j] = model->umax;
			}
			else if(utau.elem(i)[j] < model->umin){
					utau.elem(i)[j] = model->umin;
			}
			duvec = (utau - utautmp)/ht;
		}
		errfunc(t,x1,utau,hutau1);

		//double lim = 1000;
		//if(abs(hutau1.norm()) > lim){
		//for(int i = 0; i < N; i++)for(int j = 0; j < modeltmp.NCar; j++){
		//	if(xtau.elem(i)[3*j+0] <= modeltmp.Dmin+0.1){
		//		utau.elem(i)[j] *= utau.elem(i)[j]>0?0.1:1.5;
		//		//if(utau.elem(i)[j] < modeltmp.umin[j]) utau.elem(i)[j] = modeltmp.umin[j];
		//		//utau.elem(i)[j] = modeltmp.umin[j];
		//	}
		//	if(xtau.elem(i)[3*j+0] >= modeltmp.Dmin && abs(utau.elem(i)[2*modeltmp.NCar + j])>lim){
		//		utau.elem(i)[1*modeltmp.NCar+j] = sqrt(xtau.elem(i)[3*j+0]-modeltmp.Dmin);
		//		utau.elem(i)[2*modeltmp.NCar+j] = modeltmp.s1[j]/(2*modeltmp.sc[j]*utau.elem(i)[1*modeltmp.NCar+j]);
		//	}
		//}
		//duvec = (utau - utautmp)/ht;
		//errfunc(t,x1,utau,hutau1);
		//}

		//// もし更新したutauでのhutauが更新前より悪化した場合は戻す
		//if(hutau1.norm() > hutau.norm()){
		//	utau = utautmp;
		//	duvec = (utau-utautmp)/ht;
		//	errfunc(t,x1,utau,hutau1);
		//}

		// Apply the first elements of U as actual input u
		u = utau.elem(0);
		utautmp = utau;
		//dutmp = duvec;
	}

	private:
        const double tsim0 = 0.0;
};


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/count_if.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/mod.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
namespace hana = boost::hana;
using namespace hana::literals;

auto is_odd = [](auto x) {
    return x % 2_c != 0_c;
};

int main() {
    constexpr auto ints = hana::tuple_c<int, 1, 2, 3>;
    BOOST_HANA_CONSTANT_CHECK(hana::count_if(ints, is_odd) == hana::size_c<2>);

    constexpr auto types = hana::tuple_t<int, char, long, short, char, double>;
    BOOST_HANA_CONSTANT_CHECK(hana::count_if(types, hana::trait<std::is_floating_point>) == hana::size_c<1>);
    BOOST_HANA_CONSTANT_CHECK(hana::count_if(types, hana::equal.to(hana::type_c<char>)) == hana::size_c<2>);
    BOOST_HANA_CONSTANT_CHECK(hana::count_if(types, hana::equal.to(hana::type_c<void>)) == hana::size_c<0>);
}


#include <fstream>
#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/random.hpp>

#include "range.hpp"

using namespace std;

template<class Graph, class Generator>
Graph random_graph(int n, double p, Generator generator) {
  Graph g(n);
  bernoulli_distribution distribution(p);
  auto trial = bind(distribution, generator);
  for(int i = 0; i < n; ++i)
    for(int j = i + 1; j < n; ++j)
      if(trial()) add_edge(i, j, g);
  return g;
}

class Timing {
  timespec c_start, c_end;
  clockid_t c_id;
  vector<time_t> times;
public:
  Timing(clockid_t id) : c_id(id) {}
  void start() {
    clock_gettime(c_id, &c_start);
  }
  void stop() {
    clock_gettime(c_id, &c_end);
  }
  void tick() {
    stop();
    times.push_back(elapsed());
    start();
  }
  time_t begin() const {
    return c_start.tv_sec * 1e9 + c_start.tv_nsec;
  }
  time_t end() const {
    return c_end.tv_sec * 1e9 + c_end.tv_nsec;
  }
  time_t elapsed() const {
    return end() - begin();
  }
  const vector<time_t>& elems() {
    return times;
  }
  void clear() {
    times.clear();
  }
};

template <class Graph>
void test(string name) {
  default_random_engine gen;
  ofstream file(name);
  Timing t(CLOCK_PROCESS_CPUTIME_ID);
  for(int n = 100; n <= 1000; n += 100) {
    cerr << n << endl;
    for(int j = 0; j < 100; ++j) {
      auto g = random_graph<Graph>(n, 0.1, gen);

      int foo = 0;
      t.start();
      for(auto v : range(vertices(g))) ++foo;
      t.stop();
      auto t1 = t.elapsed();

      t.start();
      for(auto e : range(edges(g))) --foo;
      t.stop();
      auto t2 = t.elapsed();

      for(int i = 0; i < 10; ++i) {
        auto v = random_vertex(g, gen);
        auto w = random_vertex(g, gen);

        t.start();
        auto e = edge(v, w, g);
        t.tick();
        auto d = out_degree(v, g);
        t.tick();
        for(auto x : range(adjacent_vertices(v, g))) --d;
        t.tick();

        if(e.second) remove_edge(v, w, g);

        t.start();
        add_edge(v, w, g);
        t.tick();
        remove_edge(v, w, g);
        t.tick();

        if(e.second) add_edge(v, w, g);

        t.start();
        source(e.first, g);
        t.tick();
        target(e.first, g);
        t.tick();

        assert(d == 0 && foo < 0);

        file << n << ' ' << t1 << ' ' << t2 << ' ';
        for(auto x : t.elems()) file << x << ' ';
        file << endl;
        t.clear();
      }
    }
  }
  file.close();
}

int main() {
  using namespace boost;
  test<adjacency_list<vecS, vecS, undirectedS>>("vec.txt");
  test<adjacency_list<listS, vecS, undirectedS>>("list.txt");
  test<adjacency_list<slistS, vecS, undirectedS>>("slist.txt");
  test<adjacency_list<setS, vecS, undirectedS>>("set.txt");
  test<adjacency_list<multisetS, vecS, undirectedS>>("multiset.txt");
  test<adjacency_list<hash_setS, vecS, undirectedS>>("hashset.txt");
  test<adjacency_matrix<undirectedS>>("matrix.txt");
  return 0;
}