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

// Copyright 2012 - Gonzalo Iglesias, Adrià de Gispert, William Byrne

#include <main.countstrings.hpp>
#include <main.custom_assert.hpp>
#include <main.logger.hpp>
#include <boost/multiprecision/cpp_int.hpp>

//assumes topsorted fst
template<class Arc ,
         typename IntegerT >
IntegerT countstrings (fst::VectorFst<Arc>& myfst) {
  std::vector<IntegerT> counts;
  IntegerT cumcounts = 0;
  counts.assign (myfst.NumStates(), 0);
  counts[0] = 1;
  typedef typename Arc::StateId StateId;
  for (fst::StateIterator< fst::VectorFst<Arc> > si (myfst);
       !si.Done();
       si.Next() ) {
    typename Arc::StateId state_id = si.Value();
    if (myfst.Final (state_id) != Arc::Weight::Zero() ) {
      cumcounts += counts[state_id];
    }
    for (fst::MutableArcIterator< fst::MutableFst<Arc> > ai (&myfst, si.Value() );
         !ai.Done();
         ai.Next() ) {
      Arc arc = ai.Value();
      counts[arc.nextstate] += counts[state_id];
    }
  }
  return cumcounts;
};

template<class Arc>
void run(ucam::util::RegistryPO const &rg) {

  ucam::util::PatternAddress<unsigned> pi (rg.get<std::string>
      (HifstConstants::kInput) );
  ucam::util::PatternAddress<unsigned> po (rg.get<std::string>
      (HifstConstants::kOutput) );
  for ( ucam::util::IntRangePtr ir (ucam::util::IntRangeFactory ( rg,
                                    HifstConstants::kRangeOne ) );
        !ir->done();
        ir->next() ) {
    fst::VectorFst<Arc> *mfst = fst::VectorFstRead<Arc> (pi (
                                          ir->get() ) );
    if (!mfst) {
      LERROR("Could not read file:" << ir->get());
      exit(EXIT_FAILURE);
    }
    TopSort (mfst);
    boost::multiprecision::uint128_t j =
      countstrings<Arc, boost::multiprecision::uint128_t> (*mfst);
    std::stringstream ss;
    ss << j;
    ucam::util::oszfstream o (po (ir->get() ), true);
    o << ss.str()  << std::endl;
    LINFO ( pi (ir->get() ) << ":" << ss.str() ) ;
    o.close();
    delete mfst;
  }
}

int main (int argc,  const char* argv[] ) {
  ucam::util::initLogger ( argc, argv );
  FORCELINFO ( argv[0] << " starts!" );
  ucam::util::RegistryPO rg ( argc, argv );
  FORCELINFO ( rg.dump ( "CONFIG parameters:\n=====================",
                         "=====================" ) )  ;

  std::string semiring = rg.get<std::string> (HifstConstants::kHifstSemiring);
  if (semiring == HifstConstants::kHifstSemiringStdArc) {
    run<fst::StdArc> (rg);
  } else if (semiring == HifstConstants::kHifstSemiringLexStdArc) {
    run<fst::LexStdArc> (rg);
  } else if (semiring == HifstConstants::kHifstSemiringTupleArc) {
    run<TupleArc32> (rg);
  } else {
    LERROR ("Sorry, semiring option not correctly defined");
  }
  FORCELINFO ( argv[0] << " ends!" );
}


/*====================================================================
   Simple pendulum example
   Copyright (c) 2017 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
   Licensed under the zlib license. See LICENSE for more details.
 *///=================================================================

#include <string>
#include <iostream>
#include <stdio.h>
#include <rbdl/rbdl.h>
#include "csvtools.h"

#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
//using namespace std;
using namespace boost::numeric::odeint;

#ifndef RBDL_BUILD_ADDON_LUAMODEL
    #error "Error: RBDL addon LuaModel not enabled."
#endif

#include <rbdl/addons/luamodel/luamodel.h>

using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;

//====================================================================
// Boost stuff
//====================================================================

typedef std::vector< double > state_type;

typedef runge_kutta_cash_karp54< state_type > error_stepper_type;
typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;

class rbdlToBoost {

    public:
        rbdlToBoost(Model* model) : model(model) {
            q = VectorNd::Zero(model->dof_count);
            qd = VectorNd::Zero(model->dof_count);
            qdd = VectorNd::Zero(model->dof_count);
            tau = VectorNd::Zero(model->dof_count);

        }

        //3c. Boost uses this 'operator()' function to evaluate the state
        //    derivative of the pendulum.
        void operator() (const state_type &x,
                         state_type &dxdt,
                         const double t){

            //3d. Here we split out q (generalized positions) and qd
            //    (generalized velocities) from the x (state vector)
            //q
            int j = 0;
            for(int i=0; i<model->dof_count; i++){
                q[i] = (double)x[j];
                j++;
            }

            //qd
            for(int i=0; i<model->dof_count; i++){
                qd[i] = (double)x[j];
                j++;
            }

            //3e. Here we set the applied generalized forces to zero
            for(int i=0; i<model->dof_count; i++){
                tau[i] = 0;
            }

            //3f. RBDL's ForwardDynamics function is used to evaluate
            //    qdd (generalized accelerations)
            ForwardDynamics (*model,q,qd,tau,qdd);

            //3g. Here qd, and qdd are used to populate dxdt
            //(the state derivative)
            j = 0;
            for(int i = 0; i < model->dof_count; i++){
                dxdt[j] = (double)qd[i];
                j++;
            }
            for(int i = 0; i < model->dof_count; i++){
                dxdt[j] = (double)qdd[i];
                j++;
            }

        }

    private:
        Model* model;
        VectorNd q, qd, qdd, tau;
};

struct pushBackStateAndTime
{
    std::vector< state_type >& states;
    std::vector< double >& times;

    pushBackStateAndTime( std::vector< state_type > &states ,
                              std::vector< double > &times )
    : states( states ) , times( times ) { }

    void operator()( const state_type &x , double t )
    {
        states.push_back( x );
        times.push_back( t );
    }
};

void f(const state_type &x, state_type &dxdt, const double t);

/* Problem Constants */
int main (int argc, char* argv[]) {
    rbdl_check_api_version (RBDL_API_VERSION);

    //problem specific constants
    int     nPts    = 100;
    double  t0      = 0;
    double  t1      = 3;

    //Integration settings
    double absTolVal   = 1e-10;
    double relTolVal   = 1e-6;

    VectorNd q, qd;

    Model* model  = NULL;
    model         = new Model();

    //3a. The Lua model is read in here, and turned into a series of
    //    vectors and matricies in model which RBDL uses to evaluate
    //    dynamics quantities
    if (!Addons::LuaModelReadFromFile ("./../model/pendulum.lua",
                                       model, false)             ){
        std::cerr     << "Error loading model ./model/pendulum.lua"
                    << std::endl;
        abort();
    }

    q       = VectorNd::Zero (model->dof_count);
    qd      = VectorNd::Zero (model->dof_count);

    double t        = 0;             //time
    double ts       = 0;            //scaled time
    double dtsdt    = M_PI/(t1-t0);    //dertivative scaled time
                                    //w.r.t. time

    printf("DoF: %i\n",model->dof_count);

    printf("Forward Dynamics \n");

    //3b. Here we instantiate a wrapper class which is needed so that
    //    Boost can evaluate the state derivative of the model.
    rbdlToBoost rbdlModel(model);
    state_type xState(2);
    int steps = 0;
    xState[0] = -M_PI/4.0;
    xState[1] = 0;

    double dt   = (t1-t0)/((double)nPts);

    double ke, pe = 0;

    std::vector<std::vector< double > > matrixData;
    std::vector<std::vector< double > > matrixErrorData;
    std::vector< double > rowData(model->dof_count+1);
    std::vector< double > rowErrorData(2);

    double a_x = 1.0 , a_dxdt = 1.0;
    controlled_stepper_type
    controlled_stepper(
        default_error_checker< double ,
                               range_algebra ,
                               default_operations >
        ( absTolVal , relTolVal , a_x , a_dxdt ) );

    double tp = 0;
    rowData[0] = 0;
    for(int z=0; z < model->dof_count; z++){
        rowData[z+1] = xState[model->dof_count + z];
    }
    matrixData.push_back(rowData);

    double kepe0 = 0;

    printf("Columns\n");
    printf("      t,         q,       qd,       ke,        pe,   ke+pe-(kepe0)\n");
    for(int i = 0; i <= nPts; i++){

        t = t0 + dt*i;

        //3h. Here we integrate forward in time between a series of nPts from
        //    t0 to t1
        integrate_adaptive(
            controlled_stepper ,
            rbdlModel , xState , tp , t , (t-tp)/10 );
        tp = t;

        //3i. At each point the state, kinetic (ke), and potential energy (pe)
        //    is evaluated. In this conservative system the sum of kinetic and
        //    potential energy should be constant. Any error that accumulates
        //    is due to the cumulation of integration error.

        q[0]  = xState[0];
        qd[0] = xState[1];

        pe = Utils::CalcPotentialEnergy(*model, q, true);
        ke = Utils::CalcKineticEnergy(*model, q, qd, true);

        printf("%f, %f, %f, %f, %f, %f\n",
                    t, q[0],qd[0],ke,
                    pe,(ke+pe-kepe0));

        rowData[0] = t;
        for(int z=0; z < model->dof_count; z++){
            rowData[z+1] = xState[z];
        }
        matrixData.push_back(rowData);

        if(i==0) kepe0 = (ke+pe);

        rowErrorData[0] = t;
        rowErrorData[1] = (ke + pe) - kepe0;
        matrixErrorData.push_back(rowErrorData);
    }
    printf("Columns\n");
    printf("      t,         q,       qd,       ke,        pe,      ke+pe-(kepe0)\n");

    //3j. Now the data we have accumulated is written to file
    std::string header = "";
    std::string fname   = "../output/meshup.csv";
    printMatrixToFile(matrixData, header, fname);
    printf("Wrote: ../output/meshup.csv (meshup animation file)\n");

    fname               = "../output/kepe.csv";
    header = "time,systemEnergy,";
    printMatrixToFile(matrixErrorData,header,fname);
    printf("Wrote: ../output/kepe.csv (simulation data)\n");
    delete model;

    return 0;

}


/*
Copyright (C) 2012 Mathias Eitz and Ronald Richter.
All rights reserved.

This file is part of the imdb library and is made available under
the terms of the BSD license (see the LICENSE file).
*/

#include "image_sampler.hpp"

#include <ctime>
#include <algorithm>

#include <boost/random.hpp>

namespace imdb {

void grid_sampler::setParameters(ptree &params)
{
    _numSamples = parse<uint>(params, "num_samples", 625);
}

// the resulting samples contain (x,y) coordinates within the sampling area
void grid_sampler::sample(vec_vec_f32_t& samples, const cv::Mat& image) const
{
    cv::Rect samplingArea(0, 0, image.size().width, image.size().height);

    uint numSamples1D = std::ceil(std::sqrt(static_cast<float>(_numSamples)));
    float stepX = samplingArea.width / static_cast<float>(numSamples1D+1);
    float stepY = samplingArea.height / static_cast<float>(numSamples1D+1);

    for (uint x = 1; x <= numSamples1D; x++) {
        uint posX = x*stepX;
        for (uint y = 1; y <= numSamples1D; y++) {
            uint posY = y*stepY;
            vec_f32_t p(2);
            p[0] = posX;
            p[1] = posY;
            samples.push_back(p);
        }
    }
}

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

void random_area_sampler::setParameters(ptree &params)
{
    _numSamples = parse<uint>(params, "num_samples", 500);
}

void random_area_sampler::sample(vec_vec_f32_t& samples, const cv::Mat& image) const
{
    // prevent logical error
    assert(samples.size() == 0);

    cv::Rect samplingArea(0, 0, image.size().width, image.size().height);

    int w = samplingArea.width;
    int h = samplingArea.height;

    // TODO: move rng and seed to constructor???
    boost::mt19937 rng;
    rng.seed(static_cast<unsigned int>(std::time(0)));

    boost::variate_generator<boost::mt19937&, boost::uniform_int<> > x_generator(rng, boost::uniform_int<>(0,w-1));
    boost::variate_generator<boost::mt19937&, boost::uniform_int<> > y_generator(rng, boost::uniform_int<>(0,h-1));

    vec_f32_t p(2);
    for (uint i = 0; i < _numSamples; i++)
    {
        p[0] = x_generator();
        p[1] = y_generator();
        samples.push_back(p);
    }
}

bool gridsampler_registered = ImageSampler::register_sampler<grid_sampler>("grid");
bool randomsampler_registered = ImageSampler::register_sampler<random_area_sampler>("random_area");

} // end namespace


//-----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax output
#include <boost/test/unit_test.hpp>

#include <mitrax/matrix.hpp>
#include <mitrax/io.hpp>

#include <sstream>

using boost::typeindex::type_id;
using boost::typeindex::type_id_runtime;
using namespace mitrax;
using namespace mitrax::literals;

constexpr int m33[3][3] = {
	{1, 2, 3},
	{4, 5, 6},
	{7, 8, 9}
};

constexpr char const* m33_ref = "[3,3]((1,2,3),(4,5,6),(7,8,9))";

constexpr int m23[3][2] = {
	{1, 2},
	{4, 5},
	{7, 8}
};

constexpr char const* m23_ref = "[2,3]((1,2),(4,5),(7,8))";

constexpr int m32[2][3] = {
	{1, 2, 3},
	{4, 5, 6}
};

constexpr char const* m32_ref = "[3,2]((1,2,3),(4,5,6))";

template < typename T >
std::string out(T const& m){
	std::ostringstream os;
	os << m;
	return os.str();
}

BOOST_AUTO_TEST_SUITE(suite_output)

BOOST_AUTO_TEST_CASE(test_output_3x3){
	auto m1 = make_matrix(3_CS, 3_RS, m33);
	auto m2 = make_matrix(3_CD, 3_RS, m33);
	auto m3 = make_matrix(3_CS, 3_RD, m33);
	auto m4 = make_matrix(3_CD, 3_RD, m33);

	BOOST_TEST(out(m1) == m33_ref);
	BOOST_TEST(out(m2) == m33_ref);
	BOOST_TEST(out(m3) == m33_ref);
	BOOST_TEST(out(m4) == m33_ref);
}

BOOST_AUTO_TEST_CASE(test_output_2x3){
	auto m1 = make_matrix(2_CS, 3_RS, m23);
	auto m2 = make_matrix(2_CD, 3_RS, m23);
	auto m3 = make_matrix(2_CS, 3_RD, m23);
	auto m4 = make_matrix(2_CD, 3_RD, m23);

	BOOST_TEST(out(m1) == m23_ref);
	BOOST_TEST(out(m2) == m23_ref);
	BOOST_TEST(out(m3) == m23_ref);
	BOOST_TEST(out(m4) == m23_ref);
}

BOOST_AUTO_TEST_CASE(test_output_3x2){
	auto m1 = make_matrix(3_CS, 2_RS, m32);
	auto m2 = make_matrix(3_CD, 2_RS, m32);
	auto m3 = make_matrix(3_CS, 2_RD, m32);
	auto m4 = make_matrix(3_CD, 2_RD, m32);

	BOOST_TEST(out(m1) == m32_ref);
	BOOST_TEST(out(m2) == m32_ref);
	BOOST_TEST(out(m3) == m32_ref);
	BOOST_TEST(out(m4) == m32_ref);
}

BOOST_AUTO_TEST_SUITE_END()


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, logical_negation) {
  using stan::math::logical_negation;
  EXPECT_TRUE(logical_negation(0));
  EXPECT_TRUE(logical_negation(0.0));
  EXPECT_TRUE(logical_negation(0.0f));

  EXPECT_FALSE(logical_negation(1));
  EXPECT_FALSE(logical_negation(2.0));
  EXPECT_FALSE(logical_negation(2.0f));
}

TEST(MathFunctions, logical_negation_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_FALSE(stan::math::logical_negation(nan));
}


#ifndef CIRCLE_DETECTION_INCLUDE_GAURD
#define CIRCLE_DETECTION_INCLUDE_GAURD
#include <Eigen/Dense>
#include <vector>

using std::vector;
using Eigen::Vector3d;

namespace circleDetection
{
struct Point2d
{
  Point2d(double x, double y);
  double x, y;
};

Vector3d fitCircle(vector<Point2d> circlePoints);
double calculateError(const vector<Point2d>& observedPoints, Vector3d circleCoeff);
}

#endif


/*
@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/config.hpp>
#include <boost/hana/ext/std/array.hpp>
#include <boost/hana/ext/std/tuple.hpp>
#include <boost/hana/foldable.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/logical.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/sequence.hpp>
#include <boost/hana/tuple.hpp>

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

int main() {

{

//! [make<Tuple>]
auto xs = make<Tuple>(1, 2, '3', std::string{"456"});

constexpr auto ys = make<Tuple>(1, '2', 3.456);
//! [make<Tuple>]
(void)xs; (void)ys;

}{

//! [tuple_t]
BOOST_HANA_CONSTANT_CHECK(
    to<Tuple>(tuple_t<int, char, void, int(float)>)
    ==
    make<Tuple>(type<int>, type<char>, type<void>, type<int(float)>)
);
//! [tuple_t]

}{

//! [tuple_c]
BOOST_HANA_CONSTANT_CHECK(
    to<Tuple>(tuple_c<int, 0, 1, 2>)
    ==
    make<Tuple>(int_<0>, int_<1>, int_<2>)
);
BOOST_HANA_CONSTANT_CHECK(head(tuple_c<int, 0, 1, 2>) == int_<0>);
//! [tuple_c]

}{

//! [Foldable]
using namespace literals;

constexpr auto numbers = tuple_c<int, 5, -1, 0, -7, -2, 0, -5, 4>;
constexpr auto negatives = tuple_c<int, -1, -7, -2, -5>;

BOOST_HANA_CONSTEXPR_LAMBDA auto keep_negatives = [](auto n, auto acc) {
    return if_(n < 0_c, prepend(n, acc), acc);
};

BOOST_HANA_CONSTANT_CHECK(foldr(numbers, tuple_c<int>, keep_negatives) == negatives);
//! [Foldable]

}{

//! [Tuple_interop]
BOOST_HANA_CONSTEXPR_CHECK(
    to<Tuple>(std::make_tuple(1, '2', 3.3)) == make<Tuple>(1, '2', 3.3)
);

BOOST_HANA_CONSTANT_CHECK(
    to<Tuple>(range(int_<1>, int_<4>)) == make<Tuple>(int_<1>, int_<2>, int_<3>)
);

BOOST_HANA_CONSTEXPR_CHECK(
    to<Tuple>(std::array<int, 3>{{1, 2, 3}}) == make<Tuple>(1, 2, 3)
);
//! [Tuple_interop]

}

}


// Boost.Geometry
// Unit Test

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

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

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/equals.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/segment.hpp>

#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/io/wkt/write.hpp>

#include <boost/geometry/policies/relate/direction.hpp>
#include <boost/geometry/policies/relate/intersection_points.hpp>
#include <boost/geometry/policies/relate/tupled.hpp>

#include <boost/geometry/strategies/spherical/intersection.hpp>

template <typename P1, typename P2, typename T>
bool equals_relaxed(P1 const& p1, P2 const& p2, T const& eps_scale)
{
    typedef typename bg::select_coordinate_type<P1, P2>::type calc_t;
    calc_t c1 = 1;
    calc_t p10 = bg::get<0>(p1);
    calc_t p11 = bg::get<1>(p1);
    calc_t p20 = bg::get<0>(p2);
    calc_t p21 = bg::get<1>(p2);
    calc_t relaxed_eps = std::numeric_limits<calc_t>::epsilon()
        * bg::math::detail::greatest(c1, bg::math::abs(p10), bg::math::abs(p11), bg::math::abs(p20), bg::math::abs(p21))
        * eps_scale;
    calc_t diff0 = p10 - p20;
    // needed e.g. for -179.999999 - 180.0
    if (diff0 < -180)
        diff0 += 360;
    return bg::math::abs(diff0) <= relaxed_eps
        && bg::math::abs(p11 - p21) <= relaxed_eps;
}

template <typename S1, typename S2, typename P>
void test_spherical_strategy_one(S1 const& s1, S2 const& s2, char m, std::size_t expected_count, P const& ip0 = P(), P const& ip1 = P())
{
    typedef typename bg::coordinate_type<P>::type coord_t;
    typedef bg::policies::relate::segments_tupled
                <
                    bg::policies::relate::segments_intersection_points
                        <
                            bg::segment_intersection_points<P, bg::segment_ratio<coord_t> >
                        >,
                    bg::policies::relate::segments_direction
                > policy_t;
    typedef bg::strategy::intersection::relate_spherical_segments
        <
            policy_t
        > strategy_t;
    typedef typename strategy_t::return_type return_type;

    // NOTE: robust policy is currently ignored
    return_type res = strategy_t::apply(s1, s2, 0);

    size_t const res_count = boost::get<0>(res).count;
    char const res_method = boost::get<1>(res).how;

    BOOST_CHECK_MESSAGE(res_method == m,
                        "IP method: " << res_method << " different than expected: " << m
                            << " for " << bg::wkt(s1) << " and " << bg::wkt(s2));

    BOOST_CHECK_MESSAGE(res_count == expected_count,
                        "IP count: " << res_count << " different than expected: " << expected_count
                            << " for " << bg::wkt(s1) << " and " << bg::wkt(s2));

    // The EPS is scaled because during the conversion various angles may be not converted
    // to cartesian 3d the same way which results in a different intersection point
    // For more information read the info in spherical intersection strategy file.

    int eps_scale = res_method != 'i' ? 1 : 1000;

    if (res_count > 0 && expected_count > 0)
    {
        P const& res_i0 = boost::get<0>(res).intersections[0];
        BOOST_CHECK_MESSAGE(equals_relaxed(res_i0, ip0, eps_scale),
                            "IP0: " << std::setprecision(16) << bg::wkt(res_i0) << " different than expected: " << bg::wkt(ip0)
                                << " for " << bg::wkt(s1) << " and " << bg::wkt(s2));
    }
    if (res_count > 1 && expected_count > 1)
    {
        P const& res_i1 = boost::get<0>(res).intersections[1];
        BOOST_CHECK_MESSAGE(equals_relaxed(res_i1, ip1, eps_scale),
                            "IP1: " << std::setprecision(16) << bg::wkt(res_i1) << " different than expected: " << bg::wkt(ip1)
                                << " for " << bg::wkt(s1) << " and " << bg::wkt(s2));
    }
}

template <typename T>
T translated(T v, double t)
{
    v += T(t);
    if (v > 180)
        v -= 360;
    return v;
}

template <typename S1, typename S2, typename P>
void test_spherical_strategy(S1 const& s1, S2 const& s2, char m, std::size_t expected_count, P const& ip0 = P(), P const& ip1 = P())
{
    S1 s1t = s1;
    S2 s2t = s2;
    P ip0t = ip0;
    P ip1t = ip1;

    double t = 0;
    for (int i = 0; i < 4; ++i, t += 90)
    {
        bg::set<0, 0>(s1t, translated(bg::get<0, 0>(s1), t));
        bg::set<1, 0>(s1t, translated(bg::get<1, 0>(s1), t));
        bg::set<0, 0>(s2t, translated(bg::get<0, 0>(s2), t));
        bg::set<1, 0>(s2t, translated(bg::get<1, 0>(s2), t));
        if (expected_count > 0)
            bg::set<0>(ip0t, translated(bg::get<0>(ip0), t));
        if (expected_count > 1)
            bg::set<0>(ip1t, translated(bg::get<0>(ip1), t));

        test_spherical_strategy_one(s1t, s2t, m, expected_count, ip0t, ip1t);
    }
}

template <typename S1, typename S2, typename P>
void test_spherical_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                             char m, std::size_t expected_count,
                             std::string const& ip0_wkt = "", std::string const& ip1_wkt = "")
{
    S1 s1;
    S2 s2;
    P ip0, ip1;

    bg::read_wkt(s1_wkt, s1);
    bg::read_wkt(s2_wkt, s2);
    if (! ip0_wkt.empty())
        bg::read_wkt(ip0_wkt, ip0);
    if (!ip1_wkt.empty())
        bg::read_wkt(ip1_wkt, ip1);

    test_spherical_strategy(s1, s2, m, expected_count, ip0, ip1);
}

template <typename S, typename P>
void test_spherical_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                             char m, std::size_t expected_count,
                             std::string const& ip0_wkt = "", std::string const& ip1_wkt = "")
{
    test_spherical_strategy<S, S, P>(s1_wkt, s2_wkt, m, expected_count, ip0_wkt, ip1_wkt);
}

template <typename T>
void test_spherical()
{
    typedef bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > point_t;
    typedef bg::model::segment<point_t> segment_t;

    // crossing   X
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-45 -45, 45 45)", "SEGMENT(-45 45, 45 -45)", 'i', 1, "POINT(0 0)");
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-45 -45, 45 45)", "SEGMENT(45 -45, -45 45)", 'i', 1, "POINT(0 0)");
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(45 45, -45 -45)", "SEGMENT(-45 45, 45 -45)", 'i', 1, "POINT(0 0)");
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(45 45, -45 -45)", "SEGMENT(45 -45, -45 45)", 'i', 1, "POINT(0 0)");
    // crossing   X
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-1 1, 1 -1)", 'i', 1, "POINT(0 0)");
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(1 -1, -1 1)", 'i', 1, "POINT(0 0)");
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(1 1, -1 -1)", "SEGMENT(-1 1, 1 -1)", 'i', 1, "POINT(0 0)");
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(1 1, -1 -1)", "SEGMENT(1 -1, -1 1)", 'i', 1, "POINT(0 0)");

    // equal
    //   //
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-45 -45, 45 45)", "SEGMENT(-45 -45, 45 45)", 'e', 2, "POINT(-45 -45)", "POINT(45 45)");
    //   //
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-45 -45, 45 45)", "SEGMENT(45 45, -45 -45)", 'e', 2, "POINT(-45 -45)", "POINT(45 45)");

    // starting outside s1
    //    /
    //   |
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-2 -2, -1 -1)", 'a', 1, "POINT(-1 -1)");
    //   /
    //  /|
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-2 -2, 0 0)", 'm', 1, "POINT(0 0)");
    //   /|
    //  / |
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-2 -2, 1 1)", 't', 1, "POINT(1 1)");
    //   |/
    //  /|
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-2 -2, 2 2)", 'i', 1, "POINT(0 0)");
    //       ------
    // ------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-2 0, -1 0)", 'a', 1, "POINT(-1 0)");
    //    ------
    // ------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-2 0, 0 0)", 'c', 2, "POINT(-1 0)", "POINT(0 0)");
    //    ------
    // ---------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-2 0, 1 0)", 'c', 2, "POINT(-1 0)", "POINT(1 0)");
    //    ------
    // ------------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-2 0, 2 0)", 'c', 2, "POINT(-1 0)", "POINT(1 0)");

    // starting at s1
    //  /
    // //
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-1 -1, 0 0)", 'c', 2, "POINT(-1 -1)", "POINT(0 0)");
    //  //
    // //
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-1 -1, 1 1)", 'e', 2, "POINT(-1 -1)", "POINT(1 1)");
    // | /
    // |/
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-1 -1, 2 2)", 'f', 1, "POINT(-1 -1)");
    // ------
    // ---
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-1 0, 0 0)", 'c', 2, "POINT(-1 0)", "POINT(0 0)");
    // ------
    // ------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-1 0, 1 0)", 'e', 2, "POINT(-1 0)", "POINT(1 0)");
    // ------
    // ---------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-1 0, 2 0)", 'c', 2, "POINT(-1 0)", "POINT(1 0)");

    // starting inside
    //   //
    //  /
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(0 0, 1 1)", 'c', 2, "POINT(0 0)", "POINT(1 1)");
    //   |/
    //   /
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(0 0, 2 2)", 's', 1, "POINT(0 0)");
    // ------
    //    ---
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(0 0, 1 0)", 'c', 2, "POINT(0 0)", "POINT(1 0)");
    // ------
    //    ------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(0 0, 2 0)", 'c', 2, "POINT(0 0)", "POINT(1 0)");

    // starting at p2
    //    |
    //   /
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(1 1, 2 2)", 'a', 1, "POINT(1 1)");
    // ------
    //       ---
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(1 0, 2 0)", 'a', 1, "POINT(1 0)");

    // disjoint, crossing
    //     /
    //  |
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-3 -3, -2 -2)", 'd', 0);
    //     |
    //  /
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(2 2, 3 3)", 'd', 0);
    // disjoint, collinear
    //          ------
    // ------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-3 0, -2 0)", 'd', 0);
    // ------
    //           ------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(2 0, 3 0)", 'd', 0);

    // degenerated
    //    /
    // *
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-2 -2, -2 -2)", 'd', 0);
    //    /
    //   *
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(-1 -1, -1 -1)", '0', 1, "POINT(-1 -1)");
    //    /
    //   *
    //  /
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(0 0, 0 0)", '0', 1, "POINT(0 0)");
    //    *
    //   /
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(1 1, 1 1)", '0', 1, "POINT(1 1)");
    //       *
    //   /
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 -1, 1 1)", "SEGMENT(2 2, 2 2)", 'd', 0);
    // similar to above, collinear
    // *   ------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-2 0, -2 0)", 'd', 0);
    //    *------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(-1 0, -1 0)", '0', 1, "POINT(-1 0)");
    //    ---*---
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(0 0, 0 0)", '0', 1, "POINT(0 0)");
    //    ------*
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(1 0, 1 0)", '0', 1, "POINT(1 0)");
    //    ------   *
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 0, 1 0)", "SEGMENT(2 0, 2 0)", 'd', 0);

    // Northern hemisphere
    // ---   ------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(-3 50, -2 50)", 'd', 0);
    //    ------
    // ---
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(-2 50, -1 50)", 'a', 1, "POINT(-1 50)");
    //  \/
    //  /\                   (avoid multi-line comment)
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(-2 50, 0 50)", 'i', 1, "POINT(-0.5 50.0032229484023)");
    //  ________
    // /   _____\            (avoid multi-line comment)
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(-2 50, 1 50)", 't', 1, "POINT(1 50)");
    //  _________
    // /  _____  \           (avoid multi-line comment)
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(-2 50, 2 50)", 'd', 0);
    //  ______
    // /___   \              (avoid multi-line comment)
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(-1 50, 0 50)", 'f', 1, "POINT(-1 50)");
    // ------
    // ------
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(-1 50, 1 50)", 'e', 2, "POINT(-1 50)", "POINT(1 50)");
    //  ________
    // /_____   \            (avoid multi-line comment)
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(-1 50, 2 50)", 'f', 1, "POINT(-1 50)");
    //  ______
    // /   ___\              (avoid multi-line comment)
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(0 50, 1 50)", 't', 1, "POINT(1 50)");
    //  \/
    //  /\                   (avoid multi-line comment)
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(0 50, 2 50)", 'i', 1, "POINT(0.5 50.0032229484023)");
    // ------
    //       ---
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(1 50, 2 50)", 'a', 1, "POINT(1 50)");
    // ------   ---
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(-1 50, 1 50)", "SEGMENT(2 50, 3 50)", 'd', 0);

    // ___|
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(0 0, 1 0)", "SEGMENT(1 0, 1 1)", 'a', 1, "POINT(1 0)");
    // ___|
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(1 0, 1 1)", "SEGMENT(0 0, 1 0)", 'a', 1, "POINT(1 0)");

    //   |/
    //  /|
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(10 -1, 20 1)", "SEGMENT(12.5 -1, 12.5 1)", 'i', 1, "POINT(12.5 -0.50051443471392)");
    //   |/
    //  /|
    test_spherical_strategy<segment_t, point_t>(
        "SEGMENT(10 -1, 20 1)", "SEGMENT(17.5 -1, 17.5 1)", 'i', 1, "POINT(17.5 0.50051443471392)");
}

int test_main(int, char* [])
{
    //test_spherical<float>();
    test_spherical<double>();

    return 0;
}


//
// Copyright Vardan Akopian 2007
//
// 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)
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GBSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GBSV_HPP

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    namespace detail {
      inline
      void gbtrf (integer_t const n, integer_t const m, integer_t const kl, integer_t const ku,
                  float* ab, integer_t const ldab, integer_t* ipiv, integer_t* info)
      {
        LAPACK_SGBTRF (&n, &m, &kl, &ku, ab, &ldab, ipiv, info);
      }
      inline
      void gbtrf (integer_t const n, integer_t const m, integer_t const kl, integer_t const ku,
                  double* ab, integer_t const ldab, integer_t* ipiv, integer_t* info)
      {
        LAPACK_DGBTRF (&n, &m, &kl, &ku, ab, &ldab, ipiv, info);
      }
      inline
      void gbtrf (integer_t const n, integer_t const m, integer_t const kl, integer_t const ku,
                  traits::complex_f* ab, integer_t const ldab, integer_t* ipiv, integer_t* info)
      {
        LAPACK_CGBTRF (&n, &m, &kl, &ku, traits::complex_ptr(ab), &ldab, ipiv, info);
      }
      inline
      void gbtrf (integer_t const n, integer_t const m, integer_t const kl, integer_t const ku,
                  traits::complex_d* ab, integer_t const ldab, integer_t* ipiv, integer_t* info)
      {
        LAPACK_ZGBTRF (&n, &m, &kl, &ku, traits::complex_ptr(ab), &ldab, ipiv, info);
      }
    }

    template <typename MatrA, typename IVec>
    int gbtrf (MatrA& a, IVec& ipiv) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::banded_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::ordering_type,
        traits::column_major_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      integer_t const m = traits::matrix_size2 (a);
      assert (traits::vector_size (ipiv) == (m < n ? m : n));

      // if the matrix has kl lower and ku upper diagonals, then we should have
      // allocated kl lower and kl+ku upper diagonals
      integer_t const kl = traits::matrix_lower_bandwidth (a);
      integer_t const ku = traits::matrix_upper_bandwidth (a) - kl;
      integer_t const ld = traits::leading_dimension (a);

      assert(ku >= 0);

      integer_t info;
      detail::gbtrf (n, m, kl, ku,
                     traits::matrix_storage (a),
                     ld,
                     traits::vector_storage (ipiv),
                     &info);
      return info;
    }

    namespace detail {
      inline
      void gbtrs (char const trans, integer_t const n, integer_t const kl, integer_t const ku, integer_t const m,
                  float const* ab, integer_t const ldab, integer_t const* ipiv,
                  float* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_SGBTRS (&trans, &n, &kl, &ku, &m, ab, &ldab, ipiv, b, &ldb, info);
      }
      inline
      void gbtrs (char const trans, integer_t const n, integer_t const kl, integer_t const ku, integer_t const m,
                  double const* ab, integer_t const ldab, integer_t const* ipiv,
                  double* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_DGBTRS (&trans, &n, &kl, &ku, &m, ab, &ldab, ipiv, b, &ldb, info);
      }
      inline
      void gbtrs (char const trans, integer_t const n, integer_t const kl, integer_t const ku, integer_t const m,
                  traits::complex_f const* ab, integer_t const ldab, integer_t const* ipiv,
                  traits::complex_f* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_CGBTRS (&trans, &n, &kl, &ku, &m, traits::complex_ptr(ab), &ldab, ipiv, traits::complex_ptr(b), &ldb, info);
      }
      inline
      void gbtrs (char const trans, integer_t const n, integer_t const kl, integer_t const ku, integer_t const m,
                  traits::complex_d const* ab, integer_t const ldab, integer_t const* ipiv,
                  traits::complex_d* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_ZGBTRS (&trans, &n, &kl, &ku, &m, traits::complex_ptr(ab), &ldab, ipiv, traits::complex_ptr(b), &ldb, info);
      }
    }

    template <typename MatrA, typename MatrB, typename IVec>
    int gbtrs (char const trans, MatrA const& a, IVec const& ipiv, MatrB& b)
    {
      assert (trans == 'N' || trans == 'T' || trans == 'C');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::banded_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));
      assert (n == traits::vector_size (ipiv));

      // if the matrix has kl lower and ku upper diagonals, then we should have
      // allocated kl lower and kl+ku upper diagonals
      integer_t const kl = traits::matrix_lower_bandwidth (a);
      integer_t const ku = traits::matrix_upper_bandwidth (a) - kl;
      integer_t const ld = traits::leading_dimension (a);

      assert(ku >= 0);

      integer_t info;
      detail::gbtrs (trans, n, kl, ku, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::matrix_storage (a),
#else
                     traits::matrix_storage_const (a),
#endif
                     ld,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::vector_storage (ipiv),
#else
                     traits::vector_storage_const (ipiv),
#endif
                     traits::matrix_storage (b),
                     traits::leading_dimension (b),
                     &info);
      return info;
    }

  }

}}}

#endif


#include "gdrawer.hpp"

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/spirit/include/phoenix_fusion.hpp>
#if BOOST_VERSION >= 106000
#include <boost/phoenix/object/new.hpp>
#else
#include <boost/spirit/home/phoenix/object/new.hpp>
#endif
#include <stdexcept>
#include <QDebug>

using namespace boost;
using namespace spirit;
using namespace ascii;
using namespace phoenix;

template<class Iterator>
struct ExprGrammar : qi::grammar<Iterator, expr_t*(), ascii::space_type>
{
	ExprGrammar(): ExprGrammar::base_type(expr, "Expression")
	{
		primitive.name("Primitive");
		factor.name("Factor");
		term.name("Term");
		expr.name("Expression");

		primitive
			= ('(' >> expr >> ')')[_val = _1]
			| ('|' >> expr >> '|')[_val = new_<unop_t>('|', _1)]
			| (char_('a', 'z') | char_('A', 'Z'))[_val = new_<var_t>(_1)]
			| real[_val = new_<const_t>(_1)];

		primitive2
			= primitive[_val = _1]
			| ('-' >> primitive)[_val = new_<unop_t>('-', _1)];

		factor
			= (primitive2 >> '^' >> factor)[_val = new_<binop_t>('^', _1, _2)]
			| primitive2[_val = _1];

		term
			= factor[_val = _1] >>
				*( (char_("/*") >> factor)[_val = new_<binop_t>(_1, _val, _2)]
				| !char_("-+") >> factor[_val = new_<binop_t>('*', _val, _1)] );

		expr
			= term[_val = _1] >> *(char_("+-") >> term)[_val = new_<binop_t>(_1, _val, _2)];

		qi::on_error<qi::fail>
		(
			expr,
			std::cout
                << val("Error! Expecting ")
                << _4                               // what failed?
                << val(" here: \"")
                << construct<std::string>(_3, _2)   // iterators to error-pos, end
                << val("\"")
                << std::endl
		);
		/* qi::debug(expr); qi::debug(term); qi::debug(factor); qi::debug(primitive); qi::debug(primitive2); */
	}

	qi::rule<Iterator, expr_t*(), ascii::space_type> primitive, primitive2, factor, term, expr;
	qi::real_parser<real_t> real;
};

Vm *MathVm::get(const QString& expr)
{
	expr_t* tree = NULL;
	std::string s = expr.toStdString();
	ExprGrammar<std::string::const_iterator> g;
	std::string::const_iterator begin = s.begin(), end = s.end();
	bool r = phrase_parse(begin, end, g, space, tree);
	if (!r || begin != end || !tree)
	{
//		qDebug() << "end - begin = " << int(end - begin) << " tree " << (long long)tree << std::endl;
		if (tree) delete tree;
		throw Exception("Syntax error");
	}

	MathVm *ret = new MathVm;
	tree->addInstr(ret);
	ret->requiredStackSize = tree->getDepth();
	delete tree;
	return ret;
}


#include <kdop_tandem_traversal.h>
#include <kdop_refit_tree.h>
#include <kdop_mesh_reorder.h>
#include <kdop_make_tree.h>
#include <kdop_io.h>

#include <mesh_array.h>
#include <tiny.h>

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

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::real_type          T;

class GeometryInfo
{
public:

  kdop::Tree<T,6>                                   m_tree;
  mesh_array::T4Mesh                                m_mesh;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> m_X;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> m_Y;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> m_Z;

  mesh_array::TetrahedronAttribute<mesh_array::TetrahedronSurfaceInfo,mesh_array::T4Mesh> m_surface_map;

};

class TestCallback : public geometry::ContactsCallback<V>
{
public:

  void operator()( V const & p, V const & n, V::real_type const & d )
  {
    // Do something?
  }
};

/**
 * Creates a test mesh which is basically a tetrahedron and its x-y plane mirrored counter part.
 *
 * @param p   Initial translation of the test mesh
 */
void make_test_mesh(
                      mesh_array::T3Mesh & mesh
                      , mesh_array::VertexAttribute<T,mesh_array::T3Mesh> & X
                      , mesh_array::VertexAttribute<T,mesh_array::T3Mesh> & Y
                      , mesh_array::VertexAttribute<T,mesh_array::T3Mesh> & Z
                      , V const & p
                      )
{
  mesh.clear();

  mesh.set_capacity( 5u, 6u );

  X.bind(mesh);
  Y.bind(mesh);
  Z.bind(mesh);

  mesh_array::Vertex const v1 = mesh.push_vertex();
  mesh_array::Vertex const v2 = mesh.push_vertex();
  mesh_array::Vertex const v3 = mesh.push_vertex();
  mesh_array::Vertex const vp = mesh.push_vertex();
  mesh_array::Vertex const vm = mesh.push_vertex();

  X(v1) = -1.0f + p(0);    Y(v1) = -1.0f + p(1);    Z(v1) =  0.0f + p(2);
  X(v2) =  1.0f + p(0);    Y(v2) = -1.0f + p(1);    Z(v2) =  0.0f + p(2);
  X(v3) =  0.0f + p(0);    Y(v3) =  1.0f + p(1);    Z(v3) =  0.0f + p(2);
  X(vp) =  0.0f + p(0);    Y(vp) =  0.0f + p(1);    Z(vp) =  2.0f + p(2);
  X(vm) =  0.0f + p(0);    Y(vm) =  0.0f + p(1);    Z(vm) = -2.0f + p(2);

  mesh.push_triangle( v1, v2, vp );
  mesh.push_triangle( v2, v3, vp );
  mesh.push_triangle( v3, v1, vp );

  mesh.push_triangle( v2, v1, vm );
  mesh.push_triangle( v3, v2, vm );
  mesh.push_triangle( v1, v3, vm );
}

void make_geometry( GeometryInfo & info, V const & p )
{
  mesh_array::T3Mesh surface;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  make_test_mesh(surface,sX,sY,sZ, p);

  mesh_array::T4Mesh mesh_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> X_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Y_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Z_in;
  mesh_array::tetgen(surface, sX, sY, sZ, mesh_in, X_in, Y_in, Z_in);

  kdop::mesh_reorder( mesh_in, X_in, Y_in, Z_in, info.m_mesh, info.m_X, info.m_Y, info.m_Z );

  mesh_array::compute_surface_map( info.m_mesh, info.m_X, info.m_Y, info.m_Z, info.m_surface_map );

  info.m_tree = kdop::make_tree<V,6,T>( 32000, info.m_mesh, info.m_X, info.m_Y, info.m_Z, kdop::sequential() );

  BOOST_CHECK( info.m_surface_map.size()>0u );

}

void make_geometry( GeometryInfo & info )
{
  make_geometry(info, V::zero() );
}

BOOST_AUTO_TEST_SUITE(kdop);

BOOST_AUTO_TEST_CASE(kdop_tandem_traversal)
{
  using namespace kdop::io;

  GeometryInfo A;
  GeometryInfo B;

  //
  // After building 6DOP tree there should be a total of 3 nodes. the
  // root is AABB( (-1,-1,-2), (1,1,2) ) and the root should have two
  // children nodes AABB( (-1,-1,-2), (1,1,0) ) and AABB( (-1,-1,0), (1,1,2) )
  // and both children are leafs
  //
  make_geometry(A, V::make(0.0f,0.0f,-1.5f));
  make_geometry(B, V::make(0.0f,0.0f, 1.5f));

  TestCallback my_test_callback;

  std::vector<kdop::TestPair<V, 6, T> > test_pairs;

  kdop::TestPair<V, 6, T> test =  kdop::TestPair<V, 6, T> (
                                                           A.m_tree
                                                           , B.m_tree
                                                           , A.m_mesh
                                                           , B.m_mesh
                                                           , A.m_X
                                                           , B.m_X
                                                           , A.m_Y
                                                           , B.m_Y
                                                           , A.m_Z
                                                           , B.m_Z
                                                           , A.m_surface_map
                                                           , B.m_surface_map
                                                           , my_test_callback
                                                           );

  test_pairs.push_back(test);

  kdop::tandem_traversal<V,6,T>(
                                test_pairs
                                , kdop::sequential()
                                );
}

BOOST_AUTO_TEST_SUITE_END();


/*
 Copyright (C) 2019 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/test/unit_test.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/weekendsonly.hpp>
#include <ql/time/schedule.hpp>
#include <qle/instruments/forwardbond.hpp>
#include <qle/pricingengines/discountingforwardbondengine.hpp>
#include <qle/pricingengines/discountingriskybondengine.hpp>

#include <boost/make_shared.hpp>
#include <iomanip>

using namespace boost::unit_test_framework;
using namespace QuantLib;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(ForwardBondTest)

BOOST_AUTO_TEST_CASE(testForwardBond1) { // test if bond and forwardbond comp coincide at maturity
    BOOST_TEST_MESSAGE("Testing QuantExt Forward Bond pricing. Case 1");

    SavedSettings backup;

    Settings::instance().evaluationDate() = Date(8, Dec, 2016);
    Date today = Settings::instance().evaluationDate();
    Settings::instance().includeReferenceDateEvents() = true;

    // bond market data
    Handle<Quote> rateQuote(boost::make_shared<SimpleQuote>(0.02));
    Handle<Quote> issuerSpreadQuote(boost::make_shared<SimpleQuote>(0.01));
    DayCounter dc = Actual365Fixed();
    Handle<YieldTermStructure> yts(boost::make_shared<FlatForward>(today, rateQuote, dc, Compounded, Semiannual));
    Handle<DefaultProbabilityTermStructure> dpts(boost::make_shared<FlatHazardRate>(today, issuerSpreadQuote, dc));
    Handle<Quote> bondSpecificSpread(boost::make_shared<SimpleQuote>(0.005));

    // build the underlying fixed rate bond
    Date startDate = today;
    Date endDate = startDate + Period(10, Years);
    Period tenor = 6 * Months;
    Calendar calendar = WeekendsOnly();
    BusinessDayConvention bdc = Following;
    BusinessDayConvention bdcEnd = bdc;
    DateGeneration::Rule rule = DateGeneration::Forward;
    bool endOfMonth = false;
    Date firstDate, lastDate;
    Schedule schedule(startDate, endDate, tenor, calendar, bdc, bdcEnd, rule, endOfMonth, firstDate, lastDate);

    Real redemption = 100.0;
    Real couponRate = 0.04;
    bool settlementDirty = true;
    Real compensationPayment = 0.0;
    Date compensationPaymentDate = today;

    Leg leg =
        FixedRateLeg(schedule).withNotionals(redemption).withCouponRates(couponRate, dc).withPaymentAdjustment(bdc);

    boost::shared_ptr<QuantLib::Bond> bond(boost::make_shared<QuantLib::Bond>(0, WeekendsOnly(), today, leg));
    Handle<Quote> recovery;
    boost::shared_ptr<PricingEngine> pricingEngine(
        boost::make_shared<QuantExt::DiscountingRiskyBondEngine>(yts, dpts, recovery, bondSpecificSpread, 1 * Months));
    bond->setPricingEngine(pricingEngine);

    Real price = bond->NPV();
    BOOST_TEST_MESSAGE("Bond price = " << price);

    // additional forward bond market data
    Handle<Quote> discountQuote(boost::make_shared<SimpleQuote>(0.01));
    Handle<YieldTermStructure> discountTS(
        boost::make_shared<FlatForward>(today, discountQuote, dc, Compounded, Semiannual));
    Handle<Quote> incomeQuote(boost::make_shared<SimpleQuote>(0.005));
    Handle<YieldTermStructure> incomeTS(
        boost::make_shared<FlatForward>(today, incomeQuote, dc, Compounded, Semiannual));
    // Handle<Quote> recoveryRate(boost::make_shared<SimpleQuote>(0.4));

    // build the forward bond
    // Date valueDate = today;
    Date fwdMaturityDate = today;
    Real strikePrice = 103.0;
    boost::shared_ptr<Payoff> payoff = boost::make_shared<QuantExt::ForwardBondTypePayoff>(Position::Long, strikePrice);
    // Natural settlementDays = 0; // why do we need this if we specify the value date explicitly ?
    boost::shared_ptr<QuantExt::ForwardBond> fwdBond(
        new QuantExt::ForwardBond(bond, payoff, fwdMaturityDate, fwdMaturityDate, true, settlementDirty,
                                  compensationPayment, compensationPaymentDate));
    boost::shared_ptr<PricingEngine> fwdBondEngine(new QuantExt::DiscountingForwardBondEngine(
        discountTS, incomeTS, yts, bondSpecificSpread, dpts, recovery, 2 * Months));
    fwdBond->setPricingEngine(fwdBondEngine);

    BOOST_TEST_MESSAGE("Forward Bond price = " << fwdBond->NPV());
    BOOST_CHECK_CLOSE(fwdBond->NPV() + strikePrice, bond->dirtyPrice(), 0.000001);
}

BOOST_AUTO_TEST_CASE(testForwardBond2) { // same testcase as above, but different setting of params

    BOOST_TEST_MESSAGE("Testing QuantExt Forward Bond pricing. Case 2");

    SavedSettings backup;

    Settings::instance().evaluationDate() = Date(8, Dec, 2016);
    Date today = Settings::instance().evaluationDate();
    Settings::instance().includeReferenceDateEvents() = true;

    // bond market data
    Handle<Quote> rateQuote(boost::make_shared<SimpleQuote>(0.02));
    Handle<Quote> issuerSpreadQuote(boost::make_shared<SimpleQuote>(0.01));
    DayCounter dc = Actual365Fixed();
    Handle<YieldTermStructure> yts(boost::make_shared<FlatForward>(today, rateQuote, dc, Compounded, Semiannual));
    Handle<DefaultProbabilityTermStructure> dpts(boost::make_shared<FlatHazardRate>(today, issuerSpreadQuote, dc));
    Handle<Quote> bondSpecificSpread(boost::make_shared<SimpleQuote>(0.005));

    // build the underlying fixed rate bond
    Date startDate = today;
    Date endDate = startDate + Period(10, Years);
    Period tenor = 6 * Months;
    Calendar calendar = WeekendsOnly();
    BusinessDayConvention bdc = Following;
    BusinessDayConvention bdcEnd = bdc;
    DateGeneration::Rule rule = DateGeneration::Forward;
    bool endOfMonth = false;
    Date firstDate, lastDate;
    Schedule schedule(startDate, endDate, tenor, calendar, bdc, bdcEnd, rule, endOfMonth, firstDate, lastDate);

    Real redemption = 95.0;
    Real couponRate = 0.04;
    bool settlementDirty = true;
    Real compensationPayment = 0.0;
    Date compensationPaymentDate = today;
    Leg leg =
        FixedRateLeg(schedule).withNotionals(redemption).withCouponRates(couponRate, dc).withPaymentAdjustment(bdc);

    boost::shared_ptr<QuantLib::Bond> bond(boost::make_shared<QuantLib::Bond>(0, WeekendsOnly(), today, leg));
    Handle<Quote> recovery; /*Handle<Quote> recovery(boost::make_shared<SimpleQuote>(0.4));*/
    boost::shared_ptr<PricingEngine> pricingEngine(
        boost::make_shared<QuantExt::DiscountingRiskyBondEngine>(yts, dpts, recovery, bondSpecificSpread, 2 * Months));
    bond->setPricingEngine(pricingEngine);

    Real price = bond->NPV();
    BOOST_TEST_MESSAGE("Bond price = " << price);

    // additional forward bond market data
    Handle<Quote> discountQuote(boost::make_shared<SimpleQuote>(0.01));
    Handle<YieldTermStructure> discountTS(
        boost::make_shared<FlatForward>(today, discountQuote, dc, Compounded, Semiannual));
    Handle<Quote> incomeQuote(boost::make_shared<SimpleQuote>(0.005));
    Handle<YieldTermStructure> incomeTS(
        boost::make_shared<FlatForward>(today, incomeQuote, dc, Compounded, Semiannual));

    // build the forward bond
    // Date valueDate = today;
    Date fwdMaturityDate = today;
    Real strikePrice = 98.0;
    boost::shared_ptr<Payoff> payoff = boost::make_shared<QuantExt::ForwardBondTypePayoff>(Position::Long, strikePrice);
    // Natural settlementDays = 0; // why do we need this if we specify the value date explicitly ?

    boost::shared_ptr<QuantExt::ForwardBond> fwdBond(
        new QuantExt::ForwardBond(bond, payoff, fwdMaturityDate, fwdMaturityDate, true, settlementDirty,
                                  compensationPayment, compensationPaymentDate));

    boost::shared_ptr<PricingEngine> fwdBondEngine(new QuantExt::DiscountingForwardBondEngine(
        discountTS, incomeTS, yts, bondSpecificSpread, dpts, recovery, 2 * Months));
    fwdBond->setPricingEngine(fwdBondEngine);

    BOOST_TEST_MESSAGE("Forward Bond price = " << fwdBond->NPV());
    BOOST_CHECK_CLOSE(fwdBond->NPV() + strikePrice, bond->NPV(), 0.000001);
}

BOOST_AUTO_TEST_CASE(testForwardBond3) { // now true forward bond, but coupon payments are excluded

    BOOST_TEST_MESSAGE("Testing QuantExt Forward Bond pricing. Case 3");

    SavedSettings backup;

    Settings::instance().evaluationDate() = Date(8, Dec, 2016);
    Date today = Settings::instance().evaluationDate();
    Settings::instance().includeReferenceDateEvents() = true;

    // bond market data
    Handle<Quote> rateQuote(boost::make_shared<SimpleQuote>(0.02));
    Handle<Quote> issuerSpreadQuote(boost::make_shared<SimpleQuote>(0.0));
    DayCounter dc = Actual365Fixed();
    Handle<YieldTermStructure> yts(boost::make_shared<FlatForward>(today, rateQuote, dc, Compounded, Semiannual));
    Handle<DefaultProbabilityTermStructure> dpts(boost::make_shared<FlatHazardRate>(today, issuerSpreadQuote, dc));
    Handle<Quote> bondSpecificSpread(boost::make_shared<SimpleQuote>(0.005));

    // build the underlying fixed rate bond
    Date startDate = today;
    Date endDate = startDate + Period(10, Years);
    Period tenor = 6 * Months;
    Calendar calendar = WeekendsOnly();
    BusinessDayConvention bdc = Following;
    BusinessDayConvention bdcEnd = bdc;
    DateGeneration::Rule rule = DateGeneration::Forward;
    bool endOfMonth = false;
    Date firstDate, lastDate;
    Schedule schedule(startDate, endDate, tenor, calendar, bdc, bdcEnd, rule, endOfMonth, firstDate, lastDate);

    Real redemption = 100.0;
    Real couponRate = 0.04;
    bool settlementDirty = true;
    Real compensationPayment = 0.0;
    Date compensationPaymentDate = today;
    Leg leg =
        FixedRateLeg(schedule).withNotionals(redemption).withCouponRates(couponRate, dc).withPaymentAdjustment(bdc);

    boost::shared_ptr<QuantLib::Bond> bond(boost::make_shared<QuantLib::Bond>(0, WeekendsOnly(), today, leg));
    Handle<Quote> recovery(boost::make_shared<SimpleQuote>(0.0));
    boost::shared_ptr<PricingEngine> pricingEngine(
        boost::make_shared<QuantExt::DiscountingRiskyBondEngine>(yts, dpts, recovery, bondSpecificSpread, 2 * Months));
    bond->setPricingEngine(pricingEngine);

    Real price = bond->NPV();
    BOOST_TEST_MESSAGE("Bond price = " << price);

    // additional forward bond market data
    Handle<Quote> discountQuote(boost::make_shared<SimpleQuote>(0.01));
    Handle<YieldTermStructure> discountTS(
        boost::make_shared<FlatForward>(today, discountQuote, dc, Compounded, Semiannual));
    Handle<Quote> incomeQuote(boost::make_shared<SimpleQuote>(0.005));
    Handle<YieldTermStructure> incomeTS(
        boost::make_shared<FlatForward>(today, incomeQuote, dc, Compounded, Semiannual));

    // build the forward bond
    // Date valueDate = today;
    Date fwdMaturityDate = today + 2 * Months; // there are no cfs in these two months.
    Real strikePrice = 98.0;
    boost::shared_ptr<Payoff> payoff = boost::make_shared<QuantExt::ForwardBondTypePayoff>(Position::Long, strikePrice);
    // Natural settlementDays = 0; // why do we need this if we specify the value date explicitly ?

    boost::shared_ptr<QuantExt::ForwardBond> fwdBond(
        new QuantExt::ForwardBond(bond, payoff, fwdMaturityDate, fwdMaturityDate, true, settlementDirty,
                                  compensationPayment, compensationPaymentDate));

    boost::shared_ptr<PricingEngine> fwdBondEngine(new QuantExt::DiscountingForwardBondEngine(
        discountTS, incomeTS, yts, bondSpecificSpread, dpts, recovery, 2 * Months));
    fwdBond->setPricingEngine(fwdBondEngine);

    BOOST_TEST_MESSAGE("Forward Bond price = " << fwdBond->NPV());
    BOOST_CHECK_CLOSE((fwdBond->NPV() / (discountTS->discount(fwdMaturityDate)) + strikePrice) *
                          (incomeTS->discount(fwdMaturityDate)),
                      bond->NPV(), 0.000001); // no differnce in cfs. only check compounding factors
}

BOOST_AUTO_TEST_CASE(testForwardBond4) { // now true forward bond, one coupon before maturity

    BOOST_TEST_MESSAGE("Testing QuantExt Forward Bond pricing. Case 4");

    SavedSettings backup;

    Settings::instance().evaluationDate() = Date(8, Dec, 2016);
    Date today = Settings::instance().evaluationDate();
    Settings::instance().includeReferenceDateEvents() = true;

    // bond market data
    Handle<Quote> rateQuote(boost::make_shared<SimpleQuote>(0.02));
    Handle<Quote> issuerSpreadQuote(boost::make_shared<SimpleQuote>(0.00));
    DayCounter dc = Actual365Fixed();
    Handle<YieldTermStructure> yts(boost::make_shared<FlatForward>(today, rateQuote, dc, Compounded, Semiannual));
    Handle<DefaultProbabilityTermStructure> dpts(boost::make_shared<FlatHazardRate>(today, issuerSpreadQuote, dc));
    Handle<Quote> bondSpecificSpread(boost::make_shared<SimpleQuote>(0.000));

    // build the underlying fixed rate bond
    Date startDate = today;
    Date endDate = startDate + Period(10, Years);
    Period tenor = 6 * Months;
    Calendar calendar = WeekendsOnly();
    BusinessDayConvention bdc = Following;
    BusinessDayConvention bdcEnd = bdc;
    DateGeneration::Rule rule = DateGeneration::Forward;
    bool endOfMonth = false;
    Date firstDate, lastDate;
    Schedule schedule(startDate, endDate, tenor, calendar, bdc, bdcEnd, rule, endOfMonth, firstDate, lastDate);

    Real redemption = 100.0;
    Real couponRate = 0.04;
    bool settlementDirty = true;
    Real compensationPayment = 0.0;
    Date compensationPaymentDate = today;
    Leg leg =
        FixedRateLeg(schedule).withNotionals(redemption).withCouponRates(couponRate, dc).withPaymentAdjustment(bdc);

    boost::shared_ptr<QuantLib::Bond> bond(boost::make_shared<QuantLib::Bond>(0, WeekendsOnly(), today, leg));
    Handle<Quote> recovery(boost::make_shared<SimpleQuote>(0.0));
    boost::shared_ptr<PricingEngine> pricingEngine(
        boost::make_shared<QuantExt::DiscountingRiskyBondEngine>(yts, dpts, recovery, bondSpecificSpread, 2 * Months));
    bond->setPricingEngine(pricingEngine);

    Real price = bond->NPV();
    BOOST_TEST_MESSAGE("Bond price = " << std::setprecision(12) << price);

    // additional forward bond market data
    Handle<Quote> discountQuote(boost::make_shared<SimpleQuote>(0.01));
    Handle<YieldTermStructure> discountTS(
        boost::make_shared<FlatForward>(today, discountQuote, dc, Compounded, Semiannual));
    Handle<Quote> incomeQuote(boost::make_shared<SimpleQuote>(0.005));
    Handle<YieldTermStructure> incomeTS(
        boost::make_shared<FlatForward>(today, incomeQuote, dc, Compounded, Semiannual));

    // build the forward bond
    // Date valueDate = today;
    Date fwdMaturityDate = today + 7 * Months + 7 * Days; // there is one CF in those two months. today+7*Months happens
                                                          // to be a saturday, thus two days are added.

    Real strikePrice = 98.0;
    boost::shared_ptr<Payoff> payoff = boost::make_shared<QuantExt::ForwardBondTypePayoff>(Position::Long, strikePrice);
    //  Natural settlementDays = 0; // why do we need this if we specify the value date explicitly ?

    boost::shared_ptr<QuantExt::ForwardBond> fwdBond(
        new QuantExt::ForwardBond(bond, payoff, fwdMaturityDate, fwdMaturityDate, true, settlementDirty,
                                  compensationPayment, compensationPaymentDate));

    boost::shared_ptr<PricingEngine> fwdBondEngine(new QuantExt::DiscountingForwardBondEngine(
        discountTS, incomeTS, yts, bondSpecificSpread, dpts, recovery, 2 * Months));
    fwdBond->setPricingEngine(fwdBondEngine);

    BOOST_TEST_MESSAGE("Forward Bond price = " << std::setprecision(12) << fwdBond->NPV());
    BOOST_CHECK_CLOSE((fwdBond->NPV() / (discountTS->discount(fwdMaturityDate)) + strikePrice) *
                          (incomeTS->discount(fwdMaturityDate)),
                      bond->NPV() - (0.04 * 100.0 * 182 / 365 * (yts->discount(Date(8, Jun, 2017))) *
                                     (dpts->survivalProbability(Date(8, Jun, 2017)))),
                      0.000001); // one cf before maturity factors; the cf occurs at today+6*Months
    BOOST_TEST_MESSAGE("Present discount factors:");

    BOOST_TEST_MESSAGE("ytsDiscountFactors = " << std::setprecision(12) << yts->discount(Date(8, Jun, 2017)));
    BOOST_TEST_MESSAGE("ytsDiscountFactors = " << std::setprecision(12)
                                               << yts->discount(Date(8, Jun, 2017) + 1 * Days));
    BOOST_TEST_MESSAGE("ytsDiscountFactors = " << std::setprecision(12)
                                               << yts->discount(Date(8, Jun, 2017) + 5 * Days));

    BOOST_TEST_MESSAGE("incDiscountFactors = " << std::setprecision(12) << incomeTS->discount(Date(8, Jun, 2017)));
    BOOST_TEST_MESSAGE("incDiscountFactors = " << std::setprecision(12)
                                               << incomeTS->discount(Date(8, Jun, 2017) + 1 * Days));
    BOOST_TEST_MESSAGE("incDiscountFactors = " << std::setprecision(12)
                                               << incomeTS->discount(Date(8, Jun, 2017) + 5 * Days));

    BOOST_TEST_MESSAGE("disDiscountFactors = " << std::setprecision(12) << discountTS->discount(Date(8, Jun, 2017)));
    BOOST_TEST_MESSAGE("disDiscountFactors = " << std::setprecision(12)
                                               << discountTS->discount(Date(8, Jun, 2017) + 1 * Days));
    BOOST_TEST_MESSAGE("disDiscountFactors = " << std::setprecision(12)
                                               << discountTS->discount(Date(8, Jun, 2017) + 5 * Days));
}

BOOST_AUTO_TEST_CASE(testForwardBond5) { // now true forward bond, one coupon before maturity

    BOOST_TEST_MESSAGE("Testing QuantExt Forward Bond pricing. Case 5");

    SavedSettings backup;

    Settings::instance().evaluationDate() = Date(8, Dec, 2016);
    Date today = Settings::instance().evaluationDate();
    Settings::instance().includeReferenceDateEvents() = true;

    // bond market data
    Handle<Quote> rateQuote(boost::make_shared<SimpleQuote>(0.02));
    Handle<Quote> issuerSpreadQuote(boost::make_shared<SimpleQuote>(0.00));
    DayCounter dc = Actual365Fixed();
    Handle<YieldTermStructure> yts(boost::make_shared<FlatForward>(today, rateQuote, dc, Compounded, Semiannual));
    Handle<DefaultProbabilityTermStructure> dpts(boost::make_shared<FlatHazardRate>(today, issuerSpreadQuote, dc));
    Handle<Quote> bondSpecificSpread(boost::make_shared<SimpleQuote>(0.000));

    // build the underlying fixed rate bond
    Date startDate = today;
    Date endDate = startDate + Period(10, Years);
    Period tenor = 6 * Months;
    Calendar calendar = WeekendsOnly();
    BusinessDayConvention bdc = Following;
    BusinessDayConvention bdcEnd = bdc;
    DateGeneration::Rule rule = DateGeneration::Forward;
    bool endOfMonth = false;
    Date firstDate, lastDate;
    Schedule schedule(startDate, endDate, tenor, calendar, bdc, bdcEnd, rule, endOfMonth, firstDate, lastDate);

    Real redemption = 100.0;
    Real couponRate = 0.04;
    bool settlementDirty = true;
    Real compensationPayment = 5.0;
    Date compensationPaymentDate = today + 2 * Days;
    Leg leg =
        FixedRateLeg(schedule).withNotionals(redemption).withCouponRates(couponRate, dc).withPaymentAdjustment(bdc);

    boost::shared_ptr<QuantLib::Bond> bond(boost::make_shared<QuantLib::Bond>(0, WeekendsOnly(), today, leg));
    Handle<Quote> recovery(boost::make_shared<SimpleQuote>(0.0));
    boost::shared_ptr<PricingEngine> pricingEngine(
        boost::make_shared<QuantExt::DiscountingRiskyBondEngine>(yts, dpts, recovery, bondSpecificSpread, 2 * Months));
    bond->setPricingEngine(pricingEngine);

    Real price = bond->NPV();
    BOOST_TEST_MESSAGE("Bond price = " << std::setprecision(12) << price);

    // additional forward bond market data
    Handle<Quote> discountQuote(boost::make_shared<SimpleQuote>(0.01));
    Handle<YieldTermStructure> discountTS(
        boost::make_shared<FlatForward>(today, discountQuote, dc, Compounded, Semiannual));
    Handle<Quote> incomeQuote(boost::make_shared<SimpleQuote>(0.005));
    Handle<YieldTermStructure> incomeTS(
        boost::make_shared<FlatForward>(today, incomeQuote, dc, Compounded, Semiannual));

    // build the forward bond
    // Date valueDate = today;
    Date fwdMaturityDate = today + 7 * Months + 7 * Days; // there is one CF in those two months. today+7*Months happens
                                                          // to be a saturday, thus two days are added.

    Real strikePrice = 98.0;
    boost::shared_ptr<Payoff> payoff = boost::make_shared<QuantExt::ForwardBondTypePayoff>(Position::Long, strikePrice);
    //  Natural settlementDays = 0; // why do we need this if we specify the value date explicitly ?

    boost::shared_ptr<QuantExt::ForwardBond> fwdBond(
        new QuantExt::ForwardBond(bond, payoff, fwdMaturityDate, fwdMaturityDate, true, settlementDirty,
                                  compensationPayment, compensationPaymentDate));

    boost::shared_ptr<PricingEngine> fwdBondEngine(new QuantExt::DiscountingForwardBondEngine(
        discountTS, incomeTS, yts, bondSpecificSpread, dpts, recovery, 2 * Months));
    fwdBond->setPricingEngine(fwdBondEngine);

    BOOST_TEST_MESSAGE("Forward Bond price = " << std::setprecision(12) << fwdBond->NPV());
    BOOST_CHECK_CLOSE(((fwdBond->NPV() + compensationPayment * discountTS->discount(compensationPaymentDate)) /
                           (discountTS->discount(fwdMaturityDate)) +
                       strikePrice) *
                          (incomeTS->discount(fwdMaturityDate)),
                      bond->NPV() - (0.04 * 100.0 * 182 / 365 * (yts->discount(Date(8, Jun, 2017))) *
                                     (dpts->survivalProbability(Date(8, Jun, 2017)))),
                      0.000001); // one cf before maturity factors; the cf occurs at today+6*Months
}

BOOST_AUTO_TEST_CASE(testForwardBond6) { // now true forward bond, but coupon payments are excluded

    BOOST_TEST_MESSAGE("Testing QuantExt Forward Bond pricing: Clean vs dirty Strike. Case 6");
    BOOST_TEST_MESSAGE("This is like case 3 but with clean strike. Check long/short in particular.");

    SavedSettings backup;

    Settings::instance().evaluationDate() = Date(8, Dec, 2016);
    Date today = Settings::instance().evaluationDate();
    Settings::instance().includeReferenceDateEvents() = true;

    // bond market data
    Handle<Quote> rateQuote(boost::make_shared<SimpleQuote>(0.02));
    Handle<Quote> issuerSpreadQuote(boost::make_shared<SimpleQuote>(0.01));
    DayCounter dc = Actual365Fixed();
    Handle<YieldTermStructure> yts(boost::make_shared<FlatForward>(today, rateQuote, dc, Compounded, Semiannual));
    Handle<DefaultProbabilityTermStructure> dpts(boost::make_shared<FlatHazardRate>(today, issuerSpreadQuote, dc));
    Handle<Quote> bondSpecificSpread(boost::make_shared<SimpleQuote>(0.005));

    // build the underlying fixed rate bond
    Date startDate = today;
    Date endDate = startDate + Period(10, Years);
    Period tenor = 6 * Months;
    Calendar calendar = WeekendsOnly();
    BusinessDayConvention bdc = Following;
    BusinessDayConvention bdcEnd = bdc;
    DateGeneration::Rule rule = DateGeneration::Forward;
    bool endOfMonth = false;
    Date firstDate, lastDate;
    Schedule schedule(startDate, endDate, tenor, calendar, bdc, bdcEnd, rule, endOfMonth, firstDate, lastDate);

    Real redemption = 100.0;
    Real couponRate = 0.04;
    bool settlementDirty = false;
    Real compensationPayment = 0.0;
    Date compensationPaymentDate = today;
    Leg leg =
        FixedRateLeg(schedule).withNotionals(redemption).withCouponRates(couponRate, dc).withPaymentAdjustment(bdc);

    boost::shared_ptr<QuantLib::Bond> bond(boost::make_shared<QuantLib::Bond>(0, WeekendsOnly(), today, leg));
    Handle<Quote> recovery(boost::make_shared<SimpleQuote>(0.0));
    boost::shared_ptr<PricingEngine> pricingEngine(
        boost::make_shared<QuantExt::DiscountingRiskyBondEngine>(yts, dpts, recovery, bondSpecificSpread, 2 * Months));
    bond->setPricingEngine(pricingEngine);

    Real price = bond->NPV();
    BOOST_TEST_MESSAGE("Bond price = " << price);

    // additional forward bond market data
    Handle<Quote> discountQuote(boost::make_shared<SimpleQuote>(0.01));
    Handle<YieldTermStructure> discountTS(
        boost::make_shared<FlatForward>(today, discountQuote, dc, Compounded, Semiannual));
    Handle<Quote> incomeQuote(boost::make_shared<SimpleQuote>(0.005));
    Handle<YieldTermStructure> incomeTS(
        boost::make_shared<FlatForward>(today, incomeQuote, dc, Compounded, Semiannual));

    // build the forward bond
    // Date valueDate = today;
    Date fwdMaturityDate = today + 2 * Months; // there are no cfs in these two months.
    Real strikePrice = 98.0;
    boost::shared_ptr<Payoff> payoff = boost::make_shared<QuantExt::ForwardBondTypePayoff>(Position::Long, strikePrice);
    // Natural settlementDays = 0; // why do we need this if we specify the value date explicitly ?

    boost::shared_ptr<QuantExt::ForwardBond> fwdBond_sh(
        new QuantExt::ForwardBond(bond, payoff, fwdMaturityDate, fwdMaturityDate, true, settlementDirty,
                                  compensationPayment, compensationPaymentDate));

    boost::shared_ptr<PricingEngine> fwdBondEngine_sh(new QuantExt::DiscountingForwardBondEngine(
        discountTS, incomeTS, yts, bondSpecificSpread, dpts, recovery, 2 * Months));
    fwdBond_sh->setPricingEngine(fwdBondEngine_sh);

    BOOST_TEST_MESSAGE("Forward Bond price long = " << fwdBond_sh->NPV());

    Real fwdBondNPV = fwdBond_sh->NPV(); // save result of long computation.

    boost::shared_ptr<Payoff> payoff_sh =
        boost::make_shared<QuantExt::ForwardBondTypePayoff>(Position::Short, strikePrice);
    // Natural settlementDays = 0; // why do we need this if we specify the value date explicitly ?

    boost::shared_ptr<QuantExt::ForwardBond> fwdBond(
        new QuantExt::ForwardBond(bond, payoff_sh, fwdMaturityDate, fwdMaturityDate, true, settlementDirty,
                                  compensationPayment, compensationPaymentDate));

    boost::shared_ptr<PricingEngine> fwdBondEngine(new QuantExt::DiscountingForwardBondEngine(
        discountTS, incomeTS, yts, bondSpecificSpread, dpts, recovery, 2 * Months));
    fwdBond->setPricingEngine(fwdBondEngine);

    BOOST_TEST_MESSAGE("Forward Bond price short = " << fwdBond->NPV());

    BOOST_CHECK_CLOSE(fwdBondNPV, (-1.0) * fwdBond->NPV(), 0.000001);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/**
    @file
    @author  Alexander Sherikov

    @copyright 2017 Alexander Sherikov. Licensed under the Apache License,
    Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0)

    @brief
*/

#include "utf_common.h"

#include <boost/mpl/vector.hpp>

#include <qpmad/solver.h>
#include <qpmad/testing.h>

//===========================================================================
// ResolveFixture
//===========================================================================

template <class t_Solver>
class ResolveFixture
{
public:
    Eigen::VectorXd x;
    Eigen::MatrixXd H;
    Eigen::MatrixXd H_copy;
    Eigen::VectorXd h;
    Eigen::MatrixXd A;
    Eigen::VectorXd Alb;
    Eigen::VectorXd Aub;
    Eigen::VectorXd lb;
    Eigen::VectorXd ub;

    t_Solver solver;
    typename t_Solver::ReturnStatus status;
    qpmad::SolverParameters param;

public:
    ResolveFixture()
    {
        qpmad::MatrixIndex size = 20;
        qpmad::MatrixIndex num_general_ctr = 1;

        qpmad_utils::getRandomPositiveDefiniteMatrix(H, size);
        H_copy = H;
        h.setOnes(size);

        A.resize(num_general_ctr, size);
        A.setOnes();
        Alb.resize(num_general_ctr);
        Aub.resize(num_general_ctr);
        Alb << -1.5;
        Aub << 1.5;

        lb.resize(size);
        ub.resize(size);
        lb << 1, 2, 3, 4, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5, -5;
        ub << 1, 2, 3, 4, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5;
    }

    void solve()
    {
        status = solver.solve(x, H, h, lb, ub, A, Alb, Aub, param);
        BOOST_CHECK_EQUAL(status, qpmad::Solver::OK);
    }
};

using TypeListResolve = boost::mpl::vector<qpmad::Solver, qpmad::SolverTemplate<double, 20, 1, 1> >;

BOOST_FIXTURE_TEST_CASE_TEMPLATE(resolve_with_cholesky, t_Solver, TypeListResolve, ResolveFixture<t_Solver>)
{
    this->solve();
    // Hessian changed;
    BOOST_CHECK(not this->H_copy.isApprox(this->H, g_default_tolerance));

    // next iteration
    this->H_copy = this->H;
    Eigen::VectorXd x_copy = this->x;
    this->param.hessian_type_ = this->solver.getHessianType();
    BOOST_CHECK_EQUAL(this->param.hessian_type_, qpmad::SolverParameters::HESSIAN_CHOLESKY_FACTOR);

    this->solve();
    // Hessian not changed
    BOOST_CHECK(this->H_copy.isApprox(this->H, g_default_tolerance));
    // solution is the same
    BOOST_CHECK(x_copy.isApprox(this->x, g_default_tolerance));
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(resolve_with_inverted_cholesky, t_Solver, TypeListResolve, ResolveFixture<t_Solver>)
{
    this->param.return_inverted_cholesky_factor_ = true;
    this->solve();
    // Hessian changed;
    BOOST_CHECK(not this->H_copy.isApprox(this->H, g_default_tolerance));

    // next iteration
    this->H_copy = this->H;
    Eigen::VectorXd x_copy = this->x;
    this->param.hessian_type_ = this->solver.getHessianType();
    BOOST_CHECK_EQUAL(this->param.hessian_type_, qpmad::SolverParameters::HESSIAN_INVERTED_CHOLESKY_FACTOR);

    this->solve();
    // Hessian not changed
    BOOST_CHECK(this->H_copy.isApprox(this->H, g_default_tolerance));
    // solution is the same
    BOOST_CHECK(x_copy.isApprox(this->x, g_default_tolerance));
}

//===========================================================================
// ResolveUnconstrainedFixture
//===========================================================================

template <class t_Solver>
class ResolveUnconstrainedFixture
{
public:
    Eigen::VectorXd x;
    Eigen::MatrixXd H;
    Eigen::MatrixXd H_copy;
    Eigen::VectorXd h;
    Eigen::VectorXd lb;
    Eigen::VectorXd ub;

    t_Solver solver;
    typename t_Solver::ReturnStatus status;
    qpmad::SolverParameters param;

public:
    ResolveUnconstrainedFixture()
    {
        qpmad::MatrixIndex size = 20;

        qpmad_utils::getRandomPositiveDefiniteMatrix(H, size);
        H_copy = H;
        h.setOnes(size);

        lb.setConstant(size, -1e20);
        ub.setConstant(size, 1e20);

        lb(0) = 1;
        ub(0) = 1;
    }

    void solve()
    {
        status = solver.solve(x, H, h, lb, ub, param);
        BOOST_CHECK_EQUAL(status, qpmad::Solver::OK);
    }
};

using TypeListResolveUnconstrained = boost::mpl::vector<qpmad::Solver, qpmad::SolverTemplate<double, 20, 1, 0> >;

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        resolve_unconstrained_with_cholesky,
        t_Solver,
        TypeListResolveUnconstrained,
        ResolveUnconstrainedFixture<t_Solver>)
{
    this->solve();
    // Hessian changed;
    BOOST_CHECK(not this->H_copy.isApprox(this->H, g_default_tolerance));

    // next iteration
    this->H_copy = this->H;
    Eigen::VectorXd x_copy = this->x;
    this->param.hessian_type_ = this->solver.getHessianType();
    BOOST_CHECK_EQUAL(this->param.hessian_type_, qpmad::SolverParameters::HESSIAN_CHOLESKY_FACTOR);

    this->solve();
    // Hessian not changed
    BOOST_CHECK(this->H_copy.isApprox(this->H, g_default_tolerance));
    // solution is the same
    BOOST_CHECK(x_copy.isApprox(this->x, g_default_tolerance));
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE(
        resolve_unconstrained_with_inverted_cholesky,
        t_Solver,
        TypeListResolveUnconstrained,
        ResolveUnconstrainedFixture<t_Solver>)
{
    this->param.return_inverted_cholesky_factor_ = true;
    this->solve();
    // Hessian changed;
    BOOST_CHECK(not this->H_copy.isApprox(this->H, g_default_tolerance));

    // next iteration
    this->H_copy = this->H;
    Eigen::VectorXd x_copy = this->x;
    this->param.hessian_type_ = this->solver.getHessianType();
    BOOST_CHECK_EQUAL(this->param.hessian_type_, qpmad::SolverParameters::HESSIAN_INVERTED_CHOLESKY_FACTOR);

    this->solve();
    // Hessian not changed
    BOOST_CHECK(this->H_copy.isApprox(this->H, g_default_tolerance));
    // solution is the same
    BOOST_CHECK(x_copy.isApprox(this->x, g_default_tolerance));
}


#include <boost/test/unit_test.hpp>
#include <envire_core/SpatialItem.hpp>
#include <Eigen/Geometry>
#include <envire_core/AlignedBoundingBox.hpp>

using namespace envire::core;

BOOST_AUTO_TEST_CASE(boundary_test)
{
    SpatialItem<Eigen::Vector3d> vector_item;
    vector_item.setData(Eigen::Vector3d(0,0,0));
    boost::intrusive_ptr<AlignedBoundingBox> bounding_box(new AlignedBoundingBox);
    bounding_box->extend(Eigen::Vector3d(1.0,1.0,1.0));
    bounding_box->extend(Eigen::Vector3d(-1.0,-1.0,-1.0));
    vector_item.setBoundary(bounding_box);

    BOOST_ASSERT(vector_item.contains(vector_item.getData()));

    SpatialItem<Eigen::Vector3d> vector_item_2;
    vector_item_2.setData(Eigen::Vector3d(1,1,1));
    boost::intrusive_ptr<AlignedBoundingBox> bounding_box_2(new AlignedBoundingBox);
    bounding_box_2->extend(Eigen::Vector3d(2.0,2.0,2.0));
    bounding_box_2->extend(Eigen::Vector3d(0.0,0.0,0.0));
    vector_item_2.setBoundary(bounding_box_2);

    SpatialItem<Eigen::Vector3d> vector_item_3;
    boost::intrusive_ptr<AlignedBoundingBox> bounding_box_3(new AlignedBoundingBox);
    bounding_box_3->extend(Eigen::Vector3d(2.0,2.0,2.0));
    bounding_box_3->extend(Eigen::Vector3d(-2.0,-2.0,-2.0));
    vector_item_3.setBoundary(bounding_box_3);

     BOOST_ASSERT(!vector_item.contains(vector_item_2));
     BOOST_ASSERT(vector_item_3.contains(vector_item));
     BOOST_ASSERT(vector_item_3.contains(vector_item_2));

}


#ifndef EFANNA_IKHAMMING_INDEX_H_
#define EFANNA_IKHAMMING_INDEX_H_

#include "algorithm/base_index.hpp"
#include <boost/dynamic_bitset.hpp>
#include <time.h>
//for Debug
#include <iostream>
#include <fstream>
#include <string>
#include <unordered_map>
#include <map>
#include <sstream>
#include <set>
#include <numeric>
#include <utility>

namespace efanna{

struct IKHAMMINGIndexParams : public IndexParams
{
	IKHAMMINGIndexParams(int codelen, char*& BaseCodeFile, char*& QueryCodeFile, char*& InvertedKmeansFile)
	{
		init_index_type = IKHAMMING;
		ValueType len;
		len.int_val = codelen;
		extra_params.insert(std::make_pair("codelen",len));
		ValueType bcf;
		bcf.str_pt = BaseCodeFile;
		extra_params.insert(std::make_pair("bcfile",bcf));
		ValueType qcf;
		qcf.str_pt = QueryCodeFile;
		extra_params.insert(std::make_pair("qcfile",qcf));
		ValueType ikf;
		ikf.str_pt = InvertedKmeansFile;
		extra_params.insert(std::make_pair("ikfile",ikf));
	}
};

template <typename DataType>
class IKHAMMINGIndex : public InitIndex<DataType>
{
public:

	typedef InitIndex<DataType> BaseClass;

	typedef std::vector<unsigned int> Code;
	typedef std::vector<Code> Codes;
	typedef std::vector<Codes> BucketCodes;

	IKHAMMINGIndex(const Matrix<DataType>& dataset, const Distance<DataType>* d, const IndexParams& params = IKHAMMINGIndexParams(0,NULL,NULL,NULL)) :
		BaseClass(dataset,d,params)
	{
		std::cout<<"HASHING initial" <<std::endl;

		ExtraParamsMap::const_iterator it = params_.extra_params.find("codelen");
		if(it != params_.extra_params.end()){
			codelength = (it->second).int_val;
		}
		else{
			std::cout << "error: no code length setting" << std::endl;
		}

		it = params_.extra_params.find("bcfile");
		if(it != params_.extra_params.end()){
			char* fpath = (it->second).str_pt;
			std::string str(fpath);
			std::cout << "Loading base code from " << str << std::endl;

			LoadCode(fpath, BaseCode);
		}
		else{
			std::cout << "error: no base code file" << std::endl;
		}

		it = params_.extra_params.find("qcfile");
		if(it != params_.extra_params.end()){
			char* fpath = (it->second).str_pt;
			std::string str(fpath);
			std::cout << "Loading query code from " << str << std::endl;

			LoadCode(fpath, QueryCode);

		}
		else{
			std::cout << "error: no query code file" << std::endl;
		}

		it = params_.extra_params.find("ikfile");
		if(it != params_.extra_params.end()){
			char* fpath = (it->second).str_pt;
			std::string str(fpath);
			std::cout << "Loading kmenas partitions from " << str << std::endl;

			LoadPartition(fpath);

		}
		else{
			std::cout << "error: no kmeans file" << std::endl;
		}
	}

	void LoadCode(char* filename, Codes& base){

		unsigned tableNum = codelength / 32;
		unsigned lastLen = codelength % 32;
		if(lastLen > 0){
			std::cout <<"codelength not supported!" <<std::endl;
			return;
		}

		std::stringstream ss;
		for(unsigned j = 0; j < tableNum; j++){

			ss << filename << "_" << j+1 ;
			std::string codeFile;
			ss >> codeFile;
			ss.clear();

			std::ifstream in(codeFile.c_str(), std::ios::binary);
			if(!in.is_open()){std::cout<<"open file " << filename <<" error"<< std::endl;return;}

			int codeNum;
			in.read((char*)&codeNum,4);
			if (codeNum != 1){
				std::cout<<"codefile  "<< j << " error!"<<std::endl;
			}

			unsigned len;
			in.read((char*)&len,4);
			unsigned num;
			in.read((char*)&num,4);

			Code b;
			for(unsigned i = 0; i < num; i++){
				unsigned int codetmp;
				in.read((char*)&codetmp,4);
				b.push_back(codetmp);
			}
			base.push_back(b);
			in.close();
		}

	}

	void LoadPartition(char* filename){

		std::ifstream in(filename, std::ios::binary);
		if(!in.is_open()){std::cout<<"open file " << filename <<" error"<< std::endl;return;}

		unsigned nCluster;
		in.read((char*)&nCluster,4);

		unsigned Dim;
		in.read((char*)&Dim,4);

		centers.resize(nCluster);
		hashTable.resize(nCluster);

		unsigned tableNum = BaseCode.size();

		for(unsigned i = 0; i < nCluster; i++){

			Codes tmp(tableNum);
			BaseBucket.push_back(tmp);

			unsigned nSmp;
			in.read((char*)&nSmp,4);

			for(unsigned j = 0; j < Dim; j++){
				float tmp;
				in.read((char*)&tmp,4);
				centers[i].push_back(tmp);
			}

			for(unsigned j = 0; j < nSmp; j++){
				unsigned tmp;
				in.read((char*)&tmp,4);
				hashTable[i].push_back(tmp);

				for(unsigned k=0; k<tableNum; k++){
					BaseBucket[i][k].push_back(BaseCode[k][tmp]);
				}

			}

		}

		in.close();

	}

	void buildIndexImpl(){
		nGroup = SP.search_epoches;

		if(nGroup > centers.size()){
			nGroup = centers.size();
		}

		std::cout << "Search begin..." << std::endl;
	}

	void getNeighbors(size_t K, const Matrix<DataType>& query){

		size_t nTable = BaseCode.size();

		unsigned nCluster = centers.size();

		std::vector<std::vector<int> >hammingDistance(nGroup);
		std::vector<size_t> hammingDistanceIndex;;
		std::vector<std::pair<int, unsigned> >hammingDistanceSort;

		nn_results.clear();
		for (size_t cur = 0; cur < query.get_rows(); cur++) {

			std::vector<std::pair<float, unsigned> > CenterDistance;
			for(unsigned j = 0; j < nCluster; j++){
				CenterDistance.push_back(std::make_pair(distance_->compare(query.get_row(cur), centers[j].data(), query.get_cols()), j));
			}
			std::partial_sort(CenterDistance.begin(), CenterDistance.begin() + nGroup, CenterDistance.end());

			size_t pNum = 0;
			std::vector<unsigned> compareIndex;

			for(unsigned j = 0; j < nGroup; j++){
				compareIndex.push_back(CenterDistance[j].second);
				pNum += hashTable[CenterDistance[j].second].size();
			}
			if ((unsigned)SP.search_init_num > pNum){
				SP.search_init_num = pNum;
			}

			// resize hammingDistance
			for (size_t i = 0; i < compareIndex.size(); i++) {
				hammingDistance[i].resize(hashTable[compareIndex[i]].size());
				std::fill(hammingDistance[i].begin(), hammingDistance[i].end(), 0);
			}

			unsigned IndexNum = compareIndex.size();
			for (unsigned j = 0; j < nTable; j++) {
				for (unsigned idx = 0; idx < IndexNum; idx++) {
					unsigned tNum = BaseBucket[compareIndex[idx]][j].size();
					Code &code = BaseBucket[compareIndex[idx]][j];
					std::vector<int> &dst = hammingDistance[idx];
					for (unsigned i = 0; i < tNum; i++) {
						dst[i] += parallel_popcnt32(code[i] ^ QueryCode[j][cur]);
					}
				}
			}

			hammingDistanceSort.resize(0);
			for (unsigned idx = 0; idx < IndexNum; idx++) {
				size_t sort_num = hammingDistance[idx].size();
				for (size_t i = 0; i < sort_num; i++) {
					hammingDistanceSort.push_back(std::pair<int, unsigned>(hammingDistance[idx][i], hashTable[compareIndex[idx]][i]));
				}
			}

			// sort the index by its value
			size_t sort_num = std::min((size_t)SP.search_init_num, hammingDistanceSort.size());
			std::partial_sort(hammingDistanceSort.begin(), hammingDistanceSort.begin() + sort_num, hammingDistanceSort.end());

			std::vector<int> res;
			if ((unsigned) SP.search_init_num <= K){
				for (unsigned int j = 0; j < (unsigned) sort_num; j++) res.push_back(hammingDistanceSort[j].second);
			}else{
				std::vector<std::pair<float, unsigned> > Distance;
				for (size_t i = 0; i < (unsigned) sort_num; i++) {
					Distance.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(hammingDistanceSort[i].second), features_.get_cols()), hammingDistanceSort[i].second));
				}
				std::partial_sort(Distance.begin(), Distance.begin() + K, Distance.end());

				for (unsigned int j = 0; j < K; j++) res.push_back(Distance[j].second);
			}
			nn_results.push_back(res);
		}
	}

	void examineCodeImpl(){}
	void locateNeighbors(const Matrix<DataType>& query){}
	void outputVisitBucketNum(){}
	void loadGraph(char* filename){}

protected:
	USING_BASECLASS_SYMBOLS

	unsigned codelength;
	unsigned nGroup;

	Codes BaseCode;
	Codes QueryCode;

	std::vector<std::vector<float>> centers;
	std::vector<std::vector<unsigned>> hashTable;
	BucketCodes BaseBucket;

	// for statistic info
	std::vector<unsigned> HammingDistNum;

};
}
#endif


#include <iostream>
#include <time.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include "pcl/point_cloud.h"
#include "pcl/kdtree/kdtree_flann.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/features/fpfh.h"

using namespace pcl;
using namespace std;

namespace pc{

void voxelFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out, float gridsize){
	VoxelGrid<PointXYZ> vox_grid;
	vox_grid.setLeafSize(gridsize, gridsize, gridsize);
	vox_grid.setInputCloud(cloud_in);
	vox_grid.filter(*cloud_out);
	return;
}

void passFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr pc, double filter_limit){
	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud(pc);
	pass.setFilterFieldName("x");
	pass.setFilterLimits(0, filter_limit);
	pass.setFilterFieldName("y");
	pass.setFilterLimits(0, filter_limit);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(0, filter_limit);
	pass.filter(*pc);
	return;
}

}


//
// Created by chen-tian on 17-7-19.
//

#include <iostream>
#include <fstream>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <Eigen/Geometry>

#include <boost/format.hpp>
//字符串格式化

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char** argv){
    vector<cv::Mat> colorImgs, depthImgs;
    vector<Eigen::Isometry3d> poses;

    ifstream f("../pose.txt");//f is name
    if (!f)
    {
        cerr<<"please run this program with pose.txt"<<endl;
        return 1;
    }

    for (int i=0; i<5; i++)
    {
        //boost::format to format the srting
        boost::format fmt("../%s/%d.%s");//the format of the image
        colorImgs.push_back(cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back(cv::imread( (fmt%"depth"%(i+1)%"pgm").str() ,-1 ));//-1 is used to read initial image

        double data[7] = {0};
        for (auto& d:data )
            f>>d;

        Eigen::Quaterniond q(data[6], data[3], data[4], data[5] );//the last number is the real
        Eigen::Isometry3d T(q);//euler 4*4
        T.pretranslate(Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back(T);
    }

    //K
    double cx=325.5;
    double cy=253.5;
    double fx=518.0;
    double fy=519.0;
    double depthScale=100.0;

    cout<<"transfering image to point clouds..."<<endl;

    //point clouds typedef: XYZRGB
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;

    PointCloud::Ptr pointCloud(new PointCloud);
    for (int i=0; i<5; i++)
    {
        cout<<"transfering..."<<i+1<<endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        for(int v=0; v<color.rows;v++)
            for(int u=0; u<color.cols;u++)
            {
                unsigned int d = depth.ptr<unsigned short>(v)[u];
                if (d==0)continue;
                Eigen::Vector3d point;
                point[2] = double(d)/depthScale;
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy;
                Eigen::Vector3d pointWorld = T*point;

                PointT p;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[v*color.step+u*color.channels()];
                p.g = color.data[v*color.step+u*color.channels()+1];
                p.r = color.data[v*color.step+u*color.channels()+2];
                pointCloud->points.push_back(p);
            }
    }

    pointCloud->is_dense = false;
    cout<<"pointcloud has "<<pointCloud->size()<<" points"<<endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud);
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
#define CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_

#include <vector>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

struct BoxQPSolution {
  BoxQPSolution() {}
  BoxQPSolution(const Eigen::MatrixXd& Hff_inv, const Eigen::VectorXd& x, const std::vector<size_t>& free_idx,
                const std::vector<size_t>& clamped_idx)
      : Hff_inv(Hff_inv), x(x), free_idx(free_idx), clamped_idx(clamped_idx) {}

  Eigen::MatrixXd Hff_inv;
  Eigen::VectorXd x;
  std::vector<size_t> free_idx;
  std::vector<size_t> clamped_idx;
};

// Based on Yuval Tassa's BoxQP
// Cf. https://www.mathworks.com/matlabcentral/fileexchange/52069-ilqg-ddp-trajectory-optimization
class BoxQP {
 public:
  BoxQP(const std::size_t nx, std::size_t maxiter = 100, const double th_acceptstep = 0.1, const double th_grad = 1e-9,
        const double reg = 1e-9);
  ~BoxQP();

  const BoxQPSolution& solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& lb,
                             const Eigen::VectorXd& ub, const Eigen::VectorXd& xinit);
  const BoxQPSolution& get_solution() const;
  const std::size_t& get_nx() const;
  const std::size_t& get_maxiter() const;
  const double& get_th_acceptstep() const;
  const double& get_th_grad() const;
  const double& get_reg() const;
  const std::vector<double>& get_alphas() const;
  void set_nx(const std::size_t& nx);
  void set_maxiter(const std::size_t& maxiter);
  void set_th_acceptstep(const double& th_acceptstep);
  void set_th_grad(const double& th_grad);
  void set_reg(const double& reg);
  void set_alphas(const std::vector<double>& alphas);

 private:
  std::size_t nx_;
  BoxQPSolution solution_;
  std::size_t maxiter_;
  double th_acceptstep_;
  double th_grad_;
  double reg_;

  double fold_;
  double fnew_;
  std::size_t nf_;
  std::size_t nc_;
  std::vector<double> alphas_;
  Eigen::VectorXd x_;
  Eigen::VectorXd xnew_;
  Eigen::VectorXd g_;
  Eigen::VectorXd dx_;

  Eigen::VectorXd qf_;
  Eigen::VectorXd xf_;
  Eigen::VectorXd xc_;
  Eigen::VectorXd dxf_;
  Eigen::MatrixXd Hff_;
  Eigen::MatrixXd Hfc_;
  Eigen::LLT<Eigen::MatrixXd> Hff_inv_llt_;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_