#include <iostream>
#include <stdlib.h>
#include <cmath>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/timer/timer.hpp>

namespace ublas  = boost::numeric::ublas;

template<class mat, class vec>
double diff(const mat& A, const vec& x, const vec& b) {
  vec temp(prod(A, x) - b);
  double result = 0;
  for (typename vec::size_type i=0; i<temp.size(); ++i) {
    result += temp(i)*temp(i);
  }
  return std::sqrt(result);
}

template<class mat, class vec>
double diff(const vec& x, const mat& A, const vec& b) {
  return diff(trans(A), x, b);
}

namespace ublas  = boost::numeric::ublas;

int main() {
  const int n=7000;
#if 1
  ublas::compressed_matrix<double, ublas::row_major>     mat_row_upp(n, n);
  ublas::compressed_matrix<double, ublas::column_major>  mat_col_upp(n, n);
  ublas::compressed_matrix<double, ublas::row_major>     mat_row_low(n, n);
  ublas::compressed_matrix<double, ublas::column_major>  mat_col_low(n, n);
#else
  ublas::matrix<double, ublas::row_major>     mat_row_upp(n, n, 0);
  ublas::matrix<double, ublas::column_major>  mat_col_upp(n, n, 0);
  ublas::matrix<double, ublas::row_major>     mat_row_low(n, n, 0);
  ublas::matrix<double, ublas::column_major>  mat_col_low(n, n, 0);
#endif
  ublas::vector<double>  b(n, 1);

  std::cerr << "Constructing..." << std::endl;
  for (int i=0; i<n; ++i) {
    b(i) = std::rand() % 10;
    double main = -10 + std::rand() % 20 ;
    if (main == 0) main+=1;
    double side = -10 + std::rand() % 20 ;
    if (i-1>=0) {
      mat_row_low(i, i-1) = side;
    }
    mat_row_low(i, i) = main;

    mat_col_low(i, i) = main;
    if (i+1<n) {
      mat_col_low(i+1, i) = side;
    }

    mat_row_upp(i, i) = main;
    if (i+1<n) {
      mat_row_upp(i, i+1) = side;
    }

    if (i-1>=0) {
      mat_col_upp(i-1, i) = side;
    }
    mat_col_upp(i, i) = main;
  }

  std::cerr << "Starting..." << std::endl;
  {
    boost::timer::auto_cpu_timer t(std::cerr, "col_low x: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(mat_col_low,  x, ublas::lower_tag());
    std::cerr << "delta: " << diff(mat_col_low, x, b) << "\n";
  }
  {
    boost::timer::auto_cpu_timer t(std::cerr, "row_low x: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(mat_row_low, x, ublas::lower_tag());
    std::cerr << "delta: " << diff(mat_row_low, x, b) << "\n";
  }

  {
    boost::timer::auto_cpu_timer t(std::cerr, "col_upp x: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(mat_col_upp,  x, ublas::upper_tag());
    std::cerr << "delta: " << diff(mat_col_upp, x, b) << "\n";
  }
  {
    boost::timer::auto_cpu_timer t(std::cerr, "row_upp x: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(mat_row_upp, x, ublas::upper_tag());
    std::cerr << "delta: " << diff(mat_row_upp, x, b) << "\n";
  }

  {
    boost::timer::auto_cpu_timer t(std::cerr, "x col_low: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, mat_col_low, ublas::lower_tag());
    std::cerr << "delta: " << diff(x, mat_col_low, b) << "\n";
  }
  {
    boost::timer::auto_cpu_timer t(std::cerr, "x row_low: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, mat_row_low, ublas::lower_tag());
    std::cerr << "delta: " << diff(x, mat_row_low, b) << "\n";
  }

  {
    boost::timer::auto_cpu_timer t(std::cerr, "x col_upp: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, mat_col_upp, ublas::upper_tag());
    std::cerr << "delta: " << diff(x, mat_col_upp, b) << "\n";
  }
  {
    boost::timer::auto_cpu_timer t(std::cerr, "x row_upp: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, mat_row_upp, ublas::upper_tag());
    std::cerr << "delta: " << diff(x, mat_row_upp, b) << "\n";
  }

}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/extensions/gis/latlong/point_ll.hpp>

#include <test_common/test_point.hpp>

template <typename P>
void test_all()
{
    typedef typename bg::coordinate_type<P>::type type;

    P p1(bg::latitude<type>(bg::dms<bg::south, type>(12, 2, 36)),
         bg::longitude<type>(bg::dms<bg::west, type>(77, 1, 42)));

    // Check decimal/degree conversion
    BOOST_CHECK_CLOSE(bg::get<0>(p1), type(-77.0283), 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(p1), type(-12.0433), 0.001);

    // Check degree/radian conversion
    bg::model::ll::point<bg::radian, type> p2;
    bg::transform(p1, p2);

    BOOST_CHECK_CLOSE(bg::get<0>(p2), type(-1.3444), 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(p2), type(-0.210196), 0.001);

    // Check degree/radian conversion back
    P p3;
    bg::transform(p2, p3);
    BOOST_CHECK_CLOSE(bg::get<0>(p3), type(-77.0283), 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(p3), type(-12.0433), 0.001);

    // Check decimal/degree conversion back
    int d;
    int m;
    double s;
    bool positive;
    char cardinal;

    bg::dms<bg::cd_lat, type> d1(bg::get<0>(p3));
    d1.get_dms(d, m, s, positive, cardinal);

    BOOST_CHECK(d == 77);
    BOOST_CHECK(m == 1);
    BOOST_CHECK_CLOSE(s, double(42), 0.1);
    BOOST_CHECK(positive == false);
    BOOST_CHECK(cardinal == 'S');

    // Check dd conversion as string, back. We cannot do that always because of the precision.
    // Only double gives correct results
    //std::string st = d1.get_dms();
    //std::cout << st << std::endl;
    //BOOST_CHECK(st == "77 1'42\" S");
}

int test_main(int, char* [])
{
    test_all<bg::model::ll::point<bg::degree, float> >();
    test_all<bg::model::ll::point<bg::degree, double> >();

    return 0;
}


#pragma once
#include <type_traits>

#include <Eigen/Core>

namespace ubs {
namespace internal {
/**
 * @brief The smoothness cost functor.
 *
 * This cost functor is used to calculate the smoothness of a spline during optimization. The operation for
 * uniform B-splines boils down to a matrix matrix product, where the first matrix is build by concatenating the
 * control points and the second one is the fixed smoothing matrix.
 *
 * @tparam N_ The number of control points the cost function depends on.
 */
template <int OutputDims_, int N_>
class SmoothnessCostFunctor {
private:
    /* @brief Flag, which indicates if dynamic sized matrices should be used.
     *
     * Due to stack limitation of the number of elements of the matrix use Eigen::Dynamic for big ones.
     */
    static constexpr bool UseDynamicSizedMatrix_ = (N_ * OutputDims_) > 16;
    static constexpr int EigenN_ = UseDynamicSizedMatrix_ ? Eigen::Dynamic : N_;

    /**
     * @brief Helper function to create the control points matrix.
     *
     * This overload is picked if fixed size matrices are used.
     *
     * @tparam T The value type of the matrix.
     * @return The control points matrix with the correct size.
     */
    template <typename T>
    UBS_NO_DISCARD Eigen::Matrix<T, OutputDims_, EigenN_> makeControlPoints(std::false_type /* isDynamic */) const {
        return {};
    }

    /**
     * @brief Helper function to create the control points matrix.
     *
     * This overload is picked if dynamic sized matrices are used.
     *
     * @tparam T The value type of the matrix.
     * @return The control points matrix with the correct size.
     */
    template <typename T>
    UBS_NO_DISCARD Eigen::Matrix<T, OutputDims_, EigenN_> makeControlPoints(std::true_type /* isDynamic */) const {
        return {OutputDims_, N_};
    }

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /**
     * @brief The constructor.
     * @param[in] m The smoothness cost factors.
     */
    explicit SmoothnessCostFunctor(Eigen::Matrix<double, N_, N_> m) : m_{std::move(m)} {
    }

    /**
     * @brief Cost function used in DynamicAutoDiffCostFunction.
     * @param[in] controlPointsRaw The control points pointer.
     * @param[out] residualRaw The residual pointer.
     * @return True on success, otherwise false.
     */
    template <typename T>
    bool operator()(T const* const* controlPointsRaw, T* residualRaw) const {
        // Copy the control points to an Eigen matrix.
        auto controlPoints = makeControlPoints<T>(std::integral_constant<bool, UseDynamicSizedMatrix_>());
        for (int i = 0; i < N_; ++i) {
            controlPoints.col(i) = Eigen::Map<const Eigen::Matrix<T, OutputDims_, 1>>(controlPointsRaw[i]);
        }

        // Evaluate smoothness values.
        Eigen::Map<Eigen::Matrix<T, OutputDims_, N_>> residuals(residualRaw);
        residuals = controlPoints * m_.template cast<T>();

        return true;
    }

    /**
     * @brief Cost function used in AutoDiffCostFunction.
     *
     * Enable if is necessary because other the wrong overload would be picked if it is used in a dynamic cost
     * function.
     *
     * @param[in] controlPoint The control points.
     * @param[in,out] ts The other control points and the residual pointer.
     * @return True on success, otherwise false.
     */
    template <typename T, typename... Ts>
    typename std::enable_if<!std::is_pointer<T>::value, bool>::type operator()(const T* controlPoint, Ts*... ts) const {
        static_assert(N_ == sizeof...(Ts), "Invalid number of control points specified.");

        // Build control points array.
        std::array<const T*, sizeof...(Ts) + 1> controlPointsRaw{{controlPoint, ts...}};

        // The last element of the control points is the place to store the output.
        T* residualRaw = std::get<sizeof...(Ts)>(std::make_tuple(controlPoint, ts...));

        return this->operator()(controlPointsRaw.data(), residualRaw);
    }

private:
    /** @brief The smoothness matrix. */
    Eigen::Matrix<double, EigenN_, EigenN_> m_;
};

} // namespace internal
} // namespace ubs


#define BOOST_TEST_MODULE CNRTests

#include <boost/test/unit_test.hpp>

#include <bts/utilities/combinatorics.hpp>
#include <fc/exception/exception.hpp>
#include <fc/log/logger.hpp>
#include <fc/thread/thread.hpp>
#include <fc/filesystem.hpp>
#include <fc/network/ip.hpp>
#include <fc/io/json.hpp>

#include <iostream>

BOOST_AUTO_TEST_CASE( util_cnr )
{
    BOOST_CHECK_EQUAL(bts::utilities::cnr(5, 2), 10);

    auto res = bts::utilities::unranking( bts::utilities::cnr(5, 2) - 1, 2, 5);

    std::cout << res.size() << " "<< res[0] << " " << res[1] << "\n";
}


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// Geometric.hpp
// This file is part of the Garamon for project_namespace.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file Geometric.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Recursive geometric product for project_namespace.

#ifndef project_inclusion_guard
#define project_inclusion_guard
#pragma once

#include <Eigen/Core>

#include "project_namespace/Mvec.hpp"
#include "project_namespace/Constants.hpp"

/*!
 * @namespace project_namespace
 */
namespace project_namespace {

    template<typename T> class Mvec;

    /// \brief Recursively compute the geometric product between two multivectors mv1 and mv2, the result is put into the multivector mv3
    /// \tparam the type of value that we manipulate, either float or double or something.
    /// \param mv1 - the first multivector
    /// \param mv2 - the second multivector
    /// \param mv3 - the multivector that will content the result of the operation mv3 = mv1 * mv2
    /// \param grade_mv1 - the grade of the first multivector
    /// \param grade_mv2 - the grade of the second multivector
    /// \param grade_mv3 - could be the grade of the result, however here it is useless due to the fact that the resulting multivector may not be homogeneous
    /// \param currentGradeMv1 - the current grade of the traversed tree of mv1
    /// \param currentGradeMv2 - the current grade of the traversed tree of mv2
    /// \param currentGradeMv3 - the current grade of the traversed tree of mv3
    /// \param sign - compute the sign of the geometric product between two blades
    /// \param complement - activate the flip of sign
    /// \param indexLastVector_mv1 - last vector traversed in the multivector mv1
    /// \param indexLastVector_mv2 - last vector traversed in the multivector mv2
    /// \param indexLastVector_mv3 - last vector traversed in the multivector mv3
    /// \param currentMetricCoefficient - coefficient that is used for handling the metric in the recursive formula
    /// \param depth - depth in the resulting multivector tree
    template<typename T>
    void geoProduct(const Eigen::Matrix<T, Eigen::Dynamic, 1> &mv1,
                    const Eigen::Matrix<T, Eigen::Dynamic, 1> &mv2,
                    Mvec<T> &mv3,          // multivectors to be processed
                    const unsigned int grade_mv1,
                    const unsigned int grade_mv2,
                    const unsigned int grade_mv3,   // grade of the k-vectors
                    unsigned int currentXorIdx1=0, unsigned int currentXorIdx2=0, unsigned int currentXorIdx3=0,    // position in the prefix tree
                    unsigned int currentGradeMv1=0, unsigned int currentGradeMv2=0, unsigned int currentGradeMv3=0, // grade relative to the position in the prefix tree
                    int sign=1, int complement=1,
                    unsigned int indexLastVector_mv1=0, unsigned int indexLastVector_mv2=0, unsigned int indexLastVector_mv3=0,
                    double currentMetricCoefficient=1.0, int depth=0) {

        // sign updating
        int tmpSign = sign;
        if(complement == -1) {
            tmpSign = -tmpSign;
        }

        // when we reach the grade of mv1 and mv2
        if( (currentGradeMv1==grade_mv1) && (currentGradeMv2==grade_mv2) ){
            // do the required computation
            mv3.at(currentGradeMv3, xorIndexToHomogeneousIndex[currentXorIdx3]) += sign * currentMetricCoefficient * mv1(xorIndexToHomogeneousIndex[currentXorIdx1]) * mv2(xorIndexToHomogeneousIndex[currentXorIdx2]);
        }else {
            // if position in the tree for mv3 is not yet of grade of mv3, just call the recursive calls, without computation
            // for each possible children of the current node whose index is given by depth
            for (unsigned int i = (unsigned int) (1 << depth);
                 i < (1 << algebraDimension); i *= 2) {

                unsigned int xorIndexMv1Child = currentXorIdx1 + i; // xor index of the child of the first tree multivector
                unsigned int xorIndexMv2Child = currentXorIdx2 + i; // xor index of the child of the second tree multivector
                unsigned int xorIndexMv3Child = currentXorIdx3 + i; // xor index of the child of the third tree multivector

                // if we reach neither the grade of mv1 nor the grade of mv2 AND if the child of the node of mv1 lead to at least one node whose grade is grade_mv1
                // AND if the child of the node of mv2 lead to at least one node whose grade is grade_mv2
                if ((currentGradeMv1 < grade_mv1) &&
                    ((i << (grade_mv1 - (currentGradeMv1 + 1))) < (1 << algebraDimension)) &&
                    (currentGradeMv2 < grade_mv2) &&
                    ((i << (grade_mv2 - (currentGradeMv2 + 1))) < (1 << algebraDimension))) {
                    geoProduct<T>(mv1, mv2, mv3,
                                  grade_mv1, grade_mv2, grade_mv3,
                                  xorIndexMv1Child, xorIndexMv2Child, currentXorIdx3,
                                  currentGradeMv1 + 1, currentGradeMv2 + 1, currentGradeMv3,
                                  tmpSign, -complement,
                                  i, i << 1, indexLastVector_mv3,
                                  diagonalMetric<T>(depth) * currentMetricCoefficient,
                                  depth + 1); // scalar product part of the geometric product
                }

                // if we do not reach the grade of mv1 AND if the child of the node of mv1 lead to at least one node whose grade is grade_mv1
                if ((currentGradeMv1 < grade_mv1) &&
                    ((i << (grade_mv1 - (currentGradeMv1 + 1))) < (1 << algebraDimension))) {
                    geoProduct<T>(mv1, mv2, mv3,
                                  grade_mv1, grade_mv2, grade_mv3,
                                  xorIndexMv1Child, currentXorIdx2, xorIndexMv3Child,
                                  currentGradeMv1 + 1, currentGradeMv2, currentGradeMv3 + 1,
                                  tmpSign, complement,
                                  i, i << 1, indexLastVector_mv3,
                                  currentMetricCoefficient, depth + 1); // outer part of the geometric product to CHECK
                }

                // if we do not reach the grade of mv2 AND if the child of the node of mv2 lead to at least one node whose grade is grade_mv2
                if ((currentGradeMv2 < grade_mv2) &&
                    ((i << (grade_mv2 - (currentGradeMv2 + 1))) < (1 << algebraDimension))) {
                    geoProduct<T>(mv1, mv2, mv3,
                                  grade_mv1, grade_mv2, grade_mv3,
                                  currentXorIdx1, xorIndexMv2Child, xorIndexMv3Child,
                                  currentGradeMv1, currentGradeMv2 + 1, currentGradeMv3 + 1,
                                  sign, -complement,
                                  indexLastVector_mv1, i << 1, i,
                                  currentMetricCoefficient, depth + 1); // outer part of the geometric product
                }
                depth++;
            }
        }
    }

}/// End of Namespace

#endif // project_inclusion_guard


//  (C) Copyright John Maddock 2005.
//  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)

#ifdef TEST_STD_HEADERS
#include <complex>
#else
#include <boost/tr1/complex.hpp>
#endif

#include "verify_return.hpp"

int main()
{
   verify_return_type(std::arg(0), double(0));
   verify_return_type(std::arg(0.0), double(0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::arg(0.0L), (long double)(0));
#endif
   verify_return_type(std::arg(0.0F), float(0));
   verify_return_type(std::norm(0), double(0));
   verify_return_type(std::norm(0.0), double(0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::norm(0.0L), (long double)(0));
#endif
   verify_return_type(std::norm(0.0F), float(0));
   verify_return_type(std::conj(0), std::complex<double>(0));
   verify_return_type(std::conj(0.0), std::complex<double>(0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::conj(0.0L), std::complex<long double>(0));
#endif
   verify_return_type(std::conj(0.0F), std::complex<float>(0));
   verify_return_type(std::imag(0), double(0));
   verify_return_type(std::imag(0.0), double(0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::imag(0.0L), (long double)(0));
#endif
   verify_return_type(std::imag(0.0F), float(0));
   verify_return_type(std::real(0), double(0));
   verify_return_type(std::real(0.0), double(0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::real(0.0L), (long double)(0));
#endif
   verify_return_type(std::real(0.0F), float(0));
   verify_return_type(std::polar(0), std::complex<double>(0));
   verify_return_type(std::polar(0.0), std::complex<double>(0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::polar(0.0L), std::complex<long double>(0));
#endif
   verify_return_type(std::polar(0.0F), std::complex<float>(0));
   verify_return_type(std::polar(0, 0L), std::complex<double>(0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::polar(0.0, 0.0L), std::complex<long double>(0));
   verify_return_type(std::polar(0.0L, 0.0F), std::complex<long double>(0));
#endif
   verify_return_type(std::polar(0.0F, 0), std::complex<double>(0));

   std::complex<float> f;
   std::complex<double> d;
   std::complex<long double> l;
   float sf;
   double sd;
   long double sl;

   verify_return_type(std::pow(f, f), f);
   verify_return_type(std::pow(f, d), d);
   verify_return_type(std::pow(d, f), d);
   verify_return_type(std::pow(f, l), l);
   verify_return_type(std::pow(l, f), l);
   verify_return_type(std::pow(d, l), l);
   verify_return_type(std::pow(l, d), l);

   verify_return_type(std::pow(f, sf), f);
   verify_return_type(std::pow(f, sd), d);
   verify_return_type(std::pow(d, sf), d);
   verify_return_type(std::pow(f, sl), l);
   verify_return_type(std::pow(l, sf), l);
   verify_return_type(std::pow(d, sl), l);
   verify_return_type(std::pow(l, sd), l);
   verify_return_type(std::pow(f, 0), f);
   verify_return_type(std::pow(d, 0), d);
   verify_return_type(std::pow(l, 0), l);
   verify_return_type(std::pow(f, 0L), d);
   verify_return_type(std::pow(d, 0L), d);
   verify_return_type(std::pow(l, 0L), l);

   verify_return_type(std::pow(sf, f), f);
   verify_return_type(std::pow(sf, d), d);
   verify_return_type(std::pow(sd, f), d);
   verify_return_type(std::pow(sf, l), l);
   verify_return_type(std::pow(sl, f), l);
   verify_return_type(std::pow(sd, l), l);
   verify_return_type(std::pow(sl, d), l);
   verify_return_type(std::pow(2, f), d);
   verify_return_type(std::pow(2, d), d);
   verify_return_type(std::pow(2, l), l);
   verify_return_type(std::pow(2L, f), d);
   verify_return_type(std::pow(2L, d), d);
   verify_return_type(std::pow(2L, l), l);

   verify_return_type(std::tr1::acos(f), f);
   verify_return_type(std::tr1::acos(d), d);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::tr1::acos(l), l);
#endif
   verify_return_type(std::tr1::asin(f), f);
   verify_return_type(std::tr1::asin(d), d);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::tr1::asin(l), l);
#endif
   verify_return_type(std::tr1::atan(f), f);
   verify_return_type(std::tr1::atan(d), d);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::tr1::atan(l), l);
#endif
   verify_return_type(std::tr1::asinh(f), f);
   verify_return_type(std::tr1::asinh(d), d);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::tr1::asinh(l), l);
#endif
   verify_return_type(std::tr1::acosh(f), f);
   verify_return_type(std::tr1::acosh(d), d);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::tr1::acosh(l), l);
#endif
   verify_return_type(std::tr1::atanh(f), f);
   verify_return_type(std::tr1::atanh(d), d);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::tr1::atanh(l), l);
#endif

   //
   // There is a bug in the TR text here, that means we can't always
   // check these as we'd like:
   //
#if !(defined(__GNUC__) && defined(BOOST_HAS_TR1_COMPLEX_INVERSE_TRIG) && !defined(_GLIBCXX_INCLUDE_AS_CXX0X))
   verify_return_type(std::tr1::fabs(f), sf);
   verify_return_type(std::tr1::fabs(d), sd);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   verify_return_type(std::tr1::fabs(l), sl);
#endif
#endif
   return 0;
}


#define DERIVE_TIMEDNESS_FROM_NUMBER_OF_TEMPLATE_ARGUMENTS 1
#include <value_grid.hpp>
#include <Eigen/Dense>
void print_first(const multi_array<float, 3, 4, 5>& x){
    std::cout << x[0][0][0] << "\n";
}
int main(){
    multi_array <float, 3> x(3,4,5);
    multi_array <float, 3> y(10, 10, 20);

    for(size_t i = 0;i < x.m_im.extent<0>();i++){
        for(size_t j = 0;j < x.m_im.extent<1>();j++){
            for(size_t k = 0;k < x.m_im.extent<2>();k++){
                //std::cout << x(i,j,k) << ", ";
                x(i,j,k) = i + j + k;
            }
        }
    }
    //x.m_im.enumerate_index_combinations([&x](std::array<size_t, 3> indices){std::cout << x(indices[0], indices[1], indices[2]) << ", ";});
    //
    //std::cout << "\n";
    ////print_first(x);
    //y[0][0][5] = 5;
    //std::cout << y[0][0][5] << "\n";
    //std::cout << x.nDims() << "\n";
    //std::cout << reinterpret_cast<size_t>(x.data()) % 32 << "\n";
    value_grid<float, 8, 8, 8> grid;
    value_grid<float, 8, 8, 8> grid2;
    grid [0ul][0ul][0ul] = 5;
    grid2[0ul][0ul][0ul] = 5;
    grid = grid + grid2;
    std::cout << grid(0ul,0ul,0ul) << "\n";
    //grid.extent<2>();
}


//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

#include "../test/min_cost_max_flow_utils.hpp"

int main() {
    boost::SampleGraph::vertex_descriptor s,t;
    boost::SampleGraph::Graph g;
    boost::SampleGraph::getSampleGraph(g, s, t);

    boost::successive_shortest_path_nonnegative_weights(g, s, t);

    int cost =  boost::find_flow_cost(g);
    assert(cost == 29);

    return 0;
}


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

#include <iostream>
#include <utility>
#include <cmath>
// #include <boost/test/minimal.hpp>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

using namespace std;

template <typename Vector>
struct f_test
{
    f_test() : s(3)
    {
	mtl::vec::inserter<Vector> ins(s);
	ins[0] << 1; ins[1] << 2; ins[2] << 2;
    }

    typename mtl::Collection<Vector>::value_type
    operator() (const Vector& x) const
    {
	return dot(s, Vector(ele_prod(x, x)));
    }
    Vector s;
};

template <typename Vector>
struct grad_f_test
{
    grad_f_test() : s(3)
    {
	mtl::vec::inserter<Vector> ins(s);
	ins[0] << 2; ins[1] << 4; ins[2] << 4;
    }

    Vector operator() (const Vector& x) const {  return Vector(ele_prod(s, x)); }
    Vector s;
};

int main(int, char**)
{
    using namespace mtl;
    using mtl::io::tout;

    mtl::dense_vector<double>       x(3, 8);
    tout << "x= " << x << "\n";

    grad_f_test<mtl::dense_vector<double> > grad_f;
    f_test<mtl::dense_vector<double> >      f;

    itl::cyclic_iteration<double>  iter(grad_f(x), 1000, 0, 1e-4, 100);
    itl::cyclic_iteration<double> iter1(grad_f(x), 1000, 0, 1e-4, 100);
    itl::cyclic_iteration<double> iter2(grad_f(x), 1000, 0, 1e-4, 100);
    itl::cyclic_iteration<double> iter3(grad_f(x), 1000, 0, 1e-4, 100);
    itl::cyclic_iteration<double> iter4(grad_f(x), 1000, 0, 1e-4, 100);

    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::bfgs(), iter);
    iter.error_code();

   // tout << "x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n\n";
    if (two_norm(x) > 10 * iter.atol()) throw "x should be 0.";
    x= 8;
    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::dfp(), iter1);
    iter1.error_code();

   // tout << "dfp x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n\n";
    if (two_norm(x) > 10 * iter1.atol()) throw "x should be 0.";

    x= 8;
    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::broyden(), iter2);
    iter2.error_code();

    tout << "broyden x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n\n";
    if (two_norm(x) > 10 * iter2.atol()) throw "x should be 0.";

 #if 0  //bad condition on some compiler
    x= 8;
    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::sr1(), iter3);
    iter3.error_code();

    tout << "sr1 x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n";
    if (two_norm(x) > 10 * iter3.atol())
	throw "x should be 0.";
#endif

    x= 8;
    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::psb(), iter4);
    iter4.error_code();

    tout << "psb x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n\n";
    if (two_norm(x) > 10 * iter4.atol()) throw "x should be 0.";

    return 0;
}


// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#include "main.hxx"

#include <dune/stuff/common/validation.hh>
#include <dune/stuff/common/type_utils.hh>
#include <dune/stuff/common/math.hh>
#include <dune/stuff/common/random.hh>
#include <dune/common/tuples.hh>
#include <dune/common/tupleutility.hh>
#include <dune/common/exceptions.hh>
#include <dune/common/bigunsignedint.hh>
#include <limits>
#include <iostream>

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/array.hpp>

using namespace Dune::Stuff::Common;

typedef testing::Types<double, float, // Dune::bigunsignedint,
                       int, unsigned int, unsigned long, long long, char> MathTestTypes;

template <class T>
struct ValidationTest : public testing::Test
{
  typedef DefaultRNG<T> RNGType;
  static const T eps;
  /** for some weird reason my compiler thinks ValidationTest is an abstract class
   * if I don't implement "void TestBody();"
   * \see common_math.cc testcases for why I think it's weird
   **/
  void TestBody()
  {
    using namespace boost::assign;
    const int samples = 100000;
    std::cout << "\tTesting Validators for type " << Typename<T>::value() << "\n\t\t" << samples
              << " random numbers ..." << std::endl;
    {
      const T lower = std::numeric_limits<T>::min() + eps;
      const T upper = std::numeric_limits<T>::max() - eps;
      RNGType rng(lower, upper);
      for (int i = samples; i > 0; --i) {
        const T arg = rng();
        test(lower, upper, arg);
      }
      boost::array<T, 10> ar = list_of<T>().repeat_fun(9, rng);
      ValidateInList<T, boost::array<T, 10>> validator(ar);
      for (T t : ar) {
        EXPECT_TRUE(validator(t));
      }
      std::vector<T> a;
      EXPECT_FALSE(ValidateInList<T>(a)(rng()));
    }
    std::cout << "\t\tfixed interval" << std::endl;
    {
      const T lower = T(0);
      const T upper = T(2);
      const T arg = T(1);
      test(lower, upper, arg);
      EXPECT_FALSE(ValidateLess<T>(upper)(lower));
      EXPECT_FALSE(ValidateGreater<T>(lower)(upper));
      EXPECT_FALSE(ValidateGreaterOrEqual<T>(lower)(upper + Epsilon<T>::value));
    }
    std::cout << "\t\tdone." << std::endl;
  }

  void test(const T lower, const T upper, const T arg) const
  {

    const T clamped_arg = clamp(arg, T(lower + eps), T(upper - eps));
    EXPECT_TRUE(ValidateAny<T>()(arg));
    EXPECT_TRUE(ValidateLess<T>(clamped_arg)(upper));
    EXPECT_TRUE(ValidateGreaterOrEqual<T>(arg)(lower));
    EXPECT_TRUE(ValidateGreater<T>(clamped_arg)(lower));
    EXPECT_TRUE(ValidateInterval<T>(lower, upper)(arg));
    EXPECT_FALSE(ValidateNone<T>()(arg));
  }
};

template <typename T>
const T ValidationTest<T>::eps = Epsilon<T>::value;

TYPED_TEST_CASE(ValidationTest, MathTestTypes);
TYPED_TEST(ValidationTest, All)
{
  ValidationTest<TypeParam> k;
  k.TestBody();
}


#ifndef STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_LOGIT_LPMF_HPP
#define STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_LOGIT_LPMF_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/arr/fun/log_sum_exp.hpp>
#include <stan/math/prim/mat/fun/log_softmax.hpp>
#include <stan/math/prim/mat/fun/log_sum_exp.hpp>
#include <stan/math/prim/mat/fun/sum.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
namespace math {

// CategoricalLog(n|theta)  [0 < n <= N, theta unconstrained], no checking
template <bool propto, typename T_prob>
return_type_t<T_prob> categorical_logit_lpmf(
    int n, const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& beta) {
  static const char* function = "categorical_logit_lpmf";

  check_bounded(function, "categorical outcome out of support", n, 1,
                beta.size());
  check_finite(function, "log odds parameter", beta);

  if (!include_summand<propto, T_prob>::value) {
    return 0.0;
  }

  // FIXME:  wasteful vs. creating term (n-1) if not vectorized
  return beta(n - 1) - log_sum_exp(beta);  // == log_softmax(beta)(n-1);
}

template <typename T_prob>
inline return_type_t<T_prob> categorical_logit_lpmf(
    int n, const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& beta) {
  return categorical_logit_lpmf<false>(n, beta);
}

template <bool propto, typename T_prob>
return_type_t<T_prob> categorical_logit_lpmf(
    const std::vector<int>& ns,
    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& beta) {
  static const char* function = "categorical_logit_lpmf";

  for (const auto& x : ns) {
    check_bounded(function, "categorical outcome out of support", x, 1,
                  beta.size());
  }
  check_finite(function, "log odds parameter", beta);

  if (!include_summand<propto, T_prob>::value) {
    return 0.0;
  }

  if (ns.empty()) {
    return 0.0;
  }

  Eigen::Matrix<T_prob, Eigen::Dynamic, 1> log_softmax_beta = log_softmax(beta);

  // FIXME:  replace with more efficient sum()
  Eigen::Matrix<return_type_t<T_prob>, Eigen::Dynamic, 1> results(ns.size());
  for (size_t i = 0; i < ns.size(); ++i) {
    results[i] = log_softmax_beta(ns[i] - 1);
  }
  return sum(results);
}

template <typename T_prob>
inline return_type_t<T_prob> categorical_logit_lpmf(
    const std::vector<int>& ns,
    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& beta) {
  return categorical_logit_lpmf<false>(ns, beta);
}

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


#pragma once

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <vector>

template <typename State, typename Input, typename ProcessNoiseVec>
class ManifoldCDKF
{
 public:
  using Scalar = typename State::Scalar;

  template <int N>
  using Vec = Eigen::Matrix<Scalar, N, 1>;

  template <int N, int M>
  using Mat = Eigen::Matrix<Scalar, N, M>;

  using StateCov = Mat<State::tangent_dim_, State::tangent_dim_>;

  using ProcessModelFunc = std::function<State(
      State const &state, Input const &u, ProcessNoiseVec const &w, Scalar dt)>;

  ManifoldCDKF(State const &state, StateCov const &state_covariance,
               ProcessModelFunc const &process_model)
      : state_{state},
        state_cov_{state_covariance},
        process_model_{process_model}
  {
    if(!process_model_)
    {
      throw std::invalid_argument("Invalid process_model");
    }
  }

  /// Get current state
  State getState() const { return state_; }
  /// Set current state
  void setState(State const &state) { state_ = state; }

  /// Get current state covariance
  StateCov getStateCovariance() const { return state_cov_; }
  /// Set current state covariance
  void setStateCovariance(StateCov const &cov) { state_cov_ = cov; }

  /**
   * Set the CDKF sigma point spread parameter. The sigma points are generated
   * as \f$ x \pm h \sqrt{P} \f$. The recommended value for gaussian noise is
   * \f$ \sqrt{3} \f$.
   *
   * @param[in] h Sigma point spread parameter
   */
  void setParameter(Scalar h) { h_ = h; }

  void setSigmaPointMeanParameters(Scalar threshold,
                                   unsigned int max_iterations)
  {
    sigma_points_mean_threshold_ = threshold;
    sigma_points_mean_max_iterations_ = max_iterations;
  }

  /**
   * Run the process update using the provided process model, input and input
   * covariance.
   *
   * @param[in] dt Time duration since last processUpdate
   * @param[in] u Input
   * @param[in] Q Input covariance
   * @param[in] debug Flag indicating whether to print internal debug messages
   *
   * @return True if the process update was successful else false
   */
  template <typename InputCov>
  bool processUpdate(Scalar const dt, Input const &u, InputCov const &Q,
                     bool debug = false);

  /**
   * Run the measurement update using the provided measurement model , current
   * measurement and measurement covariance.
   *
   * @param[in] measurement_func takes in the state and returns the expected
   * measurement with type MeasurementType
   * @param[in] z Current measurement
   * @param[in] R Measurement covariance
   * @param[in] debug Flag indicating whether to print internal debug messages
   *
   * @return True if the measurement update was successful else false
   */
  template <typename MeasurementFunc, typename MeasurementType,
            typename MeasCovType>
  bool measurementUpdate(MeasurementFunc const &measurement_func,
                         MeasurementType const &z, MeasCovType const &R,
                         bool debug = false);

  // TODO(Kartik): Add a MeasurementUpdateLinear function

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  /**
   * Generate weights for the CDKF equations
   *
   * @param[in] L Number of sigma points that would be used
   *
   * @return Array containing the 4 weights, [wm0, wm1, wc1, wc2]
   */
  std::array<Scalar, 4> generateWeights(unsigned int L) const
  {
    Scalar const h_sq = h_ * h_;
    Scalar const wm0 = (h_sq - L) / h_sq;
    Scalar const wm1 = 1 / (2 * h_sq);
    Scalar const wc1 = 1 / (4 * h_sq);
    Scalar const wc2 = (h_sq - 1) / (4 * h_sq * h_sq);
    return {wm0, wm1, wc1, wc2};
  }

  /**
   * Compute the mean of the sigma points
   *
   * @param[in] sigma_points Input sigma points
   * @param[in] wm0 Weight wm0 for CDKF
   * @param[in] wm1 Weight wm1 for CDKF
   *
   * @return Mean of the sigma points
   */
  template <typename T>
  T meanOfSigmaPoints(
      std::vector<T, Eigen::aligned_allocator<T>> const &sigma_points,
      Scalar wm0, Scalar wm1) const
  {
    T mean = sigma_points[0];
    unsigned int iterations = 0;
    typename T::TangentVec dx;
    do
    {
      dx = wm0 * (sigma_points[0] - mean);
      for(unsigned int i = 1; i < sigma_points.size(); ++i)
      {
        dx += wm1 * (sigma_points[i] - mean);
      }
      mean = mean + dx;
      iterations += 1;
      // std::cout << "meanOfSigmaPoints: dx.norm(): " << dx.norm()
      //           << ", iterations: " << iterations << "\n";
    } while((dx.squaredNorm() >
             sigma_points_mean_threshold_ * sigma_points_mean_threshold_) &&
            (iterations < sigma_points_mean_max_iterations_));

    return mean;
  }

  /**
   * Calculate the matrix square root of a positive (semi-)definite matrix.
   *
   * @param[in] mat The positive (semi-)definite matrix
   *
   * @return The square root of the input matrix which satisfies \f$ret *
   * ret^{T} = mat\f$
   */
  template <typename Derived>
  static typename Derived::PlainObject matrixSquareRoot(
      Eigen::MatrixBase<Derived> const &mat)
  {
    // Try LLT first
    {
      Eigen::LLT<typename Derived::PlainObject> const cov_chol{mat};
      if(cov_chol.info() == Eigen::Success)
      {
        return cov_chol.matrixL();
      }
    }
    // If not successful, try LDLT
    {
      Eigen::LDLT<typename Derived::PlainObject> const cov_chol{mat};
      if(cov_chol.info() == Eigen::Success)
      {
        typename Derived::PlainObject const L = cov_chol.matrixL();
        auto const &P = cov_chol.transpositionsP();
        auto const D = cov_chol.vectorD().array();
        if((D >= 0).all())
        {
          auto const D2 = D.sqrt().matrix().asDiagonal();
          return P.transpose() * L * D2;
        }
      }
    }
    // If not successful, try eigen-decomposition with slightly inflated
    // eigenvalues
    {
      Eigen::SelfAdjointEigenSolver<typename Derived::PlainObject> const cov_es{
          mat};
      auto eigvals = cov_es.eigenvalues().array().eval();
      auto const lowest_eval = eigvals(0);
      if(lowest_eval < 0)
      {
        eigvals -= lowest_eval;
      }
      return cov_es.eigenvectors() * eigvals.sqrt().matrix().asDiagonal() *
             cov_es.eigenvectors().transpose();
    }
  }

  /// State
  State state_;

  /// State Covariance
  StateCov state_cov_;

  /// Process model
  ProcessModelFunc const process_model_;

  /// CDKF Parameter
  Scalar h_ = std::sqrt(Scalar(3));

  /// Threshold for change in state during sigma point mean computation
  Scalar sigma_points_mean_threshold_ =
      10 * std::numeric_limits<Scalar>::epsilon();

  /// Maximum number of iterations for sigma point mean computation
  unsigned int sigma_points_mean_max_iterations_ = 10;
};

template <typename State, typename Input, typename ProcNoiseVec>
template <typename InputCov>
bool ManifoldCDKF<State, Input, ProcNoiseVec>::processUpdate(Scalar const dt,
                                                             Input const &u,
                                                             InputCov const &Q,
                                                             bool debug)
{
  if(dt < 0)
  {
    return false;
  }

  if(debug) // For debugging
  {
    std::cout << std::string(32, '=') << " Process Update "
              << std::string(32, '=') << "\n";
    std::cout << "State:\n" << state_ << std::endl;
    std::cout << "state_cov:\n" << state_cov_ << std::endl;
  }

  auto const proc_noise_count = static_cast<unsigned int>(Q.rows());
  auto const L = State::tangent_dim_ + proc_noise_count;

  // Generate sigma points
  auto const X = (h_ * matrixSquareRoot(state_cov_)).eval();
  auto const W = (h_ * matrixSquareRoot(Q)).eval();
  auto const weights = generateWeights(L);
  auto const wm0 = weights[0], wm1 = weights[1], wc1 = weights[2],
             wc2 = weights[3];
  // auto const [wm0, wm1, wc1, wc2] = generateWeights(L); // In C++17

  std::vector<State, Eigen::aligned_allocator<State>> Xa(2 * L + 1);

  // Apply process model
  Xa[0] = process_model_(state_, u, ProcNoiseVec::Zero(), dt);
  for(unsigned int k = 1; k <= State::tangent_dim_; ++k)
  {
    Xa[k] = process_model_(state_ + X.col(k - 1), u, ProcNoiseVec::Zero(), dt);
    Xa[L + k] =
        process_model_(state_ + -X.col(k - 1), u, ProcNoiseVec::Zero(), dt);
  }
  for(unsigned int k = 1; k <= proc_noise_count; ++k)
  {
    Xa[State::tangent_dim_ + k] = process_model_(state_, u, W.col(k - 1), dt);
    Xa[L + State::tangent_dim_ + k] =
        process_model_(state_, u, -W.col(k - 1), dt);
  }

  state_ = meanOfSigmaPoints(Xa, wm0, wm1);

  // Covariance
  state_cov_.setZero();
  for(unsigned int k = 1; k <= L; ++k)
  {
    auto const x1 = Xa[k] - Xa[0];
    auto const x2 = Xa[L + k] - Xa[0];
    auto const d1 = (x1 - x2).eval();
    auto const d2 = (x1 + x2).eval();
    state_cov_ += wc1 * d1 * d1.transpose() + wc2 * d2 * d2.transpose();
  }

  if(debug)
  {
    std::cout << std::string(80, '=') << "\n";
  }

  return true;
}

template <typename State, typename Input, typename ProcessNoiseVec>
template <typename MeasurementFunc, typename MeasurementType,
          typename MeasCovType>
bool ManifoldCDKF<State, Input, ProcessNoiseVec>::measurementUpdate(
    MeasurementFunc const &measurement_func, MeasurementType const &z,
    MeasCovType const &R, bool debug)
{
  if(debug)
  {
    std::cout << std::string(30, '=') << " Measurement Update "
              << std::string(30, '=') << "\n";
  }

  constexpr unsigned int L = State::tangent_dim_;

  // Generate sigma points
  auto const X = (h_ * matrixSquareRoot(state_cov_)).eval();
  auto const weights = generateWeights(L);
  auto const wm0 = weights[0], wm1 = weights[1], wc1 = weights[2],
             wc2 = weights[3];
  // auto const [wm0, wm1, wc1, wc2] = generateWeights(L); // In C++17

  if(debug)
  {
    std::cout << "Pa:\n" << state_cov_ << std::endl;
    std::cout << "X:\n" << X << std::endl;
  }

  // Apply measurement model
  std::vector<MeasurementType, Eigen::aligned_allocator<MeasurementType>> Zaa(
      2 * L + 1);

  Zaa[0] = measurement_func(state_);
  for(unsigned int k = 1; k <= L; k++)
  {
    Zaa[k] = measurement_func(state_ + X.col(k - 1));
    Zaa[L + k] = measurement_func(state_ + -X.col(k - 1));
  }
  if(debug)
  {
    for(unsigned int k = 0; k < 2 * L + 1; ++k)
    {
      std::cout << "Z[" << k << "]:\n" << Zaa[k] << std::endl;
    }
  }

  auto const z_pred = meanOfSigmaPoints(Zaa, wm0, wm1);

  // Covariance
  auto Pzz = MeasCovType::Zero(R.rows(), R.cols()).eval();
  auto Pxz =
      Mat<State::tangent_dim_, MeasurementType::tangent_dim_>::Zero().eval();
  for(unsigned int k = 1; k <= L; k++)
  {
    auto const z1 = Zaa[k] - Zaa[0];
    auto const z2 = Zaa[L + k] - Zaa[0];
    auto const dz1 = (z1 - z2).eval();
    auto const dz2 = (z1 + z2).eval();
    Pzz += wc1 * dz1 * dz1.transpose() + wc2 * dz2 * dz2.transpose();
    Pxz += wm1 * X.col(k - 1) * dz1.transpose();
  }
  Pzz += R;

  // Innovation
  auto const inno = decltype(z_pred){z} - z_pred;
  if(debug)
  {
    std::cout << "z:\n" << z << "\n";
    std::cout << "z_pred:\n" << z_pred << "\n";
    std::cout << "Pxz:\n" << Pxz << "\n";
    std::cout << "Pzz:\n" << Pzz << "\n";
    // Kalman Gain;
    auto const K = Pxz * Pzz.inverse();
    std::cout << "K:\n" << K << "\n";
    std::cout << "inno: " << inno.transpose() << "\n";
  }

  auto dx = State::TangentVec::Zero().eval();

  auto const Pzz_llt = Pzz.llt();
  if(Pzz_llt.info() == Eigen::Success) // Pzz is positive-definite
  {
    dx = Pxz * Pzz_llt.solve(inno);
    // Covariance around the old mean
    state_cov_ -= Pxz * Pzz_llt.solve(Pxz.transpose());
  }
  else
  {
    // Pzz is not positive definite, try LDLT
    auto const Pzz_ldlt = Pzz.ldlt();
    if(Pzz_ldlt.info() == Eigen::Success) // Pzz is positive semi-definite
    {
      dx = Pxz * Pzz_ldlt.solve(inno);
      // Covariance around the old mean
      state_cov_ -= Pxz * Pzz_ldlt.solve(Pxz.transpose());
    }
  }

  // Make sure that diagonal elements of state_cov_ are non-negative
  for(unsigned int i = 0; i < State::tangent_dim_; ++i)
  {
    if(state_cov_(i, i) < 0)
    {
      for(unsigned int j = 0; j < State::tangent_dim_; ++j)
      {
        state_cov_(i, j) = 0;
        state_cov_(j, i) = 0;
      }
    }
  }

  if(debug)
  {
    std::cout << "dx: " << dx.transpose() << "\n";
    std::cout << "state_cov:\n" << state_cov_ << "\n";
  }

  // Compute the mean and move the covariance to be around the new mean
  auto const X_new = (h_ * matrixSquareRoot(state_cov_)).eval();
  if(debug)
  {
    std::cout << "X_new:\n" << X_new << "\n";
  }

  std::vector<State, Eigen::aligned_allocator<State>> Xa(2 * L + 1);
  Xa[0] = state_ + dx;
  for(unsigned int k = 1; k <= L; k++)
  {
    Xa[k] = state_ + (dx + X_new.col(k - 1));
    Xa[L + k] = state_ + (dx - X_new.col(k - 1));
  }

  // Get mean
  state_ = meanOfSigmaPoints(Xa, wm0, wm1);

  // Calculate covariance around the new mean
  state_cov_.setZero();
  for(unsigned int k = 1; k <= L; ++k)
  {
    auto const x1 = Xa[k] - Xa[0];
    auto const x2 = Xa[L + k] - Xa[0];
    auto const d1 = x1 - x2;
    auto const d2 = x1 + x2;
    state_cov_ += wc1 * d1 * d1.transpose() + wc2 * d2 * d2.transpose();
  }

  if(debug)
  {
    std::cout << std::string(80, '=') << "\n";
  }

  return true;
}


#include <boost/math/distributions/rayleigh.hpp>


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


/**
 * @file nbc_test.cpp
 *
 * Test for the Naive Bayes classifier.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/naive_bayes/naive_bayes_classifier.hpp>

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

using namespace mlpack;
using namespace naive_bayes;

BOOST_AUTO_TEST_SUITE(NBCTest);

BOOST_AUTO_TEST_CASE(NaiveBayesClassifierTest)
{
  const char* trainFilename = "trainSet.csv";
  const char* testFilename = "testSet.csv";
  const char* trainResultFilename = "trainRes.csv";
  const char* testResultFilename = "testRes.csv";
  const char* testResultProbsFilename = "testResProbs.csv";
  size_t classes = 2;

  arma::mat trainData, trainRes, calcMat;
  data::Load(trainFilename, trainData, true);
  data::Load(trainResultFilename, trainRes, true);

  // Get the labels out.
  arma::Row<size_t> labels(trainData.n_cols);
  for (size_t i = 0; i < trainData.n_cols; ++i)
    labels[i] = trainData(trainData.n_rows - 1, i);
  trainData.shed_row(trainData.n_rows - 1);

  NaiveBayesClassifier<> nbcTest(trainData, labels, classes);

  size_t dimension = nbcTest.Means().n_rows;
  calcMat.zeros(2 * dimension + 1, classes);

  for (size_t i = 0; i < dimension; i++)
  {
    for (size_t j = 0; j < classes; j++)
    {
      calcMat(i, j) = nbcTest.Means()(i, j);
      calcMat(i + dimension, j) = nbcTest.Variances()(i, j);
    }
  }

  for (size_t i = 0; i < classes; i++)
    calcMat(2 * dimension, i) = nbcTest.Probabilities()(i);

  for (size_t i = 0; i < calcMat.n_rows; i++)
    for (size_t j = 0; j < classes; j++)
      BOOST_REQUIRE_CLOSE(trainRes(i, j) + .00001, calcMat(i, j), 0.01);

  arma::mat testData;
  arma::Mat<size_t> testRes;
  arma::mat testResProbs;
  arma::Row<size_t> calcVec;
  arma::mat calcProbs;
  data::Load(testFilename, testData, true);
  data::Load(testResultFilename, testRes, true);
  data::Load(testResultProbsFilename, testResProbs, true);

  testData.shed_row(testData.n_rows - 1); // Remove the labels.

  nbcTest.Classify(testData, calcVec, calcProbs);

  for (size_t i = 0; i < testData.n_cols; i++)
    BOOST_REQUIRE_EQUAL(testRes(i), calcVec(i));

  for (size_t i = 0; i < testResProbs.n_cols; ++i)
  {
    for (size_t j = 0; j < testResProbs.n_rows; ++j)
    {
      BOOST_REQUIRE_CLOSE(testResProbs(j, i) + 0.0001, calcProbs(j, i) + 0.0001,
          0.01);
    }
  }
}

// The same test, but this one uses the incremental algorithm to calculate
// variance.
BOOST_AUTO_TEST_CASE(NaiveBayesClassifierIncrementalTest)
{
  const char* trainFilename = "trainSet.csv";
  const char* testFilename = "testSet.csv";
  const char* trainResultFilename = "trainRes.csv";
  const char* testResultFilename = "testRes.csv";
  const char* testResultProbsFilename = "testResProbs.csv";
  size_t classes = 2;

  arma::mat trainData, trainRes, calcMat;
  data::Load(trainFilename, trainData, true);
  data::Load(trainResultFilename, trainRes, true);

  // Get the labels out.
  arma::Row<size_t> labels(trainData.n_cols);
  for (size_t i = 0; i < trainData.n_cols; ++i)
    labels[i] = trainData(trainData.n_rows - 1, i);
  trainData.shed_row(trainData.n_rows - 1);

  NaiveBayesClassifier<> nbcTest(trainData, labels, classes, true);

  size_t dimension = nbcTest.Means().n_rows;
  calcMat.zeros(2 * dimension + 1, classes);

  for (size_t i = 0; i < dimension; i++)
  {
    for (size_t j = 0; j < classes; j++)
    {
      calcMat(i, j) = nbcTest.Means()(i, j);
      calcMat(i + dimension, j) = nbcTest.Variances()(i, j);
    }
  }

  for (size_t i = 0; i < classes; i++)
    calcMat(2 * dimension, i) = nbcTest.Probabilities()(i);

  for (size_t i = 0; i < calcMat.n_cols; i++)
    for (size_t j = 0; j < classes; j++)
      BOOST_REQUIRE_CLOSE(trainRes(j, i) + .00001, calcMat(j, i), 0.01);

  arma::mat testData;
  arma::Mat<size_t> testRes;
  arma::mat testResProba;
  arma::Row<size_t> calcVec;
  arma::mat calcProbs;
  data::Load(testFilename, testData, true);
  data::Load(testResultFilename, testRes, true);
  data::Load(testResultProbsFilename, testResProba, true);

  testData.shed_row(testData.n_rows - 1); // Remove the labels.

  nbcTest.Classify(testData, calcVec, calcProbs);

  for (size_t i = 0; i < testData.n_cols; i++)
    BOOST_REQUIRE_EQUAL(testRes(i), calcVec(i));

  for (size_t i = 0; i < testResProba.n_cols; ++i)
    for (size_t j = 0; j < testResProba.n_rows; ++j)
    {
      BOOST_REQUIRE_CLOSE(
          testResProba(j, i) + .00001, calcProbs(j, i) + .00001, 0.01);
    }
}

/**
 * Ensure that separate training gives the same model.
 */
BOOST_AUTO_TEST_CASE(SeparateTrainTest)
{
  const char* trainFilename = "trainSet.csv";
  const char* trainResultFilename = "trainRes.csv";
  size_t classes = 2;

  arma::mat trainData, trainRes, calcMat;
  data::Load(trainFilename, trainData, true);
  data::Load(trainResultFilename, trainRes, true);

  // Get the labels out.
  arma::Row<size_t> labels(trainData.n_cols);
  for (size_t i = 0; i < trainData.n_cols; ++i)
    labels[i] = trainData(trainData.n_rows - 1, i);
  trainData.shed_row(trainData.n_rows - 1);

  NaiveBayesClassifier<> nbc(trainData, labels, classes, true);
  NaiveBayesClassifier<> nbcTrain(trainData.n_rows, classes);
  nbcTrain.Train(trainData, labels, classes, false);

  BOOST_REQUIRE_EQUAL(nbc.Means().n_rows, nbcTrain.Means().n_rows);
  BOOST_REQUIRE_EQUAL(nbc.Means().n_cols, nbcTrain.Means().n_cols);
  BOOST_REQUIRE_EQUAL(nbc.Variances().n_rows, nbcTrain.Variances().n_rows);
  BOOST_REQUIRE_EQUAL(nbc.Variances().n_cols, nbcTrain.Variances().n_cols);
  BOOST_REQUIRE_EQUAL(nbc.Probabilities().n_elem,
                      nbcTrain.Probabilities().n_elem);

  for (size_t i = 0; i < nbc.Means().n_elem; ++i)
  {
    if (std::abs(nbc.Means()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(nbcTrain.Means()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(nbc.Means()[i], nbcTrain.Means()[i], 1e-5);
  }

  for (size_t i = 0; i < nbc.Variances().n_elem; ++i)
  {
    if (std::abs(nbc.Variances()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(nbcTrain.Variances()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(nbc.Variances()[i], nbcTrain.Variances()[i], 1e-5);
  }

  for (size_t i = 0; i < nbc.Probabilities().n_elem; ++i)
  {
    if (std::abs(nbc.Probabilities()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(nbcTrain.Probabilities()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(nbc.Probabilities()[i], nbcTrain.Probabilities()[i],
          1e-5);
  }
}

BOOST_AUTO_TEST_CASE(SeparateTrainIncrementalTest)
{
  const char* trainFilename = "trainSet.csv";
  const char* trainResultFilename = "trainRes.csv";
  size_t classes = 2;

  arma::mat trainData, trainRes, calcMat;
  data::Load(trainFilename, trainData, true);
  data::Load(trainResultFilename, trainRes, true);

  // Get the labels out.
  arma::Row<size_t> labels(trainData.n_cols);
  for (size_t i = 0; i < trainData.n_cols; ++i)
    labels[i] = trainData(trainData.n_rows - 1, i);
  trainData.shed_row(trainData.n_rows - 1);

  NaiveBayesClassifier<> nbc(trainData, labels, classes, true);
  NaiveBayesClassifier<> nbcTrain(trainData.n_rows, classes);
  nbcTrain.Train(trainData, labels, classes, true);

  BOOST_REQUIRE_EQUAL(nbc.Means().n_rows, nbcTrain.Means().n_rows);
  BOOST_REQUIRE_EQUAL(nbc.Means().n_cols, nbcTrain.Means().n_cols);
  BOOST_REQUIRE_EQUAL(nbc.Variances().n_rows, nbcTrain.Variances().n_rows);
  BOOST_REQUIRE_EQUAL(nbc.Variances().n_cols, nbcTrain.Variances().n_cols);
  BOOST_REQUIRE_EQUAL(nbc.Probabilities().n_elem,
                      nbcTrain.Probabilities().n_elem);

  for (size_t i = 0; i < nbc.Means().n_elem; ++i)
  {
    if (std::abs(nbc.Means()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(nbcTrain.Means()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(nbc.Means()[i], nbcTrain.Means()[i], 1e-5);
  }

  for (size_t i = 0; i < nbc.Variances().n_elem; ++i)
  {
    if (std::abs(nbc.Variances()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(nbcTrain.Variances()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(nbc.Variances()[i], nbcTrain.Variances()[i], 1e-5);
  }

  for (size_t i = 0; i < nbc.Probabilities().n_elem; ++i)
  {
    if (std::abs(nbc.Probabilities()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(nbcTrain.Probabilities()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(nbc.Probabilities()[i], nbcTrain.Probabilities()[i],
          1e-5);
  }
}

BOOST_AUTO_TEST_CASE(SeparateTrainIndividualIncrementalTest)
{
  const char* trainFilename = "trainSet.csv";
  const char* trainResultFilename = "trainRes.csv";
  size_t classes = 2;

  arma::mat trainData, trainRes, calcMat;
  data::Load(trainFilename, trainData, true);
  data::Load(trainResultFilename, trainRes, true);

  // Get the labels out.
  arma::Row<size_t> labels(trainData.n_cols);
  for (size_t i = 0; i < trainData.n_cols; ++i)
    labels[i] = trainData(trainData.n_rows - 1, i);
  trainData.shed_row(trainData.n_rows - 1);

  NaiveBayesClassifier<> nbc(trainData, labels, classes, true);
  NaiveBayesClassifier<> nbcTrain(trainData.n_rows, classes);
  for (size_t i = 0; i < trainData.n_cols; ++i)
    nbcTrain.Train(trainData.col(i), labels[i]);

  BOOST_REQUIRE_EQUAL(nbc.Means().n_rows, nbcTrain.Means().n_rows);
  BOOST_REQUIRE_EQUAL(nbc.Means().n_cols, nbcTrain.Means().n_cols);
  BOOST_REQUIRE_EQUAL(nbc.Variances().n_rows, nbcTrain.Variances().n_rows);
  BOOST_REQUIRE_EQUAL(nbc.Variances().n_cols, nbcTrain.Variances().n_cols);
  BOOST_REQUIRE_EQUAL(nbc.Probabilities().n_elem,
                      nbcTrain.Probabilities().n_elem);

  for (size_t i = 0; i < nbc.Means().n_elem; ++i)
  {
    if (std::abs(nbc.Means()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(nbcTrain.Means()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(nbc.Means()[i], nbcTrain.Means()[i], 1e-5);
  }

  for (size_t i = 0; i < nbc.Variances().n_elem; ++i)
  {
    if (std::abs(nbc.Variances()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(nbcTrain.Variances()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(nbc.Variances()[i], nbcTrain.Variances()[i], 1e-5);
  }

  for (size_t i = 0; i < nbc.Probabilities().n_elem; ++i)
  {
    if (std::abs(nbc.Probabilities()[i]) < 1e-5)
      BOOST_REQUIRE_SMALL(nbcTrain.Probabilities()[i], 1e-5);
    else
      BOOST_REQUIRE_CLOSE(nbc.Probabilities()[i], nbcTrain.Probabilities()[i],
          1e-5);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include "highgui.h"
#include "imgproc.h"
#include "feature.h"
#include <armadillo>
#include <vector>
#include <cmath>
#include <cassert>

using namespace arma;
using namespace std;

static void mergesort(vec &keys, uvec &values);
static mat atan2(const mat &Y, const mat &X);

// try to get adaptive thresholding working
mat edge2(const mat &F, int op) {
  switch (op) {
    case EDGE_SOBEL:
      return sobel_edge2(F);
    case EDGE_CANNY:
      return canny2(F);
    case EDGE_DOG:
      return dog2(F);
    case EDGE_LOG:
      return log2(F);
    default:
      return mat();
  }
}

cube edge2(const cube &F, int op) {
  return gray2rgb(edge2(rgb2gray(F), op));
}

mat sobel_edge2(const mat &F, uword n, double sigma2) {
  // smooth first
  mat H = conv2(F, gauss2(n, sigma2));
  mat DX, DY;
  gradient2(DX, DY, H);
  mat G = sqrt(DX % DX + DY % DY);
  return G;
}

mat canny2(const mat &F, double low, double high, uword n, double sigma2) { // not the complete canny
  // filter
  mat H = conv2(F, gauss2(n, sigma2));
  // intensity gradient
  mat DX, DY;
  gradient2(DX, DY, H);
  // non-maximal suppression
  mat G = sqrt(DX % DX + DY % DY);
  mat T = atan2(DY, DX);
  H = nmm2(G, T);
  G /= G.max(); // normalise
  // double threshold
  mat strong = (G >= high) % H;
  mat weak = (low < G && G < high) % H;
  // hysteresis edge-tracking
//  mat K = strong + weak;
//  K /= K;
  return strong;
}

mat dog2(const mat &F, double alpha, uword n, double sigma2) {
  return F - alpha * conv2(F, gauss2(n, sigma2));
}

mat log2(const mat &F, uword n, double sigma2) {
  return conv2(F, laplace_gauss2(n, sigma2));
}

void blob2(const mat &F, vector<vec> &centroids) {
  mat visited(F.n_rows, F.n_cols, fill::zeros);
  vector<ivec> tovisit;
  size_t nvisited = 0;
  centroids.clear();
  ivec pt;
  vec mid(2);
  int npts;
  for (uword i = 0; i < F.n_rows; i++) {
    for (uword j = 0; j < F.n_cols; j++) {
      if (!visited(i, j)) {
        mid.zeros();
        npts = 0;
        tovisit.push_back(ivec({(sword)i,(sword)j}));
        visited(i,j) = 1;
        while (tovisit.size() > nvisited) {
          pt = tovisit[nvisited];
          if (pt(0)-1>=0 && !visited(pt(0)-1,pt(1)) && F(pt(0)-1,pt(1)) == F(pt(0),pt(1))) {
            visited(pt(0)-1,pt(1)) = 1;
            tovisit.push_back(ivec({pt(0)-1,pt(1)}));
          }
          if (pt(0)+1<F.n_rows && !visited(pt(0)+1,pt(1)) && F(pt(0)+1,pt(1)) == F(pt(0),pt(1))) {
            visited(pt(0)+1,pt(1)) = 1;
            tovisit.push_back(ivec({pt(0)+1,pt(1)}));
          }
          if (pt(1)-1>=0 && !visited(pt(0),pt(1)-1) && F(pt(0),pt(1)-1) == F(pt(0),pt(1))) {
            visited(pt(0),pt(1)-1) = 1;
            tovisit.push_back(ivec({pt(0),pt(1)-1}));
          }
          if (pt(1)+1<F.n_cols && !visited(pt(0),pt(1)+1) && F(pt(0),pt(1)+1) == F(pt(0),pt(1))) {
            visited(pt(0),pt(1)+1) = 1;
            tovisit.push_back(ivec({pt(0),pt(1)+1}));
          }
          mid += vec({(double)pt(0), (double)pt(1)});
          npts++;
          nvisited++;
        }
        if (npts > 24) {
          centroids.push_back(mid/npts);
        }
      }
    }
  }
}

mat corner2(const mat &I, int op) {
  switch (op) {
    case CORNER_SOBEL:
      return sobel_corner2(I);
    case CORNER_HARRIS:
      return harris2(I, reshape(mat({1,2,1,2,4,2,1,2,1}),3,3)/16.0);
    default:
      return mat();
  }
}

mat sobel_corner2(const mat &F, uword n, double sigma2) {
  mat H = reshape(mat({
    1, -2, 1,
    -2, 4, -2,
    1, -2, 1
  }), 3, 3).t();
  mat G = conv2(F, gauss2(n, sigma2));
  G = conv2(G, H);
}

mat harris2(const mat &I, const mat &W) {
  assert(W.n_rows == W.n_cols);
  mat DX, DY;
  gradient2(DX, DY, I); // grab the gradients
  // place gradients into padded matrix
  mat wIxx = conv2(DX % DX, W);
  mat wIxy = conv2(DX % DY, W);
  mat wIyy = conv2(DY % DY, W);
  // find the taylor expansion-based corner detector
  //double k = 0.07;
  //return ((wIxx % wIyy) - (wIxy % wIxy)) - (k * ((wIxx + wIyy) % (wIxx + wIyy)));
  double eps = 0.05;
  return 2.0 * ((wIxx % wIyy) - (wIxy % wIxy)) / (wIxx + wIyy + eps);
}

mat lines2(const mat &I, const vector<vec> &pts, int op) {
  assert(op == LINE_RANSAC || op == LINE_HOUGH);
  switch (op) {
    case LINE_RANSAC:
      return ransac(pts, 10.0, (int)pts.size());
    case LINE_HOUGH:
      return hough_line(pts, 0.5, I.n_rows, I.n_cols);
    default:
      return mat();
  }
}

mat ransac(const vector<vec> &pts, double sigma2, int k) {
  int consensus = 0;
  vec pt1, pt2;
  int i1, i2;
  int ind1 = 0, ind2 = 0;
  for (int i = 0; i < k; i++) {
    // choose two random points
    i1 = i;
    i2 = rand() % pts.size();
    while (i1 == i2) {
      i2 = rand() % pts.size();
    }

    // fit a particular model
    vec u_i2_i1 = normalise(pts[i2] - pts[i1]);

    // get the set of inliers that are close enough to the line
    int hyp_consensus = 0;
    for (int j = 0; j < k; j++) {
      // compute the distance from the point
      vec v_j_i1 = pts[j] - pts[i1];
      vec para = dot(v_j_i1.t(), u_i2_i1) * u_i2_i1;
      vec perp = v_j_i1 - para;
      double distance = sqrt(dot(perp, perp));
      if (distance < sigma2) {
        hyp_consensus++;
      }
    }

    // compare the consensus to the known consensus
    // if larger, then replace
    if (hyp_consensus > consensus) {
      pt1 = pts[i1];
      pt2 = pts[i2];
      consensus = hyp_consensus;
      ind1 = i1;
      ind2 = i2;
    } else if (consensus == 0) {
      i = 0; // keep repeating until a consensus is reached
    }
  }

  mat ans(2, 2);
  ans.col(0) = pt1;
  ans.col(1) = pt2;
  return ans;
}

mat hough_line(const vector<vec> &pts, double sigma2, size_t n_rows, size_t n_cols) {
  // create an accumulator
  mat accumulator(2 * (n_rows + n_cols), 180, fill::zeros); // radius, degrees
  for (const vec &pt : pts) {
    for (uword theta = 0; theta < accumulator.n_cols; theta++) {
      double rad = (double)theta * M_PI / 180.0;
      double y = pt(0);
      double x = pt(1);
      uword r = (uword)(y * sin(rad) + x * cos(rad)) + n_rows + n_cols;
      if (r < accumulator.n_rows && r != (n_rows + n_cols)) { // error otherwise
        accumulator(r, theta) += 1.0;
      }
    }
  }
  // find the approximate gradient of the accumulator
  mat DX, DY;
  gradient2(DX, DY, accumulator);
  mat theta = atan2(DY, DX);
  mat radius = sqrt(DX % DX + DY % DY);
  accumulator = nmm2(accumulator, theta, 2);
  accumulator /= accumulator.max(); // normalized

  mat II = imresize2(accumulator, 500, accumulator.n_cols);
  II /= II.max();
  disp_image("accumulator", II);
  disp_wait();

  // now grab all the local maximums
  uvec ind = find(accumulator > 0.0);
  vec votes(ind.n_elem);
  mat rt(3, ind.n_elem);
  for (uword i = 0; i < ind.n_elem; i++) {
    rt.col(i) = vec({ 0.0,
        (double)(ind(i) % accumulator.n_rows),
        (double)(ind(i) / accumulator.n_rows) });
    uword radius = rt(1, i);
    uword theta = rt(2, i);
    votes(i) = accumulator(radius, theta);
    rt(0, i) = votes(i);
  }

  // sort them to get the best matches
  uvec vv = cumsum(ones<uvec>(votes.n_elem)) - 1;
  mergesort(votes, vv);
  mat hlines(3, ind.n_elem);
  for (uword i = 0; i < vv.n_elem; i++) {
    uword j = vv(i);
    hlines.col(i) = rt.col(j);
  }
  hlines.row(1) -= (n_rows + n_cols);

  return hlines;
}

// TODO
mat hough_circle(const vector<vec> &pts, double sigma2, size_t n_rows, size_t n_cols) {
  cube accumulator(n_rows, n_cols, 2 * (n_rows + n_cols), fill::zeros);
  for (uword R = 0; R < accumulator.n_slices; R++) {
    for (vec pt : pts) {
      accumulator(pt(0), pt(1), R) += 1.0;
    }
  }
  return accumulator;
}

// STATIC

static void mergesort(vec &keys, uvec &values) {
  if (keys.size() == 1) {
    return;
  }
  uword mid = keys.n_elem/2;
  vec keys1 = keys(span(0,mid-1));
  vec keys2 = keys(span(mid,keys.n_elem-1));
  uvec values1 = values(span(0,mid-1));
  uvec values2 = values(span(mid,keys.n_elem-1));
  mergesort(keys1, values1);
  mergesort(keys2, values2);
  uword i = 0, j = 0, k = 0;
  while (k < keys.n_elem) {
    if (i == keys1.n_elem) {
      keys(k) = keys2(j);
      values(k) = values2(j);
      j++;
      k++;
    } else if (j == keys2.n_elem || keys1(i) > keys2(j)) {
      keys(k) = keys1(i);
      values(k) = values1(i);
      i++;
      k++;
    } else {
      keys(k) = keys2(j);
      values(k) = values2(j);
      j++;
      k++;
    }
  }
}

static mat atan2(const mat &Y, const mat &X) {
  assert(Y.n_rows == X.n_rows && Y.n_cols == X.n_cols);
  mat ans(Y.n_rows, Y.n_cols);
  for (uword i = 0; i < ans.n_rows; i++) {
    for (uword j = 0; j < ans.n_cols; j++) {
      ans(i, j) = atan2(Y(i, j), X(i, j));
    }
  }
  return ans;
}


#include "fa_hierarchy.h"

#include <google/protobuf/stubs/casts.h>

#include <Eigen/Dense>
#include <cassert>
#include <iostream>
#include <stan/math/prim/prob.hpp>
#include <vector>

#include "algorithm_state.pb.h"
#include "hierarchy_prior.pb.h"
#include "ls_state.pb.h"
#include "src/utils/proto_utils.h"
#include "src/utils/rng.h"

double FAHierarchy::like_lpdf(const Eigen::RowVectorXd& datum) const {
  using stan::math::NEG_LOG_SQRT_TWO_PI;
  double base = (Eigen::MatrixXd(state.cov_chol.matrixL()))
                    .diagonal()
                    .array()
                    .log()
                    .sum() -
                NEG_LOG_SQRT_TWO_PI * dim;
  double exp =
      0.5 * ((datum.transpose() - state.mu)
                 .dot(state.cov_chol.solve((datum.transpose() - state.mu))));
  return -(base + exp);
}

FA::State FAHierarchy::draw(const FA::Hyperparams& params) {
  auto& rng = bayesmix::Rng::Instance().get();
  FA::State out;
  out.mu = params.mutilde;
  out.psi = params.beta / (params.alpha0 + 1.);
  out.eta = Eigen::MatrixXd::Zero(card, params.q);
  out.lambda = Eigen::MatrixXd::Zero(dim, params.q);

  for (size_t j = 0; j < dim; j++) {
    out.mu[j] =
        stan::math::normal_rng(params.mutilde[j], sqrt(params.phi), rng);

    out.psi[j] = stan::math::inv_gamma_rng(params.alpha0, params.beta[j], rng);

    for (size_t i = 0; i < params.q; i++) {
      out.lambda(j, i) = stan::math::normal_rng(0, 1, rng);
    }
  }

  for (size_t i = 0; i < card; i++) {
    for (size_t j = 0; j < params.q; j++) {
      out.eta(i, j) = stan::math::normal_rng(0, 1, rng);
    }
  }

  out.psi_inverse = out.psi.cwiseInverse().asDiagonal();
  out.cov_chol = (out.lambda * out.lambda.transpose() +
                  Eigen::MatrixXd(out.psi.asDiagonal()))
                     .llt();

  return out;
}

void FAHierarchy::initialize_state() {
  state.mu = hypers->mutilde;
  state.psi = hypers->beta / (hypers->alpha0 + 1.);
  state.eta = Eigen::MatrixXd::Zero(card, hypers->q);
  state.lambda = Eigen::MatrixXd::Zero(dim, hypers->q);
  state.psi_inverse = state.psi.cwiseInverse().asDiagonal();
  state.cov_chol = (state.lambda * state.lambda.transpose() +
                    Eigen::MatrixXd(state.psi.asDiagonal()))
                       .llt();
}

void FAHierarchy::initialize_hypers() {
  if (prior->has_fixed_values()) {
    // Set values
    hypers->mutilde = bayesmix::to_eigen(prior->fixed_values().mutilde());
    dim = hypers->mutilde.size();
    hypers->beta = bayesmix::to_eigen(prior->fixed_values().beta());
    hypers->phi = prior->fixed_values().phi();
    hypers->alpha0 = prior->fixed_values().alpha0();
    hypers->q = prior->fixed_values().q();

    // Automatic initialization
    if (dim == 0) {
      hypers->mutilde = dataset_ptr->colwise().mean();
      dim = hypers->mutilde.size();
    }
    if (hypers->beta.size() == 0) {
      Eigen::MatrixXd centered =
          dataset_ptr->rowwise() - dataset_ptr->colwise().mean();
      auto cov_llt = ((centered.transpose() * centered) /
                      double(dataset_ptr->rows() - 1.))
                         .llt();
      Eigen::MatrixXd precision_matrix(
          cov_llt.solve(Eigen::MatrixXd::Identity(dim, dim)));
      hypers->beta =
          (hypers->alpha0 - 1) * precision_matrix.diagonal().cwiseInverse();
      if (hypers->alpha0 == 1) {
        throw std::invalid_argument(
            "Scale parameter must be different than 1 when automatic "
            "initialization is used");
      }
    }
    // Check validity
    if (dim != hypers->beta.rows()) {
      throw std::invalid_argument(
          "Hyperparameters dimensions are not consistent");
    }
    for (size_t j = 0; j < dim; j++) {
      if (hypers->beta[j] <= 0) {
        throw std::invalid_argument("Shape parameter must be > 0");
      }
    }
    if (hypers->alpha0 <= 0) {
      throw std::invalid_argument("Scale parameter must be > 0");
    }
    if (hypers->phi <= 0) {
      throw std::invalid_argument("Diffusion parameter must be > 0");
    }
    if (hypers->q <= 0) {
      throw std::invalid_argument("Number of factors must be > 0");
    }
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

void FAHierarchy::update_hypers(
    const std::vector<bayesmix::AlgorithmState::ClusterState>& states) {
  auto& rng = bayesmix::Rng::Instance().get();
  if (prior->has_fixed_values()) {
    return;
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

void FAHierarchy::update_summary_statistics(const Eigen::RowVectorXd& datum,
                                            bool add) {
  if (add) {
    data_sum += datum;
  } else {
    data_sum -= datum;
  }
}

void FAHierarchy::clear_summary_statistics() {
  data_sum = Eigen::VectorXd::Zero(dim);
}

void FAHierarchy::set_state_from_proto(
    const google::protobuf::Message& state_) {
  auto& statecast = downcast_state(state_);
  state.mu = bayesmix::to_eigen(statecast.fa_state().mu());
  state.psi = bayesmix::to_eigen(statecast.fa_state().psi());
  state.eta = bayesmix::to_eigen(statecast.fa_state().eta());
  state.lambda = bayesmix::to_eigen(statecast.fa_state().lambda());
  state.psi_inverse = state.psi.cwiseInverse().asDiagonal();
  state.cov_chol = (state.lambda * state.lambda.transpose() +
                    Eigen::MatrixXd(state.psi.asDiagonal()))
                       .llt();
  set_card(statecast.cardinality());
}

std::shared_ptr<bayesmix::AlgorithmState::ClusterState>
FAHierarchy::get_state_proto() const {
  bayesmix::FAState state_;
  bayesmix::to_proto(state.mu, state_.mutable_mu());
  bayesmix::to_proto(state.psi, state_.mutable_psi());
  bayesmix::to_proto(state.eta, state_.mutable_eta());
  bayesmix::to_proto(state.lambda, state_.mutable_lambda());

  auto out = std::make_shared<bayesmix::AlgorithmState::ClusterState>();
  out->mutable_fa_state()->CopyFrom(state_);
  return out;
}

void FAHierarchy::set_hypers_from_proto(
    const google::protobuf::Message& hypers_) {
  auto& hyperscast = downcast_hypers(hypers_).fa_state();
  hypers->mutilde = bayesmix::to_eigen(hyperscast.mutilde());
  hypers->alpha0 = hyperscast.alpha0();
  hypers->beta = bayesmix::to_eigen(hyperscast.beta());
  hypers->phi = hyperscast.phi();
  hypers->q = hyperscast.q();
}

std::shared_ptr<bayesmix::AlgorithmState::HierarchyHypers>
FAHierarchy::get_hypers_proto() const {
  bayesmix::FAPriorDistribution hypers_;
  bayesmix::to_proto(hypers->mutilde, hypers_.mutable_mutilde());
  bayesmix::to_proto(hypers->beta, hypers_.mutable_beta());
  hypers_.set_alpha0(hypers->alpha0);
  hypers_.set_phi(hypers->phi);
  hypers_.set_q(hypers->q);

  auto out = std::make_shared<bayesmix::AlgorithmState::HierarchyHypers>();
  out->mutable_fa_state()->CopyFrom(hypers_);
  return out;
}

void FAHierarchy::sample_full_cond(bool update_params) {
  if (this->card == 0) {
    // No posterior update possible
    sample_prior();
  } else {
    sample_eta();
    sample_mu();
    sample_psi();
    sample_lambda();
  }
}

void FAHierarchy::sample_eta() {
  auto& rng = bayesmix::Rng::Instance().get();
  auto sigma_eta_inv_llt =
      (Eigen::MatrixXd::Identity(hypers->q, hypers->q) +
       state.lambda.transpose() * state.psi_inverse * state.lambda)
          .llt();
  if (state.eta.rows() != card) {
    state.eta = Eigen::MatrixXd::Zero(card, state.eta.cols());
  }
  Eigen::MatrixXd temp_product(
      sigma_eta_inv_llt.solve(state.lambda.transpose() * state.psi_inverse));
  auto iterator = cluster_data_idx.begin();
  for (size_t i = 0; i < card; i++, iterator++) {
    Eigen::VectorXd tempvector(dataset_ptr->row(
        *iterator));  // TODO use slicing when Eigen is updated to v3.4
    state.eta.row(i) = (bayesmix::multi_normal_prec_chol_rng(
        temp_product * (tempvector - state.mu), sigma_eta_inv_llt, rng));
  }
}

void FAHierarchy::sample_mu() {
  auto& rng = bayesmix::Rng::Instance().get();
  Eigen::DiagonalMatrix<double, Eigen::Dynamic> sigma_mu;

  sigma_mu.diagonal() =
      (card * state.psi_inverse.diagonal().array() + hypers->phi)
          .cwiseInverse();

  Eigen::VectorXd sum = (state.eta.colwise().sum());

  Eigen::VectorXd mumean =
      sigma_mu * (hypers->phi * hypers->mutilde +
                  state.psi_inverse * (data_sum - state.lambda * sum));

  state.mu = bayesmix::multi_normal_diag_rng(mumean, sigma_mu, rng);
}

void FAHierarchy::sample_lambda() {
  auto& rng = bayesmix::Rng::Instance().get();

  Eigen::MatrixXd temp_etateta(state.eta.transpose() * state.eta);

  for (size_t j = 0; j < dim; j++) {
    auto sigma_lambda_inv_llt =
        (Eigen::MatrixXd::Identity(hypers->q, hypers->q) +
         temp_etateta / state.psi[j])
            .llt();
    Eigen::VectorXd tempsum(card);
    const Eigen::VectorXd& data_col = dataset_ptr->col(j);
    auto iterator = cluster_data_idx.begin();
    for (size_t i = 0; i < card; i++, iterator++) {
      tempsum[i] = data_col(
          *iterator);  // TODO use slicing when Eigen is updated to v3.4
    }
    tempsum = tempsum.array() - state.mu[j];
    tempsum = tempsum.array() / state.psi[j];

    state.lambda.row(j) = bayesmix::multi_normal_prec_chol_rng(
        sigma_lambda_inv_llt.solve(state.eta.transpose() * tempsum),
        sigma_lambda_inv_llt, rng);
  }
}

void FAHierarchy::sample_psi() {
  auto& rng = bayesmix::Rng::Instance().get();

  for (size_t j = 0; j < dim; j++) {
    double sum = 0;
    auto iterator = cluster_data_idx.begin();
    for (size_t i = 0; i < card; i++, iterator++) {
      sum += std::pow(
          ((*dataset_ptr)(*iterator, j) -
           state.mu[j] -  // TODO use slicing when Eigen is updated to v3.4
           state.lambda.row(j).dot(state.eta.row(i))),
          2);
    }
    state.psi[j] = stan::math::inv_gamma_rng(hypers->alpha0 + card / 2,
                                             hypers->beta[j] + sum / 2, rng);
  }
  state.psi_inverse = state.psi.cwiseInverse().asDiagonal();
  state.cov_chol = (state.lambda * state.lambda.transpose() +
                    Eigen::MatrixXd(state.psi.asDiagonal()))
                       .llt();
}


/*
@copyright Louis Dionne 2015
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/ext/boost/mpl/integral_c.hpp>

#include <boost/hana/tuple.hpp>

#include <laws/enumerable.hpp>
#include <laws/group.hpp>
#include <laws/integral_domain.hpp>
#include <laws/monoid.hpp>
#include <laws/ring.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
using namespace boost::hana;
namespace mpl = boost::mpl;

int main() {
    auto int_constants = make<Tuple>(
        mpl::int_<-10>{}, mpl::int_<-2>{}, mpl::integral_c<int, 0>{},
        mpl::integral_c<int, 1>{}, mpl::integral_c<int, 3>{}
    );

    //////////////////////////////////////////////////////////////////////////
    // Enumerable, Monoid, Group, Ring, IntegralDomain
    //////////////////////////////////////////////////////////////////////////
    {
        // laws
        test::TestEnumerable<ext::boost::mpl::IntegralC<int>>{int_constants};
        test::TestMonoid<ext::boost::mpl::IntegralC<int>>{int_constants};
        test::TestGroup<ext::boost::mpl::IntegralC<int>>{int_constants};
        test::TestRing<ext::boost::mpl::IntegralC<int>>{int_constants};
        test::TestIntegralDomain<ext::boost::mpl::IntegralC<int>>{int_constants};
    }
}


// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <cmath>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/fpclassify.hpp> // isnan
#include <vinecopulib/misc/tools_eigen.hpp>

namespace vinecopulib {
inline JoeBicop::JoeBicop()
{
    family_ = BicopFamily::joe;
    parameters_ = Eigen::VectorXd(1);
    parameters_lower_bounds_ = Eigen::VectorXd(1);
    parameters_upper_bounds_ = Eigen::VectorXd(1);
    parameters_ << 1;
    parameters_lower_bounds_ << 1;
    parameters_upper_bounds_ << 30;
}

inline double JoeBicop::generator(const double &u)
{
    return (-1) * boost::math::log1p(-std::pow(1 - u, parameters_(0)));
}

inline double JoeBicop::generator_inv(const double &u)
{
    return 1 - std::pow(-boost::math::expm1(-u), 1 / parameters_(0));
}

inline double JoeBicop::generator_derivative(const double &u)
{
    double theta = double(parameters_(0));
    return (-theta) * std::pow(1 - u, theta - 1) / (1 - std::pow(1 - u, theta));
}

//inline double JoeBicop::generator_derivative2(const double &u)
//{
//    double theta = double(parameters_(0));
//    double res = theta * (theta - 1 + std::pow(1 - u, theta));
//    return res * std::pow(1 - u, theta - 2) /
//           std::pow(-1 + std::pow(1 - u, theta), 2);
//}

inline Eigen::VectorXd JoeBicop::pdf_raw(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double theta = static_cast<double>(parameters_(0));
    auto f = [theta](const double &u1, const double &u2) {
        double t1 = std::pow(1-u1,theta);
        double t2 = std::pow(1-u2,theta);
        return std::pow(t1 + t2 - t1 * t2,1/theta-2)
               * std::pow(1-u1,theta-1)*std::pow(1-u2,theta-1)
               * (theta-1 + t1 + t2 - t1 * t2);
    };
    return tools_eigen::binaryExpr_or_nan(u, f);
}

// inverse h-function
inline Eigen::VectorXd JoeBicop::hinv1(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double theta = double(parameters_(0));
    double u1, u2;
    Eigen::VectorXd hinv = Eigen::VectorXd::Zero(u.rows());
    for (int j = 0; j < u.rows(); ++j) {
        u1 = u(j, 1);
        u2 = u(j, 0);
        if ((boost::math::isnan)(u1) | (boost::math::isnan)(u2)) {
            hinv(j) = std::numeric_limits<double>::quiet_NaN();
        } else {
            hinv(j) = qcondjoe(&u1, &u2, &theta);
        }
    }

    return hinv;
}

// link between Kendall's tau and the par_bicop parameter
inline Eigen::MatrixXd JoeBicop::tau_to_parameters(const double &tau)
{
    Eigen::VectorXd tau0 = Eigen::VectorXd::Constant(1, std::fabs(tau));
    auto f = [&](const Eigen::VectorXd &v) {
        return Eigen::VectorXd::Constant(1, std::fabs(parameters_to_tau(v)));
    };
    return tools_eigen::invert_f(tau0,
                                 f,
                                 parameters_lower_bounds_(0) + 1e-6,
                                 parameters_upper_bounds_(0) - 1e-6);
}

inline double JoeBicop::parameters_to_tau(const Eigen::MatrixXd &parameters)
{
    double par = parameters(0);
    double tau = 2 / par + 1;
    tau = boost::math::digamma(2.0) - boost::math::digamma(tau);
    return 1 + 2 * tau / (2 - par);
}

inline Eigen::VectorXd JoeBicop::get_start_parameters(const double tau)
{
    Eigen::VectorXd par = tau_to_parameters(tau);
    par = par.cwiseMax(parameters_lower_bounds_);
    par = par.cwiseMin(parameters_upper_bounds_);
    return par;
}
}

// This is copy&paste from the VineCopula package
inline double qcondjoe(double *q, double *u, double *de)
{
    double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t13, t15, t16, t19, t23, t28, t31;
    double c21, pdf;
    int iter;
    double diff, v, de1, dtem, de1inv, tem;

    t1 = 1.0 - *u;
    t2 = pow(t1, 1.0 * (*de));
    t7 = 1. / (*de);
    t10 = t2 * (*de);
    t11 = 1. / t1;
    t19 = (*de) * (*de);
    de1 = *de - 1;  // may need better modification for large delta
    dtem = -de1 / (1. + de1);
    de1inv = -1. / de1;

    // v = 0.5 * (q+u); // starting guess

    // Use a better starting point based on reflected B4 copula
    // A good starting point is crucial when delta is large because
    //    C_{2|1} will be steep
    // C_{R,2|1}(v|u)=1-C_{2|1}(1-v|1-u),
    // C_{R,2|1}^{-1}(q|u)=1-C_{2|1}^{-1}(1-q|1-u)
    tem = pow(1. - *q, dtem) - 1.;
    tem = tem * pow(1. - *u, -de1) + 1.;
    v = pow(tem, de1inv);
    v = 1. - v;
    diff = 1;
    iter = 0;
    while (fabs(diff) > 1.e-6 && iter < 20) {
        t3 = 1. - v;
        t4 = pow(t3, *de);
        t5 = t2 * t4;
        t6 = t2 + t4 - t5;
        t8 = pow(t6, t7);
        t9 = t7 * t8;
        t13 = t11 * t4;
        t15 = -t10 * t11 + t10 * t13;
        t16 = 1. / t6;
        t23 = 1. / t3;
        t28 = t6 * t6;
        t31 = (-t4 * (*de) * t23 + t5 * (*de) * t23) / t28 * t15;
        c21 = -t9 * t15 * t16;
        pdf = -t8 / t19 * t31 + t8 * (*de) * t2 * t13 * t23 * t16 + t9 * t31;
        iter++;
        if ((boost::math::isnan)(pdf) ||
            (boost::math::isnan)(c21)) {
            diff /= -2.;
        }  // added for de>=30
        else
            diff = (c21 - *q) / pdf;
        v -= diff;
        int iter2 = 0;
        while ((v <= 0 || v >= 1 || fabs(diff) > 0.25) & (iter2 < 20)) {
            ++iter2;
            diff /= 2.;
            v += diff;
        }
    }

    // make sure that boundaries are respected
    if (v <= 0) {
        v = 1e-10;
    } else if (v >= 1) {
        v = 1 - 1e-10;
    }

    return v;
}


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


//
// Created by Zhongshi Jiang on 4/17/17.
//

#include <Eigen/Core>
#include <igl/viewer/Viewer.h>
#include <igl/triangle/triangulate.h>
#include <igl/cat.h>
#include <igl/writeOBJ.h>

void bars_stack_construction(Eigen::MatrixXd w_uv, Eigen::MatrixXi w_F,
int mv_num, int mf_num) {
  using namespace Eigen;
  using namespace std;

  const double thick =  1;
  const double space = 4; // vertical
  const int length = 100;
  const int layer = 400; // this is to be going to infinity!
  const int num_hole = 1; // to prevent bug of triangle.
  assert(num_hole < length);

  auto v_num = 2 + length*2;
  auto f_num = length*2;
  Eigen::MatrixXd H(num_hole*layer,2);
  Eigen::MatrixXd V(layer*v_num, 2);
  Eigen::MatrixXi F(layer*f_num, 3);
  Eigen::MatrixXi E(layer*v_num ,2);

  for (auto l=0; l<layer; l++) {
    auto start_v = l * v_num;
    auto start_f = l * f_num;
    for(int i=0; i<length+1; i++) {
      V.row(start_v + 2 * i) << i * thick, l * space;
      V.row(start_v + 2 * i + 1) << i * thick, l * space + thick;
    }
    for(int i=0; i<length; i++) {
      F.row(start_f + 2*i) = Eigen::Array3i(2*i, 2*i+2,2*i+1)+start_v;
      F.row(start_f + 2*i+1) = Eigen::Array3i(2*i+1, 2*i+2,2*i+3)+start_v;
    }

    for(int i=0; i<length; i++) {
      E.row(start_v+i) << start_v+ 2*i, start_v + 2*i+2;
      E.row(start_v+length+i+1)<<start_v+2*i+3, start_v+2*i+1;
    }
    E.row(start_v+length) << start_v+2*length, start_v+2*length+1;
    E.row(start_v+2*length+1) << start_v+1,start_v+0;

    for(int i=0 ; i<num_hole; i++) {
      RowVector2d hh(0,0);
      for(auto v : {0,1,2})
        hh += V.row(F(start_f + i, v));
      H.row(l*num_hole + i) = hh/3;
    }
  }

  Matrix2d ob;// = rect_corners;
  {
    VectorXd uv_max = V.colwise().maxCoeff();
    VectorXd uv_min = V.colwise().minCoeff();
    VectorXd uv_mid = (uv_max + uv_min) / 2.;
    ob.row(0) = uv_mid;
    ob.row(1) = uv_mid;

    double scaf_range = 3;
    Array2d scaf_scale(2,1.5);
    ob.row(0) += (scaf_scale * (uv_min - uv_mid).array()).matrix();
    ob.row(1) += (scaf_scale * (uv_max - uv_mid).array()).matrix();
  }
  Vector2d rect_len;
  rect_len << ob(1, 0) - ob(0, 0), ob(1, 1) - ob(0, 1);

  int rect_side = 6;
  MatrixXd V_rect(2*rect_side,2);
  MatrixXi E_rect(2*rect_side,2);
  for(int i=0; i<rect_side; i++) {
    V_rect.row(i) << ob(0, 0) + i * rect_len(0) / (rect_side-1), ob(0, 1);
    V_rect.row(rect_side + i) << ob(0, 0) + i * rect_len(0) / (rect_side-1),
        ob(1, 1);

    E_rect.row(i) << i, i + 1;
    E_rect.row(rect_side + i) << 2 * rect_side - 1 - i, 2 * rect_side - 2 - i;
  }
  E_rect.row(rect_side - 1) << rect_side - 1, 2*rect_side - 1;
  E_rect.row(2*rect_side - 1) << rect_side, 0;

  MatrixXd whole_V;
  igl::cat(1, V, V_rect, whole_V);
  MatrixXi whole_E(V.rows() + V_rect.rows(), 2);
  whole_E.topRows(V.rows()) = E;
  whole_E.bottomRows(V_rect.rows()) = E_rect.array() + V.rows();

  MatrixXd s_uv; MatrixXi s_F;
  igl::triangle::triangulate(whole_V,whole_E, H, "qQ", s_uv, s_F);
//  cout<<V<<endl<<E_rect<<endl;
  mesh_cat(V,F,s_uv,s_F, w_uv,w_F);

  mv_num = V.rows();
  mf_num = F.rows();

  igl::viewer::Viewer v_;
  v_.data.set_mesh(w_uv,w_F);
  v_.data.add_points(H,Eigen::RowVector3d(1,0,0) );
  MatrixXd mesh_color(w_F.rows(),3);
  for (int i = 0; i < mf_num; i++)
    mesh_color.row(i) << 148/255., 195/255., 128/255.;

  for (int i = mf_num; i < w_F.rows(); i++)
    mesh_color.row(i) << 0.86, 0.86, 0.86;
  v_.data.set_colors(mesh_color);
  MatrixXd w_uv3 =MatrixXd::Zero(w_uv.rows(),3);
  w_uv3.leftCols(2) = w_uv;
  igl::writeOBJ("400layer.obj",w_uv3,w_F);
  v_.launch();
}


/*
*  @file 		ex5.cpp
*  @details  	This file is the solution to exercise 5.
*  @author    	Alexander Rettkowski
*  @date      	08.06.2017
*/
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/property_map/property_map.hpp>

#include <iostream>
#include <string>
#include <complex>
#include <fstream>

using namespace boost;

namespace exercise5
{
	namespace qi = boost::spirit::qi;
	namespace ascii = boost::spirit::ascii;
	struct edge
	{
		int startNode;
		int endNode;
		int length;
	};
}

BOOST_FUSION_ADAPT_STRUCT(
	exercise5::edge,
	(int, startNode)
	(int, endNode)
	(int, length)
)

namespace exercise5
{
	template <typename Iterator>
	struct line_parser : qi::grammar<Iterator, edge()>
	{
		line_parser() : line_parser::base_type(start)
		{
			using qi::int_;
			start %= int_ >> ' ' >> int_ >> ' ' >> int_;
		}

		qi::rule<Iterator, edge()> start;
	};
}

/**
* The main function that reads in a file and processes it.
* @param argc Number of command line arguments.
* @param *argv a pointer to the array of command line arguments.
*/
int main(int argc, char *argv[])
{
	typedef adjacency_list < listS, vecS, undirectedS, no_property, property < edge_weight_t, int > > graph_t;
	typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
	typedef std::pair<int, int> Edge;

	std::vector<Edge> edges;
	std::vector<int> weights;

	using boost::spirit::ascii::space;
	typedef std::string::const_iterator iterator_type;
	typedef exercise5::line_parser<iterator_type> line_parser;
	line_parser parser;
	std::string currentLine;
	std::ifstream file(argv[1]);

	// get number of nodes
	char delimiter = ' ';
	getline(file, currentLine, delimiter);
	int numberOfNodes = std::stoi(currentLine);
	getline(file, currentLine);
	int constSub = 1;
	while (getline(file, currentLine))
	{
		exercise5::edge parsedLine;
		std::string::const_iterator currentPosition = currentLine.begin();
		std::string::const_iterator lineEnd = currentLine.end();
		bool parsingSucceeded = phrase_parse(currentPosition, lineEnd, parser, space, parsedLine);

		if (parsingSucceeded && currentPosition == lineEnd)
		{
			edges.push_back(Edge(parsedLine.startNode-constSub, parsedLine.endNode-constSub));
			weights.push_back(parsedLine.length);
		}
	}

	file.close();

	graph_t g(edges.data(), edges.data() + edges.size(), weights.data(), numberOfNodes);

	property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
	std::vector<vertex_descriptor> p(num_vertices(g));
	std::vector<int> d(num_vertices(g));
	vertex_descriptor s = vertex(0, g);

	dijkstra_shortest_paths_no_color_map(g, s, predecessor_map(make_iterator_property_map(p.begin(), get(vertex_index, g))).
			distance_map(make_iterator_property_map(d.begin(), get(vertex_index, g))));

	graph_traits < graph_t >::vertex_iterator vi, vend;
	size_t vertex = -1;
	double distance = -1;
	for (tie(vi, vend) = vertices(g); vi != vend; ++vi) {
		if (d[*vi] > distance)
		{
			distance = d[*vi];
			vertex = *vi;
		}
		if ((d[*vi] == distance) && (*vi > vertex))
		{
			vertex = *vi;
		}
	}

	std::cout << "RESULT VERTEX " << vertex << std::endl;
	std::cout << "RESULT DIST " << distance << std::endl;

	return 0;
}


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/**
 * \file
 * \brief core fundamental matrix template implementations
 */

#include "fundamental_matrix.h"

#include <cmath>

#include <vital/exceptions/math.h>

#include <Eigen/SVD>

namespace kwiver {
namespace vital {

/// Construct from a provided matrix
template <typename T>
fundamental_matrix_<T>
::fundamental_matrix_( Eigen::Matrix<T,3,3> const &mat )
{
  Eigen::JacobiSVD<matrix_t> svd(mat, Eigen::ComputeFullU |
                                      Eigen::ComputeFullV);
  auto S = svd.singularValues();
  const matrix_t& U = svd.matrixU();
  const matrix_t& V = svd.matrixV();

  // clear the last singular value
  S[2] = T(0);
  S /= S.norm();
  mat_ = U*S.asDiagonal()*V.transpose();
}

/// Conversion Copy constructor -- float specialization
template <>
template <>
fundamental_matrix_<float>
::fundamental_matrix_( fundamental_matrix_<float> const &other )
  : mat_( other.mat_ )
{
}

/// Conversion Copy constructor -- double specialization
template <>
template <>
fundamental_matrix_<double>
::fundamental_matrix_( fundamental_matrix_<double> const &other )
  : mat_( other.mat_ )
{
}

/// Construct from a generic fundamental_matrix
template <typename T>
fundamental_matrix_<T>
::fundamental_matrix_( fundamental_matrix const &base )
  : mat_( base.matrix().template cast<T>() )
{
}

/// Construct from a generic fundamental_matrix -- double specialization
template <>
fundamental_matrix_<double>
::fundamental_matrix_( fundamental_matrix const &base )
  : mat_( base.matrix() )
{
}

/// Create a clone of outself as a shared pointer
template <typename T>
fundamental_matrix_sptr
fundamental_matrix_<T>
::clone() const
{
  return fundamental_matrix_sptr( new fundamental_matrix_<T>( *this ) );
}

/// Get a double-typed copy of the underlying matrix
template <typename T>
Eigen::Matrix<double,3,3>
fundamental_matrix_<T>
::matrix() const
{
  return this->mat_.template cast<double>();
}

/// Specialization for matrices with native double type
template <>
Eigen::Matrix<double,3,3>
fundamental_matrix_<double>
::matrix() const
{
  return this->mat_;
}

// ===========================================================================
// Other Functions
// ---------------------------------------------------------------------------

/// Output stream operator for \p fundamental_matrix instances
std::ostream&
operator<<( std::ostream &s, fundamental_matrix const &f )
{
  s << f.matrix();
  return s;
}

// ===========================================================================
// Template class instantiation
// ---------------------------------------------------------------------------
/// \cond DoxygenSuppress
#define INSTANTIATE_FUNDAMENTAL_MATRIX(T) \
  template class fundamental_matrix_<T>;

INSTANTIATE_FUNDAMENTAL_MATRIX(float);
INSTANTIATE_FUNDAMENTAL_MATRIX(double);
#undef INSTANTIATE_FUNDAMENTAL_MATRIX
/// \endcond

} } // end vital namespace