//
// Created by Aman LaChapelle on 1/7/18.
//
// BatchingRPCServer
// Copyright (c) 2018 Aman LaChapelle
// Full license at BatchingRPCServer/LICENSE.txt
//

/*
    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
 */

#include <dlib/data_io.h>
#include <sstream>

#include "BatchingRPC.pb.h"
#include "DlibServable.hpp"

#include "gtest/gtest.h"

namespace {

using namespace dlib;

using net_type = loss_multiclass_log<fc<
    10,
    relu<fc<
        84,
        relu<fc<
            120,
            max_pool<2, 2, 2, 2,
                     relu<con<16, 5, 5, 1, 1,
                              max_pool<2, 2, 2, 2,
                                       relu<con<6, 5, 5, 1, 1,
                                                input<matrix<
                                                    unsigned char>>>>>>>>>>>>>>;

Serving::TensorMessage ToMessage(std::vector<matrix<unsigned char>> &&arr) {

  Serving::TensorMessage message;
  std::ostringstream buffer_stream(std::ios::binary);
  serialize(arr, buffer_stream);
  message.set_serialized_buffer(buffer_stream.str());
  message.set_n(arr.size());

  return message;
}

void TrainNetwork(const std::string &dirname) {
  std::vector<matrix<unsigned char>> training_images;
  std::vector<unsigned long> training_labels;
  std::vector<matrix<unsigned char>> testing_images;
  std::vector<unsigned long> testing_labels;
  load_mnist_dataset(dirname, training_images, training_labels, testing_images,
                     testing_labels);

  std::ifstream f(
      "../../../Servable/DlibServable/test/assets/mnist_network.dat");
  if (f.is_open()) {
    f.close();
    return;
  }

  net_type net;
  dnn_trainer<net_type> trainer(net);
  trainer.set_learning_rate(0.01);
  trainer.set_min_learning_rate(0.0001);
  trainer.set_mini_batch_size(128);
  trainer.be_verbose();
  trainer.set_synchronization_file("mnist_sync", std::chrono::seconds(20));
  trainer.train(training_images, training_labels);
  net.clean();
  serialize("../../../Servable/DlibServable/test/assets/mnist_network.dat")
      << net;
}

class TestDlibServable : public ::testing::Test {
protected:
  void SetUp() override {
    TrainNetwork("../../../Servable/DlibServable/test/assets");
    deserialize(
        "../../../Servable/DlibServable/test/assets/mnist_network.dat") >>
        raw_args.net;
    file_args.filename =
        "../../../Servable/DlibServable/test/assets/mnist_network.dat";

    std::vector<matrix<unsigned char>> training_images;
    std::vector<unsigned long> training_labels;
    load_mnist_dataset("../../../Servable/DlibServable/test/assets",
                       training_images, training_labels, input_, output_);
  }

  Serving::DlibRawBindArgs<net_type> raw_args;
  Serving::DlibFileBindArgs file_args;

  std::vector<matrix<unsigned char>> input_;
  std::vector<unsigned long> output_;
};

TEST_F(TestDlibServable, Bind) {
  Serving::DlibServable<net_type, matrix<unsigned char>, unsigned long>
      servable(4);

  EXPECT_NO_THROW(servable.Bind(raw_args));
}

TEST_F(TestDlibServable, BindFile) {
  Serving::DlibServable<net_type, matrix<unsigned char>, unsigned long>
      servable(4);

  EXPECT_NO_THROW(servable.Bind(file_args));
}

TEST_F(TestDlibServable, Single) {
  Serving::DlibServable<net_type, matrix<unsigned char>, unsigned long>
      servable(1);

  servable.Bind(raw_args);

  Serving::TensorMessage msg = ToMessage({input_[0]});
  msg.set_client_id("test");

  Serving::ReturnCodes r = servable.AddToBatch(msg);
  EXPECT_EQ(r, Serving::ReturnCodes::OK);

  Serving::TensorMessage output;
  r = servable.GetResult("test", &output);
  EXPECT_EQ(r, Serving::ReturnCodes::OK);
  std::istringstream output_buffer(output.serialized_buffer(),
                                   std::ios::binary);

  std::vector<unsigned long> results;
  deserialize(results, output_buffer);
  // this particular image is a seven, since the net is trained we might as well
  // run a prediction
  EXPECT_EQ(results[0], 7);
}

TEST_F(TestDlibServable, NoBind) {
  Serving::DlibServable<net_type, matrix<unsigned char>, unsigned long>
      servable(1);

  Serving::TensorMessage msg = ToMessage({input_[0]});
  msg.set_client_id("no_bind");

  Serving::ReturnCodes r = servable.AddToBatch(msg);
  EXPECT_EQ(r, Serving::ReturnCodes::NEED_BIND_CALL);
}

TEST_F(TestDlibServable, WrongSize) {
  Serving::DlibServable<net_type, matrix<unsigned char>, unsigned long>
      servable(2);

  servable.Bind(raw_args);

  Serving::TensorMessage msg;
  std::ostringstream buffer_stream(std::ios::binary);

  std::vector<matrix<unsigned char>> arr;
  arr.push_back(input_[0]);

  serialize(arr, buffer_stream);
  msg.set_serialized_buffer(buffer_stream.str());
  msg.set_n(2);

  msg.set_client_id("wrong_size");

  Serving::ReturnCodes r = servable.AddToBatch(msg);
  EXPECT_EQ(r, Serving::ReturnCodes::SHAPE_INCORRECT);
}

TEST_F(TestDlibServable, TooBig) {
  Serving::DlibServable<net_type, matrix<unsigned char>, unsigned long>
      servable(1);

  servable.Bind(raw_args);

  Serving::TensorMessage msg = ToMessage({input_[0], input_[1]});
  msg.set_client_id("too_big");

  Serving::ReturnCodes r = servable.AddToBatch(msg);
  EXPECT_EQ(r, Serving::ReturnCodes::BATCH_TOO_LARGE);
}

TEST_F(TestDlibServable, NextBatch) {
  Serving::DlibServable<net_type, matrix<unsigned char>, unsigned long>
      servable(3);

  servable.Bind(raw_args);

  Serving::TensorMessage msg = ToMessage({input_[0], input_[1]});
  msg.set_client_id("too_big");
  Serving::ReturnCodes r = servable.AddToBatch(msg);
  EXPECT_EQ(r, Serving::ReturnCodes::OK);

  msg = ToMessage({input_[0], input_[1]});
  msg.set_client_id("too_big");

  r = servable.AddToBatch(msg);
  EXPECT_EQ(r, Serving::ReturnCodes::NEXT_BATCH);
}

} // namespace


//==============================================================================
//
//   (c) Copyright, 2010 University Corporation for Atmospheric Research (UCAR).
//       All rights reserved.
//       Do not copy or distribute without authorization.
//
//       File: $RCSfile: ructest.cc,v $
//       Version: $Revision: 1.1 $  Dated: $Date: 2010-05-20 21:21:04 $
//
//==============================================================================

/**
 * @file ruc2wrfgrid.cc
 *
 * Convert RUC 130 conus lat-longs to WRF grid coordinates
 *
 * @date 3/25/10
 */

// Include files
#include <stdio.h>
#include <string>
#include <vector>
#include <projects.h>
#include <boost/format.hpp>
#include "Proj4Wrap.hh"

using boost::format;
using std::string;
using std::vector;

// Functions

int main(int argc, char **argv)
{
  // Uses latitude where lambert conformal projection is true and
  // median aligned with cartesian y-axis
  double ruclov = -95;
  double rucla1 = 45.2931;
  double ruclo1 = -97.588;
  double ruclatin1 = 25.0;
  double ruclatin2 = 25.0;
  double rucdx = 13545.087;
  double rucdy = 13545.087;
  int rucnx = 5;
  int rucny = 4;

  string rucParamString = str(format("+proj=lcc +R=6371200 +lon_0=%1% +lat_0=%2% +lat_1=%3% +lat_2=%4%") % ruclov % ruclatin1 % ruclatin1 % ruclatin2);

  p4w::Proj4Wrap rucLambertProj(rucParamString, p4w::Proj4Wrap::LON_LAT_TYPE, ruclo1, rucla1, rucdx, rucdy);
  printf("ruc false easting, false northing: %g %g\n", rucLambertProj.getFalseEasting(), rucLambertProj.getFalseNorthing());
  double xc;
  double yc;
  double lon;
  double lat;

  rucLambertProj.ll2xy(ruclo1, rucla1, &xc, &yc);
  printf("xc, yc: %g %g\n", xc, yc);

  double la2 = 45.6415;
  double lo2 = -96.9482;

  rucLambertProj.ll2xy(lo2, la2, &xc, &yc);
  printf("xc, yc for lo2, la2: %g %g\n", xc, yc);
  rucLambertProj.xy2ll(0, 0, &lon, &lat);
  printf("lon, lat for origin: %g %g\n", lon, lat);

  rucLambertProj.xy2ll(4, 3, &lon, &lat);
  printf("lon, lat for 4,3: %g %g\n", lon, lat);

  double la3 = 45.5245;
  double lo3 = -97.27;
  rucLambertProj.ll2xy(lo3, la3, &xc, &yc);
  printf("xc, yc for lo3, la3: %g %g\n", xc, yc);
}


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

//libraries
#include <Eigen/Eigen>
#include <boost/python.hpp>
#include <boost/shared_ptr.hpp>

//local
#include "hierarchical_optimizer.hpp"
#include "../nonrigid_optimization/hierarchical/optimizer_with_telemetry.hpp"
#include "eigen_numpy.hpp"
#include "../math/typedefs.hpp"

namespace bp = boost::python;
namespace impl = nonrigid_optimization::hierarchical;
namespace eig = Eigen;

namespace python_export {
namespace hierarchical_optimizer {

template<typename ScalarContainer, typename VectorContainer>
void export_algorithms(const char* suffix) {
	auto sufy = [&](const char* name) {
		return (std::string(name) + std::string(suffix));
	};
	bp::scope outer =
			bp::class_<impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>>(sufy("HierarchicalOptimizer").c_str(),
					bp::init<
							bp::optional<
									bool, bool,
									int, float, int, float,
									float, float, eig::VectorXf,
									typename impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::ResamplingStrategy,
									typename impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::VerbosityParameters,
									typename impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::LoggingParameters> >(
							bp::args("tikhonov_term_enabled",
									"gradient_kernel_enabled",

									"maximum_chunk_size",
									"rate",
									"maximum_iteration_count",
									"maximum_warp_update_threshold",

									"data_term_amplifier",
									"tikhonov_strength",
									"kernel",

									"resampling_strategy",

									"verbosity_parameters",
									"logging_parameters"
									)))
					.def("optimize", &impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::optimize,
					"Find optimal warp to map given live SDF to given canonical SDF",
					bp::args("canonical_field", "live_field"))
					.def("get_per_level_convergence_reports",
					&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::get_per_level_convergence_reports)
					.def("get_per_level_iteration_data",
					&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::get_per_level_iteration_data)
					;
	bp::enum_<typename impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::ResamplingStrategy>(
			"ResamplingStrategy")
			.value("NEAREST_AND_AVERAGE",
			impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::ResamplingStrategy::NEAREST_AND_AVERAGE)
			.value("LINEAR",
			impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::ResamplingStrategy::LINEAR);
	bp::class_<typename impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::VerbosityParameters>(
			"VerbosityParameters",
			"Parameters that control verbosity to stdout. "
					"Assumes being used in an \"immutable\" manner, i.e. just a structure that holds values",
			bp::init<bp::optional<bool, bool, bool, bool, bool>>(
					bp::args(/*"self",*/
					"print_max_warp_update",
							"print_iteration_mean_tsdf_difference",
							"print_iteration_std_tsdf_difference",
							"print_iteration_data_energy",
							"print_iteration_tikhonov_energy")))
			.add_property("print_iteration_max_warp_update",
			&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::VerbosityParameters
			::print_iteration_max_warp_update)
			.add_property("print_iteration_mean_tsdf_difference",
			&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::VerbosityParameters
			::print_iteration_mean_tsdf_difference)
			.add_property("print_iteration_std_tsdf_difference",
			&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::VerbosityParameters
			::print_iteration_std_tsdf_difference)
			.add_property("print_iteration_data_energy",
			&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::VerbosityParameters
			::print_iteration_data_energy)
			.add_property("print_iteration_tikhonov_energy",
			&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::VerbosityParameters
			::print_iteration_tikhonov_energy)
			//============================================
			.add_property("print_per_iteration_info",
			&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::VerbosityParameters
			::print_per_iteration_info)
			.add_property("print_per_level_info",
			&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::VerbosityParameters
			::print_per_level_info)
			;

	bp::class_<typename impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::LoggingParameters>(
			"LoggingParameters",
			"Parameters that control what intermediate results are gathered during run",
			bp::init<bp::optional<bool, bool>>(
					bp::args(
							"collect_per_level_convergence_reports",
							"collect_per_level_iteration_data"
							)))
			.add_property("collect_per_level_convergence_reports",
			&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::LoggingParameters::collect_per_level_convergence_reports)
			.add_property("collect_per_level_iteration_data",
			&impl::OptimizerWithTelemetry<ScalarContainer, VectorContainer>::LoggingParameters::collect_per_level_iteration_data)
			;

}

} // namespace hierarchical_optimizer
} // namespace python_export


#define BOOST_TEST_MODULE MyModuleName
#include <boost/test/included/unit_test.hpp>
#include "rational.h"
using namespace ExactArithmetic;

BOOST_AUTO_TEST_CASE(test_case_operator)
{
	// Check equality of the results of two expressions
	BOOST_CHECK_EQUAL(Rational(1,3) * Rational(4,9), Rational(4,27));
	system("pause");

}


#include <cassert>
#include <type_traits>
#include <boost/hana/basic_tuple.hpp>
#include <boost/hana/none_of.hpp>
#include <boost/hana/type.hpp>
#include <range/v3/utility/concepts.hpp>
#include <jegp/utility.hpp>
#include <mia/concepts.hpp>
#include <mia/spacial.hpp>

static_assert(jegp::underlying(mia::Axis::x) == 0);
static_assert(jegp::underlying(mia::Axis::y) == 1);
static_assert(jegp::underlying(mia::Axis::z) == 2);

static_assert(!std::is_constructible_v<mia::Abscissa<int>, mia::Ordinate<int>>);
static_assert(!mia::QuantityWith<mia::Abscissa<int>, mia::Ordinate<int>>{});

constexpr void test_point()
{
    static_assert(ranges::Regular<mia::Point2d<int>>{});

    static_assert(ranges::Common<mia::Point3d<int>, mia::Point3d<double>>{});
    static_assert(!ranges::Common<mia::Point2d<int>, mia::Point3d<double>>{});

    static_assert(ranges::Same<
                  mia::Point2d<int>, mia::Point<mia::Coordinates{2}, int>>{});
    static_assert(ranges::Same<
                  mia::Point3d<int>, mia::Point<mia::Coordinates{3}, int>>{});

    static_assert(mia::Point2d<int>::coordinates == mia::Coordinates{2});
    static_assert(mia::Point3d<int>::coordinates == mia::Coordinates{3});
    static_assert(ranges::Same<mia::Point2d<int>::quantity, int>{});
    static_assert(ranges::Same<mia::Point3d<double>::quantity, double>{});

    // Construction
    {
        const mia::Abscissa<int> x{0};
        const mia::Ordinate<int> y{0};
        const mia::Applicate<int> z{0};
        const mia::Coordinate<mia::Axis{3}, int> coord4d{0};

        const mia::Point pt1d{x};
        const mia::Point pt2d{x, y};
        const mia::Point pt3d{x, y, z};
        const mia::Point pt4d{x, y, z, coord4d};

        static_assert(
            ranges::Same<
                const mia::Point<mia::Coordinates{1}, int>, decltype(pt1d)>{});
        static_assert(ranges::Same<const mia::Point2d<int>, decltype(pt2d)>{});
        static_assert(ranges::Same<const mia::Point3d<int>, decltype(pt3d)>{});
        static_assert(
            ranges::Same<
                const mia::Point<mia::Coordinates{4}, int>, decltype(pt4d)>{});

        mia::Point<mia::Coordinates{1}, double>{x};
        mia::Point<mia::Coordinates{1}, double>{pt1d};
        mia::Point2d<double>{x, y};
        mia::Point2d<double>{pt2d};
        mia::Point3d<double>{x, y, z};
        mia::Point3d<double>{pt3d};
        mia::Point<mia::Coordinates{4}, double>{x, y, z, coord4d};
        mia::Point<mia::Coordinates{4}, double>{pt4d};

        constexpr auto is_constructible = [](auto point, auto... args) {
            return ranges::Constructible<decltype(point), decltype(args)...>{};
        };

        assert(!is_constructible(pt1d, y));
        assert(!is_constructible(pt1d, pt2d));
        assert(!is_constructible(pt2d, x));
        assert(!is_constructible(pt2d, y));
        assert(!is_constructible(pt2d, y, x));
        assert(!is_constructible(pt2d, pt1d));
        assert(!is_constructible(pt2d, pt1d, y));
        assert(!is_constructible(pt2d, pt3d));
        assert(!is_constructible(pt3d, x, y));
        assert(!is_constructible(pt3d, pt2d));
        assert(!is_constructible(pt3d, pt2d, z));
        assert(!is_constructible(pt3d, pt1d, y, z));
        assert(!is_constructible(pt3d, pt4d));
        assert(!is_constructible(pt4d, x, y, z));
        assert(!is_constructible(pt4d, pt3d));
        assert(!is_constructible(pt4d, pt3d, coord4d));
        assert(!is_constructible(pt4d, pt2d, z, coord4d));
        assert(!is_constructible(pt4d, pt1d, y, z, coord4d));

        constexpr auto is_ctad_constructible_with = boost::hana::is_valid(
            [](auto... coord) -> decltype(mia::Point{coord...}) {});

        static_assert(!is_ctad_constructible_with(
            mia::Abscissa<int>{}, mia::Ordinate<double>{}));
    }
    // Coordinate read access
    {
        constexpr auto same_and_equal = [](auto l, auto r) {
            return ranges::Same<decltype(l), decltype(r)>{} && l == r;
        };

        const mia::Point2d<int> pt2d;
        const auto [x0, y0]{pt2d};

        assert(same_and_equal(pt2d.x(), mia::Abscissa<int>{0}));
        assert(same_and_equal(pt2d.y(), mia::Ordinate<int>{0}));
        assert(same_and_equal(pt2d.x(), x0));
        assert(same_and_equal(pt2d.y(), y0));
        assert(same_and_equal(pt2d.x(), mia::get<0>(pt2d)));
        assert(same_and_equal(pt2d.y(), mia::get<1>(pt2d)));
        assert(same_and_equal(pt2d.x(), mia::get<mia::Axis::x>(pt2d)));
        assert(same_and_equal(pt2d.y(), mia::get<mia::Axis::y>(pt2d)));
        assert(same_and_equal(pt2d.x(), mia::get<mia::Abscissa<int>>(pt2d)));
        assert(same_and_equal(pt2d.y(), mia::get<mia::Ordinate<int>>(pt2d)));

        const mia::Point3d<int> pt3d;
        const auto [x1, y1, z0]{pt3d};

        assert(same_and_equal(pt2d.x(), pt3d.x()));
        assert(same_and_equal(pt2d.y(), pt3d.y()));
        assert(same_and_equal(pt3d.z(), mia::Applicate<int>{0}));
        assert(same_and_equal(pt2d.x(), x1));
        assert(same_and_equal(pt2d.y(), y1));
        assert(same_and_equal(pt3d.z(), z0));
        assert(same_and_equal(pt2d.x(), mia::get<0>(pt3d)));
        assert(same_and_equal(pt2d.y(), mia::get<1>(pt3d)));
        assert(same_and_equal(pt3d.z(), mia::get<2>(pt3d)));
        assert(same_and_equal(pt2d.x(), mia::get<mia::Axis::x>(pt3d)));
        assert(same_and_equal(pt2d.y(), mia::get<mia::Axis::y>(pt3d)));
        assert(same_and_equal(pt3d.z(), mia::get<mia::Axis::z>(pt3d)));
        assert(same_and_equal(pt2d.x(), mia::get<mia::Abscissa<int>>(pt3d)));
        assert(same_and_equal(pt2d.y(), mia::get<mia::Ordinate<int>>(pt3d)));
        assert(same_and_equal(pt3d.z(), mia::get<mia::Applicate<int>>(pt3d)));

        constexpr auto illformed_pt2d_reads{boost::hana::make_basic_tuple(
            [](auto pt) -> decltype(pt.z()) {},
            [](auto pt) -> decltype(mia::get<2>(pt)) {},
            [](auto pt) -> decltype(mia::get<mia::Axis::z>(pt)) {},
            [](auto pt) -> decltype(mia::get<mia::Ordinate<double>>(pt)) {},
            [](auto pt) -> decltype(mia::get<mia::Applicate<int>>(pt)) {})};

        assert(boost::hana::none_of(illformed_pt2d_reads, [=](auto read) {
            return boost::hana::is_valid(read, pt2d);
        }));
    }
#ifndef MIA_GCC_BUG_71504
    // Coordinate write access
    {
        mia::Point pt2d{mia::Abscissa<int>{2}, mia::Ordinate<int>{2}};
        auto& [x0, y0]{pt2d};

        assert(--pt2d.x() == mia::Abscissa<int>{1});
        assert(--pt2d.y() == mia::Ordinate<int>{1});
        assert(--x0 == mia::Abscissa<int>{0});
        assert(--y0 == mia::Ordinate<int>{0});

        mia::Point pt3d{mia::Abscissa<int>{2}, mia::Ordinate<int>{2},
                        mia::Applicate<int>{2}};
        auto& [x1, y1, z0]{pt3d};

        assert(--pt3d.x() == mia::Abscissa<int>{1});
        assert(--pt3d.y() == mia::Ordinate<int>{1});
        assert(--pt3d.z() == mia::Applicate<int>{1});
        assert(--x1 == mia::Abscissa<int>{0});
        assert(--y1 == mia::Ordinate<int>{0});
        assert(--z0 == mia::Applicate<int>{0});
    }
#endif // MIA_GCC_BUG_71504
    // Comparisons
    {
        assert(
            (mia::Point{mia::Abscissa<int>{0}, mia::Ordinate<int>{0}} ==
             mia::Point{mia::Abscissa<double>{0}, mia::Ordinate<double>{0}}));
        assert(
            (mia::Point{mia::Abscissa<int>{0}, mia::Ordinate<int>{0}} !=
             mia::Point{mia::Abscissa<double>{0}, mia::Ordinate<double>{1}}));
        assert(
            (mia::Point{mia::Abscissa<int>{0}, mia::Ordinate<int>{0}} !=
             mia::Point{mia::Abscissa<double>{1}, mia::Ordinate<double>{0}}));
        assert(
            (mia::Point{mia::Abscissa<int>{0}, mia::Ordinate<int>{0}} !=
             mia::Point{mia::Abscissa<double>{1}, mia::Ordinate<double>{1}}));

        static_assert(!ranges::EqualityComparable<
                      mia::Point2d<int>, mia::Point3d<int>>{});
    }
}

static_assert(!std::is_constructible_v<mia::Width<int>, mia::Height<int>>);
static_assert(!mia::QuantityWith<mia::Width<int>, mia::Height<int>>{});

constexpr void test_size()
{
    static_assert(ranges::Copyable<mia::Size2d<int>>{});
    static_assert(!ranges::SemiRegular<mia::Size2d<int>>{});

    static_assert(ranges::Common<mia::Size3d<int>, mia::Size3d<double>>{});
    static_assert(!ranges::Common<mia::Size2d<int>, mia::Size3d<double>>{});

    using namespace mia::dimensions_literals;

    static_assert(ranges::Same<mia::Size2d<int>, mia::Size<2_D, int>>{});
    static_assert(ranges::Same<mia::Size3d<int>, mia::Size<3_D, int>>{});

    static_assert(mia::Size2d<int>::dimensions == 2_D);
    static_assert(mia::Size3d<int>::dimensions == 3_D);
    static_assert(ranges::Same<mia::Size2d<int>::quantity, int>{});
    static_assert(ranges::Same<mia::Size3d<double>::quantity, double>{});

    // Construction
    {
        const mia::Width<int> w{1};
        const mia::Height<int> h{1};
        const mia::Depth<int> d{1};
        const mia::Dimension<mia::Axis{3}, int> dim4d{1};

        const mia::Size sz1d{w};
        const mia::Size sz2d{w, h};
        const mia::Size sz3d{w, h, d};
        const mia::Size sz4d{w, h, d, dim4d};

        static_assert(
            ranges::Same<const mia::Size<1_D, int>, decltype(sz1d)>{});
        static_assert(ranges::Same<const mia::Size2d<int>, decltype(sz2d)>{});
        static_assert(ranges::Same<const mia::Size3d<int>, decltype(sz3d)>{});
        static_assert(
            ranges::Same<const mia::Size<4_D, int>, decltype(sz4d)>{});

        mia::Size<1_D, double>{w};
        mia::Size<1_D, double>{sz1d};
        mia::Size2d<double>{w, h};
        mia::Size2d<double>{sz2d};
        mia::Size3d<double>{w, h, d};
        mia::Size3d<double>{sz3d};
        mia::Size<4_D, double>{w, h, d, dim4d};
        mia::Size<4_D, double>{sz4d};

        constexpr auto is_constructible = [](auto size, auto... args) {
            return ranges::Constructible<decltype(size), decltype(args)...>{};
        };

        assert(!is_constructible(sz1d, h));
        assert(!is_constructible(sz1d, sz2d));
        assert(!is_constructible(sz2d, w));
        assert(!is_constructible(sz2d, h));
        assert(!is_constructible(sz2d, h, w));
        assert(!is_constructible(sz2d, sz1d));
        assert(!is_constructible(sz2d, sz1d, h));
        assert(!is_constructible(sz2d, sz3d));
        assert(!is_constructible(sz3d, w, h));
        assert(!is_constructible(sz3d, sz2d));
        assert(!is_constructible(sz3d, sz2d, d));
        assert(!is_constructible(sz3d, sz1d, h, d));
        assert(!is_constructible(sz3d, sz4d));
        assert(!is_constructible(sz4d, w, h, d));
        assert(!is_constructible(sz4d, sz3d));
        assert(!is_constructible(sz4d, sz3d, dim4d));
        assert(!is_constructible(sz4d, sz2d, d, dim4d));
        assert(!is_constructible(sz4d, sz1d, h, d, dim4d));

        constexpr auto is_ctad_constructible_with = boost::hana::is_valid(
            [](auto... dim) -> decltype(mia::Size{dim...}) {});

        static_assert(!is_ctad_constructible_with(
            mia::Width<int>{}, mia::Height<double>{}));
    }
    // Dimension read access
    {
        constexpr auto same_and_equal = [](auto l, auto r) {
            return ranges::Same<decltype(l), decltype(r)>{} && l == r;
        };

        const mia::Size sz2d{mia::Width<int>{1}, mia::Height<int>{1}};
        const auto [w0, h0]{sz2d};

        assert(same_and_equal(sz2d.w(), mia::Width<int>{1}));
        assert(same_and_equal(sz2d.h(), mia::Height<int>{1}));
        assert(same_and_equal(sz2d.w(), w0));
        assert(same_and_equal(sz2d.h(), h0));
        assert(same_and_equal(sz2d.w(), mia::get<0>(sz2d)));
        assert(same_and_equal(sz2d.h(), mia::get<1>(sz2d)));
        assert(same_and_equal(sz2d.w(), mia::get<mia::Axis::x>(sz2d)));
        assert(same_and_equal(sz2d.h(), mia::get<mia::Axis::y>(sz2d)));
        assert(same_and_equal(sz2d.w(), mia::get<mia::Width<int>>(sz2d)));
        assert(same_and_equal(sz2d.h(), mia::get<mia::Height<int>>(sz2d)));

        const mia::Size3d<int> sz3d{w0, h0, mia::Depth<int>{1}};
        const auto [w1, h1, d0]{sz3d};

        assert(same_and_equal(sz2d.w(), sz3d.w()));
        assert(same_and_equal(sz2d.h(), sz3d.h()));
        assert(same_and_equal(sz3d.d(), mia::Depth<int>{1}));
        assert(same_and_equal(sz2d.w(), w1));
        assert(same_and_equal(sz2d.h(), h1));
        assert(same_and_equal(sz3d.d(), d0));
        assert(same_and_equal(sz2d.w(), mia::get<0>(sz3d)));
        assert(same_and_equal(sz2d.h(), mia::get<1>(sz3d)));
        assert(same_and_equal(sz3d.d(), mia::get<2>(sz3d)));
        assert(same_and_equal(sz2d.w(), mia::get<mia::Axis::x>(sz3d)));
        assert(same_and_equal(sz2d.h(), mia::get<mia::Axis::y>(sz3d)));
        assert(same_and_equal(sz3d.d(), mia::get<mia::Axis::z>(sz3d)));
        assert(same_and_equal(sz2d.w(), mia::get<mia::Width<int>>(sz3d)));
        assert(same_and_equal(sz2d.h(), mia::get<mia::Height<int>>(sz3d)));
        assert(same_and_equal(sz3d.d(), mia::get<mia::Depth<int>>(sz3d)));

        constexpr auto illformed_sz2d_reads{boost::hana::make_basic_tuple(
            [](auto sz) -> decltype(sz.z()) {},
            [](auto sz) -> decltype(mia::get<2>(sz)) {},
            [](auto sz) -> decltype(mia::get<mia::Axis::z>(sz)) {},
            [](auto sz) -> decltype(mia::get<mia::Height<double>>(sz)) {},
            [](auto sz) -> decltype(mia::get<mia::Applicate<int>>(sz)) {})};

        assert(boost::hana::none_of(illformed_sz2d_reads, [=](auto read) {
            return boost::hana::is_valid(read, sz2d);
        }));
    }
    // Dimension write access
    {
        mia::Size sz2d{mia::Width<int>{3}, mia::Height<int>{3}};

        sz2d.w(--sz2d.w());
        sz2d.h(--sz2d.h());
        assert(sz2d.w() == mia::Width<int>{2});
        assert(sz2d.h() == mia::Height<int>{2});

        sz2d.set(--sz2d.w());
        sz2d.set(--sz2d.h());
        assert(sz2d.w() == mia::Width<int>{1});
        assert(sz2d.h() == mia::Height<int>{1});

        mia::Size sz3d{mia::Width<int>{3}, mia::Height<int>{3},
                       mia::Depth<int>{3}};

        sz3d.w(--sz3d.w());
        sz3d.h(--sz3d.h());
        sz3d.d(--sz3d.d());
        assert(sz3d.w() == mia::Width<int>{2});
        assert(sz3d.h() == mia::Height<int>{2});
        assert(sz3d.d() == mia::Depth<int>{2});

        sz3d.set(--sz3d.w());
        sz3d.set(--sz3d.h());
        sz3d.set(--sz3d.d());
        assert(sz3d.w() == mia::Width<int>{1});
        assert(sz3d.h() == mia::Height<int>{1});
        assert(sz3d.d() == mia::Depth<int>{1});

        static_assert(!std::is_reference_v<decltype(sz3d.w())>);
        static_assert(!std::is_reference_v<decltype(sz3d.h())>);
        static_assert(!std::is_reference_v<decltype(sz3d.d())>);
        static_assert(!std::is_reference_v<decltype(mia::get<0>(sz2d))>);
        static_assert(
            !std::is_reference_v<decltype(mia::get<mia::Axis::x>(sz2d))>);
        static_assert(
            !std::is_reference_v<decltype(mia::get<mia::Width<int>>(sz2d))>);
    }
    // Comparisons
    {
        assert(
            (mia::Size{mia::Width<int>{1}, mia::Height<int>{1}} ==
             mia::Size{mia::Width<double>{1}, mia::Height<double>{1}}));
        assert(
            (mia::Size{mia::Width<int>{1}, mia::Height<int>{1}} !=
             mia::Size{mia::Width<double>{1}, mia::Height<double>{2}}));
        assert(
            (mia::Size{mia::Width<int>{1}, mia::Height<int>{1}} !=
             mia::Size{mia::Width<double>{2}, mia::Height<double>{1}}));
        assert(
            (mia::Size{mia::Width<int>{1}, mia::Height<int>{1}} !=
             mia::Size{mia::Width<double>{2}, mia::Height<double>{2}}));

        static_assert(
            !ranges::EqualityComparable<mia::Size2d<int>, mia::Size3d<int>>{});
    }
}

constexpr int test()
{
    test_point();
    test_size();

    return 0;
}

int main()
{
    constexpr int ret{test()};
    return ret;
}


//
// Copyright Karl Meerbergen 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_SYTRD_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_SYTRD_HPP

#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/vector_traits.hpp>
#include <boost/numeric/bindings/traits/matrix_traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/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

  /********************************************************************/
  /*                      eigenvalue problems                         */
  /********************************************************************/

  /* tridiagonal symmetric */

namespace boost { namespace numeric { namespace bindings { namespace lapack {

    namespace detail {

      inline
      void sytrd ( char uplo, int n, float* a, int lda, float* d, float* e, float* tau, float* work, int lwork, int& info )
      {
        LAPACK_SSYTRD( &uplo, &n, a, &lda, d, e, tau, work, &lwork, &info ) ;
      }

      inline
      void sytrd ( char uplo, int n, double* a, int lda, double* d, double* e, double* tau, double* work, int lwork, int& info )
      {
        LAPACK_DSYTRD( &uplo, &n, a, &lda, d, e, tau, work, &lwork, &info ) ;
      }

    } // namespace detail

    template <typename A, typename D, typename E, typename Tau, typename W>
    inline
    int sytrd( char uplo, A& a, D& d, E& e, Tau& tau, W& work ) {

      int const n = traits::matrix_size1 (a);
      assert( traits::matrix_size2 (a) == n );
      assert( traits::vector_size (d) == n );
      assert( traits::vector_size (e) == n-1 );
      assert( traits::vector_size (tau) == n-1 );
      assert( uplo=='U' || uplo=='L' );

      int lwork = traits::vector_size( work ) ;
      assert( lwork >= 1 );

      int info;
      detail::sytrd( uplo, n,
                     traits::matrix_storage( a ),
                     traits::leading_dimension( a ),
                     traits::vector_storage( d ),
                     traits::vector_storage( e ),
                     traits::vector_storage( tau ),
                     traits::vector_storage( work ), lwork,
                     info ) ;
      return info;
    } // sytrd()

    template <typename A, typename D, typename E, typename Tau>
    inline
    int sytrd( char uplo, A& a, D& d, E& e, Tau& tau, optimal_workspace=optimal_workspace() ) {
      int info;
      detail::sytrd( uplo, traits::matrix_size1( a ),
                     traits::matrix_storage( a ),
                     traits::leading_dimension( a ),
                     traits::vector_storage( d ),
                     traits::vector_storage( e ),
                     traits::vector_storage( tau ),
                     traits::vector_storage( tau ), -1,
                     info ) ;
      if (info) return info ;
      int lwork = * traits::vector_storage( tau ) ;

      traits::detail::array<typename traits::vector_traits<D>::value_type> work( lwork );

      return sytrd( uplo, a, d, e, tau, work ) ;
    }

    template <typename A, typename D, typename E, typename Tau>
    inline
    int sytrd( char uplo, A& a, D& d, E& e, Tau& tau, minimal_workspace ) {
      int lwork = 1 ;
      traits::detail::array<typename traits::vector_traits<D>::value_type> work( lwork );

      return sytrd( uplo, a, d, e, tau, work ) ;
    }

}}}}

#endif // BOOST_NUMERIC_BINDINGS_LAPACK_GBSV_HPP


#include <boost/test/unit_test.hpp>

#include <iostream>

#include "../../lib/datastructures/AssignmentGenerator.h"
#include "../../lib/datastructures/ExplanationGenerator.h"

using namespace smtrat;

BOOST_AUTO_TEST_SUITE(Test_AssignmentGenerator);
(or (<= x (NR -1 R)) (>= x (NR 1 R)) (>= y root(_z^2+x^2+(-1), 1, _z)) (<= (+ (* (- 1) (* z z)) (* (- 1) (* y y)) (* (- 1) (* x x)) 1) 0))
BOOST_AUTO_TEST_CASE(Test_NLSATPaper_Ex6)
{
	carl::Variable x = carl::freshRealVariable("x");
	carl::Variable y = carl::freshRealVariable("y");
	carl::Variable z = carl::freshRealVariable("z");

	Poly p = Poly(x*x) + y*y + z*z - Rational(1);
	ConstraintT c(p, carl::Relation::LESS);

	AssignmentGenerator ag;
	ag.pushAssignment(x, Rational(3)/Rational(4));
	ag.pushAssignment(y, -Rational(3)/Rational(4));
	ag.pushConstraint(c);
	BOOST_CHECK(!ag.hasAssignment(z));

	ExplanationGenerator eg(ag.getConflictingCore(), {z,y,x}, ag.getModel());
	FormulaT res = eg.generateExplanation(c);
	std::cout << res << std::endl;
}

BOOST_AUTO_TEST_CASE(Test_NLSATPaper)
{
	// Implements the example from the nlsat paper.
	carl::Variable x = carl::freshRealVariable("x");
	carl::Variable y = carl::freshRealVariable("y");
	ConstraintT c1(Poly(x)+Rational(1), carl::Relation::LEQ);
	ConstraintT c2(Poly(x)-Rational(1), carl::Relation::GEQ);
	ConstraintT c3(Poly(x)+y, carl::Relation::GREATER);
	ConstraintT c4(Poly(x)-y, carl::Relation::GREATER);
	ConstraintT c4Inv(c4.lhs(), carl::invertRelation(c4.relation()));

	AssignmentGenerator ag;
	// Select c1, Decide c1
	ag.pushConstraint(c1);
	BOOST_CHECK(ag.hasAssignment(x));
	BOOST_CHECK(ag.getAssignment() == carl::RealAlgebraicNumber<Rational>(-2));
	// Lift-Level -> x = -1
	ag.pushAssignment(x, Rational(-1));

	// Select c3, Decide c3
	ag.pushConstraint(c3);
	// R-Propagate !c4
	ag.pushConstraint(c4);

	BOOST_CHECK(!ag.hasAssignment(y));
	BOOST_CHECK(ag.getConflictingCore().size() == 2);

	ExplanationGenerator eg(ag.getConflictingCore(), {y,x}, ag.getModel());
	FormulaT res = eg.generateExplanation(c4);
	std::cout << res << std::endl;
}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "learnreg.h"
#include "mexutils.h"

using namespace Eigen;

/***** MATLAB mex function *****/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    // Verify and load arguments
    Eigen::SparseMatrix<double> X;
    VectorXd lamVec;
    double nodeBeta = 0;
    VectorXd thetaNode;
    SparseMatrix<double> thetaEdge;
    size_t nThreads = 0;
    try {
        if( nrhs < 2 ) { throw std::invalid_argument( "Too few arguments.  Need at least 2." ); }
        X = getSparseMatrix( prhs[0] );
        lamVec = getVector( prhs[1] );
        if(nrhs >= 3)
            nodeBeta = getScalar<double>( prhs[2] );
        if(nrhs >= 4)
            nThreads = getScalar<size_t>( prhs[3] );
        if(nrhs >= 5)
            thetaNode = getVector( prhs[4] );
        if(nrhs >= 6)
            thetaEdge = getSparseMatrix( prhs[5] );
    } catch ( const std::invalid_argument& e ) {
        std::cerr << "Error loading arguments: " << e.what() << std::endl;
        fake_answer(plhs);
        return;
    }

    // Run code
    GeneralizedMRF<Poisson> pmrf(2, nThreads);
    std::vector< VectorXd > thetaNodeArray;
    std::vector< SparseMatrix<double> > thetaEdgeArray;
    for(size_t li = 0; li < lamVec.size(); ++li) {
        // Run MRF
        pmrf.learnmrf(X, lamVec(li), nodeBeta, thetaNode, thetaEdge);
        // Extract results
        thetaNodeArray.push_back( thetaNode );
        thetaEdgeArray.push_back( thetaEdge );
    }

    // Save final answer into MATLAB output */
    if( nlhs >= 1 ) {
        if(lamVec.size() == 1) {
            plhs[0] = eigenVec2matlabVec( thetaNodeArray[0] );
        } else {
            // Save cell array of outputs
            plhs[0] = mxCreateCellMatrix((mwSize)lamVec.size(),1);
            for(size_t li = 0; li < lamVec.size(); ++li)
                mxSetCell(plhs[0], li, eigenVec2matlabVec( thetaNodeArray[li] ) );
        }
    }
    if( nlhs >= 2 ) {
        if(lamVec.size() == 1) {
            plhs[1] = eigenSparse2matlabSparse( thetaEdgeArray[0] );
        } else {
            // Save cell array of outputs
            plhs[1] = mxCreateCellMatrix((mwSize)lamVec.size(),1);
            for(size_t li = 0; li < lamVec.size(); ++li)
                mxSetCell(plhs[1], li, eigenSparse2matlabSparse( thetaEdgeArray[li] ) );
        }
    }
}


/*!
 * @file
 * This file defines unit tests for `boost::mpl::is_reachable`.
 */

#include <boost/mpl/is_reachable.hpp>
#include <boost/mpl/vector.hpp>

using namespace boost::mpl;

template <typename ...AdjacentVertices>
struct vertex {
    using open_neighborhood = vector<AdjacentVertices...>;
};

struct dummy_graph;

namespace test_graph_with_cycle {
    // cycle: u0 -> u4 -> u1 -> u0
    struct u0 : vertex<struct u4> { };
    struct u1 : vertex<u0> { };
    struct u2 : vertex<> { };
    struct u3 : vertex<u2> { };
    struct u4 : vertex<u1, u3> { };

    static_assert(is_reachable<dummy_graph, u0, u2>::value, "");
}

namespace test_acyclic_graph {
    struct v0 : vertex<struct v3> { };
    struct v1 : vertex<v0> { };
    struct v2 : vertex<v1> { };
    struct v3 : vertex<> { };
    struct v4 : vertex<v3, v2> { };

    static_assert(is_reachable<dummy_graph, v0, v0>::value, "");
    static_assert(!is_reachable<dummy_graph, v0, v1>::value, "");
    static_assert(!is_reachable<dummy_graph, v0, v2>::value, "");
    static_assert(is_reachable<dummy_graph, v0, v3>::value, "");
    static_assert(!is_reachable<dummy_graph, v0, v4>::value, "");

    static_assert(is_reachable<dummy_graph, v1, v0>::value, "");
    static_assert(is_reachable<dummy_graph, v1, v1>::value, "");
    static_assert(!is_reachable<dummy_graph, v1, v2>::value, "");
    static_assert(is_reachable<dummy_graph, v1, v3>::value, "");
    static_assert(!is_reachable<dummy_graph, v1, v4>::value, "");

    static_assert(is_reachable<dummy_graph, v2, v0>::value, "");
    static_assert(is_reachable<dummy_graph, v2, v1>::value, "");
    static_assert(is_reachable<dummy_graph, v2, v2>::value, "");
    static_assert(is_reachable<dummy_graph, v2, v3>::value, "");
    static_assert(!is_reachable<dummy_graph, v2, v4>::value, "");

    static_assert(!is_reachable<dummy_graph, v3, v0>::value, "");
    static_assert(!is_reachable<dummy_graph, v3, v1>::value, "");
    static_assert(!is_reachable<dummy_graph, v3, v2>::value, "");
    static_assert(is_reachable<dummy_graph, v3, v3>::value, "");
    static_assert(!is_reachable<dummy_graph, v3, v4>::value, "");

    static_assert(is_reachable<dummy_graph, v4, v0>::value, "");
    static_assert(is_reachable<dummy_graph, v4, v1>::value, "");
    static_assert(is_reachable<dummy_graph, v4, v2>::value, "");
    static_assert(is_reachable<dummy_graph, v4, v3>::value, "");
    static_assert(is_reachable<dummy_graph, v4, v4>::value, "");
}

int main() { }


/*
Lightweight Automated Planning Toolkit
Copyright (C) 2012
Miquel Ramirez <miquel.ramirez@rmit.edu.au>
Nir Lipovetzky <nirlipo@gmail.com>

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef __OPEN_LIST__
#define __OPEN_LIST__

#include <vector>
#include <queue>
#include <boost/heap/fibonacci_heap.hpp>
#include <aptk/ext_math.hxx>

namespace aptk
{

namespace search {

template <typename Node>
class Node_Comparer
{
public:
	bool operator()( Node* a, Node* b ) const {
		if ( dless( b->fn(), a->fn() ) ) return true;
		if( dequal( b->fn(), a->fn() ) ){
			if ( dequal( b->hn(), a->hn() ) ) {
				if (dless(b->gn(),a->gn())) return true;
				return false;
			}
		}
		return false;

		//return (dless(b->fn(), a->fn()) || (dequal(a->fn(), b->fn()) && dless(b->hn(), a->hn())));
	}
};

template <typename Node>
class Node_Comparer_DH
{
public:
	bool operator()( Node* a, Node* b ) const {
		if ( dless( b->fn(), a->fn() ) )  return true;
		if( dequal( b->fn(), a->fn() ) ){
			if ( dless( b->h1n(), a->h1n() ) ) return true;
			if( dequal( b->h1n(), a->h1n() ) ){

				if ( dless( b->h2n(), a->h2n() ) ) return true;
			}
		}
		return false;

		//return (dless(b->fn(), a->fn()) || (dequal(a->fn(), b->fn()) && dless(b->hn(), a->hn())));
	}
};

template <typename Node>
class Node_Comparer_3H
{
public:
	bool operator()( Node* a, Node* b ) const {
		if ( dless( b->h1n(), a->h1n() ) ) return true;
		if( dequal( b->h1n(), a->h1n() ) ){
			if ( dless( b->h2n(), a->h2n() ) ) return true;
			if( dequal( b->h2n(), a->h2n() ) ){
				if ( dless( b->h3n(), a->h3n() ) )  return true;
			}
		}
		//if ( dless( b->gn(), a->gn() ) )  return true;
		return false;

		//return (dless(b->fn(), a->fn()) || (dequal(a->fn(), b->fn()) && dless(b->hn(), a->hn())));
	}
};

template < class Node_Comp, class Node >
class Open_List
{

	//std::priority< Node, std::vector< Node >, Node_Comp > m_queue;
public:

	typedef	Node		Node_Type;

	Open_List();
	~Open_List();

	void 		insert( Node* );
	Node* 		pop();
	bool		empty() const;
	float		min() const;
	void		clear();
	Node*           top(){ return m_queue.top(); }

private:

	std::priority_queue< Node*, std::vector< Node* >, Node_Comp > m_queue;
};

template < class Node_Comp, class Node >
Open_List<Node_Comp, Node>::Open_List()
{
}

template < typename Node_Comp, typename Node >
Open_List<Node_Comp, Node>::~Open_List()
{
}

template < typename Node_Comp, typename Node >
void	Open_List<Node_Comp, Node>::insert( Node* n )
{
	m_queue.push( n );
}

template <typename Node_Comp, typename Node >
Node*	Open_List<Node_Comp, Node>::pop()
{
        if( empty() ) return NULL;
	Node* elem = m_queue.top();
	m_queue.pop();
	return elem;
}

template < typename Node_Comp, typename Node >
bool	Open_List<Node_Comp, Node>::empty() const
{
	return m_queue.empty();
}

template < typename Node_Comp, typename Node >
float     Open_List<Node_Comp, Node>::min() const
{
	if ( empty() ) return 0.0f;
	return m_queue.top()->f;
}

template < typename Node_Comp, typename Node >
void	Open_List<Node_Comp, Node>::clear()
{
	while ( !empty() )
	{
		Node* elem = pop();
		delete elem;
	}
}

	// MRJ: Open List allowing for nodes to be incrementally sorted when keys are updated. Wraps
	// Boost.Heap.Fibonacci_Heap, that implements Fibonacci heaps.
	//
	// In order to use these heaps, one needs to change the Search Node interface and is required
	// to make appropiate calls as explained below:
	//
	// 1) Interface requirements on Node classes
	//
	// The following types, attributes and methods need to be added:
	//
	// * Types:
	//
	// typedef	Fibonacci_Open_List< Node >	Open_List;
	//
	// This type is defined mostly for convenience and clarity.
	//
	// * Attributes:
	//
	// Open_List::Handle	heap_handle;
	// Open_List*		current;
	//
	// Both attributes need to be public, as the method Fibonacci_Open_List<Node>::insert() will set
	// them respectively to the address of the node inside the heap and a pointer to the Fibonacci_Open_List
	// object (to allow the node to callback Fibonacci_Open_List<Node>::update() when necessary).
	//
	// * Methods:
	//
	// bool	operator<( const Node& b ) const
	//
	// Boost heap libraries require this operator to be overloaded on heap element classes. Note that in order
	// to get an ascending ordering according to f(n1) < f(n2) - i.e. best first - the operator needs to return true whenever
	// f(n1) is greater than f(n2).
	//
	// void notify_update() {
	//	if (current) current->update(this);
	// }
	//
	// This method needs to be called right after any changes are done to the values that govern the ordering of elements in
	// the heap (i.e. right after any changes in the value of f(n)). After calling this method, the heap is guaranteed to
	// be sorted.
	//
	// void detach() {
	//	if (current) current->erase(this);
	// }
	//
	// This methods allow to remove an element from the heap. After calling this method, the heap is guaranteed to be sorted.
	//
	// 2) Example
	//
	// Below a very simple Node class example can be found
	/*
	class Node {
	public:
		typedef Fibonacci_Open_List< Node > 		Open_List;

		State			state;
		float			fn;
		Open_List::Handle	heap_handle;
		Open_List*		current;

		// NOTE: It is very important to make sure the pointer to the current heap is initialized to a known value.
		Node()
		: current(nullptr) {
		}

		bool	operator<( const Node& b ) const {
			return fn > b.fn;
		}

		void notify_update( ) {
			assert( current != nullptr );
			if ( current )
				current->update( this );
		}

		void detach() {
			assert( current != nullptr );
			if ( current )
				current->erase( this );
		}
	};
	*/
	//
	// 3) Declaring open lists
	//
	// The easiest and cleanest way of defining open lists as members of classes implementing search algorithms is as
	// follows:
	//
	// i) Instantiate the following type
	//
	// typedef Node::Open_List	My_BFS_Open_List;
	//
	// ii) Declaring the attribute as
	//
	// My_BFS_Open_List	m_primary_open;

	template < class Node >
	class Fibonacci_Open_List
	{
		struct	compare_node {
			bool operator()( Node* n1, Node* n2 ) const {
				return *n1 < *n2;
			}
		};

		typedef typename	boost::heap::fibonacci_heap< Node*, boost::heap::compare<compare_node> > Container;

	 	Container m_queue;
		public:
		typedef	typename 	Container::handle_type		     Handle;

		typedef	Node		Node_Type;

		Fibonacci_Open_List();
		~Fibonacci_Open_List();

		void 				insert( Node* );
		void				update( Node* );
		Node* 				pop();
		bool				empty() const;
		float				min() const;
		void				clear();
		Node*				first();
		typename Container::ordered_iterator
						begin() const 	{ return m_queue.ordered_begin(); }
		typename Container::ordered_iterator
						end() const	{ return m_queue.ordered_end(); }
		void				erase( Node* );

	};

	template < class Node >
	Fibonacci_Open_List<Node>::Fibonacci_Open_List()
	{
	}

	template < typename Node >
	Fibonacci_Open_List<Node>::~Fibonacci_Open_List()
	{
	}

	template < typename Node >
	void	Fibonacci_Open_List<Node>::insert( Node* n )
	{
		typename Container::handle_type handle = m_queue.push( n );
		n->heap_handle = handle;
		n->current = this;
	}

	template < typename Node >
	void	Fibonacci_Open_List<Node>::update( Node* n )
	{
		m_queue.update( n->heap_handle );
	}

	template < typename Node >
	void	Fibonacci_Open_List<Node>::erase( Node* n )
	{
		m_queue.erase( n->heap_handle );
	}

	template <typename Node >
	Node*	Fibonacci_Open_List<Node>::pop()
	{
		if( empty() ) return NULL;
		Node* elem = m_queue.top();
		m_queue.pop();
		elem->current = nullptr;
		return elem;
	}

	template <typename Node >
	bool	Fibonacci_Open_List<Node>::empty() const
	{
		return m_queue.empty();
	}

	template <typename Node >
	Node*     Fibonacci_Open_List<Node>::first()
	{
		if ( empty() ) return nullptr;
		return m_queue.top();
	}

	template <typename Node >
	float     Fibonacci_Open_List<Node>::min() const
	{
		if ( empty() ) return std::numeric_limits<float>::max();
		return m_queue.top()->fn;
	}

	template <typename Node >
	void	Fibonacci_Open_List<Node>::clear()
	{
		while ( !empty() )
		{
			Node* elem = pop();
			delete elem;
		}
	}

}

}

#endif // Open_List.hxx


/*
 * Copyright (c) 2010-2013 Steffen Kieß
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#ifndef MATH_VECTOR2_HPP_INCLUDED
#define MATH_VECTOR2_HPP_INCLUDED

#include <Core/Util.hpp>
#include <Core/Assert.hpp>

#include <Math/Forward.hpp>
#include <Math/Abs.hpp>

#include <complex>

#include <boost/type_traits/is_convertible.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/utility/enable_if.hpp>

//#define VECTOR2_USE_ARRAY

namespace Math {
  template <typename T> class
  Vector2 {
#ifdef VECTOR2_USE_ARRAY
    T data_[2];
#else
    T x_, y_;
#endif

    class PrivateType {
      friend class Vector2;
      NVCC_HOST_DEVICE PrivateType () {}
    };

  public:
    NVCC_HOST_DEVICE Vector2 () {}
#if !defined (__CUDACC__)
#if defined (__clang__) || GCC_VERSION_IS_ATLEAST(4, 6)
#pragma GCC diagnostic push
#endif
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic ignored "-Wfloat-conversion"
#endif
#ifdef VECTOR2_USE_ARRAY
    //NVCC_HOST_DEVICE explicit Vector2 (T v) { data_[0] = v; data_[1] = v }
    NVCC_HOST_DEVICE Vector2 (T x, T y) { data_[0] = x; data_[1] = y; }
    template <typename U> NVCC_HOST_DEVICE Vector2 (Vector2<U> v, UNUSED typename boost::enable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) { data_[0] = v.x (); data_[1] = v.y (); }
    template <typename U> NVCC_HOST_DEVICE explicit Vector2 (Vector2<U> v, UNUSED typename boost::disable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) { data_[0] = (T) v.x (); data_[1] = (T) v.y (); }
#else
    //NVCC_HOST_DEVICE explicit Vector2 (T v) : x_ (v), y_ (v) {}
    NVCC_HOST_DEVICE Vector2 (T x, T y) : x_ (x), y_ (y) {}
    template <typename U> NVCC_HOST_DEVICE Vector2 (Vector2<U> v, UNUSED typename boost::enable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) : x_ (v.x ()), y_ (v.y ()) {}
    template <typename U> NVCC_HOST_DEVICE explicit Vector2 (Vector2<U> v, UNUSED typename boost::disable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) : x_ (v.x ()), y_ (v.y ()) {}
#endif
#if !defined (__CUDACC__)
#if defined (__clang__) || GCC_VERSION_IS_ATLEAST(4, 6)
#pragma GCC diagnostic pop
#endif
#endif

    NVCC_HOST_DEVICE const T& x () const {
#ifdef VECTOR2_USE_ARRAY
      return data_[0];
#else
      return x_;
#endif
    }

    NVCC_HOST_DEVICE const T& y () const {
#ifdef VECTOR2_USE_ARRAY
      return data_[1];
#else
      return y_;
#endif
    }

    NVCC_HOST_DEVICE T& x () {
#ifdef VECTOR2_USE_ARRAY
      return data_[0];
#else
      return x_;
#endif
    }

    NVCC_HOST_DEVICE T& y () {
#ifdef VECTOR2_USE_ARRAY
      return data_[1];
#else
      return y_;
#endif
    }

    NVCC_HOST_DEVICE const T& operator[] (size_t i) const {
#ifdef VECTOR2_USE_ARRAY
      ASSERT (i >= 0 && i < 2);
      return data_[i];
#else
      if (i == 0)
        return x_;
      else if (i == 1)
        return y_;
      else
        ABORT ();
#endif
    }

    NVCC_HOST_DEVICE T& operator[] (size_t i) {
#ifdef VECTOR2_USE_ARRAY
      ASSERT (i >= 0 && i < 2);
      return data_[i];
#else
      if (i == 0)
        return x_;
      else if (i == 1)
        return y_;
      else
        ABORT ();
#endif
    }
  };

  // Operations on Vector2

#define RTS(op) DECLTYPE ((*(T*)NULL) op (*(U*)NULL))
#define RT(op) Vector2<RTS(op)>
#ifdef __CUDACC__ // Workaround BugPlayground/nvcc-5.0-templates-1.cu
  template <typename T> static ERROR_ATTRIBUTE ("should not be called") Vector2<T> helperGetVector2Type (T t);
#define RT2(op) DECLTYPE (helperGetVector2Type ((*(T*)NULL) op (*(U*)NULL)))
#define RETURN_RT(op) typedef RT2(op) Ty; return Ty
#else
#define RETURN_RT(op) return RT(op)
#endif
  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(+) operator+ (Vector2<T> v1, Vector2<U> v2) {
    RETURN_RT(+) (v1.x () + v2.x (), v1.y () + v2.y ());
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector2<T>& operator+= (Vector2<T>& v1, Vector2<U> v2) {
    v1.x () += v2.x (); v1.y () += v2.y ();
    return v1;
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(-) operator- (Vector2<T> v1, Vector2<U> v2) {
    RETURN_RT(-) (v1.x () - v2.x (), v1.y () - v2.y ());
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector2<T>& operator-= (Vector2<T>& v1, Vector2<U> v2) {
    v1.x () -= v2.x (); v1.y () -= v2.y ();
    return v1;
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(*) operator* (Vector2<T> v, U scalar) {
    RETURN_RT(*) (v.x () * scalar, v.y () * scalar);
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(*) operator* (T scalar, Vector2<U> v) {
    RETURN_RT(*) (scalar * v.x (), scalar * v.y ());
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector2<T>& operator*= (Vector2<T>& v, U scalar) {
    v.x () *= scalar; v.y () *= scalar;
    return v;
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(/) operator/ (Vector2<T> v, U scalar) {
    RETURN_RT(/) (v.x () / scalar, v.y () / scalar);
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector2<T>& operator/= (Vector2<T>& v, U scalar) {
    v.x () /= scalar; v.y () /= scalar;
    return v;
  }

  template <typename T> NVCC_HOST_DEVICE inline bool operator== (Vector2<T> v1, Vector2<T> v2) {
    return v1.x () == v2.x () && v1.y () == v2.y ();
  }
  template <typename T> NVCC_HOST_DEVICE inline bool operator!= (Vector2<T> v1, Vector2<T> v2) {
    return !(v1 == v2);
  }

  template <typename T> struct Abs2Impl<Vector2<T> > {
    static DECLTYPE(Math::abs2 (*(T*)0)) apply (Vector2<T> v) {
      return Math::abs2 (v.x ()) + Math::abs2 (v.y ());
    }
  };

  // dot product
  template <typename T, typename U> NVCC_HOST_DEVICE inline RTS(*) operator* (Vector2<T> v1, Vector2<U> v2) {
    return v1.x () * v2.x () + v1.y () * v2.y ();
  }

#undef RTS
#undef RT
#ifdef __CUDACC__ // Workaround BugPlayground/nvcc-5.0-templates-1.cu
#undef RT2
#undef RETURN_RT
#endif

  // Unary +/-
  template <typename T> NVCC_HOST_DEVICE inline Vector2<T> operator+ (Vector2<T> v) {
    return Vector2<T> (+v.x (), +v.y ());
  }
  template <typename T> NVCC_HOST_DEVICE inline Vector2<T> operator- (Vector2<T> v) {
    return Vector2<T> (-v.x (), -v.y ());
  }

  template <typename F> Vector2<F> inline real (Vector2<std::complex<F> > v) {
    return Vector2<F> (real (v.x ()), real (v.y ()));
  }

  template <typename F> Vector2<F> inline imag (Vector2<std::complex<F> > v) {
    return Vector2<F> (imag (v.x ()), imag (v.y ()));
  }

  template <typename F> Vector2<std::complex<F> > inline conj (Vector2<std::complex<F> > v) {
    return Vector2<std::complex<F> > (conj (v.x ()), conj (v.y ()));
  }
}

#endif // !MATH_VECTOR2_HPP_INCLUDED


#include <cfloat>
#include <type_traits>

#include <boost/qvm/all.hpp>

#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

#include <cppid/gm3d/point_2d.hpp>

using cppid::gm3d::point_2d;
using point2f = point_2d<float>;

TEST_CASE("float_point2_test traits")
{
  REQUIRE(std::is_trivial_v<point2f>);
  REQUIRE(std::is_standard_layout_v<point2f>);
  REQUIRE(sizeof(point2f) == (sizeof(float) * 2));

  REQUIRE((
   std::is_same_v<float, typename boost::qvm::vec_traits<point2f>::scalar_type>));
  REQUIRE(boost::qvm::vec_traits<point2f>::dim == 2);
}

TEST_CASE("float_point2_test initialization")
{
  {
    auto vt = point2f{0};
    REQUIRE(vt.x() == 0.0f);
    REQUIRE(vt.y() == 0.0f);
  }
  {
    auto vt = point2f{1.5, 2.2};
    REQUIRE(vt.x() == 1.5f);
    REQUIRE(vt.y() == 2.2f);
  }
  {
    auto vt = point2f{point2f{1.5, 2.3}};
    REQUIRE(vt.x() == 1.5f);
    REQUIRE(vt.y() == 2.3f);
  }
  {
    auto vt = point2f{point_2d<int>{0}};
    REQUIRE(vt.x() == 0.0f);
    REQUIRE(vt.y() == 0.0f);
  }
  {
    auto vt = point2f{0};
    vt = point_2d<int>{0};
    REQUIRE(vt.x() == 0.0f);
    REQUIRE(vt.y() == 0.0f);
  }
  {
    auto vd = point_2d<double>{0};
    point2f vt = vd;
    CHECK(vt.x() == 0.0f);
    CHECK(vt.y() == 0.0f);
  }
}

TEST_CASE("float_point2_test comparison")
{
  REQUIRE((point2f{0} == point2f{0, 0}));
  REQUIRE((point2f{3.5} == point2f{3.5, 3.5}));
  REQUIRE((point2f{-.5} == point2f{-.5, -.5}));

  REQUIRE((point2f{-.5} != point2f{-.5, -.1}));
}


/*
 * Copyright (c) 2017, Adrian Michel
 * http://www.amichel.com
 *
 * This software is released under the 3-Clause BSD License
 *
 * The complete terms can be found in the attached LICENSE file
 * or at https://opensource.org/licenses/BSD-3-Clause
 */

#pragma once

#include <boost/enable_shared_from_this.hpp>
#include <boost/make_shared.hpp>
#include <boost/scope_exit.hpp>
#include <boost/shared_array.hpp>
#include <memory>

#include "individual.hpp"
#include "listener.hpp"
#include "multithread.hpp"
#include "mutation_strategy.hpp"
#include "population.hpp"
#include "processors.hpp"
#include "random_generator.hpp"
#include "selection_strategy.hpp"
#include "termination_strategy.hpp"

namespace amichel {
namespace de {

/**
 * Exception thrown in case of an error during an optimization
 * session
 */
class differential_evolution_exception {};

/**
 * Differential evolution main class
 *
 * Runs an optimization session based on various input
 * parameters or strategies
 */
class differential_evolution {
 private:
  const size_t m_varCount;
  const size_t m_popSize;

  population m_pop1;
  population m_pop2;
  individual_ptr m_bestInd;

  const constraints& m_constraints;
  processors& m_processors;
  termination_strategy& m_terminationStrategy;
  selection_strategy& m_selectionStrategy;
  mutation_strategy_ptr m_mutationStrategy;
  listener_ptr m_listener;

  const bool m_minimize;

 public:
  /**
   * constructs a differential_evolution object
   *
   * @param varCount total number of variables. It includes the
   *  			   variables required by the objective function
   *  			   but has many more elements as required by the
   *  			   algorithm
   * @param popSize total number of individuals in a population
   * @param processors number of parallel processors used
   *  				 during an optimization session
   * @param constraints a vector of constraints that contains the
   *  				  constraints for the variables used by the
   *  				  objective function as well as constraints
   *  				  for all other variables used internally by
   *  				  the algorithm
   * @param minimize will attempt to minimize the cost if true, or
   *  			   maximize the cost if false
   * @param terminationStrategy a termination strategy
   * @param selectionStrategy a selection strategy
   * @param mutationStrategy a mutation strategy
   * @param listener a listener
   */
  differential_evolution(size_t varCount, size_t popSize,
                         processors& processors,
                         const constraints& constraints, bool minimize,
                         termination_strategy& terminationStrategy,
                         selection_strategy& selectionStrategy,
                         mutation_strategy_ptr mutationStrategy,
                         de::listener_ptr listener) try

      : m_varCount(varCount),
        m_popSize(popSize),
        m_pop1(popSize, varCount, constraints),
        m_pop2(popSize, varCount),
        m_constraints(constraints),
        m_processors(processors),
        m_minimize(minimize),
        m_terminationStrategy(terminationStrategy),
        m_listener(listener),
        m_selectionStrategy(selectionStrategy),
        m_mutationStrategy(mutationStrategy) {
    assert(terminationStrategy);
    assert(listener);
    assert(mutationStrategy);

    assert(popSize > 0);
    assert(varCount > 0);

    // initializing population 1 by running all objective functions with
    // the initial random arguments
    m_bestInd = m_pop1.best(minimize);
    processors.push(m_pop1);
    processors.start();
    processors.wait();

  } catch (const processors_exception&) {
    throw differential_evolution_exception();
  }

  virtual ~differential_evolution(void) {}

  /**
   * starts a differential evolution optimization process
   *
   * although the processing is done in parallel, this function is
   * synchronous and won't return until the optimization is
   * complete, or an error triggered an exception
   */
  void run() {
    try {
      m_listener->start();
      individual_ptr bestIndIteration(m_bestInd);
      for (size_t genCount = 0; std::invoke(m_terminationStrategy, m_bestInd, genCount); ++genCount) {
        m_listener->startGeneration(genCount);
        for (size_t i = 0; i < m_popSize; ++i) {
          mutation_strategy::mutation_info mutationInfo((*m_mutationStrategy)(m_pop1, bestIndIteration, i));

          individual_ptr tmpInd(mutationInfo.first);

          tmpInd->ensureConstraints(m_constraints, mutationInfo.second);

          // populate the queue
          m_processors.push(tmpInd);

          // put temps in a temp vector for now (they are empty until
          // processed), will be moved to the right place after processed
          m_pop2.at(i) = tmpInd;
        }

        m_listener->startProcessors(genCount);
        m_processors.start();
        m_processors.wait();
        m_listener->endProcessors(genCount);

        // BestParentChildSelectionStrategy()( m_pop1, m_pop2, m_bestInd,
        // m_minimize );
        m_listener->startSelection(genCount);
        std::invoke(m_selectionStrategy, m_pop1, m_pop2, m_bestInd, m_minimize);
        bestIndIteration = m_bestInd;

        m_listener->endSelection(genCount);

        m_listener->endGeneration(genCount, bestIndIteration, m_bestInd);
      }

      // make sure to call listener end() on scope exit,
      // whether due to normal end, or exception
      BOOST_SCOPE_EXIT_TPL((m_listener)) {
        m_listener->end();
      }
      BOOST_SCOPE_EXIT_END
    }
    catch (const processors_exception&) {
      m_listener->error();
      throw differential_evolution_exception();
    }
  }

  /**
   * returns the best individual resulted from the optimization
   * process
   *
   * @return individual_ptr
   */
  individual_ptr best() const { return m_bestInd; }
};
}  // namespace de
}  // namespace amichel


#include <stdio.h>
#include <inttypes.h>
#include <iostream>

#include <boost/shared_ptr.hpp>
#include <lcm/lcm-cpp.hpp>

#include "lcmtypes/bot_core.hpp"
#include <lcmtypes/microstrain.hpp>
#include <ConciseArgs>

#include <Eigen/Dense>
#include <Eigen/StdVector>

using namespace std;
using namespace Eigen;

class Pass{
  public:
    Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_,
         std::string ins_channel_);

    ~Pass(){
    }
  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    bool verbose_;
    std::string ins_channel_;

    void insHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  microstrain::ins_t* msg);
    microstrain::ins_t lidar_msgout_;
};

Pass::Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_,
         std::string ins_channel_):
    lcm_(lcm_), verbose_(verbose_),
    ins_channel_(ins_channel_){
  lcm_->subscribe( ins_channel_ ,&Pass::insHandler,this);
  cout << "Finished setting up\n";
}

void Pass::insHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  microstrain::ins_t* msg){
  Eigen::Quaterniond meas_rot(msg->quat[0],msg->quat[1],msg->quat[2],msg->quat[3]);
  Eigen::Matrix3d meas_rotation(meas_rot);

  // rotate coordinate frame so that look vector is +X, and up is +Z
  Eigen::Matrix3d M;
  M <<  -1, 0,  0,
         0, 1,  0,
         0, 0, -1;

  // which is equivalent to:
  /*
  double rpy[] = {M_PI, 0,M_PI};
  double q[4],rot[9];
  roll_pitch_yaw_to_quat (rpy,q);
  quat_to_matrix(q,rot);
  M << rot[0], rot[1], rot[2],
       rot[3], rot[4], rot[5],
       rot[6], rot[7], rot[8];

  cout << rot[0] << " " << rot[1] << " "<< rot[2] << "\n";
  cout << rot[3] << " "<< rot[4] << " "<< rot[5] << "\n";
  cout << rot[6] << " "<< rot[7] << " "<< rot[8] << "\n";
  */

  meas_rotation= M * meas_rotation;
  Eigen::Quaterniond rotation = Eigen::Quaterniond( meas_rotation * M.transpose() );

  bot_core::pose_t pose_msg;
  pose_msg.utime = msg->utime;
  pose_msg.pos[0] =0;
  pose_msg.pos[1] =0;
  pose_msg.pos[2] =1.6;
  pose_msg.orientation[0] = rotation.w();
  pose_msg.orientation[1] = rotation.x();
  pose_msg.orientation[2] = rotation.y();
  pose_msg.orientation[3] = rotation.z();
  lcm_->publish("POSE_BODY", &pose_msg);
}

int main( int argc, char** argv ){
  ConciseArgs parser(argc, argv, "lidar-passthrough");
  bool verbose=false;
  string ins_channel="MICROSTRAIN_INS";
  parser.add(verbose, "v", "verbose", "Verbosity");
  parser.add(ins_channel, "l", "ins_channel", "Incoming INS channel");
  parser.parse();
  cout << verbose << " is verbose\n";
  cout << ins_channel << " is ins_channel\n";

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  Pass app(lcm,verbose,ins_channel);
  cout << "Ready to convert from imu to pose" << endl << "============================" << endl;
  while(0 == lcm->handle());
  return 0;
}


/**
 * \ file VariableDelayLineFilter.cpp
 */

#include <iostream>

#include <ATK/Delay/VariableDelayLineFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/scoped_array.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( VariableDelayLineFilter_sinus_line16000_delaysinus_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }
  boost::scoped_array<float> datadelay(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    datadelay[i] = i * .25f;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);
  ATK::InPointerFilter<float> generatordelay(datadelay.get(), 1, PROCESSSIZE, false);
  generatordelay.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::VariableDelayLineFilter<float> filter(16000);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generatordelay, 0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(1);
  output.process(49);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < PROCESSSIZE/4; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i*4 - i], outdata[i*4]);
  }
  for(int64_t i = 0; i < PROCESSSIZE/4; ++i)
  {
    int64_t current = 4*i+1;
    BOOST_REQUIRE_EQUAL((data[current - i]*3 + data[current - i-1])/4, outdata[current]);
  }
  for(int64_t i = 0; i < PROCESSSIZE/4; ++i)
  {
    int64_t current = 4*i+2;
    BOOST_REQUIRE_EQUAL((data[current - i] + data[current - i-1])/2, outdata[current]);
  }
  for(int64_t i = 0; i < PROCESSSIZE/4; ++i)
  {
    int64_t current = 4*i+3;
    BOOST_REQUIRE_EQUAL((data[current - i] + data[current - i-1]*3)/4, outdata[current]);
  }
}