// © 2016 PORT INC.

#ifndef OPTIMIZATION__HPP
#define OPTIMIZATION__HPP

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

namespace Optimizer {

	using namespace boost::numeric::ublas;
    using vector = boost::numeric::ublas::vector<double>;
    using matrix = boost::numeric::ublas::matrix<double>;

	enum {
		ENABLE_ADAGRAD = 0x1
	};

	////////

	class ObjectiveFunction_ {
	public:
		ObjectiveFunction_(){};
		virtual ~ObjectiveFunction_(){};
		virtual double value(vector& x) = 0;
		virtual double savedValue() = 0;
		virtual vector grad(vector& x) = 0;
		virtual void preProcess(vector& x) = 0;
		virtual void beginLoopProcess(vector& x) = 0;
		virtual void afterUpdateXProcess(vector& x) = 0;
		virtual void endLoopProcess(vector& x) = 0;
		virtual void postProcess(vector& x) = 0;
	};

	typedef std::shared_ptr<ObjectiveFunction_> ObjectiveFunction;

	////////

	class UnconstrainedNLP_ {

	public:

		UnconstrainedNLP_(int dim, ObjectiveFunction ofunc);
		virtual ~UnconstrainedNLP_(){};
		virtual void optimize() = 0;
		virtual bool isConv();
		void setAe(double arg) { ae = arg; };
		void setRe(double arg) { re = arg; };
		void setMaxIteration(int arg) { maxIteration = arg; };
		void setE0(double arg) { e0 = arg; };
		void setFlg(int arg) { flg = arg; };

	protected:

		virtual double avoidDivergence(vector& g, double& f1);
		virtual double linearSearch(vector& g);
		void iteration_limit_error(std::string msg, double beta);
		void minimum_limit_error(std::string msg, double beta);

	protected:

		int flg;
		int dim;
		int itr;
		int maxIteration;
		ObjectiveFunction ofunc;
		double e0;
		double alpha;
		double beta;
		double minBeta;
		double xi;
		double tau;
		double r0;
		double f0;
		double re;
		double ae;
		vector x;
		vector dx;
		vector g0;
		vector g1;
		vector d;
	};

	typedef std::shared_ptr<UnconstrainedNLP_> UnconstrainedNLP;

	////////

	class SteepestDescent : public UnconstrainedNLP_ {

	public:

		SteepestDescent(int dim, ObjectiveFunction ofunc);
		virtual ~SteepestDescent(){};
		virtual void optimize();
		virtual bool isConv();

	protected:

		vector adagrad;
	};

	UnconstrainedNLP createSteepestDescent(int dim, ObjectiveFunction ofunc);

	////////

	class QuasiNewton_ : public UnconstrainedNLP_ {

	public:

		QuasiNewton_(int dim, ObjectiveFunction ofunc);
		virtual ~QuasiNewton_(){};
		virtual void optimize();
		virtual bool isConv();
		void setBeta(double arg) { beta = arg; };
		void setXi(double arg) { xi = arg; };
		void setTau(double arg) { tau = arg; };

	protected:

		virtual void updateMatrix() = 0;

	protected:

		vector y;
		identity_matrix<double> I;
		matrix H0;
		matrix H1;
		matrix A;
		matrix B;
	};

	typedef std::shared_ptr<QuasiNewton_> QuasiNewton;

	////////

	class Bfgs : public QuasiNewton_ {
	public:
		Bfgs(int dim, ObjectiveFunction ofunc);
		virtual void updateMatrix();
	};

	UnconstrainedNLP createBfgs(int dim, ObjectiveFunction ofunc);

	void test();

}

#endif // OPTIMIZATION__HPP


/*
 */

#include <iostream>

#include <Eigen/Eigen>
#include <Eigen/Core>
#include <sophus/se3.h>

#include "pinhole_camera.h"
#include "param_reader.h"

using namespace std;

PinHoleCamera::PinHoleCamera(ParameterReader::Ptr param_reader) :
fx_(param_reader->getParam<double>("camera.fx")),
fy_(param_reader->getParam<double>("camera.fy")),
cx_ (param_reader->getParam<double>("camera.cx")),
cy_ (param_reader->getParam<double>("camera.cx")),
factor_ (param_reader->getParam<double>("camera.factor")),
skew_ (param_reader->getParam<double>("camera.skew"))
{
	// 初始化内参矩阵
	K_ << fx_, skew_, cx_,   0.0, fy_, cy_,   0.0, 0.0, 1.0;
	K_inv_ = K_.inverse();
}

PinHoleCamera::~PinHoleCamera()
{

}

// // 将根据像素点的横纵坐标和深度值恢复其在相机坐标系中的三维坐标
// cv::Point3f PinHoleCamera::point2dTo3d(cv::Point3f& pixel_point_with_depth)
// {
// 	int x = pixel_point_with_depth.x;
// 	int y = pixel_point_with_depth.y;
// 	double d = pixel_point_with_depth.z;
//
// 	cv::Point3f p;
// 	p.z = d / factor_;
// 	p.x = (x - cx_) * p.z / fx_;
// 	p.y = (y - cy_) * p.z / fy_;
//
// 	return p;
// }


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenMatRowIndGenDouble : public Expected {
    public:
      ExpectedInPlaceGenMatRowIndGenDouble() {
        cout << "Compute ExpectedInPlaceGenMatRowIndGenDouble(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenMat,
            InputClass::RowInd,
            InputClass::GenDouble
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genMat = *static_cast<Mat<double>*>(value.second);
                  break;
                case 1:
                  _fileSuffix += "," + value.first;
                  _rowInd = *static_cast<int*>(value.second);
                  break;
                case 2:
                  _fileSuffix += "," + value.first;
                  _genDouble = *static_cast<double*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenMat = _genMat;
            _copyOfRowInd = _rowInd;
            _copyOfGenDouble = _genDouble;

            expectedMatDiagPlus();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _genDouble = _copyOfGenDouble;
            expectedMatDiagMinus();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _genDouble = _copyOfGenDouble;
            expectedMatDiagTimes();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _genDouble = _copyOfGenDouble;
            expectedMatDiagDivide();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _genDouble = _copyOfGenDouble;
            expectedMatColPlus();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _genDouble = _copyOfGenDouble;
            expectedMatColMinus();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _genDouble = _copyOfGenDouble;
            expectedMatColTimes();

            _genMat = _copyOfGenMat;
            _rowInd = _copyOfRowInd;
            _genDouble = _copyOfGenDouble;
            expectedMatColDivide();
          }

          cout << "done." << endl;
        }

    protected:
      Mat<double> _genMat;
      Mat<double> _copyOfGenMat;

      int _rowInd;
      int _copyOfRowInd;

      double _genDouble;
      double _copyOfGenDouble;

      void expectedMatDiagPlus() {
        if(_rowInd >= _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedMatDiagPlus() ... ";

        _genMat.diag(-_rowInd) += _genDouble;
        save<double>("Mat.diagSubPlus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatDiagMinus() {
        if(_rowInd >= _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedMatDiagMinus() ... ";

        _genMat.diag(-_rowInd) -= _genDouble;
        save<double>("Mat.diagSubMinus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatDiagTimes() {
        if(_rowInd >= _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedMatDiagTimes() ... ";

        _genMat.diag(-_rowInd) *= _genDouble;
        save<double>("Mat.diagSubTimes", _genMat);

        cout << "done." << endl;
      }

      void expectedMatDiagDivide() {
        if(_rowInd >= _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedMatDiagDivide() ... ";

        _genMat.diag(-_rowInd) /= _genDouble;
        save<double>("Mat.diagSubDivide", _genMat);

        cout << "done." << endl;
      }

      void expectedMatColPlus() {
        if(_rowInd >= _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedMatColPlus() ... ";

        _genMat.row(_rowInd) += _genDouble;
        save<double>("Mat.rowPlus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatColMinus() {
        if(_rowInd >= _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedMatColMinus() ... ";

        _genMat.row(_rowInd) -= _genDouble;
        save<double>("Mat.rowMinus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatColTimes() {
        if(_rowInd >= _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedMatColTimes() ... ";

        _genMat.row(_rowInd) *= _genDouble;
        save<double>("Mat.rowTimes", _genMat);

        cout << "done." << endl;
      }

      void expectedMatColDivide() {
        if(_rowInd >= _genMat.n_rows) {
          return;
        }

        cout << "- Compute expectedMatColDivide() ... ";

        _genMat.row(_rowInd) /= _genDouble;
        save<double>("Mat.rowDivide", _genMat);

        cout << "done." << endl;
      }
  };
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/10/problem10.hpp"

BOOST_AUTO_TEST_SUITE( Problem10 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem10::solve(10);
        BOOST_CHECK_EQUAL(res, 17);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem10::solve();
        BOOST_CHECK_EQUAL(res, 142913828922);
    }

BOOST_AUTO_TEST_SUITE_END()


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/list/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/list.hpp>
#include <boost/hana/detail/minimal/product.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/maybe.hpp>

#include <tuple>
using namespace boost::hana;

constexpr auto prod = detail::minimal::product<>;

// stop_at must return a Maybe, so we need the comparison to be compile-time.
template <int i>
constexpr auto x = int_<i>;

template <typename mcd>
void test() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto list = detail::minimal::list<mcd>;
    using L = detail::minimal::List<mcd>;

    BOOST_HANA_CONSTEXPR_LAMBDA auto f = [=](auto ...xs) {
        return std::make_tuple(xs...);
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto stop_at = [=](auto stop) {
        return [=](auto x) {
            return if_(equal(stop, x),
                nothing,
                just(prod(x + int_<1>, f(x)))
            );
        };
    };

    BOOST_HANA_CONSTANT_ASSERT(unfoldl<L>(stop_at(x<0>), x<0>) == list());
    BOOST_HANA_CONSTEXPR_ASSERT(unfoldl<L>(stop_at(x<1>), x<0>) == list(f(x<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(unfoldl<L>(stop_at(x<2>), x<0>) == list(f(x<1>), f(x<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(unfoldl<L>(stop_at(x<3>), x<0>) == list(f(x<2>), f(x<1>), f(x<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(unfoldl<L>(stop_at(x<4>), x<0>) == list(f(x<3>), f(x<2>), f(x<1>), f(x<0>)));
}

// Make sure it can revert foldl under some conditions
template <typename mcd>
void test_revert() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto list = detail::minimal::list<mcd>;
    using L = detail::minimal::List<mcd>;
    static constexpr auto z = x<999>;

    BOOST_HANA_CONSTEXPR_LAMBDA auto f = prod;
    BOOST_HANA_CONSTEXPR_LAMBDA auto g = [=](auto k) {
        return if_(equal(k, z), nothing, just(k));
    };

    // Make sure the special conditions are met
    BOOST_HANA_CONSTANT_ASSERT(g(z) == nothing);
    BOOST_HANA_CONSTANT_ASSERT(g(f(z, x<0>)) == just(prod(z, x<0>)));

    // Make sure the reversing works
    auto lists = list(
        list(), list(x<0>), list(x<0>, x<1>), list(x<0>, x<1>, x<2>)
    );
    for_each(lists, [=](auto xs) {
        BOOST_HANA_CONSTANT_ASSERT(unfoldl<L>(g, foldl(xs, z, f)) == xs);
    });
}

int main() {
    test<List::mcd<void>>();
    test_revert<List::mcd<void>>();
}


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

// With contributions from Cornelius Steinhardt

#ifndef MTL_MATRIX_HESSENBERG_INCLUDE
#define MTL_MATRIX_HESSENBERG_INCLUDE

#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/operation/householder.hpp>
#include <boost/numeric/mtl/operation/rank_one_update.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace matrix {

/// Hessenberg-Factorization of matrix A with householder-vectors
/** Return Hessenberg matrix and tril(B,-2) are Householder-vectors **/
template <typename Matrix>
Matrix inline hessenberg_factors(const Matrix& A)
{
    vampir_trace<5014> tracer;
    if (num_rows(A) < 3)
	return A;

    using mtl::imax;
    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols = num_cols(A), nrows = num_rows(A);
    value_type       zero= math::zero(A[0][0]), beta;
    Matrix           B(clone(A));

    for(size_type i= 0; i < ncols-2; i++){
	// mtl::vector::dense_vector<value_type>  v(B[irange(i+1, imax)][i]);
		mtl::vector::dense_vector<value_type, vector::parameters<> >  v(nrows-i-1), w(nrows);
        for (size_type j = 0; j < size(v); j++)
            v[j]= B[j+i+1][i];
        beta= householder(v).second;
        v= householder(v).first;
;
	if( beta != zero){
            w= beta * B[irange(0,imax)][irange(i+1,imax)] * v;
	    //rank_one_update(B[irange(0,imax)][irange(i+1,imax)],-w,v);
            for(size_type row = 0; row < nrows; row++){
                for(size_type col = i+1; col < ncols; col++){
                    B[row][col] -= w[row] * v[col-i-1];
                }
            }
            //vector*Matrix
            for(size_type k=0; k < size(w); k++){
                w[k]= zero;
                for(size_type j = 0; j < size(v); j++){
                    w[k] += beta * v[j] * B[j+i+1][k];
                }
            }
            //rank_one_update(A[irange(i+1,imax)][irange(0,imax)],-v,w);
            for(size_type row = i+1; row < nrows; row++){
                for(size_type col = 0; col < ncols; col++){
                    B[row][col] -= v[row-i-1] * w[col];
                }
            }
	    // B[irange(i+2, imax)][i]= v[irange(1, nrows-i-1)];
            for(size_type row = i+2; row < nrows; row++){
                B[row][i] = v[row-i-1];
            }
        }
    }
    return B;
}

/// Extract Householder vectors from Hessenberg factorization H of some A
template <typename Matrix>
Matrix inline extract_householder_hessenberg(const Matrix& H)
{
    vampir_trace<5015> tracer;
    return Matrix(tril(H, -2));
}

/// Compute Householder vectors from Hessenberg factorization of A
template <typename Matrix>
Matrix inline householder_hessenberg(const Matrix& A)
{
    vampir_trace<5016> tracer;
    return Matrix(tril(hessenberg_factors(A), -2));
}

/// Extract Hessenberg form from factorization H of some A
template <typename Matrix>
Matrix inline extract_hessenberg(const Matrix& H)
{
    vampir_trace<5017> tracer;
    return Matrix(triu(H, -1));
}

/// Hessenberg form of A
template <typename Matrix>
Matrix inline hessenberg(const Matrix& A)
{
    vampir_trace<5018> tracer;
    // return triu(hessenberg_factors(A), -1);
    MTL_THROW_IF(num_rows(A) < 3, matrix_too_small());

    typedef typename Collection<Matrix>::value_type   value_type;
    // typedef typename Magnitude<value_type>::type      magnitude_type; // to multiply with 2 not 2+0i
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols = num_cols(A), nrows = num_rows(A);
    value_type       zero= math::zero(A[0][0]);
    Matrix           H(nrows,ncols);

    H= hessenberg_factors(A);

    // H= bands(hessenberg_factors(A), -nrows, -1);
    // set (doubly) strict lower triangle to zero
    for(size_type row = 2; row < nrows; row++){
        for(size_type col = 0; col < row-1; col++){
            H[row][col]= zero;
        }
    }

    return H;
}

/// Return Q where Q'*A*Q == hessenberg(A)
template <typename Matrix>
Matrix inline hessenberg_q(const Matrix& A)
{
    vampir_trace<5013> tracer;
    using std::abs;
    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Magnitude<value_type>::type      magnitude_type; // to multiply with 2 not 2+0i
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols = num_cols(A), nrows = num_rows(A), mini;
    value_type       zero= math::zero(A[0][0]), one= math::one(A[0][0]);
    const magnitude_type two(2);
    Matrix           Q(nrows,ncols);

    MTL_THROW_IF(num_rows(A) < 3, matrix_too_small());

    Q= one;

    //Extract Q
    for(size_type i = 0; i < nrows-2; i++){
		mtl::vector::dense_vector<value_type, vector::parameters<> >   v(nrows-1), w(nrows);
        v[0]= one;
// 	std::cout<< "v=" << v << "\n";
// 	std::cout<< "w=" << w << "\n";
        for(size_type k = 1; k < size(v); k++)
            v[k]= A[nrows-k][i];

	magnitude_type beta= two / abs(dot(v, v)); // abs: x+0i -> x
        if (beta != two) {
            //trans(Vector)*Matrix
            for(size_type k = 0; k < size(w); k++) {
                w[k]= zero;
                for(size_type j = 0; j < size(v); j++){
// 		     std::cout<< "k=" << k << "  j=" << j << "\n";
                    w[k]+= beta * v[j] * Q[j+i+1][k];
		}
            }
            //rank_one_update(Q[irange(i+1,imax)][irange(0,imax)],-v,w);
            for(size_type row = i+1; row < nrows; row++)
                for(size_type col = 0; col < ncols; col++){
// 		    std::cout<< "row=" << row << "  col=" << col << "\n";
                    Q[row][col] -= v[row-1] * w[col];
		}
        }
    }
    return Q;
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_HESSENBERG_INCLUDE


/**
 * @brief
 *
 * @file admitanceCalcTest.cpp
 * @author Luiz Victor Linhares Rocha <luizvictorlrocha@gmail.com>
 * @date 2018-09-22
 * @copyright 2018
 */

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/test/test_tools.hpp>
#include "../libraries/Eigen/Eigen"
#include "../rede/barra.hpp"
#include "../rede/branch.hpp"
#include "../rede/powerNet.hpp"
#include "../rede/admitanceCalc.hpp"
#include "../util/complexutils.h"

namespace tt = boost::test_tools;

using neuralFlux::PowerNet;
using neuralFlux::PowerNetPtr;
using neuralFlux::Bar;
using neuralFlux::BarPtr;
using neuralFlux::BranchPtr;
using neuralFlux::AdmitanceCalc;

struct FPower {
    FPower() : net(new PowerNet()) {
        net->addSlackBar(1, 1., 0.);
        net->addPQBar(2, 0.3, 0.);
        net->addPQBar(3, 0.3, 0.);
        net->addPQBar(4, 0.3, 0.);
        net->addPQBar(5, 0.3, 0.);

        net->connect(1, 2, 0., 0.1, 0., 0.98);
        net->connect(2, 3, 0., 0.1, 0., 1., 0.2);
        net->connect(2, 5, 0.1, 0.1);
        net->connect(3, 4, 0.1, 1.0);
        net->connect(4, 5, 0.1, 1.0);

        answer << std::complex<double>(0., -9.604),
            std::complex<double>(0, 9.8),
            std::complex<double>(0., 0.),
            std::complex<double>(0., 0.),
            std::complex<double>(0., 0.),
            std::complex<double>(0., 9.8),
            std::complex<double>(5., -25.),
            std::complex<double>(1.98669, 9.80067),
            std::complex<double>(0., 0.),
            std::complex<double>(-5., 5.),
            std::complex<double>(0., 0.),
            std::complex<double>(-1.98669, 9.80067),
            std::complex<double>(0.0990099, -10.9901),
            std::complex<double>(-0.0990099, 0.990099),
            std::complex<double>(0., 0.),
            std::complex<double>(0., 0.),
            std::complex<double>(0., 0.),
            std::complex<double>(-0.0990099, 0.990099),
            std::complex<double>(0.19802, -1.9802),
            std::complex<double>(-0.0990099, 0.990099),
            std::complex<double>(0., 0.),
            std::complex<double>(-5., 5.),
            std::complex<double>(0., 0.),
            std::complex<double>(-0.0990099, 0.990099),
            std::complex<double>(5.09901, -5.9901);
    }

    ~FPower() {
    }

    PowerNetPtr net;
    Eigen::Matrix<std::complex<double>, 5, 5> answer;
};

BOOST_FIXTURE_TEST_CASE(Admitance11, FPower) {
    BarPtr bar = net->getBar(1);
    std::complex<double> y = AdmitanceCalc::getAdmitanceKk(bar);
    BOOST_TEST(y.real() == answer(0, 0).real(), tt::tolerance(0.001));
    BOOST_TEST(y.imag() == answer(0, 0).imag(), tt::tolerance(0.001));
}

BOOST_FIXTURE_TEST_CASE(Admitance22, FPower) {
    BarPtr bar = net->getBar(2);
    std::complex<double> y = AdmitanceCalc::getAdmitanceKk(bar);
    BOOST_TEST(y.real() == answer(1, 1).real(), tt::tolerance(0.001));
    BOOST_TEST(y.imag() == answer(1, 1).imag(), tt::tolerance(0.001));
}

BOOST_FIXTURE_TEST_CASE(Admitance33, FPower) {
    BarPtr bar = net->getBar(3);
    std::complex<double> y = AdmitanceCalc::getAdmitanceKk(bar);
    BOOST_TEST(y.real() == answer(2, 2).real(), tt::tolerance(0.001));
    BOOST_TEST(y.imag() == answer(2, 2).imag(), tt::tolerance(0.001));
}

BOOST_FIXTURE_TEST_CASE(Admitance44, FPower) {
    BarPtr bar = net->getBar(4);
    std::complex<double> y = AdmitanceCalc::getAdmitanceKk(bar);
    BOOST_TEST(y.real() == answer(3, 3).real(), tt::tolerance(0.001));
    BOOST_TEST(y.imag() == answer(3, 3).imag(), tt::tolerance(0.001));
}

BOOST_FIXTURE_TEST_CASE(Admitance55, FPower) {
    BarPtr bar = net->getBar(5);
    std::complex<double> y = AdmitanceCalc::getAdmitanceKk(bar);
    BOOST_TEST(y.real() == answer(4, 4).real(), tt::tolerance(0.001));
    BOOST_TEST(y.imag() == answer(4, 4).imag(), tt::tolerance(0.001));
}

BOOST_FIXTURE_TEST_CASE(Admitance12, FPower) {
    BranchPtr branch = net->getBranchByIndex(0);
    std::complex<double> y = AdmitanceCalc::getAdmitanceKm(branch);
    BOOST_TEST(y.real() == answer(0, 1).real(), tt::tolerance(0.001));
    BOOST_TEST(y.imag() == answer(0, 1).imag(), tt::tolerance(0.001));
}

BOOST_FIXTURE_TEST_CASE(Admitance21, FPower) {
    BranchPtr branch = net->getBranchByIndex(0);
    std::complex<double> y = AdmitanceCalc::getAdmitanceMk(branch);
    BOOST_TEST(y.real() == answer(1, 0).real(), tt::tolerance(0.001));
    BOOST_TEST(y.imag() == answer(1, 0).imag(), tt::tolerance(0.001));
}

BOOST_FIXTURE_TEST_CASE(admitance, FPower) {
    Eigen::MatrixXcd Y = AdmitanceCalc::getMatrix(net);
    for (size_t i = 0; i < 5; i++) {
        for (size_t j = 0; j < 5; j++) {
            BOOST_TEST(Y(i, j).real() == answer(i, j).real(), tt::tolerance(0.001));
            BOOST_TEST(Y(i, j).imag() == answer(i, j).imag(), tt::tolerance(0.001));
        }
    }
}

BOOST_FIXTURE_TEST_CASE(NetAdmitance, FPower) {
    for (size_t i = 0; i < 5; i++) {
        for (size_t j = 0; j < 5; j++) {
            BOOST_TEST(net->getY(i, j).real() == answer(i, j).real(), tt::tolerance(0.001));
            BOOST_TEST(net->getY(i, j).imag() == answer(i, j).imag(), tt::tolerance(0.001));
        }
    }
}

BOOST_FIXTURE_TEST_CASE(BranchOnOff, FPower) {
    net->getBranchByIndex(4)->setBranchOn(false);
    PowerNetPtr net2(new PowerNet());
    net2->addSlackBar(1, 1., 0.);
    net2->addPQBar(2, 0.3, 0.);
    net2->addPQBar(3, 0.3, 0.);
    net2->addPQBar(4, 0.3, 0.);
    net2->addPQBar(5, 0.3, 0.);
    net2->connect(1, 2, 0., 0.1, 0., 0.98);
    net2->connect(2, 3, 0., 0.1, 0., 1., 0.2);
    net2->connect(2, 5, 0.1, 0.1);
    net2->connect(3, 4, 0.1, 1.0);
    for (size_t i = 0; i < 5; i++) {
        for (size_t j = 0; j < 5; j++) {
            BOOST_TEST(net->getY(i, j).real() == net2->getY(i, j).real(), tt::tolerance(0.001));
            BOOST_TEST(net->getY(i, j).imag() == net2->getY(i, j).imag(), tt::tolerance(0.001));
        }
    }
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/monoid.hpp>
#include <boost/hana/tuple.hpp>

#include <test/auto/base.hpp>
#include <test/auto/monoid.hpp>
using namespace boost::hana;

namespace boost { namespace hana { namespace test {
    template <> auto objects<int> = make<Tuple>(0,1,2,3,4,5);
    template <> auto objects<unsigned int> = make<Tuple>(0u,1u,2u,3u,4u,5u);
    template <> auto objects<long> = make<Tuple>(0l,1l,2l,3l,4l,5l);
    template <> auto objects<unsigned long> = make<Tuple>(0ul,1ul,2ul,3ul,4ul,5ul);
}}}

int main() {
    test::laws<Monoid, int>();
    test::laws<Monoid, unsigned int>();

    test::laws<Monoid, long>();
    test::laws<Monoid, unsigned long>();

    using T = int;
    constexpr T x = 6, y = 4;

    // zero
    {
        BOOST_HANA_CONSTEXPR_CHECK(zero<T>() == static_cast<T>(0));
    }

    // plus
    {
        BOOST_HANA_CONSTEXPR_CHECK(plus(x, y) == x + y);
    }
}


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_geometry_linear_cone_hpp__
#define __multicontact_api_geometry_linear_cone_hpp__

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <pinocchio/spatial/se3.hpp>

#include "multicontact-api/geometry/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"

#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS_SIZE(TYPE, ROWS) \
  EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime == ROWS, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)

namespace multicontact_api {
namespace geometry {

template <typename _Scalar, int _dim, int _Options>
struct LinearCone : public serialization::Serializable<LinearCone<_Scalar, _dim, _Options> > {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef _Scalar Scalar;
  enum { dim = _dim, Options = _Options };
  typedef Eigen::Matrix<Scalar, dim, -1, Options> MatrixDx;
  typedef Eigen::Matrix<Scalar, dim, dim, Options> MatrixD;
  typedef Eigen::Matrix<Scalar, dim, 1, Options> VectorD;
  typedef Eigen::DenseIndex Index;

  /// \brief Default constructor
  LinearCone() : m_rays() {}

  /// \brief Constructor from a set of rays
  template <typename EigenDerived>
  explicit LinearCone(const Eigen::MatrixBase<EigenDerived>& rays)
      //      : m_rays(_dim,rays.cols())
      : m_rays(rays) {
    //        EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(EigenDerived,MatrixDx)
    //        for(int k=0; k<rays.cols(); ++k)
    //          m_rays.col(k) = rays.col(k).normalized();
  }

  /// \brief Contrustor from a given size.
  explicit LinearCone(const Index size) : m_rays(_dim, size) {}

  /// \brief Copy constructor
  template <typename S2, int O2>
  LinearCone(const LinearCone<S2, dim, O2>& other) : m_rays(other.m_rays) {}

  void addRay(const VectorD& ray) {
    m_rays.conservativeResize(Eigen::NoChange_t(), m_rays.cols() + 1);
    m_rays.template rightCols<1>() = ray.normalized();
  }

  template <typename EigenDerived>
  void stack(const Eigen::MatrixBase<EigenDerived>& rays) {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS_SIZE(EigenDerived, dim);
    m_rays.conservativeResize(Eigen::NoChange_t(), m_rays.cols() + rays.cols());
    m_rays.rightCols(rays.cols()) = rays;
  }

  template <typename S2, int O2>
  void stack(const LinearCone<S2, dim, O2>& other) {
    stack(other.rays());
  }

  /// \returns the rays of the linear cone.
  const MatrixDx& rays() const { return m_rays; }
  MatrixDx& rays() { return m_rays; }

  /// \returns the number of rays, i.e. the number of cols of m_rays
  Index size() const { return m_rays.cols(); }

  template <typename S2, int O2>
  bool operator==(const LinearCone<S2, dim, O2>& other) const {
    return m_rays == other.m_rays;
  }

  template <typename S2, int O2>
  bool operator!=(const LinearCone<S2, dim, O2>& other) const {
    return !(*this == other);
  }

  template <typename S2, int O2>
  bool isApprox(const LinearCone<S2, dim, O2>& other,
                const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
    return m_rays.isApprox(other.m_rays, prec);
  }

  void disp(std::ostream& os) const { os << "Rays:\n" << m_rays << std::endl; }

  friend std::ostream& operator<<(std::ostream& os, const LinearCone& C) {
    C.disp(os);
    return os;
  }

 protected:
  /// \brief Rays of the linear cone
  MatrixDx m_rays;

 private:
  // Serialization of the class
  friend class boost::serialization::access;

  template <class Archive>
  void save(Archive& ar, const unsigned int /*version*/) const {
    ar& boost::serialization::make_nvp("rays", m_rays);
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int /*version*/) {
    ar >> boost::serialization::make_nvp("rays", m_rays);
  }

  BOOST_SERIALIZATION_SPLIT_MEMBER()
};

template <typename _Scalar, int _Options>
struct ForceConeTpl : public LinearCone<_Scalar, 3, _Options> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef LinearCone<_Scalar, 3, _Options> Base;
  typedef WrenchConeTpl<_Scalar, _Options> WrenchCone;
  typedef pinocchio::SE3Tpl<_Scalar, _Options> SE3;
  using typename Base::Scalar;
  enum { dim = Base::dim };
  using Base::rays;
  using Base::size;
  using typename Base::Index;
  using typename Base::MatrixD;
  using typename Base::MatrixDx;
  using typename Base::VectorD;
  using Base::operator==;
  using Base::operator!=;
  //      using Base::isApprox; // Leads to a bug with clang

  typedef MatrixDx Matrix3x;
  typedef VectorD Vector3;
  typedef Eigen::AngleAxis<Scalar> AngleAxis;

  /// \brief Default constructor
  ForceConeTpl() : Base() {}

  template <typename EigenDerived>
  explicit ForceConeTpl(const Eigen::MatrixBase<EigenDerived>& rays) : Base(rays) {}

  explicit ForceConeTpl(const Index size) : Base(size) {}

  /// \returns a linear cone built from a friction coefficient and the number of rays along the Z axis.
  static ForceConeTpl RegularCone(const Scalar mu, const VectorD& direction, const int num_rays,
                                  const Scalar theta_offset = 0.) {
    assert(mu >= 0. && "mu must be positive");
    assert(num_rays >= 1 && "The number of rays must be at least one");

    const VectorD normalized_direction(direction.normalized());
    ForceConeTpl cone(num_rays);

    const Scalar angle = (2. * M_PI) / num_rays;

    const MatrixD Po(MatrixD::Identity() - normalized_direction * normalized_direction.transpose());

    const MatrixD rot_offset(AngleAxis(theta_offset, normalized_direction).toRotationMatrix());
    const VectorD init_direction(rot_offset * (Po * VectorD::Ones()).normalized());
    const MatrixD rot(AngleAxis(angle, normalized_direction).toRotationMatrix());

    VectorD ray((direction + mu * init_direction).normalized());

    for (int k = 0; k < num_rays; ++k) {
      cone.rays().col(k) = ray;
      if (k != num_rays - 1) ray = rot * ray;
    }

    return cone;
  }

  WrenchCone SE3ActOn(const SE3& M) const {
    WrenchCone res(size());
    typedef typename WrenchCone::MatrixDx::ColXpr Col6Xpr;
    typedef typename MatrixDx::ConstColXpr ConstCol3Xpr;

    const typename SE3::Matrix3& R = M.rotation();
    const typename SE3::Vector3& t = M.translation();

    for (Index k = 0; k < size(); ++k) {
      ConstCol3Xpr in_col = rays().col(k);
      Col6Xpr out_col = res.rays().col(k);

      out_col.template head<3>() = R * in_col;
      out_col.template tail<3>() = t.cross(out_col.template head<3>());
    }

    return res;
  }

  template <typename S2, int O2>
  bool isApprox(const ForceConeTpl<S2, O2>& other,
                const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
    return Base::isApprox(other, prec);
  }

  operator WrenchCone() const {
    WrenchCone res(size());
    res.rays().template topRows<3>() = rays();
    res.rays().template bottomRows<3>().setZero();
    return res;
  }
};

template <typename _Scalar, int _Options>
struct WrenchConeTpl : public LinearCone<_Scalar, 6, _Options> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef LinearCone<_Scalar, 6, _Options> Base;
  typedef ForceConeTpl<_Scalar, _Options> ForceCone;
  typedef pinocchio::SE3Tpl<_Scalar, _Options> SE3;
  using typename Base::Scalar;
  enum { dim = Base::dim };
  using Base::rays;
  using Base::size;
  using typename Base::Index;
  using typename Base::MatrixDx;
  using typename Base::VectorD;
  using Base::operator==;
  using Base::operator!=;
  //      using Base::isApprox; // Leads to a bug with clang

  typedef MatrixDx Matrix6x;
  typedef VectorD Vector6;

  typedef typename ForceCone::Matrix3x Matrix3x;

  typedef typename Matrix6x::template NRowsBlockXpr<3>::Type LinearBlock;
  typedef typename Matrix6x::template ConstNRowsBlockXpr<3>::Type ConstLinearBlock;

  typedef LinearBlock AngularBlock;
  typedef ConstLinearBlock ConstAngularBlock;

  /// \brief Default constructor
  WrenchConeTpl() : Base() {}

  /// \brief Constructor from a set of rays.
  template <typename EigenDerived>
  explicit WrenchConeTpl(const Eigen::MatrixBase<EigenDerived>& rays) : Base(rays) {}

  /// \brief Constructs a WrenchCone of a given size.
  explicit WrenchConeTpl(const Index size) : Base(size) {}

  /// \brief Constructs a WrenchCone of a given size.
  template <typename S2, int O2>
  explicit WrenchConeTpl(const ForceConeTpl<S2, O2>& force_cone) : Base(force_cone.size()) {
    rays().template topRows<3>() = force_cone.rays();
    rays().template bottomRows<3>().setZero();
  }

  /// \brief Copy constructor
  template <typename S2, int O2>
  WrenchConeTpl(const WrenchConeTpl<S2, O2>& other) : Base(other) {}

  WrenchConeTpl SE3ActOn(const SE3& M) const {
    WrenchConeTpl res(size());
    typedef typename MatrixDx::ColXpr Col6Xpr;
    typedef typename MatrixDx::ConstColXpr ConstCol6Xpr;

    const typename SE3::Matrix3& R = M.rotation();
    const typename SE3::Vector3& t = M.translation();

    for (Index k = 0; k < size(); ++k) {
      ConstCol6Xpr in_col = rays().col(k);
      Col6Xpr out_col = res.rays().col(k);

      out_col.template head<3>() = R * in_col.template head<3>();
      out_col.template tail<3>() = t.cross(out_col.template head<3>()) + R * in_col.template tail<3>();
    }

    return res;
  }

  template <typename S2, int O2>
  bool isApprox(const WrenchConeTpl<S2, O2>& other,
                const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
    return Base::isApprox(other, prec);
  }

  ConstLinearBlock linear() const { return rays().template topRows<3>(); }
  LinearBlock linear() { return rays().template topRows<3>(); }

  ConstAngularBlock angular() const { return rays().template bottomRows<3>(); }
  AngularBlock angular() { return rays().template bottomRows<3>(); }

  ForceCone toForceCone() const { return ForceCone(linear()); }
};
}  // namespace geometry
}  // namespace multicontact_api

#endif  // ifndef __multicontact_api_geometry_linear_cone_hpp__


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdBinomialCoefficientLog, Fvar) {
  using boost::math::digamma;
  using stan::math::binomial_coefficient_log;
  using stan::math::fvar;

  fvar<double> x(2004.0, 1.0);
  fvar<double> y(1002.0, 2.0);

  fvar<double> a = stan::math::binomial_coefficient_log(x, y);
  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0, 1002.0), a.val_);
  EXPECT_FLOAT_EQ(0.69289774, a.d_);
}

TEST(AgradFwdBinomialCoefficientLog, FvarFvarDouble) {
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;
  using stan::math::fvar;

  fvar<fvar<double> > x;
  x.val_.val_ = 2004.0;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > y;
  y.val_.val_ = 1002.0;
  y.d_.val_ = 1.0;

  fvar<fvar<double> > a = binomial_coefficient_log(x, y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0, 1002.0), a.val_.val_);
  EXPECT_FLOAT_EQ(0.69289774, a.val_.d_);
  EXPECT_NEAR(0, a.d_.val_, 1e-8);
  EXPECT_FLOAT_EQ(0.0009975062, a.d_.d_);
}

struct binomial_coefficient_log_fun {
  template <typename T0, typename T1>
  inline typename boost::math::tools::promote_args<T0, T1>::type operator()(
      const T0 arg1, const T1 arg2) const {
    return binomial_coefficient_log(arg1, arg2);
  }
};

TEST(AgradFwdBinomialCoefficientLog, nan) {
  binomial_coefficient_log_fun binomial_coefficient_log_;
  test_nan_fwd(binomial_coefficient_log_, 3.0, 5.0, false);
}


/*
 * sdf2sdf_optimizer2d.cpp
 *
 *  Created on: Mar 05, 2019
 *      Author: Fei Shan
 */

#pragma once

//libraries
#include <Eigen/Eigen>

//local
#include "../tsdf/parameters.hpp"
#include "../tsdf/generator.hpp"

namespace eig = Eigen;

namespace rigid_optimization {

class Sdf2SdfOptimizer2d {
    public:
        struct VerbosityParameters {
            VerbosityParameters(bool print_iteration_max_warp_update = false,
                                bool print_iteration_energy = false);
            //per-iteration parameters
            const bool print_iteration_max_warp_update = false;
            const bool print_iteration_energy = false;
            const bool print_per_iteration_info = false;
        };

        Sdf2SdfOptimizer2d(
                float rate = 0.5f,
                int maximum_iteration_count = 60,
				tsdf::Parameters2d tsdf_generation_parameters = tsdf::Parameters2d(),
                VerbosityParameters verbosity_parameters = VerbosityParameters()
        );

        virtual ~Sdf2SdfOptimizer2d();

    eig::Matrix3f optimize(int image_y_coordinate,
                           const eig::MatrixXf canonical_field,
						   const eig::Matrix<unsigned short, eig::Dynamic, eig::Dynamic>& live_depth_image,
                           float eta = 0.01f,
						   const eig::Matrix4f& initial_camera_pose = eig::Matrix4f::Identity());

    private:
        const float rate = 0.5f;
        const int maximum_iteration_count = 60;
        const tsdf::Generator2d tsdf_generator;
        const Sdf2SdfOptimizer2d::VerbosityParameters verbosity_parameters;

};

}


//####### Test module for Quantum Synchrotron emission engine (SI units) #######

//Define Module name
 #define BOOST_TEST_MODULE "Quantum Synchrotron emission engine (SI units)"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

 #include <utility>

 //Include Boost unit tests library & library for floating point comparison
 #include <boost/test/unit_test.hpp>
 #include <boost/test/floating_point_comparison.hpp>

//SI units are used for this test
#define PXRMP_USE_SI_UNITS
#include "quantum_sync_engine.hpp"

//Uses random numbers
#include "rng_wrapper.hpp"

using namespace picsar::multi_physics;

//________________________________

//Helper function
template<typename REAL>
quantum_synchrotron_engine<REAL, stl_rng_wrapper<REAL>> get_qs_stl_set_lambda(uint64_t seed, REAL lambda,
quantum_synchrotron_engine_ctrl<REAL> qs_ctrl = quantum_synchrotron_engine_ctrl<REAL>())
{
    stl_rng_wrapper<REAL> wrap{seed};
    auto qs_engine =  quantum_synchrotron_engine<REAL, stl_rng_wrapper<REAL>>{std::move(wrap), 1.0, qs_ctrl};
    qs_engine.set_lambda(static_cast<REAL>(lambda));
    return qs_engine;
}

// ------------- Tests --------------

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-3;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-2;

//Templated tolerance
template <typename T>
T tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

//***SI UNITS***
//SI units for momenta
const double me_c = electron_mass * light_speed;
//SI units for fields
double lambda = 800.0 * si_nanometer;
double eref = 2.0*pi*electron_mass*light_speed*light_speed/
            (lambda*elementary_charge);
double bref = eref/light_speed;
//SI units for dt and rate
double dtref = lambda/(2.0*pi*light_speed);
double rateref = 1.0/dtref;

//Test get/set lambda for quantum_synchrotron_engine generic
template <typename T>
void quantum_sync_engine_gs()
{
    auto qs_engine = get_qs_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));
    BOOST_CHECK_EQUAL( static_cast<T>(1.0), qs_engine.get_lambda());
}

//Test get/set lambda for quantum_synchrotron_engine (double precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_gs_double_1 )
{
    quantum_sync_engine_gs<double>();
}

//Test get/set lambda for quantum_synchrotron_engine (single precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_gs_single_1 )
{
    quantum_sync_engine_gs<float>();
}

// ------------- optical depth --------------
//Test get/set lambda for quantum_synchrotron_engine generic
template <typename T>
void quantum_sync_engine_opt()
{
    auto qs_engine = get_qs_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));
    BOOST_TEST ( qs_engine.get_optical_depth() >= static_cast<T>(0.0) );
}

//Test get new optical depth (double precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_opt_double_1 )
{
    quantum_sync_engine_opt<double>();
}

//Test get new optical depth (single precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_opt_single_1 )
{
    quantum_sync_engine_opt<float>();
}


#include <catch2/catch.hpp>

#include <trianglelite/trianglelite.h>

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

TEST_CASE("Point cloud", "[trianglelite]")
{
    using namespace trianglelite;

    Config config;
    Engine engine;

    SECTION("Convex hull")
    {
        config.convex_hull = true;
        config.verbose_level = 0;
    }
    SECTION("With max area")
    {
        config.max_area = 0.1;
        config.verbose_level = 0;
    }
    SECTION("With max area too small")
    {
        config.max_area = 1e-7;
        config.verbose_level = 0;
    }

    Eigen::Matrix<Scalar, 3, 2, Eigen::RowMajor> points;
    points << 0.0, 0.0, 1.0, 0.0, 0.0, 1.0;
    engine.set_in_points(points.data(), static_cast<int>(points.rows()));
    engine.run(config);

    auto out_points = engine.get_out_points();
    REQUIRE(out_points.rows() >= 3);
    auto out_triangles = engine.get_out_triangles();
    REQUIRE(out_triangles.rows() >= 1);
}

TEST_CASE("Quad", "[trianglelite]")
{
    using namespace trianglelite;

    Config config;
    Engine engine;

    config.max_area = 0.1;
    config.verbose_level = 0;

    Eigen::Matrix<Scalar, 4, 2, Eigen::RowMajor> points;
    points << 0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0;
    engine.set_in_points(points.data(), static_cast<int>(points.rows()));

    Eigen::Matrix<int, 4, 2, Eigen::RowMajor> segments;
    segments << 0, 1, 1, 2, 2, 3, 3, 0;
    engine.set_in_segments(segments.data(), static_cast<int>(segments.rows()));

    engine.run(config);

    auto out_points = engine.get_out_points();
    REQUIRE(out_points.rows() >= 3);
    auto out_triangles = engine.get_out_triangles();
    REQUIRE(out_triangles.rows() >= 1);
}

TEST_CASE("QuadWithHole", "[trianglelite][hole]")
{
    using namespace trianglelite;

    Config config;
    Engine engine;

    config.max_area = 0.05;
    config.verbose_level = 0;
    config.auto_hole_detection = true;

    Eigen::Matrix<Scalar, 8, 2, Eigen::RowMajor> points;
    points << 0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.2, 0.2, 0.2, 0.8, 0.8, 0.8, 0.8, 0.2;
    engine.set_in_points(points.data(), static_cast<int>(points.rows()));

    Eigen::Matrix<int, 8, 2, Eigen::RowMajor> segments;
    segments << 0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6, 6, 7, 7, 4;
    engine.set_in_segments(segments.data(), static_cast<int>(segments.rows()));

    engine.run(config);

    auto out_points = engine.get_out_points();
    auto out_edges = engine.get_out_edges();
    auto out_triangles = engine.get_out_triangles();

    const int euler = static_cast<int>(out_points.rows()) - static_cast<int>(out_edges.rows()) +
                      static_cast<int>(out_triangles.rows());
    REQUIRE(euler == 0);
}

TEST_CASE("Marker", "[trianglelite][marker]")
{
    using namespace trianglelite;

    Config config;
    Engine engine;

    std::vector<Scalar> points{0, 0, 1, 0, 0, 1};
    std::vector<Index> segments{0, 1, 1, 2, 2, 0};
    std::vector<Index> point_markers{4, 5, 6};
    std::vector<Index> segment_markers{1, 2, 3};
    engine.set_in_points(points.data(), static_cast<int>(points.size() / 2));
    engine.set_in_segments(segments.data(), static_cast<int>(segments.size() / 2));
    engine.set_in_point_markers(point_markers.data(), static_cast<int>(point_markers.size()));
    engine.set_in_segment_markers(segment_markers.data(), static_cast<int>(segment_markers.size()));

    config.max_area = 0.1;
    config.verbose_level = 0;

    engine.run(config);

    auto out_points = engine.get_out_points();
    auto out_edges = engine.get_out_edges();
    auto out_segments = engine.get_out_segments();
    auto out_triangles = engine.get_out_triangles();

    REQUIRE(out_points.rows() > 3);
    REQUIRE(out_edges.rows() > 3);
    REQUIRE(out_segments.rows() > 3);
    REQUIRE(out_triangles.rows() > 1);

    SECTION("Point markers") {
        auto out_point_markers = engine.get_out_point_markers();
        const size_t num_out_point_markers = out_point_markers.size();
        REQUIRE(num_out_point_markers == out_points.rows());
        for (size_t i=0; i<num_out_point_markers; i++) {
            if (out_point_markers[i] == 4) {
                REQUIRE(out_points(i, 0) == Approx(0));
                REQUIRE(out_points(i, 1) == Approx(0));
            } else if (out_point_markers[i] == 5) {
                REQUIRE(out_points(i, 0) == Approx(1));
                REQUIRE(out_points(i, 1) == Approx(0));
            } else if (out_point_markers[i] == 6) {
                REQUIRE(out_points(i, 0) == Approx(0));
                REQUIRE(out_points(i, 1) == Approx(1));
            }
        }
    }

    SECTION("Edge markers") {
        auto edge_markers = engine.get_out_edge_markers();
        REQUIRE(edge_markers.size() == out_edges.rows());

        const size_t num_edges = out_edges.rows();
        for (size_t i=0; i<num_edges; i++) {
            const Index v0 = out_edges(i, 0);
            const Index v1 = out_edges(i, 1);
            if (edge_markers[i] == 1) {
                REQUIRE(out_points(v0, 1) == Approx(0.0));
                REQUIRE(out_points(v1, 1) == Approx(0.0));
            } else if (edge_markers[i] == 2) {
                REQUIRE(out_points.row(v0).sum() == Approx(1.0));
                REQUIRE(out_points.row(v1).sum() == Approx(1.0));
            } else if (edge_markers[i] == 3) {
                REQUIRE(out_points(v0, 0) == Approx(0.0));
                REQUIRE(out_points(v1, 0) == Approx(0.0));
            } else {
                REQUIRE(edge_markers[i] == 0);
            }
        }
    }

    SECTION("Segment markers") {
        auto segment_markers = engine.get_out_segment_markers();
        REQUIRE(out_segments.rows() == segment_markers.size());
    }
}


/* Boost libs/numeric/odeint/examples/openmp/lorenz_ensemble.cpp

 Copyright 2013 Karsten Ahnert
 Copyright 2013 Mario Mulansky
 Copyright 2013 Pascal Germroth

 Parallelized Lorenz ensembles

 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 <omp.h>
#include <vector>
#include <iostream>
#include <iterator>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/openmp/openmp.hpp>
#include <boost/lexical_cast.hpp>
#include "point_type.hpp"

using namespace std;
using namespace boost::numeric::odeint;

typedef point<double, 3> point_type;
typedef vector< point_type > inner_state_type;
typedef openmp_state<point_type> state_type;

const double sigma = 10.0;
const double b = 8.0 / 3.0;

struct sys_func {
    const vector<double> &R;
    sys_func( vector<double> &R ) : R(R) {}

    void operator()( const state_type &x , state_type &dxdt , double t ) const {
#       pragma omp parallel for
        for(size_t j = 0 ; j < x.size() ; j++) {
            size_t offset = 0;
            for(size_t i = 0 ; i < j ; i++)
                offset += x[i].size();

            for(size_t i = 0 ; i < x[j].size() ; i++) {
                const point_type &xi = x[j][i];
                point_type &dxdti = dxdt[j][i];
                dxdti[0] = -sigma * (xi[0] - xi[1]);
                dxdti[1] = R[offset + i] * xi[0] - xi[1] - xi[0] * xi[2];
                dxdti[2] = -b * xi[2] + xi[0] * xi[1];
            }
        }
    }
};

int main(int argc, char **argv) {
    size_t n = 1024;
    if(argc > 1) n = boost::lexical_cast<size_t>(argv[1]);

    vector<double> R(n);
    const double Rmin = 0.1, Rmax = 50.0;
#   pragma omp parallel for
    for(size_t i = 0 ; i < n ; i++)
        R[i] = Rmin + (Rmax - Rmin) / (n - 1) * i;

    vector<point_type> inner(n, point_type(10, 10, 10));
    state_type state;
    split(inner, state);

    cerr << "openmp_state split " << n << " into";
    for(size_t i = 0 ; i != state.size() ; i++)
        cerr << ' ' << state[i].size();
    cerr << endl;

    typedef runge_kutta4< state_type, double > stepper;

    const double t_max = 10.0, dt = 0.01;

    integrate_const(
        stepper(),
        sys_func(R),
        state,
        0.0, t_max, dt
    );

    unsplit(state, inner);
    std::copy( inner.begin(), inner.end(), ostream_iterator<point_type>(cout, "\n") );

    return 0;
}


/** ****************************************************************************
 *  @file    FaceHeadPose.cpp
 *  @brief   Face detection and recognition framework
 *  @author  Roberto Valle Fernandez
 *  @date    2015/06
 *  @copyright All rights reserved.
 *  Software developed by UPM PCR Group: http://www.dia.fi.upm.es/~pcr
 ******************************************************************************/

// ----------------------- INCLUDES --------------------------------------------
#include <FaceHeadPose.hpp>
#include <ModernPosit.h>
#include <boost/filesystem.hpp>

namespace upm {

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
cv::Mat
projectAxis
  (
  const cv::Point3f &headpose
  )
{
  cv::Mat axis = cv::Mat::eye(3,3,cv::DataType<float>::type); // [yaw (blue), pitch (green), roll (red)]
  cv::Mat ann_axis, rot_matrix = ModernPosit::eulerToRotationMatrix(headpose);
  rot_matrix = (cv::Mat_<float>(3,3) <<  rot_matrix.at<float>(1,2), rot_matrix.at<float>(1,1),-rot_matrix.at<float>(1,0),
                                        -rot_matrix.at<float>(0,2),-rot_matrix.at<float>(0,1), rot_matrix.at<float>(0,0),
                                         rot_matrix.at<float>(2,2), rot_matrix.at<float>(2,1),-rot_matrix.at<float>(2,0));
  ann_axis = rot_matrix*axis;
  return ann_axis;
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
void
FaceHeadPose::show
  (
  const boost::shared_ptr<upm::Viewer> &viewer,
  const std::vector<upm::FaceAnnotation> &faces,
  const upm::FaceAnnotation &ann
  )
{
  // Ground truth
  cv::Scalar blue_color(255,0,0), green_color(0,255,0), red_color(0,0,255);
  double length = static_cast<int>(roundf(ann.bbox.pos.height)*0.5f);
  int thickness = MAX(static_cast<int>(roundf(ann.bbox.pos.height*0.01f)), 3);
  cv::Mat ann_axis = projectAxis(ann.headpose) * length;
  cv::Point mid = (ann.bbox.pos.tl() + ann.bbox.pos.br()) * 0.5;
  viewer->line(mid.x, mid.y, mid.x+ann_axis.at<float>(1,0), mid.y-ann_axis.at<float>(0,0), thickness, blue_color);
  viewer->line(mid.x, mid.y, mid.x+ann_axis.at<float>(1,1), mid.y-ann_axis.at<float>(0,1), thickness, green_color);
  viewer->line(mid.x, mid.y, mid.x+ann_axis.at<float>(1,2), mid.y-ann_axis.at<float>(0,2), thickness, red_color);

  // Estimated head-pose
  cv::Scalar cyan_color(122,0,0), lime_color(0,122,0), salmon_color(0,0,122);
  for (const FaceAnnotation &face : faces)
  {
    length = static_cast<int>(roundf(face.bbox.pos.height)*0.5f);
    thickness = MAX(static_cast<int>(roundf(face.bbox.pos.height*0.01f)), 3);
    cv::Mat face_axis = projectAxis(face.headpose) * length;
    mid = (face.bbox.pos.tl() + face.bbox.pos.br()) * 0.5;
    viewer->line(mid.x, mid.y, mid.x+face_axis.at<float>(1,0), mid.y-face_axis.at<float>(0,0), thickness, cyan_color);
    viewer->line(mid.x, mid.y, mid.x+face_axis.at<float>(1,1), mid.y-face_axis.at<float>(0,1), thickness, lime_color);
    viewer->line(mid.x, mid.y, mid.x+face_axis.at<float>(1,2), mid.y-face_axis.at<float>(0,2), thickness, salmon_color);
  }
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
void
FaceHeadPose::evaluate
  (
  boost::shared_ptr<std::ostream> output,
  const std::vector<upm::FaceAnnotation> &faces,
  const upm::FaceAnnotation &ann
  )
{
  for (const FaceAnnotation &face : faces)
    *output << getComponentClass() << " " << ann.filename << " " << ann.headpose << " " << face.headpose << std::endl;
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
void
FaceHeadPose::save
  (
  const std::string dirpath,
  const std::vector<upm::FaceAnnotation> &faces,
  const upm::FaceAnnotation &ann
  )
{
  // Save images with mean error greater than threshold
  const float threshold = 25.0f;
  cv::Scalar blue_color(255,0,0), green_color(0,255,0), red_color(0,0,255), cyan_color(122,0,0), lime_color(0,122,0), salmon_color(0,0,122);
  double length = static_cast<int>(roundf(ann.bbox.pos.height)*0.5f);
  int thickness = MAX(static_cast<int>(roundf(ann.bbox.pos.height*0.01f)), 3);
  for (const FaceAnnotation &face : faces)
  {
    cv::Mat image = cv::imread(face.filename, cv::IMREAD_COLOR);
    cv::Mat ann_axis = projectAxis(ann.headpose) * length;
    cv::Point mid = (ann.bbox.pos.tl() + ann.bbox.pos.br()) * 0.5;
    cv::line(image, mid, cv::Point2f(mid.x+ann_axis.at<float>(1,0), mid.y-ann_axis.at<float>(0,0)), blue_color, thickness);
    cv::line(image, mid, cv::Point2f(mid.x+ann_axis.at<float>(1,1), mid.y-ann_axis.at<float>(0,1)), green_color, thickness);
    cv::line(image, mid, cv::Point2f(mid.x+ann_axis.at<float>(1,2), mid.y-ann_axis.at<float>(0,2)), red_color, thickness);

    length = static_cast<int>(roundf(face.bbox.pos.height)*0.5f);
    thickness = MAX(static_cast<int>(roundf(face.bbox.pos.height*0.01f)), 3);
    cv::Mat face_axis = projectAxis(face.headpose) * length;
    mid = (face.bbox.pos.tl() + face.bbox.pos.br()) * 0.5;
    cv::line(image, mid, cv::Point2f(mid.x+face_axis.at<float>(1,0), mid.y-face_axis.at<float>(0,0)), cyan_color, thickness);
    cv::line(image, mid, cv::Point2f(mid.x+face_axis.at<float>(1,1), mid.y-face_axis.at<float>(0,1)), lime_color, thickness);
    cv::line(image, mid, cv::Point2f(mid.x+face_axis.at<float>(1,2), mid.y-face_axis.at<float>(0,2)), salmon_color, thickness);

    // Absolute head-pose error
    float error = static_cast<float>(cv::sum(cv::abs(cv::Mat(ann.headpose-face.headpose).t()))[0]);
    std::string text = std::to_string(error);
    cv::putText(image, text, cv::Point(10, image.rows-10), cv::FONT_HERSHEY_SIMPLEX, 1, red_color);
    if (error > threshold)
    {
      std::size_t found = face.filename.find_last_of('/');
      std::string filepath;
      unsigned int num = 0;
      do
      {
        filepath = dirpath + std::to_string(num) + "_" + face.filename.substr(found+1);
        num++;
      }
      while (boost::filesystem::exists(filepath));
      cv::imwrite(filepath, image);
    }
  }
};

} // namespace upm


#include <scitbx/lstbx/normal_equations.h>

#include <boost_adaptbx/optional_conversions.h>

#include <boost/python/class.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/return_internal_reference.hpp>

namespace scitbx { namespace lstbx { namespace normal_equations {
namespace boost_python {

  template <typename FloatType>
  struct linear_ls_wrapper
  {
    typedef linear_ls<FloatType> wt;
    typedef typename wt::scalar_t scalar_t;
    typedef typename wt::symmetric_matrix_t symmetric_matrix_t;
    typedef typename wt::vector_t vector_t;

    static void wrap(char const *name) {
      using namespace boost::python;

      boost_adaptbx::optional_conversions::to_and_from_python<
        boost::optional<wt> >();

      class_<wt>(name, no_init)
        .def(init<int>(arg("n_parameters")))
        .def(init<symmetric_matrix_t const &, vector_t const &>(
             (arg("normal_matrix"), arg("right_hand_side"))))
        .add_property("n_parameters", &wt::n_parameters)
        .def("add_equation",
             &wt::add_equation,
             (arg("right_hand_side"), arg("design_matrix_row"), arg("weight")))
        .def("add_equations",
             &wt::add_equations,
             (arg("right_hand_side"), arg("design_matrix"), arg("weights"),
              arg("negate_right_hand_side")=false,
              arg("optimise_for_tall_matrix")=true))
        .def("reset", &wt::reset)
        .def("solve", &wt::solve)
        .add_property("solved", &wt::solved)
        /* We use 'def' instead of add_property for those because they may
           throw if called on an instanced which is not solved.
           On the Python side, an attribute lookup which may throw is a
           source of confusion (e.g. hasattr does not work correctly for those).
         */
        .def("normal_matrix_packed_u", &wt::normal_matrix)
        .def("right_hand_side", &wt::right_hand_side)
        .def("cholesky_factor_packed_u", &wt::cholesky_factor)
        .def("solution", &wt::solution)
        ;
    }
  };

  template <typename FloatType>
  struct non_linear_ls_wrapper
  {
    typedef non_linear_ls<FloatType> wt;
    typedef typename wt::scalar_t scalar_t;
    typedef typename wt::vector_t vector_t;
    typedef typename wt::symmetric_matrix_t symmetric_matrix_t;

    static void wrap(char const *name) {
      using namespace boost::python;
      return_internal_reference<> rir;
      void (wt::*add_dense_eqns)(af::const_ref<scalar_t> const &,
                                 af::const_ref<scalar_t, af::mat_grid> const &,
                                 af::const_ref<scalar_t> const &)
        = &wt::add_equations;
      void (wt::*add_sparse_eqns)(af::const_ref<scalar_t> const &,
                                  sparse::matrix<scalar_t> const &,
                                  af::const_ref<scalar_t> const &,
                                  bool, bool)
        = &wt::add_equations;

      class_<wt>(name, no_init)
        .def(init<int>(arg("n_parameters")))
        .def(init<std::size_t,
                  scalar_t,
                  vector_t const &,
                  symmetric_matrix_t const &>
             ((arg("n_equations"),
               arg("objective"),
               arg("opposite_of_grad_objective"),
               arg("normal_matrix"))))
        .add_property("n_parameters", &wt::n_parameters)
        .add_property("n_equations", &wt::n_equations)
        .add_property("dof", &wt::dof)
        .def("add_residual",
             &wt::add_residual,
             (arg("residual"), arg("weight")))
        .def("add_residuals",
             &wt::add_residuals,
             (arg("residuals"), arg("weights")))
        .def("add_equation",
             &wt::add_equation,
             (arg("residual"), arg("grad_residual"), arg("weight")))
        .def("add_equations",
             add_dense_eqns,
             (arg("residuals"), arg("jacobian"), arg("weights")))
        .def("add_equations",
             add_sparse_eqns,
             (arg("residuals"), arg("jacobian"), arg("weights"),
             arg("negate_right_hand_side")=true, arg("optimise_for_tall_matrix")=true))
        .def("reset", &wt::reset)
        /* We use 'def' instead of add_property for those to stay consistent
           with the other wrappers in this module which can't use properties
         */
        .def("objective", &wt::objective)
        .def("chi_sq", &wt::chi_sq)
        .def("step_equations", &wt::step_equations, rir)
        ;
    }
  };

  template <typename FloatType, template<typename> class SumOfRank1Updates>
  struct non_linear_ls_with_separable_scale_factor_wrapper
  {
    typedef non_linear_ls_with_separable_scale_factor<FloatType,
                                                      SumOfRank1Updates>
            wt;
    typedef typename wt::scalar_t scalar_t;

    static void add_equation(wt &self,
                             scalar_t yc, af::const_ref<scalar_t> const &grad_yc,
                             scalar_t yo, scalar_t w)
    {
      self.add_equation(yc, grad_yc, yo, w);
    }

    static void wrap(std::string const &name) {
      using namespace boost::python;
      return_internal_reference<> rir;
      class_<wt>(name.c_str(), no_init)
        .def(init<int, bool>((arg("n_parameters"), arg("normalised")=true)))
        .add_property("n_parameters", &wt::n_parameters)
        .add_property("n_equations", &wt::n_equations)
        .add_property("dof", &wt::dof)
        .def("add_residual",
             &wt::add_residual,
             (arg("y_calc"), arg("y_obs"), arg("weight")))
        .def("add_equation", add_equation,
             (arg("y_calc"), arg("grad_y_calc"), arg("y_obs"), arg("weight")))
        .def("add_equations", &wt::add_equations,
             (arg("ys_calc"), arg("jacobian_y_calc"), arg("ys_obs"),
              arg("weights")))
        .def("finalise", &wt::finalise, arg("objective_only")=false)
        .add_property("finalised", &wt::finalised)
        .def("reset", &wt::reset)
        /* We use 'def' instead of add_property for those because they may
           throw if called on an instanced which is not finalised.
           On the Python side, an attribute lookup which may throw is a
           source of confusion (e.g. hasattr does not work correctly for those).
         */
        .def("optimal_scale_factor", &wt::optimal_scale_factor)
        .def("sum_w_yo_sq", &wt::sum_w_yo_sq)
        .def("objective", &wt::objective)
        .def("chi_sq", &wt::chi_sq)
        .def("step_equations", &wt::step_equations, rir)
        .def("reduced_problem", &wt::reduced_problem, rir)
        ;
    }
  };

  void wrap_normal_equations() {
    linear_ls_wrapper<double>::wrap("linear_ls");
    non_linear_ls_wrapper<double>::wrap("non_linear_ls");
    std::string basename("non_linear_ls_with_separable_scale_factor");
    non_linear_ls_with_separable_scale_factor_wrapper<
      double, matrix::sum_of_symmetric_rank_1_updates>
      ::wrap(basename + "__level_2_blas_impl");
    non_linear_ls_with_separable_scale_factor_wrapper<
      double, matrix::rank_n_update>
      ::wrap(basename + "__level_3_blas_impl");
  }

}}}}


#include "debug_macros.h"
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Robust_circumcenter_traits_3.h>
#include <CGAL/Delaunay_triangulation_3.h>

#include <CGAL/Polyhedral_surface_3.h>

#include <CGAL/Surface_mesher/Standard_criteria.h>

#define CGAL_C2T3_USE_POLYHEDRON
#include <CGAL/IO/Complex_2_in_triangulation_3_file_writer.h>

#include <CGAL/Surface_mesher/Point_surface_indices_oracle_visitor.h>

#include "parameters.h"

#include <CGAL/Timer.h>

#include <iostream>
#include <fstream>

#include <sstream>

#include <boost/tuple/tuple.hpp> // boost::tie

using boost::tie;

/////////////// Types ///////////////

struct K2 : public CGAL::Exact_predicates_inexact_constructions_kernel {};
typedef CGAL::Robust_circumcenter_traits_3<K2>  K;
typedef CGAL::Complex_2_in_triangulation_vertex_base_3<K> Vb;
typedef CGAL::Surface_mesh_cell_base_3<K> Cb;
typedef CGAL::Triangulation_data_structure_3<Vb, Cb> Tds;
typedef CGAL::Delaunay_triangulation_3<K, Tds> Tr;

typedef K::Point_3 Point_3;
typedef K::Sphere_3 Sphere_3;
typedef K::FT FT;

typedef CGAL::Polyhedral_surface_3<K> Surface;

typedef CGAL::Surface_mesher::Refine_criterion
 Criterion;
typedef CGAL::Surface_mesher::Standard_criteria <Criterion > Criteria;

typedef CGAL::Complex_2_in_triangulation_3
 C2t3;

typedef CGAL::Surface_mesher::Polyhedral_oracle<Surface> Surface_mesh_traits;

typedef Surface_mesh_traits::Construct_initial_points Initial_points;

typedef CGAL::Simple_cartesian<double> Simple_kernel;
typedef Simple_kernel::Iso_rectangle_2 Rectangle_2;
typedef Simple_kernel::Segment_2 Segment_2;
typedef Simple_kernel::Point_2 Point_2;

/// Global variables
std::ostream *out = 0;
std::string filename = std::string();
std::string function_name = "";
char* argv0 = "";

void usage(std::string error = "")
{
  if( error != "" )
    std:: cerr << "Error: " << error << std::endl;
  std::cerr << "Usage:\n  "
            << argv0
            << " -f function_name"
            << " [output_file.off|-]\n"
            << "If output_file.off is '-', outputs to standard out.\n"
            << "-f define the OFF file to remesh.\n";
  for(String_options::iterator it = string_options.begin();
      it != string_options.end();
      ++it)
    std::cerr << "--" << it->first << " default value is \""
	      << it->second << "\".\n";
  for(Double_options::iterator it = double_options.begin();
      it != double_options.end();
      ++it)
    std::cerr << "--" << it->first << " default value is "
	      << it->second << ".\n";
  exit(EXIT_FAILURE);
}

std::pair<std::ostream*, const bool>
open_file_for_writing(std::string filename,
                      std::string display_string = "Writing to ")
{
  if( filename != "")
  {
    if( filename == "-" )
    {
      std::cerr << display_string << "standard out...\n";
      return std::make_pair(&std::cout, false);
    }
    else
    {
      std::ofstream* result = new std::ofstream(filename.c_str());
      if( *result )
      {
        std::cerr << display_string << "file " << filename << "...\n";
        return std::make_pair(result, true);
      }
      else
      {
        delete result;
        std::cerr << "Error: cannot create " << filename << "\n";
        usage();
        return std::pair<std::ostream*, bool>(0, false);
      }
    }
  }
  else
    return std::pair<std::ostream*, bool>(0, false);
}

void parse_argv(int argc, char** argv, int extra_args = 0)
{
  if (argc >=(2 + extra_args))
    {
      std::string arg = argv[1+extra_args];
      if( arg == "-h" || arg == "--help")
        usage();
      else if( arg == "-f" )
        {
          if( argc < (3 + extra_args) )
            usage("-f must be followed by a function name!");
          function_name = argv[2 + extra_args];
          parse_argv(argc, argv, extra_args + 2);
        }
      else if( arg.substr(0, 2) == "--" )
	{
	  Double_options::iterator opt_it =
	    double_options.find(arg.substr(2, arg.length()-2));
	  if( opt_it != double_options.end() )
	    {
	      if( argc < (3 + extra_args) )
		usage((arg + " must be followed by a double!").c_str());
	      std::stringstream s;
	      double val;
	      s << argv[extra_args + 2];
	      s >> val;
	      if( !s )
		usage(("Bad double after " + arg + "!").c_str());
	      opt_it->second = val;
	      parse_argv(argc, argv, extra_args + 2);
	    }
	  else
          {
            String_options::iterator opt_it =
                string_options.find(arg.substr(2, arg.length()-2));
            if( opt_it != string_options.end() )
            {
              if( argc < (3 + extra_args) )
                usage((arg + " must be followed by a string!").c_str());
              std::string s = argv[extra_args + 2];
              opt_it->second = s;
              parse_argv(argc, argv, extra_args + 2);
            }
            else
              usage(("Invalid option " + arg).c_str());
          }
	}
      else
	{
	  filename = argv[1+extra_args];
	  parse_argv(argc, argv, extra_args + 1);
	}
    }
}

/////////////// Main function ///////////////

int main(int argc, char **argv) {
  argv0 = argv[0];

  usage_ptr = &usage;

  init_parameters();

  parse_argv(argc, argv);

  if( function_name == "" )
    usage("Empty input file name");

  std::ifstream surface_ifs(function_name.c_str());
  Surface surface(surface_ifs);
  surface_ifs.close();

  std::cerr << "Surface bounding box: " << surface.bbox() << "\n";

  // 2D-complex in 3D-Delaunay triangulation
  Tr tr;
  C2t3 c2t3(tr);

  CGAL::Timer timer;

  bool need_delete = false;
  std::ostream* out = 0;

  Surface_mesh_traits surface_mesh_traits;

  // Initial point sample
  std::string read_initial_points = get_string_option("read_initial_points");
  if( read_initial_points != "")
  {
    std::ifstream in( read_initial_points.c_str() );
    int n;
    in >> n;
    CGAL_assertion(in);
    while( !in.eof() )
      {
	Point_3 p;
	if(in >> p)
	  {
	    tr.insert(p);
	    --n;
	  }
      }
    CGAL_assertion( n == 0 );
    double_options["number_of_initial_points"] = 0;
  }
  else
  {
    const int number_of_initial_points =
      static_cast<int>(get_double_option("number_of_initial_points"));

    std::vector<Point_3> initial_point_sample;
    initial_point_sample.reserve(number_of_initial_points);

    Initial_points get_initial_points =
      surface_mesh_traits.construct_initial_points_object();

    get_initial_points(surface,
                       std::back_inserter(initial_point_sample),
                       number_of_initial_points);

    tie(out, need_delete) =
      open_file_for_writing(get_string_option("dump_of_initial_points"),
                            "Writing initial points to ");
    if( out )
    {
      *out << initial_point_sample.size() << "\n";
      for(std::vector<Point_3>::const_iterator it =
            initial_point_sample.begin();
          it != initial_point_sample.end();
          ++it)
        *out << *it <<"\n";
      if(need_delete)
        delete out;
    }
    tr.insert (initial_point_sample.begin(), initial_point_sample.end());
  }

  // Meshing criteria
  CGAL::Surface_mesher::Curvature_size_criterion

    curvature_size_criterion (get_double_option("distance_bound"));
  CGAL::Surface_mesher::Uniform_size_criterion

    uniform_size_criterion (get_double_option("radius_bound"));
  CGAL::Surface_mesher::Aspect_ratio_criterion

    aspect_ratio_criterion (get_double_option("angle_bound"));

  std::vector<Criterion*> criterion_vector;
  criterion_vector.push_back(&aspect_ratio_criterion);
  criterion_vector.push_back(&uniform_size_criterion);
  criterion_vector.push_back(&curvature_size_criterion);
  Criteria criteria (criterion_vector);

  std::cerr << "\nInitial number of points: " << tr.number_of_vertices()
            << std::endl;

  // Surface meshing

  timer.start();
  make_surface_mesh(c2t3,
                    surface,
                    criteria,
                    CGAL::Manifold_with_boundary_tag(),
                    0);
  timer.stop();
  std::cerr << "\nFinal number of points: " << tr.number_of_vertices()
            << std::endl
            << "Total time: " << timer.time() << std::endl;

  tie(out, need_delete) =
    open_file_for_writing(filename,
                          "Writing finale surface off to ");
  if( out )
  {
    CGAL::output_surface_facets_to_off(*out, c2t3);
    if(need_delete)
      delete out;
  }
  std::cerr << " done\n";

#ifdef CGAL_SURFACE_MESHER_TEST_OPTIONS
  check_all_options_have_been_used();
#endif
}


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

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_ref_snip6

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

using namespace boost::math;

// Lower quantile rounded down:
double x = quantile(negative_binomial(20, 0.3), 0.05);
// Upper quantile rounded up:
double y = quantile(complement(negative_binomial(20, 0.3), 0.05));

//]

#include <iostream>

int main()
{
   std::cout << x << " " << y << std::endl;
}


//==================================================================================================
/*
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
*/
//==================================================================================================
#include <eve/module/core.hpp>
#include <eve/module/bessel.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>

int main()
{
  EVE_VALUE mid = sizeof(EVE_VALUE) == 4 ? 2 : 5;
  {
    auto lmin = EVE_VALUE(0);
    auto lmax = EVE_VALUE(mid);

    auto arg0 = eve::bench::random_<EVE_VALUE>(lmin,lmax);
    auto stdy0 = [](auto x){return std::cyl_neumann(0, x);};
    auto boosty0= [](auto x){return boost::math::cyl_neumann(0, x);};
    eve::bench::experiment xp;
    run<EVE_TYPE> (EVE_NAME(cyl_bessel_y0_small) , xp, eve::cyl_bessel_y0 , arg0);
    run<EVE_VALUE>(EVE_NAME(cyl_bessel_y0_small) , xp, eve::cyl_bessel_y0 , arg0);
    run<EVE_VALUE>(EVE_NAME(stdy0_small) , xp, stdy0 , arg0);
    run<EVE_VALUE>(EVE_NAME(boosty0_small), xp, boosty0 , arg0);
  }
  {
    auto lmin = EVE_VALUE(mid);
    auto lmax = EVE_VALUE(8);

    auto arg0 = eve::bench::random_<EVE_VALUE>(lmin,lmax);
    auto stdy0 = [](auto x){return std::cyl_neumann(0, x);};
    auto boosty0= [](auto x){return boost::math::cyl_neumann(0, x);};
    eve::bench::experiment xp;
    run<EVE_TYPE> (EVE_NAME(cyl_bessel_y0_medium) , xp, eve::cyl_bessel_y0 , arg0);
    run<EVE_VALUE>(EVE_NAME(cyl_bessel_y0_medium) , xp, eve::cyl_bessel_y0 , arg0);
    run<EVE_VALUE>(EVE_NAME(stdy0_medium) , xp, stdy0 , arg0);
    run<EVE_VALUE>(EVE_NAME(boosty0_medium), xp, boosty0 , arg0);
  }
  {
    auto lmin = EVE_VALUE(8);
    auto lmax = EVE_VALUE(10000);

    auto arg0 = eve::bench::random_<EVE_VALUE>(lmin,lmax);
    auto stdy0 = [](auto x){return std::cyl_neumann(0, x);};
    auto boosty0= [](auto x){return boost::math::cyl_neumann(0, x);};
    eve::bench::experiment xp;
    run<EVE_TYPE> (EVE_NAME(cyl_bessel_y0_large) , xp, eve::cyl_bessel_y0 , arg0);
    run<EVE_VALUE>(EVE_NAME(cyl_bessel_y0_large) , xp, eve::cyl_bessel_y0 , arg0);
    run<EVE_VALUE>(EVE_NAME(stdy0_large) , xp, stdy0 , arg0);
    run<EVE_VALUE>(EVE_NAME(boosty0_large), xp, boosty0 , arg0);
  }
  {
    auto lmin = EVE_VALUE(0);
    auto lmax = EVE_VALUE(11);

    auto arg0 = eve::bench::random_<EVE_VALUE>(lmin,lmax);
    auto stdy0 = [](auto x){return std::cyl_neumann(0, x);};
    auto boosty0= [](auto x){return boost::math::cyl_neumann(0, x);};
    eve::bench::experiment xp;
    run<EVE_TYPE> (EVE_NAME(cyl_bessel_y0_mixed) , xp, eve::cyl_bessel_y0 , arg0);
    run<EVE_VALUE>(EVE_NAME(cyl_bessel_y0_mixed) , xp, eve::cyl_bessel_y0 , arg0);
    run<EVE_VALUE>(EVE_NAME(stdy0_mixed) , xp, stdy0 , arg0);
    run<EVE_VALUE>(EVE_NAME(boosty0_mixed), xp, boosty0 , arg0);
  }
}


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <logger/logger.hpp>
#include <sframe/integer_pack.hpp>
#include <serialization/serialization_includes.hpp>
using namespace turi;
using namespace integer_pack;
struct integer_pack_test {
 public:
  void test_variable_code() {
    for (size_t shift = 0; shift < 64; shift += 8) {
      for (uint64_t i = 0; i < 256; ++i) {
        oarchive oarc;
        variable_encode(oarc, i << shift);
        uint64_t j;
        iarchive iarc(oarc.buf, oarc.off);
        variable_decode(iarc, j);
        TS_ASSERT_EQUALS(oarc.off, iarc.off);
        free(oarc.buf);
        TS_ASSERT_EQUALS(i << shift, j);
      }
    }
  }
  void test_pack() {
    {
      size_t len = 8;
      uint64_t in[8] = {19,20,21,22,23,24,25,26};
      uint64_t out[8];
      oarchive oarc;
      frame_of_reference_encode_128(in, 8, oarc);

      iarchive iarc(oarc.buf, oarc.off);
      frame_of_reference_decode_128(iarc, 8, out);
      TS_ASSERT_EQUALS(oarc.off, iarc.off);
      free(oarc.buf);

      for (size_t i = 0;i < len; ++i) {
        TS_ASSERT_EQUALS(in[i], out[i]);
      }
    }
    // simple cases
    for (size_t mod = 1; mod < 63; ++mod) {
      for (size_t len = 0; len <= 128; ++len) {
        uint64_t in[len];
        uint64_t out[len];
        for (size_t i = 0;i < len; ++i) {
          if (mod == 0) {
            in[i] = 0;
          } else {
            in[i] = (i % mod) & (1 << (mod - 1));
          }
        }
        oarchive oarc;
        frame_of_reference_encode_128(in, len, oarc);

        iarchive iarc(oarc.buf, oarc.off);
        frame_of_reference_decode_128(iarc, len, out);
        TS_ASSERT_EQUALS(oarc.off, iarc.off);
        free(oarc.buf);

        for (size_t i = 0;i < len; ++i) {
          if (in[i] != out[i]) std::cout << mod << " " << len << " " << i << "\n";
          TS_ASSERT_EQUALS(in[i], out[i]);
        }
      }
    }

    // harder cases
    for (size_t multiplier = 1; multiplier < 63; ++multiplier) {
      for (size_t shift = 1; shift < 63; ++shift) {
        size_t len = 128;
        uint64_t in[len];
        uint64_t out[len];
        for (size_t i = 0;i < len; ++i) {
          in[i] = shift + (multiplier * i);
        }
        oarchive oarc;
        frame_of_reference_encode_128(in, len, oarc);

        iarchive iarc(oarc.buf, oarc.off);
        frame_of_reference_decode_128(iarc, len, out);
        TS_ASSERT_EQUALS(oarc.off, iarc.off);
        free(oarc.buf);

        for (size_t i = 0;i < len; ++i) {
          TS_ASSERT_EQUALS(in[i], out[i]);
        }
      }
      for (size_t mod = 1; mod < 63; ++mod) {
        size_t len = 128;
        uint64_t in[len];
        uint64_t out[len];
        for (size_t i = 0;i < len; ++i) {
          in[i] = (multiplier * i) % mod;
        }
        oarchive oarc;
        frame_of_reference_encode_128(in, len, oarc);

        iarchive iarc(oarc.buf, oarc.off);
        frame_of_reference_decode_128(iarc, len, out);
        TS_ASSERT_EQUALS(oarc.off, iarc.off);
        free(oarc.buf);

        for (size_t i = 0;i < len; ++i) {
          TS_ASSERT_EQUALS(in[i], out[i]);
        }
      }
    }

    // integer boundary cases
    int64_t maxint = std::numeric_limits<int64_t>::max() >> 4;
    for (size_t multiplier = maxint; multiplier < maxint; ++multiplier) {
      size_t len = 128;
      uint64_t in[len];
      uint64_t out[len];
      for (size_t i = 0;i < len; ++i) {
        in[i] = (multiplier * i);
      }
      oarchive oarc;
      frame_of_reference_encode_128(in, len, oarc);

      iarchive iarc(oarc.buf, oarc.off);
      frame_of_reference_decode_128(iarc, len, out);
      TS_ASSERT_EQUALS(oarc.off, iarc.off);
      free(oarc.buf);

      for (size_t i = 0;i < len; ++i) {
        TS_ASSERT_EQUALS(in[i], out[i]);
      }
    }
  }
  void test_shift_encode() {
    int64_t maxint = std::numeric_limits<int64_t>::max();
    int64_t minint = std::numeric_limits<int64_t>::min();
    for (int64_t i = maxint - 256; i < maxint; ++i) {
      uint64_t j = shifted_integer_encode(i);
      int64_t i2 = shifted_integer_decode(j);
      TS_ASSERT_EQUALS(i, i2);
    }
    for (int64_t i = minint; i < minint + 256; ++i) {
      uint64_t j = shifted_integer_encode(i);
      int64_t i2 = shifted_integer_decode(j);
      TS_ASSERT_EQUALS(i, i2);
    }
    for (int64_t i = -256; i < 256; ++i) {
      uint64_t j = shifted_integer_encode(i);
      int64_t i2 = shifted_integer_decode(j);
      TS_ASSERT_EQUALS(i, i2);
    }
  }
};

BOOST_FIXTURE_TEST_SUITE(_integer_pack_test, integer_pack_test)
BOOST_AUTO_TEST_CASE(test_variable_code) {
  integer_pack_test::test_variable_code();
}
BOOST_AUTO_TEST_CASE(test_pack) {
  integer_pack_test::test_pack();
}
BOOST_AUTO_TEST_CASE(test_shift_encode) {
  integer_pack_test::test_shift_encode();
}
BOOST_AUTO_TEST_SUITE_END()


/***
 *  File: BDE_score.hpp
 *  Created: June 4, 2012
 *
 *  Author: Olga Nikolova <olga.nikolova@gmail.com>
 */

#ifndef BDE_SCORE_HPP
#define BDE_SCORE_HPP

#include <jaz/math_add.hpp>
#include <jaz/plain_array.hpp>
#include <algorithm>
#include <vector>
#include <cmath> // this doesn't seem to work
#include <boost/math/special_functions/gamma.hpp>

class BDE_score {
public:
	/**
	 */
	typedef unsigned int index_type;

	/**
	 */
	typedef double value_type;

	BDE_score(unsigned int n, unsigned int m,
			  const jaz::plain_array<unsigned char>& D)
    : n_(n), m_(m), D_(D), ess_(1.0) {

		r_ = *std::max_element(D.begin(), D.end()) + 1;
		px_.resize(r_, 0);
		init_bde_x_();
		data_bins_.resize(m_);
		data_index_.resize(m_);
	} // BDE_score

	value_type operator()(index_type xi) { return bde_x_[xi]; }

	template <typename Iter>
	value_type operator()(index_type xi, Iter first, Iter last) {
		if (first == last) {
			return bde_x_[xi];
		}

		if (check_key_(first, last) == false) {
			generate_index_(first, last);
		} // if

		unsigned int k = std::distance(first, last);
		double qi = static_cast<double>(pow(r_, k));

		// Set priors
		double ess_qi = ess_ / qi;
		double ess_riqi = ess_ / ( qi * static_cast<double>(r_) );

		// Compute commonly used Gamma-fn. values
		double lg_ess_qi = boost::math::lgamma(ess_qi);
		double lg_ess_riqi = boost::math::lgamma(ess_riqi);

		// compute internal sum
		int pos = -1;
		const unsigned char* tab = D_.begin() + xi * m_;

		double H = 0.0;
		S_ = 0.0;

		px_.zero();

		for (unsigned int i = 0; i < m_; ++i) {
			px_[tab[data_index_[i].second]]++;
			if (data_bins_[i] == true) {
				// p(Pa)
				double pa_cnt = i - pos; // Nij
				//double p_pa = pa_cnt / m_;

				H = 0.0;
				for (unsigned char j = 0; j < r_; ++j) {
					if (px_[j] != 0) {
						// p(x|Pa)
						//double p_xpa = static_cast<double>(px_[j]) / pa_cnt;
						double p_xpa = static_cast<double>(px_[j]); // Nijk
						//	    H += p_pa * p_xpa * log2(p_xpa);
						H += boost::math::lgamma( p_xpa + ess_riqi ) - lg_ess_riqi;

					}
				} // for j

				S_ += lg_ess_qi - boost::math::lgamma( pa_cnt + ess_qi ) + H;

				//std::cout<< "S( " << xi << ",{ ";
				//for(unsigned int ii = 0; ii < k; ii++)
				//  std::cout << pa_[ii] << ", ";
				//std::cout << "}) = " << lg_ess_qi - boost::math::lgamma( pa_cnt + ess_qi ) << " + " << H << std::endl;

				px_.zero();
				pos = i;
			} // if
		} // for i

		return ((-1)*S_);
	} // operator()

private:
	template <typename Iter> bool check_key_(Iter first, Iter last) {
		unsigned int k = std::distance(first, last);
		if (k != pa_.size()) return false;
		for (unsigned int i = 0; i < k; ++i) if (first[i] != pa_[i]) return false;
		return true;
	} // check_key_

	void init_bde_x_() {
		//    mdl_x_.resize(n_, logn_ + hlogm_ * (r_ - 1));
		bde_x_.resize(n_, 0);
		const unsigned char* tab = D_.begin();

		double ess_ri = ess_ /static_cast<double>(r_);
		double lg_ess_ri = boost::math::lgamma( ess_ri );

		//std::cout<< "ess_=" << ess_ << " ess_ri = " << ess_ri << " Gamma(ess_/ri) = " <<  lg_ess_ri << std::endl;

		for (unsigned int i = 0; i < n_; ++i) {
			px_.zero();
			double H = 0.0;
			for (unsigned int j = 0; j < m_; ++j) px_[tab[j]]++;
			for (unsigned int j = 0; j < r_; ++j) {
				if (px_[j] != 0) {
					//	  double pi = static_cast<double>(px_[j]) / m_;
					double pi = static_cast<double>(px_[j]); // Nijk
					//std::cout << "N_i=" << i << ",k=" << j << " == " << pi << "(Nijk)" << std::endl;
					//H += pi * log2(pi);
					//std::cout << "log(Gamma(Nijk + alpha/ri)) = " <<  boost::math::lgamma(pi + ess_ri ) << std::endl;
					//std::cout << "log(Gamma(alpha/ri)) = " <<  boost::math::lgamma( ess_ri ) << std::endl;

					H += boost::math::lgamma(pi + ess_ri ) - lg_ess_ri ;
				}
			}
			bde_x_[i] = (-1)*(boost::math::lgamma(ess_) - boost::math::lgamma( static_cast<double>(m_) + ess_) + H);
			//std::cout << "BDe(" << i << ", {}) = " <<   boost::math::lgamma(ess_) - boost::math::lgamma( static_cast<double>(m_) + ess_) << " + " << H << " = "   << bde_x_[i] << std::endl;
			tab += m_;
		}
	} // init_mdl_x_

	typedef std::vector<char> key_t;
	typedef std::pair<key_t, unsigned int> data_index_t;

	struct data_index_lt_ {
		bool operator()(const data_index_t& lhs, const data_index_t& rhs) {
			return lhs.first < rhs.first;
		}
	}; // struct data_index_lt_

	// index of D for given set of parents
	std::vector<bool> data_bins_;
	std::vector<data_index_t> data_index_;

	template <typename Iter> void generate_index_(Iter first, Iter last) {
		// store new parents set
		unsigned int k = std::distance(first, last);

		pa_.resize(k);
		std::copy(first, last, pa_.begin());

		// prepare index data
		for (unsigned int i = 0; i < m_; ++i) {
			data_bins_[i] = false;
			data_index_[i].first.resize(k);
			data_index_[i].second = i;
		}

		// generate index
		for (unsigned int i = 0; i < k; ++i) {
			const unsigned char* tab = D_.begin() + pa_[i] * m_;
			for (unsigned int j = 0; j < m_; ++j) data_index_[j].first[i] = tab[j];
		}

		std::sort(data_index_.begin(), data_index_.end(), data_index_lt_());

		// find bins boundaries
		key_t key = data_index_[0].first;

		for (unsigned int i = 0; i < m_; ++i) {
			if (key < data_index_[i].first) {
				key = data_index_[i].first;
				data_bins_[i - 1] = true;
			}
		}

		data_bins_[m_ - 1] = true;
	} // generate_index_

	unsigned int n_;
	unsigned int m_;

	const jaz::plain_array<unsigned char>& D_;

	unsigned int r_;

	jaz::plain_array<unsigned int> px_;

	double S_;
	double ess_;

	// BDE score of x with no parents
	jaz::plain_array<double> bde_x_;

	// set of parents
	std::vector<unsigned int> pa_;

}; // class MDL_score

#endif // MDL_SCORE_HPP


#include <vector>
#include <cmath>
#include <Eigen/Core>

#include "ihgp/Matern32model.hpp"

Matern32model::Matern32model()
{
    magnSigma2 = 1.0;
    lengthScale = 1.0;
    sigma2 = 1.0;
    Matern32model::updateModel();
}

void Matern32model::updateModel()
{
    // Model matrices for Matern v=3/2
    double lambda = sqrt(3.0)/lengthScale;
    F.setZero(2,2); Pinf.setZero(2,2); H.setZero(1,2);
    F << 0.0, 1.0, -lambda*lambda, -2*lambda;
    Pinf << magnSigma2, 0.0, 0.0, magnSigma2*lambda*lambda;
    H << 1.0, 0.0;
    R = sigma2;

    // Derivatives dF
    dF.clear();
    dF.push_back(Eigen::MatrixXd::Zero(2,2));
    dF.push_back(Eigen::MatrixXd::Zero(2,2));
    Eigen::MatrixXd foo(2,2);
    foo << 0, 0, 6/lengthScale/lengthScale/lengthScale, 2*lambda/lengthScale;
    dF.push_back(foo);

    // Derivatives dPinf
    dPinf.clear();
    dPinf.push_back(Eigen::MatrixXd::Zero(2,2));
    foo << 1.0, 0, 0, 3.0/lengthScale/lengthScale;
    dPinf.push_back(foo);
    foo << 0, 0, 0,-6*magnSigma2/lengthScale/lengthScale/lengthScale;
    dPinf.push_back(foo);

    // Derivatives dR
    dR.clear();
    dR.push_back(1.0);
    dR.push_back(0.0);
    dR.push_back(0.0);
}

void Matern32model::setMagnSigma2(const double &val)
{
    magnSigma2 = val;
    Matern32model::updateModel();
}

void Matern32model::setLengthScale(const double &val)
{
    lengthScale = val;
    Matern32model::updateModel();
}

void Matern32model::setSigma2(const double &val)
{
    sigma2 = val;
    Matern32model::updateModel();
}

MatrixXd Matern32model::getF()
{
    return F;
}

MatrixXd Matern32model::getPinf()
{
    return Pinf;
}

MatrixXd Matern32model::getH()
{
    return H;
}

double Matern32model::getR()
{
    return R;
}

vector< MatrixXd > Matern32model::getdF()
{
   return dF;
}

vector< MatrixXd > Matern32model::getdPinf()
{
    return dPinf;
}

vector< double > Matern32model::getdR()
{
    return dR;
}

double Matern32model::getMagnSigma2()
{
    return magnSigma2;
}

double Matern32model::getLengthScale()
{
    return lengthScale;
}

double Matern32model::getSigma2()
{
    return sigma2;
}


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

#include "transfer_bbox_with_depth_map.h"

#include <math.h>
#include <assert.h>
#include <tuple>
#include <iostream>
#include <sstream>
#include <vital/algo/image_io.h>
#include <vital/io/camera_io.h>
#include <vital/config/config_difference.h>
#include <vital/util/string.h>
#include <Eigen/Core>

using namespace kwiver::vital;

namespace kwiver {
namespace arrows {
namespace core {

// ---------------------------------------------------------------------------
int
nearest_index(int max, double value)
{
  if (abs(value - 0.0) < 1e-6)
  {
    return 0;
  }
  else if (abs(value - (double)max) < 1e-6)
  {
    return max - 1;
  }
  else
  {
    return (int)round(value - 0.5);
  }
}

// ---------------------------------------------------------------------------
vector_3d
backproject_to_depth_map
(kwiver::vital::camera_perspective_sptr const camera,
 kwiver::vital::image_container_sptr const depth_map,
 vector_2d const& img_pt)
{
  vector_2d npt_ = camera->intrinsics()->unmap(img_pt);
  auto npt = vector_3d(npt_(0), npt_(1), 1.0);

  matrix_3x3d M = camera->rotation().matrix().transpose();
  vector_3d cam_pos = camera->center();

  vector_3d Mp = M * npt;

  kwiver::vital::image dm_data = depth_map->get_image();
  auto dm_width = (int)dm_data.width();
  auto dm_height = (int)dm_data.height();

  int img_pt_x = nearest_index(dm_width, img_pt(0));
  int img_pt_y = nearest_index(dm_height, img_pt(1));

  if (img_pt_x < 0 || img_pt_y < 0 ||
      img_pt_x >= dm_width ||
      img_pt_y >= dm_height)
  {
    throw std::invalid_argument("Provided image point is outside of image "
                                "bounds");
  }

  float depth = dm_data.at<float>(img_pt_x, img_pt_y);

  vector_3d world_pos = cam_pos + (Mp * depth);

  return world_pos;
}

// ---------------------------------------------------------------------------
std::tuple<vector_3d, vector_3d>
backproject_wrt_height
(kwiver::vital::camera_perspective_sptr const camera,
 kwiver::vital::image_container_sptr const depth_map,
 vector_2d const& img_pt_bottom,
 vector_2d const& img_pt_top)
{
  vector_3d world_pos_bottom = backproject_to_depth_map
    (camera, depth_map, img_pt_bottom);

  vector_2d npt_ = camera->intrinsics()->unmap(img_pt_top);
  auto npt = vector_3d(npt_(0), npt_(1), 1.0);

  matrix_3x3d M = camera->rotation().matrix().transpose();
  vector_3d cam_pos = camera->center();

  vector_3d Mp = M * npt;

  double xf = world_pos_bottom(0);
  double yf = world_pos_bottom(1);

  double xc = cam_pos(0);
  double yc = cam_pos(1);
  double zc = cam_pos(2);

  double nx = Mp(0);
  double ny = Mp(1);
  double nz = Mp(2);

  // If we assume that the top world point for given pair is directly
  // above the bottom world point at (xf, yf, zf), then the top world
  // point is at (xf, yf, zh), where we need to solve for zh.  The
  // camera is located at (xc, yc, zc) and the ray coming out at the
  // top image point is along the direction (in world coordinates)
  // <nx, ny, nz>, but we don't know how far along this ray's
  // direction we need to go to be as close as possible to (xf, yf,
  // zf). If we travel 't' along the ray, then the squared distance
  // from (xf, yf) is (xc+nx*t - xf)^2 + (yc+ny*t - yf)^2, and we want
  // 't' that minimizes this. Take the derivative wrt 't' and set
  // equal to zero:
  // 2 * (xc + nx * t - xf) * nx + 2 * (yc + ny * t - yf) * ny = 0
  // Rearranged as:
  double t = (ny * (yf - yc) + nx * (xf - xc)) /
    (std::pow(nx, 2) + std::pow(ny, 2));

  double zh = zc + t*nz;

  auto world_pos_top = vector_3d(xf, yf, zh);

  return std::tuple<vector_3d, vector_3d> (world_pos_bottom, world_pos_top);
}

// ---------------------------------------------------------------------------
vital::bounding_box<double>
transfer_bbox_with_depth_map_stationary_camera
(kwiver::vital::camera_perspective_sptr const src_camera,
 kwiver::vital::camera_perspective_sptr const dest_camera,
 kwiver::vital::image_container_sptr const depth_map,
 vital::bounding_box<double> const bbox)
{
  double bbox_min_x = bbox.min_x();
  double bbox_max_x = bbox.max_x();
  double bbox_min_y = bbox.min_y();
  double bbox_max_y = bbox.max_y();
  double bbox_aspect_ratio = (bbox_max_x - bbox_min_x) /
    (bbox_max_y - bbox_min_y);

  auto bbox_bottom_center = vector_2d
    ((bbox_max_x + bbox_min_x) / 2, bbox_max_y);
  auto bbox_top_center = vector_2d
    ((bbox_max_x + bbox_min_x) / 2, bbox_min_y);

  vector_3d world_pos_bottom;
  vector_3d world_pos_top;

  std::tie(world_pos_bottom, world_pos_top) =
    backproject_wrt_height
    (src_camera, depth_map, bbox_bottom_center, bbox_top_center);

  vector_2d dest_img_pos_bottom = dest_camera->project(world_pos_bottom);
  vector_2d dest_img_pos_top = dest_camera->project(world_pos_top);

  double dest_bbox_min_y = dest_img_pos_top(1);
  double dest_bbox_max_y = dest_img_pos_bottom(1);
  double dest_bbox_height = dest_bbox_max_y - dest_bbox_min_y;

  // Using the original bbox aspect ratio to compute the width of our
  // transferred box.  Could use a more sophisticated method here
  double dest_bbox_width_d = bbox_aspect_ratio * dest_bbox_height;

  // Use the average center x coordinate of transfered top and bottom
  // points as the center of the bounding box
  double dest_bbox_min_x =
    ((dest_img_pos_top(0) + dest_img_pos_bottom(0)) / 2) -
    (dest_bbox_width_d / 2);
  double dest_bbox_max_x =
    ((dest_img_pos_top(0) + dest_img_pos_bottom(0)) / 2) +
    (dest_bbox_width_d / 2);

  return vital::bounding_box<double>
    (dest_bbox_min_x, dest_bbox_min_y, dest_bbox_max_x, dest_bbox_max_y);
}

// ---------------------------------------------------------------------------
transfer_bbox_with_depth_map::
transfer_bbox_with_depth_map()
{
}

// ---------------------------------------------------------------------------
transfer_bbox_with_depth_map::
transfer_bbox_with_depth_map
(kwiver::vital::camera_perspective_sptr src_cam,
 kwiver::vital::camera_perspective_sptr dest_cam,
 kwiver::vital::image_container_sptr src_cam_depth_map)
  : src_camera( src_cam )
  , dest_camera( dest_cam )
  , depth_map( src_cam_depth_map )
{
}

// ---------------------------------------------------------------------------
vital::config_block_sptr
transfer_bbox_with_depth_map::
get_configuration() const
{
  // Get base config from base class
  vital::config_block_sptr config = vital::algorithm::get_configuration();

  config->set_value( "src_camera_krtd_file_name", src_camera_krtd_file_name,
                     "Source camera KRTD file name path" );

  config->set_value( "dest_camera_krtd_file_name", dest_camera_krtd_file_name,
                     "Destination camera KRTD file name path" );

  config->set_value( "src_camera_depth_map_file_name",
                     src_camera_depth_map_file_name,
                     "Source camera depth map file name path" );

  vital::algo::image_io::
    get_nested_algo_configuration( "image_reader", config, image_reader );

  return config;
}

// ---------------------------------------------------------------------------
void
transfer_bbox_with_depth_map::
set_configuration( vital::config_block_sptr config_in )
{
  vital::config_block_sptr config = this->get_configuration();

  config->merge_config( config_in );
  this->src_camera_krtd_file_name =
    config->get_value< std::string > ( "src_camera_krtd_file_name" );
  this->dest_camera_krtd_file_name =
    config->get_value< std::string > ( "dest_camera_krtd_file_name" );
  this->src_camera_depth_map_file_name =
    config->get_value< std::string > ( "src_camera_depth_map_file_name" );

  // Setup actual reader algorithm
  vital::algo::image_io::
    set_nested_algo_configuration( "image_reader", config, image_reader );

  this->src_camera =
    kwiver::vital::read_krtd_file( this->src_camera_krtd_file_name );
  this->dest_camera =
    kwiver::vital::read_krtd_file( this->dest_camera_krtd_file_name );

  this->depth_map = image_reader->load( this->src_camera_depth_map_file_name );
}

// ---------------------------------------------------------------------------
bool
transfer_bbox_with_depth_map::
check_configuration( vital::config_block_sptr config ) const
{
  kwiver::vital::config_difference cd( this->get_configuration(), config );
  const auto key_list = cd.extra_keys();

  if ( ! key_list.empty() )
  {
    LOG_WARN( logger(), "Additional parameters found in config block that are "
                        "not required or desired: "
                        << kwiver::vital::join( key_list, ", " ) );
  }

  return true;
}

// ---------------------------------------------------------------------------
vital::detected_object_set_sptr
transfer_bbox_with_depth_map::
filter( vital::detected_object_set_sptr const input_set ) const
{
  auto ret_set = std::make_shared<vital::detected_object_set>();

  for ( auto det : *input_set )
  {
    auto out_det = det->clone();
    auto out_bbox = out_det->bounding_box();

    try
    {
      vital::bounding_box<double> new_out_bbox =
        transfer_bbox_with_depth_map_stationary_camera
        (src_camera, dest_camera, depth_map, out_bbox);
      out_det->set_bounding_box( new_out_bbox );
      ret_set->add( out_det );
    }
    catch (const std::invalid_argument& e)
    {
      std::ostringstream strs;
      strs << "Bounding box ("
           << out_bbox.min_x() << ", "
           << out_bbox.min_y() << ", "
           << out_bbox.max_x() << ", "
           << out_bbox.max_y() << ") "
           << "couldn't be transferred, skipping!";

      LOG_WARN(logger(), strs.str());
    }
  }

  return ret_set;
}

}}} // end namespace