#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <boost/iterator/zip_iterator.hpp>
#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel         K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned, K>    Vb;
typedef CGAL::Triangulation_data_structure_2<Vb>                    Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds>                      Delaunay;
typedef Delaunay::Point                                             Point;
typedef Delaunay::Vertex_handle                                     Vertex_handle;

int main()
{

  std::vector<unsigned> indices;
  indices.push_back(0);
  indices.push_back(1);
  indices.push_back(2);
  indices.push_back(3);
  indices.push_back(4);
  indices.push_back(5);

  std::vector<Point> points;
  points.push_back(Point(0,0));
  points.push_back(Point(1,0));
  points.push_back(Point(0,1));
  points.push_back(Point(1,47));
  points.push_back(Point(2,2));
  points.push_back(Point(-1,0));

  Delaunay T;
  T.insert( boost::make_zip_iterator(boost::make_tuple( points.begin(),indices.begin() )),
            boost::make_zip_iterator(boost::make_tuple( points.end(),indices.end() ) )  );

  CGAL_assertion( T.number_of_vertices() == 6 );

  // check that the info was correctly set.

  for (Vertex_handle v : T.finite_vertex_handles())
    if( points[ v->info() ] != v->point() ){
      std::cerr << "Error different info" << std::endl;
      exit(EXIT_FAILURE);
    }

  return 0;
}


#include <iostream>
#include <iomanip>
#include <vector>
#include <opencv2/opencv.hpp>
#include <dlib/matrix.h>

using namespace std;
using namespace cv;
using namespace dlib;

matrix<double> getColMatrix( string filename ) {
    FileStorage fs( filename, FileStorage::READ );
    if( !fs.isOpened() ){
        cout << "File not found : " << filename << endl;
        return ones_matrix<double>(1, 1);
    }
    FileNode features = fs["features"];
    int num_points = features.size()*2;
    matrix<double> p( num_points, 1 );
    FileNodeIterator it_start = features.begin();
    FileNodeIterator it_end = features.end();
    std::vector<double> x_points, y_points;
    for( ; it_start != it_end; ++it_start ){
        double x = (double)(*it_start)["x"];
        double y = (double)(*it_start)["y"];
        x_points.push_back( x );
        y_points.push_back( y );
    }
    x_points.insert( x_points.end(), y_points.begin(), y_points.end() );
    fs.release();

    return mat(x_points);;
}

int main ( int argc, char *argv[] ) {
    matrix<double> points_pool, mean_matrix;
    int num_shape = atoi(argv[1]);
    bool init = true;
    // Initialize Points pool
    for( int i = 1; i <= num_shape; ++i ){
        string l_filename = "./dataset/"+to_string(i)+"L.yaml";
        string r_filename = "./dataset/"+to_string(i)+"R.yaml";
        matrix<double> l_p = getColMatrix( l_filename );
        matrix<double> r_p = getColMatrix( r_filename );
        bool found_left = points_pool.nr() == l_p.nr();
        bool found_right = points_pool.nr() == r_p.nr();
        if( init ){
            points_pool = join_rows( points_pool, l_p );
            init = false;
        }
        else {
            if( found_left )
                points_pool = join_rows( points_pool, l_p );

            if( found_right )
                points_pool = join_rows( points_pool, r_p );
        }

    }

    int pool_nr = points_pool.nr();
    int pool_nc = points_pool.nc();
    std::vector<double> mean_vector;
    for( int i = 0; i < pool_nr; ++i ){
        mean_vector.push_back( sum_cols( rowm( points_pool, i )/pool_nc ) );
    }

    // Writing MeanShape
    string fn = "./dataset/meanShape.yaml";
    FileStorage fs( fn, FileStorage::WRITE );
    fs << "filename" << fn;

    mean_matrix = mat( mean_vector );
    std::vector<Point2f> p_store;
    matrix<Point2f> mean_point;
    fs << "features" << "[";
    for( int i = 0; i < mean_matrix.nr()/2; ++i ){
            fs << "{:";
            fs << "p" << i << "x" << (int)mean_matrix(i) << "y" << (int)mean_matrix(i+68);
            fs << "}";
        Point2f p( mean_matrix(i),  mean_matrix(i+68) );
        p_store.push_back( p );
    }
    fs << "]";
    mean_point = mat( p_store );
    cout << mean_matrix << endl;
    cout << mean_point << endl;
    return 0;
}


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

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

#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include "utility.hpp"

BOOST_AUTO_TEST_SUITE(test_tensor_static_arithmetic_operations)

using double_extended = boost::multiprecision::cpp_bin_float_double_extended;

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

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

    fixture() = default;

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

};

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

    auto check = [](auto const& /*unused*/, auto& e)
    {
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type,extents_type,layout_type>;
        auto t  = tensor_type ();
        auto t2 = tensor_type ();
        auto r  = tensor_type ();
        auto v  = value_type  {};

        std::iota(t.begin(), t.end(), v);
        std::iota(t2.begin(), t2.end(), v+2);

        r = t + t + t + t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 3*t(i) + t2(i) );

        r = t2 / (t+3) * (t+1) - t2; // r = ( t2/ ((t+3)*(t+1)) ) - t2

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), t2(i) / (t(i)+3)*(t(i)+1) - t2(i) );

        r = 3+t2 / (t+3) * (t+1) * t - t2; // r = 3+( t2/ ((t+3)*(t+1)*t) ) - t2

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 3+t2(i) / (t(i)+3)*(t(i)+1)*t(i) - t2(i) );

        r = t2 - t + t2 - t;

        for(auto i = 0ul; i < r.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 4 );

        r = tensor_type (1) + tensor_type (1);

        for(auto i = 0ul; i < r.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2 );

        r = t * t * t * t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), t(i)*t(i)*t(i)*t2(i) );

        r = (t2/t2) * (t2/t2);

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 1 );
    };

    for_each_in_tuple(extents,check);
}

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

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

        std::iota(t.begin(), t.end(), v);
        std::iota(t2.begin(), t2.end(), v+2);

        tensor_type r1 = t + 2 + t + 2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r1(i), 2*t(i) + 4 );

        tensor_type r2 = 2 + t + 2 + t;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r2(i), 2*t(i) + 4 );

        tensor_type r3 = (t-2) + (t-2);

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r3(i), 2*t(i) - 4 );

        tensor_type r4 = (t*2) * (3*t);

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r4(i), 2*3*t(i)*t(i) );

        tensor_type r5 = (t2*2) / (2*t2) * t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r5(i), (t2(i)*2) / (2*t2(i)) * t2(i) );

        tensor_type r6 = (t2/2+1) / (2/t2+1) / t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r6(i), (t2(i)/2+1) / (2/t2(i)+1) / t2(i) );

    };

    for_each_in_tuple(extents,check);
}

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

    auto check = [](auto const& /*unused*/, auto& e)
    {
        using extents_type = std::decay_t<decltype(e)>;
        using tensor_type = ublas::tensor_static<value_type,extents_type,layout_type>;
        auto t  = tensor_type ();
        auto t2 = tensor_type ();
        auto r  = tensor_type ();
        auto v  = value_type  {};

        std::iota(t.begin(), t.end(), v);
        std::iota(t2.begin(), t2.end(), v+2);

        r  = t + 2;
        r += t;
        r += 2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*t(i) + 4 );

        r  = 2 + t;
        r += t;
        r += 2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*t(i) + 4 );

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*t(i) + 4 );

        r = (t-2);
        r += t;
        r -= 2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*t(i) - 4 );

        r  = (t*2);
        r *= 3;
        r *= t;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*3*t(i)*t(i) );

        r  = (t2*2);
        r /= 2;
        r /= t2;
        r *= t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), (t2(i)*2) / (2*t2(i)) * t2(i) );

        r  = (t2/2+1);
        r /= (2/t2+1);
        r /= t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), (t2(i)/2+1) / (2/t2(i)+1) / t2(i) );

        tensor_type q = -r;
        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( q(i), -r(i) );

        tensor_type p = +r;
        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( p(i), r(i) );
    };

    for_each_in_tuple(extents,check);
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SCALAR_NTHROOT_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SCALAR_NTHROOT_HPP_INCLUDED
#include <nt2/exponential/functions/nthroot.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/is_odd.hpp>
#include <nt2/include/functions/scalar/minusone.hpp>
#include <nt2/include/functions/scalar/pow.hpp>
#include <nt2/include/functions/scalar/rec.hpp>
#include <nt2/include/functions/scalar/sign.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/scalar/is_inf.hpp>
#endif
#include <iostream>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( nthroot_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< floating_<A0> >)(scalar_< integer_<A1> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(2)
    {
      bool is_ltza0 = is_ltz(a0);
      bool is_odda1 = is_odd(a1);
      if (is_ltza0 && !is_odda1) return Nan<A0>();
      A0 x = nt2::abs(a0);
      if (x == One<A0>())  return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (is_inf(a0)) return (a1) ? a0 : One<A0>();
      #endif
      A0 aa1 = static_cast<A0>(a1);
      A0 y = nt2::pow(x,rec(aa1));
      // Correct numerical errors (since, e.g., 64^(1/3) is not exactly 4)
      // by one iteration of Newton's method
      if (y) y -= (nt2::pow(y, a1) - x) / (aa1* nt2::pow(y,minusone(a1)));
      return (is_ltza0 && is_odda1)? -y : y;
    }
  };
} }

#endif


//
// $Id$
//
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//
// Original author: Robert Burke <robert.burke@gmail.com>
//
// This code taken from the following site:
// http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?Effective_UBLAS/Matrix_Inversion
//

#ifndef HOUSEHOLDERQR_HPP
#define HOUSEHOLDERQR_HPP

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>

namespace ublas = boost::numeric::ublas;

namespace pwiz {
namespace math {

template<class T>
void TransposeMultiply (const ublas::vector<T>& vector,
			ublas::matrix<T>& result,
			size_t size)
{
  result.resize (size,size);
  result.clear ();
  for(unsigned int row=0; row< vector.size(); ++row)
    {
      for(unsigned int col=0; col < vector.size(); ++col)
	result(row,col) = vector(col) * vector(row);

    }
}

template<class T>
void HouseholderCornerSubstraction (ublas::matrix<T>& LeftLarge,
				    const ublas::matrix<T>& RightSmall)
{
  using namespace boost::numeric::ublas;
  using namespace std;
  if(
     !(
       (LeftLarge.size1() >= RightSmall.size1())
       && (LeftLarge.size2() >= RightSmall.size2())
	)
      )
    {
      cerr << "invalid matrix dimensions" << endl;
      return;
    }

  size_t row_offset = LeftLarge.size2() - RightSmall.size2();
  size_t col_offset = LeftLarge.size1() - RightSmall.size1();

  for(unsigned int row = 0; row < RightSmall.size2(); ++row )
    for(unsigned int col = 0; col < RightSmall.size1(); ++col )
      LeftLarge(col_offset+col,row_offset+row) -= RightSmall(col,row);
}

template<class T>
void HouseholderQR (const ublas::matrix<T>& M,
		    ublas::matrix<T>& Q,
		    ublas::matrix<T>& R)
{
  using namespace boost::numeric::ublas;
  using namespace std;

  if(
     !(
       (M.size1() == M.size2())
	)
      )
    {
      cerr << "invalid matrix dimensions" << endl;
      return;
    }
  size_t size = M.size1();

  // init Matrices
  matrix<T> H, HTemp;
  HTemp = identity_matrix<T>(size);
  Q = identity_matrix<T>(size);
  R = M;

  // find Householder reflection matrices
  for(unsigned int col = 0; col < size-1; ++col)
    {
      // create X vector
      ublas::vector<T> RRowView = column(R,col);
      vector_range< ublas::vector<T> > X2 (RRowView, range (col, size));
      ublas::vector<T> X = X2;

      // X -> U~
      if(X(0) >= 0)
	X(0) += norm_2(X);
      else
	X(0) += -1*norm_2(X);

      HTemp.resize(X.size(),X.size(),true);

      TransposeMultiply(X, HTemp, X.size());

      // HTemp = the 2UUt part of H
      HTemp *= ( 2 / inner_prod(X,X) );

      // H = I - 2UUt
      H = identity_matrix<T>(size);
      HouseholderCornerSubstraction(H,HTemp);

      // add H to Q and R
      Q = prod(Q,H);
      R = prod(H,R);
    }
}

}
}

#endif // HOUSEHOLDERQR_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestNormalDistribution
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/count_if.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/random/normal_distribution.hpp>
#include <boost/compute/lambda.hpp>

#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(normal_distribution_doctest)
{
    using boost::compute::lambda::_1;

    boost::compute::vector<float> vec(10, context);

//! [generate]
// initialize the default random engine
boost::compute::default_random_engine engine(queue);

// setup the normal distribution to produce floats centered at 5
boost::compute::normal_distribution<float> distribution(5.0f, 1.0f);

// generate the random values and store them to 'vec'
distribution.generate(vec.begin(), vec.end(), engine, queue);
//! [generate]
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/unit_test.hpp>
#include <Eigen/Dense>

#include "../src/KRAverager.hpp"

#include "utils/log.hpp"

BOOST_AUTO_TEST_SUITE(kr_averager_tests)
    BOOST_AUTO_TEST_CASE(cis_averager) {
        TEST_MESSAGE("cis_averager");

        KRAveragerCis averager(3.0);
        Eigen::VectorXd ks(3);
        Eigen::VectorXd rs(3);

        ks << 1.0, 1.5, 4.0;
        rs << 2.0, 2.5, 5.0;

        averager.add_force_constant_vector(ks, rs);
        double r_cis = averager.get_range_cis();
        double k_cis = averager.get_force_constant_cis();

        BOOST_CHECK_EQUAL(r_cis, 2.25);
        BOOST_CHECK_EQUAL(k_cis, 1.25);

    }
BOOST_AUTO_TEST_SUITE_END()


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>
#include <time.h>
#include <vector>

#include <boost/random/gamma_distribution.hpp> // for gamma_distribution.
#include <boost/math/special_functions/gamma.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/cat.hpp>
#include <dpMM/mult.hpp>

using namespace Eigen;
using std::cout;
using std::endl;
using std::vector;

#ifdef BOOST_OLD
using boost::gamma_distribution;
#else
using boost::random::gamma_distribution;
#endif

#ifdef WIN32
	using boost::math::lgamma;
#endif

template<class Disc, typename T>
class Dir : public Distribution<T>
{
public:
  uint32_t K_;
  Matrix<T,Dynamic,1> alpha_;

  Dir(const Matrix<T,Dynamic,1>& alpha, boost::mt19937 *pRndGen);
  Dir(const Matrix<T,Dynamic,1>& alpha, const Matrix<T,Dynamic,1>& counts, boost::mt19937 *pRndGen);
  Dir(const Dir& other);
  ~Dir();

  Dir<Disc,T>* copy();

  Disc sample();
  Dir<Disc,T> posterior() const;
  Dir<Disc,T> posterior(const VectorXu& z);
  Dir<Disc,T> posterior(const Matrix<T,Dynamic,Dynamic>& x,
      const VectorXu& z, uint32_t k);
  Dir<Disc,T> posteriorFromCounts(const Matrix<T,Dynamic,1>& counts);
  Dir<Disc,T> posteriorFromCounts(const VectorXu& counts);
  Dir<Disc,T> posteriorFromCounts(const vector<Matrix<T,Dynamic,1> > &
      counts, const VectorXu& z, uint32_t k);

  T logPdf(const Disc& cat);

  uint32_t K(){return K_;}

  T logPdf(const Disc& cat) const;
  T logPdfMarginalized() const; // log pdf of SS under NIW prior
  T logPdfUnderPriorMarginalizedMerged(const Dir<Disc,T>& other) const;

  T logLikelihoodMarginalized(const Matrix<T,Dynamic,1>& counts) const;
  void print() const;

  virtual Dir<Disc,T>* merge(const Dir<Disc,T>& other);
  void fromMerge(const Dir<Disc,T>& niwA, const Dir<Disc,T>& niwB);

//  const Matrix<T,Dynamic,Dynamic>& scatter() const {return scatter_;};
//  Matrix<T,Dynamic,Dynamic>& scatter() {return scatter_;};
//  const Matrix<T,Dynamic,1>& mean() const {return mean_;};
//  Matrix<T,Dynamic,1>& mean() {return mean_;};
//  T count() const {return count_;};
//  T& count() {return count_;};
//
  const Matrix<T,Dynamic,1>& counts() const {return counts_;};
  Matrix<T,Dynamic,1>& counts() {return counts_;};
  void setCounts(const Matrix<T,Dynamic,1>& counts) {counts_ = counts;};
  T count() const {return counts_.sum();};

  void computeMergedSS( const Dir<Disc,T>& dirA,
      const Dir<Disc,T>& dirB, Matrix<T,Dynamic,1>& NsM) const;

private:

  Matrix<T,Dynamic,1> counts_; // counts for the different classes -> SS
  vector<gamma_distribution<> > gammas_;

  Matrix<T,Dynamic,1> samplePdf();
};

typedef Dir<Cat<double>, double> DirCatd;
typedef Dir<Cat<float>, float> DirCatf;
typedef Dir<Mult<double>, double> DirMultd;
typedef Dir<Mult<float>, float> DirMultf;


// full credits to https://github.com/ethz-asl/geodetic_utils
// todo add as external lib
#ifndef GEODETIC_CONVERTER_H_
#define GEODETIC_CONVERTER_H_

#include "math.h"
#include <Eigen/Dense>

namespace geodetic_converter
{
// Geodetic system parameters
static double kSemimajorAxis = 6378137;
static double kSemiminorAxis = 6356752.3142;
static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
static double kFlattening = 1 / 298.257223563;

class GeodeticConverter
{
public:
    GeodeticConverter()
    {
        haveReference_ = false;
    }

    ~GeodeticConverter()
    {
    }

    // Default copy constructor and assignment operator are OK.

    bool isInitialised()
    {
        return haveReference_;
    }

    void getReference(double* latitude, double* longitude, double* altitude)
    {
        *latitude = initial_latitude_;
        *longitude = initial_longitude_;
        *altitude = initial_altitude_;
    }

    void initialiseReference(const double latitude, const double longitude, const double altitude)
    {
        // Save NED origin
        initial_latitude_ = deg2Rad(latitude);
        initial_longitude_ = deg2Rad(longitude);
        initial_altitude_ = altitude;

        // Compute ECEF of NED origin
        geodetic2Ecef(latitude, longitude, altitude, &initial_ecef_x_, &initial_ecef_y_, &initial_ecef_z_);

        // Compute ECEF to NED and NED to ECEF matrices
        double phiP = atan2(initial_ecef_z_, sqrt(pow(initial_ecef_x_, 2) + pow(initial_ecef_y_, 2)));

        ecef_to_ned_matrix_ = nRe(phiP, initial_longitude_);
        ned_to_ecef_matrix_ = nRe(initial_latitude_, initial_longitude_).transpose();

        haveReference_ = true;
    }

    void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x,
                       double* y, double* z)
    {
        // Convert geodetic coordinates to ECEF.
        // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
        double lat_rad = deg2Rad(latitude);
        double lon_rad = deg2Rad(longitude);
        double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
        *x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
        *y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
        *z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad);
    }

    void ecef2Geodetic(const double x, const double y, const double z, double* latitude,
                       double* longitude, double* altitude)
    {
        // Convert ECEF coordinates to geodetic coordinates.
        // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
        // to geodetic coordinates," IEEE Transactions on Aerospace and
        // Electronic Systems, vol. 30, pp. 957-961, 1994.

        double r = sqrt(x * x + y * y);
        double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
        double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
        double G = r * r + (1 - kFirstEccentricitySquared) * z * z - kFirstEccentricitySquared * Esq;
        double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
        double S = cbrt(1 + C + sqrt(C * C + 2 * C));
        double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
        double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
        double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q) + sqrt(
                                                                          0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) - P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
        double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
        double V = sqrt(
            pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * z * z);
        double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
        *altitude = U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V));
        *latitude = rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r));
        *longitude = rad2Deg(atan2(y, x));
    }

    void ecef2Ned(const double x, const double y, const double z, double* north, double* east,
                  double* down)
    {
        // Converts ECEF coordinate position into local-tangent-plane NED.
        // Coordinates relative to given ECEF coordinate frame.

        Eigen::Vector3d vect, ret;
        vect(0) = x - initial_ecef_x_;
        vect(1) = y - initial_ecef_y_;
        vect(2) = z - initial_ecef_z_;
        ret = ecef_to_ned_matrix_ * vect;
        *north = ret(0);
        *east = ret(1);
        *down = -ret(2);
    }

    void ned2Ecef(const double north, const double east, const double down, double* x, double* y,
                  double* z)
    {
        // NED (north/east/down) to ECEF coordinates
        Eigen::Vector3d ned, ret;
        ned(0) = north;
        ned(1) = east;
        ned(2) = -down;
        ret = ned_to_ecef_matrix_ * ned;
        *x = ret(0) + initial_ecef_x_;
        *y = ret(1) + initial_ecef_y_;
        *z = ret(2) + initial_ecef_z_;
    }

    void geodetic2Ned(const double latitude, const double longitude, const double altitude,
                      double* north, double* east, double* down)
    {
        // Geodetic position to local NED frame
        double x, y, z;
        geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
        ecef2Ned(x, y, z, north, east, down);
    }

    void ned2Geodetic(const double north, const double east, const double down, double* latitude,
                      double* longitude, double* altitude)
    {
        // Local NED position to geodetic coordinates
        double x, y, z;
        ned2Ecef(north, east, down, &x, &y, &z);
        ecef2Geodetic(x, y, z, latitude, longitude, altitude);
    }

    void geodetic2Enu(const double latitude, const double longitude, const double altitude,
                      double* east, double* north, double* up)
    {
        // Geodetic position to local ENU frame
        double x, y, z;
        geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);

        double aux_north, aux_east, aux_down;
        ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);

        *east = aux_east;
        *north = aux_north;
        *up = -aux_down;
    }

    void enu2Geodetic(const double east, const double north, const double up, double* latitude,
                      double* longitude, double* altitude)
    {
        // Local ENU position to geodetic coordinates

        const double aux_north = north;
        const double aux_east = east;
        const double aux_down = -up;
        double x, y, z;
        ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
        ecef2Geodetic(x, y, z, latitude, longitude, altitude);
    }

private:
    inline Eigen::Matrix3d nRe(const double lat_radians, const double lon_radians)
    {
        const double sLat = sin(lat_radians);
        const double sLon = sin(lon_radians);
        const double cLat = cos(lat_radians);
        const double cLon = cos(lon_radians);

        Eigen::Matrix3d ret;
        ret(0, 0) = -sLat * cLon;
        ret(0, 1) = -sLat * sLon;
        ret(0, 2) = cLat;
        ret(1, 0) = -sLon;
        ret(1, 1) = cLon;
        ret(1, 2) = 0.0;
        ret(2, 0) = cLat * cLon;
        ret(2, 1) = cLat * sLon;
        ret(2, 2) = sLat;

        return ret;
    }

    inline double rad2Deg(const double radians)
    {
        return (radians / M_PI) * 180.0;
    }

    inline double deg2Rad(const double degrees)
    {
        return (degrees / 180.0) * M_PI;
    }

    double initial_latitude_;
    double initial_longitude_;
    double initial_altitude_;

    double initial_ecef_x_;
    double initial_ecef_y_;
    double initial_ecef_z_;

    Eigen::Matrix3d ecef_to_ned_matrix_;
    Eigen::Matrix3d ned_to_ecef_matrix_;

    bool haveReference_;

}; // class GeodeticConverter
}; // namespace geodetic_conv

#endif // GEODETIC_CONVERTER_H_


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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 <fcppt/math/matrix/comparison.hpp>
#include <fcppt/math/matrix/output.hpp>
#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/math/matrix/transpose.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_transpose
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		int,
		2,
		3
	>
	matrix_type;

	typedef
	fcppt::math::matrix::static_<
		int,
		3,
		2
	>
	transposed_matrix_type;

	transposed_matrix_type const transposed(
		fcppt::math::matrix::transpose(
			matrix_type(
				fcppt::math::matrix::row(
					1, 2, 3
				),
				fcppt::math::matrix::row(
					4, 5, 6
				)
			)
		)
	);

	BOOST_CHECK_EQUAL(
		transposed,
		transposed_matrix_type(
			fcppt::math::matrix::row(
				1, 4
			),
			fcppt::math::matrix::row(
				2, 5
			),
			fcppt::math::matrix::row(
				3, 6
			)
		)
	);
}


#pragma once
#ifndef TYPEDEF_HPP
#define TYPEDEF_HPP

#define EIGEN_NO_DEBUG

// extern - Eigen
#include <Eigen/Dense>

typedef Eigen::Matrix<double,3,1> Vec3d;
typedef Eigen::Matrix<double,Eigen::Dynamic,1> VecXd;
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatXd;

#endif


#include <iostream>
#include <armadillo>
#include <boost/tuple/tuple.hpp>
#include "gnuplot-iostream.h"
#include "ODE.h"
#include "sysFunc.h"

/* I should make it compatible with armadillo, currently it is working with std::vector containers
to be able to make it work with armadillo I need to make it generic.
*/

using namespace STASC;
int main()
{

  std::vector<State> systemTrajectory;
  constexpr size_t timeStepsCount = 1000;
  constexpr double stepSize = 0.1;
  std::vector<double> timeStepsVec(timeStepsCount);
  std::vector<double> position;

  // ODE
  {
    for (size_t i = 0; i < timeStepsCount; i++)
    {
      timeStepsVec.at(i) = i * stepSize;
    }

    ODE ode(SysFunc::sysFuncOne, SysFunc::initialStateOne, timeStepsVec);

    // Integrate and collect the state at each time point
    for (size_t i = 0; i < timeStepsCount - 1; i++)
    {
      ode.int_u_dt(STASC::IntegrationMode::ERK1);
      systemTrajectory.push_back(ode.getStateVector());
    }
    assert(systemTrajectory.size() == timeStepsCount - 1);
     for ( auto element : systemTrajectory)
        position.push_back(element.at(0).real());
    //   std::cout << element.at(0).real() << "\n"; // show the position.
  }

  // GNU Plot
  // std::cout << "position size: " << position.size() << std::endl;
  // std::cout << "time vector size: " << timeStepsVec.size() << std::endl;

  if(true){
    /* output */
    // to make sizes match.
    position.push_back(*position.cend()); // Duplicating the last element.
    Gnuplot gp;
    gp << "plot '-' using 1:2 with linespoint" << std::endl;
    gp.send1d(std::make_tuple(timeStepsVec, position));
  }

  return 0;
}


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

#pragma once

#include "../util/FluidEigenMappings.hpp"
#include "../util/Novelty.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

class NoveltyFeature
{

public:
  using ArrayXd = Eigen::ArrayXd;

  NoveltyFeature(index maxKernelSize, index maxFilterSize)
      : mFilterBufferStorage(maxFilterSize), mNovelty(maxKernelSize)
  {}

  void init(index kernelSize, index filterSize, index nDims)
  {
    assert(kernelSize % 2);
    mNovelty.init(kernelSize, nDims);
    mFilterBuffer = mFilterBufferStorage.segment(0, filterSize);
    mFilterBuffer.setZero();
  }

  double processFrame(const RealVectorView input)
  {
    double novelty = mNovelty.processFrame(_impl::asEigen<Eigen::Array>(input));
    index  filterSize = mFilterBuffer.size();

    if (filterSize > 1)
    {
      mFilterBuffer.segment(0, filterSize - 1) =
          mFilterBuffer.segment(1, filterSize - 1);
    }

    mFilterBuffer(filterSize - 1) = novelty;

    return mFilterBuffer.mean();
  }

private:
  ArrayXd mFilterBuffer;
  ArrayXd mFilterBufferStorage;
  Novelty mNovelty;
};
} // namespace algorithm
} // namespace fluid


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef NL_PROBLEM_SO3_HPP_
#define NL_PROBLEM_SO3_HPP_

#include <Eigen/Dense>
#include <cbr_math/lie/Tn.hpp>
#include <cbr_math/lie/group_product.hpp>
#include <sophus/so3.hpp>

/**
 * @brief Defines an optimal control problem on (X, V) \in SO(3) \timex R3 with three inputs
 *   d^r X_t = V
 *   d^r V_t = u
 */
struct SO3Problem
{
  using state_t = cbr::lie::GroupProduct<double, 0, Sophus::SO3, cbr::lie::T3>;
  using deriv_t = typename state_t::Tangent;
  using input_t = Eigen::Vector3d;

  static constexpr std::size_t nx = state_t::DoF;
  static constexpr std::size_t nu = input_t::SizeAtCompileTime;

  // Dynamics (must be differentiable so we make a generic template)
  template<typename T, typename Derived>
  auto get_f(const T & x, const Eigen::MatrixBase<Derived> & u) const
  {
    using Scalar = typename decltype(x.log() * u.transpose())::EvalReturnType::Scalar;

    Eigen::Matrix<Scalar, 6, 1> ret;
    ret.template segment<3>(0) = std::get<1>(x).translation();
    ret.template segment<3>(3) = u.eval();
    return ret;
  }

  void get_input_lb(double, Eigen::Ref<input_t> input_lb) const
  {
    input_lb.setConstant(-0.3);
  }

  void get_input_ub(double, Eigen::Ref<input_t> input_ub) const
  {
    input_ub.setConstant(0.3);
  }

  Eigen::Matrix<double, nx, nx> get_Q(double) const
  {
    return 0.1 * Eigen::Matrix<double, nx, nx>::Identity();
  }
  Eigen::Matrix<double, nx, nx> get_QT() const
  {
    return Eigen::Matrix<double, nx, nx>::Identity();
  }
  Eigen::Matrix<double, nu, nu> get_R(double) const
  {
    return 0.01 * Eigen::Matrix<double, nu, nu>::Identity();
  }
};

#endif  // NL_PROBLEM_SO3_HPP_


/***************************************************************************
 *   Copyright (C) 2007 by BEEKHOF, Fokko                                  *
 *   fpbeekhof@gmail.com                                                   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include <limits>
#include <climits>
#include <cassert>
#include <utility>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "../base/stl_cmath.h"

#include "../base/use_omp.h"
#include "../../omptl/omptl_algorithm"
#include "../../omptl/omptl_numeric"

#include "../base/Functors"
#include "../base/CyclicBuffer"

#include "../math/ContainerOps.h"

namespace cvmlcpp
{

namespace detail
{

template <typename T, typename U>
T round_cast_(const U &u, std::tr1::true_type)
{
	return static_cast<T>(u);
}

template <typename T, typename U>
T round_cast_(const U &u, std::tr1::false_type)
{
	return static_cast<T>(u + ((u>=0)?U(0.5):U(-0.5)) );
}

} // end namespace detail

template <typename T, typename U>
T round_cast(const U &u)
{
	return detail::round_cast_<T, U>(u, std::tr1::is_floating_point<T>());
}

namespace detail
{

template <typename T>
T log2_(T n, std::tr1::false_type)
{
	assert(n > 0);
	const std::size_t N = CHAR_BIT*sizeof(T);

	T result = 0;
	for (std::size_t i = 1; i < N; ++i)
	{
		const std::size_t M = N-i;
		if ( n >= (std::size_t(1) << M) )
		{
			n >>= M;
			result |= M;
		}
	}

	return result;
}

template <typename T>
T log2_(const T &n, std::tr1::true_type)
{
#ifdef _HAVE_TR1_CMATH
	using std::tr1::log2;
	return log2(n);
#else
	using std::log;
	return log(n) / log(2.0);
#endif
}

} // end namespace detail

template <typename T>
T log2(const T n)
{
	assert(n > T(0)); // Bogus user input ?
	return detail::log2_(n, std::tr1::is_floating_point<T>());
}

template <typename T>
T factorial(const T &x)
{
	// Check user input
	assert(x >= 0);

	static std::size_t ntop = 12;
	const std::size_t cache_size = 32;
	static T cache[cache_size] = {
	1, 1, 2, 6, 24, 120, 720, 5040,
	40320, 362880, 3628800, 39916800, 479001600};

#ifdef _HAVE_TR1_CMATH
	assert(x == std::tr1::round(x));
#endif

	if (x >= T(cache_size))
	{
		#ifdef _HAVE_TR1_CMATH
 		return std::tr1::round(std::tr1::tgamma(x+1.0));
		#else
		assert(ntop > 0);
		T v = cache[ntop-1];
		std::size_t part1 = round_cast<std::size_t>(std::min(T(cache_size), x));
		for (std::size_t i = ntop+1; i < part1; ++i)
		{
			v *= static_cast<T>(i);
			cache[i] = v;
		}
		ntop = part1;

		// Non-cached part
		for (T i = part1+1; i < x; ++i)
			v *= static_cast<T>(i);

		return v;
		#endif
	}

	while (x > ntop)
	{
		cache[ntop] = cache[ntop-1u] * ntop;
		++ntop;
	}
	assert(ntop >= x);

#ifdef _HAVE_TR1_CMATH
	return cache[std::tr1::lround(x)];
#else
	return cache[round_cast<std::size_t>(x)];
#endif
}

// Function gcd adapted from wikipedia: http://en.wikipedia.org/wiki/Binary_GCD_algorithm
namespace detail
{

template <bool is_integer = false>
struct GCD
{
	// euclidean based implementation
	template <typename T>
	static const T gcd(T u, T v)
	{
		assert(u >= 0);
		assert(v >= 0);

		using std::floor;
		u = floor(u+T(0.5));
		v = floor(v+T(0.5));

		using std::max;
		if (u < 1 || v < 1) // in case one is zero
			max(u, v);

		while (u > T(0.5)) // take care of roundoff problems
		{
			const T t = u;
			using std::floor;
			u = floor(v - floor(v / u)*u + T(0.5)); // modulus
			v = t;
		}
		assert(floor(v) == v);

		return v;
	}
};

template <>
struct GCD<true>
{
	// Function gcd adapted from wikipedia: http://en.wikipedia.org/wiki/Binary_GCD_algorithm
	template <typename T>
	static const T gcd(T u, T v)
	{
		// Use this function only for integers
		assert(std::tr1::is_integral<T>::value);

		/* GCD(0,x) := x */
		if (u == 0 || v == 0)
			return u | v;

		/* Let shift := lg K, where K is the greatest power of 2
		dividing both u and v. */
		T shift = 0;
		for (; ((u | v) & 1) == 0; ++shift)
		{
			u >>= 1;
			v >>= 1;
		}

		while ((u & 1) == 0)
			u >>= 1;

		/* From here on, u is always odd. */
		assert(u & 1);
		do {
			while ((v & 1) == 0)  /* Loop X */
				v >>= 1;

			/* Now u and v are both odd, so diff(u, v) is even.
			Let u = min(u, v), v = diff(u, v)/2. */
			if (u < v)
				v -= u;
			else
			{
				const T diff = u - v;
				u = v;
				v = diff;
			}
			v >>= 1;
		} while (v != 0);

		return u << shift;
	}
};

}

template <typename T>
T gcd(const T u, const T v)
{
	return detail::GCD<std::tr1::is_integral<T>::value>::gcd(u, v);
}

namespace detail
{

template <bool is_integer = false>
struct Binomial
{
	template <typename T>
	static const T binomial(const T n, const T k)
	{
		//return factorial(n) / (factorial(k) * factorial(n-k));
		if (n == k)
			return 1;
		if (n == k+1)
			return n;

		// Factors
		assert(k < n);
		std::vector<T> ns(n-k), ks(n-k-1);
		assert(ks.size() >= 1);
		for (T i = 0; i < n-k; ++i)
			ns[i] = k+1+i;
		for (T i = 2; i <= n-k; ++i)
			ks[i-2] = i;

		// Simplify
		for (std::size_t i = 0; i < ks.size(); ++i)
		{
			if (ks[i] > 1)
			{
				for (std::size_t j = 0; j < ns.size(); ++j)
					if (ns[j] > 1)
					{
						const T d = gcd(ns[j], ks[i]);
//						assert( (ks[i] % d) == 0 );
//						assert( (ns[j] % d) == 0 );
						ks[i] /= d;
						ns[j] /= d;
					}
			}
		}

		// After simplification, denominator is one
		assert(std::abs(std::accumulate(ks.begin(), ks.end(), T(1), std::multiplies<T>()) - T(1)) < 0.001);

		// Final division not needed, denominator is one
		return std::accumulate(ns.begin(), ns.end(), T(1), std::multiplies<T>());
	}
};

template <>
struct Binomial<true>
{
	template <typename T>
	static const T binomial(const T n, T k)
	{
		enum {N, K, RESULT};
		const T CACHE_SIZE = 29;
		static std::vector<std::vector<std::tr1::array<T, 3> > > cache;

		// Init cache if needed
		if (cache.empty())
		{
			cache.resize(CACHE_SIZE);
			for (T i = 0; i < CACHE_SIZE; ++i)
			{
				cache[i].resize(CACHE_SIZE);
				for (T j = 0; j < CACHE_SIZE; ++j)
					std::fill(cache[i][j].begin(), cache[i][j].end(), 0);
			}
		}

		// Check in cache
		const T cn = n % CACHE_SIZE;
		const T ck = k % CACHE_SIZE;
		if ( (cache[cn][ck][N] == n) && (cache[cn][ck][K] == k) )
			return cache[cn][ck][RESULT];

		/*
		 * General approach:
		 *      n!       prod(k+1:n)
		 * ---------- = ----------
		 * (n-k)! k!      (n-k)!
		 * Then factor out GCD's to try to avoid overflow
		 */
		assert (k <= n);

		// Optimize for complexity, binomial is symmetric for
		// k or n-k, but selecting the largest of the 2 leads to
		// less terms in the sequences.
		k = std::max(k, n-k);

		// Simple cases that lead to complex indices below
		if (n == k)
			return 1;
		if (n == k+1)
			return n;

		// Factors
		assert(k < n);
		std::vector<T> ns(n-k), ks(n-k-1);
		assert(ks.size() >= 1);
		for (T i = 0; i < n-k; ++i)
			ns[i] = k+1+i;
		for (T i = 2; i <= n-k; ++i)
			ks[i-2] = i;

		// Simplify
		for (std::size_t i = 0; i < ks.size(); ++i)
		{
			if (ks[i] > 1)
			{
				for (std::size_t j = 0; j < ns.size(); ++j)
					if (ns[j] > 1)
					{
						const T d = gcd(ns[j], ks[i]);
						assert( (ks[i] % d) == 0 );
						assert( (ns[j] % d) == 0 );
						ks[i] /= d;
						ns[j] /= d;
					}
			}
		}

		// After simplification, denominator is one
		assert(std::accumulate(ks.begin(), ks.end(), T(1), std::multiplies<T>()) == T(1));

		// Final division not needed, denominator is one
		const T result = std::accumulate(ns.begin(), ns.end(), T(1), std::multiplies<T>());
				// / std::accumulate(ks.begin(), ks.end(), T(1), std::multiplies<T>());

		// keep result in cache
		cache[cn][ck][RESULT] = result;

		return result;
	}
};

}

template <typename T>
T binomial(const T n, const T k)
{
	// verify user input
	assert(n >= k);
	return detail::Binomial<std::tr1::is_integral<T>::value>::binomial(n, k);
}

template <typename T>
T binopmf(const std::size_t n, const std::size_t k, const T p)
{
	BOOST_STATIC_ASSERT(!std::numeric_limits<T>::is_integer);
	assert(n >= k);
	assert(p >= 0);
	assert(p <= 1);
	using std::pow;

	const std::size_t max_bits_needed_for_binomial = (n+1)/2 * (log2(n)+1);

	const T binomial_n_k = (max_bits_needed_for_binomial < CHAR_BIT * sizeof(std::size_t) ) ?
		binomial(n, k) : // fast integer binomial
		binomial(T(n), T(k));  // FP binomial, no overflow

	return binomial_n_k * pow(p, T(k)) * pow( T(1)-p, T(n-k) );
}

template <typename T>
T binocdf(const std::size_t n, const std::size_t k, const T p)
{
	BOOST_STATIC_ASSERT(!std::numeric_limits<T>::is_integer);
	assert(n >= k);
	assert(p >= 0);
	assert(p <= 1);

	T bin = 1;
	T result = 0;
	T pk = 1;
	using std::pow;
	T pnk = pow(T(1)-p, T(n));
	for (std::size_t i = 0; i < k; ++i)

	{
		result += T(bin) * pk * pnk;
		//const T g = gcd(n-i, i+1);
		bin *= (n-i);  //((n-i) / g);
		bin /= (i+1);  //((i+1) / g);
//		assert(bin == binomial(n, i+1)); // can be triggered by numerical issues
		pk  *= p;
		pnk /= T(1)-p;
	}
	result += bin * pk * pnk;

	return result;
}

template <typename T, typename U>
std::size_t binocdfinv(const T p_arg, const std::size_t n, const U p)
{
	BOOST_STATIC_ASSERT(!std::numeric_limits<T>::is_integer);
	assert(n > 0);

	assert(p_arg >= 0.0);
	assert(p_arg <= 1.0);

	assert(p >= 0.0);
	assert(p <= 1.0);

	std::size_t result = 0;
	T cdf = binopmf(0, n, p);
	while ( (cdf < p_arg) && (result < n) )
	{
		++result;
		cdf += binopmf(result, n, p);
	}
	assert(result <= n);
	return result;
}

#ifdef __GXX_EXPERIMENTAL_CXX0X__
template <typename T>
T qfunc(const T x) { return T(0.5) * std::erfc(x / std::sqrt(T(2))); }
#endif

/**
 * Simple Statistics
 */

namespace detail
{

template <typename T>
typename T::value_type modulus_(const T &x, std::tr1::false_type)
{
	typedef typename T::value_type VT;
	const VT rv = omptl::transform_accumulate(x.begin(), x.end(), VT(0),
						cvmlcpp::Square<VT>());

	return round_cast<VT>(std::sqrt(rv));
}

template <typename T>
T modulus_arithmic_(const T &x, std::tr1::true_type)
{
	return x;
}

template <typename T>
T modulus_arithmic_(const T &x, std::tr1::false_type)
{
	using std::abs;
	return abs(x);
}

template <typename T>
T modulus_(const T &x, std::tr1::true_type)
{
	return modulus_arithmic_(x, std::tr1::is_unsigned<T>());
}

} // end namespace detail

template <typename T>
typename ValueType<T>::value_type modulus(const T &x)
{
	return round_cast<typename ValueType<T>::value_type>
		(detail::modulus_(x, std::tr1::is_arithmetic<T>()));
}
/*
template <typename Iterator, typename T>
typename promote_trait1<T>::value_type average(Iterator begin, Iterator end, const T init)
{
	typedef typename promote_trait1<T>::value_type R;
	const typename std::iterator_traits<Iterator>::difference_type
		n = std::distance(begin, end);
	assert(n >= 0);

	if (n == 0)
		return R(init);

	return R(omptl::accumulate(begin, end, init)) / static_cast<R>(n);
}
*/
/*
template <typename Iterator, typename T>
T median(Iterator begin, Iterator end, const T init)
{
	if (std::distance(begin, end) < 1)
		return init;
	std::vector<T> data(std::distance(begin, end));
	std::copy(begin, end, data.begin());
	omptl::sort(data.begin(), data.end());
	return init + *(data.begin() + data.size() / 2u);
}
*/
namespace detail
{

template <typename T>
class Variance_
{
	public:
		typedef T value_type;
		typedef T result_type;

		Variance_(const T mean = T(0)) : mean_(mean) {}

		T operator()(const T &elem) const
		{
			return std::pow(elem - mean_, T(2));
		}

	private:
		const T mean_;
};

} // end namespace detail
/*
template <typename Iterator, typename T, typename U>
typename promote_trait1<T>::value_type variance(Iterator begin, Iterator end, const T mean, const U init)
{
	typedef typename promote_trait1<T>::value_type R;
	const typename std::iterator_traits<Iterator>::difference_type
		n = std::distance(begin, end);
	assert(n >= 0);

	if (n < 2)
		return init;

	if (mean == T(0))
		return R(init) +
			R(omptl::transform_accumulate(begin,end,T(0),Square<T>()))
			// ----------------------------------------------
					/ static_cast<R>(n-1);

	detail::Variance_<T> var((mean));
	return R(init) + R(omptl::transform_accumulate(begin, end, T(0), var))
			// ----------------------------------------------
					/ static_cast<R>(n-1);
}
*/
template <typename Iterator, typename T>
typename promote_trait1<T>::value_type variance(Iterator begin, Iterator end, const T init)
{
	return variance(begin, end, average(begin, end, T(0)), init);
}

template <typename Iterator, typename T>
typename promote_trait1<T>::value_type deviation(Iterator begin, Iterator end, const T init)
{
	return init + std::sqrt(variance(begin, end, T(0), init));
}

template <typename Iterator, typename T, typename U>
typename promote_trait1<T>::value_type deviation(Iterator begin, Iterator end, const T mean, const U init)
{
	return init + std::sqrt(variance(begin, end, mean, U(0)));
}

/*
template <typename Iterator, typename T>
T generalizedGaussianShape(Iterator begin, Iterator end,
			   const T avg, const T init)
{
	const typename std::iterator_traits<Iterator>::difference_type
		n = std::distance(begin, end);
	assert(n >= 0);

	if (n < 2)
		return init;

	const T lut [] = {
		0.0000000e+00, 1.3426932e-03, 1.1876961e-02, 3.5319079e-02,
		6.7911558e-02, 1.0500095e-01, 1.4333004e-01, 1.8099448e-01,
		2.1699493e-01, 2.5087260e-01, 2.8247508e-01, 3.1181684e-01,
		3.3899861e-01, 3.6416155e-01, 3.8746151e-01, 4.0905505e-01,
		4.2909201e-01, 4.4771200e-01, 4.6504301e-01, 4.8120124e-01,
		4.9629158e-01, 5.1040840e-01, 5.2363643e-01, 5.3605172e-01,
		5.4772256e-01, 5.5871029e-01, 5.6907008e-01, 5.7885162e-01,
		5.8809974e-01, 5.9685492e-01, 6.0515382e-01, 6.1302967e-01,
		6.2051264e-01, 6.2763020e-01, 6.3440738e-01, 6.4086703e-01,
		6.4703005e-01, 6.5291558e-01, 6.5854121e-01, 6.6392307e-01,
		6.6907603e-01, 6.7401380e-01, 6.7874902e-01, 6.8329340e-01,
		6.8765777e-01, 6.9185215e-01, 6.9588588e-01, 6.9976761e-01,
		7.0350541e-01, 7.0710678e-01, 7.1057873e-01, 7.1392780e-01,
		7.1716011e-01, 7.2028138e-01, 7.2329696e-01, 7.2621189e-01,
		7.2903089e-01, 7.3175837e-01, 7.3439853e-01, 7.3695527e-01,
		7.3943229e-01, 7.4183308e-01, 7.4416092e-01, 7.4641892e-01,
		7.4861001e-01, 7.5073697e-01, 7.5280242e-01, 7.5480885e-01,
		7.5675862e-01, 7.5865396e-01, 7.6049699e-01, 7.6228974e-01,
		7.6403410e-01, 7.6573191e-01, 7.6738490e-01, 7.6899471e-01,
		7.7056292e-01, 7.7209102e-01, 7.7358045e-01, 7.7503256e-01,
		7.7644865e-01, 7.7782996e-01, 7.7917769e-01, 7.8049297e-01,
		7.8177687e-01, 7.8303045e-01, 7.8425468e-01, 7.8545054e-01,
		7.8661892e-01, 7.8776070e-01, 7.8887673e-01, 7.8996780e-01,
		7.9103468e-01, 7.9207813e-01, 7.9309885e-01, 7.9409752e-01,
		7.9507481e-01, 7.9603134e-01, 7.9696773e-01, 7.9788456e-01 };

	const detail::Variance_<T> var;
	if (avg != 0.0)
	{
		// Must substract mean ...
		std::vector<T> zeroMeanData( (std::distance(begin, end)) );
		omptl::transform(begin, end, zeroMeanData.begin(),
				 std::bind2nd(std::plus<T>(), -avg) );

		const T sig = std::sqrt(
			omptl::transform_accumulate(zeroMeanData.begin(),
					    zeroMeanData.end(), T(0), var)
			// ----------------------------------------------
					/ static_cast<T>(n-1) );

		const T mean_abs =
			omptl::transform_accumulate(zeroMeanData.begin(),
						    zeroMeanData.end(), T(0),
				 std::ptr_fun( ( (T(*)(T))std::abs) ) )
			// ----------------------------------------------
					/ static_cast<T>(n);

		const T* pos = std::lower_bound(lut, lut+100, mean_abs/sig );

		return init + T(std::distance(lut, pos)) * 0.02;
	}

	// Avg == 0.0; faster calculation

	const T sig =std::sqrt(
			omptl::transform_accumulate(begin, end, T(0), var)
		// ----------------------------------------------
				/ static_cast<T>(n-1) );

	const T mean_abs =
		omptl::transform_accumulate(begin, end, T(0),
					    std::ptr_fun( (T(*)(T))std::abs ) )
		// ----------------------------------------------
				/ static_cast<T>(n);

	const T* pos = std::lower_bound(lut, lut+100, mean_abs/sig );

	return init + T(std::distance(lut, pos)) * 0.02;
}
*/
template <typename Iterator, typename T>
T generalizedGaussianShape(Iterator begin, Iterator end, const T init)
{
	return generalizedGaussianShape(begin, end,
					average(begin, end, T(0)), init);
}

/*
 * Creative optimalization.
 * No idea if this is supposed to work
 */

namespace detail
{

template <class T>
struct OptimizationUpdater;

template <bool value_is_arithmetic, bool result_is_arithmetic>
struct OptimizationUpdater_
{
	template <class value_type, class result_type>
	static bool update(value_type &low,  value_type &dLow,  result_type &fLow,//  value_type &bLow,
			   value_type &high, value_type &dHigh, result_type &fHigh,// value_type &bHigh,
			   const value_type &x, const value_type &dx, const result_type &fx)
	{
		assert(dLow * dHigh <= 0);
		if (dx*dLow > 0) // same sign ?
		{
			assert (x >= low);
			assert(dx * dHigh <= 0); // need opposing signs
			low  = x;
			dLow = dx;
			fLow = fx;
//			bLow  = fLow - dLow  * low;
		}
		else if (dx*dHigh > 0)
		{
			assert (x <= high);
			assert(dx * dLow <= 0); // need opposing signs
			high  = x;
			dHigh = dx;
			fHigh = fx;
//			bHigh = fHigh - dHigh * high;
		}
		else
		{
/*			std::cout << std::scientific;
			if (!(dx == 0))
			{
				std::cout << low << " " << x << " " << high << std::endl;
				std::cout << dLow << " " << dx << " " << dHigh << std::endl;
			}
*/
			assert(dx == 0);

/*			// just in case
			high  = low  = x;
			dHigh = dLow = dx;
			fHigh = fLow = fx;
*/
			return false; // derivative zero --> optimum found
		}
		assert(dLow * dHigh <= 0); // need opposing signs

		return true;
	}

};

template <>
struct OptimizationUpdater_<false, false>
{
	template <class value_type, class result_type>
	static bool update(value_type &low,  value_type &dLow,  result_type &fLow,//  value_type &bLow,
			   value_type &high, value_type &dHigh, result_type &fHigh,// value_type &bHigh,
			   const value_type &x, const value_type &dx, const result_type &fx)
	{
		typedef typename value_type::value_type VT;
		std::size_t n = 0;
		for (std::size_t i = 0; i < low.size(); ++i)
		{
			if (OptimizationUpdater< VT >::
			    update(low[i], dLow[i], fLow[i],// bLow[i],
				   high[i], dHigh[i], fHigh[i],// bHigh[i],
				   x[i], dx[i], fx[i]))
				++n;
		}

		// Update considered successful if at least one parameter was updated
		return n > 0;
	}
};

template <>
struct OptimizationUpdater_<true, false>
{
	template <class value_type, class result_type>
	static bool update(value_type &low,  value_type &dLow,  result_type &fLow,//  value_type &bLow,
			   value_type &high, value_type &dHigh, result_type &fHigh,// value_type &bHigh,
			   const value_type &x, const value_type &dx, const result_type &fx)
	{
		typedef typename value_type::value_type VT;
		std::size_t n = 0;
		for (std::size_t i = 0; i < low.size(); ++i)
		{
			if (OptimizationUpdater< VT >::
			    update(low, dLow, fLow[i],// bLow,
				   high, dHigh, fHigh[i],// bHigh,
				   x, dx, fx[i]))
				++n;
		}

		// Update considered successful if at least one parameter was updated
		return n > 0;
	}
};

template <>
struct OptimizationUpdater_<false, true>
{
	template <class value_type, class result_type>
	static bool update(value_type &low,  value_type &dLow,  result_type &fLow,//  value_type &bLow,
			   value_type &high, value_type &dHigh, result_type &fHigh,// value_type &bHigh,
			   const value_type &x, const value_type &dx, const result_type &fx)
	{
		typedef typename value_type::value_type VT;
		std::size_t n = 0;
		for (std::size_t i = 0; i < low.size(); ++i)
		{
			if (OptimizationUpdater< VT >::
			    update(low[i], dLow[i], fLow,// bLow[i],
				   high[i], dHigh[i], fHigh,// bHigh[i],
				   x[i], dx[i], fx))
				++n;
		}

		// Update considered successful if at least one parameter was updated
		return n > 0;
	}
};

template <class T>
struct OptimizationUpdater
{
	template <class value_type, class result_type>
	static bool update(value_type &low,  value_type &dLow,  result_type &fLow,//  value_type &bLow,
			   value_type &high, value_type &dHigh, result_type &fHigh,// value_type &bHigh,
			   const value_type &x, const value_type &dx, const result_type &fx)
	{
		return OptimizationUpdater_<	std::tr1::is_arithmetic< value_type>::value,
						std::tr1::is_arithmetic<result_type>::value >::
			    update(low, dLow, fLow,/* bLow,*/ high, dHigh, fHigh,/* bHigh,*/ x, dx, fx);
	}
};

} // end namespace detail

template <class Function, class Derivative>
typename Function::value_type optimize(const Function &f, const Derivative &d,
		typename Function::value_type low,
		typename Function::value_type high,
		const std::size_t N)
{
	typedef typename Function  :: value_type  value_type;
	typedef typename Derivative::result_type result_type;

	// Calculate derivatives in end points
	value_type dLow  = d(low);  if (dLow  == 0) return low;
	value_type dHigh = d(high); if (dHigh == 0) return high;

	value_type x = (low + high) / value_type(2);
	assert(dLow * dHigh < 0); // need opposing signs
	if (!(dLow * dHigh < 0)) // Invalid input, actually
		return x;

	result_type fLow  = f(low);
	result_type fHigh = f(high);

//	value_type bLow  = fLow  - dLow  * low;
//	value_type bHigh = fHigh - dHigh * high;
/*	std::cout << "INIT" << std::endl;
	std::cout << "low: " << low << std::endl;
	std::cout << "dLow: " << dLow << std::endl;
	std::cout << "fLow: " << fLow << std::endl;
//	std::cout << "bLow: " << bLow << std::endl;
	std::cout << "high: " << high << std::endl;
	std::cout << "dHigh: " << dHigh << std::endl;
	std::cout << "fHigh: " << fHigh << std::endl;
//	std::cout << "bHigh: " << bHigh << std::endl;
 	std::cout << std::endl;
*/
	for (std::size_t i = 0; i < N; ++i)
	{
		assert(dLow * dHigh <= 0); // need opposing signs

		// find two lines y = d*x + b

		// find intersection point of derivatives as
		// estimation of center, find derivative at center
		//x = (bHigh - bLow) / (dLow - dHigh);
		x = (low + high) / value_type(2);
		assert(low <= x);
		assert(x <= high);
		const result_type fx = f(x);
		const  value_type dx = d(x);
/*		std::cout << "x: " << x << std::endl;
		std::cout << "fx: " << fx << std::endl;
		std::cout << "dx: " << dx << std::endl;
*/
		if (!detail::OptimizationUpdater< value_type >::
		    update(low, dLow, fLow,/* bLow,*/ high, dHigh, fHigh,/* bHigh,*/ x, dx, fx))
		{
			// No parameters updated, derivative zero --> optimum found
			break;
		}
/*		std::cout << "low: " << low << std::endl;
		std::cout << "dLow: " << dLow << std::endl;
		std::cout << "fLow: " << fLow << std::endl;
//		std::cout << "bLow: " << bLow << std::endl;
		std::cout << "high: " << high << std::endl;
		std::cout << "dHigh: " << dHigh << std::endl;
		std::cout << "fHigh: " << fHigh << std::endl;
//		std::cout << "bHigh: " << bHigh << std::endl;
		std::cout << std::endl;
*/
	}
	return x;
}

template <class Function>
typename Function::value_type optimize(const Function &f,
			typename Function::value_type low,
			typename Function::value_type high,
			const std::size_t N)
{
	return optimize(f, Derivative<Function>(f), low, high, N);
}

/*
 * Newton-Raphson
 */

template <class Function, class Derivative>
bool doNewtonRaphson(const Function &f, const Derivative &d,
			typename Function::value_type &x, const std::size_t N)
{
	typedef typename Function::value_type value_type;
	typedef typename ValueType<value_type>::value_type T;
	CyclicBuffer<T> values((16u));

	value_type dx = value_type(T(1));
	T m = T(0);
	value_type fx = f(x);

	value_type oldx [2] = {x, x};
	value_type avgx = x;

	value_type best = x;
	value_type lowest = fx;

	// Start loop, terminate on convergence or hopelessness
	for (std::size_t i = 0u; i < N; ++i)
	{
// std::cout << i << " X: " << x << " f(x): " << fx << " df(x): " << d(x)
// 	<< " dx: " << dx << " avg: " << values.avg() << std::endl;

		if (modulus(fx) == T(0))
			return true;

		// The method 2nd-order, i.e. error should be quadratic. Hence,
		// the updates to x should reduce at 2nd order as well, and thus
		// allways be smaller than previous updates, at least on
		// average.
		// Convergence is reached when the updates no longer become
		// smaller and smaller. To have a meaningful average value,
		// run at least "values.capacity()" times.
		if ( (i > values.capacity()) && (values.avg() <= m) )
			break;

		values.add(m);			// Add metric _after_ comparison

		// Compute standard Newton-Raphson, but avoid divide-by-zero.
		value_type next = x - dx; 	// if df==0, repeat previous
		const value_type df = d(x);	// Compute derivative
		if (modulus(df) > T(0))		// Not reliable for Vectors...
			next = x - fx / df;

		// Select best option of standard Newton-Raphson and
		// bisection of previous values.
		value_type fnext = f(next);
		const value_type favgx = f(avgx);
		if (modulus(favgx) < modulus(fnext))
		{
			next = avgx;
			fnext= favgx;
		}

		dx   = next - x;		// Displacement
		avgx = (oldx[0] + oldx[1]) / T(2); // bisection of previous
		oldx[i%2u] = x;
		x    = next;
		fx   = fnext;
		m    = modulus(dx);		// Quality metric = abs(update)

		// Select best result.
		if (modulus(fx) < modulus(lowest))
		{
			best   = x;
			lowest = fx;
		}
	}
	x = best;

	return (values.avg() <= m); // Converged or ran out of iterations ?
}

template <class Function>
bool doNewtonRaphson(const Function &f, typename Function::value_type &x,
			const std::size_t N)
{
	return doNewtonRaphson(f, Derivative<Function>(f), x, N);
}

template <class Function, class Derivative>
bool doNewtonRaphson(const Function &f, const Derivative &d,
			typename Function::value_type &x,
			const typename Function::value_type low,
			const typename Function::value_type high,
			const std::size_t N)
{
	typedef typename Function::value_type value_type;
	typedef typename ValueType<value_type>::value_type T;

	// Make sure x1 < x2
	value_type xl = std::min(low, high);
	value_type xh = std::max(low, high);

	if ( (xl > x) || (x > xh) )
		x = (xl + xh) / T(2);

	CyclicBuffer<T> values((16u));

	T m = T(0);
	value_type dx	= T(0);
	value_type fx	= f(x);
	value_type fl	= f(xl);
	value_type fh	= f(xh);

	value_type oldx [2] = {x, x};
	value_type avgx = x;

	value_type best = x;
	value_type lowest = fx;

	// Start loop, terminate on convergence or hopelessness
	for (std::size_t i = 0u; i < N; ++i)
	{
// std::cout << i << " X: " << x << " f(x): " << fx << " df(x): " << d(x)
// 	<< " dx: " << dx << " avg: " << values.avg()
// 	<< " [" << xl << ", " << xh << "]"<< std::endl;

		if (modulus(fx) == T(0)) // Jackpot ?
			return true;

		// The method 2nd-order, i.e. error should be quadratic. Hence,
		// the updates to x should reduce at 2nd order as well, and thus
		// allways be smaller than previous updates, at least on
		// average.
		// Convergence is reached when the updates no longer become
		// smaller and smaller. To have a meaningful average value,
		// run at least "values.capacity()" times.
		if ( (i > values.capacity()) && (values.avg() <= m) )
			break;

		values.add(m);		// Add metric _after_ comparison

		T next = x + dx;
		const value_type df = d(x);

		if (modulus(df) > T(0)) // Not reliable for Vectors...
			next = x - fx / df;
		next = clamp(next, xl, xh);

		// Select an alternative option as best of
		// bi-section of range and bisection of previous values.
		value_type fnext = f(next);
		value_type alternative  = (xh + xl) / T(2); // bisect of range
		value_type falternative = f(alternative);

// std::cout << "\t next: " << next << " middle: " << alternative << " avgx " <<
// 	avgx << " fnext "<< fnext << " falternative " << falternative <<
// 	" favg " << favgx << std::endl;

		const value_type favgx = f(avgx); // bisect of previous values
		if (modulus(favgx) < modulus(falternative))
		{
			alternative = avgx;
			falternative= favgx;
		}

		// Select best option of standard Newton-Raphson and alternative
		if ( !((xl <= next) && (next <= xh)) ||
		      (modulus(falternative) < modulus(fnext)) )
		{
			next = alternative;
			fnext= falternative;
		}

		dx	= next - x;		// Displacement
		m	= modulus(dx);		// Quality metric = abs(update)
		avgx = (oldx[0] + oldx[1]) / T(2); // bisection of previous
		oldx[i%2u] = x;
		x	= next;			// Update position
		fx	= fnext;		// Update function value

		// Try to reduce the range such that the function has one
		// end-point below zero and one above zero.
		if (fl * fx < T(0))
		{
			xh = x;
			fh = fx;
		}
		if (fh * fx < T(0))
		{
			xl = x;
			fl = fx;
		}

		// Select the best option
		if (modulus(fx) < modulus(lowest))
		{
			best   = x;
			lowest = fx;
		}
	}
	x = best;

	return (values.avg() <= m);  // Converged or ran out of iterations ?
}

template <class Function>
bool doNewtonRaphson(const Function &f, typename Function::value_type &x,
			const typename Function::value_type low,
			const typename Function::value_type high,
			const std::size_t N)
{
	return doNewtonRaphson(f, Derivative<Function>(f),
				x, low, high, N);
}

template <class Function>
void doRungeKutta(const Function &f,
		const typename Function::value_type t0,
		const typename Function::value_type tN,
		const std::size_t N,
		const typename Function::value_type y0,
		std::vector<typename Function::value_type> &y)
{
	assert(tN > t0); // Bogus user input ?

	typedef typename Function::value_type T;
	assert(std::tr1::is_floating_point<T>::value); // Float types only.

	y.clear();
	y.reserve(N+1u); // Ensure 1 memory alloc only

	// "physical" spacing between two nodes
	const T h = (tN - t0) / static_cast<T>(N);

	assert(T(0.5) * h > T(0.)); // Numerically still stable ?

	y.push_back(y0);          // initial condition
        for (std::size_t i = 0u; i < N; ++i)
        {
		assert(i == y.size() - 1u);

		const T t_n	= t0 + static_cast<T>(i+1u) * h;
		const T halfH	= T(0.5) * h;

		const T k1 = f(t_n,           y[i]);
		const T k2 = f(t_n + halfH,   y[i] + halfH * k1);
		const T k3 = f(t_n + halfH,   y[i] + halfH * k2);
		const T k4 = f(t_n + h,       y[i] + h * k3);

		y.push_back(y[i] + h / T(6) * (k1 + T(2)*k2 + T(2)*k3 + k4));
        }
	assert(y.size() == N+1u);
}

/*
template <template <typename Tm, std::size_t D, typename Aux> class Array_t,
	  typename T, typename A>
void identity_matrix(Array_t<T, 2u, A> &m, const std::size_t N)
{
	typedef array_traits<Array_t, T, 2u, A>	AT;

	const std::size_t sz [] = {N, N};
	AT::resize(m, sz);
	omptl::fill(m.begin(), m.end(), T(0));
#ifdef _OPENMP
	#pragma omp parallel for
#endif
	for (int i = 0; i < int(N); ++i)
		m[i][i] = T(1);
}
*/

/*
 * Adapated from:
 * http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
 *
 * Matrix inversion routine.
 * Uses lu_factorize and lu_substitute in uBLAS to invert a matrix
 */

template<class T, class F, class A>
bool invert(boost::numeric::ublas::matrix<T, F, A>& m)
{
 	namespace ublas = boost::numeric::ublas;

 	// create a working copy of the input
 	ublas::matrix<T> a(m);

 	// create a permutation matrix for the LU-factorization
 	ublas::permutation_matrix<std::size_t> pm(a.size1());

 	// perform LU-factorization
 	if (ublas::lu_factorize(a, pm)) // returns zero on success
		return false;

	// create identity_matrix matrix of "inverse"
 	m.assign(ublas::identity_matrix<T>(a.size1()));

 	// backsubstitute to get the inverse
 	ublas::lu_substitute(a, pm, m);

 	return true;
}
/*
template <template <typename Tm, std::size_t D, typename Aux> class Array_t,
	  typename T, typename A>
bool invert(Array_t<T, 2u, A> &m)
{
	typedef Array_t<T, 2u, A> Matrix_t;
	typedef array_traits<Array_t, T, 2u, A>	AT;

	// Must be NxN matrix
	if (AT::shape(m)[Y] != AT::shape(m)[X])
		return false;

	const std::size_t dim = AT::shape(m)[X];
	if (dim == 0)
		return true;

	// Load identity_matrix
	Matrix_t inv;
	cvmlcpp::identity_matrix(inv, dim);
	assert(AT::shape(inv)[X] == dim);
	assert(AT::shape(inv)[Y] == dim);

	for (std::size_t x = 0; x < dim; ++x)
	{
		// Scale
		if (m[x][x] == T(0))
			return false;

		const T scale = T(1) / m[x][x];
		m[x][x] = T(1);

#ifdef _OPENMP
		#pragma omp parallel for
#endif
		for (int y = x+1; y < int(dim); ++y)
			m[x][y]   *= scale;
#ifdef _OPENMP
		#pragma omp parallel for
#endif
		for (int y = 0; y < int(dim); ++y)
			inv[x][y] *= scale;

		// Sweep
#ifdef _OPENMP
		#pragma omp parallel for
#endif
		for (int xx = x+1; xx < int(dim); ++xx)
		{
			const T factor = m[xx][x];
			if (factor == T(0))
				continue;

			m[xx][x] = T(0);
			for (std::size_t yy = x+1; yy < dim; ++yy)
			{
				assert(x >= 0);
				assert(xx>= 0);
				assert(yy >= 0);
				assert(x < dim);
				assert(std::size_t(xx)< dim);
				assert(yy < dim);
				  m[xx][yy] -= factor *   m[x][yy];
			}
			for (std::size_t yy = 0;   yy < dim; ++yy)
			{
				assert(x >= 0);
				assert(xx>= 0);
				assert(yy >= 0);
				assert(x < dim);
				assert(std::size_t(xx)< dim);
				assert(yy < dim);
				inv[xx][yy] -= factor * inv[x][yy];
			}
		}
	}

	assert(dim > 0);
	for (long int x = dim - 1; x >= 0; --x)
	{
		// Sweep
#ifdef _OPENMP
		#pragma omp parallel for
#endif
		for (int xx = x-1; xx >= 0; --xx)
		{
			const T factor = m[xx][x];
			if (factor == T(0))
				continue;

			m[xx][x] = T(0);
			for (std::size_t y = x+1; y < dim; ++y)
			{
				assert(x >= 0);
				assert(xx>= 0);
				assert(y >= 0);
				assert(x < D);
				assert(xx< D);
				assert(y < D);
				m[xx][y] -= factor *   m[x][y];
			}
			for (std::size_t y = 0;   y < dim; ++y)
			{
				assert(x >= 0);
				assert(xx>= 0);
				assert(y >= 0);
				assert(x < D);
				assert(xx< D);
				assert(y < D);
				inv[xx][y] -= factor * inv[x][y];
			}
		}
	}

	m.swap(inv);

	return true;
}
*/
template <template <typename Tm, std::size_t D, typename Aux> class Array_t,
	  typename T, typename A>
void transpose(Array_t<T, 2u, A> &m)
{
	typedef array_traits<Array_t, T, 2u, A>	AT;
	const std::size_t ext [] = {AT::shape(m)[Y], AT::shape(m)[X]};

	if ( (AT::size(m) == 0) || (ext[X] == 0) || (ext[Y] == 0) )
		return;

	std::vector<bool> todo(AT::size(m), true);
	todo[0] = false;

	typename std::vector<bool>::const_iterator next;
	T temp = *AT::begin(m);
	while ( (next=std::find(todo.begin(), todo.end(), true)) != todo.end())
	{
		using std::swap;
		std::size_t src  = next - todo.begin();

		std::size_t x1 = src / AT::shape(m)[Y];
		std::size_t y1 = src % AT::shape(m)[Y];
		swap(x1, y1);
		std::size_t dest = x1 * ext[Y] + y1;
		temp = *(AT::begin(m) + src);
		if (src == dest)
			todo[dest] = false;
		else
		{
			while (  todo[dest] )
			{
				typename AT::iterator destIt = AT::begin(m);
				std::advance(destIt, dest);

				swap(temp, *destIt);
				todo[dest] = false;
				src = dest;
				std::size_t x = src / AT::shape(m)[Y];
				std::size_t y = src % AT::shape(m)[Y];
				swap(x, y);
				dest = x * ext[Y] + y;
			}
		}
	}

	AT::resize(m, ext);

// 	for (std::size_t i = 0u; i < ext[1]; ++i)
// 	for (std::size_t j = 0u; j < ext[0]; ++j)
// 		r[i][j] = static_cast<T>(m[j][i]);
// 	r.swap(m);
}

namespace detail
{

template <template <typename Tm, std::size_t Dm, typename A> class Array_t,
	  typename Ta, typename Aux, class XVector_t, class YVector_t>
bool leastSquaresFit_ublas(const Array_t<Ta, 2, Aux> &A, const YVector_t &y,
		     XVector_t &x)
{
	// Copy to ublas matrix: A
	boost::numeric::ublas::matrix<Ta> A2 (A.extents()[X], A.extents()[Y]);
	for (std::size_t i = 0; i < A2.size1 (); ++i)
	for (std::size_t j = 0; j < A2.size2 (); ++j)
		A2(i, j) = A[i][j];

	// transposeA A^T
	const boost::numeric::ublas::matrix<Ta> At =
		boost::numeric::ublas::trans(A2);

	// Multiply, before invert: A^T A
	boost::numeric::ublas::matrix<Ta> AtAinv = //At * A2;
		boost::numeric::ublas::prod(At, A2);

/*
	std::cout << "AtA" << std::endl;
	for (std::size_t i = 0u; i < AtAinv.size1(); ++i)
	{
		for (std::size_t j = 0u; j < AtAinv.size2(); ++j)
	    		std::cout << AtAinv(i,j) << " ";
		std::cout << std::endl;
	}
	std::cout << std::endl;
*/

	// Invert: (A^T A)^{-1}
	if (!invert(AtAinv))
		return false;

/*
	std::cout << "AtAinv" << std::endl;
	for (std::size_t i = 0u; i < AtAinv.size1(); ++i)
	{
		for (std::size_t j = 0u; j < AtAinv.size2(); ++j)
	    		std::cout << AtAinv(i,j) << " ";
		std::cout << std::endl;
	}
	std::cout << std::endl;
*/

	// Copy observation vector
	boost::numeric::ublas::vector<Ta> y2 (y.size());
	for (std::size_t i = 0; i < y2.size (); ++ i)
		y2(i) = y[i];

	// x = (A^T A)^{-1} * At * y2;
	const boost::numeric::ublas::matrix<Ta> AtAinvAt  =
		boost::numeric::ublas::prod(AtAinv, At);
	const boost::numeric::ublas::vector<Ta> x2 =
		boost::numeric::ublas::prod(AtAinvAt, y2);

	assert(x.size() == x2.size());
	for (std::size_t i = 0; i < x.size (); ++ i)
		x[i] = x2(i);
}

template <template <typename Tm, std::size_t Dm, typename A> class Array_t,
	  typename Ta, typename Aux, class XVector_t, class YVector_t>
bool leastSquaresFit_cvmlcpp(const Array_t<Ta, 2, Aux> &A, const YVector_t &y,
		     XVector_t &x)
{
	typedef array_traits<Array_t, Ta, 2u, Aux> AT;
/*
	std::cout << "A" << std::endl;
	for (std::size_t i = 0u; i < A.extents()[X]; ++i)
	{
		for (std::size_t j = 0u; j < A.extents()[Y]; ++j)
	    		std::cout << A[i][j] << " ";
		std::cout << std::endl;
	}
	std::cout << std::endl;
*/
	Array_t<Ta, 2, Aux> At = AT::copy_of(A);
	transpose(At);

	Array_t<Ta, 2, Aux> AtAinv; // = At * A;
	mat_mat_mult(At, A, AtAinv);
/*
	std::cout << "AtA" << std::endl;
	for (std::size_t i = 0u; i < AtAinv.extents()[X]; ++i)
	{
		for (std::size_t j = 0u; j < AtAinv.extents()[Y]; ++j)
	    		std::cout << AtAinv[i][j] << " ";
		std::cout << std::endl;
	}
	std::cout << std::endl;
*/
	if (!invert(AtAinv))
		return false;
/*
	std::cout << "AtAinv" << std::endl;
	for (std::size_t i = 0u; i < AtAinv.extents()[X]; ++i)
	{
		for (std::size_t j = 0u; j < AtAinv.extents()[Y]; ++j)
	    		std::cout << AtAinv[i][j] << " ";
		std::cout << std::endl;
	}
	std::cout << std::endl;
*/

	Array_t<Ta, 2, Aux> AtAinvAt;
	mat_mat_mult(AtAinv, At, AtAinvAt);
/*	std::cout << "B" << std::endl;
	for (std::size_t i = 0u; i < B.extents()[X]; ++i)
	{
		for (std::size_t j = 0u; j < B.extents()[Y]; ++j)
	    		std::cout << B[i][j] << " ";
		std::cout << std::endl;
	}
	std::cout << std::endl;

	std::cout << "Y" << std::endl;
	for (std::size_t i = 0u; i < y.size(); ++i)
		std::cout << y[i] << " ";
	std::cout << std::endl;
*/
	mat_vec_mult(AtAinvAt, y, x);

	return true;
}

} // end namespace detail

template <template <typename Tm, std::size_t Dm, typename A> class Array_t,
	  typename Ta, typename Aux, class XVector_t, class YVector_t>
bool leastSquaresFit(const Array_t<Ta, 2, Aux> &A, const YVector_t &y,
		     XVector_t &x)
{
	//return detail::leastSquaresFit_ublas(A, y, x);
	return detail::leastSquaresFit_cvmlcpp(A, y, x);
}

/*
 * The bits about the hungarian matching algorithm WAS part of
 * "the Stanford GraphBase (c) Stanford University 1993"
 * In accordance to the Stanford GraphBase license, this notice is here
 * to inform you that it is NO LONGER part of that library, and this file
 * has a different name than the one the original code was in, because the
 * code has been modified.
 */

namespace detail
{

// Subtract column minima in order to start with lots of zeroes
template <template <typename Tm, std::size_t D, typename Aux> class Matrix_type,
	typename T, typename A>
void to_zero(Matrix_type<T, 2, A> &aa)
{
	typedef array_traits<Matrix_type, T, 2, A> AT;
	const std::size_t m = AT::shape(aa)[0];
	const std::size_t n = AT::shape(aa)[1];

	#ifdef _OPENMP
	#pragma omp parallel for
	#endif
	for (int l = 0; l < int(n); ++l)
	{
		T s = aa[0][l];
		for (std::size_t k = 1; k < m; ++k)
			s = std::min(aa[k][l], s);
		if (s != 0)
			for (std::size_t k = 0; k < m; ++k)
				aa[k][l] -= s;
	}
}

template <template <typename Tm, std::size_t D, typename Aux> class Matrix_type,
	typename T, typename A>
void hungarian( const Matrix_type<T, 2, A> &aa,
		std::vector<std::ptrdiff_t> &col_mate,
		std::vector<std::ptrdiff_t> &row_mate)
{
//	printf("1\n");
	typedef array_traits<Matrix_type, T, 2, A> AT;

	/* number of rows and columns desired */
	const std::size_t m = AT::shape(aa)[0];
	const std::size_t n = AT::shape(aa)[1];

	col_mate.resize(m);
	row_mate.resize(n);
	std::vector<std::ptrdiff_t> parent_row(n);
	std::vector<std::ptrdiff_t> unchosen_row(m);
	std::vector<T> row_dec(m);
	std::vector<T> col_inc(n);
	std::vector<T> slack(n);
	std::vector<std::ptrdiff_t> slack_row(n);

	std::fill(row_mate  .begin(), row_mate  .end(), -1);
	std::fill(col_inc   .begin(), col_inc   .end(),  0);
	std::fill(parent_row.begin(), parent_row.end(), -1);
	std::fill(slack     .begin(), slack     .end(), std::numeric_limits<T>::max());
//	printf("2\n");

	/*
	 * The algorithm operates in stages, where each stage terminates
	 * when we are able to increase the number of matched elements.
	 *
	 * The first stage is different from the others; it simply goes through
	 * the matrix and looks for zeroes, matching as many rows and columns
	 * as it can. This stage also initializes table entries that will be
	 * useful in later stages.
	 */

	// total number of nodes in the forest
	std::size_t t = 0; /* the forest starts out empty */

	for (std::size_t k = 0; k < m; ++k)
	{
		// the minimum entry of row $k$
		const T s = *std::min_element(aa[k].begin(), aa[k].end());

		row_dec[k] = s;
		for (std::size_t l = 0; l < n; ++l)
			if ( (s==aa[k][l]) && (row_mate[l]<0))
			{
				col_mate[k]=l;
				row_mate[l]=k;
				goto row_done;
			}
		col_mate[k]=-1; // k unmatched
		unchosen_row[t++]=k;
//		printf("  node %ld: unmatched row %ld\n",t,k);
		row_done:;
		assert(t <= unchosen_row.size());
	}
//	printf("3\n");

	if (t==0)
		return;
	std::size_t unmatched = t;
	while(unmatched > 0)
	{
		std::ptrdiff_t k; /* the current row of interest */
		std::ptrdiff_t l; /* the current column of interest */
		/* the current matrix element of interest */

		T s;
//		printf("4\n");

//		std::size_t q = 0;
		while (1)
		{
			for (std::size_t q = 0; q < t; ++q)
			{
//				printf("5\n");
				// Explore node |q| of the forest;
				// if the matching can be increased, |goto breakthru|
				assert(q < m);
				k = unchosen_row[q];
//				const T s = row_dec[k];
				s = row_dec[k];
				for (l = 0; l < std::ptrdiff_t(n); ++l)
				{
//					printf("6\n");
					if (slack[l])
					{
						const T del = aa[k][l] - s + col_inc[l];
						if (del < slack[l])
						{
							if (del==0)
							{ /* we found a new zero */
								if (row_mate[l]<0)
									goto breakthru;
								slack[l] = 0; /* this column will now be chosen */
								parent_row[l] = k;
//								printf("  node %ld: row %ld==col %ld--row %ld\n", t,row_mate[l],l,k);
								assert(t < m);
								unchosen_row[t++] = row_mate[l];
							}
							else
							{
								slack[l] = del;
								slack_row[l] = k;
							}
						}
					}
				}
			}

//			fprintf(stderr, "7\n");

			// Introduce a new zero into the matrix by modifying |row_dec| and
			// |col_inc|; if the matching can be increased, |goto breakthru|
			/*T*/ s = std::numeric_limits<T>::max();
			for (std::size_t i = 0; i < n; ++i)
				if (slack[i] != 0)
					s = std::min(slack[i], s);
			assert(t <= unchosen_row.size());
			for (std::size_t q = 0; q < t; ++q) {
//				fprintf(stderr, "unchosen_row[%ld] = %ld\n", q, unchosen_row[q]);
				assert(unchosen_row[q] >= 0);
				assert(unchosen_row[q] < std::ptrdiff_t(row_dec.size()));
				row_dec[unchosen_row[q]] += s;
			}
			for (/*std::size_t*/ l = 0; l < std::ptrdiff_t(n); ++l)
			{
//				fprintf(stderr, "8\n");
				if (slack[l])
				{
					// column $l$ is not chosen
					slack[l] -= s;
					if (slack[l]==0)
					{
						// Look at a new zero, and |goto breakthru| with
						// |col_inc| up to date if there's a breakthrough
						k = slack_row[l];
//						fprintf(stderr, " Decreasing uncovered elements by %ld produces zero at [%ld,%ld]\n", s,k,l);
						if (row_mate[l]<0)
						{
							for (std::size_t j=l+1; j<n; ++j)
								if (slack[j] == 0)
									col_inc[j] += s;
							goto breakthru;
						}
						else
						{
							// not a breakthrough, but the forest continues to grow
							parent_row[l]=k;
//							fprintf(stderr, "  node %ld: row %ld==col %ld--row %ld\n", t,row_mate[l],l,k);
							unchosen_row[t++]=row_mate[l];
						}
					}
				}
				else
					col_inc[l]+=s;
			}
		}

		breakthru:
//		fprintf(stderr, "9\n");
		// Update the matching by pairing row $k$ with column $l$
		while (1)
		{
//			fprintf(stderr, "10\n");
			const std::ptrdiff_t j = col_mate[k];
			col_mate[k]=l;
			row_mate[l]=k;
//			fprintf(stderr, " rematching col %ld==row %ld\n",l,k);
			if (j < 0)
				break;
			k = parent_row[j];
			l = j;
		}

//		fprintf(stderr, "11\n");
		assert(unmatched > 0);
		if(--unmatched == 0)
			break;
		assert(unmatched > 0);

//		fprintf(stderr, "12\n");

		/*
		 *  get_ready_for_another_stage
		 */
		std::fill(parent_row.begin(), parent_row.end(), -1);
		std::fill(slack     .begin(), slack     .end(), std::numeric_limits<T>::max());
		t = 0;
		for (std::size_t i = 0; i < m; ++i)
			if (col_mate[i] < 0)
			{
				unchosen_row[t++] = i;
//				fprintf(stderr, "  node %ld: unmatched row %ld\n",t,k);
			}
	}
//	fprintf(stderr, "done!\n");
}

} // end namespace detail

/*
 * Interfacing.
 * A few nitty-gritty details still need to be handled: Our algorithm
 * is not symmetric between rows and columns, and it works only for $m\le n$;
 * so we will transpose the matrix when $m>n$. Furthermore, our
 * algorithm minimizes, but we actually want it to maximize (except
 * when |compl| is nonzero).
 *
 * Hence, we want to make the following transformations to the data
 * before processing it with the algorithm developed above.
 */
/*
template <template <typename Tm, std::size_t D, typename Aux> class Matrix_type,
	typename T, typename A>
void find_matching(Matrix_type<T, 2, A> &costs,
		   std::vector<std::pair<std::size_t, std::size_t> > &matches,
		   const bool minimize_costs, const bool heuristic)
{
	typedef array_traits<Matrix_type, T, 2, A> AT;

	matches.clear();
	std::vector<std::ptrdiff_t> a_mate, b_mate;

	const std::size_t m = AT::shape(costs)[0];
	const std::size_t n = AT::shape(costs)[1];

	const bool transposed = m > n;
	if (transposed)
		transpose(costs);

	if (!minimize_costs)
	{
		const T max_cost = *omptl::max_element(costs.begin(), costs.end());
		omptl::transform(costs.begin(), costs.end(), costs.begin(),
					std::bind1st(std::minus<T>(), max_cost));
	}

	if ( heuristic && (m == n) )
		detail::to_zero(costs);
//fprintf(stderr, "GO!\n");
	detail::hungarian(costs, a_mate, b_mate);

	if (transposed)
		a_mate.swap(b_mate);

	assert(a_mate.size() <= b_mate.size());
	for (std::size_t a = 0; a < a_mate.size(); ++a)
		if (a_mate[a] >= 0)
		{
			// Check reprociatability
			assert(b_mate[a_mate[a]] == std::ptrdiff_t(a));
			matches.push_back( std::make_pair(a, a_mate[a]) );
		}
}
*/
} // end namespace cvmlcpp


/*=============================================================================
    Copyright (c) 2002-2003 Joel de Guzman
    http://spirit.sourceforge.net/

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A parser for a real number parser that parses thousands separated numbers
//  with at most two decimal places and no exponent. This is discussed in the
//  "Numerics" chapter in the Spirit User's Guide.
//
//  [ JDG 12/16/2003 ]
//
///////////////////////////////////////////////////////////////////////////////
#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_assign_actor.hpp>
#include <iostream>
#include <string>

///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace BOOST_SPIRIT_CLASSIC_NS;

template <typename T>
struct ts_real_parser_policies : public ureal_parser_policies<T>
{
    //  These policies can be used to parse thousand separated
    //  numbers with at most 2 decimal digits after the decimal
    //  point. e.g. 123,456,789.01

    typedef uint_parser<int, 10, 1, 2>  uint2_t;
    typedef uint_parser<T, 10, 1, -1>   uint_parser_t;
    typedef int_parser<int, 10, 1, -1>  int_parser_t;

    //////////////////////////////////  2 decimal places Max
    template <typename ScannerT>
    static typename parser_result<uint2_t, ScannerT>::type
    parse_frac_n(ScannerT& scan)
    { return uint2_t().parse(scan); }

    //////////////////////////////////  No exponent
    template <typename ScannerT>
    static typename parser_result<chlit<>, ScannerT>::type
    parse_exp(ScannerT& scan)
    { return scan.no_match(); }

    //////////////////////////////////  No exponent
    template <typename ScannerT>
    static typename parser_result<int_parser_t, ScannerT>::type
    parse_exp_n(ScannerT& scan)
    { return scan.no_match(); }

    //////////////////////////////////  Thousands separated numbers
    template <typename ScannerT>
    static typename parser_result<uint_parser_t, ScannerT>::type
    parse_n(ScannerT& scan)
    {
        typedef typename parser_result<uint_parser_t, ScannerT>::type RT;
        static uint_parser<unsigned, 10, 1, 3> uint3_p;
        static uint_parser<unsigned, 10, 3, 3> uint3_3_p;
        if (RT hit = uint3_p.parse(scan))
        {
            T n;
            typedef typename ScannerT::iterator_t iterator_t;
            iterator_t save = scan.first;
            while (match<> next = (',' >> uint3_3_p[assign_a(n)]).parse(scan))
            {
                hit.value((hit.value() * 1000) + n);
                scan.concat_match(hit, next);
                save = scan.first;
            }
            scan.first = save;
            return hit;

            // Note: On erroneous input such as "123,45", the result should
            // be a partial match "123". 'save' is used to makes sure that
            // the scanner position is placed at the last *valid* parse
            // position.
        }
        return scan.no_match();
    }
};

real_parser<double, ts_real_parser_policies<double> > const
    ts_real_p = real_parser<double, ts_real_parser_policies<double> >();

////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tA real number parser that parses thousands separated\n";
    cout << "\t\tnumbers with at most two decimal places and no exponent...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";

    cout << "Give me a number.\n";
    cout << "Type [q or Q] to quit\n\n";

    string str;
    double n;
    while (getline(cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        if (parse(str.c_str(), ts_real_p[assign_a(n)]).full)
        {
            cout << "-------------------------\n";
            cout << "Parsing succeeded\n";
            cout << str << " Parses OK: " << endl;
            cout << "n=" << n << endl;
            cout << "-------------------------\n";
        }
        else
        {
            cout << "-------------------------\n";
            cout << "Parsing failed\n";
            cout << "-------------------------\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}