//
// Created by dominik on 29.05.21.
//

#ifndef SQSGENERATOR_UTILS_HPP
#define SQSGENERATOR_UTILS_HPP

#include "types.hpp"
#include <sstream>
#include <stdexcept>
#include <boost/multi_array.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/storage.hpp>

using namespace boost;
using namespace boost::numeric::ublas;

namespace boost{

    template<class MultiArray>
    void extent_to(MultiArray & ma, const MultiArray & other){ //this function is adapted from
        auto& otherShape = reinterpret_cast<boost::array<size_t, MultiArray::dimensionality> const&>(*other.shape());
        ma.resize(otherShape);
    }

    template <typename T, typename F=row_major>
    matrix<T, F> matrix_from_vector(size_t m, size_t n, const std::vector<T> & v)
    {
        if(m*n!=v.size()) {
            throw std::invalid_argument("Matrix size does not match storage size");
        }
        unbounded_array<T> storage(m*n);
        std::copy(v.begin(), v.end(), storage.begin());
        return matrix<T>(m, n, storage);
    }

    template<typename MultiArray>
    std::vector<typename MultiArray::size_type> shape_from_multi_array(const MultiArray &a) {
        return std::vector<typename MultiArray::size_type>(a.shape(), a.shape() + a.num_dimensions());
    }

    // TODO: Probably unused function -> Maybe we should remove it

    template<typename MultiArrayRef, typename MultiArray>
    [[maybe_unused]] MultiArrayRef make_array_ref(MultiArray &array) {
        return MultiArrayRef(array.data(), shape_from_multi_array<MultiArray>(array));
    }

    template <typename MultiArray>
    matrix<typename MultiArray::element> matrix_from_multi_array(MultiArray &ref) {
        typedef typename MultiArray::index index_t;
        auto shape = shape_from_multi_array(ref);
        auto [sx, sy] = std::make_tuple(static_cast<index_t>(shape[0]), static_cast<index_t>(shape[1]));
        matrix<typename MultiArray::element> result(sx, sy);
        for (index_t i = 0; i < sx; i++) {
            for (index_t j = 0; j < sy; j++) {
                result(i,j) = ref[i][j];
            }
        }
        return result;
    }

    // Helper class to determine the full number of elements in the
    // multi-dimensional array
    template <std::size_t... vs> struct ArraySize;
    template <std::size_t v, std::size_t... vs> struct ArraySize<v, vs...>
    { static constexpr std::size_t size = v * ArraySize<vs...>::size; };
    template <> struct ArraySize<>
    { static constexpr std::size_t size = 1; };

    // Creates your multi_array
    template <typename T, int... dims>
    boost::multi_array<T, sizeof...(dims)>
    make_multi_array(std::initializer_list<T> l)
    {
        constexpr std::size_t asize = ArraySize<dims...>::size;
        assert(l.size() == asize); // could be a static assert in C++14

        // Dump data into a vector (because it has the right kind of ctor)
        const std::vector<T> a(l);
        // This can be used in a multi_array_ref ctor.
        boost::const_multi_array_ref<T, sizeof...(dims)> mar(
                &a[0],
                std::array<int, sizeof...(dims)>{dims...});
        // Finally, deep-copy it into the structure we can return.
        return boost::multi_array<T, sizeof...(dims)>(mar);
    }

    template<typename T1>
    using formatter_t = std::function<std::string(const T1&)>;

    template<typename T1, typename T1Cast = T1>
    std::function<std::string(const T1&)> make_default_formatter() {
        return [](const T1 &v) {
            return std::to_string(static_cast<T1Cast>(v));
        };
    }

    template<typename T1, typename T1Cast = T1>
    std::string format_vector(std::vector<T1> values, formatter_t<T1> formatter = make_default_formatter<T1, T1Cast>()) {
        std::stringstream message;
        message << "{";
        for (auto it = values.begin(); it != values.end() - 1; ++it) message << formatter(*it) << ", ";
        message << formatter(values.back()) << "}";
        return message.str();
    }

    template<typename MultiArray>
    std::vector<typename MultiArray::element> to_flat_vector(const MultiArray &array) {
        return std::vector<typename MultiArray::element>(array.data(), array.data() + array.num_elements());
    }

    template<typename T1, typename T2, typename T1Cast = T1, typename T2Cast = T2>
    std::string format_dict(const std::vector<T1> &keys, const std::vector<T2> &values, formatter_t<T1> key_formatter = make_default_formatter<T1, T1Cast>(), formatter_t<T2> value_formatter = make_default_formatter<T2, T2Cast>()) {
        assert(keys.size() == values.size());
        std::stringstream message;
        message << "{";
        for (size_t i = 0; i < keys.size()-1 ; i++) message << key_formatter(keys[i]) << ": " << value_formatter(values[i]) << ", ";
        message << key_formatter(keys.back()) << ": " << value_formatter(values.back()) << "}";
        return message.str();
    }

    template<typename T1, typename T2, typename T1Cast = T1, typename T2Cast = T2>
    std::string format_map(const std::map<T1, T2> &map, formatter_t<T1> key_formatter = make_default_formatter<T1, T1Cast>(), formatter_t<T2> value_formatter = make_default_formatter<T2, T2Cast>()) {
        std::vector<T1> keys;
        std::vector<T2> values;
        for (const auto&[key, value] : map) {
            keys.push_back(key);
            values.push_back(value);
        }
        return format_dict<T1, T2, T1Cast, T2Cast>(keys, values, key_formatter, value_formatter);
    }

    template<typename MultiArray, typename T1 = typename MultiArray::element>
    std::string format_matrix(MultiArray array) {
        typedef typename MultiArray::index index_t;
        auto shape_ = shape_from_multi_array(array);
        std::vector<index_t> shape {static_cast<index_t>(shape_[0]), static_cast<index_t>(shape_[1]) };
        std::stringstream message;
        message << "[";
        for (index_t i = 0; i < shape[0]; i++) {
            message << "[";
            for (index_t j = 0; j < shape[1]; j++) {
                message << static_cast<T1>(array[i][j]);
                if (j != shape[1] - 1) message << ", ";
            }
            message << "]";
            if (i != shape[0] -1) message << "," << std::endl;
        }
        message << "]";
        return message.str();
    }
}

namespace sqsgenerator::utils {

        template<typename T>
        inline
        bool is_close(T a, T b, T atol=1.0e-5, T rtol=1.0e-8) {
            return std::abs(a - b) <= (atol + rtol * std::abs(b));
        }

        template<typename R, typename P>
        inline R factorial(P p) {
            R r {1}, c {1};
            for (; c <= p; c += 1 ) r *= c;
            return r;
        }

        template<typename T>
        T identity(T &t) {
            return t;
        }

        template<typename T>
        T round_nplaces(T value, uint8_t to)
        {
            T places = std::pow(10.0, to);
            return std::round(value * places) / places;
        }

        template<typename T>
        int get_index(std::vector<T> v, T el)
        {
            auto it = std::find(v.begin(), v.end(), el);
            return  it != v.end() ? it - v.begin() : -1;
        }

        template<typename TOut, typename TIn>
        std::vector<TOut> apply(const std::vector<TIn> &input, std::function<TOut(TIn)> f) {
            std::vector<TOut> result;
            std::transform(input.begin(),  input.end(), std::back_inserter(result), f);
            return result;
        }

        template<typename T>
        std::vector<size_t> argsort(const std::vector<T> &array) {
            std::vector<size_t> indices(array.size());
            std::iota(indices.begin(), indices.end(), 0);
            std::sort(indices.begin(), indices.end(),
                      [&array](int left, int right) -> bool {
                          // sort indices according to corresponding array element
                          return array[left] < array[right];
                      });

            return indices;
        }

        template<typename T>
        std::vector<T> rearrange(const std::vector<T> &input, const std::vector<size_t> &by) {
            return apply<T, size_t>(by, [&input](const size_t index) -> T { return input[index]; });
        }

    template<typename Iterable>
    class enumerate_object
    {
    private:
        Iterable _iter;
        std::size_t _size;
        decltype(std::begin(_iter)) _begin;
        const decltype(std::end(_iter)) _end;

        public:
            enumerate_object(Iterable iter):
                    _iter(iter),
                    _size(0),
                    _begin(std::begin(iter)),
                    _end(std::end(iter))
            {}

            const enumerate_object& begin() const { return *this; }
            const enumerate_object& end()   const { return *this; }

            bool operator!=(const enumerate_object&) const
            {
                return _begin != _end;
            }

            void operator++()
            {
                ++_begin;
                ++_size;
            }

            auto operator*() const-> std::pair<std::size_t, decltype(*_begin)>
            {
                return { _size, *_begin };
            }
        };

        template<typename Iterable>
        auto enumerate(Iterable&& iter)-> enumerate_object<Iterable>
        {
            return { std::forward<Iterable>(iter) };
        }

        configuration_t unique_species(configuration_t conf);
        std::vector<size_t> configuration_histogram(const configuration_t &conf);
        std::tuple<configuration_t, configuration_t> pack_configuration(const configuration_t &configuration);
        configuration_t unpack_configuration(const configuration_t &indices, const configuration_t &packed);
        std::tuple<arrangement_t, arrangement_t, configuration_t, shuffling_bounds_t> build_configuration(const configuration_t &initial, const composition_t &composition);

        // random number generator
        uint32_t random_bounded(uint32_t range, uint64_t *seed);
        void shuffle_configuration(configuration_t &configuration, uint64_t *seed, const shuffling_bounds_t &bounds);
};

#endif //SQSGENERATOR_UTILS_HPP


/***************************************************************************
 * Copyright 1998-2020 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxCoreRender.                                   *
 *                                                                         *
 * 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 <stdexcept>
#include <boost/foreach.hpp>
#include <boost/regex.hpp>

#include "slg/film/film.h"
#include "slg/kernels/kernels.h"
#include "slg/film/imagepipeline/plugins/gammacorrection.h"
#include "luxrays/kernels/kernels.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Gamma correction plugin
//------------------------------------------------------------------------------

BOOST_CLASS_EXPORT_IMPLEMENT(slg::GammaCorrectionPlugin)

GammaCorrectionPlugin::GammaCorrectionPlugin(const float g, const u_int tableSize) {
	gamma = g;

	gammaTable.resize(tableSize, 0.f);
	float x = 0.f;
	const float dx = 1.f / tableSize;
	for (u_int i = 0; i < tableSize; ++i, x += dx)
		gammaTable[i] = powf(Clamp(x, 0.f, 1.f), 1.f / g);

	hardwareDevice = nullptr;
	hwGammaTable = nullptr;
	applyKernel = nullptr;
}

GammaCorrectionPlugin::~GammaCorrectionPlugin() {
	delete applyKernel;

	if (hardwareDevice)
		hardwareDevice->FreeBuffer(&hwGammaTable);
}

ImagePipelinePlugin *GammaCorrectionPlugin::Copy() const {
	return new GammaCorrectionPlugin(gamma, gammaTable.size());
}

float GammaCorrectionPlugin::Radiance2PixelFloat(const float x) const {
	// Very slow !
	//return powf(Clamp(x, 0.f, 1.f), 1.f / 2.2f);

	const u_int tableSize = gammaTable.size();
	const int index = Clamp<int>(Floor2UInt(tableSize * Clamp(x, 0.f, 1.f)), 0, tableSize - 1);
	return gammaTable[index];
}

//------------------------------------------------------------------------------
// CPU version
//------------------------------------------------------------------------------

void GammaCorrectionPlugin::Apply(Film &film, const u_int index) {
	Spectrum *pixels = (Spectrum *)film.channel_IMAGEPIPELINEs[index]->GetPixels();
	const u_int pixelCount = film.GetWidth() * film.GetHeight();

	const bool hasPN = film.HasChannel(Film::RADIANCE_PER_PIXEL_NORMALIZED);
	const bool hasSN = film.HasChannel(Film::RADIANCE_PER_SCREEN_NORMALIZED);

	#pragma omp parallel for
	for (
			// Visual C++ 2013 supports only OpenMP 2.5
#if _OPENMP >= 200805
			unsigned
#endif
			int i = 0; i < pixelCount; ++i) {
		if (film.HasSamples(hasPN, hasSN, i)) {
			pixels[i].c[0] = Radiance2PixelFloat(pixels[i].c[0]);
			pixels[i].c[1] = Radiance2PixelFloat(pixels[i].c[1]);
			pixels[i].c[2] = Radiance2PixelFloat(pixels[i].c[2]);
		}
	}
}

//------------------------------------------------------------------------------
// OpenCL version
//------------------------------------------------------------------------------

void GammaCorrectionPlugin::ApplyHW(Film &film, const u_int index) {
	if (!applyKernel) {
		film.ctx->SetVerbose(true);

		hardwareDevice = film.hardwareDevice;

		// Allocate buffers
		hardwareDevice->AllocBufferRO(&hwGammaTable, &gammaTable[0], gammaTable.size() * sizeof(float), "Gamma table");

		// Compile sources
		const double tStart = WallClockTime();

		vector<string> opts;
		opts.push_back("-D LUXRAYS_OPENCL_KERNEL");
		opts.push_back("-D SLG_OPENCL_KERNEL");

		HardwareDeviceProgram *program = nullptr;
		hardwareDevice->CompileProgram(&program,
				opts,
				luxrays::ocl::KernelSource_utils_funcs +
				slg::ocl::KernelSource_plugin_gammacorrection_funcs,
				"GammaCorrectionPlugin");

		SLG_LOG("[GammaCorrectionPlugin] Compiling GammaCorrectionPlugin_Apply Kernel");
		hardwareDevice->GetKernel(program, &applyKernel, "GammaCorrectionPlugin_Apply");

		delete program;

		// Set kernel arguments
		u_int argIndex = 0;
		hardwareDevice->SetKernelArg(applyKernel, argIndex++, film.GetWidth());
		hardwareDevice->SetKernelArg(applyKernel, argIndex++, film.GetHeight());
		hardwareDevice->SetKernelArg(applyKernel, argIndex++, film.hw_IMAGEPIPELINE);
		hardwareDevice->SetKernelArg(applyKernel, argIndex++, hwGammaTable);
		hardwareDevice->SetKernelArg(applyKernel, argIndex++, (u_int)gammaTable.size());

		const double tEnd = WallClockTime();
		SLG_LOG("[GammaCorrectionPlugin] Kernels compilation time: " << int((tEnd - tStart) * 1000.0) << "ms");

		film.ctx->SetVerbose(false);
	}

	hardwareDevice->EnqueueKernel(applyKernel, HardwareDeviceRange(RoundUp(film.GetWidth() * film.GetHeight(), 256u)),
			HardwareDeviceRange(256));
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing indeg capabilities

    convert radian to degree.

    @par Semantic:

    For every parameter of floating type

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

    is similar to:

    @code
    auto r = Radindeg*x;
    @endcode

    @see  inrad, Radindeg, Radindegr, Deginrad

  **/
  Value indeg(Value const & v0);
} }
#endif

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

#endif


#ifndef CY_NNQS_UTILITY_HPP
#define CY_NNQS_UTILITY_HPP
#include <memory>
#include <random>
#include <utility>

#include <Eigen/Dense>
#include "Utilities/type_traits.hpp"

namespace yannq
{

//! stable implementation of log(cosh(x)) for real x
template<typename T>
inline typename std::enable_if<!is_complex_type<T>::value, T>::type logCosh(T x)
{
    const T xp = std::abs(x);
    if (xp <= 12.) {
        return std::log(std::cosh(xp));
    } else {
        const static T log2v = std::log(2.);
        return xp - log2v;
    }
}

//! stable implementation of log(cosh(x)) for complex x
template<typename T>
inline typename std::enable_if<is_complex_type<T>::value, T>::type logCosh(T x)
{
    const auto xr = x.real();
    const auto xi = x.imag();

    T res = logCosh(xr);
    res += std::log(T{std::cos(xi), std::tanh(xr) * std::sin(xi)});

    return res;
}

template<typename T>
typename std::enable_if<!is_complex_type<T>::value, T>::type real(const T& v)
{
	return v;
}

/*
template<typename StateT, class AuxData, class Container, class Observable>
auto calcObs(const AuxData& ad, const Container& sr, Observable& obs)
	-> typename std::result_of<Observable(StateT)>::type
{
	using ResultType = typename std::result_of<Observable(StateT)>::type;
	ResultType res{};
	for(const auto& elt: sr)
	{
		auto s = construct_state<StateT>(ad, make_rtuple(elt));
		res += obs(s);
	}
	res /= sr.size();
	return res;
}
*/

//! generate a random vector in the computational basis
template <typename RandomEngine>
Eigen::VectorXi randomSigma(int n, RandomEngine& re)
{
	Eigen::VectorXi sigma(n);
	std::uniform_int_distribution<> uid(0, 1);
	//randomly initialize currSigma
	for(int i = 0; i < n; i++)
	{
		sigma(i) = -2*uid(re)+1;
	}
	return sigma;
}
//! generate a random vector in the computational basis with the constraint that
//! the number |1>=|\sigma_z = -1> is nup.
template <typename RandomEngine>
Eigen::VectorXi randomSigma(int n, int nup, RandomEngine& re)
{
	std::vector<int> sigma(n,-1);
	//randomly initialize currSigma
	for(int i = nup; i < n; i++)
	{
		sigma[i] = 1;
	}
	std::shuffle(sigma.begin(), sigma.end(), re);
	return Eigen::Map<Eigen::VectorXi>(sigma.data(), n);
}

//! generate a vector that the binary representation is val.
Eigen::VectorXi toSigma(int length, uint32_t val)
{
	Eigen::VectorXi res(length);
	for(int i = 0; i < length; i++)
	{
		res(i) = 1-2*((val >> i) & 1);
	}
	return res;
}

//! returns the binary representation of sigma.
long long int toValue(const Eigen::VectorXi& sigma)
{
	long long int res = 0;
	for(int i = 0; i < sigma.size(); i++)
	{
		res += (1 << i)*((1-sigma(i))/2);
	}
	return res;
}

//! for complex type T, generate a vector filled with samples from normal distribution.
template<typename T, class RandomEngine, typename std::enable_if<is_complex_type<T>::value, int>::type = 0 >
Eigen::Matrix<T, Eigen::Dynamic, 1> randomVector(RandomEngine&& re,
		remove_complex_t<T> sigma, std::size_t nelt)
{
	Eigen::Matrix<T, Eigen::Dynamic, 1> res(nelt);
	std::normal_distribution<typename remove_complex<T>::type> dist(0.0, sigma);
	for(std::size_t i = 0; i < nelt; i++)
	{
		res(i) = T{dist(re),dist(re)};
	}
	return res;
}
//! for real type T, generate a vector filled with samples from normal distribution.
template<typename T, class RandomEngine, typename std::enable_if<!is_complex_type<T>::value, int>::type = 0 >
Eigen::Matrix<T, Eigen::Dynamic, 1> randomVector(RandomEngine&& re,
		remove_complex_t<T> sigma, std::size_t nelt)
{
	Eigen::Matrix<T, Eigen::Dynamic, 1> res(nelt);
	std::normal_distribution<T> dist(0.0, sigma);
	for(std::size_t i = 0; i < nelt; i++)
	{
		res(i) = dist(re);
	}
	return res;
}

}//namespace yannq

#endif//CY_NNQS_UTILITY_HPP


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

#include <boost/hana/range.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>

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

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

int main() {
    BOOST_HANA_CONSTEXPR_ASSERT(unpack(range(int_<0>, int_<0>), f) == f());
    BOOST_HANA_CONSTEXPR_ASSERT(unpack(range(int_<0>, int_<1>), f) == f(int_<0>));
    BOOST_HANA_CONSTEXPR_ASSERT(unpack(range(int_<0>, int_<2>), f) == f(int_<0>, int_<1>));
    BOOST_HANA_CONSTEXPR_ASSERT(unpack(range(int_<0>, int_<3>), f) == f(int_<0>, int_<1>, int_<2>));

    BOOST_HANA_CONSTANT_ASSERT(length(range(int_<0>, int_<0>)) == int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(length(range(int_<0>, int_<1>)) == int_<1>);
    BOOST_HANA_CONSTANT_ASSERT(length(range(int_<0>, int_<2>)) == int_<2>);
    BOOST_HANA_CONSTANT_ASSERT(length(range(int_<4>, int_<4>)) == int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(length(range(int_<4>, int_<10>)) == int_<6>);
}


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

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

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_ANDOYER_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_ANDOYER_HPP

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/geographic/detail/ellipsoid.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Point-point distance approximation taking flattening into account
\ingroup distance
\tparam RadiusType Type of specified radius of the Earth
\tparam CalculationType \tparam_calculation
\author After Andoyer, 19xx, republished 1950, republished by Meeus, 1999
\note Although not so well-known, the approximation is very good: in all cases the results
are about the same as Vincenty. In my (Barend's) testcases the results didn't differ more than 6 m
\see http://nacc.upc.es/tierra/node16.html
\see http://sci.tech-archive.net/Archive/sci.geo.satellite-nav/2004-12/2724.html
\see http://home.att.net/~srschmitt/great_circle_route.html (implementation)
\see http://www.codeguru.com/Cpp/Cpp/algorithms/article.php/c5115 (implementation)
\see http://futureboy.homeip.net/frinksamp/navigation.frink (implementation)
\see http://www.voidware.com/earthdist.htm (implementation)
*/
template
<
    typename RadiusType,
    typename CalculationType = void
>
class andoyer
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef RadiusType radius_type;

    inline andoyer()
        : m_ellipsoid()
    {}

    explicit inline andoyer(RadiusType f)
        : m_ellipsoid(f)
    {}

    explicit inline andoyer(geometry::detail::ellipsoid<RadiusType> const& e)
        : m_ellipsoid(e)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return calc<typename calculation_type<Point1, Point2>::type>
            (
                get_as_radian<0>(point1), get_as_radian<1>(point1),
                get_as_radian<0>(point2), get_as_radian<1>(point2)
            );
    }

    inline geometry::detail::ellipsoid<RadiusType> ellipsoid() const
    {
        return m_ellipsoid;
    }

    inline RadiusType radius() const
    {
        return m_ellipsoid.a();
    }

private :
    geometry::detail::ellipsoid<RadiusType> m_ellipsoid;

    template <typename CT, typename T>
    inline CT calc(T const& lon1,
                T const& lat1,
                T const& lon2,
                T const& lat2) const
    {
        CT const G = (lat1 - lat2) / 2.0;
        CT const lambda = (lon1 - lon2) / 2.0;

        if (geometry::math::equals(lambda, 0.0)
            && geometry::math::equals(G, 0.0))
        {
            return 0.0;
        }

        CT const F = (lat1 + lat2) / 2.0;

        CT const sinG2 = math::sqr(sin(G));
        CT const cosG2 = math::sqr(cos(G));
        CT const sinF2 = math::sqr(sin(F));
        CT const cosF2 = math::sqr(cos(F));
        CT const sinL2 = math::sqr(sin(lambda));
        CT const cosL2 = math::sqr(cos(lambda));

        CT const S = sinG2 * cosL2 + cosF2 * sinL2;
        CT const C = cosG2 * cosL2 + sinF2 * sinL2;

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;
        CT const c3 = 3;

        if (geometry::math::equals(S, c0) || geometry::math::equals(C, c0))
        {
            return c0;
        }

        CT const omega = atan(sqrt(S / C));
        CT const r3 = c3 * sqrt(S * C) / omega; // not sure if this is r or greek nu
        CT const D = c2 * omega * m_ellipsoid.a();
        CT const H1 = (r3 - c1) / (c2 * C);
        CT const H2 = (r3 + c1) / (c2 * S);
        CT const f = m_ellipsoid.f();

        return D * (c1 + f * H1 * sinF2 * cosG2 - f * H2 * cosF2 * sinG2);
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename RadiusType, typename CalculationType>
struct tag<andoyer<RadiusType, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct return_type<andoyer<RadiusType, CalculationType>, P1, P2>
    : andoyer<RadiusType, CalculationType>::template calculation_type<P1, P2>
{};

template <typename RadiusType, typename CalculationType>
struct comparable_type<andoyer<RadiusType, CalculationType> >
{
    typedef andoyer<RadiusType, CalculationType> type;
};

template <typename RadiusType, typename CalculationType>
struct get_comparable<andoyer<RadiusType, CalculationType> >
{
    static inline andoyer<RadiusType, CalculationType> apply(andoyer<RadiusType, CalculationType> const& input)
    {
        return input;
    }
};

template <typename RadiusType, typename CalculationType, typename P1, typename P2>
struct result_from_distance<andoyer<RadiusType, CalculationType>, P1, P2>
{
    template <typename T>
    static inline typename return_type<andoyer<RadiusType, CalculationType>, P1, P2>::type
        apply(andoyer<RadiusType, CalculationType> const& , T const& value)
    {
        return value;
    }
};

template <typename Point1, typename Point2>
struct default_strategy<point_tag, Point1, Point2, geographic_tag, geographic_tag>
{
    typedef strategy::distance::andoyer<typename select_coordinate_type<Point1, Point2>::type> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_ANDOYER_HPP


#include <votca/ctp/ewald3d.h>
#include <votca/tools/globals.h>
#include <boost/format.hpp>
#include <algorithm>

using boost::format;

namespace votca { namespace ctp {

Ewald3D3D::~Ewald3D3D() { ; }

Ewald3D3D::Ewald3D3D(Topology *top, PolarTop *ptop, Property *opt, Logger *log)
  : Ewald3DnD(top, ptop, opt, log) {}

EWD::triple<> Ewald3D3D::ConvergeReciprocalSpaceSum(vector<PolarSeg*> &target) {

    const double int2eV = votca::tools::globals::conversion::int2eV;

    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;

    // CELLS OF THE RECIPROCAL LATTICE: SUM OVER ELLIPSOIDAL SHELLS
    // ... Shell-size increment is magnitude of largest reciprocal cell vector
    // ... Tschebyschow/Euclidean norm is used to group k-vectors into k-shells

    vector< vector<vec> > shell_ks;
    vector< vector<vec> >::iterator shellit;
    double shell_dk = (maxnorm(_A) > maxnorm(_B)) ?
        ((maxnorm(_A) > maxnorm(_C)) ? maxnorm(_A) : maxnorm(_C))
      : ((maxnorm(_B) > maxnorm(_C)) ? maxnorm(_B) : maxnorm(_C));
    // Determine all k-vectors within k-space cut-off
    vector< vec >::iterator kit;
    vector< vec > ks;
    for (int kx = -_NA_max; kx < _NA_max+1; ++kx) {
        for (int ky = -_NB_max; ky < _NB_max+1; ++ky) {
            for (int kz = -_NC_max; kz < _NC_max+1; ++kz) {
                if (kx == 0 && ky == 0 && kz == 0) continue;
                vec k = kx*_A + ky*_B + kz*_C;
                if (maxnorm(k) > _K_co) continue;
                ks.push_back(k);
            }
        }
    }
    // Sort according to magnitude
    std::sort(ks.begin(), ks.end(), _eucsort);
    // Group into shells
    int shell_idx = 0;
    kit = ks.begin();
    shell_ks.resize(int(_K_co/shell_dk+0.5)+1);
    double shell_k = shell_dk;
    while (shell_k <= _K_co) {
        for ( ; kit < ks.end(); ++kit) {
            vec k = *kit;
            if (maxnorm(k) <= shell_k) {
                // Add to current shell
                shell_ks[shell_idx].push_back(k);
            }
            else {
                // Open new shell
                shell_k += shell_dk;
                shell_idx += 1;
                shell_ks[shell_idx].push_back(k);
                break;
            }
        }
        // All k-vectors consumed?
        if (kit == ks.end()) break;
    }
//    for (int i = 0; i < shell_ks.size(); ++i) {
//        std::ofstream ofs;
//        string outfile = (format("shell_%1$d.out") % (i+1)).str();
//        ofs.open(outfile.c_str(), ofstream::out);
//        for (kit = shell_ks[i].begin(); kit < shell_ks[i].end(); ++kit) {
//            ofs << (*kit).getX() << " " << (*kit).getY() << " " << (*kit).getZ() << endl;
//        }
//        ofs.close();
//    }

    // K=K TERM
    CTP_LOG(logDEBUG,*_log) << flush;
    double EKK_fgC_bgP = 0.0;
    unsigned int N_EKK_memory = (unsigned int)(0.5*(_NA_max+_NB_max)+0.5);
    int N_K_proc = 0;
    int N_shells_proc = 0;
    vector< double > dEKKs;
    _converged_K = false;

    double re_E = 0.0;
    double im_E = 0.0;

    for (shellit = shell_ks.begin(); shellit < shell_ks.end(); ++shellit, ++N_shells_proc) {

        for (kit = (*shellit).begin(); kit < (*shellit).end(); ++kit, ++N_K_proc) {
            vec k = *kit;

            // K-DEPENDENT FACTOR
            double K = abs(k);
            double expkk_k = 4*M_PI*exp(-K*K/(4*_alpha*_alpha)) / (K*K);

            CTP_LOG(logDEBUG,*_log)
                << (format("k[%5$d] = %1$+1.3f %2$+1.3f %3$+1.3f   |K| = %4$+1.3f 1/nm")
                % (k.getX()) % (k.getY()) % (k.getZ()) % K % (N_shells_proc+1));

            // STRUCTURE FACTORS
            // Calculate structure factor S(k) for FGC
            double qcos_fgC = 0.0;
            double qsin_fgC = 0.0;
            for (sit = target.begin(); sit < target.end(); ++sit) {
                for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
                    qcos_fgC += (*pit)->Q00 * cos(k * (*pit)->getPos());
                    qsin_fgC += (*pit)->Q00 * sin(k * (*pit)->getPos());
                }
            }
            // Calculate structure factor S(-k) for BGP
            double qcos_bgP = 0.0;
            double qsin_bgP = 0.0;
            for (sit = _bg_P.begin(); sit < _bg_P.end(); ++sit) {
                for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
                    qcos_bgP += (*pit)->Q00 * cos(-k * (*pit)->getPos());
                    qsin_bgP += (*pit)->Q00 * sin(-k * (*pit)->getPos());
                }
            }
            // Structure-factor product
            double re_s1s2 = qcos_fgC*qcos_bgP - qsin_fgC*qsin_bgP;
            double im_s1s2 = qsin_fgC*qcos_bgP + qcos_fgC*qsin_bgP;

            // REAL & IMAGINARY ENERGY
            double re_dE = expkk_k * re_s1s2;
            double im_dE = expkk_k * im_s1s2;
            re_E += re_dE;
            im_E += im_dE;

            CTP_LOG(logDEBUG,*_log)
                << (format("    Re(dE) = %1$+1.7f")
                % (re_dE/_LxLyLz*int2eV));

            CTP_LOG(logDEBUG,*_log)
                << (format("    Re(E) = %1$+1.7f Im(E) = %2$+1.7f")
                % (re_E/_LxLyLz*int2eV) % (im_E/_LxLyLz*int2eV));

            // CONVERGED?
            double dEKK = sqrt(re_dE*re_dE + im_dE*im_dE);
            double dEKK_rms = 0.0;
            if (dEKKs.size() < N_EKK_memory) {
                dEKKs.resize(N_EKK_memory,dEKK);
            }
            else {
                dEKKs[N_K_proc % N_EKK_memory] = dEKK;
            }
            for (unsigned int i = 0; i < dEKKs.size(); ++i) {
                dEKK_rms += dEKKs[i]*dEKKs[i];
            }
            dEKK_rms /= dEKKs.size();
            dEKK_rms = sqrt(dEKK_rms);

            CTP_LOG(logDEBUG,*_log)
                << (format("   RMS(%2$d) = %1$+1.7f")
                % (dEKK_rms/_LxLyLz*int2eV) % N_EKK_memory) << flush;

            if (dEKK_rms/_LxLyLz*int2eV <= _crit_dE && N_K_proc > 2 && N_shells_proc > 0) {
                _converged_K = true;
                CTP_LOG(logDEBUG,*_log)
                    << (format(":::: Converged to precision as of |K| = %1$+1.3f 1/nm")
                    % K ) << flush;
                break;
            }
        } // Sum over k's in k-shell
        if (_converged_K) break;
    } // Sum over k-shells

    EKK_fgC_bgP = re_E/_LxLyLz;
    return EWD::triple<>(EKK_fgC_bgP,0,0);
}

EWD::triple<> Ewald3D3D::CalculateShapeCorrection(vector<PolarSeg*> &target) {

    vector<PolarSeg*>::iterator sit1;
    vector<APolarSite*> ::iterator pit1;
    vector<PolarSeg*>::iterator sit2;
    vector<APolarSite*> ::iterator pit2;

    double EJ = 0.0;

    if (_shape == "xyslab") {
        // DIRECT CALCULATION VIA DOUBLE LOOP
        // TODO The double-loop can be avoided, but direct summation as below
        //      appears to be more stable from a numerical point of view
        for (sit1 = target.begin(); sit1 < target.end(); ++sit1) {
           for (sit2 = _bg_P.begin(); sit2 < _bg_P.end(); ++sit2) {
              for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                 for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
                    double za = (*pit1)->getPos().getZ();
                    double zb = (*pit2)->getPos().getZ();
                    EJ += (za-zb)*(za-zb) * (*pit1)->getQ00()*(*pit2)->getQ00();
                 }
              }
           }
        }

        //    // DECOMPOSITION INTO CHARGE-DENSITY MULTIPOLE MOMENTS
        //    vec DA = vec(0,0,0);
        //    vec DA = vec(0,0,0);
        //    double QA = 0.0;
        //    double DZZBG = 0.0;
        //    for (sit1 = target.begin(); sit1 < target.end(); ++sit1) {
        //        for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
        //            DA += (*pit1)->getQ00() * (*pit1)->getPos();
        //            QA += (*pit1)->getQ00();
        //        }
        //    }
        //    for (sit1 = _bg_P.begin(); sit1 < _bg_P.end(); ++sit1) {
        //        for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
        //            double zb = (*pit1)->getPos().getZ();
        //            DB += (*pit1)->getQ00() * (*pit1)->getPos();
        //            DZZB += (*pit1)->getQ00() * zb*zb;
        //        }
        //    }
        //    EJ = QA*DZZB - 2*(DA.getZ())*(DB.getZ());

        EJ *= - 2*M_PI/_LxLyLz;
    }
    else {
        CTP_LOG(logERROR,*_log)
            << (format("Shape %1$s not implemented. Setting EJ = 0.0 ...")
            % _shape) << flush;
        EJ = 0.0;
    }

    return EWD::triple<>(EJ,0,0);
}

double Ewald3D3D::CalculateSq2(vec &k) {
    vector<PolarSeg*>::iterator sit;
    vector<APolarSite*> ::iterator pit;
    double cs = 0.0;
    double ss = 0.0;
    for (sit = _bg_P.begin(); sit < _bg_P.end(); ++sit) {
        for (pit = (*sit)->begin(); pit < (*sit)->end(); ++pit) {
            cs += (*pit)->Q00 * cos(k * (*pit)->getPos());
            ss += (*pit)->Q00 * sin(k * (*pit)->getPos());
        }
    }
    return cs*cs + ss*ss;
}

}}


#define DEBUG 1
/**
 * File    : D.cpp
 * Author  : Kazune Takahashi
 * Created : 2020/1/18 21:28:34
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
ll gcd(ll x, ll y) { return y ? gcd(y, x % y) : x; }
ll lcm(ll x, ll y) { return x / gcd(x, y) * y; }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
constexpr int infty{100000};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- SegTree -----

template <typename T>
class SegTree
{ // 0-indexed, [0, N).
private:
  int N;
  vector<T> dat;
  T unit;  // モノイドの単位元
  T(*func) // モノイドの演算
  (T, T);
  T(*_update) // update で値をどうするか書く
  (T, T);

public:
  SegTree() {}

  SegTree(int n, T unit, T (*func)(T, T), T (*_update)(T, T)) : N{1}, unit{unit}, func{func}, _update{_update}
  {
    while (N < n)
    {
      N *= 2;
    }
    dat = vector<T>(2 * N - 1, unit);
  }

  void update(int k, T a)
  {
    k += N - 1;
    dat[k] = _update(dat[k], a);
    while (k > 0)
    {
      k = (k - 1) / 2;
      dat[k] = func(dat[k * 2 + 1], dat[k * 2 + 2]);
    }
  }

private:
  T find(int a, int b, int k, int l, int r)
  {
    if (r <= a || b <= l)
    {
      return unit;
    }
    if (a <= l && r <= b)
    {
      return dat[k];
    }
    T vl = find(a, b, k * 2 + 1, l, (l + r) / 2);
    T vr = find(a, b, k * 2 + 2, (l + r) / 2, r);
    return func(vl, vr);
  }

public:
  T find(int a, int b)
  { // [a, b) の find をする。
    return find(a, b, 0, 0, N);
  }
};

// ----- frequently used examples -----

// for +
auto func2 = [](auto x, auto y) {
  return x + y;
};
auto _update2 = [](auto x, auto y) {
  return y;
};
constexpr ll unit2{0LL};

// ----- main() -----

struct Card
{
  int A, B;
};

using Info = tuple<int, int>;

int main()
{
  int N;
  cin >> N;
  vector<Card> V(N);
  for (auto i = 0; i < N; ++i)
  {
    cin >> V[i].A;
  }
  for (auto i = 0; i < N; ++i)
  {
    cin >> V[i].B;
  }
  for (auto i = 1; i < N; i += 2)
  {
    swap(V[i].A, V[i].B);
  }
  if (N == 1)
  {
    cout << 0 << endl;
    return 0;
  }
  int ans{infty};
  for (auto k = 0; k < (1 << N); ++k)
  {
    int cnt{0};
    for (auto i = 0; i < N; ++i)
    {
      cnt += (k >> i) & 1;
    }
    if (cnt == N - N / 2)
    {
      vector<Info> X, Y;
      for (auto i = 0; i < N; ++i)
      {
        if ((k >> i) & 1)
        {
          X.push_back(Info(V[i].A, i));
        }
        else
        {
          Y.push_back(Info(V[i].B, i));
        }
      }
      sort(X.begin(), X.end());
      sort(Y.begin(), Y.end());
      vector<Info> Z;
      int x = 0, y = 0;
      while (x < static_cast<int>(X.size()))
      {
        Z.push_back(X[x]);
        ++x;
        if (y < static_cast<int>(Y.size()))
        {
          Z.push_back(Y[y]);
          ++y;
        }
      }
      assert(static_cast<int>(Z.size()) == N);
      bool ok{true};
      for (auto i = 0; i < N - 1; ++i)
      {
        if (get<0>(Z[i]) > get<0>(Z[i + 1]))
        {
          ok = false;
          break;
        }
      }
      if (!ok)
      {
        continue;
      }
#if DEBUG == 1
      for (auto i = 0; i < N; ++i)
      {
        cerr << "Z[" << i << "] = (" << get<0>(Z[i]) << ", " << get<1>(Z[i]) << ")" << endl;
      }
#endif
      SegTree<ll> tree{N, unit2, func2, _update2};
      int tmp{0};
      for (auto i = 0; i < N; ++i)
      {
        int v{get<1>(Z[i])};
        tmp += tree.find(v, N);
        tree.update(v, 1);
      }
      ch_min(ans, tmp);
    }
  }
  if (ans == infty)
  {
    cout << -1 << endl;
  }
  else
  {
    cout << ans << endl;
  }
}


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_CORE_RADIAN_ACCESS_HPP
#define BOOST_GEOMETRY_CORE_RADIAN_ACCESS_HPP

#include <cstddef>

#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/coordinate_type.hpp>

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template<std::size_t Dimension, typename Geometry>
struct degree_radian_converter
{
    typedef typename fp_coordinate_type<Geometry>::type coordinate_type;

    static inline coordinate_type get(Geometry const& geometry)
    {
        return boost::numeric_cast
            <
                coordinate_type
            >(geometry::get<Dimension>(geometry) * geometry::math::d2r);
    }

    static inline void set(Geometry& geometry, coordinate_type const& radians)
    {
        geometry::set<Dimension>(geometry, boost::numeric_cast
            <
                coordinate_type
            >(radians * geometry::math::r2d));
    }

};

// Default, radian (or any other coordinate system) just works like "get"
template <std::size_t Dimension, typename Geometry, typename DegreeOrRadian>
struct radian_access
{
    typedef typename fp_coordinate_type<Geometry>::type coordinate_type;

    static inline coordinate_type get(Geometry const& geometry)
    {
        return geometry::get<Dimension>(geometry);
    }

    static inline void set(Geometry& geometry, coordinate_type const& radians)
    {
        geometry::set<Dimension>(geometry, radians);
    }
};

// Specialize, any "degree" coordinate system will be converted to radian
// but only for dimension 0,1 (so: dimension 2 and heigher are untouched)

template
<
    typename Geometry,
    template<typename> class CoordinateSystem
>
struct radian_access<0, Geometry, CoordinateSystem<degree> >
    : degree_radian_converter<0, Geometry>
{};

template
<
    typename Geometry,
    template<typename> class CoordinateSystem
>
struct radian_access<1, Geometry, CoordinateSystem<degree> >
    : degree_radian_converter<1, Geometry>
{};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief get coordinate value of a point, result is in Radian
\details Result is in Radian, even if source coordinate system
    is in Degrees
\return coordinate value
\ingroup get
\tparam Dimension dimension
\tparam Geometry geometry
\param geometry geometry to get coordinate value from
\note Only applicable to coordinate systems templatized by units,
    e.g. spherical or geographic coordinate systems
*/
template <std::size_t Dimension, typename Geometry>
inline typename fp_coordinate_type<Geometry>::type get_as_radian(Geometry const& geometry)
{
    return detail::radian_access<Dimension, Geometry,
            typename coordinate_system<Geometry>::type>::get(geometry);
}

/*!
\brief set coordinate value (in radian) to a point
\details Coordinate value will be set correctly, if coordinate system of
    point is in Degree, Radian value will be converted to Degree
\ingroup set
\tparam Dimension dimension
\tparam Geometry geometry
\param geometry geometry to assign coordinate to
\param radians coordinate value to assign
\note Only applicable to coordinate systems templatized by units,
    e.g. spherical or geographic coordinate systems
*/
template <std::size_t Dimension, typename Geometry>
inline void set_from_radian(Geometry& geometry,
            typename fp_coordinate_type<Geometry>::type const& radians)
{
    detail::radian_access<Dimension, Geometry,
            typename coordinate_system<Geometry>::type>::set(geometry, radians);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_CORE_RADIAN_ACCESS_HPP


#include "timer.hpp"
#include "read_map.hpp"
#include <jps_basis/data_utils.h>
#include <jps_planner/jps_planner/jps_planner.h>
#include <jps_planner/distance_map_planner/distance_map_planner.h>

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

using namespace JPS;

int main(int argc, char ** argv){
  if(argc != 2) {
    printf(ANSI_COLOR_RED "Input yaml required!\n" ANSI_COLOR_RESET);
    return -1;
  }

  // Read the map from yaml
  MapReader<Vec2i, Vec2f> reader(argv[1], true); // Map read from a given file
  if(!reader.exist()) {
    printf(ANSI_COLOR_RED "Cannot read input file [%s]!\n" ANSI_COLOR_RESET, argv[1]);
    return -1;
  }

  // store map in map_util
  std::shared_ptr<OccMapUtil> map_util = std::make_shared<OccMapUtil>();
  map_util->setMap(reader.origin(), reader.dim(), reader.data(), reader.resolution());

  const Vec2f start(reader.start(0), reader.start(1));
  const Vec2f goal(reader.goal(0), reader.goal(1));

  // Set up JPS planner
  JPSPlanner2D jps(false); // Declare a planner
  jps.setMapUtil(map_util); // Set collision checking function
  jps.updateMap();

  // Run JPS planner
  Timer time_jps(true);
  bool valid_jps = jps.plan(start, goal, 1, true);
  double dt_jps = time_jps.Elapsed().count();
  const auto path_jps = jps.getRawPath(); // Get the planned raw path from JPS
  printf("JPS Planner takes: %f ms\n", dt_jps);
  printf("JPS Path Distance: %f\n", total_distance2f(path_jps));

  // Set up DMP planner
  DMPlanner2D dmp(false);
  dmp.setPotentialRadius(Vec2f(1.0, 1.0)); // Set 2D potential field radius
  dmp.setSearchRadius(Vec2f(0.5, 0.5)); // Set the valid search region around given path
  dmp.setMap(map_util, start); // Set map util for collision checking, must be called before planning

  // Run DMP planner
  Timer time_dist(true);
  bool valid_dist = dmp.computePath(start, goal, path_jps); // Compute the path given the jps path
  double dt_dist = time_dist.Elapsed().count();
  const auto path_dist = dmp.getRawPath();
  printf("DMP Planner takes: %f ms\n", dt_dist);
  printf("DMP Path Distance: %f\n", total_distance2f(path_dist));

  // Plot the result in svg image
  typedef boost::geometry::model::d2::point_xy<double> point_2d;
  // Declare a stream and an SVG mapper
  std::ofstream svg("output.svg");
  boost::geometry::svg_mapper<point_2d> mapper(svg, 1000, 1000);

  // Draw the canvas
  boost::geometry::model::polygon<point_2d> bound;
  const Vec2i dim = map_util->getDim();
  const Vec2f ori = map_util->getOrigin();
  const double res = map_util->getRes();

  const double origin_x = ori(0);
  const double origin_y = ori(1);
  const double range_x = dim(0) * res;
  const double range_y = dim(1) * res;
  std::vector<point_2d> points;
  points.push_back(point_2d(origin_x, origin_y));
  points.push_back(point_2d(origin_x, origin_y+range_y));
  points.push_back(point_2d(origin_x+range_x, origin_y+range_y));
  points.push_back(point_2d(origin_x+range_x, origin_y));
  points.push_back(point_2d(origin_x, origin_y));
  boost::geometry::assign_points(bound, points);
  boost::geometry::correct(bound);

  mapper.add(bound);
  mapper.map(bound, "fill-opacity:1.0;fill:rgb(255,255,255);stroke:rgb(0,0,0);stroke-width:2"); // White

  // Draw start and goal
  point_2d start_pt, goal_pt;
  boost::geometry::assign_values(start_pt, start(0), start(1));
  mapper.add(start_pt);
  mapper.map(start_pt, "fill-opacity:1.0;fill:rgb(255,0,0);", 10); // Red
  boost::geometry::assign_values(goal_pt, goal(0), goal(1));
  mapper.add(goal_pt);
  mapper.map(goal_pt, "fill-opacity:1.0;fill:rgb(255,0,0);", 10); // Red

  // Draw the obstacles
  const auto data = dmp.getMapUtil()->getMap();
  for(int x = 0; x < dim(0); x ++) {
    for(int y = 0; y < dim(1); y ++) {
      auto value = data[map_util->getIndex(Vec2i(x,y))];
      if(value > 0) {
        Vec2f pt = map_util->intToFloat(Vec2i(x, y));
        decimal_t occ = (decimal_t) value/100;
        point_2d a;
        boost::geometry::assign_values(a, pt(0), pt(1));
        mapper.add(a);
        if(occ < 1)
          mapper.map(a, "fill-opacity:"+std::to_string(occ/2.0) +";fill:rgb(118,215,234);", 1);
        else
          mapper.map(a, "fill-opacity:1.0;fill:rgb(0,0,0);", 1);
      }
    }
  }

  // Draw searched region
  for (const auto &pt : dmp.getSearchRegion()) {
    point_2d a;
    boost::geometry::assign_values(a, pt(0), pt(1));
    mapper.add(a);
    mapper.map(a, "fill-opacity:0.2;fill:rgb(100,200,100);", 1); // Green
  }

  // Draw the path from JPS
  if(valid_jps) {
    vec_Vec2f path = path_jps;
    boost::geometry::model::linestring<point_2d> line;
    for(auto pt: path)
      line.push_back(point_2d(pt(0), pt(1)));
    mapper.add(line);
    mapper.map(line, "opacity:0.4;fill:none;stroke:rgb(212,0,0);stroke-width:5"); // Red
  }

  // Draw the path from DM
  if(valid_dist) {
    vec_Vec2f path = path_dist;
    boost::geometry::model::linestring<point_2d> line;
    for(auto pt: path)
      line.push_back(point_2d(pt(0), pt(1)));
    mapper.add(line);
    mapper.map(line, "opacity:0.8;fill:none;stroke:rgb(10,10,250);stroke-width:5"); // Blue
  }

  // Write title at the lower right corner on canvas
  mapper.text(point_2d(origin_x + range_x - 11, origin_y+2.4), "test_distance_map_planner_2d",
              "fill-opacity:1.0;fill:rgb(10,10,250);");

  mapper.text(point_2d(origin_x + range_x - 13, origin_y+1.8), "Green: ",
              "fill-opacity:1.0;fill:rgb(100,200,100);");
  mapper.text(point_2d(origin_x + range_x - 10.5, origin_y+1.8), "search region",
              "fill-opacity:1.0;fill:rgb(0,0,0);");

  mapper.text(point_2d(origin_x + range_x - 13, origin_y+1.2), "Red: ",
              "fill-opacity:1.0;fill:rgb(212,0,0);");
  mapper.text(point_2d(origin_x + range_x - 10.5, origin_y+1.2), "original path",
              "fill-opacity:1.0;fill:rgb(0,0,0);");

  mapper.text(point_2d(origin_x + range_x - 13, origin_y+0.6), "Blue:",
              "fill-opacity:1.0;fill:rgb(10,10,250);");
  mapper.text(point_2d(origin_x + range_x - 10.5, origin_y+0.6), "perturbed path",
              "fill-opacity:1.0;fill:rgb(0,0,0);");

  return 0;
}


/* boost random/faure.hpp header file
 *
 * Copyright Justinas Vygintas Daugmaudis 2010-2018
 * 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_RANDOM_FAURE_HPP
#define BOOST_RANDOM_FAURE_HPP

#include <boost/random/detail/qrng_base.hpp>

#include <cmath>
#include <vector>
#include <algorithm>

#include <boost/assert.hpp>

namespace boost {
namespace random {

/** @cond */
namespace detail {

namespace qrng_tables {

// There is no particular reason why 187 first primes were chosen
// to be put into this table. The only reason was, perhaps, that
// the number of dimensions for Faure generator would be around
// the same order of magnitude as the number of dimensions supported
// by the Sobol qrng.
struct primes
{
  typedef unsigned short value_type;

  BOOST_STATIC_CONSTANT(int, number_of_primes = 187);

  // A function that returns lower bound prime for a given n
  static value_type lower_bound(std::size_t n)
  {
    static const value_type prim_a[number_of_primes] = {
      2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 43, 47, 53,
      59, 61, 67, 71, 73, 79, 83, 89, 97, 101, 103, 107, 109, 113,
      127, 131, 137, 139, 149, 151, 157, 163, 167, 173, 179, 181,
      191, 193, 197, 199, 211, 223, 227, 229, 233, 239, 241, 251,
      257, 263, 269, 271, 277, 281, 283, 293, 307, 311, 313, 317,
      331, 337, 347, 349, 353, 359, 367, 373, 379, 383, 389, 397,
      401, 409, 419, 421, 431, 433, 439, 443, 449, 457, 461, 463,
      467, 479, 487, 491, 499, 503, 509, 521, 523, 541, 547, 557,
      563, 569, 571, 577, 587, 593, 599, 601, 607, 613, 617, 619,
      631, 641, 643, 647, 653, 659, 661, 673, 677, 683, 691, 701,
      709, 719, 727, 733, 739, 743, 751, 757, 761, 769, 773, 787,
      797, 809, 811, 821, 823, 827, 829, 839, 853, 857, 859, 863,
      877, 881, 883, 887, 907, 911, 919, 929, 937, 941, 947, 953,
      967, 971, 977, 983, 991, 997, 1009, 1013, 1019, 1021, 1031,
      1033, 1039, 1049, 1051, 1061, 1063, 1069, 1087, 1091, 1093,
      1097, 1103, 1109, 1117 };

    qrng_detail::dimension_assert("Faure", n, prim_a[number_of_primes - 1]);

    return *std::lower_bound(prim_a, prim_a + number_of_primes, n);
  }
};

} // namespace qrng_tables
} // namespace detail

namespace qrng_detail {
namespace fr {

// Returns the integer part of the logarithm base Base of arg.
// In erroneous situations, e.g., integer_log(base, 0) the function
// returns 0 and does not report the error. This is the intended
// behavior.
template <typename T>
inline T integer_log(T base, T arg)
{
  T ilog = T();
  while (base <= arg)
  {
    arg /= base; ++ilog;
  }
  return ilog;
}

// Perform exponentiation by squaring (potential for code reuse in multiprecision::powm)
template <typename T>
inline T integer_pow(T base, T e)
{
  T result = static_cast<T>(1);
  while (e)
  {
    if (e & static_cast<T>(1))
      result *= base;
    e >>= 1;
    base *= base;
  }
  return result;
}

} // namespace fr

// Computes a table of binomial coefficients modulo qs.
template<typename RealType, typename SeqSizeT, typename PrimeTable>
struct binomial_coefficients
{
  typedef RealType value_type;
  typedef SeqSizeT size_type;

  // Binomial values modulo qs_base will never be bigger than qs_base.
  // We can choose an appropriate integer type to hold modulo values and
  // shave off memory footprint.
  typedef typename PrimeTable::value_type packed_uint_t;

  // default copy c-tor is fine

  explicit binomial_coefficients(std::size_t dimension)
  {
    resize(dimension);
  }

  void resize(std::size_t dimension)
  {
    qs_base = PrimeTable::lower_bound(dimension);

    // Throw away previously computed coefficients.
    // This will trigger recomputation on next update
    coeff.clear();
  }

  template <typename Iterator>
  void update(size_type seq, Iterator first, Iterator last)
  {
    if (first != last)
    {
      const size_type ilog = fr::integer_log(static_cast<size_type>(qs_base), seq);
      const size_type hisum = ilog + 1;
      if (coeff.size() != size_hint(hisum)) {
        ytemp.resize(static_cast<std::size_t>(hisum)); // cast safe because log is small
        compute_coefficients(hisum);
        qs_pow = fr::integer_pow(static_cast<size_type>(qs_base), ilog);
      }

      *first = compute_recip(seq, ytemp.rbegin());

      // Find other components using the Faure method.
      ++first;
      for ( ; first != last; ++first)
      {
        *first = RealType();
        RealType r = static_cast<RealType>(1);

        for (size_type i = 0; i != hisum; ++i)
        {
          RealType ztemp = ytemp[static_cast<std::size_t>(i)] * upper_element(i, i, hisum);
          for (size_type j = i + 1; j != hisum; ++j)
            ztemp += ytemp[static_cast<std::size_t>(j)] * upper_element(i, j, hisum);

          // Sum ( J <= I <= HISUM ) ( old ytemp(i) * binom(i,j) ) mod QS.
          ytemp[static_cast<std::size_t>(i)] = std::fmod(ztemp, static_cast<RealType>(qs_base));
          r *= static_cast<RealType>(qs_base);
          *first += ytemp[static_cast<std::size_t>(i)] / r;
        }
      }
    }
  }

private:
  inline static size_type size_hint(size_type n)
  {
    return n * (n + 1) / 2;
  }

  packed_uint_t& upper_element(size_type i, size_type j, size_type dim)
  {
    BOOST_ASSERT( i < dim );
    BOOST_ASSERT( j < dim );
    BOOST_ASSERT( i <= j );
    return coeff[static_cast<std::size_t>((i * (2 * dim - i + 1)) / 2 + j - i)];
  }

  template<typename Iterator>
  RealType compute_recip(size_type seq, Iterator out) const
  {
    // Here we do
    //   Sum ( 0 <= J <= HISUM ) YTEMP(J) * QS**J
    //   Sum ( 0 <= J <= HISUM ) YTEMP(J) / QS**(J+1)
    // in one go
    RealType r = RealType();
    size_type m, k = qs_pow;
    for( ; k != 0; ++out, seq = m, k /= qs_base )
    {
      m  = seq % k;
      RealType v  = static_cast<RealType>((seq - m) / k); // RealType <- size type
      r += v;
      r /= static_cast<RealType>(qs_base);
      *out = v; // saves double dereference
    }
    return r;
  }

  void compute_coefficients(const size_type n)
  {
    // Resize and initialize to zero
    coeff.resize(static_cast<std::size_t>(size_hint(n)));
    std::fill(coeff.begin(), coeff.end(), packed_uint_t());

    // The first row and the diagonal is assigned to 1
    upper_element(0, 0, n) = 1;
    for (size_type i = 1; i < n; ++i)
    {
      upper_element(0, i, n) = 1;
      upper_element(i, i, n) = 1;
    }

    // Computes binomial coefficients MOD qs_base
    for (size_type i = 1; i < n; ++i)
    {
      for (size_type j = i + 1; j < n; ++j)
      {
        upper_element(i, j, n) = ( upper_element(i, j-1, n) +
                                   upper_element(i-1, j-1, n) ) % qs_base;
      }
    }
  }

private:
  packed_uint_t qs_base;

  // here we cache precomputed data; note that binomial coefficients have
  // to be recomputed iff the integer part of the logarithm of seq changes,
  // which happens relatively rarely.
  std::vector<packed_uint_t> coeff; // packed upper (!) triangular matrix
  std::vector<RealType> ytemp;
  size_type qs_pow;
};

} // namespace qrng_detail

typedef detail::qrng_tables::primes default_faure_prime_table;

/** @endcond */

//!Instantiations of class template faure_engine model a \quasi_random_number_generator.
//!The faure_engine uses the algorithm described in
//! \blockquote
//!Henri Faure,
//!Discrepance de suites associees a un systeme de numeration (en dimension s),
//!Acta Arithmetica,
//!Volume 41, 1982, pages 337-351.
//! \endblockquote
//
//! \blockquote
//!Bennett Fox,
//!Algorithm 647:
//!Implementation and Relative Efficiency of Quasirandom
//!Sequence Generators,
//!ACM Transactions on Mathematical Software,
//!Volume 12, Number 4, December 1986, pages 362-376.
//! \endblockquote
//!
//!In the following documentation @c X denotes the concrete class of the template
//!faure_engine returning objects of type @c RealType, u and v are the values of @c X.
//!
//!Some member functions may throw exceptions of type @c std::bad_alloc.
template<typename RealType, typename SeqSizeT, typename PrimeTable = default_faure_prime_table>
class faure_engine
  : public qrng_detail::qrng_base<
      faure_engine<RealType, SeqSizeT, PrimeTable>
    , qrng_detail::binomial_coefficients<RealType, SeqSizeT, PrimeTable>
    , SeqSizeT
    >
{
  typedef faure_engine<RealType, SeqSizeT, PrimeTable> self_t;

  typedef qrng_detail::binomial_coefficients<RealType, SeqSizeT, PrimeTable> lattice_t;
  typedef qrng_detail::qrng_base<self_t, lattice_t, SeqSizeT> base_t;

  friend class qrng_detail::qrng_base<self_t, lattice_t, SeqSizeT>;

public:
  typedef RealType result_type;

  /** @copydoc boost::random::niederreiter_base2_engine::min() */
  static BOOST_CONSTEXPR result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return static_cast<result_type>(0); }

  /** @copydoc boost::random::niederreiter_base2_engine::max() */
  static BOOST_CONSTEXPR result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return static_cast<result_type>(1); }

  //!Effects: Constructs the `s`-dimensional default Faure quasi-random number generator.
  //!
  //!Throws: bad_alloc, invalid_argument.
  explicit faure_engine(std::size_t s)
    : base_t(s) // initialize the binomial table here
  {}

  /** @copydetails boost::random::niederreiter_base2_engine::seed(UIntType)
   * Throws: bad_alloc.
   */
  void seed(SeqSizeT init = 0)
  {
    compute_seq(init);
    base_t::reset_seq(init);
  }

#ifdef BOOST_RANDOM_DOXYGEN
  //=========================Doxygen needs this!==============================

  /** @copydoc boost::random::niederreiter_base2_engine::dimension() */
  std::size_t dimension() const { return base_t::dimension(); }

  /** @copydoc boost::random::niederreiter_base2_engine::operator()() */
  result_type operator()()
  {
    return base_t::operator()();
  }

  /** @copydoc boost::random::niederreiter_base2_engine::discard(boost::uintmax_t)
   * Throws: bad_alloc.
   */
  void discard(boost::uintmax_t z)
  {
    base_t::discard(z);
  }

  /** Returns true if the two generators will produce identical sequences of outputs. */
  BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(faure_engine, x, y)
  { return static_cast<const base_t&>(x) == y; }

  /** Returns true if the two generators will produce different sequences of outputs. */
  BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(faure_engine)

  /** Writes the textual representation of the generator to a @c std::ostream. */
  BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, faure_engine, s)
  { return os << static_cast<const base_t&>(s); }

  /** Reads the textual representation of the generator from a @c std::istream. */
  BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, faure_engine, s)
  { return is >> static_cast<base_t&>(s); }

#endif // BOOST_RANDOM_DOXYGEN

private:
/** @cond hide_private_members */
  void compute_seq(SeqSizeT seq)
  {
    qrng_detail::check_seed_sign(seq);
    this->lattice.update(seq, this->state_begin(), this->state_end());
  }
/** @endcond */
};

/**
 * @attention This specialization of \faure_engine supports up to 1117 dimensions.
 *
 * However, it is possible to provide your own prime table to \faure_engine should the default one be insufficient.
 */
typedef faure_engine<double, boost::uint_least64_t, default_faure_prime_table> faure;

} // namespace random

} // namespace boost

#endif // BOOST_RANDOM_FAURE_HPP


#include "generalizedassignmentsolver/algorithms/lagrelax_lbfgs.hpp"

#include "knapsacksolver/algorithms/minknap.hpp"
#include "knapsacksolver/algorithms/bellman.hpp"

#include <dlib/optimization.h>

#include <algorithm>
#include <iomanip>
#include <limits>

using namespace generalizedassignmentsolver;
using namespace dlib;

typedef matrix<double,0,1> column_vector;

////////////////////////////////////////////////////////////////////////////////
/////////////////////////// lagrelax_assignment_lbfgs //////////////////////////
////////////////////////////////////////////////////////////////////////////////

LagRelaxAssignmentLbfgsOutput& LagRelaxAssignmentLbfgsOutput::algorithm_end(Info& info)
{
    //FFOT_PUT(info, "Algorithm", "Iterations", it);
    Output::algorithm_end(info);
    //FFOT_VER(info, "Iterations: " << it << std::endl);
    return *this;
}

class LagRelaxAssignmentLbfgsFunction
{

public:

    LagRelaxAssignmentLbfgsFunction(
            const Instance& instance,
            LagRelaxAssignmentLbfgsOptionalParameters& p,
            ItemIdx number_of_unfixed_items,
            const std::vector<ItemIdx>& item_indices):
        instance_(instance), p_(p), item_indices_(item_indices), grad_(number_of_unfixed_items)
    {
        ItemIdx n = instance_.number_of_items();
        AgentIdx m = instance_.number_of_agents();

        // Compute knapsack capacities
        kp_capacities_.resize(m);
        for (AgentIdx i = 0; i < m; ++i) {
            kp_capacities_[i] = instance.capacity(i);
            for (ItemIdx j = 0; j < n; ++j) {
                if (p.fixed_alt != NULL && (*p.fixed_alt)[j][i] == 1)
                    kp_capacities_[i] -= instance.weight(j, i);
            }
            if (kp_capacities_[i] < 0)
                std::cout << "ERROR i " << i << " c " << kp_capacities_[i] << std::endl;
        }

        // Initialize kp_indices_
        kp_indices_.resize(n);
    }

    virtual ~LagRelaxAssignmentLbfgsFunction() { }

    double f(const column_vector& x);

    const column_vector der(const column_vector& x) const { (void)x; return grad_; }

private:

    const Instance& instance_;
    LagRelaxAssignmentLbfgsOptionalParameters& p_;
    /** item_indices_[j] is the index of item j in mu and grad_. */
    const std::vector<ItemIdx>& item_indices_;

    column_vector grad_;

    std::vector<knapsacksolver::Weight> kp_capacities_;
    /** kp_indices_[j] is the index of item j in the current KP. */
    std::vector<knapsacksolver::ItemIdx> kp_indices_;

};

double LagRelaxAssignmentLbfgsFunction::f(const column_vector& mu)
{
    ItemIdx n = instance_.number_of_items();
    AgentIdx m = instance_.number_of_agents();

    // Initialize bound and gradient;
    double l = 0;
    for (ItemIdx j = 0; j < n; ++j)
        if (item_indices_[j] >= 0)
            l += mu(item_indices_[j]);
    std::fill(grad_.begin(), grad_.end(), 1);

    Weight mult = 10000;
    for (AgentIdx i = 0; i < m; ++i) {
        // Create knapsack instance
        knapsacksolver::Instance kp_instance;
        kp_instance.set_capacity(kp_capacities_[i]);
        knapsacksolver::ItemIdx j_kp = 0;
        for (ItemIdx j = 0; j < n; ++j) {
            if ((p_.fixed_alt != NULL && (*p_.fixed_alt)[j][i] >= 0)
                    || instance_.weight(j, i) > kp_capacities_[i]) {
                kp_indices_[j] = -1;
                continue;
            }
            knapsacksolver::Profit profit = std::ceil(mult * mu(j) - mult * instance_.cost(j, i));
            if (profit <= 0) {
                kp_indices_[j] = -1;
                continue;
            }
            kp_instance.add_item(instance_.weight(j, i), profit);
            kp_indices_[j] = j_kp;
            j_kp++;
        }

        // Solve knapsack instance
        //auto kp_output = knapsacksolver::bellman_array_all(kp_instance, Info().set_verbose(false));
        auto kp_output = knapsacksolver::minknap(kp_instance);
        //std::cout << "i " << i << " opt " << kp_output.solution.profit() << std::endl;

        // Update bound and gradient
        for (ItemIdx j = 0; j < n; ++j) {
            if (kp_indices_[j] >= 0 && kp_output.solution.contains_idx(kp_indices_[j])) {
                grad_(item_indices_[j])--;
                l += instance_.cost(j, i) - mu(item_indices_[j]);
            }
        }
    }

    return l;
}

LagRelaxAssignmentLbfgsOutput generalizedassignmentsolver::lagrelax_assignment_lbfgs(
        const Instance& instance,
        LagRelaxAssignmentLbfgsOptionalParameters parameters)
{
    init_display(instance, parameters.info);
    FFOT_VER(parameters.info,
               "Algorithm" << std::endl
            << "---------" << std::endl
            << "Lagrangian Relaxation - Assignment Constraints (LBFGS)" << std::endl
            << std::endl);

    LagRelaxAssignmentLbfgsOutput output(instance, parameters.info);

    ItemIdx n = instance.number_of_items();
    AgentIdx m = instance.number_of_agents();

    // Compute c0, item_indices and number_of_unfixed_items
    ItemIdx item_idx = 0;
    Cost c0 = 0;
    std::vector<ItemIdx> item_indices(n, -2);
    for (ItemIdx j = 0; j < n; ++j) {
        for (AgentIdx i = 0; i < m; ++i) {
            if (parameters.fixed_alt != NULL && (*parameters.fixed_alt)[j][i] == 1) {
                c0 += instance.cost(j, i);
                item_indices[j] = -1;
                break;
            }
        }
        if (item_indices[j] == -2) {
            item_indices[j] = item_idx;
            item_idx++;
        }
    }
    ItemIdx number_of_unfixed_items = item_idx;

    // Initialize multipliers
    column_vector mu(number_of_unfixed_items);
    if (parameters.initial_multipliers != NULL) {
        for (ItemIdx j = 0; j < n; ++j)
            if (item_indices[j] >= 0)
                mu(item_indices[j]) = (*parameters.initial_multipliers)[j];
    } else {
        for (ItemIdx j = 0; j < n; ++j)
            mu(j) = 0;
    }

    // Solve
    LagRelaxAssignmentLbfgsFunction func(instance, parameters, number_of_unfixed_items, item_indices);
    auto f   = [&func](const column_vector& x) { return func.f(x); };
    auto def = [&func](const column_vector& x) { return func.der(x); };
    auto stop_strategy = objective_delta_stop_strategy(0.0001);
    //auto stop_strategy = gradient_norm_stop_strategy().be_verbose(),
    double res = find_max(
            lbfgs_search_strategy(256),
            stop_strategy,
            f,
            def,
            mu,
            std::numeric_limits<double>::max());

    // Compute output parameters
    Cost lb = c0 + std::ceil(res - FFOT_TOL);
    output.update_lower_bound(lb, std::stringstream(""), parameters.info);
    output.multipliers.resize(n);
    for (ItemIdx j = 0; j < n; ++j)
        if (item_indices[j] >= 0)
            output.multipliers[j] = mu(item_indices[j]);

    return output.algorithm_end(parameters.info);
}

////////////////////////////////////////////////////////////////////////////////
//////////////////////////// lagrelax_knapsack_lbfgs ///////////////////////////
////////////////////////////////////////////////////////////////////////////////

LagRelaxKnapsackLbfgsOutput& LagRelaxKnapsackLbfgsOutput::algorithm_end(Info& info)
{
    //FFOT_PUT(info, "Algorithm", "Iterations", it);
    Output::algorithm_end(info);
    //FFOT_VER(info, "Iterations: " << it << std::endl);
    return *this;
}

class LagRelaxKnapsackLbfgsFunction
{

public:

    LagRelaxKnapsackLbfgsFunction(const Instance& instance):
        instance_(instance),
        x_(instance.number_of_items()),
        grad_(instance.number_of_agents())
    {  }
    virtual ~LagRelaxKnapsackLbfgsFunction() { };

    double f(const column_vector& x);

    const column_vector der(const column_vector& x) const { (void)x; return grad_; }

    AgentIdx agent(ItemIdx j) const { return x_(j); }

private:

    const Instance& instance_;
    column_vector x_;
    column_vector grad_;

};

double LagRelaxKnapsackLbfgsFunction::f(const column_vector& mu)
{
    ItemIdx n = instance_.number_of_items();
    AgentIdx m = instance_.number_of_agents();

    // Initialize bound and gradient
    double l = 0;
    for (AgentIdx i = 0; i < m; ++i) {
        l += mu(i) * instance_.capacity(i);
        grad_(i) = instance_.capacity(i);
    }

    for (ItemIdx j = 0; j < n; ++j) {
        // Solve the trivial Generalized Upper Bound Problem
        AgentIdx i_best = -1;
        double rc_best = -1;
        for (AgentIdx i = 0; i < m; ++i) {
            double rc = instance_.cost(j, i) - mu(i) * instance_.weight(j, i);
            if (i_best == -1
                    || rc_best > rc
                    // If the minimum reduced cost of a job is reached for
                    // several agents, schedule the job on the agent with the
                    // most available remaining capacity.
                    // Without this condition, the relaxation fails to get the
                    // optimal bound (the one from the linear relaxation) for
                    // some instances.
                    || (rc_best == rc && grad_(i) > grad_(i_best))) {
                i_best = i;
                rc_best = rc;
            }
        }

        // Update bound and gradient
        grad_(i_best) -= instance_.weight(j, i_best);
        x_(j) = i_best;
        l += rc_best;
    }

    return l;
}

LagRelaxKnapsackLbfgsOutput generalizedassignmentsolver::lagrelax_knapsack_lbfgs(
        const Instance& instance,
        Info info)
{
    init_display(instance, info);
    FFOT_VER(info,
               "Algorithm" << std::endl
            << "---------" << std::endl
            << "Lagrangian Relaxation - Knapsack Constraints (LBFGS)" << std::endl
            << std::endl);

    LagRelaxKnapsackLbfgsOutput output(instance, info);

    AgentIdx m = instance.number_of_agents();
    ItemIdx n = instance.number_of_items();

    // Initialize multipliers
    column_vector mu(m);
    column_vector mu_lower(m);
    column_vector mu_upper(m);
    for (AgentIdx i = 0; i < m; ++i) {
        //mu_lower(i) = 0;
        //mu_upper(i) = std::numeric_limits<double>::max();
        mu(i) = 0;
        mu_lower(i) = -std::numeric_limits<double>::max();
        mu_upper(i) = 0;
    }

    // Solve
    LagRelaxKnapsackLbfgsFunction func(instance);
    auto f   = [&func](const column_vector& x) { return func.f(x); };
    auto def = [&func](const column_vector& x) { return func.der(x); };
    auto stop_strategy = objective_delta_stop_strategy();
    //auto stop_strategy = gradient_norm_stop_strategy();
    double res = find_max_box_constrained(
            lbfgs_search_strategy(256),
            stop_strategy,
            f,
            def,
            mu,
            mu_lower,
            mu_upper);

    // Compute output parameters
    Cost lb = std::ceil(res - FFOT_TOL);
    output.update_lower_bound(lb, std::stringstream(""), info);
    output.multipliers.resize(m);
    for (AgentIdx i = 0; i < m; ++i)
        output.multipliers[i] = mu(i);
    func.f(mu);
    for (ItemIdx j = 0; j < n; ++j) {
        output.x.push_back(std::vector<double>(instance.number_of_agents(), 0));
        output.x[j][func.agent(j)] = 1;
    }

    return output.algorithm_end(info);
}


#include <path_follower/collision_avoidance/collision_detector_ackermann.h>

#include <Eigen/Core>
//#include <opencv2/opencv.hpp> // only for debugging
#include <path_follower/utils/visualizer.h>

using namespace Eigen;

bool CollisionDetectorAckermann::avoid(MoveCommand * const cmd,
                                    const CollisionAvoider::State &state)
{
    if (externalError_ !=  0)
    {
        cmd->setVelocity(0);
        if (cmd->canRotate()) cmd->setRotationalVelocity(0);
        return true;
    }
    velocity_ = cmd->getVelocity();

    return CollisionDetectorPolygon::avoid(cmd, state);
}

CollisionDetectorPolygon::PolygonWithTfFrame CollisionDetectorAckermann::getPolygon(float width, float length, float course_angle, float curve_enlarge_factor) const
{
    /// Based on http://stackoverflow.com/questions/1217585/parallelogram-contains-point

    /*
     * The parallelogram is defined by four points p,q,r,s:
     * p and q are the corners near the robot, r and s are on the opposite site.
     *
     * For course_angle = 0, the parallelogram is a rectangle. If course_angle != 0, a and b
     * stay constant, but c is moved to the side, depending on course_angle:
     *
     * course_angle = 0:
     *          p------------------r
     *  ##   ## |                  | |
     *  ####### |                  | width
     *  ####### |                  | |
     *  ##   ## |                  |
     *          q------------------s
     *   ^robot    <-  length  ->
     *
     *
     * course_angle < 0:
     *          p---+
     *  ##   ## |    +--+
     *  ####### |        +--+
     *  ####### |            +--r
     *  ##   ## |               |
     *          |               |
     *          |               |
     *          q---+           |
     *               +--+       |
     *                   +--+   |
     *                       +--s
     *
     * The Box is enlarged toward the inside curve. In the above example, q is moved away from
     * the robot, while p is fixed. For course_angle > 0 it is vice verca.
     * The amount of this enlarging is controlled by the argument 'curve_enlarge_factor'.
     */

    Vector2f p(0.0f,  width/2.0f);
    Vector2f q(0.0f, -width/2.0f);

    float dir = velocity_ < 0.f ? -1.f : 1.f;
    float sin_angle = std::sin(course_angle);
    float cos_angle = std::cos(course_angle);

    // stretch box sidewards, depending on course angle
    if (course_angle > 0) {
        p(1) += curve_enlarge_factor * sin_angle;
    } else if (course_angle < 0) {
        q(1) += curve_enlarge_factor * sin_angle;
    }

    Vector2f r = p + dir * length * Vector2f(cos_angle, sin_angle);
    Vector2f s = q + dir * length * Vector2f(cos_angle, sin_angle);

    PolygonWithTfFrame pwf;
    pwf.frame = robot_frame_;

    pwf.polygon.push_back( cv::Point2f(p[0], p[1]) );
    pwf.polygon.push_back( cv::Point2f(q[0], q[1]) );
    pwf.polygon.push_back( cv::Point2f(s[0], s[1]) );
    pwf.polygon.push_back( cv::Point2f(r[0], r[1]) );

    return pwf;
}


#ifndef UKF_H
#define UKF_H

#include <functional>
#include <string>
#include <vector>

#include <Eigen/Dense>

class UKF {

    public:

        // Available sigma point types
        enum sig_type {JU, CUT4, CUT6, CUT8};

        // Dynamical model type
        typedef std::function <Eigen::VectorXd (double, double,
            const Eigen::VectorXd&, const Eigen::VectorXd&)> dyn_model;

        // Measurement model type
        typedef std::function <Eigen::VectorXd (double,
            const Eigen::VectorXd&)> meas_model;

        // Dimensions of state, measurement, & process noise
        const int nx, nz, nw;

        // Indicates whether process noise is additive
        const bool addw;

        // Dynamical model: x(tf) = f(ti, tf, x(ti), w)
        const dyn_model f;

        // Measurement model: z = h(t, x) + v
        const meas_model h;

        // Process & measurement noise covariance
        const Eigen::MatrixXd Pww, Pnn;

        // Cholesky decomposition of process noise covariance
        const Eigen::MatrixXd Cww;

        // Standardized sigma points & weights
        const Eigen::MatrixXd Sp, Su;
        const Eigen::VectorXd wp, wu;

        // Number of sigma points in prediction & update step
        const int nsp, nsu;

        // Constructor
        UKF (
            const dyn_model& f_,
            const meas_model& h_,
            bool addw_,
            double t0,
            const Eigen::VectorXd& xm0,
            const Eigen::MatrixXd& Pxx0,
            const Eigen::MatrixXd& Pww_,
            const Eigen::MatrixXd& Pnn_,
            sig_type stype,
            double k);

        // Prediction step
        void predict(double tp);

        // Update step with one measurement
        void update(const Eigen::VectorXd& z);

        // Run filter for sequence of measurements
        void run(const Eigen::VectorXd& tz, const Eigen::MatrixXd& Z);

        // Generate standardized sigma points
        static Eigen::MatrixXd sigmaSt(sig_type stype, int n, double k);

        // Generate sigma point weights
        static Eigen::VectorXd sigmaWt(sig_type stype, int n, double k);

        // Times
        std::vector<double> t;

        // State estimates
        std::vector<Eigen::VectorXd> xest;

        // State estimate covariances
        std::vector<Eigen::MatrixXd> Pxx;

        // Reset filter
        void reset(
            double t0,
            const Eigen::VectorXd& xm0,
            const Eigen::MatrixXd& Pxx0
        );

        // Save results
        void save(const std::string& filename);

        // Directory for CUT files
        static std::string cut_dir;

};

#endif


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#define NT2_UNIT_MODULE "nt2 polynomials toolbox - unit/simd Mode"
#include <nt2/toolbox/polynomials/include/hermite.hpp>
#include <nt2/include/functions/random.hpp>
#include <nt2/include/functions/ulpdist.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/simd/native.hpp>
#include <nt2/sdk/memory/is_aligned.hpp>
#include <nt2/sdk/memory/aligned_type.hpp>
#include <nt2/sdk/memory/load.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <iostream>
//////////////////////////////////////////////////////////////////////////////
// Test behavior of arithmetic component hermite using NT2_TEST_CASE
//////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL(hermite, (float)(double)
                         )
{
 using nt2::hermite;
 using nt2::tag::hermite_;
 using nt2::load;
 using nt2::simd::native;
 using nt2::meta::cardinal_of;

 typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
 typedef native<T,ext_t>             n_t;
 typedef typename nt2::meta::call<hermite_(int32_t, n_t)>::type call_type;
 typedef typename nt2::meta::as_real<T>::type rT;
 typedef native<rT,ext_t>            rn_t;

 NT2_TEST( (boost::is_same<call_type, rn_t>::value) );
 NT2_ALIGNED_TYPE(T) data[1*cardinal_of<n_t>::value];

 for(std::size_t i=0;i<1*cardinal_of<n_t>::value;++i)
   data[i] =nt2::random(-10.0, 10.0); // good value here for hermite

 n_t a0 = load<n_t>(&data[0],0);
 for(int ii = 0; ii < 10; ii++)
   {
     rn_t v  = nt2::hermite(ii, a0);
     for(std::size_t j=0;j<cardinal_of<n_t>::value;++j)
       {
       NT2_TEST_LESSER_EQUAL( nt2::ulpdist(v[j], hermite(ii, a0[j])), 1);
       }
   }

//    NT2_TEST_EQUAL(  nt2::hermite( 1,T(0) ) , nt2::hermite( 1,nt2::splat < n_t > (T(0)) )[0] );
//    NT2_TEST_EQUAL(  nt2::hermite( 2,T(1) ) , nt2::hermite( 2,nt2::splat < n_t > (T(1) ) )[0] );
//    NT2_TEST_EQUAL(  nt2::hermite( 1,T(2) ) , nt2::hermite( 1,nt2::splat < n_t > (T(2) ) )[0] );
//    NT2_TEST_EQUAL(  nt2::hermite( 2,T(8) ) , nt2::hermite( 2,nt2::splat < n_t > (T(8) ) )[0] );
//    NT2_TEST_EQUAL(  nt2::hermite( 2, T(-1)), nt2::hermite( 2,nt2::splat < n_t > (T(-1) ))[0]);

 }


#include <boost/numeric/ublas/lu.hpp>


#ifndef JHMI_UNITS_HPP_NRC_20141204
#define JHMI_UNITS_HPP_NRC_20141204

#include "pt3.hpp"
#include <boost/units/base_units/angle/degree.hpp>
#include <boost/units/base_units/metric/hour.hpp>
#include <boost/units/base_units/metric/liter.hpp>
#include <boost/units/base_units/metric/minute.hpp>
#include <boost/units/base_units/metric/mmHg.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/io.hpp>
#include <boost/units/systems/cgs/mass.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/si/io.hpp>

namespace jhmi {
  constexpr std::size_t operator "" _z ( unsigned long long n ) { return n; }
  //This is complicated since milli * meters can only be converted explicitly
  // to an si::length.  I think var * millimeters isn't surprising if stored
  // as a meter type, so I want to support it, but without hacking boost units.
  // Hence the following:
#define JHMI_PREFIX_TYPE(prefix_,constant_,unit_)                                  \
  struct prefix_##constant_##_type {};                                             \
  static constexpr prefix_##constant_##_type const prefix_##constant_{};           \
  constexpr inline unit_ operator*(double t, prefix_##constant_##_type)            \
  { return unit_{t * boost::units::si::prefix_ * boost::units::si::constant_}; }   \
  constexpr inline unit_ operator*(prefix_##constant_##_type, double t)            \
  { return unit_{t * boost::units::si::prefix_ * boost::units::si::constant_}; }

  //Activity
  using Bq = boost::units::quantity<boost::units::si::activity, double>;
  constexpr inline Bq operator "" _Bq(unsigned long long n)
  { return Bq{static_cast<double>(n) * boost::units::si::becquerels}; }
  constexpr inline Bq operator "" _Bq(long double n)
  { return Bq{static_cast<double>(n) * boost::units::si::becquerels}; }
  constexpr inline Bq operator "" _kBq(unsigned long long n)
  { return Bq{static_cast<double>(n) * boost::units::si::kilo * boost::units::si::becquerels}; }
  constexpr inline Bq operator "" _kBq(long double n)
  { return Bq{static_cast<double>(n) * boost::units::si::kilo * boost::units::si::becquerels}; }
  BOOST_UNITS_STATIC_CONSTANT(becquerels,boost::units::si::activity);
  JHMI_PREFIX_TYPE(kilo,becquerels,Bq)
  static constexpr auto const kBq = kilobecquerels;
  JHMI_PREFIX_TYPE(giga,becquerels,Bq)
  static constexpr auto const GBq = gigabecquerels;

  //Angle
  using rad = boost::units::quantity<boost::units::si::plane_angle, double>;
  BOOST_UNITS_STATIC_CONSTANT(radians, boost::units::si::plane_angle);
  BOOST_UNITS_STATIC_CONSTANT(degrees, boost::units::angle::degree_base_unit::unit_type);
  constexpr inline rad operator "" _radians(unsigned long long n)
  { return static_cast<double>(n) * radians; }
  constexpr inline rad operator "" _radians(long double n)
  { return static_cast<double>(n) * radians; }
  constexpr inline rad operator "" _degrees(unsigned long long n)
  { return rad{static_cast<double>(n) * degrees}; }
  constexpr inline rad operator "" _degrees(long double n)
  { return rad{static_cast<double>(n) * degrees}; }

  //Time
  using s = boost::units::quantity<boost::units::si::time, double>;
  BOOST_UNITS_STATIC_CONSTANT(seconds, boost::units::si::time);
  BOOST_UNITS_STATIC_CONSTANT(minutes, boost::units::metric::minute_base_unit::unit_type);
  BOOST_UNITS_STATIC_CONSTANT(hours, boost::units::metric::hour_base_unit::unit_type);

  //Distance
  using m = boost::units::quantity<boost::units::si::length, double>;
  using m3 = pt3<m>;
  constexpr inline m operator""_mm(unsigned long long n)
  { return m{static_cast<double>(n) * boost::units::si::milli * boost::units::si::meters}; }
  constexpr inline m operator""_mm(long double n)
  { return m{static_cast<double>(n) * boost::units::si::milli * boost::units::si::meters}; }
  constexpr inline m operator""_um(unsigned long long n)
  { return m{static_cast<double>(n) * boost::units::si::micro * boost::units::si::meters}; }
  constexpr inline m operator""_um(long double n)
  { return m{static_cast<double>(n) * boost::units::si::micro * boost::units::si::meters}; }
  BOOST_UNITS_STATIC_CONSTANT(meters,boost::units::si::length);
  JHMI_PREFIX_TYPE(milli,meters,m)
  static constexpr auto const mm = millimeters;
  JHMI_PREFIX_TYPE(micro,meters,m)
  static constexpr auto const um = micrometers;

  //Mass
  using kg = boost::units::quantity<boost::units::si::mass, double>;

  //Area
  using square_meters = boost::units::quantity<boost::units::si::area, double>;

  //Volume
  using cubic_meters = boost::units::quantity<boost::units::si::volume, double>;
  using milliliter = boost::units::make_scaled_unit<
    boost::units::metric::liter_base_unit::unit_type,
    boost::units::scale<10, boost::units::static_rational<-3>>>::type;
  BOOST_UNITS_STATIC_CONSTANT(mL, milliliter);

  //Flow
  using flow_dimension = boost::units::derived_dimension<
          boost::units::length_base_dimension,3, boost::units::time_base_dimension,-1>::type;
  using flow = boost::units::unit<flow_dimension, boost::units::si::system>;
  using cubic_meters_per_second = boost::units::quantity<flow, double>;

  //density
  using density = boost::units::unit<boost::units::mass_density_dimension, boost::units::si::system>;
  using kg_per_cubic_meter = boost::units::quantity<density, double>;

  //Dose
  using Gy = boost::units::quantity<boost::units::si::absorbed_dose, double>;
  constexpr inline Gy operator""_Gy(long double n)
  { return Gy{static_cast<double>(n) * boost::units::si::grays}; }
  constexpr inline Gy operator""_Gy(unsigned long long n)
  { return Gy{static_cast<double>(n) * boost::units::si::grays}; }
  BOOST_UNITS_STATIC_CONSTANT(grays,boost::units::si::absorbed_dose);

  //Pressure
  using Pa = boost::units::quantity<boost::units::si::pressure, double>;
  BOOST_UNITS_STATIC_CONSTANT(pascals,boost::units::si::pressure);
  BOOST_UNITS_STATIC_CONSTANT(mmHg,boost::units::metric::mmHg_base_unit::unit_type);
  constexpr inline Pa operator""_mmHg(unsigned long long n)
  { return Pa{static_cast<double>(n) * mmHg}; }
  constexpr inline Pa operator""_mmHg(long double n)
  { return Pa{static_cast<double>(n) * mmHg}; }

  //Viscosity
  using Pa_s = boost::units::quantity<decltype(pascals * seconds), double>;

  //Energy
  using joules = boost::units::quantity<boost::units::si::energy,double>;
}

#endif


#include <iostream>
#include <boost/hof.hpp>
#include <functional>
#include <vector>

using namespace std;
namespace hof = boost::hof;

template<typename T>
vector<T> concat(const vector<T>& a, const vector<T>& b) {
	vector<T> ret(a.cbegin(), a.cend());
	ret.insert(ret.end(), b.cbegin(), b.cend());
	return ret;
}

// --- Example with vanilla Currying ---
auto curry2 = [](auto func) {
	return [=](auto x) {
		return [=](auto y) {
			return func(x, y);
		};
	};
};

int main()
{
	auto concatCurried = curry2(concat<int>);
	auto prepend012 = concatCurried(vector<int>{0, 1, 2});

	prepend012(vector<int>{7, 8, 9});
	// returns  [0, 1, 2, 7, 8, 9]

	// --- Example with Auto-Currying ---
	auto sum = [](auto a, auto b){
		return a + b;
	};

	auto biOp = [](auto func, auto a, auto b) {
		return func(a, b);
	};

	auto biOpCurried = hof::partial(biOp);
	auto plus = biOpCurried(sum);
	auto addFivePFive = plus(5.5);

	addFivePFive(4.5);
	// returns 10
}