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

    Use, modification and distribution is subject to the Boost Software
    License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
////////////////////////////////////////////////////////////////////////////
//
//  Full calculator example using STL functors
//  This is discussed in the "Functional" chapter in the Spirit User's Guide.
//
//  Ported to Spirit v1.5 from v1.2/1.3 example by Dan Nuffer
//  [ JDG 9/18/2002 ]
//
////////////////////////////////////////////////////////////////////////////
#include <boost/spirit/core.hpp>
#include <iostream>
#include <stack>
#include <functional>
#include <string>

////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace boost::spirit;

////////////////////////////////////////////////////////////////////////////
//
//  Semantic actions
//
////////////////////////////////////////////////////////////////////////////
struct push_int
{
    push_int(stack<long>& eval_)
    : eval(eval_) {}

    void operator()(char const* str, char const* /*end*/) const
    {
        long n = strtol(str, 0, 10);
        eval.push(n);
        cout << "push\t" << long(n) << endl;
    }

    stack<long>& eval;
};

template <typename op>
struct do_op
{
    do_op(op const& the_op, stack<long>& eval_)
    : m_op(the_op), eval(eval_) {}

    void operator()(char const*, char const*) const
    {
        long rhs = eval.top();
        eval.pop();
        long lhs = eval.top();
        eval.pop();

        cout << "popped " << lhs << " and " << rhs << " from the stack. ";
        cout << "pushing " << m_op(lhs, rhs) << " onto the stack.\n";
        eval.push(m_op(lhs, rhs));
    }

    op m_op;
    stack<long>& eval;
};

template <class op>
do_op<op>
make_op(op const& the_op, stack<long>& eval)
{
    return do_op<op>(the_op, eval);
}

struct do_negate
{
    do_negate(stack<long>& eval_)
    : eval(eval_) {}

    void operator()(char const*, char const*) const
    {
        long lhs = eval.top();
        eval.pop();

        cout << "popped " << lhs << " from the stack. ";
        cout << "pushing " << -lhs << " onto the stack.\n";
        eval.push(-lhs);
    }

    stack<long>& eval;
};

////////////////////////////////////////////////////////////////////////////
//
//  Our calculator grammar
//
////////////////////////////////////////////////////////////////////////////
struct calculator : public grammar<calculator>
{
    calculator(stack<long>& eval_)
    : eval(eval_) {}

    template <typename ScannerT>
    struct definition
    {
        definition(calculator const& self)
        {
            integer =
                lexeme_d[ (+digit_p)[push_int(self.eval)] ]
                ;

            factor =
                    integer
                |   '(' >> expression >> ')'
                |   ('-' >> factor)[do_negate(self.eval)]
                |   ('+' >> factor)
                ;

            term =
                factor
                >> *(   ('*' >> factor)[make_op(multiplies<long>(), self.eval)]
                    |   ('/' >> factor)[make_op(divides<long>(), self.eval)]
                    )
                    ;

            expression =
                term
                >> *(  ('+' >> term)[make_op(plus<long>(), self.eval)]
                    |   ('-' >> term)[make_op(minus<long>(), self.eval)]
                    )
                    ;
        }

        rule<ScannerT> expression, term, factor, integer;
        rule<ScannerT> const&
        start() const { return expression; }
    };

    stack<long>& eval;
};

////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tThe simplest working calculator...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "Type an expression...or [q or Q] to quit\n\n";

    stack<long> eval;
    calculator  calc(eval); //  Our parser

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

        parse_info<> info = parse(str.c_str(), calc, space_p);

        if (info.full)
        {
            cout << "-------------------------\n";
            cout << "Parsing succeeded\n";
            cout << "-------------------------\n";
        }
        else
        {
            cout << "-------------------------\n";
            cout << "Parsing failed\n";
            cout << "stopped at: \": " << info.stop << "\"\n";
            cout << "-------------------------\n";
        }
    }

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


#define BOOST_TEST_MODULE "ValueBundleTest"
#include <boost/test/included/unit_test.hpp>
#include "lib/ValueBundle.hpp"

BOOST_AUTO_TEST_SUITE(ValueBundleTest);

BOOST_AUTO_TEST_CASE(ValueBundleArithmetic)
{
	const int dataSize = 16;
	float data1[dataSize];
	float data2[dataSize];
	float data3[dataSize];

	auto reset = [](ValueBundle<float> &b)->void {
		for (int i = 0; i < dataSize; ++i) b[i] = float(i);
	};

	ValueBundle<float> b1 = ValueBundle<float>(&data1[0], dataSize);
	ValueBundle<float> b2 = ValueBundle<float>(&data2[0], dataSize);
	ValueBundle<float> b3 = ValueBundle<float>(&data3[0], dataSize);

	BOOST_CHECK_EQUAL(b1.size(), dataSize);

	reset(b1);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i));

	reset(b1);
	reset(b2);
	b1.multAdd(2.0f, b2);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i + 2.0f * i));

	reset(b1);
	reset(b2);
	b1.multAdd(b2, 2.0f);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i + 2.0f * i));

	reset(b1);
	reset(b2);
	reset(b3);
	b1.multAdd(b2, b3);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i + i * i));

	reset(b1);
	reset(b2);
	reset(b3);
	b1.multAdd(2.0f, b2, b3);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i + 2.0f * i * i));

	reset(b1);
	reset(b2);
	b1.multSub(2.0f, b2);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i - 2.0f * i));

	reset(b1);
	reset(b2);
	b1.multSub(b2, 2.0f);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i - 2.0f * i));

	reset(b1);
	reset(b2);
	reset(b3);
	b1.multSub(b2, b3);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i - i * i));

	reset(b1);
	reset(b2);
	reset(b3);
	b1.multSub(2.0f, b2, b3);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i - 2.0f * i * i));

	reset(b1);
	reset(b2);
	b1 += b2;
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i + i));

	reset(b1);
	reset(b2);
	b1 -= b2;
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i - i));

	reset(b1);
	b1 *= 2.0f;
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i * 2.0f));

	reset(b1);
	b1 /= 2.0f;
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(b1[i], float(i / 2.0f));
}

BOOST_AUTO_TEST_CASE(ValueSuperbundleArithmetic)
{
	const int dataSize = 16;

	auto reset = [](ValueSuperbundle<float,2> &s)->void {
		for (int i = 0; i < 2; ++i)
		{
			auto b = s.bundle(i);
			for (int j = 0; j < dataSize; ++j) b[j] = float(j);
		}
	};

	ValueSuperbundle<float, 2> s1 = ValueSuperbundle<float, 2>(dataSize);
	ValueSuperbundle<float, 2> s2 = ValueSuperbundle<float, 2>(dataSize);

	s1.reset();
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(0)[i], 0.0f);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(1)[i], 0.0f);

	ValueSuperbundle<float, 2> s3 = ValueSuperbundle<float, 2>(s1);
	s1.bundle(0)[0] = 42.0f;
	s3.bundle(0)[1] = 43.0f;
	BOOST_CHECK_EQUAL(s3.bundle(0)[0], 42.0f);
	BOOST_CHECK_EQUAL(s1.bundle(0)[1], 43.0f);

	reset(s1);
	reset(s2);
	s1.multAdd(2.0f, s2);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(0)[i], float(i + 2.0f * i));
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(1)[i], float(i + 2.0f * i));

	reset(s1);
	s1 *= 2.0f;
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(0)[i], float(2.0f * i));
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(1)[i], float(2.0f * i));

	reset(s1);
	s1 /= 2.0f;
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(0)[i], float(i) / 2.0f);
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(1)[i], float(i) / 2.0f);

	reset(s1);
	reset(s2);
	s1 += s2;
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(0)[i], float(2.0f * i));
	for (int i = 0; i < dataSize; ++i) BOOST_CHECK_EQUAL(s1.bundle(1)[i], float(2.0f * i));
}
BOOST_AUTO_TEST_SUITE_END();


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2013 Gauthier Brun <brun.gauthier@gmail.com>
// Copyright (C) 2013 Nicolas Carre <nicolas.carre@ensimag.fr>
// Copyright (C) 2013 Jean Ceccato <jean.ceccato@ensimag.fr>
// Copyright (C) 2013 Pierre Zoppitelli <pierre.zoppitelli@ensimag.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/

#include "svd_common.h"
#include <iostream>
#include <Eigen/LU>

// check if "svd" is the good image of "m"
template<typename MatrixType>
void bdcsvd_check_full(const MatrixType& m, const BDCSVD<MatrixType>& svd)
{
  svd_check_full< MatrixType, BDCSVD< MatrixType > >(m, svd);
}

// Compare to a reference value
template<typename MatrixType>
void bdcsvd_compare_to_full(const MatrixType& m,
			    unsigned int computationOptions,
			    const BDCSVD<MatrixType>& referenceSvd)
{
  svd_compare_to_full< MatrixType, BDCSVD< MatrixType > >(m, computationOptions, referenceSvd);
} // end bdcsvd_compare_to_full

template<typename MatrixType>
void bdcsvd_solve(const MatrixType& m, unsigned int computationOptions)
{
  svd_solve< MatrixType, BDCSVD< MatrixType > >(m, computationOptions);
} //  end template bdcsvd_solve

// test the computations options
template<typename MatrixType>
void bdcsvd_test_all_computation_options(const MatrixType& m)
{
  BDCSVD<MatrixType> fullSvd(m, ComputeFullU|ComputeFullV);
  svd_test_computation_options_1< MatrixType, BDCSVD< MatrixType > >(m, fullSvd);
  svd_test_computation_options_2< MatrixType, BDCSVD< MatrixType > >(m, fullSvd);
} // end bdcsvd_test_all_computation_options

// Call a test with all the computations options
template<typename MatrixType>
void bdcsvd(const MatrixType& a = MatrixType(), bool pickrandom = true)
{
  MatrixType m = pickrandom ? MatrixType::Random(a.rows(), a.cols()) : a;
  bdcsvd_test_all_computation_options<MatrixType>(m);
} // end template bdcsvd

// verify assert
template<typename MatrixType>
void bdcsvd_verify_assert(const MatrixType& m)
{
  svd_verify_assert< MatrixType, BDCSVD< MatrixType > >(m);
}// end template bdcsvd_verify_assert

// test weird values
template<typename MatrixType>
void bdcsvd_inf_nan()
{
  svd_inf_nan< MatrixType, BDCSVD< MatrixType > >();
}// end template bdcsvd_inf_nan

void bdcsvd_preallocate()
{
  svd_preallocate< BDCSVD< MatrixXf > >();
} // end bdcsvd_preallocate

// compare the Singular values returned with Jacobi and Bdc
template<typename MatrixType>
void compare_bdc_jacobi(const MatrixType& a = MatrixType(), unsigned int computationOptions = 0)
{
  std::cout << "debut compare" << std::endl;
  MatrixType m = MatrixType::Random(a.rows(), a.cols());
  BDCSVD<MatrixType> bdc_svd(m);
  JacobiSVD<MatrixType> jacobi_svd(m);
  VERIFY_IS_APPROX(bdc_svd.singularValues(), jacobi_svd.singularValues());
  if(computationOptions & ComputeFullU)
    VERIFY_IS_APPROX(bdc_svd.matrixU(), jacobi_svd.matrixU());
  if(computationOptions & ComputeThinU)
    VERIFY_IS_APPROX(bdc_svd.matrixU(), jacobi_svd.matrixU());
  if(computationOptions & ComputeFullV)
    VERIFY_IS_APPROX(bdc_svd.matrixV(), jacobi_svd.matrixV());
  if(computationOptions & ComputeThinV)
    VERIFY_IS_APPROX(bdc_svd.matrixV(), jacobi_svd.matrixV());
  std::cout << "fin compare" << std::endl;
} // end template compare_bdc_jacobi

// call the tests
void test_bdcsvd()
{
  // test of Dynamic defined Matrix (42, 42) of float
  CALL_SUBTEST_11(( bdcsvd_verify_assert<Matrix<float,Dynamic,Dynamic> >
		    (Matrix<float,Dynamic,Dynamic>(42,42)) ));
  CALL_SUBTEST_11(( compare_bdc_jacobi<Matrix<float,Dynamic,Dynamic> >
		    (Matrix<float,Dynamic,Dynamic>(42,42), 0) ));
  CALL_SUBTEST_11(( bdcsvd<Matrix<float,Dynamic,Dynamic> >
		    (Matrix<float,Dynamic,Dynamic>(42,42)) ));

  // test of Dynamic defined Matrix (50, 50) of double
  CALL_SUBTEST_13(( bdcsvd_verify_assert<Matrix<double,Dynamic,Dynamic> >
		    (Matrix<double,Dynamic,Dynamic>(50,50)) ));
  CALL_SUBTEST_13(( compare_bdc_jacobi<Matrix<double,Dynamic,Dynamic> >
		    (Matrix<double,Dynamic,Dynamic>(50,50), 0) ));
  CALL_SUBTEST_13(( bdcsvd<Matrix<double,Dynamic,Dynamic> >
		    (Matrix<double,Dynamic,Dynamic>(50, 50)) ));

  // test of Dynamic defined Matrix (22, 22) of complex double
  CALL_SUBTEST_14(( bdcsvd_verify_assert<Matrix<std::complex<double>,Dynamic,Dynamic> >
  		    (Matrix<std::complex<double>,Dynamic,Dynamic>(22,22)) ));
  CALL_SUBTEST_14(( compare_bdc_jacobi<Matrix<std::complex<double>,Dynamic,Dynamic> >
  		    (Matrix<std::complex<double>, Dynamic, Dynamic> (22,22), 0) ));
  CALL_SUBTEST_14(( bdcsvd<Matrix<std::complex<double>,Dynamic,Dynamic> >
  		    (Matrix<std::complex<double>,Dynamic,Dynamic>(22, 22)) ));

  // test of Dynamic defined Matrix (10, 10) of int
  //CALL_SUBTEST_15(( bdcsvd_verify_assert<Matrix<int,Dynamic,Dynamic> >
  //		    (Matrix<int,Dynamic,Dynamic>(10,10)) ));
  //CALL_SUBTEST_15(( compare_bdc_jacobi<Matrix<int,Dynamic,Dynamic> >
  //		    (Matrix<int,Dynamic,Dynamic>(10,10), 0) ));
  //CALL_SUBTEST_15(( bdcsvd<Matrix<int,Dynamic,Dynamic> >
  //		    (Matrix<int,Dynamic,Dynamic>(10, 10)) ));

  // test of Dynamic defined Matrix (8, 6) of double

  CALL_SUBTEST_16(( bdcsvd_verify_assert<Matrix<double,Dynamic,Dynamic> >
		    (Matrix<double,Dynamic,Dynamic>(8,6)) ));
  CALL_SUBTEST_16(( compare_bdc_jacobi<Matrix<double,Dynamic,Dynamic> >
		    (Matrix<double,Dynamic,Dynamic>(8, 6), 0) ));
  CALL_SUBTEST_16(( bdcsvd<Matrix<double,Dynamic,Dynamic> >
		    (Matrix<double,Dynamic,Dynamic>(8, 6)) ));

  // test of Dynamic defined Matrix (36, 12) of float
  CALL_SUBTEST_17(( compare_bdc_jacobi<Matrix<float,Dynamic,Dynamic> >
		    (Matrix<float,Dynamic,Dynamic>(36, 12), 0) ));
  CALL_SUBTEST_17(( bdcsvd<Matrix<float,Dynamic,Dynamic> >
		    (Matrix<float,Dynamic,Dynamic>(36, 12)) ));

  // test of Dynamic defined Matrix (5, 8) of double
  CALL_SUBTEST_18(( compare_bdc_jacobi<Matrix<double,Dynamic,Dynamic> >
		    (Matrix<double,Dynamic,Dynamic>(5, 8), 0) ));
  CALL_SUBTEST_18(( bdcsvd<Matrix<double,Dynamic,Dynamic> >
		    (Matrix<double,Dynamic,Dynamic>(5, 8)) ));

  // non regression tests
  CALL_SUBTEST_3(( bdcsvd_verify_assert(Matrix3f()) ));
  CALL_SUBTEST_4(( bdcsvd_verify_assert(Matrix4d()) ));
  CALL_SUBTEST_7(( bdcsvd_verify_assert(MatrixXf(10,12)) ));
  CALL_SUBTEST_8(( bdcsvd_verify_assert(MatrixXcd(7,5)) ));

  // SUBTESTS 1 and 2 on specifics matrix
  for(int i = 0; i < g_repeat; i++) {
    Matrix2cd m;
    m << 0, 1,
      0, 1;
    CALL_SUBTEST_1(( bdcsvd(m, false) ));
    m << 1, 0,
      1, 0;
    CALL_SUBTEST_1(( bdcsvd(m, false) ));

    Matrix2d n;
    n << 0, 0,
      0, 0;
    CALL_SUBTEST_2(( bdcsvd(n, false) ));
    n << 0, 0,
      0, 1;
    CALL_SUBTEST_2(( bdcsvd(n, false) ));

    // Statics matrix don't work with BDSVD yet
    // bdc algo on a random 3x3 float matrix
    // CALL_SUBTEST_3(( bdcsvd<Matrix3f>() ));
    // bdc algo on a random 4x4 double matrix
    // CALL_SUBTEST_4(( bdcsvd<Matrix4d>() ));
    // bdc algo on a random 3x5 float matrix
    // CALL_SUBTEST_5(( bdcsvd<Matrix<float,3,5> >() ));

    int r = internal::random<int>(1, 30),
      c = internal::random<int>(1, 30);
    CALL_SUBTEST_7(( bdcsvd<MatrixXf>(MatrixXf(r,c)) ));
    CALL_SUBTEST_8(( bdcsvd<MatrixXcd>(MatrixXcd(r,c)) ));
    (void) r;
    (void) c;

    // Test on inf/nan matrix
    CALL_SUBTEST_7( bdcsvd_inf_nan<MatrixXf>() );
  }

  CALL_SUBTEST_7(( bdcsvd<MatrixXf>(MatrixXf(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) ));
  CALL_SUBTEST_8(( bdcsvd<MatrixXcd>(MatrixXcd(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/3), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/3))) ));

  // Test problem size constructors
  CALL_SUBTEST_7( BDCSVD<MatrixXf>(10,10) );

} // end test_bdcsvd


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

  @copyright 2016 NumScale SAS

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

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

 /*!
    @ingroup group-ieee
    Function object implementing fpclassify capabilities

    Categorizes floating point value arg into the following categories:
    zero, subnormal, normal, infinite, NAN, or implementation-defined category.

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

    This function is similar to std::fpclassify,  but the return type
    is the integral signed type associated to the floating input type.

    If you want the standard behaviour which return an int in scalar mode you
    can use the std_ decorator.

    fpclassify returns a value of integral type that matches one of the classification
    macro constants, depending on the value of x:

    value description:

    - FP_INFINITE Positive or negative infinity
    - FP_NAN  Not-A-Number
    - FP_ZERO Value of zero
    - FP_SUBNORMAL  Sub-normal value
    - FP_NORMAL Normal value (none of the above)

    Note that each value pertains to a single category: for fpclassify zero is not a
    normal value.

    These macro constants of type int are defined in header cmath

    @par Decorators

    std_ for floating entries

    @see is_eqz, is_denormal, is_normal, is_inf, is_nan

  **/
  as_integer_t<Value> fpclassify(Value const & v0);
} }
#endif

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

#endif


#define PY_ARRAY_UNIQUE_SYMBOL superimg_PyArray_API
#define NO_IMPORT_ARRAY

#include <string>
#include <cmath>

#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <boost/array.hpp>

#include <boost/accumulators/accumulators.hpp>

#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/extended_p_square_quantile.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>

#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "seglib/cgp2d/cgp2d.hxx"
#include "seglib/cgp2d/cgp2d_python.hxx"

namespace python = boost::python;

namespace cgp2d {

    vigra::NumpyAnyArray  regionAffinity(
        vigra::NumpyArray<2, vigra::Multiband<vigra::UInt64>  >   labelings,
        vigra::NumpyArray<1, vigra::UInt64>                       nLabels,
        //output
        vigra::NumpyArray<2, float >    res = vigra::NumpyArray<4, float >()
    ){
        const size_t nSeg = labelings.shape(1);
        const size_t nReg = labelings.shape(1);
        // allocate output
        typedef typename vigra::NumpyArray<2, float >::difference_type Shape2;
        Shape2 shape(nReg,nReg);
        res.reshapeIfEmpty(shape);
        std::fill(res.begin(),res.end(),0.0);

        for(size_t s=0;s<nSeg;++s){

            const size_t nL=nLabels(s);

            std::vector< std::vector< vigra::UInt64> > regWithLabel;
            for(size_t r=0;r<nReg;++r){

                const vigra::UInt64 label=labelings(r,s);
                regWithLabel[label].push_back(r);
            }

            for(size_t l=0;l<nL;++l){

                for(size_t r0=0;    r0<regWithLabel[l].size()-1 ;++r0)
                for(size_t r1=r0+1; r1<regWithLabel[l].size()   ;++r1){
                    res[r0,r1]+=1.0;
                    res[r1,r0]+=1.0;
                }
            }
        }
        return res;
    }

    void export_segcompare(){

        python::def("_regionAffinity",vigra::registerConverters(&regionAffinity),
            (
                python::arg("labelings"),
                python::arg("nLabels"),
                python::arg("out")=python::object()
            )
        );

    }

}


/* Copyright (C) 2012-2019 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

/* Test_bootstrapping.cpp - Testing the recryption procedure */

#if defined(__unix__) || defined(__unix) || defined(unix)
#include <sys/time.h>
#include <sys/resource.h>
#endif

#include <NTL/ZZ.h>
#include <NTL/fileio.h>
#include <NTL/BasicThreadPool.h>
NTL_CLIENT
#include <cassert>
#include <helib/EncryptedArray.h>
#include <helib/EvalMap.h>
#include <helib/powerful.h>
#include <helib/matmul.h>
#include <helib/debugging.h>
#include <helib/fhe_stats.h>
#include <helib/ArgMap.h>

using namespace helib;

static bool noPrint = false;
static bool dry = false; // a dry-run flag
static bool debug = 0;   // a debug flag
static int scale = 0;

extern long printFlag;

#define OUTER_REP (1)
#define INNER_REP (1)

static Vec<long> global_mvec, global_gens, global_ords;
static int c_m = 100;

void TestIt(long p, long r, long L, long c, long skHwt, int build_cache=0)
{
  Vec<long> mvec;
  vector<long> gens;
  vector<long> ords;

  long m, phim;

    mvec = global_mvec;
    convert(gens, global_gens);
    convert(ords, global_ords);

    m = computeProd(mvec);
    phim = phi_N(m);
    helib::assertTrue(GCD(p, m) == 1, "GCD(p, m) == 1");

  if (!noPrint) fhe_stats = true;

  if (!noPrint) {
    cout << "*** TestIt";
    if (isDryRun()) cout << " (dry run)";
    cout << ": p=" << p
	 << ", r=" << r
	 << ", L=" << L
	 << ", t=" << skHwt
	 << ", c=" << c
	 << ", m=" << m
	 << " (=" << mvec << "), gens="<<gens<<", ords="<<ords
	 << endl;
    cout << "Computing key-independent tables..." << std::flush;
  }
  setTimersOn();
  setDryRun(false); // Need to get a "real context" to test bootstrapping

  double t = -GetTime();
  Context context(m, p, r, gens, ords);
  if (scale) {
    context.scale = scale;
  }

  context.zMStar.set_cM(c_m/100.0);
  buildModChain(context, L, c, /*willBeBootstrappable=*/true, /*t=*/skHwt);

  if (!noPrint) {
    std::cout << "security=" << context.securityLevel()<<endl;
    std::cout << "# small primes = " << context.smallPrimes.card() << "\n";
    std::cout << "# ctxt primes = " << context.ctxtPrimes.card() << "\n";
    std::cout << "# bits in ctxt primes = "
         << long(context.logOfProduct(context.ctxtPrimes)/log(2.0) + 0.5) << "\n";
    std::cout << "# special primes = " << context.specialPrimes.card() << "\n";
    std::cout << "# bits in special primes = "
         << long(context.logOfProduct(context.specialPrimes)/log(2.0) + 0.5) << "\n";
    std::cout << "scale=" << context.scale<<endl;
  }

  context.makeBootstrappable(mvec, /*t=*/skHwt, build_cache);
  t += GetTime();

  if (!noPrint) {
    cout << " done in "<<t<<" seconds\n";
    cout << "  e="    << context.rcData.e
	 << ", e'="   << context.rcData.ePrime
	 << ", t="    << context.rcData.skHwt
	 << "\n  ";
    context.zMStar.printout();
  }
  setDryRun(dry); // Now we can set the dry-run flag if desired

  long p2r = context.alMod.getPPowR();

  for (long numkey=0; numkey<OUTER_REP; numkey++) { // test with 3 keys

  t = -GetTime();
  if (!noPrint) cout << "Generating keys, " << std::flush;
  SecKey secretKey(context);
  PubKey& publicKey = secretKey;
  secretKey.GenSecKey(skHwt);      // A +-1/0 secret key
  addSome1DMatrices(secretKey); // compute key-switching matrices that we need
  addFrbMatrices(secretKey);
  if (!noPrint) cout << "computing key-dependent tables..." << std::flush;
  secretKey.genRecryptData();
  t += GetTime();
  if (!noPrint) cout << " done in "<<t<<" seconds\n";

  zz_p::init(p2r);
  zz_pX poly_p = random_zz_pX(context.zMStar.getPhiM());
  zzX poly_p1 = balanced_zzX(poly_p);
  ZZX ptxt_poly = convert<ZZX>(poly_p1);
  ZZX ptxt_poly1;
  PolyRed(ptxt_poly1, ptxt_poly, p2r, true);
  // this is the format produced by decryption

#ifdef DEBUG_PRINTOUT
      dbgEa = (EncryptedArray*) context.ea;
      dbgKey = &secretKey;
#endif

  if (debug) {
    dbgKey = &secretKey; // debugging key
  }

  ZZX poly2;
  Ctxt c1(publicKey);

  secretKey.Encrypt(c1,ptxt_poly,p2r);

  resetAllTimers();
  for (long num=0; num<INNER_REP; num++) { // multiple tests with same key
    publicKey.reCrypt(c1);
    secretKey.Decrypt(poly2,c1);

    if (ptxt_poly1 == poly2)
      cout << "GOOD\n";
    else
      cout << "BAD\n";
  }
  }
  if (!noPrint) printAllTimers();
#if (defined(__unix__) || defined(__unix) || defined(unix))
    struct rusage rusage;
    getrusage( RUSAGE_SELF, &rusage );
    if (!noPrint) cout << "  rusage.ru_maxrss="<<rusage.ru_maxrss << endl;
#endif
  if (fhe_stats) print_stats(cout);
}

/********************************************************************
 ********************************************************************/

//extern long fhe_disable_intFactor;
// extern long fhe_force_chen_han;

int main(int argc, char *argv[])
{
  ArgMap amap;

  long p=2;
  long r=1;
  long c=3;
  long L=600;
  long N=0;
  long t=64;
  long nthreads=1;

  long seed=0;
  long useCache=1;

  amap.arg("p", p, "plaintext base");

  amap.arg("r", r,  "exponent");
  amap.note("p^r is the plaintext-space modulus");

  amap.arg("c", c, "number of columns in the key-switching matrices");
  amap.arg("L", L, "# of bits in the modulus chain");
  amap.arg("N", N, "lower-bound on phi(m)");
  amap.arg("t", t, "Hamming weight of recryption secret key", "heuristic");
  amap.arg("dry", dry, "dry=1 for a dry-run");
  amap.arg("nthreads", nthreads, "number of threads");
  amap.arg("seed", seed, "random number seed");
  amap.arg("noPrint", noPrint, "suppress printouts");
  amap.arg("useCache", useCache, "0: zzX cache, 1: DCRT cache");

  amap.arg("force_bsgs", fhe_test_force_bsgs);
  amap.arg("force_hoist", fhe_test_force_hoist);

  //  amap.arg("disable_intFactor", fhe_disable_intFactor);
  amap.arg("chen_han", fhe_force_chen_han);

  amap.arg("debug", debug, "generate debugging output");
  amap.arg("scale", scale, "scale parameter");

  amap.arg("gens", global_gens);
  amap.arg("ords", global_ords);
  amap.arg("mvec", global_mvec);
  amap.arg("c_m", c_m);

  amap.parse(argc, argv);

  if (global_gens.length() == 0 || global_ords.length() == 0 || global_mvec.length() == 0)
    Error("gens, ords, and mvec must be initialized");

  if (seed)
    SetSeed(ZZ(seed));

  SetNumThreads(nthreads);

  TestIt(p,r,L,c,t,useCache);

  return 0;
}


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#ifndef LIMBO_OPT_ADAM_HPP
#define LIMBO_OPT_ADAM_HPP

#include <algorithm>

#include <Eigen/Core>

#include <limbo/opt/optimizer.hpp>
#include <limbo/tools/macros.hpp>
#include <limbo/tools/math.hpp>

namespace limbo {
    namespace defaults {
        struct opt_adam {
            /// @ingroup opt_defaults
            /// number of max iterations
            BO_PARAM(int, iterations, 300);

            /// @ingroup opt_defaults
            /// alpha - learning rate
            BO_PARAM(double, alpha, 0.001);

            /// @ingroup opt_defaults
            /// β1
            BO_PARAM(double, b1, 0.9);

            /// @ingroup opt_defaults
            /// β2
            BO_PARAM(double, b2, 0.999);

            /// @ingroup opt_defaults
            /// norm epsilon for stopping
            BO_PARAM(double, eps_stop, 0.0);
        };
    } // namespace defaults
    namespace opt {
        /// @ingroup opt
        /// Adam optimizer
        /// Equations from: http://ruder.io/optimizing-gradient-descent/index.html#gradientdescentoptimizationalgorithms
        /// (I changed a bit the notation; η to α)
        ///
        /// Parameters:
        /// - int iterations
        /// - double alpha
        /// - double b1
        /// - double b2
        /// - double eps_stop
        template <typename Params>
        struct Adam {
            template <typename F>
            Eigen::VectorXd operator()(const F& f, const Eigen::VectorXd& init, bool bounded) const
            {
                assert(Params::opt_adam::b1() >= 0. && Params::opt_adam::b1() < 1.);
                assert(Params::opt_adam::b2() >= 0. && Params::opt_adam::b2() < 1.);
                assert(Params::opt_adam::alpha() >= 0.);

                size_t param_dim = init.size();
                double b1 = Params::opt_adam::b1();
                double b2 = Params::opt_adam::b2();
                double b1_t = b1;
                double b2_t = b2;
                double alpha = Params::opt_adam::alpha();
                double stop = Params::opt_adam::eps_stop();
                double epsilon = 1e-8;

                Eigen::VectorXd m = Eigen::VectorXd::Zero(param_dim);
                Eigen::VectorXd v = Eigen::VectorXd::Zero(param_dim);

                Eigen::VectorXd params = init;

                if (bounded) {
                    for (int j = 0; j < params.size(); j++) {
                        if (params(j) < 0)
                            params(j) = 0;
                        if (params(j) > 1)
                            params(j) = 1;
                    }
                }

                for (int i = 0; i < Params::opt_adam::iterations(); ++i) {
                    Eigen::VectorXd prev_params = params;
                    auto perf = opt::eval_grad(f, params);

                    Eigen::VectorXd grad = opt::grad(perf);
                    m = b1 * m.array() + (1. - b1) * grad.array();
                    v = b2 * v.array() + (1. - b2) * grad.array().square();

                    Eigen::VectorXd m_hat = m.array() / (1. - b1_t);
                    Eigen::VectorXd v_hat = v.array() / (1. - b2_t);

                    params.array() += alpha * m_hat.array() / (v_hat.array().sqrt() + epsilon);

                    b1_t *= b1;
                    b2_t *= b2;

                    if (bounded) {
                        for (int j = 0; j < params.size(); j++) {
                            if (params(j) < 0)
                                params(j) = 0;
                            if (params(j) > 1)
                                params(j) = 1;
                        }
                    }

                    if ((prev_params - params).norm() < stop)
                        break;
                }

                return params;
            }
        };
    } // namespace opt
} // namespace limbo

#endif


// Copyright 2010-2014, D. E. Shaw Research.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt )

#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/counter_based_engine.hpp>
#include <boost/limits.hpp>

typedef boost::random::counter_based_engine<BOOST_COUNTER_BASED_ENGINE_RESULT_TYPE, BOOST_PSEUDO_RANDOM_FUNCTION, BOOST_COUNTER_BASED_ENGINE_CTRBITS> engine_t;

#define BOOST_RANDOM_URNG engine_t

#include "test_generator.ipp"

// Now we can go on to test other aspects of engine_t...

// counter-based engines can do discards in O(1) time.  Let's
// check that many such "random" discards add up correctly.
void do_test_huge_discard(boost::uintmax_t bigjump){
    BOOST_RANDOM_URNG urng0;
    BOOST_RANDOM_URNG urng;
    BOOST_RANDOM_URNG urng2;
    BOOST_RANDOM_URNG urng3;
    BOOST_RANDOM_URNG urngn;

    BOOST_CHECK_EQUAL(urng, urng2);
    boost::uintmax_t n = 0;
    bool out_of_bits1 = false;
    bool out_of_bitsn = false;
    try{
        urng2.discard(bigjump);
    }catch(std::invalid_argument&){
        // It's ok if bigjump exceeds our sequence length.
        // It just means that we are testing a counter_based_engine
        // with a small-ish number of CounterBits.
        out_of_bits1 = true;
    }
    try{
        while(n < bigjump){
            if(!out_of_bits1)
                BOOST_CHECK_NE(urng, urng2);
            boost::random::uniform_int_distribution<boost::uintmax_t> d(1, 1+(bigjump-n)/73);;
            boost::uintmax_t smalljump = d(urng3);
            urng.discard(smalljump);
            n += smalljump;
            urngn = urng0;
            urngn.discard(n);
            BOOST_CHECK_EQUAL(urng, urngn);
        }
    }catch(std::invalid_argument&){
        out_of_bitsn = true;
    }
    BOOST_CHECK_EQUAL(out_of_bits1, out_of_bitsn);
    if(!out_of_bits1)
        BOOST_CHECK_EQUAL(urng, urng2);
}

BOOST_AUTO_TEST_CASE(test_huge_discard)
{
    // Run the discard check with numbers large enough to force us to
    // "carry" between the elements of the counter.
    do_test_huge_discard((std::numeric_limits<boost::uintmax_t>::max)() - 1);
    do_test_huge_discard(((std::numeric_limits<boost::uintmax_t>::max)()>>1) + 1);
    do_test_huge_discard(((std::numeric_limits<boost::uintmax_t>::max)()>>32) + 1);
    do_test_huge_discard((std::numeric_limits<BOOST_RANDOM_URNG::result_type>::max)());
}

// TODO: restart, seed(key), constructor(Prf, start), limited counter width.


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file basic_metrics.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-02-15
 */
#ifndef PAAL_BASIC_METRICS_HPP
#define PAAL_BASIC_METRICS_HPP

#include "metric_traits.hpp"

#include <boost/multi_array.hpp>
#include <boost/range/iterator_range.hpp>

#include <array>

namespace paal {
namespace data_structures {

/**
 * @class rectangle_array_metric
 * @brief \ref metric implementation on 2 dimensional array
 *        distance calls on this metric are valid opnly when x < N and y  < M
 *        (N and M given in the constructor)
 *        when we know that only certain calls occurs it might be worthwhile to
 * use this metric
 *
 * @tparam DistanceTypeParam
 */
template <typename DistanceTypeParam> class rectangle_array_metric {
  public:
    typedef DistanceTypeParam DistanceType;
    typedef int VertexType;
    /**
     * @brief constructor
     *
     * @param N
     * @param M
     */
    rectangle_array_metric(int N = 0, int M = 0)
        : m_matrix(boost::extents[N][M]) {}

    /**
     * @brief operator(), valid only when v < N and w < M
     *
     * @param v
     * @param w
     *
     * @return
     */
    DistanceType operator()(const VertexType &v, const VertexType &w) const {
        return m_matrix[v][w];
    }

    /**
     * @brief operator(), valid only when v < N and w < M, nonconst version
     *
     * @param v
     * @param w
     *
     * @return
     */
    DistanceType &operator()(const VertexType &v, const VertexType &w) {
        return m_matrix[v][w];
    }

    /**
     * @brief constructor from another metric
     *
     * @tparam OtherMetrics
     * @param other
     * @param xrange
     * @param yrange
     */
    template <typename OtherMetrics, typename XRange, typename YRange>
    rectangle_array_metric(const OtherMetrics &other, XRange && xrange
                           , YRange && yrange)
        : rectangle_array_metric(boost::distance(xrange),
                                 boost::distance(yrange)) {
        int i = 0;
        for (auto && v : xrange) {
            int j = 0;
            for (auto && w : yrange) {
                m_matrix[i][j] = other(v, w);
                ++j;
            }
            ++i;
        }
    }

    /**
     * @brief operator=
     *
     * @param am
     *
     * @return
     */
    rectangle_array_metric &operator=(const rectangle_array_metric &am) {
        auto shape = am.m_matrix.shape();
        std::vector<std::size_t> dim(shape, shape + DIM_NR);
        m_matrix.resize(dim);
        m_matrix = am.m_matrix;
        return *this;
    }

    ///operator==
    bool operator==(const rectangle_array_metric & other) const {
        return m_matrix == other.m_matrix;
    }

  protected:
    /**
     * @brief dimention of multi array
     */
    static const int DIM_NR = 2;
    typedef boost::multi_array<DistanceType, DIM_NR> matrix_type;
    /// matrix with data
    matrix_type m_matrix;
};

/**
 * @brief this metric is rectangle_array_metric with N == M.
 *
 * @tparam DistanceTypeParam
 */
template <typename DistanceTypeParam>
class array_metric : public rectangle_array_metric<DistanceTypeParam> {
    typedef rectangle_array_metric<DistanceTypeParam> base;

  public:
    /**
     * @brief constructor
     *
     * @param N
     */
    array_metric(int N = 0) : base(N, N) {}

    /**
     * @brief returns N
     *
     * @return
     */
    int size() const { return this->m_matrix.size(); }

    /**
     * @brief constructor from another metric
     *
     * @tparam OtherMetrics
     * @tparam Items
     * @param other
     * @param items
     */
    template <typename OtherMetrics, typename Items>
    array_metric(const OtherMetrics &other, Items && items)
        : base(other, items, items) {}
};
}
}
#endif // PAAL_BASIC_METRICS_HPP


//==============================================================================
//         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
//==============================================================================
#include <nt2/trigonometric/include/functions/rem_pio2_straight.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/trigonometric/constants.hpp>

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/constant/constant.hpp>

NT2_TEST_CASE_TPL ( rem_pio2_straight_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::rem_pio2_straight;
  using nt2::tag::rem_pio2_straight_;
  typedef typename nt2::meta::as_integer<T>::type iT;
 typedef std::pair<iT, T>  r_t;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<rem_pio2_straight_(T)>::type)
                  , (std::pair<iT,T>)
                  );

  {
    r_t res = rem_pio2_straight(nt2::Pio_2<T>());
    T r1;
    NT2_TEST_EQUAL( rem_pio2_straight(nt2::Pio_2<T>(), r1), nt2::One<iT>());
    NT2_TEST_ULP_EQUAL( r1, nt2::Zero<T>(), 0.5);
    NT2_TEST_EQUAL( rem_pio2_straight(nt2::Pio_4<T>(), r1), nt2::One<iT>());
    NT2_TEST_ULP_EQUAL( r1, -nt2::Pio_4<T>(), 0.5);
  }
}


#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <random>
#include <Eigen/Cholesky>
#include <fstream>
#include <iomanip>
#include <srrg_system_utils/parse_command_line.h>
#include <srrg_boss/deserializer.h>

#include "srrg_solver/solver_core/instances.h"
#include "srrg_solver/solver_core/factor_graph.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_se3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_point3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_point_offset_error_factor.h"

#include "srrg_solver/variables_and_factors/types_2d/variable_se2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/variable_point2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_point_error_factor.h"

#include "srrg_solver/utils/factor_graph_initializer.h"
#include "srrg_solver/solver_incremental/factor_graph_incremental_sorter.h"

using namespace srrg2_core;
using namespace srrg2_solver;
using namespace std;

extern char** environ;
const std::string exe_name = environ[0];
#define LOG std::cerr << exe_name << "|"

static const char* banner[] = {
  "initializes the factor graph,  attempting first a breadth first on the poses, then a local triangulation of the landmarks",
  "usage: solver_app_graph_initializer -i <input> -o <output>",
  0
};

// ia THE PROGRAM
int main(int argc, char** argv) {
  ParseCommandLine cmd_line(argv, banner);
  ArgumentString input_file          (&cmd_line, "i",    "input-file",             "file where to read the input ", "");
  ArgumentString output_file          (&cmd_line, "o",    "output-file",           "file where to save the output", "");
  ArgumentFlag incremental(&cmd_line, "s",    "incremental",           "if toggled, triggers incremental initialization");
  ArgumentFlag verbose(&cmd_line, "v",    "verbose",           "if toggled, bloats the screen with stuff");
  ArgumentString path_type          (&cmd_line, "t",    "path-type",             "type of the variable used for sorting", "VariableSE3QuaternionRightAD");

  cmd_line.parse();

  FactorGraphInitializer initializer;
  if (verbose.isSet())
    initializer.verbose=true;
  if (! input_file.isSet()) {
    cerr << "no input file provided, returning" << std::endl;
    return 0;
  }
  FactorGraphPtr graph;
  std::set<VariableBase::Id> parameter_ids;

  if (! incremental.isSet()) {
    std::cerr << "loading file: [" << input_file.value() << "]... ";
    graph = FactorGraph::read(input_file.value());
    std::cerr << "done, factors:" << graph->factors().size() << " vars: " << graph->variables().size() << std::endl;
    std::cerr << "initializing (batch) ...";
    for (auto v_it: graph->variables()) {
      if (v_it.first!=0 && v_it.second->status()==VariableBase::Fixed) {
        initializer.parameterIds().insert(v_it.first);
      }
    }
    initializer.setGraph(*graph);
    initializer.compute();
    std::cerr << "done" << std::endl;
    int num_initialized=0;
    for (auto v_it: graph->variables()) {
      VariableBase* v=v_it.second;
      if (initializer.isInit(v)) {
        ++num_initialized;
      }
    }
    std::cerr << "total intialized: " << num_initialized << "/" << graph->variables().size() << endl;
  } else {
    graph = FactorGraphPtr(new FactorGraph);
    FactorGraphIncrementalReader reader;
    reader.setGraph(graph);
    reader.setPathType(path_type.value());
    reader.setFilePath(input_file.value());

    std::cerr << "done" << std::endl;
    initializer.setGraph(*graph);

    //process the data incrementally
    std::set<VariableBase::Id> new_vars;
    std::set<FactorBase::Id> new_factors;
    cerr << "Incremental init " << endl;
    while (reader.readEpoch(new_vars, new_factors)) {
      if (new_vars.size()) {
        cerr << "\rEpoch: " << *new_vars.begin() << " " << " v: " <<new_vars.size() << " f:" << new_factors.size();
      }
      initializer.updateGraph();
      initializer.compute();
    }
    cerr << endl;
    //final batch
    initializer.updateGraph();
    initializer.compute();
  }

  if (! output_file.isSet()) {
    cerr << "no output file provided, skipping output" << endl;
    return 0;
  }
  std::cerr << "writing output to file [ " << output_file.value() << "]... ";
  graph->write(output_file.value());
  cerr << " done" << endl;
  return 0;
}


/// \file       include/gslcpp/wrap/sum.hpp
/// \copyright  2022 Thomas E. Vaughan, all rights reserved.
/// \brief      Definition of gsl::w_sum().

#pragma once
#include "container.hpp" // w_vector
#include "element.hpp" // element_t
#include <Eigen/Core> // Dynamic, Map, Matrix, Stride

namespace gsl {

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<double const> *v) { return gsl_vector_sum(v); }

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<float const> *v) { return gsl_vector_float_sum(v); }

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<long double const> *v) {
  return gsl_vector_long_double_sum(v);
}

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<int const> *v) { return gsl_vector_int_sum(v); }

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<unsigned const> *v) {
  return gsl_vector_uint_sum(v);
}

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<long const> *v) { return gsl_vector_long_sum(v); }

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<unsigned long const> *v) {
  return gsl_vector_ulong_sum(v);
}

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<short const> *v) { return gsl_vector_short_sum(v); }

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<unsigned short const> *v) {
  return gsl_vector_ushort_sum(v);
}

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<char const> *v) { return gsl_vector_char_sum(v); }

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<unsigned char const> *v) {
  return gsl_vector_uchar_sum(v);
}

/// Sum of elements in complex vector, not covered by GSL's sum.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// This implementation uses Eigen to compute the sum.
/// @tparam C  Complex type of vector's element.
/// @param v  Reference to vector.
/// @return  Sum of elements.
template<typename C> C complex_sum(w_vector<C const> *v) {
  using Eigen::Dynamic;
  using Eigen::Map;
  using Eigen::Matrix;
  using Eigen::Stride;
  using S= Stride<Dynamic, Dynamic>;
  using map= Map<Matrix<C, Dynamic, 1> const, 0, S>;
  S const s(v->size * v->stride, v->stride);
  return map((C const *)v->data, v->size, s).sum();
}

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<gsl::complex<double> const> *v) {
  return complex_sum<gsl::complex<double>>(v);
}

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<gsl::complex<float> const> *v) {
  return complex_sum<gsl::complex<float>>(v);
}

/// Sum of elements in vector `v`.
/// https://www.gnu.org/software/gsl/doc/html/vectors.html#c.gsl_vector_sum
/// @param v  Pointer to vector.
/// @return  Sum of elements in `v`.
inline auto w_sum(w_vector<gsl::complex<long double> const> *v) {
  return complex_sum<gsl::complex<long double>>(v);
}

} // namespace gsl

// EOF


// Boost.Geometry

// Copyright (c) 2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_WITHIN_MULTI_POINT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_WITHIN_MULTI_POINT_HPP

#include <algorithm>
#include <vector>

#include <boost/range.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/algorithms/detail/disjoint/box_box.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_box.hpp>
#include <boost/geometry/algorithms/detail/expand_by_epsilon.hpp>
#include <boost/geometry/algorithms/detail/relate/less.hpp>
#include <boost/geometry/algorithms/detail/within/point_in_geometry.hpp>
#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/detail/partition.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tag_cast.hpp>
#include <boost/geometry/core/tags.hpp>

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

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

#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/disjoint.hpp>

namespace boost { namespace geometry {

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace within {

struct multi_point_point
{
    template <typename MultiPoint, typename Point, typename Strategy>
    static inline bool apply(MultiPoint const& multi_point,
                             Point const& point,
                             Strategy const& strategy)
    {
        typedef typename boost::range_const_iterator<MultiPoint>::type iterator;
        for ( iterator it = boost::begin(multi_point) ; it != boost::end(multi_point) ; ++it )
        {
            if (! strategy.apply(*it, point))
            {
                return false;
            }
        }

        // all points of MultiPoint inside Point
        return true;
    }
};

// NOTE: currently the strategy is ignored, math::equals() is used inside relate::less
struct multi_point_multi_point
{
    template <typename MultiPoint1, typename MultiPoint2, typename Strategy>
    static inline bool apply(MultiPoint1 const& multi_point1,
                             MultiPoint2 const& multi_point2,
                             Strategy const& /*strategy*/)
    {
        typedef typename boost::range_value<MultiPoint2>::type point2_type;

        relate::less const less = relate::less();

        std::vector<point2_type> points2(boost::begin(multi_point2), boost::end(multi_point2));
        std::sort(points2.begin(), points2.end(), less);

        bool result = false;

        typedef typename boost::range_const_iterator<MultiPoint1>::type iterator;
        for ( iterator it = boost::begin(multi_point1) ; it != boost::end(multi_point1) ; ++it )
        {
            if (! std::binary_search(points2.begin(), points2.end(), *it, less))
            {
                return false;
            }
            else
            {
                result = true;
            }
        }

        return result;
    }
};

// TODO: the complexity could be lesser
//   the second geometry could be "prepared"/sorted
// For Linear geometries partition could be used
// For Areal geometries point_in_geometry() would have to call the winding
//   strategy differently, currently it linearly calls the strategy for each
//   segment. So the segments would have to be sorted in a way consistent with
//   the strategy and then the strategy called only for the segments in range.
template <bool Within>
struct multi_point_single_geometry
{
    template <typename MultiPoint, typename LinearOrAreal, typename Strategy>
    static inline bool apply(MultiPoint const& multi_point,
                             LinearOrAreal const& linear_or_areal,
                             Strategy const& strategy)
    {
        typedef typename boost::range_value<MultiPoint>::type point1_type;
        typedef typename point_type<LinearOrAreal>::type point2_type;
        typedef model::box<point2_type> box2_type;

        // Create envelope of geometry
        box2_type box;
        geometry::envelope(linear_or_areal, box, strategy.get_envelope_strategy());
        geometry::detail::expand_by_epsilon(box);

        typedef typename strategy::covered_by::services::default_strategy
            <
                point1_type, box2_type
            >::type point_in_box_type;

        // Test each Point with envelope and then geometry if needed
        // If in the exterior, break
        bool result = false;

        typedef typename boost::range_const_iterator<MultiPoint>::type iterator;
        for ( iterator it = boost::begin(multi_point) ; it != boost::end(multi_point) ; ++it )
        {
            int in_val = 0;

            // exterior of box and of geometry
            if (! point_in_box_type::apply(*it, box)
                || (in_val = point_in_geometry(*it, linear_or_areal, strategy)) < 0)
            {
                result = false;
                break;
            }

            // interior : interior/boundary
            if (Within ? in_val > 0 : in_val >= 0)
            {
                result = true;
            }
        }

        return result;
    }
};

// TODO: same here, probably the complexity could be lesser
template <bool Within>
struct multi_point_multi_geometry
{
    template <typename MultiPoint, typename LinearOrAreal, typename Strategy>
    static inline bool apply(MultiPoint const& multi_point,
                             LinearOrAreal const& linear_or_areal,
                             Strategy const& strategy)
    {
        typedef typename point_type<LinearOrAreal>::type point2_type;
        typedef model::box<point2_type> box2_type;
        static const bool is_linear = is_same
            <
                typename tag_cast
                    <
                        typename tag<LinearOrAreal>::type,
                        linear_tag
                    >::type,
                linear_tag
            >::value;

        typename Strategy::envelope_strategy_type const
            envelope_strategy = strategy.get_envelope_strategy();

        // TODO: box pairs could be constructed on the fly, inside the rtree

        // Prepare range of envelopes and ids
        std::size_t count2 = boost::size(linear_or_areal);
        typedef std::pair<box2_type, std::size_t> box_pair_type;
        typedef std::vector<box_pair_type> box_pair_vector;
        box_pair_vector boxes(count2);
        for (std::size_t i = 0 ; i < count2 ; ++i)
        {
            geometry::envelope(linear_or_areal, boxes[i].first, envelope_strategy);
            geometry::detail::expand_by_epsilon(boxes[i].first);
            boxes[i].second = i;
        }

        // Create R-tree
        index::rtree<box_pair_type, index::rstar<4> > rtree(boxes.begin(), boxes.end());

        // For each point find overlapping envelopes and test corresponding single geometries
        // If a point is in the exterior break
        bool result = false;

        typedef typename boost::range_const_iterator<MultiPoint>::type iterator;
        for ( iterator it = boost::begin(multi_point) ; it != boost::end(multi_point) ; ++it )
        {
            // TODO: investigate the possibility of using satisfies
            // TODO: investigate the possibility of using iterative queries (optimization below)
            box_pair_vector inters_boxes;
            rtree.query(index::intersects(*it), std::back_inserter(inters_boxes));

            bool found_interior = false;
            bool found_boundary = false;
            int boundaries = 0;

            typedef typename box_pair_vector::const_iterator iterator;
            for ( iterator box_it = inters_boxes.begin() ; box_it != inters_boxes.end() ; ++box_it )
            {
                int in_val = point_in_geometry(*it, range::at(linear_or_areal, box_it->second), strategy);

                if (in_val > 0)
                    found_interior = true;
                else if (in_val == 0)
                    ++boundaries;

                // If the result was set previously (interior or
                // interior/boundary found) the only thing that needs to be
                // done for other points is to make sure they're not
                // overlapping the exterior no need to analyse boundaries.
                if (result && in_val >= 0)
                {
                    break;
                }
            }

            if ( boundaries > 0)
            {
                if (is_linear && boundaries % 2 == 0)
                    found_interior = true;
                else
                    found_boundary = true;
            }

            // exterior
            if (! found_interior && ! found_boundary)
            {
                result = false;
                break;
            }

            // interior : interior/boundary
            if (Within ? found_interior : (found_interior || found_boundary))
            {
                result = true;
            }
        }

        return result;
    }
};

}} // namespace detail::within
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_WITHIN_MULTI_POINT_HPP


#include <Eigen/Eigen>
#include <sophus/se3.hpp>

#include <pangolin/pangolin.h>
#include <pangolin/glcuda.h>
#include <pangolin/glvbo.h>

#include <SceneGraph/SceneGraph.h>

#include <kangaroo/kangaroo.h>
#include <kangaroo/BoundedVolume.h>
#include <kangaroo/MarchingCubes.h>
#include <kangaroo/extra/ImageSelect.h>
#include <kangaroo/extra/BaseDisplayCuda.h>
#include <kangaroo/extra/DisplayUtils.h>
#include <kangaroo/extra/Handler3dGpuDepth.h>
#include <kangaroo/extra/SavePPM.h>
#include <kangaroo/extra/SaveMeshlab.h>

#ifdef HAVE_CVARS
#include <kangaroo/extra/CVarHelpers.h>
#endif // HAVE_CVARS

#include <opencv2/opencv.hpp>
#include <unistd.h>

#include <HAL/Utils/GetPot>
#include <HAL/Utils/TicToc.h>
#include <HAL/Camera/CameraDevice.h>
#include <calibu/Calibu.h>
#include <CVars/CVar.h>

#include <string>

using namespace pangolin;

struct pni {  // pose 'n' intrinsics
  Sophus::SE3d p;
  roo::ImageIntrinsics i;
};

// camera serial number -> camera pose and intrinsics
typedef std::map<uint64_t, pni> cameraMap;

// Creates a cameraMap from the calibu camera rig
void getCameraCalibration(const calibu::CameraRig &rig,
                          cameraMap& Ks);

// Copies a pb image into a kangaroo image
void copyFrom(const std::shared_ptr<pb::Image>& pbim,
              roo::Image<unsigned short, roo::TargetDevice, roo::Manage>& kim);

// Sets a depth image to the display
void setDepthImages(const std::shared_ptr<pb::ImageArray>& pbim,
                    std::vector<SceneGraph::ImageView>& views);

int main( int argc, char* argv[] )
{
  // Initialise window
  View& container = SetupPangoGLWithCuda(1024, 768);
  SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();

  GetPot clArgs(argc, argv);

  ///----- Load camera model.
  hal::Camera cam(clArgs.follow("", "-cam"));
  std::shared_ptr< pb::ImageArray > vImages = pb::ImageArray::Create();
  calibu::CameraRig rig;
  if (!cam.GetDeviceProperty(hal::DeviceDirectory).empty()) {
    std::cout<<"Loaded camera: "<<cam.GetDeviceProperty(hal::DeviceDirectory) + '/' + clArgs.follow("cameras.xml", "-cmod")<<std::endl;
    rig = calibu::ReadXmlRig(cam.GetDeviceProperty(hal::DeviceDirectory) + '/' + clArgs.follow("cameras.xml", "-cmod"));
  }
  else {
    std::cout << "in second option" << std::endl;
    rig = calibu::ReadXmlRig(clArgs.follow("cameras.xml", "-cmod"));
  }

  cameraMap Ks;
  getCameraCalibration(rig, Ks);

  if(Ks.size() != cam.NumChannels())
  {
    std::cout << "Error: the number of cameras (" << cam.NumChannels()
              << ") differs from the number of models in the calibration file ("
              << Ks.size() << ")" << std::endl;
    exit(2);
  }

  const int w = cam.Width();
  const int h = cam.Height();

  ///----- Init aux variables and dense tracker.

  const bool use_colour = false;

  const int MaxLevels = 3;
  const int its[] = {1,0,2,3};

  //roo::ImageIntrinsics K(KL(0, 0),KL(1, 1), KL(0, 2), KL(1, 2) );
  roo::ImageIntrinsics K = Ks.rbegin()->second.i;

  const double knear = 0.01;
  const double kfar = 4.0;

  const float volrad = 2.0;
  const int volres = 256;

  roo::BoundingBox reset_bb(make_float3(-volrad,-volrad,knear), make_float3(volrad,volrad,knear+2*volrad));

#ifdef HAVE_CVARS
  CVarUtils::AttachCVar<roo::BoundingBox>("BoundingBox", &reset_bb);
#endif // HAVE_CVARS

  // Camera (rgb) to depth
  roo::Image<float, roo::TargetDevice, roo::Manage> dKinectMeters(w,h);
  roo::Image<unsigned short, roo::TargetDevice, roo::Manage> dKinect(w,h);
  roo::Pyramid<float, MaxLevels, roo::TargetDevice, roo::Manage> kin_d(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> kin_v(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> kin_n(w,h);
  roo::Image<float4, roo::TargetDevice, roo::Manage>  dDebug(w,h);

  roo::Pyramid<float, MaxLevels, roo::TargetDevice, roo::Manage> ray_i(w,h);
  roo::Pyramid<float, MaxLevels, roo::TargetDevice, roo::Manage> ray_d(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> ray_n(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> ray_v(w,h);
  roo::Pyramid<float4, MaxLevels, roo::TargetDevice, roo::Manage> ray_c(w,h);
  roo::BoundedVolume<roo::SDF_t, roo::TargetDevice, roo::Manage> vol(volres,volres,volres,reset_bb);
  roo::BoundedVolume<float, roo::TargetDevice, roo::Manage> colorVol(volres,volres,volres,reset_bb);

  std::vector<std::unique_ptr<KinectKeyframe> > keyframes;
  roo::Mat<roo::ImageKeyframe<uchar3>,10> kfs;

  SceneGraph::GLSceneGraph glgraph;
  SceneGraph::GLAxis glcamera(0.1);
  SceneGraph::GLAxisAlignedBox glboxfrustum;
  SceneGraph::GLAxisAlignedBox glboxvol;

  glboxvol.SetBounds(roo::ToEigen(vol.bbox.Min()), roo::ToEigen(vol.bbox.Max()) );
  glgraph.AddChild(&glcamera);
  glgraph.AddChild(&glboxvol);
  glgraph.AddChild(&glboxfrustum);

  pangolin::OpenGlRenderState s_cam(
        ProjectionMatrixRDF_TopLeft(w,h,K.fu,K.fv,K.u0,K.v0,0.1,1000),
        ModelViewLookAtRDF(0,0,-2,0,0,0,0,-1,0)
        );

  Var<bool> reset("ui.reset", true, false);
  Var<bool> run("ui.run", true, true);

  Var<bool> viewonly("ui.view only", true, true);
  Var<bool> fuse("ui.fuse", false, true);
  Var<float> max_rmse("ui.Max RMSE",0.10,0,0.5);
  Var<float> rmse("ui.RMSE",0);

  Var<int> show_level("ui.Show Level", 0, 0, MaxLevels-1);

  // TODO: This needs to be a function of the inverse depth
  Var<int>   biwin("ui.size",3, 1, 20);      // Bilateral filter
  Var<float> bigs("ui.gs",1.5, 1E-3, 5);     // Bilateral filter
  Var<float> icp_c("ui.icp c",0.1, 1E-3, 1);
  Var<float> bigr("ui.gr",0.1, 1E-6, 0.2);   // Bilateral filter
  roo::Image<unsigned char, roo::TargetDevice, roo::Manage> dScratch(w*sizeof(roo::LeastSquaresSystem<float,12>),h);
  Var<float> trunc_dist_factor("ui.trunc vol factor",2, 1, 4);

  Var<float> max_w("ui.max w", 1000, 1E-2, 1E3);
  Var<float> mincostheta("ui.min cos theta", 0.1, 0, 1);
  Var<float> rgb_fl("ui.RGB focal length", 535.7,400,600);

  ActivateDrawPyramid<float,MaxLevels> adrayimg(ray_i, GL_LUMINANCE32F_ARB, true, true);
  ActivateDrawPyramid<float4,MaxLevels> adraycolor(ray_c, GL_RGBA32F, true, true);
  ActivateDrawPyramid<float4,MaxLevels> adraynorm(ray_n, GL_RGBA32F, true, true);
  //    ActivateDrawPyramid<float,MaxLevels> addepth( kin_d, GL_LUMINANCE32F_ARB, false, true);
  ActivateDrawPyramid<float4,MaxLevels> adnormals( kin_n, GL_RGBA32F_ARB, false, true);
  ActivateDrawImage<float4> addebug( dDebug, GL_RGBA32F_ARB, false, true);

  Handler3DDepth<float,roo::TargetDevice> rayhandler(ray_d[0], s_cam, AxisNone);
  SetupContainer(container, 3, (float)w/h);
  container[0].SetDrawFunction(std::ref(adrayimg))
      .SetHandler(&rayhandler);
  container[1].SetDrawFunction(SceneGraph::ActivateDrawFunctor(glgraph, s_cam))
      .SetHandler( new Handler3D(s_cam, AxisNone) );
  container[2].SetDrawFunction(std::ref(use_colour?adraycolor:adraynorm))
      .SetHandler(&rayhandler);
  //container[3].SetDrawFunction(std::ref(adnormals));

  std::vector<SceneGraph::ImageView> depthViews(cam.NumChannels());
  for(size_t i = 0; i < depthViews.size(); ++i)
    container.AddDisplay( depthViews[i] );

  Sophus::SE3d T_wl;

  bool bStep = false;
  pangolin::RegisterKeyPressCallback(' ', [&viewonly]() { viewonly=!viewonly;} );
  pangolin::RegisterKeyPressCallback('l', [&vol,&viewonly]() {LoadPXM("save.vol", vol); viewonly = true;} );
  //    pangolin::RegisterKeyPressCallback('s', [&vol,&colorVol,&keyframes,&rgb_fl,w,h]() {SavePXM("save.vol", vol); SaveMeshlab(vol,keyframes,rgb_fl,rgb_fl,w/2,h/2); } );
  pangolin::RegisterKeyPressCallback('s', [&vol,&colorVol]() {roo::SaveMesh("mesh",vol,colorVol); } );
  pangolin::RegisterKeyPressCallback('/', [&]() {bStep = !bStep; } );
  //    pangolin::RegisterKeyPressCallback('s', [&vol]() {SavePXM("save.vol", vol); } );

  reset = true;

  for(long frame=-1; !pangolin::ShouldQuit();)
  {
    const float trunc_dist = trunc_dist_factor*length(vol.VoxelSizeUnits());

    if(pangolin::Pushed(reset)) {
      vol.bbox = reset_bb;
      roo::SdfReset(vol, std::numeric_limits<float>::quiet_NaN() );
      keyframes.clear();
      frame = -1;
    }

    if(run)
    {
      cam.Capture( *vImages );
      setDepthImages(vImages, depthViews);

      Sophus::SE3d T_vw(s_cam.GetModelViewMatrix());
      if(viewonly) {

        const roo::BoundingBox roi(T_vw.inverse().matrix3x4(), w, h, K, 0, 50);
        roo::BoundedVolume<roo::SDF_t> work_vol = vol.SubBoundingVolume( roi );
        if(work_vol.IsValid()) {
          roo::RaycastSdf(ray_d[0], ray_n[0], ray_i[0], work_vol, T_vw.inverse().matrix3x4(), K, 0.1, 50, trunc_dist, true );

          if(keyframes.size() > 0) {
            // populate kfs
            for( int k=0; k< kfs.Rows(); k++)
            {
              if(k < keyframes.size()) {
                kfs[k].img = keyframes[k]->img;
                kfs[k].T_iw = keyframes[k]->T_iw.matrix3x4();
                kfs[k].K = roo::ImageIntrinsics(rgb_fl, kfs[k].img);
              }else{
                kfs[k].img.ptr = 0;
              }
            }
            roo::TextureDepth<float4,uchar3,10>(ray_c[0], kfs, ray_d[0], ray_n[0], ray_i[0], T_vw.inverse().matrix3x4(), K);
          }
        }
      }else{
        const roo::BoundingBox roi(roo::BoundingBox(T_wl.inverse().matrix3x4(), w, h, K, 0, 50));
        roo::BoundedVolume<roo::SDF_t> work_vol = vol.SubBoundingVolume( roi );
        if(work_vol.IsValid()) {
          for(int l=0; l<MaxLevels; ++l) {
            if(its[l] > 0) {
              const roo::ImageIntrinsics Kl = K[l];
              roo::RaycastSdf(ray_d[l], ray_n[l], ray_i[l], work_vol, T_vw.inverse().matrix3x4(), Kl, knear,kfar, trunc_dist, true );
              roo::DepthToVbo<float>(ray_v[l], ray_d[l], Kl );
            }
          }

          if(fuse) {
            for (int ii = 0; ii < vImages->Size(); ii++) {

              // Transfer the captured image to the device
              copyFrom(vImages->at(ii), dKinect);

              roo::ElementwiseScaleBias<float,unsigned short,float>(dKinectMeters, dKinect, 1.0f/1000.0f);  // OpenNI outputs in millimeters
              roo::BilateralFilter<float,float>(kin_d[0],dKinectMeters,bigs,bigr,biwin,0.2);

              roo::BoxReduceIgnoreInvalid<float,MaxLevels,float>(kin_d);
              for(int l=0; l<MaxLevels; ++l) {
                roo::DepthToVbo<float>(kin_v[l], kin_d[l], K[l] );
                roo::NormalsFromVbo(kin_n[l], kin_v[l]);
              }

              // Set Image Intrinsics for this particular camera
              assert(Ks.find(vImages->at(ii)->SerialNumber()) != Ks.end());
              K = Ks[ vImages->at(ii)->SerialNumber() ].i;

              // Set the pose relative to the rig and then to the
              // frame of the SDF.
              Sophus::SE3d tempPose = Ks[ vImages->at(ii)->SerialNumber() ].p;

              const roo::BoundingBox roi(tempPose.matrix3x4(), w, h, K, knear,kfar);
              roo::BoundedVolume<roo::SDF_t> work_vol = vol.SubBoundingVolume( roi );
              if(work_vol.IsValid()) {
                const float trunc_dist = trunc_dist_factor*length(vol.VoxelSizeUnits());
                roo::SdfFuse(work_vol, kin_d[0], kin_n[0], tempPose.inverse().matrix3x4(), K, trunc_dist, max_w, mincostheta );
              }
            }
          } // if fuse
        } // if work vol is valid
      } // if !view only
    } // if run

    glcamera.SetPose(T_wl.matrix());

    roo::BoundingBox bbox_work(T_wl.matrix3x4(), w, h, K.fu, K.fv, K.u0, K.v0, knear,kfar);
    bbox_work.Intersect(vol.bbox);
    glboxfrustum.SetBounds(roo::ToEigen(bbox_work.Min()), roo::ToEigen(bbox_work.Max()) );

    /////////////////////////////////////////////////////////////
    // Draw
    addebug.SetImage(dDebug.SubImage(0,0,w>>show_level,h>>show_level));
    adnormals.SetLevel(show_level);
    adrayimg.SetLevel(viewonly? 0 : show_level);

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glColor3f(1,1,1);
    pangolin::FinishFrame();
  }
  return 0;
}

void getCameraCalibration(const calibu::CameraRig &rig,
                          cameraMap& Ks)
{
  Ks.clear();
  for (int ii = 0; ii < rig.cameras.size(); ii++ ){
    Eigen::Matrix3d KL = rig.cameras[ii].camera.K();
    pni temp;
    roo::ImageIntrinsics temp_i(KL(0, 0),KL(1, 1), KL(0, 2), KL(1, 2) );
    temp.i = temp_i;
    temp.p = rig.cameras[ii].T_wc;
    Ks.insert( std::pair<uint64_t, pni >(rig.cameras[ii].camera.SerialNumber() , temp ) );
  }
}

void copyFrom(const std::shared_ptr<pb::Image>& pbim,
              roo::Image<unsigned short, roo::TargetDevice, roo::Manage>& kim)
{
  cv::Mat mat;
  if(pbim->Type() == pb::PB_UNSIGNED_SHORT)
    mat = pbim->Mat();
  else if(pbim->Type() == pb::PB_FLOAT)
  {
    cv::Mat src = pbim->Mat();
    src.convertTo(mat, CV_16U);
  }
  else
  {
    std::cerr << "Unsupported type of image" << std::endl;
    exit(1);
  }
  kim.CopyFrom(roo::Image<unsigned short, roo::TargetHost>
               (mat.ptr<unsigned short>(), mat.cols, mat.rows,
                mat.cols * sizeof(unsigned short)));
}

void setDepthImages(const std::shared_ptr<pb::ImageArray>& pbims,
                    std::vector<SceneGraph::ImageView>& views)
{
  for(int i = 0; i < pbims->Size(); ++i) {
    views[i].SetImage(pbims->at(i)->data(), pbims->at(i)->Width(),
                      pbims->at(i)->Height(),
                      GL_LUMINANCE, GL_LUMINANCE, GL_FLOAT, true);
    views[i].UpdateGlTexture();
  }
}


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

#ifndef ITL_GAUSS_SEIDEL_INCLUDE
#define ITL_GAUSS_SEIDEL_INCLUDE

#include <boost/assert.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace itl {

/// Gauss-Seidel smoother
/** Constructor takes references to a matrix and a right-hand side vector.
    operator() is applied on a vector and changes it in place.
    Matrix must be square, stored row-major and free of zero entries in the diagonal.
    Vectors b and x must have the same number of rows as A.
**/
template <typename Matrix>
class gauss_seidel
{
    typedef typename mtl::Collection<Matrix>::value_type Scalar;
    typedef typename mtl::Collection<Matrix>::size_type  size_type;
  public:
    /// Construct with constant references to matrix and RHS vector
    gauss_seidel(const Matrix& A) : A(A), dia_inv(num_rows(A))
    {
	BOOST_STATIC_ASSERT((mtl::traits::is_row_major<Matrix>::value)); // No CCS
	assert(num_rows(A) == num_cols(A)); // Matrix must be square
	for (size_type i= 0; i < num_rows(A); ++i) {
	    Scalar a= A[i][i];
	    MTL_THROW_IF(a == 0, mtl::missing_diagonal());
	    dia_inv[i]= 1.0 / a;
	}
    }

    /// Apply Gauss-Seidel on vector \p x, i.e. \p x is changed
    template <typename Vector, typename RHSVector>
    Vector& operator()(Vector& x, const RHSVector& b) const
    {
	mtl::vampir_trace<8551> tracer;
	namespace tag= mtl::tag; using namespace mtl::traits;
	using mtl::begin; using mtl::end;

        typedef typename range_generator<tag::row, Matrix>::type       a_cur_type;
        typedef typename range_generator<tag::nz, a_cur_type>::type    a_icur_type;
	typename col<Matrix>::type                   col_a(A);
	typename const_value<Matrix>::type           value_a(A);

	typedef typename mtl::Collection<Vector>::value_type           value_type;

	a_cur_type ac= begin<tag::row>(A), aend= end<tag::row>(A);
	for (unsigned i= 0; ac != aend; ++ac, ++i) {
	    value_type tmp= b[i];
	    for (a_icur_type aic= begin<tag::nz>(ac), aiend= end<tag::nz>(ac); aic != aiend; ++aic)
		if (col_a(*aic) != i)
		    tmp-= value_a(*aic) * x[col_a(*aic)];
	    x[i]= dia_inv[i] * tmp;
	}
 	return x;
    }

   private:
    const Matrix&    A;
    mtl::dense_vector<Scalar>  dia_inv;
};

template <typename Value, typename Parameters>
class gauss_seidel<mtl::mat::compressed2D<Value, Parameters> >
{
    typedef mtl::mat::compressed2D<Value, Parameters> Matrix;
    typedef typename mtl::Collection<Matrix>::value_type Scalar;
    typedef typename mtl::Collection<Matrix>::size_type  size_type;
  public:
    /// Construct with constant references to matrix and RHS vector
    gauss_seidel(const Matrix& A)
      : A(A), dia_inv(num_rows(A)), dia_pos(num_rows(A))
    {
	BOOST_STATIC_ASSERT((mtl::traits::is_row_major<Matrix>::value)); // No CCS
	assert(num_rows(A) == num_cols(A)); // Matrix must be square
	for (size_type i= 0; i < num_rows(A); ++i) {
	    mtl::utilities::maybe<size_type> pos = A.indexer(A, i, i);
	    MTL_THROW_IF(!pos, mtl::missing_diagonal());
	    dia_inv[i]= 1.0 / A.value_from_offset(pos);
	    dia_pos[i]= pos;
	}
    }

    /// Apply Gauss-Seidel on vector \p x, i.e. \p x is changed
    template <typename Vector, typename RHSVector>
    Vector& operator()(Vector& x, const RHSVector& b) const
    {
	mtl::vampir_trace<8551> tracer;
	typedef typename mtl::Collection<Vector>::value_type           value_type;
	typedef typename mtl::Collection<Matrix>::size_type            size_type;
	const size_type nr= num_rows(A);
	size_type cj1= A.ref_major()[0];
	for (size_type i= 0; i < nr; ++i) {
	    value_type tmp= b[i];
	    size_type cj0= cj1, cjm= dia_pos[i];
	    cj1= A.ref_major()[i+1];
	    for (; cj0 < cjm; cj0++)
		tmp-= A.data[cj0] * x[A.ref_minor()[cj0]];
	    for (size_type j= cjm+1; j < cj1; j++)
		tmp-= A.data[j] * x[A.ref_minor()[j]];
	    x[i]= dia_inv[i] * tmp;
	}
 	return x;
    }

  private:
    const Matrix&    A;
    mtl::dense_vector<Scalar>     dia_inv;
    mtl::dense_vector<size_type>  dia_pos;
};

} // namespace itl

#endif // ITL_GAUSS_SEIDEL_INCLUDE