// Include for Visual studio, 'cos reasons.
#define _USE_MATH_DEFINES
#include <cmath>
#include <numeric>
#include <set>
#include <omp.h>

#include "hoMatrix.h"
#include "hoNDArray_elemwise.h"
#include "hoNDArray_math.h"
#include "hoNDFFT.h"
#include <boost/container/flat_set.hpp>

namespace Gadgetron {

    template <typename T> hoNDFFT<T>* hoNDFFT<T>::instance() {
        if (!instance_)
            instance_ = new hoNDFFT<T>();
        return instance_;
    }

    template <class T> hoNDFFT<T>* hoNDFFT<T>::instance_ = NULL;

    namespace {

        template <class T> struct fftw_types {};

        template <> struct fftw_types<float> {
            using complex                      = fftwf_complex;
            using plan                         = fftwf_plan_s;
            static constexpr auto plan_guru    = fftwf_plan_guru64_dft;
            static constexpr auto plan_dft     = fftwf_plan_dft;
            static constexpr auto execute_dft  = fftwf_execute_dft;
            static constexpr auto destroy_plan = fftwf_destroy_plan;
        };

        template <> struct fftw_types<double> {
            using complex                      = fftw_complex;
            using plan                         = fftw_plan_s;
            static constexpr auto plan_guru    = fftw_plan_guru64_dft;
            static constexpr auto plan_dft     = fftw_plan_dft;
            static constexpr auto execute_dft  = fftw_execute_dft;
            static constexpr auto destroy_plan = fftw_destroy_plan;
        };
        class FFTLock {
        protected:
            static std::mutex lock;
        };
        std::mutex FFTLock::lock;
        template <class T> class SingleFFTPlan : FFTLock {
        public:
            using FFTWComplex = typename fftw_types<T>::complex;

            SingleFFTPlan(int dimension, const hoNDArray<std::complex<T>>& input, hoNDArray<std::complex<T>>& output,
                bool forward) {
                std::lock_guard<std::mutex> guard(lock);

                const auto& dimensions = input.dimensions();
                size_t stride
                    = std::accumulate(dimensions.begin(), dimensions.begin() + dimension, 1, std::multiplies<>());

                auto fftw_dimensions = fftw_iodim64{ static_cast<ptrdiff_t>(dimensions[dimension]), static_cast<ptrdiff_t>(stride), static_cast<ptrdiff_t>(stride) };

                plan = fftw_types<T>::plan_guru(1, &fftw_dimensions, 0, nullptr, (FFTWComplex*)input.data(),
                    (FFTWComplex*)output.data(), forward ? FFTW_FORWARD : FFTW_BACKWARD, FFTW_ESTIMATE);
            }

            ~SingleFFTPlan() {
                std::lock_guard<std::mutex> guard(lock);
                fftw_types<T>::destroy_plan(plan);
            }

            void execute(const std::complex<T>* input, std::complex<T>* output) {
                fftw_types<T>::execute_dft(plan, (FFTWComplex*)input, (FFTWComplex*)output);
            }

        private:
            typename fftw_types<T>::plan* plan;
        };

        template <class T> class ContigousFFTPlan : FFTLock {
        public:
            using FFTWComplex = typename fftw_types<T>::complex;
            ContigousFFTPlan(
                int rank, const hoNDArray<std::complex<T>>& input, hoNDArray<std::complex<T>>& output, bool forward) {
                std::lock_guard<std::mutex> guard(lock);

                const auto& dimensions = input.dimensions();

                auto strides = std::vector<size_t>(rank + 1, 1);
                std::partial_sum(
                    dimensions.begin(), dimensions.begin() + rank, strides.begin() + 1, std::multiplies<>());

                auto fftw_dimensions = std::vector<fftw_iodim64>(rank);

                for (int i = 0; i < rank; i++) {
                    fftw_dimensions[i] = { (int64_t)dimensions[i], (int64_t)strides[i], (int64_t)strides[i] };
                }
                std::reverse(fftw_dimensions.begin(),fftw_dimensions.end());
                plan = fftw_types<T>::plan_guru(rank, fftw_dimensions.data(), 0, nullptr, (FFTWComplex*)input.data(),
                    (FFTWComplex*)output.data(), forward ? FFTW_FORWARD : FFTW_BACKWARD, FFTW_ESTIMATE);

                if (plan == nullptr) throw std::runtime_error("Illegal FFT plan created");
            }
            ~ContigousFFTPlan() {
                std::lock_guard<std::mutex> guard(lock);
                fftw_types<T>::destroy_plan(plan);
            }

            void execute(const std::complex<T>* input, std::complex<T>* output) {
                fftw_types<T>::execute_dft(plan, (FFTWComplex*)input, (FFTWComplex*)output);
            }

        private:
            typename fftw_types<T>::plan* plan;
        };

        int contigous_rank(const boost::container::flat_set<int>& dimensions) {
            if (!dimensions.count(0))
                return 0;

            int rank = std::distance(std::adjacent_find(dimensions.begin(), dimensions.end(),
                                         [](auto val1, auto val2) { return val1 != val2 - 1; }),
                dimensions.end());
            return rank;
        }

        template <typename T>
        static void contigous_fftn(const hoNDArray<std::complex<T>>& input, hoNDArray<std::complex<T>>& output, int rank,
            bool forward, bool normalize) {

            auto plan = ContigousFFTPlan<T>(rank, input, output, forward);
            size_t batch_size
                = std::accumulate(input.dimensions().begin(), input.dimensions().begin() + rank, 1, std::multiplies<>());
            size_t batches = input.size() / batch_size;

#pragma omp parallel for default(none) shared(plan,  input, output, batches, batch_size)
            for (long long i = 0; i < batches; i++) {

                plan.execute(input.data() + i * batch_size, output.data() + i * batch_size);
            }

            if (normalize)
                output *= T(1) / std::sqrt<T>(batch_size);
        }

        template <typename T>
        static void single_fft(int dimension, const hoNDArray<std::complex<T>>& a, hoNDArray<std::complex<T>>& r,
            bool forward, bool normalize) {
            assert(dimension >= 0);
            auto plan              = SingleFFTPlan<T>(dimension, a, r, forward);
            const auto& dimensions = a.dimensions();
            size_t inner_batches
                = std::accumulate(dimensions.begin(), dimensions.begin() + dimension, 1, std::multiplies<>());
            size_t outer_batches
                = std::accumulate(dimensions.begin() + dimension + 1, dimensions.end(), 1, std::multiplies<>());
            size_t outer_batchsize = inner_batches * dimensions[dimension];

#pragma omp parallel for default(none) shared(plan, a, r , outer_batches, inner_batches, outer_batchsize ) collapse(2)
            for (long long outer = 0; outer < outer_batches; outer++) {
                for (long long inner = 0; inner < inner_batches; inner++) {
                    plan.execute(
                        a.data() + inner + outer * outer_batchsize, r.data() + inner + outer * outer_batchsize);
                }
            }

            if (normalize)
                r *= T(1) / std::sqrt<T>(dimensions[dimension]);
        }
    }

    static inline size_t fftshiftPivot(size_t x) {
        return (x + 1) / 2;
    }

    static inline size_t ifftshiftPivot(size_t x) {
        return x - (x + 1) / 2;
    }

    namespace {
        template <typename T>
        inline void fftshift(const std::complex<T>* a, std::complex<T>* r, size_t stride, size_t x, size_t pivot) {
            std::rotate_copy(a, a + pivot, a + x, r);
        }

        template <typename T, typename... INDICES>
        inline void fftshift(const std::complex<T>* a, std::complex<T>* r, size_t stride, size_t n, size_t pivot,
            size_t n2, INDICES... indices) {
            for (size_t i = 0; i < n; i++) {
                auto line_begin  = a + i * stride;
                size_t new_y     = i < pivot ? i + pivot : i - pivot;
                auto output_line = r + new_y * stride;
                fftshift(line_begin, output_line, stride / n2, n2, indices...);
            }
        }

        template <typename T>
        inline void fftshift(std::complex<T>* a, std::complex<T>* a2, std::vector<std::complex<T>>& buffer,
            size_t stride, size_t ny, size_t pivoty, size_t nx, size_t pivotx) {
            for (size_t iy = 0; iy < (ny + 1) / 2; iy++) {
                auto line_begin = a + iy * stride;
                auto line_pivot = line_begin + pivotx;
                auto line_end   = line_begin + stride;

                auto line_begin2 = a2 + ((iy + pivoty) % ny) * stride;
                auto line_pivot2 = line_begin2 + pivotx;
                auto line_end2   = line_begin2 + stride;
                std::rotate_copy(line_begin2, line_pivot2, line_end2, buffer.begin());
                std::rotate_copy(line_begin, line_pivot, line_end, line_begin2);
                std::copy(buffer.begin(), buffer.end(), line_begin);
            }
        }

        template <typename T, typename... INDICES>
        inline void fftshift(std::complex<T>* a, std::complex<T>* a2, std::vector<std::complex<T>>& buffer,
            size_t stride, size_t n, size_t pivot, size_t n2, INDICES... indices) {

            for (size_t i = 0; i < n; i++) {
                auto line_begin  = a + i * stride;
                size_t new_y     = (i + pivot) % n;
                auto line_begin2 = a2 + new_y * stride;
                fftshift(line_begin, line_begin2, buffer, stride / n2, n2, indices...);
            }
        }

    }

    template <typename T> static void fftshiftPivot1D(std::complex<T>* a, size_t x, size_t n, size_t pivot) {

#pragma omp parallel for shared(n, x, pivot, a) if (n > 256) default(none)
        for (long long counter = 0; counter < (long long)n; counter++) {
            std::rotate(a + counter * x, a + counter * x + pivot, a + x + counter * x);
        }
    }

    template <typename T>
    static void fftshiftPivot1D(const std::complex<T>* a, std::complex<T>* r, size_t x, size_t n, size_t pivot) {

#pragma omp parallel for shared(n, x, pivot, a, r) if (n > 256) default(none)
        for (long long counter = 0; counter < (long long)n; counter++) {
            std::rotate_copy(a + counter * x, a + counter * x + pivot, a + x + counter * x, r + counter * x);
        }
    }

    template <typename T> void hoNDFFT<T>::fftshift1D(hoNDArray<std::complex<T>>& a) {
        size_t x           = a.get_size(0);
        size_t pivot       = fftshiftPivot(x);
        size_t numOfShifts = a.get_number_of_elements() / x;
        fftshiftPivot1D(a.begin(), x, numOfShifts, pivot);
    }

    template <typename T>
    void hoNDFFT<T>::fftshift1D(const hoNDArray<std::complex<T>>& a, hoNDArray<std::complex<T>>& r) {
        if (!r.dimensions_equal(&a)) {
            r = a;
        }

        size_t x           = a.get_size(0);
        size_t pivot       = fftshiftPivot(x);
        size_t numOfShifts = a.get_number_of_elements() / x;
        fftshiftPivot1D(a.begin(), r.begin(), x, numOfShifts, pivot);
    }

    template <typename T> void hoNDFFT<T>::ifftshift1D(hoNDArray<std::complex<T>>& a) {
        size_t x           = a.get_size(0);
        size_t pivot       = ifftshiftPivot(x);
        size_t numOfShifts = a.get_number_of_elements() / x;

        fftshiftPivot1D(a.begin(), x, numOfShifts, pivot);
    }

    template <typename T>
    void hoNDFFT<T>::ifftshift1D(const hoNDArray<std::complex<T>>& a, hoNDArray<std::complex<T>>& r) {
        if (!r.dimensions_equal(&a)) {
            r = a;
        }

        size_t x           = a.get_size(0);
        size_t pivot       = ifftshiftPivot(x);
        size_t numOfShifts = a.get_number_of_elements() / x;

        fftshiftPivot1D(a.begin(), r.begin(), x, numOfShifts, pivot);
    }

    template <typename T>
    static void fftshiftPivot2D(
        const std::complex<T>* a, std::complex<T>* r, size_t x, size_t y, size_t n, size_t pivotx, size_t pivoty) {
        if (a == NULL || r == NULL)
            throw std::runtime_error("hoNDFFT::fftshiftPivot2D: void ptr provided");
        assert(a != r);

#pragma omp parallel for shared(a, r, x, y, n, pivotx, pivoty) if (n > 16) default(none)
        for (long long tt = 0; tt < (long long)n; tt++) {
            const std::complex<T>* ac = a + tt * x * y;
            std::complex<T>* rc       = r + tt * x * y;
            fftshift(ac, rc, x, y, pivoty, x, pivotx);
        }
    }

    template <typename T>
    static void fftshiftPivot2D(std::complex<T>* a, size_t x, size_t y, size_t n, size_t pivotx, size_t pivoty) {

        if (a == NULL)
            throw std::runtime_error("hoNDFFT::fftshiftPivot2D: void ptr provided");

#pragma omp parallel if (n > 16) default(shared)
        {
            std::vector<std::complex<T>> buffer(x);

#pragma omp for
            for (long long tt = 0; tt < (long long)n; tt++) {
                std::complex<T>* ac = a + tt * x * y;
                fftshift(ac, ac, buffer, x, y, pivoty, x, pivotx);
            }
        }
    }

    template <typename T>
    static inline void fftshift2D(const std::complex<T>* a, std::complex<T>* r, size_t x, size_t y, size_t n) {

        if (a == NULL || r == NULL)
            throw std::runtime_error("hoNDFFT::fftshift2D: void ptr provided");

        size_t pivotx = fftshiftPivot(x);
        size_t pivoty = fftshiftPivot(y);

        fftshiftPivot2D(a, r, x, y, n, pivotx, pivoty);
    }

    template <typename T> inline void hoNDFFT<T>::fftshift2D(hoNDArray<ComplexType>& a) {
        size_t n = a.get_number_of_elements() / (a.get_size(0) * a.get_size(1));
        fftshiftPivot2D(
            a.data(), a.get_size(0), a.get_size(1), n, fftshiftPivot(a.get_size(0)), fftshiftPivot(a.get_size(1)));
    }

    template <typename T>
    inline void hoNDFFT<T>::fftshift2D(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r = a;
        }

        size_t n = a.get_number_of_elements() / (a.get_size(0) * a.get_size(1));
        fftshiftPivot2D(a.begin(), r.begin(), a.get_size(0), a.get_size(1), n, fftshiftPivot(a.get_size(0)),
            fftshiftPivot(a.get_size(1)));
    }

    template <typename T> inline void hoNDFFT<T>::ifftshift2D(hoNDArray<ComplexType>& a) {
        size_t n = a.get_number_of_elements() / (a.get_size(0) * a.get_size(1));
        fftshiftPivot2D(
            a.begin(), a.get_size(0), a.get_size(1), n, ifftshiftPivot(a.get_size(0)), ifftshiftPivot(a.get_size(1)));
    }

    template <typename T>
    inline void hoNDFFT<T>::ifftshift2D(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r = a;
        }
        size_t n = a.get_number_of_elements() / (a.get_size(0) * a.get_size(1));
        fftshiftPivot2D(a.begin(), r.begin(), a.get_size(0), a.get_size(1), n, ifftshiftPivot(a.get_size(0)),
            ifftshiftPivot(a.get_size(1)));
    }

    template <typename T>
    void fftshiftPivot3D(const std::complex<T>* a, std::complex<T>* r, size_t x, size_t y, size_t z, size_t n,
        size_t pivotx, size_t pivoty, size_t pivotz) {

        if (a == NULL || r == NULL)
            throw std::runtime_error("hoNDFFT::fftshift2D: void ptr provided");

        long long tt;

#pragma omp parallel for private(tt) shared(a, r, x, y, z, n, pivotx, pivoty, pivotz) if (n > 16) default(none)
        for (tt = 0; tt < (long long)n; tt++) {
            const std::complex<T>* ac = a + tt * x * y * z;
            std::complex<T>* rc       = r + tt * x * y * z;
            fftshift(ac, rc, x * y, z, pivotz, y, pivoty, x, pivotx);
        }
    }

    template <typename T>
    void fftshiftPivot3D(
        std::complex<T>* a, size_t x, size_t y, size_t z, size_t n, size_t pivotx, size_t pivoty, size_t pivotz) {

        if (a == NULL)
            throw std::runtime_error("hoNDFFT::fftshiftPivot3D: void ptr provided");

        long long tt;

#pragma omp parallel private(tt)  if (n > 16) default(shared)
        {
            std::vector<std::complex<T>> buffer(x);

#pragma omp for
            for (tt = 0; tt < (long long)n; tt++) {
                std::complex<T>* ac = a + tt * x * y * z;
                fftshift(ac, ac, buffer, x * y, z, pivotz, y, pivoty, x, pivotx);
            }
        }
    }

    template <typename T>
    inline void fftshift3D(const std::complex<T>* a, std::complex<T>* r, size_t x, size_t y, size_t z, size_t n) {

        if (a == NULL || r == NULL)
            throw std::runtime_error("hoNDFFT::fftshift3D: void ptr provided");

        size_t pivotx = fftshiftPivot(x);
        size_t pivoty = fftshiftPivot(y);
        size_t pivotz = fftshiftPivot(z);

        fftshiftPivot3D(a, r, x, y, z, n, pivotx, pivoty, pivotz);
    }

    template <typename T>
    inline void ifftshift3D(const std::complex<T>* a, std::complex<T>* r, size_t x, size_t y, size_t z, size_t n) {

        if (a == NULL || r == NULL)
            throw std::runtime_error("hoNDFFT::ifftshift3D: void ptr provided");

        size_t pivotx = ifftshiftPivot(x);
        size_t pivoty = ifftshiftPivot(y);
        size_t pivotz = ifftshiftPivot(z);

        fftshiftPivot3D(a, r, x, y, z, n, pivotx, pivoty, pivotz);
    }

    template <typename T> inline void fftshift3D(std::complex<T>* a, size_t x, size_t y, size_t z, size_t n) {
        if (a == NULL)
            throw std::runtime_error("hoNDFFT::fftshift3D: void ptr provided");
        size_t pivotx = fftshiftPivot(x);
        size_t pivoty = fftshiftPivot(y);
        size_t pivotz = fftshiftPivot(z);
        fftshiftPivot3D(a, x, y, z, n, pivotx, pivoty, pivotz);
    }

    template <typename T> inline void ifftshift3D(std::complex<T>* a, size_t x, size_t y, size_t z, size_t n) {
        if (a == NULL)
            throw std::runtime_error("hoNDFFT::ifftshift3D: void ptr provided");

        size_t pivotx = ifftshiftPivot(x);
        size_t pivoty = ifftshiftPivot(y);
        size_t pivotz = ifftshiftPivot(z);

        fftshiftPivot3D(a, x, y, z, n, pivotx, pivoty, pivotz);
    }

    template <typename T> inline void hoNDFFT<T>::fftshift3D(hoNDArray<ComplexType>& a) {
        size_t n = a.get_number_of_elements() / (a.get_size(0) * a.get_size(1) * a.get_size(2));

        fftshiftPivot3D(a.begin(), a.get_size(0), a.get_size(1), a.get_size(2), n,
                        fftshiftPivot(a.get_size(0)),
                        fftshiftPivot(a.get_size(1)),
                        fftshiftPivot(a.get_size(2)));
    }

    template <typename T>
    inline void hoNDFFT<T>::fftshift3D(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r = a;
        }

        size_t n = a.get_number_of_elements() / (a.get_size(0) * a.get_size(1) * a.get_size(2));
        fftshiftPivot3D(a.begin(), r.begin(), a.get_size(0), a.get_size(1), a.get_size(2), n,
            fftshiftPivot(a.get_size(0)), fftshiftPivot(a.get_size(1)), fftshiftPivot(a.get_size(2)));
    }

    template <typename T> inline void hoNDFFT<T>::ifftshift3D(hoNDArray<ComplexType>& a) {
        size_t n = a.get_number_of_elements() / (a.get_size(0) * a.get_size(1) * a.get_size(2));
        fftshiftPivot3D(a.begin(), a.get_size(0), a.get_size(1), a.get_size(2), n, ifftshiftPivot(a.get_size(0)),
            ifftshiftPivot(a.get_size(1)), ifftshiftPivot(a.get_size(2)));
    }

    template <typename T>
    inline void hoNDFFT<T>::ifftshift3D(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r = a;
        }
        size_t n = a.get_number_of_elements() / (a.get_size(0) * a.get_size(1) * a.get_size(2));
        fftshiftPivot3D(a.begin(), r.begin(), a.get_size(0), a.get_size(1), a.get_size(2), n,
            ifftshiftPivot(a.get_size(0)), ifftshiftPivot(a.get_size(1)), ifftshiftPivot(a.get_size(2)));
    }

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

    template <typename T> inline void hoNDFFT<T>::fft1(hoNDArray<ComplexType>& a) {
        contigous_fftn(a, a, 1, true, true);
    }

    template <typename T> inline void hoNDFFT<T>::ifft1(hoNDArray<ComplexType>& a) {
        contigous_fftn(a, a, 1, false, true);
    }

    template <typename T> inline void hoNDFFT<T>::fft1(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r.create(a.dimensions());
        }

        contigous_fftn(a, r, 1, true, true);
    }

    template <typename T> inline void hoNDFFT<T>::ifft1(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r.create(a.dimensions());
        }
        contigous_fftn(a, r, 1, false, true);
    }

    template <typename T> inline void hoNDFFT<T>::fft1c(hoNDArray<ComplexType>& a) {
        ifftshift1D(a);
        fft1(a);
        fftshift1D(a);
    }

    template <typename T> inline void hoNDFFT<T>::ifft1c(hoNDArray<ComplexType>& a) {
        ifftshift1D(a);
        ifft1(a);
        fftshift1D(a);
    }

    template <typename T> inline void hoNDFFT<T>::fft1c(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        ifftshift1D(a, r);
        fft1(r);
        fftshift1D(r);
    }

    template <typename T> inline void hoNDFFT<T>::ifft1c(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        ifftshift1D(a, r);
        ifft1(r);
        fftshift1D(r);
    }

    template <typename T>
    inline void hoNDFFT<T>::fft1c(
        const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r, hoNDArray<ComplexType>& buf) {
        ifftshift1D(a, r);
        fft1(r, buf);
        fftshift1D(buf, r);
    }

    template <typename T>
    inline void hoNDFFT<T>::ifft1c(
        const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r, hoNDArray<ComplexType>& buf) {
        ifftshift1D(a, r);
        ifft1(r, buf);
        fftshift1D(buf, r);
    }

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

    template <typename T> inline void hoNDFFT<T>::fft2(hoNDArray<ComplexType>& a) {
        contigous_fftn(a, a, 2, true, true);
    }

    template <typename T> inline void hoNDFFT<T>::ifft2(hoNDArray<ComplexType>& a) {
        contigous_fftn(a, a, 2, false, true);
    }

    template <typename T> inline void hoNDFFT<T>::fft2(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r.create(a.dimensions());
        }

        contigous_fftn(a, r, 2, true, true);
    }

    template <typename T> inline void hoNDFFT<T>::ifft2(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r.create(a.dimensions());
        }

        contigous_fftn(a, r, 2, false, true);
    }

    template <typename T> inline void hoNDFFT<T>::fft2c(hoNDArray<ComplexType>& a) {
        ifftshift2D(a);
        fft2(a);
        fftshift2D(a);
    }

    template <typename T> inline void hoNDFFT<T>::ifft2c(hoNDArray<ComplexType>& a) {
        ifftshift2D(a);
        ifft2(a);
        fftshift2D(a);
    }

    template <typename T> inline void hoNDFFT<T>::fft2c(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        ifftshift2D(a, r);
        fft2(r);
        fftshift2D(r);
    }

    template <typename T> inline void hoNDFFT<T>::ifft2c(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        ifftshift2D(a, r);
        ifft2(r);
        fftshift2D(r);
    }

    template <typename T>
    inline void hoNDFFT<T>::fft2c(
        const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r, hoNDArray<ComplexType>& buf) {
        ifftshift2D(a, r);
        fft2(r, buf);
        fftshift2D(buf, r);
    }

    template <typename T>
    inline void hoNDFFT<T>::ifft2c(
        const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r, hoNDArray<ComplexType>& buf) {
        ifftshift2D(a, r);
        ifft2(r, buf);
        fftshift2D(buf, r);
    }

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

    template <typename T> inline void hoNDFFT<T>::fft3(hoNDArray<ComplexType>& a) {
        contigous_fftn(a, a, 3, true, true);
    }

    template <typename T> inline void hoNDFFT<T>::ifft3(hoNDArray<ComplexType>& a) {
        contigous_fftn(a, a, 3, false, true);
    }

    template <typename T> inline void hoNDFFT<T>::fft3(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r.create(a.dimensions());
        }

        contigous_fftn(a, r, 3, true, true);
    }

    template <typename T> inline void hoNDFFT<T>::ifft3(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        if (!r.dimensions_equal(&a)) {
            r.create(a.dimensions());
        }

        contigous_fftn(a, r, 3, false, true);
    }

    template <typename T> inline void hoNDFFT<T>::fft3c(hoNDArray<ComplexType>& a) {
        ifftshift3D(a);
        fft3(a);
        fftshift3D(a);
    }

    template <typename T> inline void hoNDFFT<T>::ifft3c(hoNDArray<ComplexType>& a) {
        ifftshift3D(a);
        ifft3(a);
        fftshift3D(a);
    }

    template <typename T> inline void hoNDFFT<T>::fft3c(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        ifftshift3D(a, r);
        fft3(r);
        fftshift3D(r);
    }

    template <typename T> inline void hoNDFFT<T>::ifft3c(const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r) {
        ifftshift3D(a, r);
        ifft3(r);
        fftshift3D(r);
    }

    template <typename T>
    inline void hoNDFFT<T>::fft3c(
        const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r, hoNDArray<ComplexType>& buf) {
        ifftshift3D(a, r);
        fft3(r, buf);
        fftshift3D(buf, r);
    }

    template <typename T>
    inline void hoNDFFT<T>::ifft3c(
        const hoNDArray<ComplexType>& a, hoNDArray<ComplexType>& r, hoNDArray<ComplexType>& buf) {
        ifftshift3D(a, r);
        ifft3(r, buf);
        fftshift3D(buf, r);
    }

    template <typename T> void fft1(hoNDArray<std::complex<T>>& a, bool forward) {
        hoNDArray<std::complex<T>> res(a);
        fft1(res, a, forward);
    }

    template <typename T> void fft2(hoNDArray<std::complex<T>>& a, bool forward) {
        hoNDArray<std::complex<T>> res(a);
        fft2(res, a, forward);
    }

    template <typename T> void fft3(hoNDArray<std::complex<T>>& a, bool forward) {
        hoNDArray<std::complex<T>> res(a);
        fft3(res, a, forward);
    }

    template <typename T> void fft1(hoNDArray<std::complex<T>>& a, hoNDArray<std::complex<T>>& r, bool forward) {
        contigous_fftn(a, r, 1, forward, true);
    }

    template <typename T> void fft2(hoNDArray<std::complex<T>>& a, hoNDArray<std::complex<T>>& r, bool forward) {
        contigous_fftn(a, r, 2, forward, true);
    }

    template <typename T> void fft3(hoNDArray<std::complex<T>>& a, hoNDArray<std::complex<T>>& r, bool forward) {
        contigous_fftn(a, r, 3, forward, true);
    }

    template <typename T>
    void fftn(const hoNDArray<std::complex<T>>& a, hoNDArray<std::complex<T>>& r, const std::vector<size_t>& dimensions,
        bool forward) {

        auto dimensions_set = boost::container::flat_set<int>(dimensions.begin(), dimensions.end());
        int c_rank          = contigous_rank(dimensions_set);
        if (c_rank)
            contigous_fftn(a, r, c_rank, forward, false);

        for (auto it = dimensions_set.lower_bound(c_rank); it != dimensions_set.end(); ++it) {
            single_fft(*it,a, r, forward, false);
        }

        size_t batch_size = 1;
        for (auto d : dimensions)
            batch_size *= a.get_size(d);

        T fftRatio = T(1.0 / std::sqrt(T(batch_size)));
        r *= fftRatio;
    }

    // TODO: implement more optimized threading strategy
    inline int get_num_threads_fft1(size_t n0, size_t num) {
        if (omp_get_max_threads() == 1)
            return 1;

        if (n0 * num > 1024 * 128) {
            return omp_get_max_threads();
        } else if (n0 * num > 512 * 128) {
            return ((omp_get_max_threads() > 8) ? 8 : omp_get_max_threads());
        } else if (n0 * num > 256 * 128) {
            return ((omp_get_max_threads() > 4) ? 4 : omp_get_max_threads());
        } else if (n0 * num > 128 * 128) {
            return 2;
        }

        return 1;
    }

    inline int get_num_threads_fft2(size_t n0, size_t n1, size_t num) {
        if (omp_get_max_threads() == 1)
            return 1;

        if (n0 * n1 * num > 128 * 128 * 64) {
            return omp_get_max_threads();
        } else if (n0 * n1 * num > 128 * 128 * 32) {
            return ((omp_get_max_threads() > 8) ? 8 : omp_get_max_threads());
        } else if (n0 * n1 * num > 128 * 128 * 16) {
            return ((omp_get_max_threads() > 4) ? 4 : omp_get_max_threads());
        } else if (n0 * n1 * num > 128 * 128 * 8) {
            return 2;
        }

        return 1;
    }

    inline int get_num_threads_fft3(size_t n0, size_t n1, size_t n2, size_t num) {
        if (omp_get_max_threads() == 1)
            return 1;

        if (num >= omp_get_max_threads()) {
            return omp_get_max_threads();
        }

        return 1;
    }

    int get_num_threads_fftn(const std::vector<int>& dimensions, size_t num) {

        switch (dimensions.size()) {
        case 1:
            return get_num_threads_fft1(dimensions[0], num);
        case 2:
            return get_num_threads_fft2(dimensions[0], dimensions[1], num);
        case 3:
            return get_num_threads_fft3(dimensions[0], dimensions[1], dimensions[2], num);
        default:
            return std::min<long long>(omp_get_max_threads(), num);
        }
    }

    template <typename T> void hoNDFFT<T>::fft(hoNDArray<ComplexType>* input, unsigned int dim_to_transform) {
        single_fft(dim_to_transform, *input, *input, true, true);
    }
    template <typename T> void hoNDFFT<T>::ifft(hoNDArray<ComplexType>* input, unsigned int dim_to_transform) {
        single_fft(dim_to_transform, *input, *input, false, true);
    }
    template <typename T> void hoNDFFT<T>::fft(hoNDArray<ComplexType>* input) {
        contigous_fftn(*input, *input, input->get_number_of_dimensions(), true, true);
    }
    template <typename T> void hoNDFFT<T>::ifft(hoNDArray<ComplexType>* input) {
        contigous_fftn(*input, *input, input->get_number_of_dimensions(), true, true);
    }
    template <typename T> void hoNDFFT<T>::fft(hoNDArray<complext<T>>* input, unsigned int dim_to_transform) {
        fft(reinterpret_cast<hoNDArray<ComplexType>*>(input), dim_to_transform);
    }
    template <typename T> void hoNDFFT<T>::ifft(hoNDArray<complext<T>>* input, unsigned int dim_to_transform) {
        ifft(reinterpret_cast<hoNDArray<ComplexType>*>(input), dim_to_transform);
    }
    template <typename T> void hoNDFFT<T>::fft(hoNDArray<complext<T>>* input) {
        fft(reinterpret_cast<hoNDArray<ComplexType>*>(input));
    }
    template <typename T> void hoNDFFT<T>::ifft(hoNDArray<complext<T>>* input) {
        ifft(reinterpret_cast<hoNDArray<ComplexType>*>(input));
    }

    template <class ComplexType, class ENABLER>
    void FFT::fft(hoNDArray<ComplexType>& data, std::vector<size_t> dimensions) {
        std::sort(dimensions.begin(), dimensions.end());
        if (std::adjacent_find(dimensions.begin(), dimensions.end()) != dimensions.end()) {
            throw std::runtime_error("Duplicate dimensions in list to be transformed");
        }
        fftn(data,data,dimensions,true);
    }

    template <class ComplexType, class ENABLER> void FFT::fft(hoNDArray<ComplexType>& data, size_t dimension) {
        single_fft(dimension,data,data,true,true);
    }
    template <class ComplexType, class ENABLER>
    void FFT::ifft(hoNDArray<ComplexType>& data, std::vector<size_t> dimensions) {
        std::sort(dimensions.begin(), dimensions.end());
        if (std::adjacent_find(dimensions.begin(), dimensions.end()) != dimensions.end()) {
            throw std::runtime_error("Duplicate dimensions in list to be transformed");
        }
        fftn(data,data,dimensions,false);
    }

    template <class ComplexType, class ENABLER> void FFT::ifft(hoNDArray<ComplexType>& data, size_t dimension) {
        single_fft(dimension,data,data,false,true);
    }
    template <class ComplexType, class ENABLER>
    hoNDArray<ComplexType> FFT::fft1c(const hoNDArray<ComplexType> &data) {
      hoNDArray<ComplexType> output(data.dimensions());
      hoNDFFT<realType_t<ComplexType>>::instance()->fft1c(data,output);
      return output;
    }
    template <class ComplexType, class ENABLER>
    hoNDArray<ComplexType> FFT::fft2c(const hoNDArray<ComplexType> &data) {
      hoNDArray<ComplexType> output(data.dimensions());
      hoNDFFT<realType_t<ComplexType>>::instance()->fft2c(data,output);
      return output;
    }
    template <class ComplexType, class ENABLER>
    hoNDArray<ComplexType> FFT::fft3c(const hoNDArray<ComplexType> &data) {
      hoNDArray<ComplexType> output(data.dimensions());
      hoNDFFT<realType_t<ComplexType>>::instance()->fft3c(data,output);
      return output;
    }
    template <class ComplexType, class ENABLER>
    hoNDArray<ComplexType> FFT::ifft1c(const hoNDArray<ComplexType> &data) {
      hoNDArray<ComplexType> output(data.dimensions());
      hoNDFFT<realType_t<ComplexType>>::instance()->ifft1c(data, output);
      return output;
    }
    template <class ComplexType, class ENABLER>
    hoNDArray<ComplexType> FFT::ifft2c(const hoNDArray<ComplexType> &data) {
      hoNDArray<ComplexType> output(data.dimensions());
      hoNDFFT<realType_t<ComplexType>>::instance()->ifft2c(data, output);
      return output;
    }
    template <class ComplexType, class ENABLER>
    hoNDArray<ComplexType> FFT::ifft3c(const hoNDArray<ComplexType> &data) {
      hoNDArray<ComplexType> output(data.dimensions());
      hoNDFFT<realType_t<ComplexType>>::instance()->ifft3c(data, output);
      return output;
    }

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

    //
    // Instantiation
    //

    template class EXPORTCPUFFT hoNDFFT<float>;
    template class EXPORTCPUFFT hoNDFFT<double>;

    template void FFT::fft<std::complex<float>>(hoNDArray<std::complex<float>>& data, std::vector<size_t> dimensions);
    template void FFT::fft<std::complex<double>>(hoNDArray<std::complex<double>>& data, std::vector<size_t> dimensions);
    template void FFT::fft<std::complex<float>>(hoNDArray<std::complex<float>>& data, size_t dimensions);
    template void FFT::fft<std::complex<double>>(hoNDArray<std::complex<double>>& data, size_t dimensions);

    template void FFT::ifft<std::complex<float>>(hoNDArray<std::complex<float>>& data, std::vector<size_t> dimensions);
    template void FFT::ifft<std::complex<double>>(hoNDArray<std::complex<double>>& data, std::vector<size_t> dimensions);
    template void FFT::ifft<std::complex<float>>(hoNDArray<std::complex<float>>& data, size_t dimensions);
    template void FFT::ifft<std::complex<double>>(hoNDArray<std::complex<double>>& data, size_t dimensions);

    template hoNDArray<std::complex<float>> FFT::fft1c(const hoNDArray<std::complex<float>> &data);
    template hoNDArray<std::complex<float>> FFT::fft2c(const hoNDArray<std::complex<float>> &data);
    template hoNDArray<std::complex<float>> FFT::fft3c(const hoNDArray<std::complex<float>> &data);
    template hoNDArray<std::complex<float>> FFT::ifft1c(const hoNDArray<std::complex<float>> &data);
    template hoNDArray<std::complex<float>> FFT::ifft2c(const hoNDArray<std::complex<float>> &data);
    template hoNDArray<std::complex<float>> FFT::ifft3c(const hoNDArray<std::complex<float>> &data);

    template hoNDArray<std::complex<double>> FFT::fft1c(const hoNDArray<std::complex<double>> &data);
    template hoNDArray<std::complex<double>> FFT::fft2c(const hoNDArray<std::complex<double>> &data);
    template hoNDArray<std::complex<double>> FFT::fft3c(const hoNDArray<std::complex<double>> &data);
    template hoNDArray<std::complex<double>> FFT::ifft1c(const hoNDArray<std::complex<double>> &data);
    template hoNDArray<std::complex<double>> FFT::ifft2c(const hoNDArray<std::complex<double>> &data);
    template hoNDArray<std::complex<double>> FFT::ifft3c(const hoNDArray<std::complex<double>> &data);

}


/*
 * Copyright [2017] Max Planck Society. All rights reserved.
 *
 * 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/>.
 */

#pragma once

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

namespace solver {

  /** Class that contains all information about the optimization variables
   *  such as upper and lower bounds, initial guess if any, and some
   *  auxiliary functions for easier handling.
   */
  class OptimizationVariable
  {
    public:
      typedef Eigen::Matrix<double, Eigen::Dynamic,              1, Eigen::ColMajor, Eigen::Dynamic, 1> OptVector;
      typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor, Eigen::Dynamic, Eigen::Dynamic> OptMatrix;

    public:
      //! Class constructor and destructor
      OptimizationVariable() : guessValueInitialized_(false) {}
      ~OptimizationVariable() {}

      //! Some alternative initialization functions
      void initialize(const char& type, int rows, int cols, double lBnd, double uBnd, int& startIndexInOptVec );
      void initialize(const char& type, int rows, int cols, OptVector& lBnd, OptVector& uBnd, int& startIndexInOptVec );
      void initialize(const char& type, int rows, int cols, OptMatrix& lBnd, OptMatrix& uBnd, int& startIndexInOptVec );
      void initialize(const char& type, int rows, int cols, double lBnd, double uBnd, int& startIndexInOptVec, double guess );
      void initialize(const char& type, int rows, int cols, double lBnd, double uBnd, int& startIndexInOptVec, OptMatrix& guess );
      void initialize(const char& type, int rows, int cols, OptVector& lBnd, OptVector& uBnd, int& startIndexInOptVec, OptMatrix& guess );
      void initialize(const char& type, int rows, int cols, OptMatrix& lBnd, OptMatrix& uBnd, int& startIndexInOptVec, OptMatrix& guess );

      //! Some helper functions to construct the problem
      int  id(int row, int col) const { return indexPosition_ + this->getColBndInd(col)*rows_ + this->getRowBndInd(row); }
      void constraintIndexToCurrentValue(int index) { lBndMat_.col(index) = guessMat_.col(index); uBndMat_.col(index) = guessMat_.col(index); }
      void setAndConstraintIndexToCurrentValue(int index, double value) { guessMat_.col(index).setConstant(value); this->constraintIndexToCurrentValue(index); }
      void setAndConstraintIndexToCurrentValue(int index, OptVector& value) { guessMat_.col(index) = value; this->constraintIndexToCurrentValue(index); }
      void getValues(OptMatrix& lBnd, OptMatrix& uBnd, OptMatrix& guess, int& size, char& type) const { lBnd = lBndMat_; uBnd = uBndMat_; guess = guessMat_; size = this->getNumElements(); type = type_; }

      //! Some getter and setter methods
      int  getNumRows() const { return rows_; }
      int  getNumCols() const { return cols_; }
      int  getStartIndex() const { return indexPosition_; }
      int  getNumElements() const { return rows_*cols_; }
      void getGuessValue(OptMatrix& guess) const { guess.resize(rows_, cols_); guess = guessMat_; }
      void getGuessValueByCol(int index, OptVector& guess) const { guess.resize(rows_,1); guess = guessMat_.col(index); }
      void setGuessValue(const OptMatrix& guess) { guessMat_ = guess.block(0,0,rows_,cols_); guessValueInitialized_ = true; }

    private:
      //! Some helper functions to construct the problem
      int getRowBndInd(int row) const;
      int getColBndInd(int col) const;
      int getBlockLen(int rini, int cini, int rend, int cend) const { return this->id(rend, cend) - this->id(rini, cini) + 1; }

    private:
      char   type_;
      bool   guessValueInitialized_;
      int    indexPosition_, rows_, cols_;
      OptMatrix  lBndMat_, uBndMat_, guessMat_;
  };
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// 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 "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/graph/bipartite.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct get_bipartite
{
    template <class Graph, class VertexIndex, class PartMap>
    void operator()(Graph& g, VertexIndex vertex_index, PartMap part_map,
                    bool& is_bip, bool find_cycle, vector<size_t>& odd_cycle)
        const
    {
        unchecked_vector_property_map<default_color_type, VertexIndex>
            part(vertex_index, num_vertices(g));

        if (!find_cycle)
        {
            is_bip = is_bipartite(g, vertex_index, part);
        }
        else
        {
            find_odd_cycle(g, vertex_index, part, std::back_inserter(odd_cycle));
            is_bip = odd_cycle.empty();
        }

        parallel_vertex_loop
            (g,
             [&](auto v)
             {
                 part_map[v] = (part[v] == color_traits<default_color_type>::white());
             });
    }

    template <class Graph, class VertexIndex>
    void operator()(Graph& g, VertexIndex vertex_index, dummy_property_map,
                    bool& is_bip, bool find_cycle, vector<size_t>& odd_cycle) const
    {
        if (!find_cycle)
        {
            is_bip = is_bipartite(g, vertex_index);
        }
        else
        {
            find_odd_cycle(g, vertex_index, std::back_inserter(odd_cycle));
            is_bip = odd_cycle.empty();
        }
    }
};

bool is_bipartite(GraphInterface& gi, boost::any part_map, bool find_cycle,
                  boost::python::list cycle)
{
    bool is_bip;
    vector<size_t> vcycle;

    if (part_map.empty())
        part_map = dummy_property_map();

    typedef mpl::push_back<writable_vertex_scalar_properties,
                           dummy_property_map>::type vertex_map_types;

    run_action<graph_tool::detail::never_directed>()
        (gi, std::bind(get_bipartite(), std::placeholders::_1,
                       gi.get_vertex_index(), std::placeholders::_2,
                       std::ref(is_bip), find_cycle, std::ref(vcycle)),
         vertex_map_types())(part_map);

    for (auto v : vcycle)
        cycle.append(v);

    return is_bip;
}


#pragma once

#include <Eigen/Core>
#include <vector>
#include "PointNormal.hh"
#include "../adjacent_pairs.hh"

namespace kt84 {

template <int N>
struct PolylineT {
    typedef Eigen::Matrix<double, N, 1> Point;
    typedef typename std::vector<Point>::iterator               iterator;
    typedef typename std::vector<Point>::reverse_iterator       reverse_iterator;
    typedef typename std::vector<Point>::const_iterator         const_iterator;
    typedef typename std::vector<Point>::const_reverse_iterator const_reverse_iterator;

    std::vector<Point> points;
    bool is_loop;

    PolylineT()
        : is_loop()
    {}
    PolylineT(const std::vector<Point>& points_, bool is_loop_)
        : points (points_ )
        , is_loop(is_loop_)
    {}
    PolylineT(int size_, bool is_loop_)
        : points (size_   )
        , is_loop(is_loop_)
    {}

    int size() const { return static_cast<int>(points.size()); }
    bool empty() const { return points.empty(); }
    void resize(int size_)                     { points.resize(size_); }
    void resize(int size_, const Point& point) { points.resize(size_, point); }
    void clear() { points.clear(); is_loop = false; }
    void push_back(const Point& point) { points.push_back(point); }
    void pop_back ()                   { points.pop_back (); }
    iterator erase(iterator where_) { return points.erase(where_); }
    iterator erase(iterator first, iterator last) { return points.erase(first, last); }
    iterator insert(iterator where_, const Point& val) { return points.insert(where_, val); }
    void     insert(iterator where_, int count, const Point& val) { points.insert(where_, count, val); }
    template <class InputIterator>
    void     insert(iterator where_, InputIterator first, InputIterator last) { points.insert(where_, first, last); }
    iterator                begin()       { return points. begin(); }
    iterator                end  ()       { return points. end  (); }
    reverse_iterator       rbegin()       { return points.rbegin(); }
    reverse_iterator       rend  ()       { return points.rend  (); }
    const_iterator          begin() const { return points. begin(); }
    const_iterator          end  () const { return points. end  (); }
    const_reverse_iterator rbegin() const { return points.rbegin(); }
    const_reverse_iterator rend  () const { return points.rend  (); }
          Point& front()       { return points.front(); }
    const Point& front() const { return points.front(); }
          Point& back ()       { return points.back (); }
    const Point& back () const { return points.back (); }
          Point& operator[](int index)       { return points[index]; }
    const Point& operator[](int index) const { return points[index]; }

    void reverse() {
        if (is_loop) {
            points.push_back(points.front());
            std::reverse(points.begin(), points.end());
            points.pop_back();
        } else {
            std::reverse(points.begin(), points.end());
        }
    }

    void offset_front(const Point& target) {
        Point offset = target - front();
        front() = target;
        for (int i = 1; i < size() - 1; ++i) {
            points[i] += offset * (size() - 1 - i) / (size() - 1.0);
        }
    }

    void offset_back (const Point& target) {
        Point offset = target - back();
        back() = target;
        for (int i = 1; i < size() - 1; ++i) {
            points[i] += offset * i / (size() - 1.0);
        }
    }

    double length() const {
        double result = 0;
        for (auto p : adjacent_pairs(*this, is_loop))
            result += (p.first - p.second).norm();

        return result;
    }

    void resample() { resample(size()); }
    void resample_by_length(double segment_length) { resample(std::max<int>(static_cast<int>(length() / segment_length), 3)); }
    void resample(int target_num_points) {
        int NN = size();

        if (NN < 2 || target_num_points < 2)
            return;

        if (target_num_points == 2) {
            points[1] = back();
            resize(2);
            return;
        }

        PolylineT result(target_num_points, is_loop);

        if (is_loop) {
            push_back(front());
            ++NN;
        }

        std::vector<double> src_len_acc(NN, 0);
        for (int i = 1; i < NN; ++i)
            src_len_acc[i] = src_len_acc[i - 1] + (points[i] - points[i - 1]).norm();

        double tgt_len_segment = src_len_acc.back() / (target_num_points - 1.);
        int src_i = 0;
        double tgt_len_acc = 0;
        int index = 0;
        result[index++] = front();
        while (true) {
            while (tgt_len_acc + tgt_len_segment <= src_len_acc[src_i]) {
                tgt_len_acc += tgt_len_segment;
                double w1 = (tgt_len_acc - src_len_acc[src_i - 1]) / (src_len_acc[src_i] - src_len_acc[src_i - 1]);
                double w0 = 1 - w1;
                Point& p0 = points[src_i - 1];
                Point& p1 = points[src_i];
                Point p = p0 * w0 + p1 * w1;
                result[index++] = p;
                if (index == target_num_points - 1) break;
            }

            if (index == target_num_points - 1) break;

            while (src_len_acc[src_i] <= tgt_len_acc + tgt_len_segment) {
                ++src_i;
            }
        }
        result[index++] = back();

        if (is_loop)
            result.pop_back();

        *this = result;
        return;
    }
    void insert_points_per_segment(int num_inserted_points_per_segment = 3) {
        auto points_temp = points;
        auto insert_pos = points_temp.begin();
        for (auto i : adjacent_pairs(*this, is_loop)) {
            auto d = (i.second - i.first) / (num_inserted_points_per_segment + 1.0);
            Point p = i.first + d;
            ++insert_pos;
            for (int j = 0; j < num_inserted_points_per_segment; ++j, ++insert_pos, p += d)
                insert_pos = points_temp.insert(insert_pos, p);
        }
        points = points_temp;
    }
    void smooth(int num_iter = 1, double weight_first_order = 1.0, double weight_second_order = 0.0, double damping = 0.5) {
        for (int k = 0; k < num_iter; ++k) {
            auto points_old = points;
            for (int i = 0; i < size(); ++i) {
                if (!is_loop && i == 0 || i == size() - 1)
                    continue;

                int i_next  = (i + 1) % size();
                int i_prev  = (i + size() - 1) % size();
                int i_next2 = (i + 2) % size();
                int i_prev2 = (i + size() - 2) % size();

                auto p_prev  = points_old[i_prev ];
                auto p_next  = points_old[i_next ];
                auto p_prev2 = points_old[i_prev2];
                auto p_next2 = points_old[i_next2];

                Point p_first_order  = (p_prev + p_next) / 2.0;
                Point p_second_order = (-p_prev2 + 4.0 * p_prev + 4.0 * p_next - p_next2) / 6.0;

                if (!is_loop && i == 1 || i == size() - 2)
                    p_second_order = p_first_order;         // undefined

                Point p_target = (weight_first_order * p_first_order + weight_second_order * p_second_order) / (weight_first_order + weight_second_order);

                points[i] = damping * points_old[i] + (1 - damping) * p_target;
            }
        }
    }
    Point point_at(double arc_length_parameter) const {
        if (arc_length_parameter < 0 || 1 < arc_length_parameter)
            // arc length parameter should be between 0 and 1
            return Point::Zero();

        if (arc_length_parameter == 0) return front();
        if (arc_length_parameter == 1) return is_loop ? front() : back();

        const int NN = size();
        const double target_length = length() * arc_length_parameter;

        double length_acc = 0;
        for (int i = 0; i < NN; ++i) {
            if (i == NN - 1 && !is_loop)
                break;

            auto& p0 = points[i];
            auto& p1 = points[(i + 1) % NN];

            double segment_length = (p1 - p0).norm();
            if (length_acc <= target_length && target_length < length_acc + segment_length) {
                double t = (target_length - length_acc) / segment_length;
                return (1 - t) * p0 + t * p1;
            }

            length_acc += segment_length;
        }

        // something wrong happened
        return Point::Zero();
    }
};

typedef PolylineT<2> Polyline2d;
typedef PolylineT<3> Polyline3d;

struct Polyline_PointNormal {
    typedef std::vector<PointNormal>::iterator               iterator;
    typedef std::vector<PointNormal>::reverse_iterator       reverse_iterator;
    typedef std::vector<PointNormal>::const_iterator         const_iterator;
    typedef std::vector<PointNormal>::const_reverse_iterator const_reverse_iterator;

    std::vector<PointNormal> points;
    bool is_loop;

    Polyline_PointNormal()
        : is_loop()
    {}
    Polyline_PointNormal(const std::vector<PointNormal>& points_, bool is_loop_)
        : points (points_ )
        , is_loop(is_loop_)
    {}
    Polyline_PointNormal(int size_, bool is_loop_)
        : points (size_   )
        , is_loop(is_loop_)
    {}

    int size() const { return static_cast<int>(points.size()); }
    bool empty() const { return points.empty(); }
    void resize(int size_)                     { points.resize(size_); }
    void resize(int size_, const PointNormal& point) { points.resize(size_, point); }
    void clear() { points.clear(); }
    void push_back(const PointNormal& point) { points.push_back(point); }
    void pop_back ()                   { points.pop_back (); }
    // Changed const_iterator to iterator to meet "defect" in standard
    iterator  erase(iterator where_) { return points.erase(where_); }
    iterator  erase(iterator first, iterator last) { return points.erase(first, last); }
    iterator insert(iterator where_, const PointNormal& val) { return points.insert(where_, val); }
    void     insert(iterator where_, int count, const PointNormal& val) { points.insert(where_, count, val); }
    template <class InputIterator>
    void     insert(iterator where_, InputIterator first, InputIterator last) { points.insert(where_, first, last); }
    iterator                begin()       { return points. begin(); }
    iterator                end  ()       { return points. end  (); }
    reverse_iterator       rbegin()       { return points.rbegin(); }
    reverse_iterator       rend  ()       { return points.rend  (); }
    const_iterator          begin() const { return points. begin(); }
    const_iterator          end  () const { return points. end  (); }
    const_reverse_iterator rbegin() const { return points.rbegin(); }
    const_reverse_iterator rend  () const { return points.rend  (); }
          PointNormal& front()       { return points.front(); }
    const PointNormal& front() const { return points.front(); }
          PointNormal& back ()       { return points.back (); }
    const PointNormal& back () const { return points.back (); }
          PointNormal& operator[](int index)       { return points[index]; }
    const PointNormal& operator[](int index) const { return points[index]; }

    void reverse() {
        if (is_loop) {
            std::reverse(points.begin(), points.end());
        } else {
            points.push_back(points.front());
            std::reverse(points.begin(), points.end());
            points.pop_back();
        }
    }

    void offset_front(const PointNormal& target) {
        PointNormal offset = target - front();
        front() = target;
        for (int i = 1; i < size() - 1; ++i) {
            points[i] += offset * (size() - 1 - i) / (size() - 1.0);
            pn_normalize(points[i]);
        }
    }

    void offset_back (const PointNormal& target) {
        PointNormal offset = target - back();
        back() = target;
        for (int i = 1; i < size() - 1; ++i) {
            points[i] += offset * i / (size() - 1.0);
            pn_normalize(points[i]);
        }
    }

    double length() const {
        double result = 0;

        for (auto p : adjacent_pairs(*this, is_loop))
            result += pn_norm(p.first - p.second);

        return result;
    }

    void resample() { resample(size()); }
    void resample_by_length(double segment_length) { resample(std::max<int>(static_cast<int>(length() / segment_length), 3)); }
    void resample(int target_num_points) {
        int N = size();

        if (N < 2 || target_num_points < 2)
            return;

        if (target_num_points == 2) {
            points[1] = back();
            resize(2);
            return;
        }

        Polyline_PointNormal result(target_num_points, is_loop);

        if (is_loop) {
            push_back(front());
            ++N;
        }

        std::vector<double> src_len_acc(N, 0);
        for (int i = 1; i < N; ++i)
            src_len_acc[i] = src_len_acc[i - 1] + pn_norm(points[i] - points[i - 1]);

        double tgt_len_segment = src_len_acc.back() / (target_num_points - 1.);
        int src_i = 0;
        double tgt_len_acc = 0;
        int index = 0;
        result[index++] = front();
        while (true) {
            while (tgt_len_acc + tgt_len_segment <= src_len_acc[src_i]) {
                tgt_len_acc += tgt_len_segment;
                double w1 = (tgt_len_acc - src_len_acc[src_i - 1]) / (src_len_acc[src_i] - src_len_acc[src_i - 1]);
                double w0 = 1 - w1;
                PointNormal& pn0 = points[src_i - 1];
                PointNormal& pn1 = points[src_i];
                PointNormal pn = pn0 * w0 + pn1 * w1;
                pn_normalize(pn);
                result[index++] = pn;
                if (index == target_num_points - 1) break;
            }

            if (index == target_num_points - 1) break;

            while (src_len_acc[src_i] <= tgt_len_acc + tgt_len_segment) {
                ++src_i;
            }
        }
        result[index++] = back();

        if (is_loop)
            result.pop_back();

        *this = result;
        return;
    }
    void insert_points_per_segment(int num_inserted_points_per_segment = 3) {
        auto points_temp = points;
        auto insert_pos = points_temp.begin();
        for (auto i : adjacent_pairs(*this, is_loop)) {
            auto d = (i.second - i.first) / (num_inserted_points_per_segment + 1.0);
            PointNormal pn = i.first + d;
            ++insert_pos;
            for (int j = 0; j < num_inserted_points_per_segment; ++j, ++insert_pos, pn += d)
                insert_pos = points_temp.insert(insert_pos, pn_normalized(pn));
        }
        points = points_temp;
    }
    void smooth(int num_iter = 1, double weight_first_order = 1.0, double weight_second_order = 0.0, double damping = 0.5) {
        for (int k = 0; k < num_iter; ++k) {
            auto points_old = points;
            for (int i = 0; i < size(); ++i) {
                if (!is_loop && i == 0 || i == size() - 1)
                    continue;

                int i_next  = (i + 1) % size();
                int i_prev  = (i + size() - 1) % size();
                int i_next2 = (i + 2) % size();
                int i_prev2 = (i + size() - 2) % size();

                auto p_prev  = points_old[i_prev ];
                auto p_next  = points_old[i_next ];
                auto p_prev2 = points_old[i_prev2];
                auto p_next2 = points_old[i_next2];

                PointNormal p_first_order  = (p_prev + p_next) / 2.0;
                PointNormal p_second_order = (-p_prev2 + 4.0 * p_prev + 4.0 * p_next - p_next2) / 6.0;

                if (!is_loop && i == 1 || i == size() - 2)
                    p_second_order = p_first_order;         // undefined

                PointNormal p_target = (weight_first_order * p_first_order + weight_second_order * p_second_order) / (weight_first_order + weight_second_order);

                points[i] = damping * points_old[i] + (1 - damping) * p_target;
                pn_normalize(points[i]);
            }
        }
    }
    PointNormal point_at(double arc_length_parameter) const {
        if (arc_length_parameter < 0 || 1 < arc_length_parameter)
            // arc length parameter should be between 0 and 1
            return PointNormal::Zero();

        if (arc_length_parameter == 0) return front();
        if (arc_length_parameter == 1) return is_loop ? front() : back();

        const int N = size();
        const double target_length = length() * arc_length_parameter;

        double length_acc = 0;
        for (int i = 0; i < N; ++i) {
            if (i == N - 1 && !is_loop)
                break;

            auto& pn0 = points[i];
            auto& pn1 = points[(i + 1) % N];

            double segment_length = pn_norm(pn1 - pn0);
            if (length_acc <= target_length && target_length < length_acc + segment_length) {
                double t = (target_length - length_acc) / segment_length;
                PointNormal pn = (1 - t) * pn0 + t * pn1;
                pn_normalize(pn);
                return pn;
            }

            length_acc += segment_length;
        }

        // something wrong happened
        return PointNormal::Zero();
    }
};

}


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * 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 <iostream>

//#include <boost/random/mersenne_twister.hpp>
#include <boost/random/inversive_congruential.hpp>

#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include "PLOG.hh"

using std::cout ;
using std::endl  ;

void test_0()
{

    boost::mt19937 gener(1);
    boost::normal_distribution<> normal(0,1);
    boost::variate_generator<boost::mt19937&,boost::normal_distribution<> > rng(gener, normal);

    cout << rng() << endl;
    cout << rng() << endl;
    cout << rng() << endl;

    gener.seed(2);
    cout << rng() << endl;
    cout << rng() << endl;
    gener.seed(1);
    cout << rng() << endl;
    gener.seed(2);
    cout << rng() << endl;
    gener.seed(3);
    cout << rng() << endl;

}

int main(int argc, char** argv)
{
    PLOG_(argc, argv);

    test_0();

    return 0 ;
}


#ifndef GUARD_BSGS_H
#define GUARD_BSGS_H

#include <cassert>
#include <memory>
#include <ostream>
#include <stdexcept>
#include <string>
#include <unordered_set>
#include <utility>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>

#include "perm_set.hpp"
#include "timeout.hpp"

namespace mpsym
{

namespace internal
{

class Orbit;
class Perm;
class SchreierStructure;

class BSGSTransversalsBase
{
public:
  virtual ~BSGSTransversalsBase() = default;

  std::shared_ptr<SchreierStructure> schreier_structure(unsigned i) const
  { return _schreier_structures[i]; }

  void reserve_schreier_structure(
    unsigned i, unsigned root, unsigned degree);

  void update_schreier_structure(
    unsigned i, unsigned root, unsigned degree, PermSet const &generators);

  void insert_schreier_structure(
    unsigned i, unsigned root, unsigned degree, PermSet const &generators);

  void clear()
  { _schreier_structures.clear(); }

  virtual std::shared_ptr<SchreierStructure> make_schreier_structure(
    unsigned root, unsigned degree, PermSet const &generators) = 0;

private:
  std::vector<std::shared_ptr<SchreierStructure>> _schreier_structures;
};

template<typename T>
class BSGSTransversals : public BSGSTransversalsBase
{
public:
  virtual ~BSGSTransversals() = default;

private:
  std::shared_ptr<SchreierStructure> make_schreier_structure(
    unsigned root, unsigned degree, PermSet const &generators) override
  { return std::make_shared<T>(degree, root, generators); }
};

struct BSGSOptions;

class BSGS
{
  friend std::ostream &operator<<(std::ostream &os, BSGS const &bsgs);

public:
  using Base = std::vector<unsigned>;
  using order_type = boost::multiprecision::cpp_int;

  struct SolveError : public std::runtime_error
  {
    SolveError()
    : std::runtime_error("failed to solve BSGS")
    {}
  };

  explicit BSGS(unsigned degree = 1);

  BSGS(PermSet const &generators,
       BSGSOptions const *options = nullptr,
       timeout::flag aborted = timeout::unset())
  : BSGS(generators.degree(), generators, options, aborted)
  {}

  BSGS(unsigned degree,
       PermSet const &generators,
       BSGSOptions const *options = nullptr,
       timeout::flag aborted = timeout::unset());

  BSGS(unsigned degree,
       Base const &base,
       PermSet const &strong_generators,
       BSGSOptions const *options = nullptr);

  unsigned degree() const { return _degree; }
  order_type order() const;

  bool is_symmetric() const { return _is_symmetric; }

  Base base() const { return _base; }
  bool base_empty() const { return _base.empty(); }
  unsigned base_size() const { return _base.size(); }
  unsigned base_point(unsigned i) const { return _base[i]; }
  void base_change(std::vector<unsigned> prefix);

  PermSet strong_generators() const { return _strong_generators; }
  PermSet strong_generators(unsigned i) const;

  Orbit orbit(unsigned i) const;
  Perm transversal(unsigned i, unsigned o) const;
  PermSet transversals(unsigned i) const;
  PermSet stabilizers(unsigned i) const;

  std::pair<Perm, unsigned> strip(Perm const &perm, unsigned offs = 0) const;
  bool strips_completely(Perm const &perm) const;

private:
  // transversal initialization
  void transversals_init(BSGSOptions const *options);

  // construction
  void construct_symmetric(std::vector<unsigned> const &support);

  void construct_unknown(PermSet const &generators,
                         BSGSOptions const *options,
                         timeout::flag aborted);

  // schreier sims initialization
  void schreier_sims(PermSet const &generators,
                     BSGSOptions const *options,
                     timeout::flag aborted);

  void schreier_sims(std::vector<PermSet> &strong_generators,
                     std::vector<Orbit> &fundamental_orbits,
                     BSGSOptions const *options,
                     timeout::flag aborted);

  void schreier_sims_random(PermSet const &generators,
                            BSGSOptions const *options,
                            timeout::flag aborted);

  void schreier_sims_random(std::vector<PermSet> &strong_generators,
                            std::vector<Orbit> &fundamental_orbits,
                            BSGSOptions const *options,
                            timeout::flag aborted);

  void schreier_sims_init(PermSet const &generators,
                          std::vector<PermSet> &strong_generators,
                          std::vector<Orbit> &fundamental_orbits);

  void schreier_sims_update_strong_gens(
    unsigned i,
    PermSet new_strong_generators,
    std::vector<PermSet> &strong_generators,
    std::vector<Orbit> &fundamental_orbits);

  void schreier_sims_finish();

  // solvable BSGS initialization
  void solve(PermSet const &generators);

  bool solve_s_normal_closure(PermSet const &generators,
                              Perm const &w,
                              std::pair<Perm, Perm> &conjugates);

  void solve_adjoin_normalizing_generator(Perm const &gen);

  // generator reduction
  void reduce_gens();

  std::unordered_set<Perm> reduce_gens_set_difference(
    std::unordered_set<Perm> const &lhs,
    std::unordered_set<Perm> const &rhs,
    std::unordered_set<Perm> const &base) const;

  // base change
  void swap_base_points(unsigned i);
  void transpose_base_point(unsigned i, unsigned j);
  unsigned insert_redundant_base_point(unsigned bp, unsigned i_min);
  void conjugate(Perm const &conj);

  // convenience methods
  void extend_base(unsigned bp);
  void extend_base(unsigned bp, unsigned i);

  std::shared_ptr<SchreierStructure> schreier_structure(unsigned i) const
  { return _transversals->schreier_structure(i); }

  void reserve_schreier_structure(unsigned i)
  {
    _transversals->reserve_schreier_structure(
      i, base_point(i), _degree);
  }

  void update_schreier_structure(unsigned i, PermSet const &generators)
  {
    _transversals->update_schreier_structure(
      i, base_point(i), _degree, generators);
  }

  void insert_schreier_structure(unsigned i, PermSet const &generators)
  {
    _transversals->insert_schreier_structure(
      i, base_point(i), _degree, generators);
  }

  unsigned _degree;

  std::vector<unsigned> _base;
  std::shared_ptr<BSGSTransversalsBase> _transversals;
  PermSet _strong_generators;

  bool _is_symmetric = false;
  bool _is_alternating = false;
};

std::ostream &operator<<(std::ostream &os, BSGS const &bsgs);

struct BSGSOptions
{
  enum class Construction {
    AUTO,
    SCHREIER_SIMS,
    SCHREIER_SIMS_RANDOM,
    SOLVE
  };

  enum class Transversals {
    EXPLICIT,
    SCHREIER_TREES,
    SHALLOW_SCHREIER_TREES
  };

  static BSGSOptions fill_defaults(BSGSOptions const *options)
  {
    static BSGSOptions default_options;
    return options ? *options : default_options;
  }

  Construction construction = Construction::AUTO;
  Transversals transversals = Transversals::EXPLICIT;

  bool check_sym = true;
  bool reduce_gens = true;

  bool schreier_sims_random_guarantee = true;
  bool schreier_sims_random_use_known_order = true;
  BSGS::order_type schreier_sims_random_known_order = 0;
  int schreier_sims_random_retries = -1;
  unsigned schreier_sims_random_w = 100u;
};

} // namespace internal

} // namespace mpsym

#endif // GUARD_BSGS_H


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & 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
 ******************************************************************************/
#ifndef NT2_SDK_UNIT_DETAILS_TESTS_HPP_INCLUDED
#define NT2_SDK_UNIT_DETAILS_TESTS_HPP_INCLUDED
//#include <nt2/include/functions/ulpdist.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/details/ulpdist.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <iostream>

namespace nt2 { namespace details
{
  template<class T, class U, class V>
  inline bool test_ulp_eq( char const* x1
			   , char const* x2
			   , char const* x3
			   , int line
			   , char const * fn
			   , T const & t
			   , U const & u
			   , V const & v
			   )
  {
    test_count()++;
    volatile T tt(t);
    volatile U uu(u);
    volatile V vv(v);
    typedef typename nt2::meta::upgrade<T>::type TT;
    typedef typename nt2::meta::upgrade<U>::type UU;
    typedef typename boost::common_type<TT, UU>::type R;
    if( nt2::details::ulpdist(tt, uu ) <= (R)vv)
      {
	std::cout << " * Test `"
		  << "details::ulpdist(" << x1 << ", " <<  x2 << ") <= " << x3
		  << "` **passed**."
		  << " (" << line << ")"
		  << std::endl;
	return true;
      }
    else
      {
	std::cout << " * Test `"
		  << "details::ulpdist(" << x1 << ", " <<  x2 << ") <= " << x3
		  << "` **failed** in function "
		  << fn << " (" << line << ")"
		  << "details::ulpdist(" << TT(tt) << ", " <<  UU(uu) << ") == "
		  <<  nt2::details::ulpdist(tt, uu )
		  << std::endl;
	++error_count();
	return false;
      }
  }

  template<class T, class U, class V>
  inline bool test_ulp_tuple_eq( char const* x1
			   , char const* x2
			   , char const* x3
			   , int line
			   , char const * fn
			   , T const & t
			   , U const & u
			   , V const & v
			   )
  {
    test_count()++;
    volatile T tt(t);
    volatile U uu(u);
    volatile V vv(v);
    typedef typename nt2::meta::upgrade<T>::type TT;
    typedef typename nt2::meta::upgrade<U>::type UU;
    bool r =   nt2::details::ulpdist(boost::fusion::at_c<0>(u), boost::fusion::at_c<0>(t)) <= v;
    r &= nt2::details::ulpdist(boost::fusion::at_c<1>(u), boost::fusion::at_c<1>(t)) <= v;
    if(r)
      {
	std::cout << " * Test `"
		  << "details::ulpdist(" << x1 << ", " <<  x2 << ") <= " << x3
		  << "` **passed**."
		  << " (" << line << ")"
		  << std::endl;
	return true;
      }
    else
      {
// 	std::cout << " * Test `"
// 		  << "details::ulpdist(" << x1 << ", " <<  x2 << ") <= " << x3
// 		  << "` **failed** in function "
// 		  << fn << " (" << line << ")"
// 		  << "details::ulpdist(" << TT(t) << ", " <<  UU(u) << ") == "
// 		  <<  nt2::details::ulpdist(tt, uu )
// 		  << std::endl;
	++error_count();
	return false;
      }
  }

} }
#endif


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#include <unordered_set>

#include <boost/iterator/transform_iterator.hpp>

#include "mc/classic/place_bound.hh"

namespace pnmc { namespace mc { namespace classic {

/*------------------------------------------------------------------------------------------------*/

using variables_bounds_type = std::unordered_map<sdd::order_position_type, pn::valuation_type>;

struct bound_visitor
{
  std::unordered_set<const void*> visited;
  variables_bounds_type& bounds;

  bound_visitor(variables_bounds_type& b)
    : visited(), bounds(b)
  {}

  [[noreturn]]
  void
  operator()(const sdd::zero_terminal<sdd_conf>&, const order&)
  const noexcept
  {
    assert(false);
    __builtin_unreachable();
  }

  void
  operator()(const sdd::one_terminal<sdd_conf>&, const order&)
  const noexcept
  {}

  void
  operator()(const sdd::flat_node<sdd_conf>& n, const order& o)
  {
    if (not visited.count(&n))
    {
      if (bounds.count(o.position()))
      {
        pn::valuation_type current_max = 0;
        for (const auto& arc : n)
        {
          current_max = std::max(current_max, *arc.valuation().rbegin());
          visit(*this, arc.successor(), o.next());
        }
        if (bounds.at(o.position()) < current_max)
        {
          bounds[o.position()] = current_max;
        }
      }
      else
      {
        for (const auto& arc : n)
        {
          visit(*this, arc.successor(), o.next());
        }
      }
      visited.emplace_hint(end(visited), &n);
    }
  }

  void
  operator()(const sdd::hierarchical_node<sdd_conf>& n, const order& o)
  {
    if (not visited.count(&n))
    {
      for (const auto& arc : n)
      {
        visit(*this, arc.valuation(), o.nested());
        visit(*this, arc.successor(), o.next());
      }
      visited.emplace_hint(end(visited), &n);
    }
  }
};

/*------------------------------------------------------------------------------------------------*/

std::unordered_map<std::string /* place id */, pn::valuation_type>
place_bound(const order& o, const SDD& states, const std::set<std::string>& places)
{
  if (places.empty())
  {
    return std::unordered_map<std::string, pn::valuation_type>{};
  }

  const auto init = [&](const auto& place){return std::make_pair(o.node(place).position(), 0);};
  auto variables_bounds =
    variables_bounds_type{ boost::make_transform_iterator(begin(places), init)
                         , boost::make_transform_iterator(end(places), init)};

  sdd::visit(bound_visitor{variables_bounds}, states, o);

  const auto to_id = [&](auto kv)
  {
    return std::make_pair(o.node_from_position(kv.first).identifier().user(), kv.second);
  };
  return { boost::make_transform_iterator(begin(variables_bounds), to_id)
         , boost::make_transform_iterator(end(variables_bounds), to_id)};
}

/*------------------------------------------------------------------------------------------------*/

}}} // namespace pnmc::mc::classic


#ifndef ComputeProbability_hpp
#define ComputeProbability_hpp

#include <QAlgorithm.hpp>
#include <UMF/Evaluate1D.hpp>
#include <alglib/integration.h>

namespace AA {
	class ComputeProbability : public QAlgorithm {

		Q_OBJECT

		QA_INPUT(QVector<double>, IntraX)
		QA_INPUT(QVector<double>, IntraY)
		QA_INPUT(QVector<double>, IntraCoefficients)

		QA_INPUT(QVector<double>, ExtraX)
		QA_INPUT(QVector<double>, ExtraY)
		QA_INPUT(QVector<double>, ExtraCoefficients)

		QA_PARAMETER(double, LeftExtremum, 0.0)
		QA_PARAMETER(double, RightExtremum, 1.0)

		QA_OUTPUT(double, MatchingScore)

		QA_CTOR_INHERIT
		QA_IMPL_CREATE(ComputeProbability)

	public:
		void run();
	};
}

#endif /* ComputeProbability_hpp */


#ifndef KINT_H_
#define KINT_H_

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/weight.hpp>
#include <boost/accumulators/framework/accumulators/external_accumulator.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/sum_kahan.hpp>

#include <iostream>

/*!	This class provides methods for integration over momenta or energies
 *
 *	Problem:
 *		Parameters are:
 *		- a: lattice spacing
 *		- \f$ D(\eps) \f$: density of states (DOS)
 *		- Momentum, energy dependent function. In most cases this will be the impurity Green's \f$ G(k,i\omega_n) \f$
 *		- Some constant offset. In most cases \f$ i \omega_n + \mu - \Simga(i \omega_n)\f$
 *		- function and limits for the integrand e.g.:
 *		\f[
 *			\int\limits_{-\frac{\pi}{a}}^\frac{\pi}{a} d^D k \frac{1}{i \omega_n - \epsilon_k + mu - \Sigma (i \omega_n)}
 *		\f]
 *		or
 *		\f[
 *			\int\limits_{-\infty}^\infty d^D \epsilon \frac{d(\epsilon)}{i \omega_n - \epsilon_k + mu - \Sigma (i \omega_n)}
 *		\f]
 *
 *	Methods:
 *
 *	- summation (this constructs some general weighted D dimensional sum)
 *		- riemann sum
 *		- gaussian smearing
 *		.
 *
 *	- reformulation to ODE and solution via boost ODE int
 *
 *	- Linear tetrahedral method for 1,2,3 D
 * 		- issues: can break symmetry, gamma point must be included
 *		- use MFEM?
 *		.
 *
 *	- TODO: let user decide wether to use kahan summation or not
 */
namespace utility{
using KahanTag = boost::accumulators::tag::weighted_sum_kahan;
using KahanSumAccT = boost::accumulators::stats<KahanTag>;
template <typename T>
using AccT = boost::accumulators::accumulator_set<T, KahanSumAccT, T >;

namespace detail
{
	/*!	this recursively constructs the depth of the nested for loops
	 *	for the innermost loop the partially specialized struct Internal<D,0> is called
	 */
	template<unsigned int D, unsigned int ND, typename T, typename RetT >
	struct Internal
	{
		static void sumKPoints(RetT (*integrand)(std::array<T, D> x),const std::array<T, D> &min,const std::array<T, D> &incs,\
			 const std::array<unsigned long, D> &N,std::array<T, D> &xVec, AccT<RetT> &acc)
		{
			RetT xi=min[ND];
			for(unsigned int n=0; n<N[ND];n++)
			{
				xVec[ND] = xi;
				Internal<D,ND-1, T, RetT>::sumKPoints(integrand, min, incs, N, xVec, acc);
				acc(boost::accumulators::extract_result<KahanTag>(acc), boost::accumulators::weight = incs[ND]);

				xi+=incs[ND];
			}
		}
	};

	template<unsigned int D, typename T, typename RetT>
	struct Internal<D,0,T,RetT>
	{
		static void sumKPoints(RetT (*integrand)(std::array<T, D> x),const std::array<T, D> &min, \
			const std::array<T, D> &incs, const std::array<unsigned long, D> &N, std::array<T, D> &xVec, AccT<RetT> &acc)
		{
			RetT xi=min[0];
			for(unsigned int n=0; n<N[0];n++)
			{
				xVec[0] = xi;
				acc(integrand(xVec), boost::accumulators::weight = incs[0]);
				xi+=incs[0];
			}
		}
	};
}

//TODO: 2 test types: 3 or 4 standard intrgrals, mean over >1000 VERY large OR mean over >MAXINT number of samples numbers (check for overflow)
//TODO: let user specify weights, TODO: let user choose between kahan and normal sum

template<unsigned int D>
class KInt
{
	public:
		/*!	@brief	instantiates an accumulator for D nested for loops \f$ \sum\limits_{min_1}^{max_1} ... \sum\limits_{min_D}^{max_D} f \f$
		 *			with \f$ f(T x_1, ... ,T x_D) -> T \f$
		 *
		 *	@param	integrand	function pointer over which to sum
		 *	@param	min			array of start values (one element for each dimension)
		 *	@param	max			array of values for the upper limit (one element for each dimension)
		 *	@param	N			array with number of steps (one element for each dimension)
		 *
		 *	@return	accumulated value
		 */
		template<typename T, typename RetT>
		RetT sumKPoints(RetT (*integrand)(std::array<T, D> x), std::array<T, D> &min, \
			const std::array<T, D> &max, const std::array<unsigned long, D> &N) const
		{
			//acc<summand type, method, weight type>
			AccT<RetT> acc;
			std::array<T, D> incs; 						// increments in each dimension
			std::array<T, D> xVec 	= min;
			for(unsigned int d=0;d<D;d++) incs[d] = (max[d]-min[d])/N[d];

			detail::Internal<D,D-1,T,RetT>::sumKPoints(integrand, min, incs, N, xVec, acc);
			return boost::accumulators::extract_result<KahanTag>(acc);
		}

	private:

};

}
#endif


#ifndef LINE_HISTOGRAM_HPP_4R0UZ4YV
#define LINE_HISTOGRAM_HPP_4R0UZ4YV

#include <log4cxx/logger.h>
#include <log4cxx/basicconfigurator.h>
#include <log4cxx/helpers/exception.h>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/unordered_map.hpp>
#include <boost/multi_array.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "algorithm_kmeans.hpp"

//namespace std {using namespace __gnu_cxx;}
using namespace std;

namespace prhlt {
    typedef vector<float> full_histogram;
    typedef boost::unordered_map<int,float> sparse_histogram;
    typedef boost::multi_array<cv::Point2d, 2> line_array;
    typedef vector<cv::Point2d> points_list;

    class Line_Histogram{
        public:
            Line_Histogram(int ex_threshold);
            Line_Histogram(cv::Mat &ex_image, int ex_threshold);
            Line_Histogram(cv::Mat &ex_image,cv::Rect rec, int ex_threshold);
            ~Line_Histogram();
            float& operator[] (const int nIndex);
            float get_derivate_value(const int nIndex);
            void calculation_initialization(string orientation_mode, string area_mode);
            //void calculate(string orientation_mode, string area_mode, int smoothing_factor);
			void calculate(string orientation_mode, string area_mode, int smoothing_factor,int derivate_window_size = 2);
      void calculate(cv::Mat ex_angles_mat, string orientation_mode, string area_mode, int smoothing_factor,int derivate_window_size = 2);
            points_list get_maximums();
            points_list get_minimums();
            line_array get_local_drawable_histogram();
            line_array get_relative_drawable_histogram(cv::Rect rectangle);
            line_array get_drawable_list(string list_mode);
            void calculate_maxs_mins();
            int get_size();
            void set_interest_zone(cv::Rect rec);
            int get_grey_count();
            int get_binarization_threshold();
            int get_real_grey_count();
            int get_region_real_grey_count(string orientation_mode, string area_mode);
            void show_internal_image(float scale);
            static const string horizontal;
            static const string vertical;
            static const string mins;
            static const string maxs;
            static const string global;
            static const string local;
        private:
            void reset_histograms_lists();
            void perform_normalization();
            void initialize_coordinate_system(string mode);
            void initialize_line_histogram(string mode);
            void smooth_line_histogram(string mode,int num_smooth_columns);
            void calculate_line_histogram(string mode);
            void calculate_angled_line_histogram(string mode);
            void update_histogram(string mode,int rowIndex, int colIndex,float value);
            void calculate_maxs(cv::Mat line_labels);
            void calculate_mins(cv::Mat line_labels);
            void add_maximum(int index);
            void add_minimum(int index);
            int extract_index_from_point(cv::Point p);
            int get_normalization_value();
            void calculate_histogram_derivate(int window_size);
            float calculate_derivate_at(int line, int window_size);
            cv::Mat image;
            cv::Mat angles;
            cv::Mat global_image;
            cv::Rect interest_zone;
            log4cxx::LoggerPtr logger;
            full_histogram image_histogram;
            full_histogram image_histogram_derivate;
            string orientation_mode;
            string area_mode;
            int binarization_threshold;
            int count;
            int global_count;
            bool is_initialized;
            cv::Point2d zero;
            cv::Point2d increment;
            cv::Point2d max_bar_size;
		//Transition points from the histogram
		        vector<cv::Point2d> max_list;
		        vector<cv::Point2d> min_list;
		};
}
#endif /* end of include guard: LINE_HISTOGRAM_HPP_4R0UZ4YV */


// Copyright (c) 2012-2015 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "key.h"

#include "base58.h"
#include "script/script.h"
#include "uint256.h"
#include "util.h"
#include "utilstrencodings.h"
#include "test/test_devault.h"

#include <string>
#include <vector>

#include <boost/test/unit_test.hpp>

using namespace std;

static const string strSecret1     ("PHSecPt927qh8bVhLhv4Rr66JMUYWRcaQfsdPnP8nucyhE4Lhc8b");
static const string strSecret2     ("PEc7Ctgscq6fJCCBAsmi9NQr2W2dLfpraVJgYqSEhacGP4YwqaDF");
static const CDeVaultAddress addr1 ("NfWVKf7BxmvNCm8e82eegeKeHyFM2Dy2Nv");
static const CDeVaultAddress addr2 ("NMxJRcqfcgfQvzhKy42zHVSzTfrnS2HLQo");

static const string strAddressBad("1HV9Lc3sNHZxwj4Zk6fB38tEmBryq2cBiF");

#ifdef KEY_TESTS_DUMPINFO
void dumpKeyInfo(uint256 privkey)
{
    CKey key;
    key.resize(32);
    memcpy(&secret[0], &privkey, 32);
    vector<unsigned char> sec;
    sec.resize(32);
    memcpy(&sec[0], &secret[0], 32);
    printf("  * secret (hex): %s\n", HexStr(sec).c_str());

    for (int nCompressed=0; nCompressed<2; nCompressed++)
    {
        bool fCompressed = nCompressed == 1;
        printf("  * %s:\n", fCompressed ? "compressed" : "uncompressed");
        CDeVaultSecret bsecret;
        bsecret.SetSecret(secret, fCompressed);
        printf("    * secret (base58): %s\n", bsecret.ToString().c_str());
        CKey key;
        key.SetSecret(secret, fCompressed);
        vector<unsigned char> vchPubKey = key.GetPubKey();
        printf("    * pubkey (hex): %s\n", HexStr(vchPubKey).c_str());
        printf("    * address (base58): %s\n", CDeVaultAddress(vchPubKey).ToString().c_str());
    }
}
#endif

BOOST_FIXTURE_TEST_SUITE(key_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(key_test1)
{
    CDeVaultSecret bsecret1, bsecret2, bsecret1C, bsecret2C, baddress1;

    BOOST_CHECK( bsecret1.SetString (strSecret1));
    BOOST_CHECK( bsecret2.SetString (strSecret2));
    BOOST_CHECK(!baddress1.SetString(strAddressBad));

    CKey key1  = bsecret1.GetKey();
    BOOST_CHECK(key1.IsCompressed() == true);
    CKey key2  = bsecret2.GetKey();
    BOOST_CHECK(key2.IsCompressed() == true);

    CPubKey pubkey1  = key1. GetPubKey();
    CPubKey pubkey2  = key2. GetPubKey();

    BOOST_CHECK(key1.VerifyPubKey(pubkey1));
    BOOST_CHECK(!key1.VerifyPubKey(pubkey2));

    BOOST_CHECK(!key2.VerifyPubKey(pubkey1));
    BOOST_CHECK(key2.VerifyPubKey(pubkey2));

    BOOST_CHECK(addr1.Get()  == CTxDestination(pubkey1.GetID()));
    BOOST_CHECK(addr2.Get()  == CTxDestination(pubkey2.GetID()));

    for (int n=0; n<16; n++)
    {
        string strMsg = strprintf("Very secret message %i: 11", n);
        uint256 hashMsg = Hash(strMsg.begin(), strMsg.end());

        // normal signatures

        vector<unsigned char> sign1, sign2;

        BOOST_CHECK(key1.Sign (hashMsg, sign1));
        BOOST_CHECK(key2.Sign (hashMsg, sign2));

        BOOST_CHECK( pubkey1.Verify(hashMsg, sign1));
        BOOST_CHECK(!pubkey1.Verify(hashMsg, sign2));

        BOOST_CHECK(!pubkey2.Verify(hashMsg, sign1));
        BOOST_CHECK( pubkey2.Verify(hashMsg, sign2));

        // compact signatures (with key recovery)

        vector<unsigned char> csign1, csign2;

        BOOST_CHECK(key1.SignCompact (hashMsg, csign1));
        BOOST_CHECK(key2.SignCompact (hashMsg, csign2));

        CPubKey rkey1, rkey2;

        BOOST_CHECK(rkey1.RecoverCompact (hashMsg, csign1));
        BOOST_CHECK(rkey2.RecoverCompact (hashMsg, csign2));

        BOOST_CHECK(rkey1  == pubkey1);
        BOOST_CHECK(rkey2  == pubkey2);
    }
    // test EC Diffie-Hellman secret computation
    CPrivKey secret1;
    CPrivKey secret2;
    BOOST_CHECK(key1.ECDHSecret(pubkey2, secret1));
    BOOST_CHECK(key2.ECDHSecret(pubkey1, secret2));
    BOOST_CHECK(secret1.size() == 32);
    BOOST_CHECK(secret1 == secret2);

    // invalid pubkey test
    std::vector<unsigned char> pubkeydata;
    pubkeydata.insert(pubkeydata.end(), pubkey1.begin(), pubkey1.end());
    pubkeydata[0] = 0xFF;
    CPubKey pubkey1_invalid(pubkeydata);
    BOOST_CHECK(key2.ECDHSecret(pubkey1_invalid, secret2) == false);
    pubkeydata[0] = 0x03;
    CPubKey pubkey2_invalid(pubkeydata);
    BOOST_CHECK(key2.ECDHSecret(pubkey2_invalid, secret2) == true);
    pubkeydata[9] = 0xFF;
    CPubKey pubkey3_invalid(pubkeydata);
    BOOST_CHECK(key2.ECDHSecret(pubkey3_invalid, secret2) == false);

}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/accumulators/statistics/weighted_peaks_over_threshold.hpp>