/*--
    Preprocessing.cpp

    This file is part of the Cornucopia curve sketching library.
    Copyright (C) 2010 Ilya Baran (baran37@gmail.com)

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

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

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

#include "Preprocessing.h"
#include "Fitter.h"
#include "Polyline.h"
#include "Debugging.h"
#include "Line.h"
#include "PiecewiseLinearUtils.h"
#include <iostream>

#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;
NAMESPACE_Cornu

class NoScaleDetector : public Algorithm<SCALE_DETECTION>
{
public:
    string name() const { return "None"; }

protected:
    void _run(const Fitter &fitter, AlgorithmOutput<SCALE_DETECTION> &out)
    {
        out.scale = 1.;
    }
};

class AdaptiveScaleDetector : public Algorithm<SCALE_DETECTION>
{
public:
    string name() const { return "Adaptive"; }

protected:
    void _run(const Fitter &fitter, AlgorithmOutput<SCALE_DETECTION> &out)
    {
        double pixel = fitter.params().get(Parameters::PIXEL_SIZE);

        //compute the diagonal length of the bounding box of the curve
        const VectorC<Vector2d> &pts = fitter.originalSketch()->pts();
        AlignedBox<double, 2> boundingBox(pts[0]);
        for(int i = 1; i < (int)pts.size(); ++i)
            boundingBox.extend(pts[i]);
        double diag = boundingBox.diagonal().norm();

        const double smallCurvePixels = fitter.params().get(Parameters::SMALL_CURVE_PIXELS);
        const double largeCurvePixels = fitter.params().get(Parameters::LARGE_CURVE_PIXELS);
        const double maxRescale = fitter.params().get(Parameters::MAX_RESCALE);

        out.scale = 1.;

        if(diag < pixel * smallCurvePixels)
            out.scale = max(1. / maxRescale, diag / (pixel * smallCurvePixels));
        if(diag > pixel * largeCurvePixels)
            out.scale = min(maxRescale, diag / (pixel * largeCurvePixels));
    }
};

void Algorithm<SCALE_DETECTION>::_initialize()
{
    new AdaptiveScaleDetector();
    new NoScaleDetector();
};

//========================Preliminary Resampling=============================

class DefaultPrelimResampling : public Algorithm<PRELIM_RESAMPLING>
{
public:
    string name() const { return "Default"; }

protected:
    void _run(const Fitter &fitter, AlgorithmOutput<PRELIM_RESAMPLING> &out)
    {
        const VectorC<Vector2d> &pts = fitter.originalSketch()->pts();
        VectorC<Vector2d> outPts(0, NOT_CIRCULAR);
        PiecewiseLinearMonotone origToCur(PiecewiseLinearMonotone::POSITIVE);

        const double radius = fitter.scaledParameter(Parameters::MIN_PRELIM_LENGTH);

        //First mark the points on the original curve we definitely want to keep (e.g., corners) using Douglas-Peucker
        vector<bool> keep = _markDouglasPeucker(pts, fitter.scaledParameter(Parameters::DP_CUTOFF));

        //Go through the points and resample them at the rate of radius, discarding those that are too close
        //to the previous output point, and subdiving the line segments between points that are too far.
        //Note that we don't want to simply resample the curve by the arclength parameterization, because
        //noisy regions with too much arclength will get too many samples.
        outPts.push_back(pts[0]);
        origToCur.add(0, 0);
        double lengthSoFar = 0;
        for(int idx = 1; idx < pts.size(); ++idx)
        {
            Vector2d curResampled = outPts.back();
            Vector2d curPt = pts[idx];

            double distSqToCur = (curPt - curResampled).squaredNorm();

            //If it's the last point or was marked for keeping, just output it
            if(idx + 1 == pts.size() || keep[idx])
            {
                lengthSoFar += sqrt(distSqToCur);
                origToCur.add(fitter.originalSketch()->idxToParam(idx), lengthSoFar);
                if(distSqToCur > 1e-16)
                    outPts.push_back(curPt);
                continue;
            }

            //If this point is within radius of the last output point (i.e., too close), discard it.
            if(distSqToCur < SQR(radius))
                continue;

            //The previous point has to be within radius of the previous output point
            Vector2d prevPt = pts[idx - 1];

            //Find the intersection of the line segment between the previous and the current point with the circle
            //centered at the last output point whose radius is "radius".
            ParametrizedLine<double, 2> line = ParametrizedLine<double, 2>::Through(prevPt, curPt);

            double projectedLineParam = line.direction().dot(curResampled - line.origin());
            Vector2d closestOnLine = line.origin() + line.direction() * projectedLineParam;
            double distSqToLine = (curResampled - closestOnLine).squaredNorm();
            if(distSqToLine < 1e-16)
            {
                lengthSoFar += sqrt(distSqToCur);
                origToCur.add(fitter.originalSketch()->idxToParam(idx), lengthSoFar);
                outPts.push_back(curPt);
                continue;
            }
            double y = sqrt(max(0., radius * radius - distSqToLine));
            Vector2d newPt = closestOnLine + y * line.direction(); //y is positive, so this will find the correct point

            lengthSoFar += (curResampled - newPt).norm();
            origToCur.add(fitter.originalSketch()->idxToParam(idx - 1) + projectedLineParam + y, lengthSoFar);
            outPts.push_back(newPt);
            --idx;
        }

        out.output = new Polyline(outPts);
        out.parameters.resize(pts.size());
        for(int i = 0; i < pts.size(); ++i)
        {
            double paramOrig = fitter.originalSketch()->idxToParam(i);
            double paramNew;
            if(!origToCur.eval(paramOrig, paramNew))
                Debugging::get()->printf("Evaluation error!");
            out.parameters[i] = paramNew;
            //Debugging::get()->drawLine(pts[i], out.output->pos(paramNew), Vector3d(1, 0, 1), "Correspondence");
        }

        for(int i = 0; i < (int)outPts.size(); ++i)
            Debugging::get()->drawPoint(outPts[i], Vector3d(0, (i % 10 == 0) ? 0.6 : 0, 1), "Prelim resampled");
        Debugging::get()->drawCurve(out.output, Vector3d(0, 0, 1), "Prelim resampled curve");
    }

private:
    vector<bool> _markDouglasPeucker(const VectorC<Vector2d> &pts, double cutoff)
    {
        vector<bool> out(pts.size(), false);
        out.front() = out.back() = true;

        _dpHelper(pts, out, cutoff, 0, pts.size());
        return out;
    }

    void _dpHelper(const VectorC<Vector2d> &pts, vector<bool> &out, double cutoff, int start, int end)
    {
        Line cur(pts[start], pts[end - 1]);
        double maxDist = cutoff;
        int mid = -1;
        for(int i = start; i < end; ++i)
        {
            double dist = cur.distanceTo(pts[i]);
            if(dist > maxDist)
            {
                maxDist = dist;
                mid = i;
            }
        }
        if(mid < 0)
            return;
        out[mid] = true;
        _dpHelper(pts, out, cutoff, start, mid + 1);
        _dpHelper(pts, out, cutoff, mid, end);
    }
};

class NoPrelimResampling : public Algorithm<PRELIM_RESAMPLING>
{
public:
    string name() const { return "None"; }

protected:
    void _run(const Fitter &fitter, AlgorithmOutput<PRELIM_RESAMPLING> &out)
    {
        out.output = fitter.originalSketch();
        out.parameters.resize(fitter.originalSketch()->pts().size());
        for(int i = 0; i < (int)out.parameters.size(); ++i)
            out.parameters[i] = fitter.originalSketch()->idxToParam(i);
    }
};

void Algorithm<PRELIM_RESAMPLING>::_initialize()
{
    new DefaultPrelimResampling();
    new NoPrelimResampling();
}

//========================Curve Closer=============================

class OldCurveCloser : public Algorithm<CURVE_CLOSING>
{
public:
    string name() const { return "Old"; }

protected:
    void _run(const Fitter &fitter, AlgorithmOutput<CURVE_CLOSING> &out)
    {
        out.output = fitter.output<PRELIM_RESAMPLING>()->output;
        out.parameters = fitter.output<PRELIM_RESAMPLING>()->parameters;
        out.closed = false;

        if(fitter.oversketchBase())
            return; //if we're oversketching, the curve is not closed

        const double tol = fitter.scaledParameter(Parameters::CLOSEDNESS_THRESHOLD);
        const double tolSq = SQR(tol);
        const VectorC<Vector2d> &pts = fitter.output<PRELIM_RESAMPLING>()->output->pts();

        if(pts.size() < 3)
            return; //definitely not closed

        Vector2d start = pts[0], end = pts.back();

        //find point farthest away from start-end line segment
        Line startEnd(start, end);

        if(startEnd.length() > tol) //start and end too far
            return;

        double maxDistSq = 0;
        int farthest = 1;
        for(int i = 1; i + 1 < (int)pts.size(); ++i) {
            double t = startEnd.project(pts[i]);
            double distSq = (pts[i] - startEnd.pos(t)).squaredNorm();
            if(distSq > maxDistSq) {
                maxDistSq = distSq;
                farthest = i;
            }
        }

        if(maxDistSq < tolSq)
            return;

        //now find the points closest to each other that are within tol of each
        //other and of the segment connecting the start and end points
        int closest0 = 0, closest1 = (int)pts.size() - 1;
        double minDistSq = tolSq;
        for(int i = 0; i < farthest; ++i) {
            double t = startEnd.project(pts[i]);
            if((pts[i] - startEnd.pos(t)).squaredNorm() > tolSq)
                break;
            for(int j = (int)pts.size() - 1; j > farthest; --j) {
                double t2 = startEnd.project(pts[j]);
                if((pts[j] - startEnd.pos(t2)).squaredNorm() > tolSq)
                    break;
                double distSq = (pts[i] - pts[j]).squaredNorm();
                if(distSq > minDistSq)
                    continue;
                minDistSq = distSq;
                closest0 = i;
                closest1 = j;
            }
        }

        if(minDistSq == tolSq)
            return;
        //if we are here, curve is closed
        out.closed = true;

        //close and adjust curve
        VectorC<Vector2d>::Base newPts = pts;

        if(closest1 + 1 < (int)pts.size())
            newPts.erase(newPts.begin() + closest1 + 1, newPts.end());
        if(closest0 > 0)
            newPts.erase(newPts.begin(), newPts.begin() + closest0);

        PolylinePtr polyptr = new Polyline(VectorC<Vector2d>(newPts, NOT_CIRCULAR));
        Vector2d adjust = newPts[0] - newPts.back();
        const double adjustPower = 3.;
        for(int i = 0; i < (int)newPts.size(); ++i) {
            double param = polyptr->idxToParam(i) / polyptr->length();
            double adjustFactor = 0.5 * pow(fabs(param - 0.5) * 2., adjustPower) * (param > 0.5 ? 1. : -1.);
            newPts[i] += adjust * adjustFactor;
        }
        newPts.pop_back(); //the last point is duplicate

        out.output = new Polyline(VectorC<Vector2d>(newPts, CIRCULAR));

        //adjust parameters
        PiecewiseLinearMonotone prevToCur(PiecewiseLinearMonotone::POSITIVE);
        PolylineConstPtr prevOutput = fitter.output<PRELIM_RESAMPLING>()->output;

        for(int i = 0; i < (int)pts.size(); ++i)
        {
            double prevParam = prevOutput->idxToParam(i);
            if(i < closest0)
                prevToCur.add(prevParam, 0);
            else if(i > closest1)
                prevToCur.add(prevParam, out.output->length());
            else
                prevToCur.add(prevParam, out.output->idxToParam(i - closest0));
        }
        prevToCur.batchEval(out.parameters);

#if 0
        for(int i = 0; i < (int)out.parameters.size(); ++i)
            Debugging::get()->drawLine(fitter.originalSketch()->pts()[i], out.output->pos(out.parameters[i]), Vector3d(1, 0, 1), "Closed Correspondence");
#endif

        Debugging::get()->drawCurve(out.output, Debugging::Color(0., 0., 0.), "Closed", 2., Debugging::DOTTED);
    }
};

class UnscaledCurveCloser : public Algorithm<CURVE_CLOSING>
{
public:
    string name() const { return "Scaled"; }

protected:
    void _run(const Fitter &fitter, AlgorithmOutput<CURVE_CLOSING> &out)
    {
        out.output = fitter.output<PRELIM_RESAMPLING>()->output;
        out.parameters = fitter.output<PRELIM_RESAMPLING>()->parameters;
        out.closed = false;

        if(fitter.oversketchBase())
            return; //if we're oversketching, the curve is not closed

		const double tol = fitter.params().get(Parameters::CLOSEDNESS_THRESHOLD);
        const double tolSq = SQR(tol);
        const VectorC<Vector2d> &pts = fitter.output<PRELIM_RESAMPLING>()->output->pts();

        if(pts.size() < 3)
            return; //definitely not closed

        Vector2d start = pts[0], end = pts.back();

        //find point farthest away from start-end line segment
        Line startEnd(start, end);

        if(startEnd.length() > tol) //start and end too far
            return;

        double maxDistSq = 0;
        int farthest = 1;
        for(int i = 1; i + 1 < (int)pts.size(); ++i) {
            double t = startEnd.project(pts[i]);
            double distSq = (pts[i] - startEnd.pos(t)).squaredNorm();
            if(distSq > maxDistSq) {
                maxDistSq = distSq;
                farthest = i;
            }
        }

        if(maxDistSq < tolSq)
            return;

        //now find the points closest to each other that are within tol of each
        //other and of the segment connecting the start and end points
        int closest0 = 0, closest1 = (int)pts.size() - 1;
        double minDistSq = tolSq;
        for(int i = 0; i < farthest; ++i) {
            double t = startEnd.project(pts[i]);
            if((pts[i] - startEnd.pos(t)).squaredNorm() > tolSq)
                break;
            for(int j = (int)pts.size() - 1; j > farthest; --j) {
                double t2 = startEnd.project(pts[j]);
                if((pts[j] - startEnd.pos(t2)).squaredNorm() > tolSq)
                    break;
                double distSq = (pts[i] - pts[j]).squaredNorm();
                if(distSq > minDistSq)
                    continue;
                minDistSq = distSq;
                closest0 = i;
                closest1 = j;
            }
        }

        if(minDistSq == tolSq)
            return;
        //if we are here, curve is closed
        out.closed = true;

        //close and adjust curve
        VectorC<Vector2d>::Base newPts = pts;

        if(closest1 + 1 < (int)pts.size())
            newPts.erase(newPts.begin() + closest1 + 1, newPts.end());
        if(closest0 > 0)
            newPts.erase(newPts.begin(), newPts.begin() + closest0);

        PolylinePtr polyptr = new Polyline(VectorC<Vector2d>(newPts, NOT_CIRCULAR));
        Vector2d adjust = newPts[0] - newPts.back();
        const double adjustPower = 3.;
        for(int i = 0; i < (int)newPts.size(); ++i) {
            double param = polyptr->idxToParam(i) / polyptr->length();
            double adjustFactor = 0.5 * pow(fabs(param - 0.5) * 2., adjustPower) * (param > 0.5 ? 1. : -1.);
            newPts[i] += adjust * adjustFactor;
        }
        newPts.pop_back(); //the last point is duplicate

        out.output = new Polyline(VectorC<Vector2d>(newPts, CIRCULAR));

        //adjust parameters
        PiecewiseLinearMonotone prevToCur(PiecewiseLinearMonotone::POSITIVE);
        PolylineConstPtr prevOutput = fitter.output<PRELIM_RESAMPLING>()->output;

        for(int i = 0; i < (int)pts.size(); ++i)
        {
            double prevParam = prevOutput->idxToParam(i);
            if(i < closest0)
                prevToCur.add(prevParam, 0);
            else if(i > closest1)
                prevToCur.add(prevParam, out.output->length());
            else
                prevToCur.add(prevParam, out.output->idxToParam(i - closest0));
        }
        prevToCur.batchEval(out.parameters);

#if 0
        for(int i = 0; i < (int)out.parameters.size(); ++i)
            Debugging::get()->drawLine(fitter.originalSketch()->pts()[i], out.output->pos(out.parameters[i]), Vector3d(1, 0, 1), "Closed Correspondence");
#endif

        Debugging::get()->drawCurve(out.output, Debugging::Color(0., 0., 0.), "Closed", 2., Debugging::DOTTED);
    }
};

void Algorithm<CURVE_CLOSING>::_initialize()
{
    new OldCurveCloser();
	new UnscaledCurveCloser();
}

END_NAMESPACE_Cornu


//          Copyright John McFarlane 2015 - 2017.
// Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file ../../LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <cnl/auxiliary/boost.simd.h>

#include <cnl/fixed_point.h>

#include <boost/simd/function/all.hpp>
#include <boost/simd/function/compare_equal.hpp>

#include <gtest/gtest.h>

namespace {
    using cnl::fixed_point;
    using boost::simd::pack;

    template<class T, std::size_t N, int Exponent>
    using fpp = fixed_point<pack<T, N>, Exponent>;

    template<class U, class FPP>
    struct rebind_s;

    template<class U, class T, std::size_t N, int Exponent>
    struct rebind_s<U, fpp<T, N, Exponent>> {
        using type = pack<U, N>;
    };

    template<class U, class FPP>
    using rebind = typename rebind_s<U, FPP>::type;

    template<class FPP>
    using initializer = rebind<float, FPP>;
}

namespace cnl {
    template<class T, std::size_t N, int Exponent>
    bool operator==(fpp<T, N, Exponent> const& lhs, fpp<T, N, Exponent> const& rhs) noexcept {
        return boost::simd::compare_equal(cnl::_impl::to_rep(lhs), cnl::_impl::to_rep(rhs));
    }

    template<class T, std::size_t N, int Exponent>
    bool operator!=(fpp<T, N, Exponent> const& lhs, fpp<T, N, Exponent> const& rhs) noexcept {
        return !(lhs==rhs);
    }
}

namespace {
    namespace test_numeric_limits {
        static_assert(cnl::numeric_limits<pack<int>>::is_integer, "");
        static_assert(!cnl::numeric_limits<pack<int>>::is_iec559, "");

        static_assert(!cnl::numeric_limits<pack < float>>::is_integer, "");
        static_assert(cnl::numeric_limits<pack < float>>::is_iec559, "");
    }

    namespace test_set_digits {
        using actual_type = cnl::set_digits_t<boost::simd::pack<signed, 8>, 15>;
        using expected_type = boost::simd::pack<cnl::int16, 8>;
        static_assert(std::is_same<actual_type, expected_type>::value, "");
    }

    namespace test_set_make_unsigned {
        using actual_type = cnl::make_unsigned_t<boost::simd::pack<cnl::int16, 2>>;
        using expected_type = boost::simd::pack<cnl::uint16, 2>;
        static_assert(std::is_same<expected_type, actual_type>::value, "");
    }

    namespace test_set_make_signed {
        using actual_type = cnl::make_signed_t<boost::simd::pack<unsigned, 8>>;
        using expected_type = boost::simd::pack<signed, 8>;
        static_assert(std::is_same<expected_type, actual_type>::value, "");
    }

    TEST(boost_simd, scale) {
        using pack = boost::simd::pack<int, 2>;
        auto input = pack{65535, 0};
        auto output = cnl::_impl::fixed_width_scale<5>(input);
        auto expected = pack{65535<<5, 0};
        ASSERT_TRUE(boost::simd::compare_equal(expected, output));
    }

    TEST(boost_simd, shift_left) {
        using pack = boost::simd::pack<int, 2>;
        auto input = pack{65535, 0};
        auto output = cnl::_impl::scale<5>(input);
        auto expected = pack{65535*32, 0};
        ASSERT_TRUE(boost::simd::compare_equal(expected, output));
    }

    TEST(boost_simd, shift_right) {
        using pack = boost::simd::pack<cnl::int64, 2>;
        auto input = pack{65535, 0};
        auto output = cnl::_impl::scale<-5>(input);
        auto expected = pack{65535>>5, 0};
        ASSERT_TRUE(boost::simd::compare_equal(expected, output));
    }

    TEST(boost_simd, equality) {
        using fpp = fpp<short, 4, -4>;
        using init = initializer<fpp>;
        auto lhs = fpp(init(6.25, -8., 0., 1.625));
        auto rhs = fpp(init(6.25, -8., 0., 1.625));
        ASSERT_EQ(lhs, rhs);
    }

    TEST(boost_simd, inequality) {
        using fpp = fpp<cnl::uint64, 2, -32>;
        using init = initializer<fpp>;
        auto lhs = fpp{init{8237416.17618, 3278928.9012393}};
        auto rhs = fpp{init{8237416.17618, 4278928.9012393}};
        ASSERT_NE(lhs, rhs);
    }

    TEST(boost_simd, conversion) {
        using wide_type = pack<cnl::int64, 2>;
        using narrow_type = pack<cnl::uint16, 2>;
        auto w = wide_type{pack<cnl::int64, 2>{INT64_C(5000000000), INT64_C(-42000000000000)}};
        auto n = narrow_type{pack<cnl::uint16, 2>{INT16_C(32000), INT64_C(-7)}};

        w = static_cast<wide_type>(n);    // requires new explicit operator
        n = static_cast<narrow_type>(w);    // requires new explicit conversion operator
        //w = n;    // doesn't compile with or without explicit conversion operator
        //n = w;    // doesn't compile with or without explicit conversion operator

        auto narrow_implicit = narrow_type(w);    // happens already
        EXPECT_TRUE(boost::simd::compare_equal(static_cast<wide_type>(narrow_implicit), w));

        auto wide_implicit = wide_type(n);  // happens already
        EXPECT_TRUE(boost::simd::compare_equal(static_cast<narrow_type>(wide_implicit), n));

        auto narrow_explicit = narrow_type{w};    // happens already
        EXPECT_TRUE(boost::simd::compare_equal(static_cast<wide_type>(narrow_explicit), w));

        auto wide_explicit = wide_type{n};  // happens already
        EXPECT_TRUE(boost::simd::compare_equal(static_cast<narrow_type>(wide_explicit), n));
    }

    TEST(boost_simd, multiply) {
        using operand_type = fpp<short, 2, -4>;
        using result_type = fpp<short, 2, -8>;
        using initializer = initializer<operand_type>;

        auto expected = result_type{initializer{-7.9375, -1.}};
        auto multiplier = operand_type{initializer{7.9375, -8.}};
        auto multiplicand = operand_type{initializer{-1, .125}};
        auto product = multiplier * multiplicand;
        ASSERT_EQ(expected, product);
    }

    TEST(boost_simd, add) {
        using operand_type = fpp<int, 4, -16>;
        using result_type = operand_type;
        using initializer = initializer<operand_type>;

        auto expected = result_type{initializer{7.9375+-1, -8.+.125, 0+-5, 3.5+-3.5}};
        auto augend = operand_type{initializer{7.9375, -8., 0, 3.5}};
        auto addend = operand_type{initializer{-1, .125, -5, -3.5}};
        auto sum = augend + addend;
        ASSERT_EQ(expected, sum);
    }
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/units/units_fwd.hpp>

#include <boost/units/base_dimension.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/derived_dimension.hpp>
#include <boost/units/make_system.hpp>
#include <boost/units/io.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>
#include <boost/units/unit.hpp>

namespace boost {

namespace units {

struct length_base_dimension : boost::units::base_dimension<length_base_dimension,1> { };       ///> base dimension of length
struct time_base_dimension : boost::units::base_dimension<time_base_dimension,3> { };           ///> base dimension of time

typedef length_base_dimension::dimension_type    length_dimension;
typedef time_base_dimension::dimension_type      time_dimension;

struct length1_base_unit : base_unit<length1_base_unit,length_dimension,1>
{
    static std::string name()               { return "length 1"; }
    static std::string symbol()             { return "l1"; }
};

struct length2_base_unit : base_unit<length2_base_unit,length_dimension,2>
{
    static std::string name()               { return "length2"; }
    static std::string symbol()             { return "l2"; }
};

struct time1_base_unit : base_unit<time1_base_unit,time_dimension,3>
{
    static std::string name()               { return "time1"; }
    static std::string symbol()             { return "t1"; }
};

struct time2_base_unit : base_unit<time2_base_unit,time_dimension,4>
{
    static std::string name()               { return "time2"; }
    static std::string symbol()             { return "t2"; }
};

namespace s1 {

typedef make_system<length1_base_unit,time1_base_unit>::type   system;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<time_dimension,system>         time;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(length1,length);
BOOST_UNITS_STATIC_CONSTANT(time1,time);

} // namespace s1

namespace s2 {

typedef make_system<length2_base_unit,time2_base_unit>::type   system;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<time_dimension,system>         time;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(length2,length);
BOOST_UNITS_STATIC_CONSTANT(time2,time);

} // namespace s2

template<class X,class Y>
struct conversion_helper< quantity<s1::length,X>,quantity<s2::length,Y> >
{
    static quantity<s2::length,Y> convert(const quantity<s1::length,X>& source)
    {
        return quantity<s2::length,Y>::from_value(2.5*source.value());
    }
};

template<class X,class Y>
struct conversion_helper< quantity<s2::length,X>,quantity<s1::length,Y> >
{
    static quantity<s1::length,Y> convert(const quantity<s2::length,X>& source)
    {
        return quantity<s1::length,Y>::from_value((1.0/2.5)*source.value());
    }
};

template<class X,class Y>
struct conversion_helper< quantity<s1::time,X>,quantity<s2::time,Y> >
{
    static quantity<s2::time,Y> convert(const quantity<s1::time,X>& source)
    {
        return quantity<s2::time,Y>::from_value(0.5*source.value());
    }
};

} // namespace units

} // namespace boost

int main(void)
{
    using namespace boost::units;

    quantity<s1::length,float>  l1(1.0*s1::length1);
    quantity<s2::length,double> l2(1.5*l1);
    quantity<s1::length,float>  l3(2.0*l2/3.0);

    quantity<s1::time,float>    t1(1.0*s1::time1);
    quantity<s2::time,double>   t2(1.5*t1);
//    quantity<s1::time,float>    t3(2.0*t2/3.0);

    return 0;
}

/*
// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/units/units_fwd.hpp>

#include <boost/units/base_dimension.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/derived_dimension.hpp>
#include <boost/units/make_system.hpp>
#include <boost/units/io.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>
#include <boost/units/unit.hpp>

namespace boost {

namespace units {

struct length_base_dimension : boost::units::base_dimension<length_base_dimension,1> { };       ///> base dimension of length
struct mass_base_dimension : boost::units::base_dimension<mass_base_dimension,2> { };           ///> base dimension of mass
struct time_base_dimension : boost::units::base_dimension<time_base_dimension,3> { };           ///> base dimension of time

typedef length_base_dimension::dimension_type    length_dimension;
typedef mass_base_dimension::dimension_type      mass_dimension;
typedef time_base_dimension::dimension_type      time_dimension;

struct centimeter_base_unit : base_unit<centimeter_base_unit,length_dimension,1>
{
    static std::string name()               { return "centimeter"; }
    static std::string symbol()             { return "cm"; }
};

struct gram_base_unit : base_unit<gram_base_unit,mass_dimension,2>
{
    static std::string name()               { return "gram"; }
    static std::string symbol()             { return "g"; }
};

struct second_base_unit : base_unit<second_base_unit,time_dimension,3>
{
    static std::string name()               { return "second"; }
    static std::string symbol()             { return "s"; }
};

namespace CG {

typedef make_system<centimeter_base_unit,gram_base_unit>::type   system;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<mass_dimension,system>         mass;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(centimeter,length);
BOOST_UNITS_STATIC_CONSTANT(gram,mass);

} // namespace CG

namespace cgs {

typedef make_system<centimeter_base_unit,gram_base_unit,second_base_unit>::type system;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<mass_dimension,system>         mass;
typedef unit<time_dimension,system>         time;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(centimeter,length);
BOOST_UNITS_STATIC_CONSTANT(gram,mass);
BOOST_UNITS_STATIC_CONSTANT(second,time);

} // namespace cgs

namespace esu {

typedef make_system<centimeter_base_unit,
                    gram_base_unit,
                    second_base_unit>::type system;

/// derived dimension for force in electrostatic units : L M T^-2
typedef derived_dimension<length_base_dimension,1,
                          mass_base_dimension,1,
                          time_base_dimension,-2>::type                                             force_dimension;

/// derived dimension for charge in electrostatic units : L^3/2 M^1/2 T^-1
typedef make_dimension_list< mpl::list< dim<length_base_dimension,static_rational<3,2> >,
                                        dim<mass_base_dimension,static_rational<1,2> >,
                                        dim<time_base_dimension,static_rational<-1> > > >::type     charge_dimension;

/// derived dimension for current in electrostatic units : L^3/2 M^1/2 T^-2
typedef make_dimension_list< mpl::list< dim<length_base_dimension,static_rational<3,2> >,
                                        dim<mass_base_dimension,static_rational<1,2> >,
                                        dim<time_base_dimension,static_rational<-2> > > >::type     current_dimension;

/// derived dimension for electric potential in electrostatic units : L^1/2 M^1/2 T^-1
typedef make_dimension_list< mpl::list< dim<length_base_dimension,static_rational<1,2> >,
                                        dim<mass_base_dimension,static_rational<1,2> >,
                                        dim<time_base_dimension,static_rational<-1> > > >::type     electric_potential_dimension;

/// derived dimension for electric field in electrostatic units : L^-1/2 M^1/2 T^-1
typedef make_dimension_list< mpl::list< dim<length_base_dimension,static_rational<-1,2> >,
                                        dim<mass_base_dimension,static_rational<1,2> >,
                                        dim<time_base_dimension,static_rational<-1> > > >::type     electric_field_dimension;

/// unit typedefs
typedef unit<dimensionless_type,system>     dimensionless;

typedef unit<length_dimension,system>       length;
typedef unit<mass_dimension,system>         mass;
typedef unit<time_dimension,system>         time;

typedef unit<force_dimension,system>        force;

typedef unit<charge_dimension,system>               charge;
typedef unit<current_dimension,system>              current;
typedef unit<electric_potential_dimension,system>   electric_potential;
typedef unit<electric_field_dimension,system>       electric_field;

/// unit constants
BOOST_UNITS_STATIC_CONSTANT(centimeter,length);
BOOST_UNITS_STATIC_CONSTANT(gram,mass);
BOOST_UNITS_STATIC_CONSTANT(second,time);

BOOST_UNITS_STATIC_CONSTANT(dyne,force);

BOOST_UNITS_STATIC_CONSTANT(esu,charge);
BOOST_UNITS_STATIC_CONSTANT(statvolt,electric_potential);

} // namespace esu

template<class Y>
quantity<esu::force,Y> coulombLaw(const quantity<esu::charge,Y>& q1,
                                  const quantity<esu::charge,Y>& q2,
                                  const quantity<esu::length,Y>& r)
{
    return q1*q2/(r*r);
}

} // namespace units

} // namespace boost

int main(void)
{
    using namespace boost::units;

    quantity<CG::length>    cg_length(1.0*CG::centimeter);
    quantity<cgs::length>   cgs_length(1.0*cgs::centimeter);

    std::cout << cg_length/cgs_length << std::endl;

    std::cout << esu::gram*pow<2>(esu::centimeter/esu::second)/esu::esu << std::endl;
    std::cout << esu::statvolt/esu::centimeter << std::endl;

    quantity<esu::charge>   q1 = 1.0*esu::esu,
                            q2 = 2.0*esu::esu;
    quantity<esu::length>   r = 1.0*esu::centimeter;

    std::cout << coulombLaw(q1,q2,r) << std::endl;
    std::cout << coulombLaw(q1,q2,cgs_length) << std::endl;

    return 0;
}
*/


#include <boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp>


//---------------------------------------------------------------------------//
//!
//! \file   Utility_OneDDistributionEntryConverterDB.hpp
//! \author Alex Robinson
//! \brief  1D distribution parameter entry converter database declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_ONE_D_DISTRIBUTION_ENTRY_CONVERTER_DB_HPP
#define UTILITY_ONE_D_DISTRIBUTION_ENTRY_CONVERTER_DB_HPP

// Boost Includes
#include <boost/unordered_map.hpp>

// Trilinos Includes
#include <Teuchos_RCP.hpp>
#include <Teuchos_ParameterEntry.hpp>

// FRENSIE Includes
#include "Utility_OneDDistributionEntryConverter.hpp"
#include "Utility_StandardOneDDistributionEntryConverter.hpp"
#include "Utility_DeltaDistribution.hpp"
#include "Utility_DiscreteDistribution.hpp"
#include "Utility_ExponentialDistribution.hpp"
#include "Utility_HistogramDistribution.hpp"
#include "Utility_NormalDistribution.hpp"
#include "Utility_PowerDistribution.hpp"
#include "Utility_TabularDistribution.hpp"
#include "Utility_EquiprobableBinDistribution.hpp"
#include "Utility_UniformDistribution.hpp"
#include "Utility_PolynomialDistribution.hpp"

namespace Utility{

//! The 1D distribution parameter entry converter database
class OneDDistributionEntryConverterDB
{

public:

  //! Add a converter to the database
  static void addConverter(
        const Teuchos::RCP<OneDDistributionEntryConverter>& converter_to_add );

  //! Get the appropriate converter for the given parameter entry
  static Teuchos::RCP<OneDDistributionEntryConverter> getConverter(
		    const Teuchos::RCP<const Teuchos::ParameterEntry>& entry );

  //! Get the OneDDistribution from the given parameter entry
  static Teuchos::RCP<OneDDistribution> convertEntry(
		    const Teuchos::RCP<const Teuchos::ParameterEntry>& entry );

private:

  // Constructor
  OneDDistributionEntryConverterDB();

  // Initialize the class
  static bool initialize();

  // Typedef for the converter map
  typedef boost::unordered_map<std::string,
			       Teuchos::RCP<OneDDistributionEntryConverter> >
  ConverterMap;

  static ConverterMap master_map;

  // Records if the class has been initialized
  static const bool initialized;
};

} // end Utility namespace

//! Add a OneDDistribution converter to the database
#define UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(T) \
  Utility::OneDDistributionEntryConverterDB::addConverter( \
     Teuchos::rcp( new Utility::StandardOneDDistributionEntryConverter< T > ) )

//! Set up the database
#define UTILITY_ONE_D_DISTRIBUTION_ENTRY_CONVERTER_DB_SETUP() \
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(DeltaDistribution);	\
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(DiscreteDistribution); \
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(ExponentialDistribution); \
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(HistogramDistribution); \
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(NormalDistribution);	\
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(PowerDistribution<1u>); \
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(PowerDistribution<2u>); \
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(TabularDistribution<LinLin>);\
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(TabularDistribution<LogLin>);\
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(TabularDistribution<LinLog>);\
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(TabularDistribution<LogLog>);\
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(EquiprobableBinDistribution); \
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(UniformDistribution);	\
  UTILITY_ADD_ONE_D_DISTRIBUTION_ENTRY_CONVERTER(PolynomialDistribution)

#endif // end UTILITY_ONE_D_DISTRIBUTION_ENTRY_CONVERTER_DB_HPP

//---------------------------------------------------------------------------//
// end Utility_OneDDistributionEntryConverterDB.hpp
//---------------------------------------------------------------------------//


#pragma once

#include <cstddef>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>

#include "andres/graph/grid-graph.hxx"

#include "nifty/tools/runtime_check.hxx"
#include "nifty/graph/undirected_graph_base.hxx"
#include "nifty/graph/detail/adjacency.hxx"
#include "nifty/graph/graph_tags.hxx"
#include "nifty/parallel/threadpool.hxx"
#include "nifty/array/arithmetic_array.hxx"

namespace nifty{
namespace graph{

namespace detail_graph{

    template<std::size_t DIM, bool SIMPLE_NH>
    class UndirectedGridGraphIter{
    public:
        typedef andres::graph::GridGraph<DIM> AGridGraph;
        typedef typename AGridGraph::AdjacencyIterator AGridGraphAdjacencyIter;
        typedef UndirectedAdjacency<int64_t,int64_t,int64_t,int64_t> NodeAdjacency;

        struct UnaryFunction{
            typedef NodeAdjacency value_type;
            template<class ADJ>
            NodeAdjacency operator()(const ADJ & adjacency)const{
                return NodeAdjacency(adjacency.vertex(), adjacency.vertex());
            }
        };

        typedef boost::transform_iterator<
            UnaryFunction,
            typename AGridGraph::AdjacencyIterator,
            NodeAdjacency,
            NodeAdjacency
        > OldAdjacencyIter;

        class AdjacencyIter
        : public boost::iterator_facade<
            AdjacencyIter,
            NodeAdjacency,
            std::random_access_iterator_tag,
            const NodeAdjacency &
        >
        {
        public:
            AdjacencyIter(const AGridGraphAdjacencyIter & iter)
            :   iter_(iter),
                adjacency_(){
            }
            bool equal(const AdjacencyIter & other)const{
                return iter_ == other.iter_;
            }
            void increment(){
                ++iter_;
            }
            void dencrement(){
                --iter_;
            }
            void advance(const std::size_t n){
                iter_+=n;
            }
            std::ptrdiff_t distance_to(const AdjacencyIter & other)const{
                return std::distance(iter_, other.iter_);
            }
            const NodeAdjacency & dereference()const{
                adjacency_ = NodeAdjacency(iter_->vertex(), iter_->edge());
                return adjacency_;
            }
        private:
            mutable AGridGraphAdjacencyIter iter_;
            mutable NodeAdjacency adjacency_;
        };

        class NodeIter : public boost::counting_iterator<int64_t>{
            using boost::counting_iterator<int64_t>::counting_iterator;
            using boost::counting_iterator<int64_t>::operator=;
        };

        class EdgeIter : public boost::counting_iterator<int64_t>{
            using boost::counting_iterator<int64_t>::counting_iterator;
            using boost::counting_iterator<int64_t>::operator=;
        };
    };

};

template<std::size_t DIM, bool SIMPLE_NH>
class UndirectedGridGraph;

template<std::size_t DIM>
class UndirectedGridGraph<DIM,true> : public
    UndirectedGraphBase<
        UndirectedGridGraph<DIM, true>,
        typename detail_graph::UndirectedGridGraphIter<DIM,true>::NodeIter,
        typename detail_graph::UndirectedGridGraphIter<DIM,true>::EdgeIter,
        typename detail_graph::UndirectedGridGraphIter<DIM,true>::AdjacencyIter
    >
{
private:
    typedef andres::graph::GridGraph<DIM> AndresGridGraphType;
    typedef typename AndresGridGraphType::VertexCoordinate AndresVertexCoordinate;
public:
    typedef nifty::array::StaticArray<int64_t, DIM> ShapeType;
    typedef nifty::array::StaticArray<int64_t, DIM> CoordinateType;

    typedef typename detail_graph::UndirectedGridGraphIter<DIM,true>::NodeIter      NodeIter;
    typedef typename detail_graph::UndirectedGridGraphIter<DIM,true>::EdgeIter      EdgeIter;
    typedef typename detail_graph::UndirectedGridGraphIter<DIM,true>::AdjacencyIter AdjacencyIter;

    typedef ContiguousTag EdgeIdTag;
    typedef ContiguousTag NodeIdTag;

    typedef SortedTag EdgeIdOrderTag;
    typedef SortedTag NodeIdOrderTag;

    UndirectedGridGraph()
    : gridGraph_(){
    }

    template<class T>
    UndirectedGridGraph(const nifty::array::StaticArray<T, DIM> & shape)
    : gridGraph_(){

        AndresVertexCoordinate ashape;
        std::copy(shape.rbegin(), shape.rend(), ashape.begin());
        gridGraph_.assign(ashape);

    }

    template<class T>
    void assign(const nifty::array::StaticArray<T, DIM> & shape){

        AndresVertexCoordinate ashape;
        std::copy(shape.rbegin(), shape.rend(), ashape.begin());
        gridGraph_.assign(ashape);

    }

    //void assign(const uint64_t numberOfNodes = 0, const uint64_t reserveNumberOfEdges = 0);

    // MUST IMPL INTERFACE
    int64_t u(const int64_t e)const{
        return gridGraph_.vertexOfEdge(e,0);
    }
    int64_t v(const int64_t e)const{
        return gridGraph_.vertexOfEdge(e,1);
    }

    int64_t findEdge(const int64_t u, const int64_t v)const{
        const auto r = gridGraph_.findEdge(u,v);
        if(r.first)
            return r.second;
        else
            return -1;
    }
    int64_t nodeIdUpperBound() const{
         return numberOfNodes() == 0 ? 0 : numberOfNodes()-1;
    }
    int64_t edgeIdUpperBound() const{
        return numberOfEdges() == 0 ? 0 : numberOfEdges()-1;
    }

    uint64_t numberOfEdges() const{
        return gridGraph_.numberOfEdges();
    }
    uint64_t numberOfNodes() const{
        return gridGraph_.numberOfVertices();
    }

    NodeIter nodesBegin()const{
        return NodeIter(0);
    }
    NodeIter nodesEnd()const{
        return NodeIter(this->numberOfNodes());
    }
    EdgeIter edgesBegin()const{
        return EdgeIter(0);
    }
    EdgeIter edgesEnd()const{
        return EdgeIter(this->numberOfEdges());
    }

    AdjacencyIter adjacencyBegin(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexBegin(node));
    }
    AdjacencyIter adjacencyEnd(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexEnd(node));
    }
    AdjacencyIter adjacencyOutBegin(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexBegin(node));
    }
     AdjacencyIter adjacencyOutEnd(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexEnd(node));
    }

    // optional (with default impl in base)
    //std::pair<int64_t,int64_t> uv(const int64_t e)const;

    template<class F>
    void forEachEdge(F && f)const{
        for(uint64_t edge=0; edge< numberOfEdges(); ++edge){
            f(edge);
        }
    }

    template<class F>
    void forEachNode(F && f)const{
        for(uint64_t node=0; node< numberOfNodes(); ++node){
            f(node);
        }
    }

    // serialization de-serialization

    uint64_t serializationSize() const{
        return DIM + 1;
    }

    template<class ITER>
    void serialize(ITER iter) const{
        for(auto d=0; d<DIM; ++d){
            *iter = gridGraph_.shape(d);
            ++iter;
        }
        // simple nh?
        *iter = true;
        ++iter;
    }

    template<class ITER>
    void deserialize(ITER iter);

    /**
     * @brief convert an image with DIM dimension to an edge map
     * @details convert an image with DIM dimension to an edge map
     * by applying a binary functor to the values of a node map at
     * the endpoints of an edge.
     *
     * @param       image the  input image
     * @param       binaryFunctor a binary functor
     * @param[out]  the result edge map
     *
     * @return [description]
     */
    template<class IMAGE, class BINARY_FUNCTOR, class EDGE_MAP>
    void imageToEdgeMap(
        const IMAGE & image,
        BINARY_FUNCTOR binaryFunctor,
        EDGE_MAP & edgeMap
    )const{
        for(const auto edge : this->edges()){
            const auto uv = this->uv(edge);
            CoordinateType cU,cV;
            nodeToCoordinate(uv.first,  cU);
            nodeToCoordinate(uv.second, cV);
            const auto uVal = image(cU.asStdArray());
            const auto vVal = image(cU.asStdArray());
            edgeMap[edge] = binaryFunctor(uVal, vVal);
        }
    }

    /**
     * @brief convert an image with DIM dimension to an edge map
     * @details convert an image with DIM dimension to an edge map
     * by taking the values of the image at the
     * interpixel coordinates.
     * The shape of the image must be 2*shape-1
     *
     *
     * @param       image the  input image
     * @param       binaryFunctor a binary functor
     * @param[out]  the result edge map
     *
     * @return [description]
     */
    template<class IMAGE, class EDGE_MAP>
    void imageToInterpixelEdgeMap(
        const IMAGE & image,
        EDGE_MAP & edgeMap
    )const{

        for(auto d=0; d<DIM; ++d){
            NIFTY_CHECK_OP(shape(d)*2-1, ==, image.shape(d),
                "wrong shape foer image to interpixel edge map")
        }

        for(const auto edge : this->edges()){
            const auto uv = this->uv(edge);
            CoordinateType cU,cV;
            nodeToCoordinate(uv.first,  cU);
            nodeToCoordinate(uv.second, cV);
            const auto uVal = image(cU.asStdArray());
            cU += cV;
            edgeMap[edge] = image(cU.asStdArray());
        }
    }

    uint64_t shape(const std::size_t d)const{
        return gridGraph_.shape(DIM-1-d);
    }

    // COORDINATE RELATED
    CoordinateType nodeToCoordinate(const uint64_t node)const{
        CoordinateType ret;
        nodeToCoordinate(node, ret);
        return ret;
    }

    template<class NODE_COORDINATE>
    void nodeToCoordinate(
        const uint64_t node,
        NODE_COORDINATE & coordinate
    )const{
        AndresVertexCoordinate aCoordinate;
        gridGraph_.vertex(node, aCoordinate);
        for(auto d=0; d<DIM; ++d){
            coordinate[d] = aCoordinate[DIM-1-d];
        }
    }

    template<class NODE_COORDINATE>
    uint64_t coordianteToNode(const NODE_COORDINATE & coordinate)const{
        AndresVertexCoordinate aCoordinate;
        for(auto d=0; d<DIM; ++d){
            aCoordinate[DIM-1-d] = coordinate[d];
        }
        return gridGraph_.vertex(aCoordinate);
    }

private:
    andres::graph::GridGraph<DIM> gridGraph_;

};

} // namespace nifty::graph
} // namespace nifty


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <cmath>
#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTests.h"

namespace tudat
{
namespace unit_tests
{
namespace numerical_integrator_tests
{

//! Use integrateTo() to integrate one step forward in time.
void executeOneIntegrateToStep(
        const Eigen::MatrixXd benchmarkData,
        const double singleStepTestTolerance,
        const numerical_integrators::NumericalIntegratorXdPointer integrator )
{
    // Use integrateTo() to integrate one step forward in time and check results against benchmark
    // data.

    // Check that it is now not possible to roll back.
    BOOST_CHECK( !integrator->rollbackToPreviousState( ) );

    // Use integrateTo() to integrate one step.
    integrator->integrateTo( benchmarkData( SECOND_ROW, TIME_COLUMN_INDEX ),
                             benchmarkData( SECOND_ROW, TIME_COLUMN_INDEX )
                             - benchmarkData( FIRST_ROW, TIME_COLUMN_INDEX ) );

    // Check if the expected time at the end of the integration step matches the required
    // time. This test should be exact.
    BOOST_CHECK_EQUAL( benchmarkData( SECOND_ROW, TIME_COLUMN_INDEX ),
                       integrator->getCurrentIndependentVariable( ) );

    // Check if expected state at end of integration step matches required state.
    BOOST_CHECK_CLOSE_FRACTION( benchmarkData( SECOND_ROW, STATE_COLUMN_INDEX ),
                                integrator->getCurrentState( )( 0 ), singleStepTestTolerance );

    // Roll back to the previous step. This should be possible since the integrateTo() function
    // was called above.
    BOOST_CHECK( integrator->rollbackToPreviousState( ) );

    // Check that the rolled back time is as required. This test should be exact.
    BOOST_CHECK_EQUAL( benchmarkData( FIRST_ROW, TIME_COLUMN_INDEX ),
                       integrator->getCurrentIndependentVariable( ) );

    // Check that the rolled back state is as required. This test should be exact.
    BOOST_CHECK_EQUAL( benchmarkData( FIRST_ROW, STATE_COLUMN_INDEX ),
                       integrator->getCurrentState( )( 0 ) );

    // Check that it is now not possible to roll back.
    BOOST_CHECK( !integrator->rollbackToPreviousState( ) );
}

//! Use performIntegrationStep() to integrate to specified time in multiple steps.
void performIntegrationStepToSpecifiedTime(
        const Eigen::MatrixXd benchmarkData,
        const double singleStepTestTolerance,
        const double fullIntegrationTestTolerance,
        const numerical_integrators::NumericalIntegratorXdPointer integrator )
{
    // Use performIntegrationstep() to integrate to specified time in multiple steps and check
    // results against benchmark data.

    // Declare last time and state.
    double lastTime = TUDAT_NAN;
    Eigen::VectorXd lastState = Eigen::VectorXd::Zero( 1 );

    // Loop through all the integration steps in the benchmark data.
    for ( int i = 1; i < benchmarkData.rows( ); i++ )
    {
        // Set the step size based on the benchmark data.
        double stepSize = benchmarkData( i, TIME_COLUMN_INDEX )
                - benchmarkData( i - 1, TIME_COLUMN_INDEX );

        // Store last time and state.
        lastTime = integrator->getCurrentIndependentVariable( );
        lastState = integrator->getCurrentState( );

        // Perform the integration step.
        integrator->performIntegrationStep( stepSize );

        // Check if the expected time at the end of the integration step matches the required
        // time. This test should be accurate to effectively machine precision.
        if ( std::fabs( integrator->getCurrentIndependentVariable( ) )
             < std::numeric_limits< double >::epsilon( ) )
        {
            BOOST_CHECK_SMALL( benchmarkData( i, TIME_COLUMN_INDEX ),
                               singleStepTestTolerance );
        }

        else
        {
            BOOST_CHECK_CLOSE_FRACTION( benchmarkData( i, TIME_COLUMN_INDEX ),
                                        integrator->getCurrentIndependentVariable( ),
                                        singleStepTestTolerance );
        }

        // Check if expected state at end of integration step matches required state.
        // The reason this test has a different, higher tolerance than for a single integration
        // step is because the acummulated error builds up, since this test is based off of
        // a time-series. The individual steps are accurate to singleStepTestTolerance.
        BOOST_CHECK_CLOSE_FRACTION( benchmarkData( i, STATE_COLUMN_INDEX ),
                                    integrator->getCurrentState( )( 0 ),
                                    fullIntegrationTestTolerance );
    }

    // Roll back to the previous step. This should be possible since the
    // performIntegrationStep() function was called above.
    BOOST_CHECK( integrator->rollbackToPreviousState( ) );

    // Check that the rolled back time is as required. This test should be exact.
    BOOST_CHECK_EQUAL( lastTime, integrator->getCurrentIndependentVariable( ) );

    // Check that the rolled back state is as required. This test should be exact.
    BOOST_CHECK_EQUAL( lastState( 0 ), integrator->getCurrentState( )( 0 ) );

    // Check that it is now not possible to roll back.
    BOOST_CHECK( !integrator->rollbackToPreviousState( ) );
}

//! Use integrateTo() to integrate to specified time in one step.
void executeIntegrateToToSpecifiedTime(
        const Eigen::MatrixXd benchmarkData,
        const double fullIntegrationTestTolerance,
        const numerical_integrators::NumericalIntegratorXdPointer integrator,
        const double specifiedTime )
{
    // Use integrateTo() to integrate to specified time and and check results against benchmark
    // data.

    // Set index of last row of matrix containing benchmark data.
    const int indexLastRowBenchmarkData = benchmarkData.rows( ) - 1;

    // Use integrateTo() to integrate to final time.
    integrator->integrateTo( specifiedTime,
                             benchmarkData( SECOND_ROW, TIME_COLUMN_INDEX )
                             - benchmarkData( FIRST_ROW, TIME_COLUMN_INDEX ) );

    // Check if the expected time at the end of the integration step matches the required
    // time. This test should be exact.
    BOOST_CHECK_EQUAL( benchmarkData( indexLastRowBenchmarkData, TIME_COLUMN_INDEX ),
                       integrator->getCurrentIndependentVariable( ) );

    // Check if expected state at end of integration step matches required state.
    // The reason this test has a different, higher tolerance than for a single integration
    // step is because the acummulated error builds up, since this test is based off of
    // a time-series. The individual steps are accurate to the tolerance used in
    // Case 1.
    BOOST_CHECK_CLOSE_FRACTION( benchmarkData( indexLastRowBenchmarkData,
                                               STATE_COLUMN_INDEX ),
                                integrator->getCurrentState( )( 0 ),
                                fullIntegrationTestTolerance );

    // Roll back to the previous step. This should be possible since the integrateTo() function
    // was called above.
    BOOST_CHECK( integrator->rollbackToPreviousState( ) );

    // Check that it is now not possible to roll back.
    BOOST_CHECK( !integrator->rollbackToPreviousState( ) );
}

//! Use performIntegrationStep() to integrate to specified time in multiple steps, including
//! discrete events.
void performIntegrationStepToSpecifiedTimeWithEvents(
        const Eigen::MatrixXd benchmarkData,
        const double singleStepTestTolerance,
        const double fullIntegrationTestTolerance,
        const numerical_integrators::ReinitializableNumericalIntegratorXdPointer integrator )
{
    // Use performIntegrationstep() to integrate to specified time in multiple steps, including
    // discrete events and check results against benchmark data. The discrete events are enabled
    // by modifying the state instantaneously. These discrete events cannot be rolled back.

    // Declare last time and state.
    double lastTime = TUDAT_NAN;
    Eigen::VectorXd lastState = Eigen::VectorXd::Zero( 1 );

    // Loop through all the integration steps in the benchmark data.
    for ( int i = 1; i < benchmarkData.rows( ); i++ )
    {
        // Set the step size based on the benchmark data generated.
        const double stepSize = benchmarkData( i, TIME_COLUMN_INDEX )
                - benchmarkData( i - 1, TIME_COLUMN_INDEX );

        // Store last time and state.
        lastTime = integrator->getCurrentIndependentVariable( );
        lastState = integrator->getCurrentState( );

        // Perform the integration step.
        integrator->performIntegrationStep( stepSize );

        // Check if the expected time at the end of the integration step matches the required
        // time. This test should be accurate to effectively machine precision.
        if ( std::fabs( benchmarkData( i, TIME_COLUMN_INDEX ) )
             < std::numeric_limits< double >::min( ) )
        {
            BOOST_CHECK_SMALL( integrator->getCurrentIndependentVariable( ),
                               std::numeric_limits< double >::epsilon( ) );
        }

        else
        {
            BOOST_CHECK_CLOSE_FRACTION(
                        benchmarkData( i, TIME_COLUMN_INDEX ),
                        integrator->getCurrentIndependentVariable( ),
                        singleStepTestTolerance );
        }

        // Check if expected state at end of integration step matches required state.
        // The reason this test has a different, higher tolerance than for a single integration
        // step is because the acummulated error builds up, since this test is based off of
        // a time-series. The individual steps are accurate to the tolerance used in
        // Case 1.
        BOOST_CHECK_CLOSE_FRACTION( benchmarkData( i, STATE_COLUMN_INDEX ),
                                    integrator->getCurrentState( )( 0 ),
                                    fullIntegrationTestTolerance );

        // Check if a discrete event is schedule to take place, and execute discrete event
        // if so.
        if ( i < benchmarkData.rows( ) - 1
             && std::fabs( benchmarkData( i + 1, TIME_COLUMN_INDEX )
                           - benchmarkData( i, TIME_COLUMN_INDEX ) )
             < std::numeric_limits< double >::epsilon( ) )
        {
            // Increment loop to next row in matrix, which contains the discrete event that
            // affects the state.
            i++;

            // Modify the current state based on the discrete event
            integrator->modifyCurrentState(
                        Eigen::VectorXd::Constant(
                            1, benchmarkData( i, STATE_COLUMN_INDEX ) ) );

            // Check that it is now not possible to roll back.
            BOOST_CHECK( !integrator->rollbackToPreviousState( ) );
        }
    }

    // Check that it is possible to roll back to the previous step. This is allowable, because
    // the last action performed by the integrator is the performIntegrationStep() function.
    BOOST_CHECK( integrator->rollbackToPreviousState( ) );

    // Check that the rolled back time is as required. This test should be exact.
    BOOST_CHECK_EQUAL( lastTime, integrator->getCurrentIndependentVariable( ) );

    // Check that the rolled back state is as required. This test should be exact.
    BOOST_CHECK_EQUAL( lastState( 0 ), integrator->getCurrentState( )( 0 ) );

    // Check that it is now not possible to roll back to the previous step.
    BOOST_CHECK( !integrator->rollbackToPreviousState( ) );
}

} // namespace numerical_integrator_tests
} // namespace unit_tests
} // namespace tudat


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/cscd.hpp>
#include <boost/simd/function/restricted.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/sqrt_2.hpp>

STF_CASE_TPL (" cscd",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cscd;

  using r_t = decltype(cscd(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cscd(-bs::Zero<T>()), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(-T(180)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(T(180)), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cscd(-T(45)), -bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(-T(90)), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(T(45)), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(T(90)), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" cscd restricted_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cscd;

  using r_t = decltype(cscd(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::restricted_(cscd)(-bs::Zero<T>()), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cscd)(-T(180)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cscd)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cscd)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cscd)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cscd)(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cscd)(T(180)), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::restricted_(cscd)(-T(45)), -bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cscd)(-T(90)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cscd)(T(45)), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cscd)(T(90)), bs::Nan<r_t>(), 0.5);
}

STF_CASE_TPL (" cscd clipped_small_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bst = bs::tag;
  namespace bd = boost::dispatch;
  using bs::cscd;

  using r_t = decltype(cscd(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cscd(-bs::Zero<T>(), bst::clipped_small_), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(-T(180), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Inf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Minf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Nan<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Zero<T>(), bst::clipped_small_), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(T(180), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cscd(-T(45), bst::clipped_small_), -bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(-T(90), bst::clipped_small_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(T(45), bst::clipped_small_), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(T(90), bst::clipped_small_), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" cscd clipped_medium_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bst = bs::tag;
  namespace bd = boost::dispatch;
  using bs::cscd;

  using r_t = decltype(cscd(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cscd(-bs::Zero<T>(), bst::clipped_medium_), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(-T(180), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Inf<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Minf<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Nan<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(bs::Zero<T>(), bst::clipped_medium_), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(T(180), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cscd(-T(45), bst::clipped_medium_), -bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(-T(90), bst::clipped_medium_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(T(45), bst::clipped_medium_), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cscd(T(90), bst::clipped_medium_), bs::One<r_t>(), 0.5);
}


// Bring in my package's API, which is what I'm testing

#include <Eigen/Core>
// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>

// Serialization
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/filesystem.hpp>
#include <fstream>

#include <Eigen/Geometry>
#include <sm/eigen/serialization.hpp>

template <typename T, int A, int B, int C, int D>
void saveLoadMatrixTest() {
    Eigen::Matrix<T, A, B> T1, T2;

    T1.resize(C, D);

    for (int r = 0; r < T1.rows(); r++) {
        for (int c = 0; c < T1.cols(); c++) {
            T1(r, c) = (T)(((r - 1) * T1.cols()) + c);
        }
    }

    {
        std::ofstream ofs("test.ba", std::ios::binary);
        boost::archive::binary_oarchive oa(ofs);

        oa << T1;
    }

    {
        std::ifstream ifs("test.ba", std::ios::binary);
        boost::archive::binary_iarchive ia(ifs);

        ia >> T2;
    }

    for (int r = 0; r < T1.rows(); r++) {
        for (int c = 0; c < T1.cols(); c++) {
            ASSERT_EQ(T1(r, c), T2(r, c)) << "Value at row " << r << " and col " << c << " is not exactly equal";
        }
    }
}

template <typename T, int A, int B, int C, int D>
void saveLoadXmlMatrixTest() {
    Eigen::Matrix<T, A, B> T1, T2;

    T1.resize(C, D);

    for (int r = 0; r < T1.rows(); r++) {
        for (int c = 0; c < T1.cols(); c++) {
            T1(r, c) = (T)(((r - 1) * T1.cols()) + c);
        }
    }

    {
        std::ofstream ofs("test.xml", std::ios::binary);
        boost::archive::xml_oarchive oa(ofs);

        oa << ::boost::serialization::make_nvp("top", T1);
    }

    {
        std::ifstream ifs("test.xml", std::ios::binary);
        boost::archive::xml_iarchive ia(ifs);

        ia >> ::boost::serialization::make_nvp("top", T2);
    }

    ASSERT_EQ(T1.rows(), T2.rows());
    ASSERT_EQ(T1.cols(), T2.cols());

    for (int r = 0; r < T1.rows(); r++) {
        for (int c = 0; c < T1.cols(); c++) {
            ASSERT_EQ(T1(r, c), T2(r, c)) << "Value at row " << r << " and col " << c << " is not exactly equal";
        }
    }
}

TEST(EigenSerializationTestSuite, testMatrix) {
    saveLoadMatrixTest<double, 6, 6, 6, 6>();
    saveLoadMatrixTest<float, 6, 6, 6, 6>();
    saveLoadMatrixTest<int, 6, 6, 6, 6>();
    saveLoadMatrixTest<unsigned, 6, 6, 6, 6>();
    saveLoadMatrixTest<char, 6, 6, 6, 6>();

    saveLoadMatrixTest<double, 1, 64, 1, 64>();
    saveLoadMatrixTest<double, 64, 1, 64, 1>();
    saveLoadMatrixTest<float, 1, 64, 1, 64>();
    saveLoadMatrixTest<float, 64, 1, 64, 1>();

    saveLoadMatrixTest<double, Eigen::Dynamic, 6, 100, 6>();
    saveLoadMatrixTest<double, 6, Eigen::Dynamic, 6, 100>();
    saveLoadMatrixTest<double, Eigen::Dynamic, Eigen::Dynamic, 100, 100>();
}

TEST(EigenSerializationTestSuite, testMatrixXml) {
    try {
        saveLoadXmlMatrixTest<double, 6, 6, 6, 6>();
        saveLoadXmlMatrixTest<float, 6, 6, 6, 6>();
        saveLoadXmlMatrixTest<int, 6, 6, 6, 6>();
        saveLoadXmlMatrixTest<unsigned, 6, 6, 6, 6>();
        saveLoadXmlMatrixTest<char, 6, 6, 6, 6>();

        saveLoadXmlMatrixTest<double, 1, 64, 1, 64>();
        saveLoadXmlMatrixTest<double, 64, 1, 64, 1>();
        saveLoadXmlMatrixTest<float, 1, 64, 1, 64>();
        saveLoadXmlMatrixTest<float, 64, 1, 64, 1>();

        saveLoadXmlMatrixTest<double, Eigen::Dynamic, 6, 100, 6>();
        saveLoadXmlMatrixTest<double, 6, Eigen::Dynamic, 6, 100>();
        saveLoadXmlMatrixTest<double, Eigen::Dynamic, Eigen::Dynamic, 100, 100>();
    } catch (const std::exception& e) {
        FAIL() << e.what();
    }
}

template <typename Transform_t>
void saveLoadTransformTest() {
    Transform_t M1, M2;
    typedef typename Transform_t::Scalar Scalar;
    typename Transform_t::MatrixType& T1a = M1.matrix();

    for (int r = 0; r < T1a.rows(); r++) {
        for (int c = 0; c < T1a.cols(); c++) {
            T1a(r, c) = (Scalar)(((r - 1) * T1a.cols()) + c);
        }
    }

    {
        std::ofstream ofs("test.ba", std::ios::binary);
        boost::archive::binary_oarchive oa(ofs);

        oa << M1;
    }

    {
        std::ifstream ifs("test.ba", std::ios::binary);
        boost::archive::binary_iarchive ia(ifs);

        ia >> M2;
    }

    typename Transform_t::MatrixType& T1 = M1.matrix();
    typename Transform_t::MatrixType& T2 = M2.matrix();

    for (int r = 0; r < T1.rows(); r++) {
        for (int c = 0; c < T1.cols(); c++) {
            ASSERT_EQ(T1(r, c), T2(r, c)) << "Value at row " << r << " and col " << c << " is not exactly equal";
        }
    }
}

TEST(EigenSerializationTestSuite, testTransform) {
    saveLoadTransformTest<Eigen::Affine2d>();
    saveLoadTransformTest<Eigen::Affine2f>();
    saveLoadTransformTest<Eigen::Affine3d>();
    saveLoadTransformTest<Eigen::Affine3f>();
    saveLoadTransformTest<Eigen::AffineCompact2d>();
    saveLoadTransformTest<Eigen::AffineCompact2f>();
    saveLoadTransformTest<Eigen::AffineCompact3d>();
    saveLoadTransformTest<Eigen::AffineCompact3f>();
    saveLoadTransformTest<Eigen::Isometry2f>();
    saveLoadTransformTest<Eigen::Isometry2d>();
    saveLoadTransformTest<Eigen::Isometry3f>();
    saveLoadTransformTest<Eigen::Isometry3d>();
    saveLoadTransformTest<Eigen::Projective2f>();
    saveLoadTransformTest<Eigen::Projective2d>();
    saveLoadTransformTest<Eigen::Projective3f>();
    saveLoadTransformTest<Eigen::Projective3d>();
}

// /// Run all the tests that were declared with TEST()
// int main(int argc, char **argv){
//   testing::InitGoogleTest(&argc, argv);
//   return RUN_ALL_TESTS();
// }


#pragma once
#include <algorithm>
#include <ext/range/range_traits.hpp>
#include <boost/range/metafunctions.hpp>
#include <boost/range/functions.hpp>

namespace ext
{
	///аналогичен std::binary_search, но в отличии от возвращают итератор(если не найден - end)
	template <class FwdIterator, class Type>
	FwdIterator binary_find(FwdIterator beg, FwdIterator end, const Type & val)
	{
		beg = std::lower_bound(beg, end, val);          // !(el < val)
		return beg != end && !(val < *beg) ? beg : end; // !(val < el) => el === val
	}

	///аналогичен std::binary_search, но в отличии от возвращают итератор(если не найден - end)
	template <class FwdIterator, class Type, class Pred>
	FwdIterator binary_find(FwdIterator beg, FwdIterator end, const Type & val, Pred pred)
	{
		beg = std::lower_bound(beg, end, val, pred);         // !pred(el, val)
		return beg != end && !(pred(val, *beg)) ? beg : end; // !pred(val, el) => el === val
	}

	/// \overload
	template <class ForwardRange, class Type>
	typename boost::range_iterator<ForwardRange>::type
	binary_find(ForwardRange & rng, const Type & val)
	{
		return binary_find(boost::begin(rng), boost::end(rng), val);
	}

	/// \overload
	template <class ForwardRange, class Type>
	typename boost::range_iterator<const ForwardRange>::type
	binary_find(const ForwardRange & rng, const Type & val)
	{
		return binary_find(boost::begin(rng), boost::end(rng), val);
	}

	/// \overload
	// SFINAE to resolve conflict with overload(first, last, val)
	// it least on msvc 2013 it conflicts
	template <
		class ForwardRange, class Type, class Pred,
		typename std::enable_if<ext::is_range<ForwardRange>::value, int>::type = 0
	>
	typename boost::range_iterator<ForwardRange>::type
	binary_find(ForwardRange & rng, const Type & val, Pred pred)
	{
		return binary_find(boost::begin(rng), boost::end(rng), val, pred);
	}

	/// \overload
	// SFINAE to resolve conflict with overload(first, last, val)
	// it least on msvc 2013 it conflicts
	template <
		class ForwardRange, class Type, class Pred,
		typename std::enable_if<ext::is_range<ForwardRange>::value, int>::type = 0
	>
	typename boost::range_iterator<ForwardRange>::type
	binary_find(const ForwardRange & rng, const Type & val, Pred pred)
	{
		return binary_find(boost::begin(rng), boost::end(rng), val, pred);
	}
}


#include <boost/compute/algorithm/inner_product.hpp>