#pragma once
#ifndef SFMTAVXDC_HPP
#define SFMTAVXDC_HPP
/**
 * @file SFMTAVXdc.hpp
 */
#include "devavxprng.h"
#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
#include <MTToolBox/AlgorithmCalculateParity.hpp>
#include <MTToolBox/AlgorithmEquidistribution.hpp>
#include <MTToolBox/MersenneTwister.hpp>
#include <NTL/GF2X.h>
#include "AlgorithmSIMDEquidistribution.hpp"
#include "Annihilate.hpp"
#include "DCOptions.hpp"

namespace MTToolBox {
    /**
     * search parameters using all_in_one function in the file search_all.hpp
     * @param opt command line options
     * @param count number of parameters user requested
     * @return 0 if this ends normally
     */
    template<typename U, typename G, int bitWidth>
    int sfmtavx_search(DCOptions& opt, int count) {
        using namespace std;
        using namespace NTL;
        MersenneTwister mt(opt.seed);
        G g(opt.mexp);

        if (opt.fixedL) {
            g.setFixedSL1(opt.fixedSL1);
        }
        if (opt.fixedR) {
            g.setFixedSR1(opt.fixedSR1);
        }
        if (opt.fixedP) {
            g.setFixedPerm(opt.fixedPerm);
        }
        cout << "seed = " << dec << opt.seed << endl;
        if (opt.verbose) {
            time_t t = time(NULL);
            cout << "search start at " << ctime(&t);
        }
        AlgorithmReducibleRecursionSearch<U> ars(g, mt);
        int i = 0;
        AlgorithmCalculateParity<U, G> cp;
        Annihilate<G, U> annihilate;
        cout << "# " << g.getHeaderString() << ", delta32, delta64"
             << endl;
        while (i < count) {
            if (ars.start(opt.mexp * 100)) {
                GF2X irreducible = ars.getIrreducibleFactor();
                GF2X characteristic = ars.getCharacteristicPolynomial();
                GF2X quotient = characteristic / irreducible;
                if (deg(irreducible) != opt.mexp) {
                    cout << "error not erreducible" << endl;
                    return -1;
                }
#if 0
                cout << "before parity" << endl;
#endif
                cp.searchParity(g, irreducible);
                U seed;
                setBitOfPos(&seed, 0, 1);
                g.seed(seed);
#if 0
                cout << "before annihilate" << endl;
#endif
                if (!annihilate.anni(g)) {
                    cout << "error can't annihilate" << endl;
                    return -1;
                }
                int delta32 = 0;
                int delta64 = 0;
                int veq32[32];
                SIMDInfo info;
                info.bitMode = 32;
                info.elementNo = bitWidth / 32;
                info.bitSize = bitWidth;
                info.fastMode = true;
#if 0
                cout << "bitMode:" << dec << info.bitMode << endl;
                cout << "elementNo:" << dec << info.elementNo << endl;
                cout << "bitSize:" << dec << info.bitSize << endl;
                cout << "fastmode:" << info.fastMode << endl;
#endif
                delta32
                    = calc_SIMD_equidistribution<U, G>
                    (g, veq32, 32, info, opt.mexp);
                int veq64[64];
                info.bitMode = 64;
                info.elementNo = bitWidth / 64;
#if 0
                cout << "bitMode:" << dec << info.bitMode << endl;
                cout << "elementNo:" << dec << info.elementNo << endl;
                cout << "bitSize:" << dec << info.bitSize << endl;
                cout << "fastmode:" << info.fastMode << endl;
#endif
                delta64
                    = calc_SIMD_equidistribution<U, G>
                    (g, veq64, 64, info, opt.mexp);
                cout << g.getParamString();
                cout << dec << delta32 << "," << delta64;
                cout << "," << dec << veq64[63] << endl;
                i++;
            } else {
                cout << "search failed" << endl;
                break;
            }
        }
        if (opt.verbose) {
            time_t t = time(NULL);
            cout << "search end at " << ctime(&t) << endl;
        }
        return 0;
    }

}
#endif // SFMTAVXDC_HPP


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 0x01.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#define NT2_UNIT_MODULE "nt2::memory::slice"

#include <boost/array.hpp>
#include <boost/mpl/vector_c.hpp>
#include <nt2/sdk/memory/slice.hpp>
#include <nt2/sdk/memory/align_on.hpp>
#include <boost/fusion/adapted/mpl.hpp>
#include <nt2/sdk/memory/no_padding.hpp>
#include <boost/fusion/adapted/array.hpp>
#include <nt2/sdk/memory/lead_padding.hpp>
#include <nt2/sdk/memory/global_padding.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test for slice with no_padding
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(slice_no_padding)
{
  using nt2::slice;
  using boost::array;
  using boost::mpl::vector_c;
  using nt2::memory::no_padding;

  no_padding p;

  vector_c<int,9,7,5,3> ss4;
  vector_c<int,7,5,3>   ss3;
  vector_c<int,5,3>     ss2;
  vector_c<int,3>       ss1;

  array<int,4> ds4 = {{9,7,5,3}};
  array<int,3> ds3 = {{7,5,3}};
  array<int,2> ds2 = {{5,3}};
  array<int,1> ds1 = {{3}};

  NT2_TEST_EQUAL( (slice<1>(ds4,p)), 9*7*5*3 );
  NT2_TEST_EQUAL( (slice<2>(ds4,p)),   7*5*3 );
  NT2_TEST_EQUAL( (slice<1>(ds3,p)),   7*5*3 );
  NT2_TEST_EQUAL( (slice<3>(ds4,p)),     5*3 );
  NT2_TEST_EQUAL( (slice<2>(ds3,p)),     5*3 );
  NT2_TEST_EQUAL( (slice<1>(ds2,p)),     5*3 );
  NT2_TEST_EQUAL( (slice<4>(ds4,p)),       3 );
  NT2_TEST_EQUAL( (slice<3>(ds3,p)),       3 );
  NT2_TEST_EQUAL( (slice<2>(ds2,p)),       3 );
  NT2_TEST_EQUAL( (slice<1>(ds1,p)),       3 );

  NT2_TEST_EQUAL( int(slice<1>(ss4,p)), 9*7*5*3 );
  NT2_TEST_EQUAL( int(slice<2>(ss4,p)),   7*5*3 );
  NT2_TEST_EQUAL( int(slice<1>(ss3,p)),   7*5*3 );
  NT2_TEST_EQUAL( int(slice<3>(ss4,p)),     5*3 );
  NT2_TEST_EQUAL( int(slice<2>(ss3,p)),     5*3 );
  NT2_TEST_EQUAL( int(slice<1>(ss2,p)),     5*3 );
  NT2_TEST_EQUAL( int(slice<4>(ss4,p)),       3 );
  NT2_TEST_EQUAL( int(slice<3>(ss3,p)),       3 );
  NT2_TEST_EQUAL( int(slice<2>(ss2,p)),       3 );
  NT2_TEST_EQUAL( int(slice<1>(ss1,p)),       3 );
}

////////////////////////////////////////////////////////////////////////////////
// Test for slice with global_padding
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(slice_global_padding)
{
  using nt2::slice;
  using boost::array;
  using nt2::memory::align_on;
  using boost::mpl::vector_c;
  using nt2::memory::global_padding;

  global_padding p;

  vector_c<int,9,7,5,3> ss4;
  vector_c<int,7,5,3>   ss3;
  vector_c<int,5,3>     ss2;
  vector_c<int,3>       ss1;

  array<int,4> ds4 = {{9,7,5,3}};
  array<int,3> ds3 = {{7,5,3}};
  array<int,2> ds2 = {{5,3}};
  array<int,1> ds1 = {{3}};

  NT2_TEST_EQUAL( (slice<1>(ds4,p)), align_on(9*7*5*3) );
  NT2_TEST_EQUAL( (slice<2>(ds4,p)),             7*5*3 );
  NT2_TEST_EQUAL( (slice<1>(ds3,p)),   align_on(7*5*3) );
  NT2_TEST_EQUAL( (slice<3>(ds4,p)),               5*3 );
  NT2_TEST_EQUAL( (slice<2>(ds3,p)),               5*3 );
  NT2_TEST_EQUAL( (slice<1>(ds2,p)),     align_on(5*3) );
  NT2_TEST_EQUAL( (slice<4>(ds4,p)),                 3 );
  NT2_TEST_EQUAL( (slice<3>(ds3,p)),                 3 );
  NT2_TEST_EQUAL( (slice<2>(ds2,p)),                 3 );
  NT2_TEST_EQUAL( (slice<1>(ds1,p)),       align_on(3) );

  NT2_TEST_EQUAL( int(slice<1>(ss4,p)), align_on(9*7*5*3) );
  NT2_TEST_EQUAL( int(slice<2>(ss4,p)),             7*5*3 );
  NT2_TEST_EQUAL( int(slice<1>(ss3,p)),   align_on(7*5*3) );
  NT2_TEST_EQUAL( int(slice<3>(ss4,p)),               5*3 );
  NT2_TEST_EQUAL( int(slice<2>(ss3,p)),               5*3 );
  NT2_TEST_EQUAL( int(slice<1>(ss2,p)),     align_on(5*3) );
  NT2_TEST_EQUAL( int(slice<4>(ss4,p)),                 3 );
  NT2_TEST_EQUAL( int(slice<3>(ss3,p)),                 3 );
  NT2_TEST_EQUAL( int(slice<2>(ss2,p)),                 3 );
  NT2_TEST_EQUAL( int(slice<1>(ss1,p)),       align_on(3) );
}

////////////////////////////////////////////////////////////////////////////////
// Test for slice with lead_padding
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(slice_lead_padding)
{
  using nt2::slice;
  using boost::array;
  using nt2::memory::align_on;
  using boost::mpl::vector_c;
  using nt2::memory::lead_padding;

  lead_padding p;

  vector_c<int,9,7,5,3> ss4;
  vector_c<int,7,5,3>   ss3;
  vector_c<int,5,3>     ss2;
  vector_c<int,3>       ss1;

  array<int,4> ds4 = {{9,7,5,3}};
  array<int,3> ds3 = {{7,5,3}};
  array<int,2> ds2 = {{5,3}};
  array<int,1> ds1 = {{3}};

  NT2_TEST_EQUAL( (slice<1>(ds4,p)), align_on(9)*7*5*3 );
  NT2_TEST_EQUAL( (slice<2>(ds4,p)),             7*5*3 );
  NT2_TEST_EQUAL( (slice<1>(ds3,p)),   align_on(7)*5*3 );
  NT2_TEST_EQUAL( (slice<3>(ds4,p)),               5*3 );
  NT2_TEST_EQUAL( (slice<2>(ds3,p)),               5*3 );
  NT2_TEST_EQUAL( (slice<1>(ds2,p)),     align_on(5)*3 );
  NT2_TEST_EQUAL( (slice<4>(ds4,p)),                 3 );
  NT2_TEST_EQUAL( (slice<3>(ds3,p)),                 3 );
  NT2_TEST_EQUAL( (slice<2>(ds2,p)),                 3 );
  NT2_TEST_EQUAL( (slice<1>(ds1,p)),       align_on(3) );

  NT2_TEST_EQUAL( int(slice<1>(ss4,p)), align_on(9)*7*5*3 );
  NT2_TEST_EQUAL( int(slice<2>(ss4,p)),             7*5*3 );
  NT2_TEST_EQUAL( int(slice<1>(ss3,p)),   align_on(7)*5*3 );
  NT2_TEST_EQUAL( int(slice<3>(ss4,p)),               5*3 );
  NT2_TEST_EQUAL( int(slice<2>(ss3,p)),               5*3 );
  NT2_TEST_EQUAL( int(slice<1>(ss2,p)),     align_on(5)*3 );
  NT2_TEST_EQUAL( int(slice<4>(ss4,p)),                 3 );
  NT2_TEST_EQUAL( int(slice<3>(ss3,p)),                 3 );
  NT2_TEST_EQUAL( int(slice<2>(ss2,p)),                 3 );
  NT2_TEST_EQUAL( int(slice<1>(ss1,p)),       align_on(3) );
}


// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <Eigen/Core>
#include <gtest/gtest.h>
#include <string>
#include <tuple>

#include "SurgSim/Physics/UnitTests/EigenGtestAsserts.h"

namespace SurgSim
{
namespace Physics
{
namespace Testing
{
::testing::AssertionResult AssertMatricesEqual(const char *expected_expr,
											   const char *actual_expr,
											   const char *abs_err_expr,
											   const Eigen::MatrixXd &expected,
											   const Eigen::MatrixXd &actual,
											   double abs_err)
{
	if ((expected.rows() != actual.rows()) || (expected.cols() != actual.cols()))
	{
		return ::testing::AssertionFailure()
			<< "Sizes do not match." << std::endl
			<< "Actual: [" << actual.rows() << ", " << actual.cols() << "]" << std::endl
			<< "Expected: [" << expected.rows() << ", " << expected.cols() << "]" << std::endl;
	}

	if (!expected.isApprox(actual, abs_err))
	{
		::testing::AssertionResult result = ::testing::AssertionFailure()
			<< "Matrices are not within epsilon of each other." << std::endl
			<< "Difference: " << (actual - expected).norm() << std::endl
			<< "Epsilon: " << abs_err << std::endl;

		if (expected.cols() == 1 || expected.rows() == 1)
		{
			typedef std::tuple<int, double, double> DifferenceTuple;
			std::vector<DifferenceTuple> differences;
			for (int index = 0; index < expected.size(); ++index)
			{
				if (std::abs(expected(index) - actual(index)) > abs_err)
				{
					differences.emplace_back(index, expected(index), actual(index));
				}
			}

			result << "Total differing values: " << differences.size() << std::endl;

			for (std::vector<DifferenceTuple>::iterator it = std::begin(differences); it != std::end(differences); ++it)
			{
				result
					<< "[Index, Expected, Actual]: "
					<< std::get<0>(*it) << ", " << std::get<1>(*it)  << ", " << std::get<2>(*it)  << std::endl;
			}
		}

		return result;
	}

	return ::testing::AssertionSuccess();
}

} // namespace Testing
} // namespace Physics
} // namespace SurgSim


#include <iostream>
#include <string>
#include <boost/filesystem/fstream.hpp>
#include <boost/lexical_cast.hpp>

int main(int argc, char* argv[])
{
	if (argc != 4)
	{
		std::cout << "filterxls 16_prop.xls mwt_lower_bound mwt_upper_bound\n";
		return 0;
	}

	using std::string;
	using boost::filesystem::ifstream;
	using boost::lexical_cast;

	const double mwt_lb = lexical_cast<double>(argv[2]);
	const double mwt_ub = lexical_cast<double>(argv[3]);

	string line;
	line.reserve(200);
	ifstream in(argv[1]);
	getline(in, line); // Header line.
	std::cout << line << '\n';
	while (getline(in, line))
	{
		const double mwt = lexical_cast<double>(line.substr(13, line.find_first_of('\t', 16) - 13));
		if (!(mwt > 350)) std::cout << line << '\n';
//		if ((mwt_lb <= mwt) && (mwt < mwt_ub)) std::cout << line << '\n';
	}
	in.close();
	return 0;
}


/** A little desktop calculator based on Boost Matheval
 *
 * This little calculator can be used for testing Boost Matheval or
 * just as a neat little calculator on the command line.  It supports
 * all the functions and constants from Boost Matheval but defines no
 * symbol table or variables.  GNU readline is used to keep a history of the inputs.
 *
 * c++ -Wall -Wextra -Wpedantic -std=c++11 -I/path/to/boost_matheval/include calc.cpp -lreadline
 */
#include <csignal>
#include <cstdlib>
#include <cstring>
#include <limits>
#include <iostream>

#include <boost/lexical_cast.hpp>
#include <boost/optional.hpp>

#include <readline/readline.h>
#include <readline/history.h>

#include <matheval.hpp>

boost::optional<double> evaluate(std::string const &expr)
{
    try {
        auto res = matheval::parse(expr);
        std::cout << res << '\n';
        return res;
    } catch (std::exception const &e) {
        std::cerr << "Error: " << e.what() << '\n';
        return boost::none;
    }
}

void commandline(int argc, char *argv[])
{
    std::string expr;
    for (int i = 1; i < argc; ++i) {
        expr.append(argv[i]); // NOLINT
    }

    evaluate(expr);
}

void sigint_handler(int /*unused*/)
{
    std::cout << "\n";
    rl_on_new_line();
#if !defined(__APPLE__)
    rl_replace_line("", 0);
#endif
    rl_redisplay();
}

void interactive()
{
    std::signal(SIGINT, sigint_handler);

    std::cout << "Type [q or Q] to quit\n";

    char * input = readline("> ");
    double res = 0;

    while (input != nullptr)
    {
        std::size_t len = std::strlen(input);

        if (len != 0)
        {
            if (input[0] == 'q' || input[0] == 'Q') { // NOLINT
                break;
            }

            add_history(input);

            // Replace % by the last result
            std::string line{input, input + len}; // NOLINT
            std::size_t pos = line.find('%');
            if (pos != std::string::npos) {
                line.replace(pos, 1, boost::lexical_cast<std::string>(res));
            }

            // Update result on success
            if (boost::optional<double> ores = evaluate(line)) {
                res = *ores;
            }
        }

        std::free(input); // NOLINT
        input = readline("> ");
    }

    if (input == nullptr) { // nullptr signals EOF
        std::cout << '\n';
    }

    std::free(input); // NOLINT
}

int main(int argc, char *argv[])
{
    std::cout.precision(std::numeric_limits<double>::digits10);
    if (argc > 1) {
        commandline(argc,argv);
    } else {
        interactive();
    }
}


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

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

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

/**
 * @file testGaussianJunctionTreeB.cpp
 * @date Jul 8, 2010
 * @author nikai
 */

#include <CppUnitLite/TestHarness.h>

#if 0

#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/cholesky.h>

#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;

#include <iostream>

using namespace std;
using namespace gtsam;
using namespace example;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* *
 Bayes tree for smoother with "nested dissection" ordering:
   C1     x5 x6 x4
   C2      x3 x2 : x4
   C3        x1 : x2
   C4      x7 : x6
*/
TEST( GaussianJunctionTreeB, constructor2 )
{
  // create a graph
  Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
  GaussianFactorGraph fg = createSmoother(7, ordering).first;

  // create an ordering
  GaussianJunctionTree actual(fg);

  vector<Index> frontal1; frontal1 += ordering[X(5)], ordering[X(6)], ordering[X(4)];
  vector<Index> frontal2; frontal2 += ordering[X(3)], ordering[X(2)];
  vector<Index> frontal3; frontal3 += ordering[X(1)];
  vector<Index> frontal4; frontal4 += ordering[X(7)];
  vector<Index> sep1;
  vector<Index> sep2; sep2 += ordering[X(4)];
  vector<Index> sep3; sep3 += ordering[X(2)];
  vector<Index> sep4; sep4 += ordering[X(6)];
  EXPECT(assert_equal(frontal1, actual.root()->frontal));
  EXPECT(assert_equal(sep1,     actual.root()->separator));
  LONGS_EQUAL(5,               actual.root()->size());
  list<GaussianJunctionTree::sharedClique>::const_iterator child0it = actual.root()->children().begin();
  list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
  GaussianJunctionTree::sharedClique child0 = *child0it;
  GaussianJunctionTree::sharedClique child1 = *child1it;
  EXPECT(assert_equal(frontal2, child0->frontal));
  EXPECT(assert_equal(sep2,     child0->separator));
  LONGS_EQUAL(4,               child0->size());
  EXPECT(assert_equal(frontal3, child0->children().front()->frontal));
  EXPECT(assert_equal(sep3,     child0->children().front()->separator));
  LONGS_EQUAL(2,               child0->children().front()->size());
  EXPECT(assert_equal(frontal4, child1->frontal));
  EXPECT(assert_equal(sep4,     child1->separator));
  LONGS_EQUAL(2,               child1->size());
}

/* ************************************************************************* */
TEST( GaussianJunctionTreeB, optimizeMultiFrontal )
{
  // create a graph
  GaussianFactorGraph fg;
  Ordering ordering;
  boost::tie(fg,ordering) = createSmoother(7);

  // optimize the graph
  GaussianJunctionTree tree(fg);
  VectorValues actual = tree.optimize(&EliminateQR);

  // verify
  VectorValues expected(vector<size_t>(7,2)); // expected solution
  Vector v = (Vector(2) << 0., 0.);
  for (int i=1; i<=7; i++)
    expected[ordering[X(i)]] = v;
  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( GaussianJunctionTreeB, optimizeMultiFrontal2)
{
  // create a graph
  example::Graph nlfg = createNonlinearFactorGraph();
  Values noisy = createNoisyValues();
  Ordering ordering; ordering += X(1),X(2),L(1);
  GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering);

  // optimize the graph
  GaussianJunctionTree tree(fg);
  VectorValues actual = tree.optimize(&EliminateQR);

  // verify
  VectorValues expected = createCorrectDelta(ordering); // expected solution
  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST(GaussianJunctionTreeB, slamlike) {
  Values init;
  NonlinearFactorGraph newfactors;
  NonlinearFactorGraph fullgraph;
  SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0));
  SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1));

  size_t i = 0;

  newfactors = NonlinearFactorGraph();
  newfactors.add(PriorFactor<Pose2>(X(0), Pose2(0.0, 0.0, 0.0), odoNoise));
  init.insert(X(0), Pose2(0.01, 0.01, 0.01));
  fullgraph.push_back(newfactors);

  for( ; i<5; ++i) {
    newfactors = NonlinearFactorGraph();
    newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
    init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
    fullgraph.push_back(newfactors);
  }

  newfactors = NonlinearFactorGraph();
  newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
  newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise));
  newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise));
  init.insert(X(i+1), Pose2(1.01, 0.01, 0.01));
  init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
  init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
  fullgraph.push_back(newfactors);
  ++ i;

  for( ; i<5; ++i) {
    newfactors = NonlinearFactorGraph();
    newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
    init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
    fullgraph.push_back(newfactors);
  }

  newfactors = NonlinearFactorGraph();
  newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
  newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise));
  newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise));
  init.insert(X(i+1), Pose2(6.9, 0.1, 0.01));
  fullgraph.push_back(newfactors);
  ++ i;

  // Compare solutions
  Ordering ordering = *fullgraph.orderingCOLAMD(init);
  GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering);

  GaussianJunctionTree gjt(linearized);
  VectorValues deltaactual = gjt.optimize(&EliminateQR);
  Values actual = init.retract(deltaactual, ordering);

  GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
  VectorValues delta = optimize(gbn);
  Values expected = init.retract(delta, ordering);

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianJunctionTreeB, simpleMarginal) {

  typedef BayesTree<GaussianConditional> GaussianBayesTree;

  // Create a simple graph
  NonlinearFactorGraph fg;
  fg.add(PriorFactor<Pose2>(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)));
  fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0))));

  Values init;
  init.insert(X(0), Pose2());
  init.insert(X(1), Pose2(1.0, 0.0, 0.0));

  Ordering ordering;
  ordering += X(1), X(0);

  GaussianFactorGraph gfg = *fg.linearize(init, ordering);

  // Compute marginals with both sequential and multifrontal
  Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1);

  Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1);

  // Compute marginal directly from marginal factor
  GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1);
  JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
  Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));

  // Compute marginal directly from BayesTree
  GaussianBayesTree gbt;
  gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky));
  marginalFactor = gbt.marginalFactor(1, EliminateCholesky);
  marginalJacobian = boost::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
  Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));

  EXPECT(assert_equal(expected, actual1));
  EXPECT(assert_equal(expected, actual2));
  EXPECT(assert_equal(expected, actual3));
}

#endif

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


// Boost.Geometry Index
// Additional tests

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

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

#include <iostream>

#define BOOST_GEOMETRY_INDEX_DETAIL_EXPERIMENTAL

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <boost/foreach.hpp>
#include <boost/random.hpp>

int main()
{
    namespace bg = boost::geometry;
    namespace bgi = bg::index;

    size_t values_count = 1000000;
    size_t queries_count = 10000;
    size_t nearest_queries_count = 10000;
    unsigned neighbours_count = 10;

    std::vector< std::pair<float, float> > coords;

    //randomize values
    {
        boost::mt19937 rng;
        //rng.seed(static_cast<unsigned int>(std::time(0)));
        float max_val = static_cast<float>(values_count / 2);
        boost::uniform_real<float> range(-max_val, max_val);
        boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rnd(rng, range);

        coords.reserve(values_count);

        std::cout << "randomizing data\n";
        for ( size_t i = 0 ; i < values_count ; ++i )
        {
            coords.push_back(std::make_pair(rnd(), rnd()));
        }
        std::cout << "randomized\n";
    }

    typedef bg::model::point<double, 2, bg::cs::cartesian> P;
    typedef bg::model::box<P> B;
    typedef bgi::rtree<B, bgi::linear<16, 4> > RT;
    //typedef bgi::rtree<B, bgi::quadratic<8, 3> > RT;
    //typedef bgi::rtree<B, bgi::rstar<8, 3> > RT;

    std::cout << "sizeof rtree: " << sizeof(RT) << std::endl;

    {
        RT t;

        // inserting
        {
            for (size_t i = 0 ; i < values_count ; ++i )
            {
                float x = coords[i].first;
                float y = coords[i].second;
                B b(P(x - 0.5f, y - 0.5f), P(x + 0.5f, y + 0.5f));

                t.insert(b);
            }
            std::cout << "inserted values: " << values_count << '\n';
        }

        std::vector<B> result;
        result.reserve(100);

        // test
        std::vector<size_t> spatial_query_data;
        size_t spatial_query_index = 0;

        {
            size_t found_count = 0;
            for (size_t i = 0 ; i < queries_count ; ++i )
            {
                float x = coords[i].first;
                float y = coords[i].second;
                result.clear();
                t.query(bgi::intersects(B(P(x - 10, y - 10), P(x + 10, y + 10))), std::back_inserter(result));

                // test
                spatial_query_data.push_back(result.size());
                found_count += result.size();
            }
            std::cout << "spatial queries found: " << found_count << '\n';
        }

#ifdef BOOST_GEOMETRY_INDEX_DETAIL_EXPERIMENTAL
        {
            size_t found_count = 0;
            for (size_t i = 0 ; i < queries_count ; ++i )
            {
                float x = coords[i].first;
                float y = coords[i].second;
                result.clear();
                std::copy(t.qbegin_(bgi::intersects(B(P(x - 10, y - 10), P(x + 10, y + 10)))),
                          t.qend_(bgi::intersects(B(P(x - 10, y - 10), P(x + 10, y + 10)))),
                          std::back_inserter(result));

                // test
                if ( spatial_query_data[spatial_query_index] != result.size() )
                    std::cout << "Spatial query error - should be: " << spatial_query_data[spatial_query_index] << ", is: " << result.size() << '\n';
                ++spatial_query_index;
                found_count += result.size();
            }
            std::cout << "incremental spatial queries found: " << found_count << '\n';
        }
#endif

        // test
        std::vector<float> nearest_query_data;
        size_t nearest_query_data_index = 0;

        {
            size_t found_count = 0;
            for (size_t i = 0 ; i < nearest_queries_count ; ++i )
            {
                float x = coords[i].first + 100;
                float y = coords[i].second + 100;
                result.clear();
                t.query(bgi::nearest(P(x, y), neighbours_count), std::back_inserter(result));

                // test
                {
                    float max_dist = 0;
                    BOOST_FOREACH(B const& b, result)
                    {
                        float curr_dist = bgi::detail::comparable_distance_near(P(x, y), b);
                        if ( max_dist < curr_dist )
                            max_dist = curr_dist;
                    }
                    nearest_query_data.push_back(max_dist);
                }
                found_count += result.size();
            }
            std::cout << "nearest queries found: " << found_count << '\n';
        }

#ifdef BOOST_GEOMETRY_INDEX_DETAIL_EXPERIMENTAL
        {
            size_t found_count = 0;
            for (size_t i = 0 ; i < nearest_queries_count ; ++i )
            {
                float x = coords[i].first + 100;
                float y = coords[i].second + 100;
                result.clear();

                std::copy(t.qbegin_(bgi::nearest(P(x, y), neighbours_count)),
                          t.qend_(bgi::nearest(P(x, y), neighbours_count)),
                          std::back_inserter(result));

                // test
                {
                    float max_dist = 0;
                    BOOST_FOREACH(B const& b, result)
                    {
                        float curr_dist = bgi::detail::comparable_distance_near(P(x, y), b);
                        if ( max_dist < curr_dist )
                            max_dist = curr_dist;
                    }
                    if ( nearest_query_data_index < nearest_query_data.size() &&
                         nearest_query_data[nearest_query_data_index] != max_dist )
                         std::cout << "Max distance error - should be: " << nearest_query_data[nearest_query_data_index] << ", and is: " << max_dist << "\n";
                    ++nearest_query_data_index;
                }
                found_count += result.size();
            }
            std::cout << "incremental nearest queries found: " << found_count << '\n';
        }
#endif

        std::cout << "finished\n";
    }

    return 0;
}


#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/list.hpp>

#include <pyublas/numpy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
using namespace boost::numeric::ublas;

#include "statespace.hpp"

class PyCSSSM  {
public:

  PyCSSSM(const pyublas::numpy_vector<int> & start_idxs, const pyublas::numpy_vector<int> & end_idxs,
	  const pyublas::numpy_vector<int> & identities, const pyublas::numpy_matrix<double> & basis_prototypes,
	  const pyublas::numpy_vector<double> & coef_means, const pyublas::numpy_vector<double> & coef_vars,
	  const double obs_noise, const double bias) {

    /* Warning: "Indexed access to numpy_vector is much slower than iterator access."
     * For speed purposes, may be worth copying the numpy vectors into "standard" ublas vectors,
     * unless I want to go through and make sure we're using iterators everywhere possible. */

    sidx.resize(start_idxs.size());
    sidx.assign(start_idxs);

    eidx.resize(end_idxs.size());
    eidx.assign(end_idxs);

    ids.resize(identities.size());
    ids.assign(identities);

    bp.resize(basis_prototypes.size1(), basis_prototypes.size2());
    bp.assign(basis_prototypes);

    cm.resize(coef_means.size());
    cm.assign(coef_means);

    cv.resize(coef_vars.size());
    cv.assign(coef_vars);

    this->ssm = new CompactSupportSSM(sidx, eidx, ids, bp, cm, cv, obs_noise, bias);
  };

  ~PyCSSSM() {
    delete this->ssm;
  };

  double run_filter(pyublas::numpy_vector<double> z) {
    return filter_likelihood(*(this->ssm), z);
  };

  boost::python::tuple py_step_obs_likelihoods(pyublas::numpy_vector<double> z) {
    vector<double> ells(z.size());
    vector<double> preds(z.size());
    vector<double> alphas(z.size());
    step_obs_likelihoods(*(this->ssm), z, ells, preds, alphas);
    return boost::python::make_tuple(pyublas::numpy_vector<double>(ells), pyublas::numpy_vector<double>(preds), pyublas::numpy_vector<double>(alphas));
  };

  pyublas::numpy_vector<double> py_mean_obs(int n) {
    vector<double> result(n);
    mean_obs(*(this->ssm), result);
    return pyublas::numpy_vector<double>(result);
  };

  pyublas::numpy_vector<double> py_obs_var(int n) {
    vector<double> result(n);
    obs_var(*(this->ssm), result);
    return pyublas::numpy_vector<double>(result);
  };

  pyublas::numpy_vector<double> py_prior_sample(int n, unsigned long seed) {
    vector<double> result(n);
    prior_sample(*(this->ssm), result, seed);
    return pyublas::numpy_vector<double>(result);
  };

  int py_max_dimension() {
    return ssm->max_dimension;
  };

  void set_coef_prior(const pyublas::numpy_vector<double> & cmeans,
		      const pyublas::numpy_vector<double> & cvars) {
    cm.assign(cmeans);
    cv.assign(cvars);
  }

  boost::python::tuple get_coef_prior() {
    pyublas::numpy_vector<double> m(cm.size());
    pyublas::numpy_vector<double> v(cv.size());
    m.assign(cm);
    v.assign(cv);
    return boost::python::make_tuple(m,v);
  }

  StateSpaceModel *ssm;
  vector<double> cm;
  vector<double> cv;

private:

  vector<int> sidx;
  vector<int> eidx;
  vector<int> ids;
  matrix<double> bp;

};

class PyARSSM {
public:

  PyARSSM(const pyublas::numpy_vector<double> & params, double error_var,
	  const double obs_noise, const double bias) {

    p.resize(params.size());
    p.assign(params);

    this->ssm = new ARSSM(p, error_var, obs_noise, bias);
  };

  ~PyARSSM() {
    delete this->ssm;
  };

  double run_filter(pyublas::numpy_vector<double> z) {
    return filter_likelihood(*(this->ssm), z);
  };

  pyublas::numpy_vector<double> py_mean_obs(int n) {
    vector<double> result(n);
    mean_obs(*(this->ssm), result);
    return pyublas::numpy_vector<double>(result);
  };

  pyublas::numpy_vector<double> py_obs_var(int n) {
    vector<double> result(n);
    obs_var(*(this->ssm), result);
    return pyublas::numpy_vector<double>(result);
  };

  pyublas::numpy_vector<double> py_prior_sample(int n, unsigned long seed) {
    vector<double> result(n);
    prior_sample(*(this->ssm), result, seed);
    return pyublas::numpy_vector<double>(result);
  };

  void set_process(const pyublas::numpy_vector<double> & params, double error_var,
		   const double obs_noise, const double bias) {
    ARSSM * arssm = (ARSSM *)this->ssm;
    arssm->params.assign(params);
    arssm->error_var = error_var;
    arssm->obs_noise = obs_noise;
    arssm->bias = bias;
  }

  StateSpaceModel *ssm;
private:

  vector<double> p;

};

class PyTSSM  {
public:

  PyTSSM(const boost::python::list & components, double obs_noise) {

    /* TODO: figure out how to unpack components */
    /* also remember potential memory manegement issues with passed-in SSMs */

    // this->ssm = new TransientCombinedSSM(obs_noise);

    int n_components = boost::python::len(components);
    this->start_idxs.resize(n_components);
    this->end_idxs.resize(n_components);

    /* loop over the Python list of components, to build up the
     * C++ structures needed to initialize the TSSM */
    for (int i=0; i < n_components; ++i) {
      const boost::python::tuple & t = boost::python::extract<boost::python::tuple>(components[i]);

      /* If SSM is None, push a null pointer onto the list
       * of SSMs, otherwise, push an SSM pointer. */
      if (t[0]==boost::python::api::object()) {
	this->ssms.push_back(NULL);

	this->pyssms.push_back(boost::python::object());
      } else {

	boost::python::extract<PyARSSM> get_ar(t[0]);
	boost::python::extract<PyCSSSM> get_cs(t[0]);

	StateSpaceModel *ssm;
	if (get_ar.check()) {
	  ssm = get_ar().ssm;
	} else if (get_cs.check()) {
	  ssm = get_cs().ssm;
	} else {
	  PyErr_SetString(PyExc_RuntimeError, "scale array is not long enough to cover life of SSM component");
	}

	// increase the Python refcount on the SSM object, so that our
	// pointer to its internal C++ SSM will remain valid as long
	// as this TSSM is active (even if the SSM otherwise goes out
	// of scope in the Python code).
	//boost::python::object pyssm(t[0]);
	this->pyssms.push_back(t[0]);

	this->ssms.push_back(ssm);
      }

      /* the start_idx and npts integer components are easy to handle */
      int start_idx = boost::python::extract<int>(t[1]);
      unsigned int npts = boost::python::extract<int>(t[2]);
      this->start_idxs[i] = start_idx;
      this->end_idxs[i] = start_idx+npts;

      /* I hate C++ types enough that we're just going to represent the scaling vector as a (double *),
       * or NULL if we were apssed a Python None. */
      if (t[3]==boost::python::api::object()) {
	this->scales.push_back(NULL);
	//this->vec_refs.push_back(boost::python::object());
      } else {

	const pyublas::numpy_vector<double> & scale_vec = boost::python::extract<pyublas::numpy_vector<double> >(t[3]);

	if (scale_vec.size() < npts) {
	  PyErr_SetString(PyExc_RuntimeError, "scale array is not long enough to cover life of SSM component");
	}

	const double * scale_ptr = &(scale_vec(0));
	this->scales.push_back(scale_ptr);

	// Keep a reference to the Numpy array object to prevent it
	// getting garbage collected.
	//PyObject * pyvec = boost::python::object(t[3]).ptr();
	//Py_INCREF(pyvec);
	this->vec_refs.push_back(t[3]);
      }
    }

    this->ssm = new TransientCombinedSSM(this->ssms, this->start_idxs, this->end_idxs, this->scales, obs_noise);
  };

  ~PyTSSM() {

    delete this->ssm;

    // release all the Python references we've been holding
    // (SSM objects and scale vectors)
    // std::vector<PyObject *>::iterator it;
    //for (it = pyssms.begin(); it < pyssms.end(); ++it) {
    //  if (*it != NULL) Py_DECREF(*it);
    //}
    //for (it = vec_refs.begin(); it < vec_refs.end(); ++it) {
    //if (*it == NULL) continue;
    //  Py_DECREF(*it);
    //}
  };

  double run_filter(pyublas::numpy_vector<double> z) {
    return filter_likelihood(*(this->ssm), z);
  };

  boost::python::tuple py_run_filter_incremental(pyublas::numpy_vector<double> z,
						 pyublas::numpy_vector<double> ells,
						 int filter_start_idx,
						 int incr_start_idx,
						 int incr_end_idx,
						 int update_ells,
						 double step_ell_tol) {
    int errcode = 0;
    int steps_processed = 0;
    double * elldata = ells.array().data();
    double ell_delta = filter_incremental(*(this->ssm),
					  z,
					  elldata,
					  filter_start_idx,
					  incr_start_idx,
					  incr_end_idx,
					  update_ells,
					  step_ell_tol,
					  &steps_processed,
					  &errcode);

    return boost::python::make_tuple(ell_delta, errcode, steps_processed);
  };

  boost::python::tuple py_step_obs_likelihoods(pyublas::numpy_vector<double> z) {
    vector<double> ells(z.size());
    vector<double> preds(z.size());
    vector<double> alphas(z.size());
    step_obs_likelihoods(*(this->ssm), z, ells, preds, alphas);
    return boost::python::make_tuple(pyublas::numpy_vector<double>(ells), pyublas::numpy_vector<double>(preds), pyublas::numpy_vector<double>(alphas));
  };

  pyublas::numpy_vector<double> py_mean_obs(int n) {
    vector<double> result(n);
    mean_obs(*(this->ssm), result);
    return pyublas::numpy_vector<double>(result);
  };

  pyublas::numpy_vector<double> py_prior_sample(int n, unsigned long seed) {
    vector<double> result(n);
    prior_sample(*(this->ssm), result, seed);
    return pyublas::numpy_vector<double>(result);
  };

  pyublas::numpy_vector<double> py_obs_var(int n) {
    vector<double> result(n);
    obs_var(*(this->ssm), result);
    return pyublas::numpy_vector<double>(result);
  };

  boost::python::list component_means(const pyublas::numpy_vector<double> z) {
    std::vector<vector<double> > means(this->ssms.size());
    for (unsigned i=0; i < means.size(); ++i) {
      int st = this->start_idxs[i];
      int et = this->end_idxs[i];
      int npts = et-st;
      means[i].resize(npts);
      means[i].clear();
    }
    tssm_component_means(*(this->ssm), z, means);

    boost::python::list l;
    for(unsigned i=0; i < means.size(); ++i) {
      pyublas::numpy_vector<double> v(means[i]);
      l.append(v);
    }
    return l;
  }

  boost::python::list component_vars(const pyublas::numpy_vector<double> z) {
    std::vector<vector<double> > vars(this->ssms.size());
    for (unsigned i=0; i < vars.size(); ++i) {
      int st = this->start_idxs[i];
      int et = this->end_idxs[i];
      int npts = et-st;
      vars[i].resize(npts);
      vars[i].clear();
    }
    tssm_component_vars(*(this->ssm), z, vars);

    boost::python::list l;
    for(unsigned i=0; i < vars.size(); ++i) {
      pyublas::numpy_vector<double> v(vars[i]);
      l.append(v);
    }
    return l;
  }

  boost::python::tuple marginals(const pyublas::numpy_vector<double> z) {
    vector<double> step_ells(z.size());
    std::vector<vector<double> > cmeans;
    std::vector<vector<double> > cvars;
    double ell = all_filtered_cssm_coef_marginals(*(this->ssm), z, step_ells, cmeans, cvars);

    boost::python::list l;
    for(unsigned i=0; i < cmeans.size(); ++i) {
      pyublas::numpy_vector<double> m(cmeans[i]);
      pyublas::numpy_vector<double> v(cvars[i]);
      l.append( boost::python::make_tuple(m,v) );
    }
    pyublas::numpy_vector<double> ns(step_ells);
    return boost::python::make_tuple(ell, l, ns   );

  }

  /*
  int get_n_coefs(int i) {
    if (!this->ssms[i]) {
      printf("ERROR: trying to set means for a NULL ssm.\n");
      exit(-1);
    }
    if (!this->ssms[i]->is_cssm) {
      printf("ERROR: trying to set means for a non-CSSM ssm.\n");
      exit(-1);
    }
    CompactSupportSSM *ssm = (CompactSupportSSM *) (this->ssms[i]);
    return ssm->n_basis;
  }
  */

  boost::python::object get_component(int i) {
    return this->pyssms[i];
    /*if (this->pyssms[i]) {
      boost::python::handle<> h(boost::python::borrowed(this->pyssms[i]));
      return boost::python::object(h);
    } else {
      return boost::python::object();
      }*/
  }

  int max_dimension() {
    return this->ssm->max_dimension;
  }

  TransientCombinedSSM *ssm;
private:
  vector<double> p;

  // keep references to the Python objects whose internal states we depend on,
  // to prevent the Python garbage collector from killing them.
  std::vector<boost::python::object> pyssms;
  std::vector<boost::python::object> vec_refs;

  std::vector<StateSpaceModel *> ssms;
  vector<int> start_idxs;
  vector<int> end_idxs;
  std::vector<const double * > scales;

};

namespace bp = boost::python;
BOOST_PYTHON_MODULE(ssms_c) {

  bp::class_<PyCSSSM>("CompactSupportSSM", bp::init< \
	  pyublas::numpy_vector<int> const &,  pyublas::numpy_vector<int> const &,
	  pyublas::numpy_vector<int> const &,  pyublas::numpy_matrix<double> const &,
	  pyublas::numpy_vector<double> const & ,  pyublas::numpy_vector<double> const &,
	  double const , double const>())
    .def("run_filter", &PyCSSSM::run_filter)
    .def("step_obs_likelihoods", &PyCSSSM::py_step_obs_likelihoods)
    .def("mean_obs", &PyCSSSM::py_mean_obs)
    .def("prior_sample", &PyCSSSM::py_prior_sample)
    .def("obs_var", &PyCSSSM::py_obs_var)
    .def("set_coef_prior", &PyCSSSM::set_coef_prior)
    .def("get_coef_prior", &PyCSSSM::get_coef_prior)
    .def("max_dimension", &PyCSSSM::py_max_dimension)
     ;

  bp::class_<PyARSSM>("ARSSM", bp::init< \
		      pyublas::numpy_vector<double> const &,  double const,
		      double const , double const>())
    .def("run_filter", &PyARSSM::run_filter)
    .def("mean_obs", &PyARSSM::py_mean_obs)
    .def("prior_sample", &PyARSSM::py_prior_sample)
    .def("obs_var", &PyARSSM::py_obs_var)
    .def("set_process", &PyARSSM::set_process);

  bp::class_<PyTSSM>("TransientCombinedSSM", bp::init<boost::python::list const &,
		     double >())
    .def("run_filter", &PyTSSM::run_filter)
    .def("run_filter_incremental", &PyTSSM::py_run_filter_incremental)
    .def("step_obs_likelihoods", &PyTSSM::py_step_obs_likelihoods)
    .def("mean_obs", &PyTSSM::py_mean_obs)
    .def("prior_sample", &PyTSSM::py_prior_sample)
    .def("obs_var", &PyTSSM::py_obs_var)
    .def("component_means", &PyTSSM::component_means)
    .def("component_vars", &PyTSSM::component_vars)
    .def("all_filtered_cssm_coef_marginals", &PyTSSM::marginals)
    //.def("get_n_coefs", &PyTSSM::get_n_coefs)
    .def("get_component", &PyTSSM::get_component)
    .def("max_dimension", &PyTSSM::max_dimension);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#define NT2_UNIT_MODULE "nt2::sum1 function"

#include <nt2/table.hpp>
#include <nt2/include/functions/idx_linear.hpp>
#include <nt2/include/functions/linspace.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/include/functions/isequal.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <boost/fusion/include/make_vector.hpp>

NT2_TEST_CASE_TPL( idx_linear, NT2_REAL_TYPES )
{
  using nt2::_;
  nt2::table<T> x =  nt2::linspace(T(1),  T(4), 4);
  nt2::table<T> xi=  nt2::linspace(T(0),  T(5), 11);
  T nan =  nt2::Nan<T> ();
  nt2::table<T> r0 = nt2::linspace(T(0), T(5), 11);
  nt2::table<T> r1 = r0; r1(_(1, 2)) = nan; r1(_(10, 11)) = nan;
  nt2::table<T> r2 = r0; r2(_(1, 2)) = T(33), r2(_(10, 11)) = T(33);
  nt2::table<T> r3 = r2; r3(_(10, 11)) = T(42);
  NT2_DISPLAY(x);
  NT2_DISPLAY(xi);
  nt2::table<T> yi =nt2::idx_linear(x, xi);
  std::cout << "1 extrap " <<  false <<  " extrapval " << "-" << std::endl;
  NT2_DISPLAY(yi);
  NT2_TEST_EQUAL(yi, r1);
  yi =nt2::idx_linear(x, xi, false);
  std::cout << "2 extrap " <<  false <<  " extrapval " << "-" << std::endl;
  NT2_DISPLAY(yi);
  NT2_TEST_EQUAL(yi, r1);
  std::cout << "3 extrap " <<  true <<  " extrapval " << "-" << std::endl;
  yi =nt2::idx_linear(x, xi, true);
  NT2_DISPLAY(yi);
  NT2_TEST_EQUAL(yi, r0);
  T z =  T(33);
  std::cout << "4 extrap " <<  "-" <<  " extrapval " << "33" << std::endl;
  yi =nt2::idx_linear(x, xi, z);
  NT2_DISPLAY(yi);
  NT2_TEST_EQUAL(yi, r2);
  std::cout << "5 extrap " <<  "-" <<  " extrapval " << "33" << std::endl;
  yi =nt2::idx_linear(x, xi, T(33));
  NT2_DISPLAY(yi);
  NT2_TEST_EQUAL(yi, r2);
  std::cout << "6 extrap " <<  "-" <<  " extrapval1 " << "33" <<  " extrapval1 " << "42"<< std::endl;
  yi =nt2::idx_linear(x, xi, T(33), T(42));
  NT2_DISPLAY(yi);
  NT2_TEST_EQUAL(yi, r3);

}

NT2_TEST_CASE_TPL( idx_linear2, NT2_REAL_TYPES )
{
  using nt2::_;
  nt2::table<T> x =  nt2::reshape(nt2::linspace(T(1),  T(16), 16), 4, 4);
  nt2::table<T> xi=  nt2::linspace(T(0),  T(5), 11);
  NT2_DISPLAY(x);
  NT2_DISPLAY(xi);
  nt2::table<T> yi;
  yi =nt2::idx_linear(x, xi, _, 2lu);
  std::cout << "1 extrap " <<  false <<  " extrapval " << "-" <<  " dim " << "2" << std::endl;
  NT2_DISPLAY(yi);
  yi =nt2::idx_linear(x, nt2::trans(xi), _, 1lu);
  std::cout << "2 extrap " <<  false <<  " extrapval " << "-" <<  " dim " << "1" << std::endl;
  NT2_DISPLAY(yi);
  yi =nt2::idx_linear(x, xi, _, 1lu);
  std::cout << "3 extrap " <<  false <<  " extrapval " << "-" << std::endl;
  NT2_DISPLAY(yi);

  yi =nt2::idx_linear(x, xi, false);
  std::cout << "4 extrap " <<  false <<  " extrapval " << "-" << std::endl;
  NT2_DISPLAY(yi);

  yi =nt2::idx_linear(x, xi, false, _, 1);
  std::cout << "5 extrap " <<  false <<  " extrapval " << "-" << std::endl;
  NT2_DISPLAY(yi);

  std::cout << "6 extrap " <<  true <<  " extrapval " << "-" << std::endl;
  yi =nt2::idx_linear(x, xi, true, _, 1);
  NT2_DISPLAY(yi);

  std::cout << "7 extrap " <<  "-" <<  " extrapval1 " << "33" <<  " extrapval2 " << "42" << std::endl;
  yi =nt2::idx_linear(x, xi, T(33), T(42), 1);
  NT2_DISPLAY(yi);
  NT2_TEST_COMPLETE("idx_linear2");
     /*     //   std::cout << "8 extrap " <<  "-" <<  " extrapval1 " << "33" <<  " extrapval2 " << "42" << std::endl;
     //   typedef boost::fusion::vector<T,T>  result_type;
     //   result_type a =  boost::fusion::make_vector(T(-3), T(25));
     //   std::cout << boost::fusion::at_c<0>(a) << std::endl;
     //   std::cout << boost::fusion::at_c<1>(a) << std::endl;
     //   yi =nt2::idx_linear(x, xi, a);
     //   NT2_DISPLAY(yi);

     //   std::cout << "9 extrap " <<  "-" <<  " extrapval1 " << "33" <<  " extrapval2 " << "42" << std::endl;
     //   nt2::table<T>   a(nt2::of_size(1, 2));
     //   a(1) = T(-3);  a(2) = T(25);
     //   yi =nt2::idx_linear(x, xi, a);
     //   NT2_DISPLAY(yi);
     */
 }

NT2_TEST_CASE_TPL( idx_linear1, NT2_REAL_TYPES )
{
  using nt2::_;
  nt2::table<T> x =  nt2::reshape(nt2::linspace(T(1),  T(16), 16), 4, 4);
  nt2::table<T> xi=  nt2::linspace(T(1),  T(3), 11);
  NT2_DISPLAY(x);
  NT2_DISPLAY(xi);
  nt2::table<T> y0, yi;
  y0 =nt2::idx_linear(x, xi);
  std::cout << "extrap " <<  false <<  " extrapval " << "-" << std::endl;
  NT2_DISPLAY(y0);
  yi =nt2::idx_linear(x, xi, false);
  NT2_TEST(isequal(y0, yi));
  std::cout << "extrap " <<  false <<  " extrapval " << "-" << std::endl;
  NT2_DISPLAY(yi);
  std::cout << "extrap " <<  true <<  " extrapval " << "-" << std::endl;
  yi =nt2::idx_linear(x, xi, true);
  NT2_TEST(isequal(y0, yi));
  NT2_DISPLAY(yi);
  T z =  T(33);
  std::cout << "extrap " <<  "-" <<  " extrapval " << "33" << std::endl;
  yi =nt2::idx_linear(x, xi, z);
  NT2_TEST(isequal(y0, yi));
  NT2_DISPLAY(yi);
  std::cout << "extrap " <<  "-" <<  " extrapval " << "33" << std::endl;
  yi =nt2::idx_linear(x, xi, T(33));
  NT2_TEST(isequal(y0, yi));
  NT2_DISPLAY(yi);
  NT2_TEST_COMPLETE("idx_linear1");

}


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

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[make_inverse
//` Usage of make_inverse and expand to conveniently determine bounding box of several objects

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

using namespace boost::geometry;

int main()
{

    typedef model::d2::point_xy<double> point;
    typedef model::box<point> box;

    box all = make_inverse<box>();
    std::cout << dsv(all) << std::endl;
    expand(all, make<box>(0, 0, 3, 4));
    expand(all, make<box>(2, 2, 5, 6));
    std::cout << dsv(all) << std::endl;

    return 0;
}

//]

//[make_inverse_output
/*`
Output:
[pre
((1.79769e+308, 1.79769e+308), (-1.79769e+308, -1.79769e+308))
((0, 0), (5, 6))
]
*/
//]


//=========================================================================
//
// Copyright 2019 Kitware, Inc.
// Author: Guilbert Pierre (spguilbert@gmail.com)
// Data: 03-27-2019
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//=========================================================================

// LOCAL
#include "CameraCalibration.h"
#include "CameraProjection.h"
#include "vtkEigenTools.h"
#include "CeresCameraCalibrationCostFunctions.h"

// STD
#include <iostream>
#include <fstream>
#include <sstream>

// BOOST
#include <boost/algorithm/string.hpp>

// CERES
#include <ceres/ceres.h>

//----------------------------------------------------------------------------
void LoadMatchesFromCSV(std::string filename, std::vector<Eigen::Vector3d>& X, std::vector<Eigen::Vector2d>& x)
{
  // Load file and check that the file is opened
  std::ifstream file(filename.c_str());
  if (!file.is_open())
  {
    std::cout << "Error: could not load file: " << filename << std::endl;
    return;
  }

  // check the file header
  std::string tokenFileHeader("X,Y,Z,u,v");
  std::string line;
  std::getline(file, line);
  if (line != tokenFileHeader)
  {
    std::cout << "Error file header is: " << line << " expected: " << tokenFileHeader << std::endl;
    return;
  }

  // parse the file
  while (std::getline(file, line))
  {
    std::vector<std::string> values;
    boost::algorithm::split(values, line, boost::is_any_of(","));

    // check that each lines has the right number of value
    if (values.size() < 5)
    {
      std::cout << "Error, number of values is: " << values.size() << " expected 5" << std::endl;
      continue;
    }

    Eigen::Vector3d Xc(std::atof(values[0].c_str()), std::atof(values[1].c_str()), std::atof(values[2].c_str()));
    Eigen::Vector2d xc(std::atof(values[3].c_str()), std::atof(values[4].c_str()));
    X.push_back(Xc);
    x.push_back(xc);
  }
  return;
}

//----------------------------------------------------------------------------
double LinearPinholeCalibration(const std::vector<Eigen::Vector3d>& X, const std::vector<Eigen::Vector2d>& x, Eigen::Matrix<double, 3, 4>& P)
{
  P = Eigen::Matrix<double, 3, 4>::Zero();
  if (X.size() != x.size())
  {
    std::cout << "Error: matches have different sizes" << std::endl;
    return -1.0;
  }

  // We will estimate the coefficients of the projection matrix
  // by minimizing the reprojection euclidean distance between
  // the image keypoints and the 3D associated keypoints reprojected

  // Compute the normal equations
  Eigen::MatrixXd A(2 * X.size(), 12);
  for (int k = 0; k < X.size(); ++k)
  {
    A.row(2 * k) << X[k](0), X[k](1), X[k](2), 1,
                    0, 0, 0, 0,
                    -x[k](0) * X[k](0), -x[k](0) * X[k](1), -x[k](0) * X[k](2), -x[k](0);

    A.row(2 * k + 1) << 0, 0, 0, 0,
                        X[k](0), X[k](1), X[k](2), 1,
                       -x[k](1) * X[k](0), -x[k](1) * X[k](1), -x[k](1) * X[k](2), -x[k](1);
  }

  // Solve the normal equations
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::MatrixXd flattenP = svd.matrixV().col(11);
  P << flattenP(0), flattenP(1), flattenP(2), flattenP(3),
       flattenP(4), flattenP(5), flattenP(6), flattenP(7),
       flattenP(8), flattenP(9), flattenP(10), flattenP(11);
  P = P / P(2, 3);

  // Compute the RMSE
  double meanErr = 0;
  for (int k = 0; k < X.size(); ++k)
  {
    Eigen::Vector4d homoX(X[k](0), X[k](1), X[k](2), 1);
    Eigen::Vector3d projX = P * homoX;
    Eigen::Vector2d projXn(projX(0) / projX(2), projX(1) / projX(2));
    //std::cout << "X: " << X[k].transpose() << " x: " << x[k].transpose() << " prjX: " << projXn.transpose() << " dist: " << (x[k] - projXn).norm() << std::endl;
    meanErr += ((x[k] - projXn).transpose() * (x[k] - projXn))(0);
  }
  return std::sqrt(meanErr / (1.0 * X.size()));
}

//----------------------------------------------------------------------------
void CalibrationMatrixDecomposition(const Eigen::Matrix<double, 3, 4>& P, Eigen::Matrix3d& K, Eigen::Matrix3d& R, Eigen::Vector3d& T)
{
  // M = K*R
  Eigen::Matrix3d M = P.block(0, 0, 3, 3);

  // M is the product of an upper triangulate matrix
  // and an orthogonal matrix. We will use a RQ decomposition
  // to recover K and R. Since eigen only provides QR
  // decomposition we will inverse the column order of M to
  // get its RQ decomposition from the QR decomposition
  Eigen::Matrix3d D = Eigen::Matrix3d::Zero();
  D(2, 0) = 1; D(1, 1) = 1; D(0, 2) = 1;
  Eigen::Matrix3d Mtilde = D * M;
  Eigen::HouseholderQR<Eigen::Matrix3d> QRdec(Mtilde.transpose());
  Eigen::Matrix3d Qtilde = QRdec.householderQ();
  Eigen::Matrix3d Rtilde = Qtilde.transpose() * (Mtilde.transpose());
  K = D * Rtilde.transpose() * D;
  R = D * Qtilde.transpose();
  T = P.col(3);
  T = - R.transpose() * K.inverse() * T;

  // rescale the matrix
  K = K / K(2, 2);
  R = R * std::cbrt(1.0 / R.determinant());
  R = R.transpose().eval();

  Eigen::Matrix<double, 3, 4> H;
  H.block(0, 0, 3, 3) = K * R.transpose();
  H.col(3) = -K * R.transpose() * T;
  H = H / H(2, 3);
  if (((H - P).transpose() * (H - P)).trace() > 1e-6)
  {
    std::cout << "Error: decomposition failed" << std::endl;
  }
  return;
}

//----------------------------------------------------------------------------
void GetParametersFromMatrix(const Eigen::Matrix3d& K, const Eigen::Matrix3d& R, const Eigen::Vector3d& T, Eigen::Matrix<double, 11, 1>& W)
{
  Eigen::Vector3d eulerAngles = MatrixToRollPitchYaw(R);
  W(0) = eulerAngles(0); W(1) = eulerAngles(1); W(2) = eulerAngles(2);
  W(3) = T(0), W(4) = T(1), W(5) = T(2);
  W(6) = K(0, 0);
  W(7) = K(1, 1);
  W(8) = K(0, 2);
  W(9) = K(1, 2);
  W(10) = K(0, 1);
  return;
}

//----------------------------------------------------------------------------
double NonLinearPinholeCalibration(const std::vector<Eigen::Vector3d>& X, const std::vector<Eigen::Vector2d>& x, Eigen::Matrix<double, 11, 1>& W)
{
  Eigen::Matrix<double, 3, 4> P0, P1;
  GetMatrixFromParameters(W, P0);

  // We want to estimate our 11-DOF parameters using a non
  // linear least square minimization. The non linear part
  // comes from the Euler Angle parametrization of the rotation
  // endomorphism of SO(3) and the homographie rescaling
  // To minimize it, we use CERES to perform
  // the Levenberg-Marquardt algorithm.
  ceres::Problem problem;
  for (unsigned int k = 0; k < X.size(); ++k)
  {
    ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctions::PinholeModelAlgebraicDistance, 1, 11>(
                                         new CostFunctions::PinholeModelAlgebraicDistance(X[k], x[k]));
    problem.AddResidualBlock(cost_function, nullptr, W.data());
  }

  // Solve the problem
  ceres::Solver::Options options;
  options.max_num_iterations = 1000;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = false;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.BriefReport() << std::endl;

  // Compute mean error
  GetMatrixFromParameters(W, P1);
  double meanErr = 0;
  for (int k = 0; k < X.size(); ++k)
  {
    Eigen::Vector4d homoX(X[k](0), X[k](1), X[k](2), 1);
    Eigen::Vector3d projX = P1 * homoX;
    Eigen::Vector2d projXn(projX(0) / projX(2), projX(1) / projX(2));
    meanErr += ((x[k] - projXn).transpose() * (x[k] - projXn))(0);
  }
 return std::sqrt(meanErr / (1.0 * X.size()));
}

//----------------------------------------------------------------------------
double NonLinearFisheyeCalibration(const std::vector<Eigen::Vector3d>& X, const std::vector<Eigen::Vector2d>& x,
                                   Eigen::Matrix<double, 15, 1>& W, unsigned int it)
{
  // We want to estimate our 15-DOF parameters using a non
  // linear least square minimization. The non linear part
  // comes from the Euler Angle parametrization of the rotation
  // endomorphism of SO(3) and the homographie rescaling
  // To minimize it, we use CERES to perform
  // the Levenberg-Marquardt algorithm.
  ceres::Problem problem;
  for (unsigned int k = 0; k < X.size(); ++k)
  {
    ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctions::FisheyeModelAlgebraicDistance, 1, 15>(
                                         new CostFunctions::FisheyeModelAlgebraicDistance(X[k], x[k]));
    problem.AddResidualBlock(cost_function, nullptr, W.data());
  }

  ceres::Solver::Options options;
  options.max_num_iterations = it;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = false;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  double meanErr = 0;
  for (int k = 0; k < X.size(); ++k)
  {
    meanErr += ((x[k] - FisheyeProjection(W, X[k])).transpose() * (x[k] - FisheyeProjection(W, X[k])))(0);
  }
  return std::sqrt(meanErr / (1.0 * X.size()));
}

//----------------------------------------------------------------------------
double BrownConradyPinholeCalibration(const std::vector<Eigen::Vector3d>& X, const std::vector<Eigen::Vector2d>& x,
                                      Eigen::Matrix<double, 17, 1>& W, unsigned int it,
                                      double initLossScale, double finalLossScale,
                                      const std::vector<bool>& shouldOptimizeParam)
{
  unsigned int N = 100;

  // The minimization algorithm will be ran multiple time
  // with an outlier rejection loss function more restrictive
  // at each iteration. We don't want to have a higly restrictive
  // outlier rejection at the beginning since the initial point can
  // be far from the global minimum and it could create convergence
  // issues.
  for (unsigned int minId = 0; minId < N; ++minId)
  {
    double lossScale = initLossScale + static_cast<double>(minId) * (finalLossScale - initLossScale) / (1.0 * N);

    // We want to estimate our 17-DOF parameters using a non
    // linear least square minimization. The non linear part
    // comes from the Euler Angle parametrization of the rotation
    // endomorphism of SO(3), the homographie rescaling and
    // the lens distortions
    // To minimize it, we use CERES to perform
    // the Levenberg-Marquardt algorithm.
    ceres::Problem problem;
    for (unsigned int k = 0; k < X.size(); ++k)
    {
      CostFunctions::BrownConradyAlgebraicDistance* resFct = new CostFunctions::BrownConradyAlgebraicDistance(X[k], x[k]);
      resFct->SetW0(W);
      resFct->SetActivatedParams(shouldOptimizeParam);
      ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctions::BrownConradyAlgebraicDistance, 1, 17>(resFct);
      problem.AddResidualBlock(cost_function, new ceres::ArctanLoss(lossScale), W.data());
    }

    ceres::Solver::Options options;
    options.max_num_iterations = it;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = false;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;
  }

  double meanErr = 0;
  for (int k = 0; k < X.size(); ++k)
  {
    meanErr += ((x[k] - BrownConradyPinholeProjection(W, X[k])).transpose() * (x[k] - BrownConradyPinholeProjection(W, X[k])))(0);
  }
  return std::sqrt(meanErr / (1.0 * X.size()));
}

//----------------------------------------------------------------------------
void GetMatrixFromParameters(const Eigen::Matrix<double, 11, 1>& W, Eigen::Matrix<double, 3, 4>& P)
{
  // Create current rotation
  Eigen::Matrix3d R = RollPitchYawToMatrix(W(0), W(1), W(2));
  // Create current position
  Eigen::Vector3d T(W(3), W(4), W(5));

  // Create current intrinsic parameters
  Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
  K(0, 0) = W(6);
  K(1, 1) = W(7);
  K(0, 2) = W(8);
  K(1, 2) = W(9);
  K(0, 1) = W(10);
  K(2, 2) = 1;

  // Create current calibration matrix
  P.block(0, 0, 3, 3) = K * R.transpose();
  P.col(3) = -K * R.transpose() * T;
  P = P / P(2, 3);
}

//----------------------------------------------------------------------------
Eigen::Matrix<double, 3, 4> GetMatrixFromParameters(const Eigen::Matrix<double, 11, 1>& W)
{
  Eigen::Matrix<double, 3, 4> P;
  GetMatrixFromParameters(W, P);
  return P;
}

//----------------------------------------------------------------------------
Eigen::VectorXd FullCalibrationPipelineFromMatches(std::string filename, const std::vector<bool>& activatedParams)
{
  Eigen::VectorXd Wf = Eigen::VectorXd::Zero(17, 1);

  // Load the 3D - 2D matches
  std::vector<Eigen::Vector3d> X;
  std::vector<Eigen::Vector2d> x;
  LoadMatchesFromCSV(filename, X, x);
  if (X.size() == 0)
  {
    return Wf;
  }

  // First, launch a linear pinhole camera model
  // projection matrix estimation
  Eigen::Matrix<double, 3, 4> P;
  double rmse1 = LinearPinholeCalibration(X, x, P);

  // From this first linear pinhole projection matrix
  // estimation, extract the pinhole model parameters
  Eigen::Matrix3d K, R;
  Eigen::Vector3d T;
  CalibrationMatrixDecomposition(P, K, R, T);

  // Check that the optical axis of the camera is
  // correctly oriented according to the 3D points
  Eigen::Vector3d Xmean = Eigen::Vector3d::Zero();
  for (int i = 0; i < X.size(); ++i)
  {
    Xmean += X[i] / static_cast<double>(X.size());
  }
  Eigen::Vector3d ez = R.col(2);
  double angle = std::acos((Xmean.transpose() * ez)(0) / (Xmean.norm() * ez.norm())) / vtkMath::Pi() * 180.0;

  // In this case, the linear algorithm has provided
  // a solution where the camera is looking backward and
  // the resulting symmetry is handled by the intrinsic matrix.
  // To avoid that, we return the camera if it is not looking
  // forward
  if (angle > 90.0)
  {
    Eigen::Matrix3d S;
    S << -1.0, 0.0,  0.0,
          0.0, 1.0,  0.0,
          0.0, 0.0, -1.0;

    Eigen::Matrix3d Rtilde = R * S * R.transpose();

    // So that K' * R' = K * R, meaning that
    // the global projection is unchanged but
    // the camera orientation has made a 180
    // rotation around its y-axis
    K = K * Rtilde.transpose();
    K = K / K(2, 2);
    R = Rtilde * R;
  }

  Eigen::Matrix<double, 11, 1> Wpinhole;
  GetParametersFromMatrix(K, R, T, Wpinhole);
  Eigen::Matrix<double, 11, 1> Wpi = Wpinhole;

  // Then, refine the model obtained using linear
  // estimation by using a non-linear pinhole parameters
  // estimation
  double rmse2 = NonLinearPinholeCalibration(X, x, Wpinhole);

  // Finally, create a first parameter vector estimation
  // by using the pinhole parameters and setting the distortion
  // coefficients to 0
  Eigen::Matrix<double, 17, 1> West = Eigen::Matrix<double, 17, 1>::Zero();
  West.block(0, 0, 11, 1) = Wpinhole;

  double rmse3 = BrownConradyPinholeCalibration(X, x, West, 2500, 5.0, 0.6, activatedParams);

  // copy params
  for (int i = 0; i < 17; ++i)
  {
    Wf(i) = West(i);
  }

  Eigen::Vector3d angles(Wf(0), Wf(1), Wf(2));
  R = RollPitchYawToMatrix(angles);

  std::cout << "RMSE1: " << rmse1 << std::endl;
  std::cout << "RMSE2: " << rmse2 << std::endl;
  std::cout << "RMSE3: " << rmse3 << std::endl;
  std::cout << "W: ";
  for (int i = 0; i < 17; ++i)
  {
    std::cout << Wf(i) << ",";
  }
  std::cout << std::endl;
  return Wf;
}


#ifndef CRADLE_GEOMETRY_GRID_POINTS_HPP
#define CRADLE_GEOMETRY_GRID_POINTS_HPP

#include <cradle/geometry/regular_grid.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <vector>

namespace cradle {

// A regular_grid_iterator is used for traversing the points on a regular_grid.
template<unsigned N, typename T>
class regular_grid_iterator
  : public boost::iterator_facade<
        regular_grid_iterator<N,T>
      , vector<N,T> const
      , boost::random_access_traversal_tag
    >
{
 public:
    regular_grid_iterator() {}
    regular_grid_iterator(regular_grid<N,T> const* grid,
        vector<N,unsigned> const& i, vector<N,T> const& p)
      : grid(grid), i(i), p(p)
    {}

 private:
    friend class boost::iterator_core_access;

    vector<N,T> const& dereference() const { return p; }

    bool equal(regular_grid_iterator const& other) const
    { return this->i == other.i; }

    void increment();
    void decrement();

    void advance(typename regular_grid_iterator::difference_type n);

    typename regular_grid_iterator::difference_type
    distance_to(regular_grid_iterator const& other) const;

    regular_grid<N,T> const* grid;
    vector<N,unsigned> i;
    vector<N,T> p;
};

template<unsigned N, typename T>
class regular_grid_point_list
{
 public:
    regular_grid_point_list() {}
    regular_grid_point_list(regular_grid<N,T> const& grid) : grid_(grid) {}

    typedef regular_grid_iterator<N,T> iterator;
    typedef iterator const_iterator;

    const_iterator begin() const
    {
        return const_iterator(&grid_, uniform_vector<N,unsigned>(0),
            grid_.p0);
    }
    const_iterator end() const
    {
        vector<N,unsigned> i = uniform_vector<N,unsigned>(0);
        i[N - 1] = grid_.n_points[N - 1];
        return const_iterator(&grid_, i,
            grid_.p0 + cradle::vector<N,T>(i) * grid_.spacing);
    }

    typedef size_t size_type;
    size_type size() const { return product(grid_.n_points); }

 private:
    regular_grid<N,T> grid_;
};

template<unsigned N, typename T>
regular_grid_point_list<N,T>
make_grid_point_list(regular_grid<N,T> const& grid)
{ return regular_grid_point_list<N,T>(grid); }

// Get the list of points on a grid.
api(fun with(N:1,2,3,4; T:double))
template<unsigned N, typename T>
// List of points on the N dimensional grid, with N being 1 thru 4
std::vector<cradle::vector<N,T> >
get_points_on_grid(
// The grid over which to compute the point positions
regular_grid<N,T> const& grid)
{
    auto lazy = make_grid_point_list(grid);
    std::vector<cradle::vector<N,T> > eager;
    eager.reserve(lazy.size());
    for (auto i = lazy.begin(); i != lazy.end(); ++i)
        eager.push_back(*i);
    return eager;
}

namespace impl {

    template<unsigned N, typename T, unsigned I_>
    struct regular_grid_iterator_incrementer
    {
        static void apply(
            regular_grid<N,T> const& grid,
            vector<N,unsigned>& i,
            vector<N,T>& p)
        {
            static unsigned const I = N - I_;
            ++i[I];
            if (i[I] == grid.n_points[I])
            {
                i[I] = 0;
                p[I] = grid.p0[I];
                regular_grid_iterator_incrementer<N,T,N - (I + 1)>::apply(
                    grid, i, p);
            }
            else
                p[I] += grid.spacing[I];
        }
    };
    template<unsigned N, typename T>
    struct regular_grid_iterator_incrementer<N,T,1>
    {
        static void apply(
            regular_grid<N,T> const& grid,
            vector<N,unsigned>& i,
            vector<N,T>& p)
        {
            static unsigned const I = N - 1;
            ++i[I];
            p[I] += grid.spacing[I];
        }
    };
}

template<unsigned N, typename T>
void regular_grid_iterator<N,T>::increment()
{
    impl::regular_grid_iterator_incrementer<N,T,N>::apply(*grid, i, p);
}

namespace impl {

    template<unsigned N, typename T, unsigned I_>
    struct regular_grid_iterator_decrementer
    {
        static void apply(
            regular_grid<N,T> const& grid,
            vector<N,unsigned>& i,
            vector<N,T>& p)
        {
            static unsigned const I = N - I_;
            if (i[I] == 0)
            {
                i[I] = grid.n_points[I] - 1;
                p[I] = grid.p0[I] + i[I] * grid.spacing[I];
                regular_grid_iterator_decrementer<N,T,N - (I + 1)>::apply(
                    grid, i, p);
            }
            else
            {
                --i[I];
                p[I] -= grid.spacing[I];
            }
        }
    };
    template<unsigned N, typename T>
    struct regular_grid_iterator_decrementer<N,T,1>
    {
        static void apply(
            regular_grid<N,T> const& grid,
            vector<N,unsigned>& i,
            vector<N,T>& p)
        {
            static unsigned const I = N - 1;
            --i[I];
            p[I] -= grid.spacing[I];
        }
    };
}

template<unsigned N, typename T>
void regular_grid_iterator<N,T>::decrement()
{
    impl::regular_grid_iterator_decrementer<N,T,N>::apply(*grid, i, p);
}

namespace impl {

    template<unsigned N, typename T, unsigned I_>
    struct regular_grid_iterator_advancer
    {
        template<typename Difference>
        static void apply(
            regular_grid<N,T> const& grid,
            vector<N,unsigned>& i,
            vector<N,T>& p,
            Difference n)
        {
            static unsigned const I = N - I_;
            Difference t = static_cast<Difference>(i[I]) + n;
            Difference r = t % static_cast<Difference>(grid.n_points[I]);
            Difference q = t / static_cast<Difference>(grid.n_points[I]);
            if (r < 0)
            {
                r = grid.n_points[I] - r;
                --q;
            }
            i[I] = unsigned(r);
            p[I] = grid.p0[I] + i[I] * grid.spacing[I];
            if (q != 0)
            {
                regular_grid_iterator_advancer<N,T,N - (I + 1)>::apply(
                    grid, i, p, q);
            }
        }
    };
    template<unsigned N, typename T>
    struct regular_grid_iterator_advancer<N,T,1>
    {
        template<typename Difference>
        static void apply(
            regular_grid<N,T> const& grid,
            vector<N,unsigned>& i,
            vector<N,T>& p,
            Difference n)
        {
            static unsigned const I = N - 1;
            i[I] += unsigned(n);
            p[I] = grid.p0[I] + i[I] * grid.spacing[I];
        }
    };
}

template<unsigned N, typename T>
void regular_grid_iterator<N,T>::advance(
    typename regular_grid_iterator<N,T>::difference_type n)
{
    impl::regular_grid_iterator_advancer<N,T,N>::apply(*grid, i, p, n);
}

namespace impl {

    template<typename Difference, unsigned N, typename T, unsigned I_>
    struct regular_grid_iterator_distance
    {
        static Difference apply(
            regular_grid<N,T> const& grid,
            vector<N,unsigned> const& a,
            vector<N,unsigned> const& b)
        {
            static unsigned const I = N - I_;
            return (static_cast<Difference>(b[I]) -
                static_cast<Difference>(a[I])) +
                regular_grid_iterator_distance<Difference,N,T,N - (I + 1)>
                    ::apply(grid, a, b) * grid.n_points[I];
        }
    };
    template<typename Difference, unsigned N, typename T>
    struct regular_grid_iterator_distance<Difference,N,T,1>
    {
        static Difference apply(
            regular_grid<N,T> const& grid,
            vector<N,unsigned> const& a,
            vector<N,unsigned> const& b)
        {
            static unsigned const I = N - 1;
            return (static_cast<Difference>(b[I]) -
                static_cast<Difference>(a[I]));
        }
    };
}

template<unsigned N, typename T>
auto
regular_grid_iterator<N,T>::distance_to(
    regular_grid_iterator const& other) const
 -> typename regular_grid_iterator::difference_type
{
    assert(*grid == *other.grid);
    return impl::regular_grid_iterator_distance<
        typename regular_grid_iterator<N,T>::difference_type,N,T,N>::apply(
        *grid, i, other.i);
}

}

#endif


/*
 * OrConstraint.h
 * @brief OR constraint
 * @date Mar 20, 2022
 * @author Yoonwoo Kim
 */

#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <cpp/planning/OrConstraint.h>

#include <boost/make_shared.hpp>
using namespace gtsam;
using namespace std;

namespace gtsam_planner {

/* ************************************************************************* */
OrConstraint::OrConstraint(const vector<DecisionTreeFactor>& factors)
: DiscreteFactor() {
  for (const DecisionTreeFactor& factor: factors) factors_.push_back(factor);
  set<DiscreteKey> dkeys_set;
  for (DecisionTreeFactor factor : factors) {
    DiscreteKeys dkeys = factor.discreteKeys();
    for (DiscreteKey dkey : dkeys) dkeys_set.insert(dkey);
  }
  dkeys_ = {dkeys_set.begin(), dkeys_set.end()};
  for (const DiscreteKey& dkey : dkeys_) cardinalities_.insert(dkey);
}

/* ************************************************************************* */
void OrConstraint::print(const string& s, const KeyFormatter& formatter) const {
  cout << s << "OrConstraint on ";
  for (DiscreteKey dkey : dkeys_) std::cout << formatter(dkey.first) << " ";
  cout << endl;
}
/* ************************************************************************* */
double OrConstraint::operator()(const DiscreteValues& values) const {
  for (const DecisionTreeFactor factor: factors_) {
    if (factor(values) == 1.0) return 1.0;
  }
  return 0.0;
}

/* ************************************************************************* */
DecisionTreeFactor OrConstraint::toDecisionTreeFactor() const {
  vector<double> table;
  const auto assignments = DiscreteValues::CartesianProduct(dkeys_);
  for (const DiscreteValues assignment : assignments) {
    size_t is_true = 0;
    for (DecisionTreeFactor factor : factors_) {
      if (factor(assignment) == 1.0)
      is_true += factor(assignment);
    }
    table.push_back(is_true > 0);
  }
  DiscreteKeys rev_dkeys = {dkeys_.rbegin(), dkeys_.rend()};
  DecisionTreeFactor converted(rev_dkeys, table);
  return converted;
}

/* ************************************************************************* */
DecisionTreeFactor OrConstraint::operator*(const DecisionTreeFactor& f) const {
  // TODO: can we do this more efficiently?
  return toDecisionTreeFactor() * f;
}

}  // namespace gtsam_planner


#ifndef SWARM_H
#define SWARM_H

#include <cstddef>
#include <vector>
#include <Eigen/Core>
#include <tbb/mutex.h>

#include "types.hpp"
#include "particle.hpp"

namespace Optima {

struct SwarmHyperParameters {
    double omega;
    double phi_local;
    double phi_global;
};

class Swarm {
public:
  Swarm(const ParameterSpace &parameters, const Parameters &hyperParameters,
        std::size_t num_particles = 10);
  /// Optimize the cost function using the swarm
  void optimize(const CostFunction &func,
                const std::size_t numIterations = 10000);
  /// Get the best soultion found
  Eigen::ArrayXd getBestSolution() const { return bestPosition; }
  Parameters getBestParameters() const { return bestParameters; }
  std::vector<double> getCosts() const { return costs; }

private:
    /// initilise particles with random positions & velocity
    void initParticles(const ParameterSpace &parameters);
    /// find the particle wiht the best candidate solution
    Particle_ptr findBestParticle(const CostFunction& func) const;
    /// update position & velocity of each particle
    void updateParticle(const Particle_ptr particle);
    /// update best local/global positions if necessary
    void updateBestPositions(const Particle_ptr particle);

    /// Total number of particles in the swarm
    const std::size_t num_particles;
    /// Vector of particle objects in the swarm
    std::vector<Particle_ptr> particles;
    /// Current best position found by any member of the swarm
    Eigen::ArrayXd bestPosition;
    /// Current best parameters found by any member of the swarm
    Parameters bestParameters;
    /// Hyper parameters for the swarm
    Parameters hyperParameters;
    /// Cost function
    CostFunction func;
    /// Global update mutex. Locks updates to the global best position
    tbb::mutex updateMutex;
    /// variable to store costs
    std::vector<double> costs;

};
}

#endif


#include "InputFileOperator.h"
#include "CommandLineHandlers/SingleCommandLineHandler.h"
#include "NumberOperator.h"
#include <boost/algorithm/string/trim.hpp>

USING_NAMESPACE(std);
USING_NAMESPACE(clu);

class SmoothOperator : public NumberOperator
{
public:
	SmoothOperator(int window_size) : m_size(window_size), m_data(NULL), m_curpos(0), m_currsize(0)
	{
		m_data = new double[window_size];
		for (int i = 0; i < window_size; i++) {
			m_data[i] = 0;
		}
	}

	virtual ~SmoothOperator()
	{
		delete m_data;
	}

	virtual bool OnNumberRead(double number)
	{
		m_data[m_curpos] = number;
		m_curpos++;
		m_curpos = m_curpos % m_size;
		m_currsize++;

		int cur_size = m_currsize;
		if (cur_size > m_size)
			cur_size = m_size;

		double total = 0;
		for (int i = 0; i < cur_size; i++) {
			total = total + m_data[i];
		}
		m_OutputHandler->OutputDouble(total / cur_size);
		return true;
	}

	virtual void OnEnd()
	{
		for (int i = 0; i < m_size; i++)
			m_data[i] = 0;
		m_curpos = 0;
	}

protected:
	int m_size;
	double* m_data;
	int m_currsize;
	int m_curpos;
};

REGISTER_SINGLE_INTEGER("smooth", SmoothOperator)->SetHelp("Smooth Size - Running Average");


#define GrB_USE_SEQUENTIAL
#define private public

#include <vector>
#include <iostream>
#include <string>
#include <numeric>
#include <algorithm>
#include <map>

#include <cstdio>
#include <cstdlib>

#include "graphblas/graphblas.hpp"
#include "graphblas/backend/sequential/sequential.hpp"
#include "test/test.hpp"

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE triple_suite

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

void testMxm( char const* tsv, std::vector<int>& correct,
              std::vector<int>& correct_t )
{
  std::vector<graphblas::Index> row_indices;
  std::vector<graphblas::Index> col_indices;
  std::vector<float> values;
  graphblas::Index nrows, ncols, nvals;
  graphblas::Info err;

  int argc = 3;
  char** argv = (char**) malloc(3*sizeof(char*));
  argv[1] = (char *) "--verbose=false";
  argv[2] = (char *) "--merge.r=0";

  // Read in sparse matrix
  err = readTsv( tsv, row_indices, col_indices, values, nrows, ncols,
    nvals );

  graphblas::BlockState<graphblas::T> state( nrows, ncols );
  err = state.parseArgs( argc, argv );
  err = state.build( row_indices, col_indices, values, nvals );

  std::vector<int> k_out;
  std::vector<int> k_in;
  int temp;

  printArray( "curr_part", state.block_state_.curr_partition_, nrows );
  err = graphblas::mxm( state.block_state_.blk_,
                                  state.block_state_.curr_partition_,
                                  state.block_state_.adj_ );
  /*for( int i=0; i<nrows; i++ )
  {
    err = state.block_state_.adj_.getRowLength( temp, i );
    k_out.push_back( temp );
    err = state.block_state_.adj_t_.getRowLength( temp, i );
    k_in.push_back( temp );
  }

  BOOST_ASSERT_LIST( k_out, correct, std::min(nrows,10) );
  BOOST_ASSERT_LIST( k_in, correct_t, std::min(nrows,10) );

  BOOST_ASSERT( err==0 );*/
}

struct TestMatrix
{
  TestMatrix() :
    DEBUG(true) {}

  bool DEBUG;
};

BOOST_AUTO_TEST_SUITE(triple_suite)

BOOST_FIXTURE_TEST_CASE( triple1, TestMatrix )
{
  std::vector<int> adj_deg   = {1, 1, 3, 2, 2, 3, 3, 0, 1, 2};
  std::vector<int> adj_t_deg = {3, 3, 3, 2, 3, 1, 0, 3, 2, 0};
  testMxm( "data/small/test_cc.tsv", adj_deg, adj_t_deg );
}

BOOST_FIXTURE_TEST_CASE( triple2, TestMatrix )
{
  std::vector<int> adj_deg   = {1, 1, 3, 2, 2, 3, 3};
  std::vector<int> adj_t_deg = {3, 3, 3, 2, 3, 1, 0};
  testMxm( "data/small/test_bc.tsv", adj_deg, adj_t_deg );
}

BOOST_FIXTURE_TEST_CASE( triple3, TestMatrix )
{
  std::vector<int> adj_deg   = {4, 3, 3, 1, 1, 2, 4, 4, 1, 3};
  std::vector<int> adj_t_deg = {4, 4, 2, 1, 2, 1, 3, 3, 4, 1};
  testMxm( "data/small/chesapeake.tsv", adj_deg, adj_t_deg );
}

/*BOOST_FIXTURE_TEST_CASE( triple4, TestMatrix )
{
  testDup( "data/small/test_cc.tsv" );
}

BOOST_FIXTURE_TEST_CASE( triple5, TestMatrix )
{
  testDup( "data/small/test_bc.tsv" );
}

BOOST_FIXTURE_TEST_CASE( triple6, TestMatrix )
{
  testDup( "data/small/chesapeake.tsv" );
}

BOOST_FIXTURE_TEST_CASE( triple7, TestMatrix )
{
  testShiftRight( "data/small/test_cc.tsv" );
}

BOOST_FIXTURE_TEST_CASE( triple8, TestMatrix )
{
  std::map<graphblas::Index,graphblas::Index> out_edge;
  out_edge.insert({{2, 1}, {3, 0}, {5,1}});
  std::vector<graphblas::Index> correct = {3, 4, -1};
  testEwiseSub( "data/small/test_cc.tsv", out_edge, correct);
}

BOOST_FIXTURE_TEST_CASE( triple9, TestMatrix )
{
  std::map<graphblas::Index,graphblas::Index> out_edge;
  out_edge.insert({{2,0}, {4,1}});
  std::vector<graphblas::Index> correct_ind = {2, 4};
  std::vector<graphblas::T> correct_val = {1, 1};
  testEwiseAdd( "data/small/test_cc.tsv", out_edge, 0, correct_ind,
      correct_val, 3);
}

BOOST_FIXTURE_TEST_CASE( triple10, TestMatrix )
{
  std::map<graphblas::Index,graphblas::Index> out_edge;
  out_edge.insert({{1,1}, {2,0}, {4,1}, {5,2}, {6,1}});
  std::vector<graphblas::Index> correct_ind = {1, 2, 3, 4, 5, 6};
  std::vector<graphblas::T> correct_val = {1, 1, 1, 2, 2, 1};
  testEwiseAdd( "data/small/test_cc.tsv", out_edge, 4, correct_ind,
      correct_val, 2);
}

BOOST_FIXTURE_TEST_CASE( triple11, TestMatrix )
{
  std::map<graphblas::Index,graphblas::Index> out_edge;
  out_edge.insert( {{1,1}, {2,0}, {4,1}, {5,2}, {6,1}, {7,1}, {8,10}} );
  std::vector<graphblas::Index> correct_ind = {1, 2, 3, 4, 5, 6, 7, 8};
  std::vector<graphblas::T> correct_val = {1, 1, 1, 2, 2, 1, 1, 10};
  testEwiseAdd( "data/small/test_cc.tsv", out_edge, 7, correct_ind,
      correct_val, 1);
}

BOOST_FIXTURE_TEST_CASE( triple12, TestMatrix )
{
  std::map<graphblas::Index,graphblas::Index> out_edge;
  out_edge.insert({{2, 1}, {3, 0}, {4,1}});
  std::vector<graphblas::Index> correct = {1, 2, 3, 0, 2, 4, 3, 4, -1, 5, 6, 2, 6, -1, 6, 8, 9, 10, 9, 10};
  testEwiseSubCol( "data/small/test_cc.tsv", out_edge, correct);
}

BOOST_FIXTURE_TEST_CASE( triple13, TestMatrix )
{
  testResize( "data/small/test_cc.tsv" );
}

BOOST_FIXTURE_TEST_CASE( triple14, TestMatrix )
{
  testResize( "data/small/chesapeake.tsv" );
}

BOOST_FIXTURE_TEST_CASE( triple15, TestMatrix )
{
  testRequest( "data/small/test_cc.tsv", 5 );
}

BOOST_FIXTURE_TEST_CASE( triple16, TestMatrix )
{
  testRequest( "data/small/chesapeake.tsv", 5 );
}

BOOST_FIXTURE_TEST_CASE( triple17, TestMatrix )
{
  testRequest( "data/static/simulated_blockmodel_graph_1000_nodes.tsv", 5 );
}

BOOST_FIXTURE_TEST_CASE( triple18, TestMatrix )
{
  std::map<graphblas::Index,graphblas::Index> out_edge;
  out_edge.insert( {{1,1}, {2,0}, {4,1}, {5,2}, {6,1}, {7,1}, {8,10}} );
  std::vector<graphblas::Index> correct_ind = {1, 2, 3, 0, 2, 4, 5, 3, 4, 5, 5, 6, 2, 5, 6, 5, 6, 5, 5, 8, 9, 10, 5, 9, 10 };
  std::vector<graphblas::T>     correct_val = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 1, 2, 1, 1, 1, 1, 1, 1, 10, 1, 1 };
  testEwiseAddCol( "data/small/test_cc.tsv", out_edge, correct_ind, correct_val);
}*/

BOOST_AUTO_TEST_SUITE_END()


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Copyright 2012 The MITRE Corporation                                      *
 *                                                                           *
 * Licensed under the Apache License, Version 2.0 (the "License");           *
 * you may not use this file except in compliance with the License.          *
 * You may obtain a copy of the License at                                   *
 *                                                                           *
 *     http://www.apache.org/licenses/LICENSE-2.0                            *
 *                                                                           *
 * Unless required by applicable law or agreed to in writing, software       *
 * distributed under the License is distributed on an "AS IS" BASIS,         *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  *
 * See the License for the specific language governing permissions and       *
 * limitations under the License.                                            *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <Eigen/Dense>

#include <openbr/plugins/openbr_internal.h>

using namespace cv;

namespace br
{

/*!
 * \ingroup transforms
 * \brief Construct Template in a recursive decent manner.
 * \author Josh Klontz \cite jklontz
 */
class RecursiveIntegralSamplerTransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(int scales READ get_scales WRITE set_scales RESET reset_scales STORED false)
    Q_PROPERTY(float scaleFactor READ get_scaleFactor WRITE set_scaleFactor RESET reset_scaleFactor STORED false)
    Q_PROPERTY(int minSize READ get_minSize WRITE set_minSize RESET reset_minSize STORED false)
    Q_PROPERTY(br::Transform *transform READ get_transform WRITE set_transform RESET reset_transform)
    BR_PROPERTY(int, scales, 6)
    BR_PROPERTY(float, scaleFactor, 2)
    BR_PROPERTY(int, minSize, 8)
    BR_PROPERTY(br::Transform*, transform, NULL)

    Transform *subTransform;

    typedef Eigen::Map< const Eigen::Matrix<qint32,Eigen::Dynamic,1> > InputDescriptor;
    typedef Eigen::Map< Eigen::Matrix<float,Eigen::Dynamic,1> > OutputDescriptor;
    typedef Eigen::Map< const Eigen::Matrix<float,Eigen::Dynamic,1> > SecondOrderInputDescriptor;

    void init()
    {
        if (scales >= 2) {
            File subFile = file;
            subFile.set("scales", scales-1);
            subTransform = make(subFile.flat());
        } else {
            subTransform = NULL;
        }
    }

    static void integralHistogram(const Mat &src, const int x, const int y, const int width, const int height, Mat &dst, int index)
    {
        const int channels = src.channels();
        OutputDescriptor(dst.ptr<float>(index), channels, 1) =
            (  InputDescriptor(src.ptr<qint32>(y+height, x+width), channels, 1)
             - InputDescriptor(src.ptr<qint32>(y,        x+width), channels, 1)
             - InputDescriptor(src.ptr<qint32>(y+height, x),       channels, 1)
             + InputDescriptor(src.ptr<qint32>(y,        x),       channels, 1)).cast<float>()/(height*width);
    }

    void computeDescriptor(const Mat &src, Mat &dst) const
    {
        const int channels = src.channels();
        const int rows = src.rows-1; // Integral images have an extra row and column
        const int columns = src.cols-1;

        Mat tmp(5, channels, CV_32FC1);
        integralHistogram(src,         0,      0, columns/2, rows/2, tmp, 0);
        integralHistogram(src, columns/2,      0, columns/2, rows/2, tmp, 1);
        integralHistogram(src,         0, rows/2, columns/2, rows/2, tmp, 2);
        integralHistogram(src, columns/2, rows/2, columns/2, rows/2, tmp, 3);
        integralHistogram(src, columns/4, rows/4, columns/2, rows/2, tmp, 4);
        const SecondOrderInputDescriptor a(tmp.ptr<float>(0), channels, 1);
        const SecondOrderInputDescriptor b(tmp.ptr<float>(1), channels, 1);
        const SecondOrderInputDescriptor c(tmp.ptr<float>(2), channels, 1);
        const SecondOrderInputDescriptor d(tmp.ptr<float>(3), channels, 1);
        const SecondOrderInputDescriptor e(tmp.ptr<float>(4), channels, 1);

        dst = Mat(5, channels, CV_32FC1);
        OutputDescriptor(dst.ptr<float>(0), channels, 1) = (a+b+c+d)/4.f;
        OutputDescriptor(dst.ptr<float>(1), channels, 1) = ((a+b+c+d)/4.f-e);
        OutputDescriptor(dst.ptr<float>(2), channels, 1) = ((a+b)-(c+d))/2.f;
        OutputDescriptor(dst.ptr<float>(3), channels, 1) = ((a+c)-(b+d))/2.f;
        OutputDescriptor(dst.ptr<float>(4), channels, 1) = ((a+d)-(b+c))/2.f;
        dst = dst.reshape(1, 1);
    }

    Template subdivide(const Template &src) const
    {
        // Integral images have an extra row and column
        int subWidth = (src.m().cols-1) / scaleFactor + 1;
        int subHeight = (src.m().rows-1) / scaleFactor + 1;
        return Template(src.file, QList<Mat>() << Mat(src, Rect(0,                     0, subWidth, subHeight))
                                               << Mat(src, Rect(src.m().cols-subWidth, 0, subWidth, subHeight))
                                               << Mat(src, Rect(0,                     src.m().rows-subHeight, subWidth, subHeight))
                                               << Mat(src, Rect(src.m().cols-subWidth, src.m().rows-subHeight, subWidth, subHeight)));
    }

    bool canSubdivide(const Template &t) const
    {
        // Integral images have an extra row and column
        const int subWidth = (t.m().cols-1) / scaleFactor;
        const int subHeight = (t.m().rows-1) / scaleFactor;
        return ((subWidth >= minSize) && (subHeight >= minSize));
    }

    void train(const TemplateList &src)
    {
        if (src.first().m().depth() != CV_32S)
            qFatal("Expected CV_32S depth!");

        if (subTransform != NULL) {
            TemplateList subSrc; subSrc.reserve(src.size());
            foreach (const Template &t, src)
                if (canSubdivide(t))
                    subSrc.append(subdivide(t));

            if (subSrc.isEmpty()) {
                delete subTransform;
                subTransform = NULL;
            } else {
                subTransform->train(subSrc);
            }
        }

        TemplateList dst; dst.reserve(src.size());
        foreach (const Template &t, src) {
            Template u(t.file);
            computeDescriptor(t, u);
            dst.append(u);
        }
        transform->train(dst);
    }

    void project(const Template &src, Template &dst) const
    {
        computeDescriptor(src, dst);
        transform->project(dst, dst);

        if ((subTransform != NULL) && canSubdivide(src)) {
            Template subDst;
            subTransform->project(subdivide(src), subDst);
            dst.append(subDst);
        }
    }

    void store(QDataStream &stream) const
    {
        transform->store(stream);
        stream << (subTransform != NULL);
        if (subTransform != NULL)
            subTransform->store(stream);
    }

    void load(QDataStream &stream)
    {
        transform->load(stream);
        bool hasSubTransform;
        stream >> hasSubTransform;
        if (hasSubTransform) subTransform->load(stream);
        else                 { delete subTransform; subTransform = NULL; }
    }
};

BR_REGISTER(Transform, RecursiveIntegralSamplerTransform)

} // namespace br

#include "imgproc/recursiveintegralsampler.moc"