#ifndef _jsc_bioinfo_sig_hpp_included_
#define _jsc_bioinfo_sig_hpp_included_

#include <boost/config.hpp>

#include <math.h>

#include <iostream>
#include <map>
#include <set>
#include <sstream>
#include <vector>

#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/shared_ptr.hpp>

#include "jsc/util/interval_list.hpp"

using namespace std;
using namespace boost;
using namespace boost::lambda;

using namespace jsc::util;

namespace jsc
{

namespace bioinfo
{

//! Defines the format of a simple sig entry, without strand information.
class sig_entry
{
public:
	string chrName;
	long start;
	long end;
	double sig;
};

//! A shared_ptr to sig_entry.
/*!
 * \sa str2sigp().
 */
typedef shared_ptr<sig_entry> sig_ptr;
//! A vector of jsc::bioinfo::sig_ptr.
/*!
 * \sa load_sigps_noheader().
 */
typedef vector<sig_ptr> vec_sigp;
//! A map of (chrName, jsc::util::interval_list).
/*!
 * \sa get_map_chr_intervals().
 */
typedef map<string, interval_list<long> > map_chr_intervals;

double generalized_log2(double x) {
	if (x > 0) {
		return log2(x + 1);
	} else {
		return -log2(-x+1);
	}
}

//! Converts a string to jsc::bioinfo::sig_ptr
sig_ptr str2sigp(string const & line, bool apply_generalized_log2 = false) {
	sig_ptr sigp(new sig_entry());
	istringstream iss(line);
	iss >> sigp->chrName
		>> sigp->start
		>> sigp->end
		>> sigp->sig;
	if (apply_generalized_log2) {
		sigp->sig = generalized_log2(sigp->sig);
	}

	return sigp;
}

//! Loads a vector of jsc::bioinfo::sig_ptr from an istream (w/o the header).
vec_sigp load_sigps_noheader(istream & is)
{
	string line;

	/* read content */
	vec_sigp sigps;
	while (getline(is, line) && !is.eof())
	{
		sigps.push_back(str2sigp(line));
	}

	return sigps;
}

//! Loads a vector of jsc::bioinfo::sig_ptr from an istream (w/o the header) for a specific chrName.
void load_sigps_noheader(istream & is, vec_sigp & sigps)
{
	string line;

	/* read content */
	while (getline(is, line) && !is.eof())
	{
		sig_ptr sigp = str2sigp(line);
		sigps.push_back(str2sigp(line));
	}
}

//! Loads a vector of jsc::bioinfo::sig_ptr from an istream (w/o the header) for a specific chrName.
vec_sigp load_sigps_noheader(istream & is, string const & chrName)
{
	string line;

	/* read content */
	vec_sigp sigps;
	while (getline(is, line) && !is.eof())
	{
		sig_ptr sigp = str2sigp(line);
		if (sigp->chrName == chrName)
		{
			sigps.push_back(str2sigp(line));
		}
	}

	return sigps;
}

//! Loads a map from chrName to vector of jsc::bioinfo::sig_ptr from an istream (w/o the header)
void load_chr_sigps_map_noheader(istream & is, map<string, vec_sigp> & chr_sigps_map, bool apply_generalized_log2 = false)
{
	string line;

	/* read content */
	while (getline(is, line) && !is.eof())
	{
		sig_ptr sigp = str2sigp(line, apply_generalized_log2);
		chr_sigps_map[sigp->chrName].push_back(sigp);
	}
}

void sort_sigps(vec_sigp & sigps) {
	sort(sigps.begin(), sigps.end(),
			bind(&sig_entry::start, cref(*_1)) < bind(&sig_entry::start, cref(*_2)));
}

//! Returns all intervals whose signal values are greater/no greater than the given threshold.
/*!
 * \param sigps A vector of jsc::bioinfo::sig_ptr.
 * \param threshold The threshold.
 * \param greater_than_threshold A boolean variable specifying whether the criteria is > or <= threshold.
 * \return map_chr_intervals: a map of (chrName, jsc::util::interval_list).
 */
map_chr_intervals get_map_chr_intervals(vec_sigp const & sigps, double threshold, bool greater_than_threshold)
{
	map_chr_intervals cil_map;
	unsigned long i;
	for (i = 0; i < sigps.size(); ++i)
	{
		sig_ptr sigp = sigps[i];
		if ((greater_than_threshold && sigp->sig > threshold) ||
				(!greater_than_threshold && sigp->sig <= threshold))
		{
			cil_map[sigp->chrName].add_interval(sigp->start, sigp->end);
		}
	}

	return cil_map;
}

void get_map_chr_intervals(vec_sigp const & sigps, double threshold, bool greater_than_threshold, map_chr_intervals & cil_map)
{
	unsigned long i;
	for (i = 0; i < sigps.size(); ++i)
	{
		sig_ptr sigp = sigps[i];
		if ((greater_than_threshold && sigp->sig > threshold) ||
				(!greater_than_threshold && sigp->sig <= threshold))
		{
			cil_map[sigp->chrName].add_interval(sigp->start, sigp->end);
		}
	}
}

} /* end of bioinfo */

} /* end of jsc */

#endif


#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

/*
Documentation:
For my test file I have chosen to do the Minimum Gradient function.
This function determines which gradient between each position is the lowest, to check that this function works I have created 10 different tests below checking the functions against certain routes I have created. To work out the gradient between two positions you first need to work out the distance between these two points this can be done using Pythagoras. After the angle is worked out using Trigonometry, the chosen angle will depends on if the route is downhill or uphill. My test will check that the function answers are correct and will try and catch out errors. Throughout the tests majority of them are check_close this is because there could be a rounding difference, I have each case to be 1 +/- of the answer I think this is an appropriate amount.

*/

BOOST_AUTO_TEST_SUITE( Route_minGradient )

const bool isFileName = true;

//TEST 1: No Elevation change
// This test checks that the minGradient is correct for ABCD GPX route where there is no change in elevtion therefore no gradient.
BOOST_AUTO_TEST_CASE( routeABCD )
{
   Route route = Route(LogFiles::GPXRoutesDir + "ABCD.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.minGradient(), 0 );
}

//TEST 2: Constant Downwards Gradient
//This route goes downhill at a constant decrease rate
BOOST_AUTO_TEST_CASE( ConstDownGrad )
{
   Route route = Route(LogFiles::GPXRoutesDir + "ConstDownGrad.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), -84.87, 1 );

}

//TEST 3: Const Upwards Gradient
//The route will go upwards by the same elevation each time
BOOST_AUTO_TEST_CASE( ConstUpGrad )
{
   Route route = Route(LogFiles::GPXRoutesDir + "ConstUpGrad.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), 84.87, 1 );

}

//TEST 4: DiagalnalIncline
//This test will go dialgonlly on the grid testing that the function includes the Pythagorus part of the equation
BOOST_AUTO_TEST_CASE( DiagonalIncline )
{
   Route route = Route(LogFiles::GPXRoutesDir + "DiagonalIncline.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), 82.75, 1 );

}

//TEST 5: DiagalnalDecline
//This test is the same as above however it decreases in elevation at each position
BOOST_AUTO_TEST_CASE( DiagonalDecline )
{
   Route route = Route(LogFiles::GPXRoutesDir + "DiagonalDecline.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), -82.75, 1 );

}

//TEST 6: Only Elevation change = 90/-90
//This test is where the different positions will stay exactly where they are but go higher/lower in altitude ensuring that the function checks if the distance interms of latitude and longatude is equal to 0 beforehand
BOOST_AUTO_TEST_CASE( ElevationChangeOnly )
{
   Route route = Route(LogFiles::GPXRoutesDir + "ElevationChangeOnly.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.minGradient(), 90 );
}

//TEST 7: Unconstant Downward Gradient
//This GPX file has been edited so that the elevation isn't constant. The whole route will be going downwards but at a different rate
BOOST_AUTO_TEST_CASE( UnconstDownGrad )
{
   Route route = Route(LogFiles::GPXRoutesDir + "UnconstantDownGrad.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), -77.34, 1 );

}

//TEST 8: Unconstant Upward Gradient
//This route will go upwards but not at a consistant rate, this elevation will differ
BOOST_AUTO_TEST_CASE( UnconstUpGrad )
{
   Route route = Route(LogFiles::GPXRoutesDir + "UnconstantUpGrad.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minGradient(), 79.82, 1 );

}

//The following tests checks that invalid arguments have been put inplace
//TEST 9: Invalid Input i.e letter
//This GPX file includes letters instead of numbers, the function should first check the inputted data is the right format
BOOST_AUTO_TEST_CASE( InvalidInput )
{
   Route route = Route(LogFiles::GPXRoutesDir + "InvalidInput.gpx", isFileName);
   BOOST_CHECK_THROW( route.minGradient(), std::invalid_argument );
}

//TEST 10: Nothing In Vector
//For this test I have left some places blank this should throw an exception
BOOST_AUTO_TEST_CASE( NoInput )
{
   Route route = Route(LogFiles::GPXRoutesDir + "NoInput.gpx", isFileName);
   BOOST_CHECK_THROW( route.minGradient(), std::invalid_argument );
}

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 BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_COMMON_MAJORITY_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_COMMON_MAJORITY_HPP_INCLUDED
#include <boost/simd/predicates/functions/majority.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         (majority_, tag::cpu_,
                           (A0)(X),
                           ((simd_<arithmetic_<A0>,X>))
                           ((simd_<arithmetic_<A0>,X>))
                           ((simd_<arithmetic_<A0>,X>))
                          )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      result_type aa0 = is_nez(a0);
      result_type aa1 = is_nez(a1);
      result_type aa2 = is_nez(a2);
      result_type r =  logical_or(
               logical_or(
                 logical_and(aa0, aa1),
                 logical_and(aa1, aa2)
               ),
               logical_and(aa2, aa0)
             );
      return r;
    }
  };
} } }
#endif


#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/range/algorithm.hpp>
#include <boost/integer/common_factor_rt.hpp>

using namespace std;

int N, T;

int main() {

    while (cin >> N) {
        int W[N], ret = 100 * 100;
        for (int i = 0; i < N; i++) {
            cin >> W[i];
        }
        for (int i = 0; i < N; i++) {
            int left_sum = 0, right_sum = 0;
            for (int j = 0; j < i; j++) {
                left_sum += W[j];
            }
            for (int j = i; j < N; j++) {
                right_sum += W[j];
            }
            ret = min(ret, abs(left_sum - right_sum));
        }
        cout << ret << endl;
    }

    return 0;
}


/***************************************************************************
* Copyright (c) Johan Mabille, Sylvain Corlay and Wolf Vollprecht          *
* Copyright (c) QuantStack                                                 *
*                                                                          *
* Distributed under the terms of the BSD 3-Clause License.                 *
*                                                                          *
* The full license is in the file LICENSE, distributed with this software. *
****************************************************************************/

#include <benchmark/benchmark.h>

#ifdef HAS_XTENSOR
#include "xtensor/xnoalias.hpp"
#include "xtensor/xio.hpp"
#include "xtensor/xrandom.hpp"
#include "xtensor/xtensor.hpp"
#include "xtensor/xarray.hpp"
#include "xtensor/xstrided_view.hpp"
#include "xtensor/xview.hpp"
#include "xtensor/xdynamic_view.hpp"
#include "xtensor/xadapt.hpp"
#endif

#ifdef HAS_EIGEN
#include <Eigen/Dense>
#include <Eigen/Core>
#endif

#ifdef HAS_BLITZ
#include <blitz/array.h>
#endif

#ifdef HAS_ARMADILLO
#include <armadillo>
#endif

#define RANGE 3, 1000
#define MULTIPLIER 8

#ifdef HAS_XTENSOR
void Add2dView_XTensor(benchmark::State& state)
{
    using namespace xt;

    xtensor<double,2> vA = random::rand<double>({state.range(0), state.range(0)});
    xtensor<double,2> vB = random::rand<double>({state.range(0), state.range(0)});

    auto vAView = xt::view(vA, all(), all());
    auto vBView = xt::view(vB, all(), all());

    for (auto _ : state)
    {
        xtensor<double,2> vRes(vAView + vBView);
        benchmark::DoNotOptimize(vRes.data());
    }
}
BENCHMARK(Add2dView_XTensor)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

#ifdef HAS_EIGEN
void Add2dView_Eigen(benchmark::State& state)
{
    using namespace Eigen;
    MatrixXd vA = MatrixXd::Random(state.range(0), state.range(0));
    MatrixXd vB = MatrixXd::Random(state.range(0), state.range(0));

    auto vAView = vA.topLeftCorner(state.range(0), state.range(0));
    auto vBView = vB.topLeftCorner(state.range(0), state.range(0));

    for (auto _ : state)
    {
        MatrixXd vRes(state.range(0), state.range(0));
        vRes.noalias() = vAView + vBView;
        benchmark::DoNotOptimize(vRes.data());
    }
}
BENCHMARK(Add2dView_Eigen)->RangeMultiplier(MULTIPLIER)->Range(RANGE);

//void Add1dMap_Eigen(benchmark::State& state)
//{
//    using namespace Eigen;
//    MatrixXd vA = VectorXd::Random(state.range(0));
//    MatrixXd vB = VectorXd::Random(state.range(0));
//
//    auto vAView = Map<VectorXd, 0, InnerStride<1>>(vA.data(), vA.size());
//    auto vBView = Map<VectorXd, 0, InnerStride<1>>(vB.data(), vB.size());
//
//    for (auto _ : state)
//    {
//        VectorXd vRes(vAView + vBView);
//        benchmark::DoNotOptimize(vRes.data());
//    }
//}
//BENCHMARK(Add1dMap_Eigen)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

#ifdef HAS_XTENSOR
void Add2dStridedView_XTensor(benchmark::State& state)
{
    using namespace xt;

    xtensor<double, 2> vA = random::rand<double>({state.range(0), state.range(0)});
    xtensor<double, 2> vB = random::rand<double>({state.range(0), state.range(0)});

    auto vAView = xt::strided_view(vA, {all(), all()});
    auto vBView = xt::strided_view(vB, {all(), all()});

    for (auto _ : state)
    {
        xtensor<double, 2> vRes(vAView + vBView);
        benchmark::DoNotOptimize(vRes.data());
    }
}
BENCHMARK(Add2dStridedView_XTensor)->RangeMultiplier(MULTIPLIER)->Range(RANGE);

void Add2dDynamicView_XTensor(benchmark::State& state)
{
    using namespace xt;

    xtensor<double, 2> vA = random::rand<double>({state.range(0), state.range(0)});
    xtensor<double, 2> vB = random::rand<double>({state.range(0), state.range(0)});

    auto vAView = xt::dynamic_view(vA, {all(), all()});
    auto vBView = xt::dynamic_view(vB, {all(), all()});

    for (auto _ : state)
    {
        xtensor<double, 2> vRes(vAView + vBView);
        benchmark::DoNotOptimize(vRes.data());
    }
}
BENCHMARK(Add2dDynamicView_XTensor)->RangeMultiplier(MULTIPLIER)->Range(RANGE);

void Add2dAdapt_XTensor(benchmark::State& state)
{
    using namespace xt;

    xtensor<double, 2> vA = random::rand<double>({state.range(0), state.range(0)});
    xtensor<double, 2> vB = random::rand<double>({state.range(0), state.range(0)});
    std::size_t vSize = static_cast<std::size_t>(state.range(0));
    std::array<std::size_t, 2> vShape = {vSize, vSize};
    auto vAView = xt::adapt(std::move(vA.data()), vShape);
    auto vBView = xt::adapt(std::move(vB.data()), vShape);

    for (auto _ : state)
    {
        xtensor<double, 2> vRes(vAView + vBView);
        benchmark::DoNotOptimize(vRes.data());
    }
}
BENCHMARK(Add2dAdapt_XTensor)->RangeMultiplier(MULTIPLIER)->Range(RANGE);

void Add2dLoop_XTensor(benchmark::State& state)
{
    using namespace xt;

    xtensor<double, 2> vA = random::rand<double>({state.range(0), state.range(0)});
    xtensor<double, 2> vB = random::rand<double>({state.range(0), state.range(0)});
    std::array<std::size_t, 2> vShape = {static_cast<std::size_t>(state.range(0)), static_cast<std::size_t>(state.range(0))};
    for (auto _ : state)
    {
        xtensor<double, 2> vRes(vShape);
        for (std::size_t i = 0; i < vRes.shape()[0]; ++i)
        {
            for (std::size_t j = 0; j < vRes.shape()[1]; ++j)
            {
                vRes(i, j) = vA(i, j) + vB(i, j);
            }
        }
        benchmark::DoNotOptimize(vRes.data());
    }
}
BENCHMARK(Add2dLoop_XTensor)->RangeMultiplier(MULTIPLIER)->Range(RANGE);
#endif

#undef RANGE
#undef MULTIPLIER


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_EVEN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_EVEN_HPP_INCLUDED

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

 /*!

    @ingroup group-predicates
    Function object implementing is_even capabilities

    Returns @ref True or @ref False according x is even or not.

    @par Semantic:

    @code
    auto r = is_even(x);
    @endcode

    is similar to:

    @code
    auto r = to_int(x/2)*2 == x;
    @endcode

    @par Note:

    A floating number is even if it is a  flint
    and divided by two it is still a flint.

    A flint is a 'floating integer' i.e. a floating number
    representing an integer value

    Be conscious that all sufficiently great floating points values are even...

    @see is_odd, is_flint

  **/
  as_logical_t<Value> is_even(Value const& x);
} }
#endif

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

#endif


#include <vector>

#include <aslam/common/entrypoint.h>
#include <Eigen/Core>

#include "aslam/common/descriptor-utils.h"

namespace aslam {
namespace common {
namespace descriptor_utils {

TEST(ViwlsGraph, DescriptorMedianTestSingleDescriptor) {
  DescriptorsType descriptors(48, 1);
  descriptors.setRandom();
  DescriptorType median;

  descriptorMeanRoundedToBinaryValue(descriptors, &median);
  EXPECT_EQ(descriptors, median);
}

TEST(ViwlsGraph, DescriptorMedianTestTwoZeroDescriptors) {
  DescriptorType descriptor0(48, 1);
  DescriptorType descriptor1(48, 1);
  descriptor0.setZero();
  descriptor1.setZero();
  DescriptorType median;

  DescriptorsType descriptors(48, 2);
  descriptors.col(0) = descriptor0;
  descriptors.col(1) = descriptor1;

  descriptorMeanRoundedToBinaryValue(descriptors, &median);
  EXPECT_EQ(descriptor0, median);
}

TEST(ViwlsGraph, DescriptorMedianTestThreeDescriptors) {
  DescriptorsType descriptors(48, 3);
  descriptors.setZero();

  descriptors(0, 0) = 7;
  descriptors(0, 1) = 3;
  descriptors(0, 2) = 1;

  DescriptorType median(48, 1);

  descriptorMeanRoundedToBinaryValue(descriptors, &median);
  // Median of {1, 1, 1}
  EXPECT_TRUE(getBit(0, median));
  // Median of {1, 1, 0}
  EXPECT_TRUE(getBit(1, median));
  // Median of {1, 0, 0}
  EXPECT_FALSE(getBit(2, median));
  // Median of {0, 0, 0}
  EXPECT_FALSE(getBit(3, median));
}

TEST(ViwlsGraph, DescriptorMedianAbsDeviationTest) {
  DescriptorsType descriptors(48, 3);
  descriptors.setZero();

  descriptors(0, 0) = 7;
  descriptors(0, 1) = 3;
  descriptors(0, 2) = 1;

  EXPECT_EQ(descriptorMeanAbsoluteDeviation(descriptors),
            2.0 / 3.0);
}

TEST(ViwlsGraph, DescriptorMeanTestThreeDescriptors) {
  DescriptorsType descriptors(48, 3);
  descriptors.setZero();

  descriptors(0, 0) = 7;
  descriptors(0, 1) = 3;
  descriptors(0, 2) = 1;

  Eigen::Matrix<double, Eigen::Dynamic, 1> mean(48 * kBitsPerByte, 1);

  floatDescriptorMean(descriptors, &mean);
  // Mean of {1, 1, 1}
  EXPECT_DOUBLE_EQ(1.0, mean(0));
  // Mean of {1, 1, 0}
  EXPECT_DOUBLE_EQ(2.0 / 3.0, mean(1));
  // Mean of {1, 0, 0}
  EXPECT_DOUBLE_EQ(1.0 / 3.0, mean(2));
  // Mean of {0, 0, 0}
  EXPECT_DOUBLE_EQ(0.0, mean(3));
}

TEST(ViwlsGraph, DescriptorMeanStdDeviationTest) {
  DescriptorsType descriptors(48, 3);
  descriptors.setZero();

  descriptors(0, 0) = 7;
  descriptors(0, 1) = 3;
  descriptors(0, 2) = 1;

  EXPECT_DOUBLE_EQ(descriptorMeanStandardDeviation(descriptors),
                   2.0 / 3.0);
}

TEST(ViwlsGraph, DescriptorMeanStdDeviationTestZeroDescriptors) {
  DescriptorsType descriptors(48, 1);
  EXPECT_DOUBLE_EQ(descriptorMeanStandardDeviation(descriptors),
                   0.0);
}

TEST(ViwlsGraph, DescriptorMeanStdDeviationTestOneDescriptor) {
  DescriptorsType descriptors(48, 1);
  descriptors.setRandom();
  EXPECT_DOUBLE_EQ(descriptorMeanStandardDeviation(descriptors),
                   0.0);
}

TEST(ViwlsGraph, DescriptorMeanStdDeviationTestSameDescriptors) {
  DescriptorsType descriptors(48, 3);
  descriptors.setZero();

  descriptors(0, 0) = 56;
  descriptors(0, 1) = 56;
  descriptors(0, 2) = 56;

  EXPECT_DOUBLE_EQ(descriptorMeanStandardDeviation(descriptors),
                   0.0);
}

TEST(ViwlsGraph, DescriptorClosestToMedianTestZeroDescriptors) {
  DescriptorsType descriptors(48, 0);
  descriptors.setZero();

  size_t closest_to_median_descriptor_index = 0u;
  EXPECT_DEATH(
      getIndexOfDescriptorClosestToMedian(
          descriptors, &closest_to_median_descriptor_index), "");
}

TEST(ViwlsGraph, DescriptorClosestToMedianTestOneDescriptors) {
  DescriptorsType descriptors(48, 1);
  descriptors.setZero();

  descriptors(0, 0) = 7;

  size_t closest_to_median_descriptor_index = 0u;
  getIndexOfDescriptorClosestToMedian(
      descriptors, &closest_to_median_descriptor_index);

  EXPECT_EQ(0u, closest_to_median_descriptor_index);
}

TEST(ViwlsGraph, DescriptorClosestToMedianTestThreeDescriptors) {
  DescriptorsType descriptors(48, 3);
  descriptors.setZero();

  descriptors(0, 0) = 7;
  descriptors(0, 1) = 3;
  descriptors(0, 2) = 1;

  size_t closest_to_median_descriptor_index = 0u;
  getIndexOfDescriptorClosestToMedian(
      descriptors, &closest_to_median_descriptor_index);

  EXPECT_EQ(1u, closest_to_median_descriptor_index);

  descriptors(0, 1) = 255;

  getIndexOfDescriptorClosestToMedian(
      descriptors, &closest_to_median_descriptor_index);

  EXPECT_EQ(0u, closest_to_median_descriptor_index);

  descriptors(0, 2) = 127;

  getIndexOfDescriptorClosestToMedian(
      descriptors, &closest_to_median_descriptor_index);

  EXPECT_EQ(2u, closest_to_median_descriptor_index);
}

}  // namespace descriptor_utils
}  // namespace common
}  // namespace aslam

ASLAM_UNITTEST_ENTRYPOINT


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
MatrixXd m(2,3);
m << 1,2,3,4,5,6;
cout << "here's the 2x3 matrix m:" << endl << m << endl;
cout << "let's resize m to 3x2. This is a conservative resizing because 2*3==3*2." << endl;
m.resize(3,2);
cout << "here's the 3x2 matrix m:" << endl << m << endl;
cout << "now let's resize m to size 2x2. This is NOT a conservative resizing, so it becomes uninitialized:" << endl;
m.resize(2,2);
cout << m << endl;

}
  return 0;
}


#include "scope.h"

#include "math_lib/transpose.h"

#include <boost/qvm/map_mat_mat.hpp>
#include <boost/qvm/map_mat_vec.hpp>
#include <boost/qvm/map_vec_mat.hpp>
#include <boost/qvm/vec_operations.hpp>

namespace pagoda
{
Scope::Scope() : m_position{0, 0, 0}, m_size{0, 0, 0}, m_rotation(boost::qvm::diag_mat(Vec3F{1, 1, 1})) {}

Scope::Scope(const Vec3F &pos, const Vec3F &size, const Mat3x3F &rot) : m_position(pos), m_size(size), m_rotation(rot)
{
}

Scope::Scope(const std::array<Vec3F, 8> &boxPoints)
{
	auto origin = boxPoints[static_cast<int>(BoxPoints::LowerBottomLeft)];
	auto lbr = boxPoints[static_cast<int>(BoxPoints::LowerBottomRight)];
	auto ltl = boxPoints[static_cast<int>(BoxPoints::LowerTopLeft)];
	auto hbl = boxPoints[static_cast<int>(BoxPoints::HigherBottomLeft)];
	auto xAxis = lbr - origin;
	auto yAxis = ltl - origin;
	auto zAxis = boost::qvm::cross(xAxis, yAxis);

	m_position = origin;
	m_size = Vec3F{boost::qvm::mag(xAxis), boost::qvm::mag(yAxis), boost::qvm::mag(origin - hbl)};
	boost::qvm::col<0>(m_rotation) = boost::qvm::normalized(xAxis);
	boost::qvm::col<1>(m_rotation) = boost::qvm::normalized(yAxis);
	boost::qvm::col<2>(m_rotation) = boost::qvm::normalized(zAxis);
}

Vec3F Scope::GetPosition() const { return m_position; }

void Scope::SetPosition(const Vec3F &pos) { m_position = pos; }

Vec3F Scope::GetSize() const { return m_size; }

void Scope::SetSize(const Vec3F &size) { m_size = size; }

Mat3x3F Scope::GetRotation() const { return m_rotation; }

void Scope::SetRotation(const Mat3x3F &rotation) { m_rotation = rotation; }

Mat3x3F Scope::GetInverseRotation() const { return boost::qvm::transposed(m_rotation); }

Vec3F Scope::GetXAxis() const { return boost::qvm::col<0>(m_rotation); }
Vec3F Scope::GetYAxis() const { return boost::qvm::col<1>(m_rotation); }
Vec3F Scope::GetZAxis() const { return boost::qvm::col<2>(m_rotation); }

Vec3F Scope::GetAxis(const std::string &axisName) const
{
	if (axisName == "x")
	{
		return GetXAxis();
	}
	else if (axisName == "y")
	{
		return GetYAxis();
	}
	CRITICAL_ASSERT_MSG(axisName == "z", "Axis name must be one of x, y or z.");
	return GetZAxis();
}

Vec3F Scope::GetAxis(char axisName) const
{
	switch (axisName)
	{
		case 'x':
			return GetXAxis();
		case 'y':
			return GetYAxis();
		case 'z':
			return GetZAxis();
		default:
			CRITICAL_ASSERT_MSG(false, "Axis must be one of x, y or z.");
	}
	return Vec3F();
}

Plane<float> Scope::GetXYPlane() const { return Plane<float>::FromPointAndNormal(m_position, GetZAxis()); }
Plane<float> Scope::GetXZPlane() const { return Plane<float>::FromPointAndNormal(m_position, GetYAxis()); }
Plane<float> Scope::GetYZPlane() const { return Plane<float>::FromPointAndNormal(m_position, GetXAxis()); }

Vec3F Scope::LocalPointInWorld(const Vec3F &localPoint) const
{
	return m_position + GetXAxis() * X(localPoint) + GetYAxis() * Y(localPoint) + GetZAxis() * Z(localPoint);
}

Vec3F Scope::GetLocalPoint(const BoxPoints &p) const
{
	uint32_t index = static_cast<uint32_t>(p);
	Vec3F localPoint;
	X(localPoint) = ((index & (1 << 0)) != 0 ? X(m_size) : 0);
	Y(localPoint) = ((index & (1 << 1)) != 0 ? Y(m_size) : 0);
	Z(localPoint) = ((index & (1 << 2)) != 0 ? Z(m_size) : 0);
	return localPoint;
}

Vec3F Scope::GetWorldPoint(const BoxPoints &p) const { return LocalPointInWorld(GetLocalPoint(p)); }

std::array<Vec3F, 8> Scope::GetWorldPoints() const
{
	std::array<Vec3F, 8> boxPoints;
	for (int i = 0; i < 8; ++i)
	{
		boxPoints[i] = GetWorldPoint(static_cast<BoxPoints>(i));
	}
	return boxPoints;
}

Vec3F Scope::GetLocalVector(const Vec3F &worldVector) const
{
	return Vec3F{boost::qvm::dot(GetXAxis(), worldVector), boost::qvm::dot(GetYAxis(), worldVector),
	             boost::qvm::dot(GetZAxis(), worldVector)};
}

Vec3F Scope::GetWorldVector(const Vec3F &localVector) const
{
	return GetXAxis() * X(localVector) + GetYAxis() * Y(localVector) + GetZAxis() * Z(localVector);
}

Vec3F Scope::GetCenterPointInWorld() const { return LocalPointInWorld(GetCenterPointInLocal()); }

Vec3F Scope::GetCenterPointInLocal() const
{
	return 0.5f * (GetLocalPoint(BoxPoints::LowerBottomLeft) + GetLocalPoint(BoxPoints::HigherTopRight));
}
}  // namespace pagoda


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

#ifndef OPENGV_RELATIVE_POSE_MODULES_MAIN_HPP_
#define OPENGV_RELATIVE_POSE_MODULES_MAIN_HPP_

#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>

namespace opengv
{
namespace relative_pose
{
namespace modules
{
rotation_t sixpt_urban_main_onlyRot(
		const bearingVectors_t& f1,
		const bearingVectors_t& f2);
transformation_t sixpt_urban_main(
	const bearingVectors_t& f1,
	const bearingVectors_t& f2);
void fivept_stewenius_main(
    const Eigen::Matrix<double,9,4> & EE,
    complexEssentials_t & complexEssentials );
void fivept_nister_main(
    const Eigen::Matrix<double,9,4> & EE,
    essentials_t & essentials );
void fivept_kneip_main(
    const Eigen::Matrix<double,3,5> & f1,
    const Eigen::Matrix<double,3,5> & f2,
    rotations_t & rotations );
void eigensolver_main(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    eigensolverOutput_t & output );
void sixpt_main(
    Eigen::Matrix<double,6,6> & L1,
    Eigen::Matrix<double,6,6> & L2,
    rotations_t & solutions);
void sixpt_ventura_main(
	const Eigen::Matrix<double, 6, 6> &w1,
	const Eigen::Matrix<double, 6, 6> &w2,
	const Eigen::Matrix<double, 6, 6> &w3,
	const Eigen::Matrix<double, 6, 6> &w4,
	const Eigen::Matrix<double, 6, 6> &w5,
	const Eigen::Matrix<double, 6, 6> &w6,
	std::vector<Eigen::Vector3d> &rsolns);
void ge_main(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P,
    const cayley_t & startingPoint,
    geOutput_t & output );
void ge_main2(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P,
    const cayley_t & startingPoint,
    geOutput_t & output );
void ge_plot(
    const Eigen::Matrix3d & xxF,
    const Eigen::Matrix3d & yyF,
    const Eigen::Matrix3d & zzF,
    const Eigen::Matrix3d & xyF,
    const Eigen::Matrix3d & yzF,
    const Eigen::Matrix3d & zxF,
    const Eigen::Matrix<double,3,9> & x1P,
    const Eigen::Matrix<double,3,9> & y1P,
    const Eigen::Matrix<double,3,9> & z1P,
    const Eigen::Matrix<double,3,9> & x2P,
    const Eigen::Matrix<double,3,9> & y2P,
    const Eigen::Matrix<double,3,9> & z2P,
    const Eigen::Matrix<double,9,9> & m11P,
    const Eigen::Matrix<double,9,9> & m12P,
    const Eigen::Matrix<double,9,9> & m22P,
    geOutput_t & output );
}
}
}

#endif /* OPENGV_RELATIVE_POSE_MODULES_MAIN_HPP_ */


#include <limits>
#include <random>

#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <rsvd/Constants.hpp>
#include <rsvd/RandomizedRangeFinder.hpp>

using Eigen::Index;
using Rsvd::LuConditioner;
using Rsvd::MgsConditioner;
using Rsvd::NoConditioner;
using Rsvd::QrConditioner;
using Rsvd::Internal::RandomizedSubspaceIterations;
using Rsvd::Internal::singleShot;

// Note: Tolerances for the range approximation (tests "{SingleShot, NoConditioner, LuConditioner,
// MgsConditioner, QrConditioner}Approximation") were chosen after several runs with different PRNG
// seeds (444, 555, 666, 777).

template <typename T> struct RandomizedRangeFinder : public ::testing::Test {
  using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
  using RealType = typename Eigen::NumTraits<T>::Real;

  const Index numRows = 50;
  const Index numCols = 25;
  const Index dim = 15;
  // The oversampling additional to the rank is important to test for correct deflation, especially
  // for the MGS conditioner
  const Index oversampling = 5;
  const unsigned int numIter = 2;
  const unsigned int prngSeed = 777;

  const RealType macheps = std::numeric_limits<RealType>::epsilon();
};

using NumericalTypes = ::testing::Types<float, double, std::complex<float>, std::complex<double>>;

TYPED_TEST_CASE(RandomizedRangeFinder, NumericalTypes, );

// \brief Range approximation must have columns of unit length
TYPED_TEST(RandomizedRangeFinder, SingleShotNorm) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q = singleShot<MatrixType, std::mt19937_64>(
      a, TestFixture::dim + TestFixture::oversampling, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    ASSERT_NEAR(q.col(i).norm(), 1, 2 * TestFixture::macheps);
  }
}

// \brief Range approximation must have orthogonal columns
TYPED_TEST(RandomizedRangeFinder, SingleShotOrthogonality) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q = singleShot<MatrixType, std::mt19937_64>(
      a, TestFixture::dim + TestFixture::oversampling, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    for (Index j = 0; j < i; ++j) {
      const auto res = std::abs(q.col(i).dot(q.col(j)));
      ASSERT_NEAR(res, 0, 2 * TestFixture::macheps);
    }
  }
}

// \brief Range approximation must satisfy the requirement | A - Q Q* A | < eps
TYPED_TEST(RandomizedRangeFinder, SingleShotApproximation) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q = singleShot<MatrixType, std::mt19937_64>(
      a, TestFixture::dim + TestFixture::oversampling, randomEngine);
  const MatrixType res = a - q * (q.adjoint() * a);
  ASSERT_NEAR(res.norm(), 0, 2e3 * TestFixture::macheps);
}

// \brief Range approximation must have columns of unit length
TYPED_TEST(RandomizedRangeFinder, NoConditionerNorm) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, NoConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    ASSERT_NEAR(q.col(i).norm(), 1, 2 * TestFixture::macheps);
  }
}

// \brief Range approximation must have orthogonal columns
TYPED_TEST(RandomizedRangeFinder, NoConditionerOrthogonality) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, NoConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    for (Index j = 0; j < i; ++j) {
      const auto res = std::abs(q.col(i).dot(q.col(j)));
      ASSERT_NEAR(res, 0, 2 * TestFixture::macheps);
    }
  }
}

// \brief Range approximation must satisfy the requirement | A - Q Q* A | < eps
TYPED_TEST(RandomizedRangeFinder, NoConditionerApproximation) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, NoConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);
  const MatrixType res = a - q * (q.adjoint() * a);
  ASSERT_NEAR(res.norm(), 0, 1e6 * TestFixture::macheps);
}

// \brief Range approximation must have columns of unit length
TYPED_TEST(RandomizedRangeFinder, LuConditionerNorm) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, LuConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    ASSERT_NEAR(q.col(i).norm(), 1, 2 * TestFixture::macheps);
  }
}

// \brief Range approximation must have orthogonal columns
TYPED_TEST(RandomizedRangeFinder, LuConditionerOrthogonality) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, LuConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    for (Index j = 0; j < i; ++j) {
      const auto res = std::abs(q.col(i).dot(q.col(j)));
      ASSERT_NEAR(res, 0, 2 * TestFixture::macheps);
    }
  }
}

// \brief Range approximation must satisfy the requirement | A - Q Q* A | < eps
TYPED_TEST(RandomizedRangeFinder, LuConditionerApproximation) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, LuConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);
  const MatrixType res = a - q * (q.adjoint() * a);
  ASSERT_NEAR(res.norm(), 0, 4e2 * TestFixture::macheps);
}

// \brief Range approximation must have columns of unit length
TYPED_TEST(RandomizedRangeFinder, MgsConditionerNorm) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, MgsConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    ASSERT_NEAR(q.col(i).norm(), 1, 2 * TestFixture::macheps);
  }
}

// \brief Range approximation must have orthogonal columns
TYPED_TEST(RandomizedRangeFinder, MgsConditionerOrthogonality) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, MgsConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    for (Index j = 0; j < i; ++j) {
      const auto res = std::abs(q.col(i).dot(q.col(j)));
      ASSERT_NEAR(res, 0, 2 * TestFixture::macheps);
    }
  }
}

// \brief Range approximation must satisfy the requirement | A - Q Q* A | < eps
TYPED_TEST(RandomizedRangeFinder, MgsConditionerApproximation) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, MgsConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);
  const MatrixType res = a - q * (q.adjoint() * a);
  ASSERT_NEAR(res.norm(), 0, 3e2 * TestFixture::macheps);
}

// \brief Range approximation must have columns of unit length
TYPED_TEST(RandomizedRangeFinder, QrConditionerNorm) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, QrConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    ASSERT_NEAR(q.col(i).norm(), 1, 2 * TestFixture::macheps);
  }
}

// \brief Range approximation must have orthogonal columns
TYPED_TEST(RandomizedRangeFinder, QrConditionerOrthogonality) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, QrConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);

  for (Index i = 0; i < TestFixture::dim; ++i) {
    for (Index j = 0; j < i; ++j) {
      const auto res = std::abs(q.col(i).dot(q.col(j)));
      ASSERT_NEAR(res, 0, 2 * TestFixture::macheps);
    }
  }
}

// \brief Range approximation must satisfy the requirement | A - Q Q* A | < eps
TYPED_TEST(RandomizedRangeFinder, QrConditionerApproximation) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);
  std::mt19937_64 randomEngine;
  randomEngine.seed(TestFixture::prngSeed);

  // Create a matrix with rank loss
  const MatrixType a = MatrixType::Random(TestFixture::numRows, TestFixture::dim) *
                       MatrixType::Random(TestFixture::dim, TestFixture::numCols);
  // Compute the randomized range approximation
  const MatrixType q =
      RandomizedSubspaceIterations<MatrixType, std::mt19937_64, QrConditioner>::compute(
          a, TestFixture::dim + TestFixture::oversampling, TestFixture::numIter, randomEngine);
  const MatrixType res = a - q * (q.adjoint() * a);
  ASSERT_NEAR(res.norm(), 0, 3e2 * TestFixture::macheps);
}


// Copyright (C) 2008  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/optimization/find_optimal_parameters.h>
#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.find_optimal_parameters");

// ----------------------------------------------------------------------------------------

    class find_optimal_parameters : public tester
    {
    public:
        find_optimal_parameters (
        ) :
            tester ("test_find_optimal_parameters",
                    "Runs tests on find_optimal_parameters().")
        {}

        void perform_test (
        )
        {
            print_spinner();
            matrix<double,0,1> params = {0.5, 0.5};
            dlib::find_optimal_parameters(4, 0.001, 100, params, {-0.1, -0.01}, {5, 5}, [](const matrix<double,0,1>& params) {
                cout << ".";
                return sum(squared(params));
            });

            matrix<double,0,1> true_params = {0,0};

            DLIB_TEST(max(abs(true_params - params)) < 1e-10);

            params = {0.1};
            dlib::find_optimal_parameters(4, 0.001, 100, params, {-0.01}, {5}, [](const matrix<double,0,1>& params) {
                cout << ".";
                return sum(squared(params));
            });

            true_params = {0};
            DLIB_TEST(max(abs(true_params - params)) < 1e-10);
        }
    } a;

}


#include <algorithm>
#include <boost/functional/hash.hpp>
#include <iostream>
#include <sstream>
#include <unordered_map>
#include <vector>

size_t checksum(const std::vector<int>& nums) {
    size_t seed = 0;
    for (auto n : nums) {
        boost::hash_combine(seed, n);
    }
    return seed;
}

int main() {
    std::vector<int> nums;
    {
        std::string line;
        std::getline(std::cin, line);
        std::stringstream ss{line};
        while (!ss.eof()) {
            int x;
            ss >> x;
            nums.push_back(x);
        }
    }

    std::unordered_map<size_t, int> hashes;
    hashes.insert(std::make_pair(checksum(nums), 0));

    int steps = 0;
    for(;;) {
        auto it = std::max_element(std::begin(nums), std::end(nums));
        if (it == std::end(nums)) {
            std::cerr << "NO MAX!\n";
            exit(1);
        }
        int val = 0;
        std::swap(val, *it);
        for(++it; val > 0; ++it, --val) {
            if (it == std::end(nums)) {
                it = std::begin(nums);
            }
            (*it)++;
        }
        ++steps;
        auto ii = hashes.insert(std::make_pair(checksum(nums), steps));
        if (!ii.second) {
            std::cout << "last loop: " << steps-ii.first->second << "\n";
            break;
        }
    }
}


//
#include <CGAL/Surface_mesh_segmentation/internal/K_means_clustering.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
/**
 * Generates sample points using a few gauissians.
 * Then applies k-means on these generated points.
 * Provides a heuristic score for each k-means clustering result.
 *
 * EXIT_FAILURE does not mean failure but if approximate matching is too low it is best to check
 */
int main(void)
{
    boost::mt19937 engine;
    engine.seed(1340818006);

    // generate random data using gauissians below
    std::vector< boost::normal_distribution<double> > distributions;
    distributions.push_back(boost::normal_distribution<double>(0.1, 0.05));
    distributions.push_back(boost::normal_distribution<double>(0.4, 0.1));
    distributions.push_back(boost::normal_distribution<double>(0.55, 0.05));
    distributions.push_back(boost::normal_distribution<double>(0.7, 0.1));
    distributions.push_back(boost::normal_distribution<double>(0.9, 0.05));
    distributions.push_back(boost::normal_distribution<double>(1.0, 0.05));

    std::vector<double> data;
    for(std::vector< boost::normal_distribution<double> >::iterator it = distributions.begin();
      it != distributions.end(); ++it)
    {
        boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > var_nor(engine, *it);

        for(std::size_t i = 0; i < 300; ++i) { data.push_back(var_nor()); }
    }

    // calculate closest center (using above gauissians) for each generated points
    // we will compare it with k-means results
    std::vector<std::size_t> data_centers;
    for(std::vector<double>::iterator it = data.begin(); it != data.end(); ++it)
    {
        std::size_t center_id = (std::numeric_limits<std::size_t>::max)(), center_counter = 0;;
        double min_distance = (std::numeric_limits<double>::max)();
        for(std::vector< boost::normal_distribution<double> >::iterator dis_it = distributions.begin();
          dis_it != distributions.end(); ++dis_it, ++center_counter)
        {
            double distance = std::abs(*it - dis_it->mean());
            if(min_distance > distance)
            {
                min_distance = distance;
                center_id = center_counter;
            }
        }
        data_centers.push_back(center_id);
    }

    // apply k-means clustering
    typedef CGAL::internal::K_means_clustering K_means;
    std::vector<K_means> k_means;
    k_means.push_back(K_means(distributions.size(), data, K_means::PLUS_INITIALIZATION));
    k_means.push_back(K_means(distributions.size(), data, K_means::RANDOM_INITIALIZATION));

    std::vector< std::vector<std::size_t> > calculated_centers(k_means.size());
    std::vector< std::vector<std::size_t> >::iterator calc_centers_it = calculated_centers.begin();
    for(std::vector<K_means>::iterator it = k_means.begin(); it != k_means.end(); ++it, ++calc_centers_it)
    {
        it->fill_with_center_ids(*calc_centers_it);
    }

    std::cout << "Compare results of k-means with 'expected' (but be aware, it is not optimal result in terms of within-cluster error)" << std::endl;
    std::cout << "Another words a clustering which has smaller within-cluster error can result in worse score in here" << std::endl;
    for(std::vector< std::vector<std::size_t> >::iterator calc_centers_it = calculated_centers.begin();
        calc_centers_it != calculated_centers.end(); ++calc_centers_it)
    {
        std::size_t true_count = 0;
        std::vector<std::size_t>::iterator calculated_it = calc_centers_it->begin();
        for(std::vector<std::size_t>::iterator it = data_centers.begin(); it != data_centers.end(); ++it, ++calculated_it)
        {
            if( (*it) == (*calculated_it) ) { ++true_count; }
        }
        double app_fit = static_cast<double>(true_count) / data_centers.size();
        std::cout << "[0,1]: " << app_fit << std::endl;
        if(app_fit < 0.7) {
            std::cerr << "There might be a problem if above printed comparison is too low." << std::endl;
            return EXIT_FAILURE;
        }
    }
}


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

#include <Eigen/Dense>

#include <boost/numeric/odeint.hpp>
#include <cbr_control/mpc/mpc_tracking.hpp>
#include <matplot/matplot.h>

#include <chrono>
#include <vector>
#include <algorithm>

#include "pendulum_problem.hpp"

using namespace std::chrono_literals;

int main(int argc, char const * argv[])
{
  using state_t = Eigen::Matrix<double, 2, 1>;  // [ theta, theta_dot ]
  using input_t = Eigen::Matrix<double, 1, 1>;  // [ tau ]

  //  -------------------------------------------------------------------------- /
  //                              Simulation Params                              /
  //  -------------------------------------------------------------------------- /

  state_t x0 = (state_t() << cbr::deg2rad(5.), cbr::deg2rad(0.)).finished();
  const auto dt = 10ms;
  const auto tf = 5s;

  auto xd = [](nanoseconds) {
      return state_t(cbr::deg2rad(0.), 0);
    };

  //  -------------------------------------------------------------------------- /
  //                                      MPC                                    /
  //  -------------------------------------------------------------------------- /

  NlOcpPendulum nlOcp{};

  cbr::MPCTrackingParams params;
  params.T = 5;
  params.solver_params.osqp_settings.verbose = 0;

  cbr::MPCTracking<NlOcpPendulum, 101> mpc(nlOcp, params);
  mpc.set_xd(xd);

  //  -------------------------------------------------------------------------- /
  //                                RUN SIMULATION                               /
  //  -------------------------------------------------------------------------- /

  nanoseconds t(0);
  state_t x = x0;

  boost::numeric::odeint::euler<
    state_t, double, state_t, double,
    boost::numeric::odeint::vector_space_algebra
  > stepper;

  std::vector<double> sol_t;
  std::vector<state_t, Eigen::aligned_allocator<state_t>> sol_x;
  std::vector<input_t, Eigen::aligned_allocator<input_t>> sol_u;

  while (t < tf) {
    mpc.update_sync(t, x);
    const auto u = mpc.get_u(t);

    sol_t.push_back(duration_cast<duration<double>>(t).count());
    sol_x.push_back(x);
    sol_u.push_back(u);

    stepper.do_step(
      [&nlOcp, &u](const state_t & x, state_t & xdot, const double) {
        xdot = nlOcp.get_f(x, u);
      },
      x,
      duration_cast<duration<double>>(t).count(),
      duration_cast<duration<double>>(dt).count()
    );

    t += dt;
  }

  //  -------------------------------------------------------------------------- /
  //                                PLOT RESULTS                                 /
  //  -------------------------------------------------------------------------- /

  // helper function to extract stuff from solutions
  auto ex_fn = [](const auto & item, auto ex_fn) {
      std::vector<double> ret;
      std::transform(item.cbegin(), item.cend(), std::back_inserter(ret), ex_fn);
      return ret;
    };

  matplot::figure();
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return s(0);}))->line_width(2);
  matplot::title("theta");
  matplot::figure();
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return s(1);}))->line_width(2);
  matplot::title("dot theta");
  matplot::figure();
  matplot::plot(sol_t, ex_fn(sol_u, [](auto s) {return s(0);}))->line_width(2);
  matplot::title("input");
  matplot::show();

  return EXIT_SUCCESS;
}


#include "storm/constants.hpp"
#include "storm/rambo.hpp"
#include "storm/simple.hpp"
#include "storm/types.hpp"
#include <array>
#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <cmath>
#include <cstddef>
#include <functional>
#include <locale>
#include <stdexcept>
#include <utility>

namespace storm {

/**
 * Compute the partial width for a RH neutrino decaying into a active neutrino
 * and a Higgs.
 */
auto SimpleRhNeutrino::width_vl_h() const -> double {
  return ((-pow(kHIGGS_MASS, 2) + pow(p_mvr, 2)) * pow(p_theta, 2) *
          std::abs(pow(kHIGGS_MASS, 2) - pow(p_mvr, 2))) /
         (16. * p_mvr * M_PI * pow(kHIGGS_VEV, 2));
}

/**
 * Compute the partial width for a RH neutrino decaying into a active neutrino
 * and a Z.
 */
auto SimpleRhNeutrino::width_vl_z() const -> double {

  return (((pow(p_mvr, 4) + pow(p_mvr, 2) * pow(kZ_BOSON_MASS, 2) -
            2 * pow(kZ_BOSON_MASS, 4)) *
           kALPHA_EM * pow(p_theta, 2) *
           std::abs(pow(p_mvr, 2) - pow(kZ_BOSON_MASS, 2))) /
          (16.0 * pow(kCOS_THETA_WEAK, 2) * pow(p_mvr, 3) *
           pow(kZ_BOSON_MASS, 2) * pow(kSIN_THETA_WEAK, 2)));
}

/**
 * Compute the partial width for a RH neutrino decaying into a charged lepton
 * and a W.
 */
auto SimpleRhNeutrino::width_l_w() const -> double {
  return (std::sqrt(
              (p_ml - p_mvr - kW_BOSON_MASS) * (p_ml + p_mvr - kW_BOSON_MASS) *
              (p_ml - p_mvr + kW_BOSON_MASS) * (p_ml + p_mvr + kW_BOSON_MASS)) *
          (pow(pow(p_ml, 2) - pow(p_mvr, 2), 2) +
           (pow(p_ml, 2) + pow(p_mvr, 2)) * pow(kW_BOSON_MASS, 2) -
           2 * pow(kW_BOSON_MASS, 4)) *
          kALPHA_EM * pow(p_theta, 2)) /
         (16. * pow(p_mvr, 3) * pow(kW_BOSON_MASS, 2) *
          pow(kSIN_THETA_WEAK, 2));
}

/**
 * Compute the integration bounds on the Mandelstam variable `s` for three-body
 * phase-space integration.
 * @param m Mass of the decaying particle.
 * @param m1 Mass of final state particle 1.
 * @param m2 Mass of final state particle 2.
 * @param m3 Mass of final state particle 3.
 */
inline static auto compute_s_bounds(double m, double m1, double m2, double m3)
    -> std::pair<double, double> {
  return std::make_pair(pow(m2 + m3, 2), pow(m - m1, 2));
}

/**
 * @breif Compute the three body decay width given the squared matrix element
 * integrated over the Mandelstam variable `t`.
 * @param msqrd Squared matrix element integrated over t = (p1+p3)^2.
 * @param m Mass of the decaying particle.
 * @param fsp_masses Mass of the three final-state particles.
 * @param error Estimated error in the decay width.
 */
static auto compute_width_3body(const std::function<double(double)> &msqrd,
                                double m, std::array<double, 3> fsp_masses,
                                double *error = nullptr) -> double {
  using boost::math::quadrature::gauss_kronrod;

  auto bounds =
      compute_s_bounds(m, fsp_masses[0], fsp_masses[1], fsp_masses[2]);
  auto integral = gauss_kronrod<double, 15>::integrate(
      msqrd, bounds.first, bounds.second, 5, 1e-9, error);

  const double pf = 1.0 / (256.0 * pow(M_PI * m, 3));
  *error *= pf;
  return std::abs(integral) * pf;
}

static auto compute_width_3body(const SquaredMatrixElement &msqrd,
                                const double m, std::vector<double> fsp_masses,
                                size_t num_events = 10000,
                                double *error = nullptr) {

  Rambo rambo{std::move(fsp_masses), m, msqrd};
  const auto width = rambo.compute_width(num_events);
  *error = width.second;
  return width.first;
}

} // namespace storm


/* Copyright 2020 CNRS-AIST JRL */

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/QR>

#include <benchmark/benchmark.h>

#include "common.h"

using namespace Eigen;

static void BM_TriangularSolve_NoInversePrecompute(benchmark::State & state)
{
  MatrixXd R = MatrixXd::Random(50, 50);
  MatrixXd A = R.transpose() * R;
  LLT<MatrixXd> llt(A);
  VectorXd b = VectorXd::Random(50);
  for(auto _ : state)
  {
    for(int i = 0; i < state.range(0); ++i)
    {
      llt.matrixL().solveInPlace(b);
    }
  }
}
// BENCHMARK(BM_TriangularSolve_NoInversePrecompute)->Apply(testSizes)->Unit(benchmark::kMicrosecond);

static void BM_TriangularSolve_InversePrecompute(benchmark::State & state)
{
  MatrixXd R = MatrixXd::Random(50, 50);
  MatrixXd A = R.transpose() * R;
  LLT<MatrixXd> llt(A);
  VectorXd b = VectorXd::Random(50);
  VectorXd x = VectorXd::Random(50);
  MatrixXd invA(50, 50);
  for(auto _ : state)
  {
    invA.setIdentity();
    llt.matrixL().solveInPlace(invA);
    for(int i = 0; i < state.range(0); ++i)
    {
      benchmark::DoNotOptimize(x.noalias() = invA.triangularView<Lower>() * b);
    }
  }
}
BENCHMARK(BM_TriangularSolve_InversePrecompute)->Apply(testSizes)->Unit(benchmark::kMicrosecond);

static void BM_TriangularInverse_AtOnce(benchmark::State & state)
{
  const int n = state.range(0);
  MatrixXd A = MatrixXd::Random(n, n);
  MatrixXd invA(state.range(0), state.range(0));
  for(auto _ : state)
  {
    invA.setIdentity();
    A.template triangularView<Eigen::Lower>().solveInPlace(invA);
  }
}
BENCHMARK(BM_TriangularInverse_AtOnce)->Apply(testSizes)->Unit(benchmark::kMicrosecond);

static void BM_TriangularInverse_Transpose(benchmark::State & state)
{
  const int n = state.range(0);
  MatrixXd A = MatrixXd::Random(n, n);
  MatrixXd invA(state.range(0), state.range(0));
  for(auto _ : state)
  {
    invA.setIdentity();
    A.template triangularView<Eigen::Lower>().transpose().solveInPlace(invA);
  }
}
BENCHMARK(BM_TriangularInverse_Transpose)->Apply(testSizes)->Unit(benchmark::kMicrosecond);

static void BM_TriangularInverse_ByCol(benchmark::State & state)
{
  const int n = state.range(0);
  MatrixXd A = MatrixXd::Random(n, n);
  MatrixXd invA(state.range(0), state.range(0));
  VectorXd e(n);
  for(auto _ : state)
  {
    e.setZero();
    for(int i = 0; i < n; ++i)
    {
      e[i] = 1;
      invA.col(i) = A.template triangularView<Eigen::Lower>().solve(e);
      e[i] = 0;
    }
  }
}
BENCHMARK(BM_TriangularInverse_ByCol)->Apply(testSizes)->Unit(benchmark::kMicrosecond);

BENCHMARK_MAIN();


#define BOOST_TEST_MODULE common_unit_tests

#include <boost/test/included/unit_test.hpp>
#include <armadillo>
#include "../src/common.h"
#include "test_utils.cpp"
#include "../src/logging/easylogging++.h"

INITIALIZE_EASYLOGGINGPP

using namespace arma;

BOOST_AUTO_TEST_CASE(full_rank_inv_with_random_square_matrix)
{
    mat x = randu<mat>(5, 5);
    mat expected = calculate_pseudoinverse(x);
    mat actual = calculate_svd_inverse(x);
    BOOST_CHECK(is_equal(expected, actual));
}

BOOST_AUTO_TEST_CASE(full_rank_inv_with_rank_argument_with_random_square_matrix)
{
    mat x = randu<mat>(5, 5);
    mat expected = calculate_pseudoinverse(x);
    mat actual = calculate_svd_inverse(x, 5);
    BOOST_CHECK(is_equal(expected, actual));
}

BOOST_AUTO_TEST_CASE(full_rank_inv_with_rank_argument_with_random_non_square_matrix)
{
    mat x = randu<mat>(5, 6);
    mat expected = calculate_pseudoinverse(x);
    mat actual = calculate_svd_inverse(x, 5);
    BOOST_CHECK(is_equal(expected, actual));
}

BOOST_AUTO_TEST_CASE(low_rank_inv_with_random_square_matrix)
{
    mat x = randu<mat>(10, 10);
    mat expected = calculate_pseudoinverse(x);
    mat actual = calculate_svd_inverse(x, 9);
    // actual is only approximately equal to expected due to low rank approximation
    // -> use larger threshold for this test case
    BOOST_CHECK(is_equal(expected, actual, 0.1));
}

BOOST_AUTO_TEST_CASE(low_rank_inv_with_non_square_random_matrix)
{
    mat x = randu<mat>(10, 12);
    mat expected = calculate_pseudoinverse(x);
    mat actual = calculate_svd_inverse(x, 9);
    // actual is only approximately equal to expected due to low rank approximation
    // -> use larger threshold for this test case
    BOOST_CHECK(is_equal(expected, actual, 0.1));
}

BOOST_AUTO_TEST_CASE(low_rank_approximation_with_non_square_random_matrix)
{
    mat x = randu<mat>(10, 12);
    mat x_low = low_rank_approximation(x, 9);
    // actual is only approximately equal to expected due to low rank approximation
    // -> use larger threshold for this test case
    BOOST_CHECK(is_equal(x, x_low, 0.1));
}

BOOST_AUTO_TEST_CASE(trimmed_mean_integer_argument)
{
    arma::vec x;
    double result;

    x = {1, 2, 5};
    result = arma::as_scalar(trimmed_mean(x, 1));
    BOOST_CHECK_EQUAL(result, 2);

    x = {5, 2, -1, 2};
    result = arma::as_scalar(trimmed_mean(x, 1));
    BOOST_CHECK_EQUAL(result, 2);

    x = {0, 10, 2, 12, 1};
    result = arma::as_scalar(trimmed_mean(x, 2));
    BOOST_CHECK_EQUAL(result, 2);
}

BOOST_AUTO_TEST_CASE(trimmed_mean_float_argument)
{
    arma::vec x;
    double result;

    x = {1, 2, 2, 5};
    result = arma::as_scalar(trimmed_mean(x, 0.2f)); // rejects 1 point from both side
    BOOST_CHECK_EQUAL(result, 2);

    x = {-1, 3, 5, 2, 7};
    result = arma::as_scalar(trimmed_mean(x, 0.4f)); // rejects 2 points from both side
    BOOST_CHECK_EQUAL(result, 3);
}


#include <Eigen/Dense>
#include <vector>
#include "HmcSampler.h"

#include "preparation_Eig_Vect.h"

#include <fstream>

using namespace std;
using namespace Eigen;

void samples(
                int n,
                int dim,
                int seed,
                double *initial,
                int numlin,
                int numquad,
                double *lin,
                double *quad,
                double *quad_lin,
                double *offset_lin,
                double *offset_quad,
                double *samples_Carray
		 ){

  const Map<VectorXd> initial_value(initial, dim);

  ofstream logfile;
  logfile.open ("logfile.txt");

  HmcSampler hmc1(dim, seed);
  if (numlin >0){
    const Map<MatrixXd> F(lin, numlin, dim);
    const Map<VectorXd> g(offset_lin, numlin);

    for(int i=0; i<numlin; i++){
      hmc1.addLinearConstraint(F.row(i),g(i));
    }
  }

  if (numquad >0){

    for(int i=0; i<numquad; i++){
      double *indice = &quad[i*dim*dim];
      const Map<MatrixXd> A_Map(indice, dim, dim);

for(int k=0; k<dim; k++){
for(int l=0; l<dim; l++){
logfile << A_Map(k, l);
}
logfile << endl;
}
logfile << endl;

      MatrixXd A(A_Map);
      const Map<VectorXd> B_Map(&quad_lin[i*dim], dim);
      VectorXd B(B_Map);
      double C = offset_quad[i];
      hmc1.addQuadraticConstraint(A,B,C);
    }

  }

  hmc1.setInitialValue(initial_value);

  MatrixXd samples(n,dim);

  for (int i=0; i<n; i++){
      samples.row(i) = hmc1.sampleNext();
  }

//static double samples_Carray [n][dim];

  double* result = samples.data();

  for(int k=0; k<n; k++){
    for(int l=0; l<dim;l++){
      samples_Carray[k*dim + l] = result[k*dim + l];
    }
  }

for(int k=0; k< n; k++){
for(int l=0; l<dim; l++){
logfile << result[k*dim + l];
}
logfile << endl;
}

logfile << endl;

for(int k=0; k< n; k++){
for(int l=0; l<dim; l++){
logfile << samples_Carray[k*dim + l];
}
logfile << endl;
}

  logfile.close();

//return samples_Carray;

}


/** \file ElInfo2d.h */

#pragma once

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

#include "ElInfo.hpp"

namespace AMDiS
{

  /** \ingroup Traverse
   * \brief
   * ElInfo class for 2-dimensional elements (\ref Triangle).
   */
  class ElInfo2d : public ElInfo
  {
  public:
    /// Constructor. Calls ElInfo's protected Constructor.
    ElInfo2d(Mesh* aMesh);

    ///
    ~ElInfo2d();

    /// 2-dimensional realisation of ElInfo's fillElInfo method.
    virtual void fillElInfo(int ichild, const ElInfo* elinfo_old) override;

    /// 2-dimensional realisation of ElInfo's fillMacroInfo method.
    virtual void fillMacroInfo(const MacroElement*) override;

    /// 2-dimensional realisation of ElInfo's worldToCoord method.
    virtual int worldToCoord(const WorldVector<double>& w, DimVec<double>& l) const override;

    /// 2-dimensional realisation of ElInfo's calcGrdLambda method.
    virtual double calcGrdLambda(DimVec<WorldVector<double>>& grd_lam) override;

    /// 2-dimensional realisation of ElInfo's getNormal method.
    virtual double getNormal(int side, WorldVector<double>& normal) const override;

    /// 2-dimensional realisation of ElInfo's getElementNormal method.
    virtual double getElementNormal(WorldVector<double>& normal) const override;

    /// implements \ref Elnfo::getSubElemCoordsMat
    virtual mtl::dense2D<double>& getSubElemCoordsMat(int degree) const override;

  protected:
    /// Temp vectors for function \ref calcGrdLambda.
    WorldVector<double> e1, e2, normal;

    static double mat_d1_left_val[3][3];
    static mtl::dense2D<double> mat_d1_left;

    static double mat_d1_right_val[3][3];
    static mtl::dense2D<double> mat_d1_right;

    static double mat_d2_left_val[6][6];
    static mtl::dense2D<double> mat_d2_left;

    static double mat_d2_right_val[6][6];
    static mtl::dense2D<double> mat_d2_right;

    static double mat_d3_left_val[10][10];
    static mtl::dense2D<double> mat_d3_left;

    static double mat_d3_right_val[10][10];
    static mtl::dense2D<double> mat_d3_right;

    static double mat_d4_left_val[15][15];
    static mtl::dense2D<double> mat_d4_left;

    static double mat_d4_right_val[15][15];
    static mtl::dense2D<double> mat_d4_right;
  };

} // end namespace AMDiS


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINHCOSH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINHCOSH_HPP_INCLUDED

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

 /*!

    @ingroup group-hyperbolic
    Function object implementing sinhcosh capabilities

    Computes simultaneously the sinh and cosh of the input

    @par Semantic:

    @code
    T ch, sh
    std::tie(sh, ch) = sinhcoshs(x);
    @endcode

    is similar to:

    @code
    T sh = sinh(x);
    T ch = cosh(x);
    @endcode

    but speedier

    @see  sinh, cosh

  **/
  std::pair<Value, Value> sinhcosh(Value const & v0);
} }
#endif

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

#endif


#include <frovedis.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>
#include <frovedis/matrix/scalapack_wrapper.hpp>

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

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating blockcyclic matrix from file
    auto A = make_blockcyclic_matrix_load<float> ("./sample_2x2");
    auto B = make_blockcyclic_matrix_load<float> ("./sample_2x1");

    frovedis::lvec<int> ipiv;  // empty ipiv local-array
    getrf<float> (A,ipiv);   // A will be factorized and ipiv will contain pivoting info
    getrs<float> (A,B,ipiv); // solving AX=B, B will be overwritten with result matrix X

    // checking whether the above operations successfully taken place
    B.save("./out_2x1");
    BOOST_CHECK (system("diff ./out_2x1 ./ref_2x1") == 0);
    system("rm -f ./out_2x1");
}


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

// Copyright (c) 2016-2021 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AZIMUTH_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AZIMUTH_HPP

#include <cmath>

#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/coordinate_promotion.hpp>

#include <boost/geometry/strategies/azimuth.hpp>

#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace azimuth
{

template <typename CalculationType = void>
class cartesian
{
public:
    template <typename T1, typename T2>
    struct result_type
        : geometry::select_most_precise
              <
                  // NOTE: this promotes any integer type to double
                  typename geometry::promote_floating_point<T1, double>::type,
                  typename geometry::promote_floating_point<T2, double>::type,
                  CalculationType
              >
    {};

    template <typename T1, typename T2, typename Result>
    static inline void apply(T1 const& x1, T1 const& y1,
                             T2 const& x2, T2 const& y2,
                             Result& a1, Result& a2)
    {
        compute(x1, y1, x2, y2, a1, a2);
    }
    template <typename T1, typename T2, typename Result>
    static inline void apply(T1 const& x1, T1 const& y1,
                             T2 const& x2, T2 const& y2,
                             Result& a1)
    {
        compute(x1, y1, x2, y2, a1, a1);
    }
    template <typename T1, typename T2, typename Result>
    static inline void apply_reverse(T1 const& x1, T1 const& y1,
                                     T2 const& x2, T2 const& y2,
                                     Result& a2)
    {
        compute(x1, y1, x2, y2, a2, a2);
    }

private:
    template <typename T1, typename T2, typename Result>
    static inline void compute(T1 const& x1, T1 const& y1,
                               T2 const& x2, T2 const& y2,
                               Result& a1, Result& a2)
    {
        typedef typename result_type<T1, T2>::type calc_t;

        // NOTE: azimuth 0 is at Y axis, increasing right
        // as in spherical/geographic where 0 is at North axis
        a1 = a2 = atan2(calc_t(x2) - calc_t(x1), calc_t(y2) - calc_t(y1));
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <>
struct default_strategy<cartesian_tag>
{
    typedef strategy::azimuth::cartesian<> type;
};

}

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::azimuth

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AZIMUTH_HPP


/* boost histogram.cpp graphical verification of distribution functions
 *
 * Copyright Jens Maurer 2000
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id: histogram.cpp,v 1.8 2004/07/27 03:43:34 dgregor Exp $
 *
 * This test program allows to visibly examine the results of the
 * distribution functions.
 */

#include <iostream>
#include <iomanip>
#include <vector>
#include <algorithm>
#include <cmath>
#include <string>
#include <boost/random.hpp>
using namespace std;
#include "Utilities/OhmmsInfo.h"
#include "Message/Communicate.h"
#include "Message/OpenMP.h"
#include "Utilities/RandomGenerator.h"
#include "OhmmsApp/RandomNumberControl.h"
using namespace qmcplusplus;

void plot_histogram(const std::vector<int>& slots, int samples,
                    double from, double to)
{
  int m = *std::max_element(slots.begin(), slots.end());
  const int nRows = 20;
  std::cout.setf(std::ios::fixed|std::ios::left);
  std::cout.precision(5);
  for(int r = 0; r < nRows; r++)
  {
    double y = ((nRows - r) * double(m))/(nRows * samples);
    std::cout << std::setw(10) << y << "  ";
    for(unsigned int col = 0; col < slots.size(); col++)
    {
      char out = ' ';
      if(slots[col]/double(samples) >= y)
        out = 'x';
      std::cout << out;
    }
    std::cout << std::endl;
  }
  std::cout << std::setw(12) << " "
            << std::setw(10) << from;
  std::cout.setf(std::ios::right, std::ios::adjustfield);
  std::cout << std::setw(slots.size()-10) << to << std::endl;
}

// I am not sure whether these two should be in the library as well

// maintain sum of NumberGenerator results
template<class NumberGenerator,
         class Sum = typename NumberGenerator::result_type>
class sum_result
{
public:
  typedef NumberGenerator base_type;
  typedef typename base_type::result_type result_type;
  explicit sum_result(const base_type & g) : gen(g), _sum(0) { }
  result_type operator()()
  {
    result_type r = gen();
    _sum += r;
    return r;
  }
  base_type & base()
  {
    return gen;
  }
  Sum sum() const
  {
    return _sum;
  }
  void reset()
  {
    _sum = 0;
  }
private:
  base_type gen;
  Sum _sum;
};

// maintain square sum of NumberGenerator results
template<class NumberGenerator,
         class Sum = typename NumberGenerator::result_type>
class squaresum_result
{
public:
  typedef NumberGenerator base_type;
  typedef typename base_type::result_type result_type;
  explicit squaresum_result(const base_type & g) : gen(g), _sum(0) { }
  result_type operator()()
  {
    result_type r = gen();
    _sum += r*r;
    return r;
  }
  base_type & base()
  {
    return gen;
  }
  Sum squaresum() const
  {
    return _sum;
  }
  void reset()
  {
    _sum = 0;
  }
private:
  base_type gen;
  Sum _sum;
};

template<class RNG>
void histogram(RNG base, int samples, double from, double to,
               const std::string & name)
{
  typedef squaresum_result<sum_result<RNG, double>, double > SRNG;
  SRNG gen((sum_result<RNG, double>(base)));
  const int nSlots = 60;
  std::vector<int> slots(nSlots,0);
  for(int i = 0; i < samples; i++)
  {
    double val = gen();
    if(val < from || val >= to)    // early check avoids overflow
      continue;
    int slot = int((val-from)/(to-from) * nSlots);
    if(slot < 0 || slot > (int)slots.size())
      continue;
    slots[slot]++;
  }
  std::cout << name << std::endl;
  plot_histogram(slots, samples, from, to);
  double mean = gen.base().sum() / samples;
  std::cout << "mean: " << mean
            << " sigma: " << std::sqrt(gen.squaresum()/samples-mean*mean)
            << "\n" << std::endl;
}

template<class RNG, class SEEDARRAY>
void histogram_OMP(RNG base, int samples, double from, double to,
                   SEEDARRAY& seeds, const std::string & name)
{
  typedef squaresum_result<sum_result<RNG, double>, double > SRNG;
  const int nSlots = 60;
  std::vector<int> slots_tot(nSlots,0);
  double sum1=0.0;
  double sum2=0.0;
  #pragma omp parallel
  {
    std::vector<int> slots(nSlots,0);
    RNG base_copy(base);
    //base_copy.reset();
    base_copy.seed(seeds[omp_get_thread_num()]);
    SRNG gen((sum_result<RNG, double>(base_copy)));
    for(int i = 0; i < samples; i++)
    {
      double val = gen();
      if(val < from || val >= to)    // early check avoids overflow
        continue;
      int slot = int((val-from)/(to-from) * nSlots);
      if(slot < 0 || slot > (int)slots.size())
        continue;
      slots[slot]++;
    }
    #pragma omp critical
    {
      for(int i=0; i<nSlots; i++)
        slots_tot[i]+=slots[i];
      sum1 += gen.base().sum();
      sum2 += gen.squaresum();
      double mean = gen.base().sum() / samples;
      std::cout << "random seed " << seeds[omp_get_thread_num()]
                << " mean: " << mean
                << " sigma: " << std::sqrt(gen.squaresum()/samples-mean*mean)
                << std::endl;
    }
  }
  std::cout << name << std::endl;
  plot_histogram(slots_tot, samples, from, to);
  int samples_tot=samples*omp_get_max_threads();
  double mean = sum1/samples_tot;
  //double mean = gen.base().sum() / samples;
  std::cout << "mean: " << mean
            << " sigma: " << std::sqrt(sum2/samples_tot-mean*mean)
            << "\n" << std::endl;
}

template<class PRNG, class Dist>
inline boost::variate_generator<PRNG&, Dist> make_gen(PRNG & rng, Dist d)
{
  return boost::variate_generator<PRNG&, Dist>(rng, d);
}

template<class PRNG>
void histograms()
{
  PRNG rng;
  using namespace boost;
  //histogram(make_gen(rng, uniform_smallint<>(0, 5)), 100000, -1, 6,
  //          "uniform_smallint(0,5)");
  //histogram(make_gen(rng, uniform_int<>(0, 5)), 100000, -1, 6,
  //          "uniform_int(0,5)");
  histogram(make_gen(rng, uniform_real<>(0,1)), 1000000, -0.5, 1.5,
            "uniform_real(0,1)");
  //histogram(make_gen(rng, bernoulli_distribution<>(0.2)), 100000, -0.5, 1.5,
  //          "bernoulli(0.2)");
  //histogram(make_gen(rng, binomial_distribution<>(4, 0.2)), 100000, -1, 5,
  //          "binomial(4, 0.2)");
  //histogram(make_gen(rng, triangle_distribution<>(1, 2, 8)), 100000, 0, 10,
  //          "triangle(1,2,8)");
  //histogram(make_gen(rng, geometric_distribution<>(5.0/6.0)), 100000, 0, 10,
  //          "geometric(5/6)");
  //histogram(make_gen(rng, exponential_distribution<>(0.3)), 100000, 0, 10,
  //          "exponential(0.3)");
  //histogram(make_gen(rng, cauchy_distribution<>()), 100000, -5, 5,
  //          "cauchy");
  //histogram(make_gen(rng, lognormal_distribution<>(3, 2)), 100000, 0, 10,
  //          "lognormal");
  histogram(make_gen(rng, normal_distribution<>()), 1000000, -3, 3,
            "normal");
  histogram(make_gen(rng, normal_distribution<>(0.5, 0.5)), 1000000, -3, 3,
            "normal(0.5, 0.5)");
  //histogram(make_gen(rng, poisson_distribution<>(1.5)), 100000, 0, 5,
  //          "poisson(1.5)");
  //histogram(make_gen(rng, poisson_distribution<>(10)), 100000, 0, 20,
  //          "poisson(10)");
  //histogram(make_gen(rng, gamma_distribution<>(0.5)), 100000, 0, 0.5,
  //          "gamma(0.5)");
  //histogram(make_gen(rng, gamma_distribution<>(1)), 100000, 0, 3,
  //          "gamma(1)");
  //histogram(make_gen(rng, gamma_distribution<>(2)), 100000, 0, 6,
  //          "gamma(2)");
}

void simple_test()
{
  typedef qmcplusplus::RandomGenerator_t generator_type;
  typedef generator_type::uint_type uint_type;
  generator_type uni;
  uni.seed(static_cast<uint_type>(std::time(0))%1024);
  //copy a generator and seed it by random number from the original generator
  generator_type uni_copy(uni);
  uni_copy.seed(static_cast<uint_type>(uni()*1000));
  for(int i=0; i<20; i++)
    std::cout << uni() << " " << uni_copy() << std::endl;
  //check serial histogram
  histogram(uni, 1000000, -0.5, 1.5, "uniform_real(0,1)");
  //test PrimeNumberSet class
  PrimeNumberSet<uint_type> primes;
  std::vector<uint_type> myprimes;
  //use a random number as an offset
  int n=primes.get(static_cast<uint_type>(std::time(0))%1024,omp_get_max_threads(),myprimes);
  //int n=primes.get(10000,myprimes,5);
  histogram_OMP(uni, 100000, -0.5, 1.5, myprimes, "uniform_real(0,1)");
  for(int i=0; i<myprimes.size(); i++)
    std::cout << i << std::setw(12) << myprimes[i] << std::endl;
}

int main(int argc, char** argv)
{
  //histograms<boost::mt19937>();
  //histograms<boost::lagged_fibonacci607>();
  OHMMS::Controller->initialize(argc,argv);
  OhmmsInfo Welcome(argc,argv,OHMMS::Controller->rank());
  RandomNumberControl rng;
  rng.put(NULL);
  //check serial histogram
  histogram(Random, 10000, -0.5, 1.5, "uniform_real(0,1)");
  std::vector<uint32_t> myprimes;
  //rng.PrimeNumbers.get(Random.offset()+omp_get_max_threads(),omp_get_max_threads(),myprimes);
  rng.PrimeNumbers.get(1024,512,myprimes);
  std::cout <<"============================  " << endl;
  for(int i=0; i<myprimes.size();)
  {
    for(int j=0; j<8; j++, i++)
      std::cout << std::setw(12) << myprimes[i];
    std::cout << std::endl;
  }
  histogram_OMP(Random, 10000, -0.5, 1.5, myprimes, "uniform_real(0,1)");
  std::cout <<"============================  " << endl;
  int imax=8*(rng.PrimeNumbers.size()/8);
  for(int i=0; i<imax;)
  {
    for(int j=0; j<8; j++, i++)
      std::cout << std::setw(12) << rng.PrimeNumbers[i];
    std::cout << std::endl;
  }
  //std::stringstream a;
  //Random.write(a);
  //cout << "Size of string " << a.str().size() << endl;
  //vector<uint32_t> v;
  //uint32_t vt;
  //while(!a.eof())
  //{
  //  if(a>>vt) v.push_back(vt);
  //}
  //for(int i=0; i<v.size(); i++) cout << v[i] << endl;
  //Random.write(cout);
  //cout << endl;
  //cout << " size of data " << v.size() << endl;
  OHMMS::Controller->finalize();
  return 0;
}


#include <big_types.h>
#include <big_generate_random.h>
#include <big_diag.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(big_diag);

BOOST_AUTO_TEST_CASE(random_test_case)
{
  typedef ublas::compressed_matrix<double> matrix_type;
  typedef ublas::vector<double>            vector_type;

  for(size_t tst=0u;tst<5u;++tst)
  {
    matrix_type D;
    vector_type v;

    big::generate_random(10, v);
    big::diag(v,D);

    BOOST_CHECK( D.size1() == 10 );
    BOOST_CHECK( D.size2() == 10 );

    for(size_t i=0u;i<10u;++i)
    {
      for(size_t j=0u;j<10u;++j)
      {
        if(i==j)
          BOOST_CHECK( D(i,i) == v(i) );
        else
          BOOST_CHECK( D(i,j) == 0.0 );
      }
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef SM_POINT_TEST_HARNESS_HPP
#define SM_POINT_TEST_HARNESS_HPP

#include <sm/eigen/gtest.hpp>
#include <boost/bind.hpp>
#include <sm/eigen/NumericalDiff.hpp>
#include <sm/boost/serialization.hpp>

namespace sm {

  template<typename POINT_T>
  class PointTestHarness
  {
  public:
    typedef POINT_T point_t;
    double _threshold;
    PointTestHarness(double nearnessThreshold = 1e-10) : _threshold(nearnessThreshold) {}

    void testAdd(double threshold = -1)
    {
	  SCOPED_TRACE(__FUNCTION__);
      if(threshold < 0)
	threshold = _threshold;

      Eigen::Vector3d p1;
      Eigen::Vector3d p2;
      Eigen::Vector3d answer;
      for(int i = 0; i < 20; i++)
	{
	  p1.setRandom();
	  p2.setRandom();
	  answer = p1+p2;

	  point_t P1(p1);
	  point_t P2(p2);

	  point_t Answer = P1 + P2;

	  sm::eigen::assertNear(Answer.toEuclidean(), answer, threshold, SM_SOURCE_FILE_POS);
	}
    }

    void testSubtract(double threshold = -1)
    {
	  SCOPED_TRACE(__FUNCTION__);
      if(threshold < 0)
	threshold = _threshold;

      Eigen::Vector3d p1;
      Eigen::Vector3d p2;
      Eigen::Vector3d answer;
      for(int i = 0; i < 20; i++)
	{
	  p1.setRandom();
	  p2.setRandom();
	  answer = p1-p2;

	  point_t P1(p1);
	  point_t P2(p2);

	  point_t Answer = P1 - P2;

	  sm::eigen::assertNear(Answer.toEuclidean(), answer, threshold, SM_SOURCE_FILE_POS);
	}
    }

    void testAssign3(double threshold = -1)
    {
	  SCOPED_TRACE(__FUNCTION__);
      if(threshold < 0)
	threshold = _threshold;

      Eigen::Vector3d p1;
      for(int i = 0; i < 20; i++)
	{
	  p1.setRandom();

	  point_t P1;

	  P1 = p1;

	  sm::eigen::assertNear(P1.toEuclidean(), p1, threshold, SM_SOURCE_FILE_POS);
	}

    }

    void testAssign4(double threshold = -1.0)
    {
	  SCOPED_TRACE(__FUNCTION__);
      if(threshold < 0)
	threshold = _threshold;

      Eigen::Vector4d p1;
      for(int i = 0; i < 20; i++)
	{
	  p1.setRandom();

	  point_t P1;

	  P1 = p1;

	  Eigen::Vector4d p1a = p1/p1.norm();
	  Eigen::Vector4d P1a = P1.toHomogeneous() / P1.toHomogeneous().norm();

	  sm::eigen::assertNear(p1a, P1a, threshold, SM_SOURCE_FILE_POS);
	}

    }

    struct EuclideanJacobianFunctor
    {
      typedef Eigen::Vector3d value_t;
      typedef double scalar_t;
      typedef Eigen::Vector3d input_t;
      typedef Eigen::Matrix3d jacobian_t;

      point_t p;
      EuclideanJacobianFunctor(point_t p) : p(p) {}

      Eigen::Vector3d update(Eigen::Vector3d pp, int dimension, double dpd)
      {
	pp[dimension] += dpd;
	return pp;
      }

      value_t operator()(const Eigen::Vector3d & dp)
      {
	point_t j = p;
	j.oplus(dp);
	return j.toEuclidean();
      }

    };

    void testToEuclideanJacobian(double threshold = -1)
    {
	  SCOPED_TRACE(__FUNCTION__);
      if(threshold < 0)
	threshold = _threshold;

      for(int i = 0; i < 10; i++)
	{
	  point_t P;
	  P.setRandom();
	  EuclideanJacobianFunctor functor(P);
	  sm::eigen::NumericalDiff<EuclideanJacobianFunctor> nd(functor);
	  Eigen::Matrix3d Jest = nd.estimateJacobian(Eigen::Vector3d::Zero());
	  Eigen::Matrix3d J;
	  P.toEuclideanAndJacobian(J);

	  sm::eigen::assertNear(J,Jest, threshold, SM_SOURCE_FILE_POS);
	}

    }

    struct HomogeneousJacobianFunctor
    {

      typedef Eigen::Vector4d value_t;
      typedef double scalar_t;
      typedef Eigen::Vector3d input_t;
      typedef Eigen::Matrix<double,4,3> jacobian_t;

      point_t p;
      HomogeneousJacobianFunctor(point_t p) : p(p) {}

      Eigen::Vector3d update(Eigen::Vector3d pp, int dimension, double dpd)
      {
	pp[dimension] += dpd;
	return pp;
      }

      value_t operator()(const Eigen::Vector3d & dp)
      {
	point_t j = p;
	j.oplus(dp);
	return j.toHomogeneous();
      }

    };

    void testToHomogeneousJacobian(double threshold = -1)
    {
	  SCOPED_TRACE(__FUNCTION__);
      if(threshold < 0)
	threshold = _threshold;

      for(int i = 0; i < 10; i++)
	{
	  point_t P;
	  P.setRandom();
	  HomogeneousJacobianFunctor functor(P);
	  sm::eigen::NumericalDiff<HomogeneousJacobianFunctor> nd(functor);
	  Eigen::Matrix<double,4,3> Jest = nd.estimateJacobian(Eigen::Vector3d::Zero());
	  Eigen::Matrix<double,4,3> J;
	  P.toHomogeneousAndJacobian(J);

	  sm::eigen::assertNear(J,Jest, threshold, SM_SOURCE_FILE_POS);
	}

    }

	void testSerialization()
	{
	  SCOPED_TRACE(__FUNCTION__);

	  point_t p1;
	  p1.setRandom();

	  sm::boost_serialization::save(p1, "test.ba");

	  ASSERT_TRUE(p1.isBinaryEqual(p1));

	  point_t p2;

	  ASSERT_FALSE(p1.isBinaryEqual(p2));
	  ASSERT_FALSE(p2.isBinaryEqual(p1));

	  sm::boost_serialization::load(p2, "test.ba");

	  ASSERT_TRUE(p1.isBinaryEqual(p2));
	  ASSERT_TRUE(p2.isBinaryEqual(p1));

	  p1.setRandom();

	  ASSERT_FALSE(p1.isBinaryEqual(p2));
	  ASSERT_FALSE(p2.isBinaryEqual(p1));

	  sm::boost_serialization::save_xml(p1, "Point", "test.xml");

	  sm::boost_serialization::load_xml(p2, "Point", "test.xml");

	  // Too strict.
	  //ASSERT_TRUE(p1.isBinaryEqual(p2));
	  //ASSERT_TRUE(p2.isBinaryEqual(p1));

	}

    void testAll()
    {
      testAdd();
      testSubtract();
      testAssign3();
      testAssign4();
      testToEuclideanJacobian();
      testToHomogeneousJacobian();
	  testSerialization();
    }

  };

} // namespace sm

#endif /* SM_POINT_TEST_HARNESS_HPP */


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the krls object
    from the dlib C++ Library.

    The krls object allows you to perform online regression.  This
    example will train an instance of it on the sinc function.

*/

#include <iostream>
#include <vector>

#include <dlib/svm.h>

using namespace std;
using namespace dlib;

// Here is the sinc function we will be trying to learn with the krls
// object.
double sinc(double x)
{
    if (x == 0)
        return 1;
    return sin(x)/x;
}

int main()
{
    // Here we declare that our samples will be 1 dimensional column vectors.  In general,
    // you can use N dimensional vectors as inputs to the krls object.  But here we only
    // have 1 dimension to make the example simple.  (Note that if you don't know the
    // dimensionality of your vectors at compile time you can change the first number to
    // a 0 and then set the size at runtime)
    typedef matrix<double,1,1> sample_type;

    // Now we are making a typedef for the kind of kernel we want to use.  I picked the
    // radial basis kernel because it only has one parameter and generally gives good
    // results without much fiddling.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Here we declare an instance of the krls object.  The first argument to the constructor
    // is the kernel we wish to use.  The second is a parameter that determines the numerical
    // accuracy with which the object will perform part of the regression algorithm.  Generally
    // smaller values give better results but cause the algorithm to run slower.  You just have
    // to play with it to decide what balance of speed and accuracy is right for your problem.
    // Here we have set it to 0.001.
    krls<kernel_type> test(kernel_type(0.1),0.001);

    // now we train our object on a few samples of the sinc function.
    sample_type m;
    for (double x = -10; x <= 4; x += 1)
    {
        m(0) = x;
        test.train(m, sinc(x));
    }

    // now we output the value of the sinc function for a few test points as well as the
    // value predicted by krls object.
    m(0) = 2.5; cout << sinc(m(0)) << "   " << test(m) << endl;
    m(0) = 0.1; cout << sinc(m(0)) << "   " << test(m) << endl;
    m(0) = -4;  cout << sinc(m(0)) << "   " << test(m) << endl;
    m(0) = 5.0; cout << sinc(m(0)) << "   " << test(m) << endl;

    // The output is as follows:
    // 0.239389   0.239362
    // 0.998334   0.998333
    // -0.189201   -0.189201
    // -0.191785   -0.197267

    // The first column is the true value of the sinc function and the second
    // column is the output from the krls estimate.

    // Another thing that is worth knowing is that just about everything in dlib is serializable.
    // So for example, you can save the test object to disk and recall it later like so:
    ofstream fout("saved_krls_object.dat",ios::binary);
    serialize(test,fout);
    fout.close();

    // now lets open that file back up and load the krls object it contains
    ifstream fin("saved_krls_object.dat",ios::binary);
    deserialize(test, fin);

    // If you don't want to save the whole krls object (it might be a bit large)
    // you can save just the decision function it has learned so far.  You can get
    // the decision function out of it by calling test.get_decision_function() and
    // then you can serialize that object instead.  E.g.
    decision_function<kernel_type> funct = test.get_decision_function();
    fout.open("saved_krls_function.dat",ios::binary);
    serialize(funct, fout);
}