/*-------------Lanczos.cpp----------------------------------------------------//
*
* Purpose: To diagonalize a random matrix using the Lanczos algorithm
*
*   Notes: Compile with (for Arch systems):
*              g++ -I /usr/include/eigen3/ Lanczos.cpp
*          0's along the prime diagonal. I don't know what this means.
*
*-----------------------------------------------------------------------------*/

#include <iostream>
#include <Eigen/Core>
#include <Eigen/QR>
#include <random>
#include <vector>
#include <math.h>

using namespace Eigen;

// Function for the lanczos algorithm, returns Tri-diagonal matrix
MatrixXd lanczos(MatrixXd &d_matrix, int row_num);

// Function for QR decomposition
MatrixXd qrdecomp(MatrixXd &Tridiag);

// Function to perform the Power Method
void p_method(MatrixXd &Tridiag, MatrixXd &Q);

// Function to return sign of value (signum function)
int sign(double value);

// Function to check eigenvectors and values
void eigentest(MatrixXd &d_matrix, MatrixXd &Q);

/*----------------------------------------------------------------------------//
* MAIN
*-----------------------------------------------------------------------------*/

int main(){

    int size = 200;
    MatrixXd d_matrix(size,size);

    // set up random device
    static std::random_device rd;
    int seed = rd();
    static std::mt19937 gen(seed);
    std::uniform_real_distribution<double> dist(0,1);

    for (size_t i = 0; i < d_matrix.rows(); ++i){
        for (size_t j = 0; j <= i; ++j){
            d_matrix(i,j) = dist(gen);
            d_matrix(j,i) = d_matrix(i,j);
        }
    }

    MatrixXd Tridiag = lanczos(d_matrix, 20);

    //std::cout << '\n' << "Tridiagonal matrix is: \n" << Tridiag << '\n';

    /*
    // Testing for eigenvalue determination
    MatrixXd Tridiag(3,3);
    Tridiag << 1, 2, 0,
               2, 1, 0,
               0, 0, -3;

    MatrixXd Tridiag(5,5);
    Tridiag << 5, 0, 0, 0, 0,
               0, 4, 0, 0, 0,
               0, 0, 3, 0, 0,
               0, 0, 0, 2, 0,
               0, 0, 0, 0, 7;
    Tridiag = lanczos(Tridiag, 5);
    */

    //std::cout << "Tridiag is: " << '\n' << Tridiag << '\n';

    MatrixXd Q = qrdecomp(Tridiag);

    MatrixXd Qtemp = Q;

    //std::cout << "Q is: " << '\n';
    //std::cout << Q << '\n';

    std::cout << "Finding eigenvalues: " << '\n';
    p_method(Tridiag, Q);

    //std::cout << "Q is: " << '\n' << Q << '\n';

    Qtemp = Qtemp - Q;
    //std::cout << "After the Power Method: " << Qtemp.squaredNorm() << '\n';
    eigentest(Tridiag, Q);

}

/*----------------------------------------------------------------------------//
* SUBROUTINE
*-----------------------------------------------------------------------------*/

// Function for the lanczos algorithm, returns Tri-diagonal matrix
MatrixXd lanczos(MatrixXd &d_matrix, int row_num){

    // Creating random device
    static std::random_device rd;
    int seed = rd();
    static std::mt19937 gen(seed);
    std::uniform_real_distribution<double> dist(0,1);

    // Defining values
    double threshold = 0.01;
    int j = 0;
    int size = d_matrix.rows();

    // Setting beta arbitrarily large for now
    double beta = 10;

    // generating the first rayleigh vector
    // alpha is actually just a double... sorry about that.
    MatrixXd rayleigh(d_matrix.rows(),1), q(d_matrix.rows(),1),
             alpha(1, 1);
    MatrixXd identity = MatrixXd::Identity(d_matrix.rows(), d_matrix.cols());

    // krylov is the krylovian subspace... Note, there might be a dynamic way to
    // do this. Something like:
    //std::vector <MatrixXd> krylov;
    MatrixXd krylov(d_matrix.rows(), row_num);

    for (size_t i = 0; i < size; ++i){
        rayleigh(i) = dist(gen);
    }

    //std::cout << rayleigh << '\n';

    //while (beta > threshold){
    for (size_t i = 0; i < row_num; ++i){
        beta = rayleigh.norm();
        //std::cout << "beta is: \n" << beta << '\n';

        q = rayleigh / beta;
        //std::cout << "q is: \n" << q << '\n';

        alpha = q.transpose() * d_matrix * q;
        //std::cout << "alpha is \n" << alpha << '\n';

        if (j == 0){
            rayleigh = (d_matrix - alpha(0,0) * identity) * q;
        }
        else{
            rayleigh = (d_matrix - alpha(0,0) * identity) * q
                       - beta * krylov.col(j - 1);

        }
        //std::cout << "rayleigh is: \n" << rayleigh <<'\n';
        //std::cout << "i is: " << i << '\n';

        //krylov.push_back(q);
        krylov.col(j) = q;
        j = j+1;
        // std::cout << j << '\n';
    }

    /*
    MatrixXd krylov_id = krylov.transpose() * krylov;
    std::cout << "The identity matrix from the krylov subspace is: \n"
              << krylov_id << '\n';
    */

    MatrixXd T(row_num,row_num);
    T = krylov.transpose() * d_matrix * krylov;

    return T;
}

// Function for QR decomposition
// Because we only need Q for the power method, I will retun only Q
MatrixXd qrdecomp(MatrixXd &Tridiag){
    // Q is and orthonormal vector => Q'Q = 1
    MatrixXd Id = MatrixXd::Identity(Tridiag.rows(), Tridiag.cols());
    MatrixXd Q = Id;
    MatrixXd P(Tridiag.rows(), Tridiag.cols());

    // R is the upper triangular matrix
    MatrixXd R = Tridiag;

    int row_num = Tridiag.rows();
    int countx = 0, county = 0;

    // Scale R
    double sum = 0.0, sigma, tau, fak, max_val = 0;

    bool sing;

    // Defining vectors for algorithm
    MatrixXd diag(row_num,1);

    //std::cout << R << '\n';

    for (int i = 0; i < row_num-1; ++i){
        diag = MatrixXd::Zero(row_num, 1);

        sum = 0;
        for (size_t j = i; j < row_num; ++j){
            sum += R(j,i) * R(j,i);
            //std::cout << R(j,i) << '\n';
        }
        sum = sqrt(sum);

        if (R(i,i) > 0){
            sigma = -sum;
        }
        else{
            sigma = sum;
        }

        //std::cout << "sigma is: " << sigma << '\n';

        sum = 0;
        //diag = R.block(i,i, row_num - i, 1);
        //std::cout << "diag is: " << '\n';
        for (int j = i; j < row_num; ++j){
            //std::cout << i << '\t' << j << '\n';
            if (j == i){
                diag(j) = R(j,i) + sigma;
            }
            else{
                diag(j) = R(j, i);
            }
            //std::cout << diag(j) << '\n';
            sum = sum + diag(j) * diag(j);
        }
        sum = sqrt(sum);

        //std::cout << "sum is: " << sum << '\n';

        if (sum > 0.000000000000001){

            for (int j = i; j < row_num; ++j){
                diag(j) = diag(j) / sum;
            }

            //std::cout << "normalized diag is: " << '\n' << diag << '\n';

            P = Id - (diag * diag.transpose()) * 2.0;

            R = P * R;
            Q = Q * P;
        }

        //std::cout << "R is: " << R << '\n';

    }
    //std::cout << "R is: " << R << '\n';
    //std::cout << "Q is: " << Q << '\n';

    //std::cout << "QR is: " << '\n' << Q*R << '\n';

    //std::cout << "Q^T * Q is: " << '\n' << Q.transpose() * Q << '\n' << '\n';
    //std::cout << "QR - A is: " << '\n' << Q*R - Tridiag << '\n';
    //std::cout << "Q^T * A - R: " << '\n'
    //          << Q.transpose() * Tridiag - R << '\n' << '\n';

    return Q;
}

// Function to perform the Power Method
void p_method(MatrixXd &Tridiag, MatrixXd &Q){

    //std::cout << "Q is: " << '\n' << Q << '\n';
    //std::cout << "Tridiag is : " << '\n' << Tridiag << '\n';

    // Find all eigenvectors
    MatrixXd eigenvectors(Tridiag.rows(), Tridiag.cols());
    MatrixXd Z(Tridiag.rows(), Tridiag.cols());
    MatrixXd Qtemp = Q;

    // Iteratively defines eigenvectors
    for (int i = 0; i < Tridiag.rows(); ++i){
        Z = Tridiag * Q;
        Q = qrdecomp(Z);

    }

    Qtemp = Qtemp - Q;
    //std::cout << "This should not be 0: " << Qtemp.squaredNorm() << '\n';

}

// Function to return sign of value (signum function)
int sign(double value){
    if (value < 0.0){
        return -1;
    }
    else if (value > 0){
        return 1;
    }
    else {
        return 0;
    }
}

// Function to check eigenvectors and values
void eigentest(MatrixXd &Tridiag, MatrixXd &Q){

    // Calculating the Rayleigh quotient (v^t * A * v) / (v^t * v)
    // Note, that this should be a representation of eigenvalues

    std::vector<double> eigenvalues(Tridiag.rows());
    MatrixXd checkvector(Tridiag.rows(),1);
    double QQ, QAQ;

    for (size_t i = 0; i < Tridiag.rows(); ++i){
        QQ = Q.col(i).transpose() * Q.col(i);
        QAQ = Q.col(i).transpose() * Tridiag * Q.col(i);
        eigenvalues[i] =  QAQ / QQ;
        std::cout << "eigenvalue is: " << eigenvalues[i] << '\n';

        checkvector = ((Tridiag * Q.col(i)) / eigenvalues[i]) - Q.col(i);
        //std::cout << checkvector << '\n' << '\n';
        std::cout << "This should be 0: " << '\t'
                  << checkvector.squaredNorm() << '\n';

    }

}


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

#ifndef SP_ALGO_NN_LAYER_CONV_HPP
#define SP_ALGO_NN_LAYER_CONV_HPP

#include <boost/assert.hpp>

#include "layer.hpp"
#include "connectivity.hpp"
#include "detail/layers.hpp"
#include "sp/util/types.hpp"
#include "params.hpp"

SP_ALGO_NN_NAMESPACE_BEGIN

/**
 * \brief Convolutional Layer implementation
 *
 * Convolves input by a set of feature kernels and returns the convolution
 * results
 *
 * Note: This convolution implementation does not rotate the kernel
 *       and is therefore not convolution but cross correlation, mathematically
 *       speaking.
 *
 * Summary of parameters (Soft and Hard, i.e. parameterized and derived)
 *  - N - Number of images in a mini-batch
 *  - C - The number of input feature maps
 *  - H - Height of input image
 *  - W - Width of input image
 *  - K - Number of output feature maps (number of kernels)
 *  - R - Height of filter kernel
 *  - S - Width of filter kernel
 *  - U - Vertical Stride
 *  - V - Horizontal Stride
 *  - pad_h - Height of zero-padding
 *  - pad_w - Width of zero-padding
 *
 * Output O of is a four dimensional tensor in R^NKPQ,
 *  where ```P = f(H, R, u, pad_h) and Q = f(W, S, v, pad_w)```
 *  where
 *  *   ```f(H, R, u, pad_h) = ceil((H - R + 1 + 2 * pad_h) / 2)```
 *  *   ```f(W, S, v, pad_w) = ceil((W - S + 1 + 2 * pad_w) / 2)```
 *
 * Convolution Modes (common LA terms, matlab, octave)
 *  - valid - ```pad_w = pad_h = 0```
 *  - same  - ```pad_h = R/2 and pad_w = S / 2```
 *  - full  - ```pad_h = R - 1, pad_w = S - 1```
 *
 * \tparam InputDim, includes:
 * - Width
 * - Height
 * - Depth (or channels)
 * \tparam KernelDim, includes:... tbd
 * \tparam Biased (optional) default true. Whether or not the layer contains bias.
 * \tparam Sparsity The sparsity object. See #group_sparsity and #no_sparsity
 */
template<
    typename InputVolume,
    typename KernelParams = kernel_params_default,
    bool Biased = true,
    typename Connectivity = full_connectivity,
    size_t Dilation = 0
>
struct conv_layer : layer<
    InputVolume,
    detail::convolution_kernel_out_dims_t<InputVolume, KernelParams>,
    conv_layer<InputVolume, KernelParams, Biased, Connectivity, Dilation>
> {

    /**
     * Base type
     */
    using base = layer<
        InputVolume,
        detail::convolution_kernel_out_dims_t<InputVolume, KernelParams>,
        conv_layer<InputVolume, KernelParams, Biased, Connectivity, Dilation>
    >;

    /**
     * Validates KernelParams type
     */
    static_assert(util::is_instantiation_of_v<KernelParams, kernel_params>, "KernelParams template parameter must be an instance of kernel_params");

    /**
     * Kernel parameters
     */
    using kernel_params = KernelParams;

    /**
     * Connectivity type (full, ngroups, etc)
     */
    using connectivity_type = Connectivity;

    /**
     * \brief Dilation parameter
     * \todo TBD: https://arxiv.org/abs/1511.07122
     */
    static_assert(Dilation == 0, "Dilation is not implemented");

    /**
     * Whether or not this layer has biased
     */
    constexpr static bool biased = Biased;

    /**
     * \brief Input Dimensions
     */
    using input_dims = typename base::input_dims;

    /**
     * \brief Output Dimensions
     */
    using output_dims = typename base::output_dims;

    /**
     * \brief Weight dimensions
     */
    using weights_dims = weight_dims<
        output_dims::d,
        input_dims::d,
        kernel_params::h,
        kernel_params::w
    >;

    void forward_prop_impl(tensor_4& input, tensor_4& output) {

        /**
         * Number of samples in the input
         */
        const size_t samples = input.dimension(0);

        /**
         * Perform  forward propagation for every sample
         */
        #pragma omp parallel for simd
        for (size_t si = 0; si < samples; ++si) {
            /**
             * Loop over the pairs of (D_out, D_in), i.e. input_dims::d*output_dims::d
             */
            for (size_t od = 0; od < output_dims::d; ++od) {
                for (size_t id = 0; id < input_dims::d; ++id) {
                    if(connections(od, id)) {
                        /*
                         * If the output channel is connected to the input channel,
                         * then perform convolution. This is done to support limited
                         * connectivity when required
                         *
                         * Then, perform the convolution op
                         *
                         * \todo add dilation
                         * \todo Optimize for smaller kernels (common, 2x2, 3x3, etc)
                         */
                        for (size_t oy = 0, iny = 0; oy < output_dims::h; ++oy, iny += kernel_params::s_h) {
                            for (size_t ox = 0, inx = 0; ox < output_dims::w; ++ox, inx += kernel_params::s_w) {
                                float_t sum = 0;
                                for (size_t ky = 0; ky < kernel_params::h; ++ky) {
                                    for (size_t kx = 0; kx < kernel_params::w; ++kx) {
                                        auto& in_val = input(si, id, iny+ky, inx+kx);
                                        auto& w_val = w(od, id, ky, kx);
                                        sum += in_val * w_val;
                                    }
                                }
                                output(si, od, oy, ox) += sum;
                            }
                        }
                    }
                }
                if constexpr(biased) {
                    /**
                     * Add bias to every output vector the depth slice at output(od)
                     */
                    output.chip(si, 0).chip(od, 0) = output.chip(si, 0).chip(od, 0) + b(od);
                }
            }
        }
    }

    /**
     * \brief Back propagation implementation
     *
     * \todo Optimize
     */
    void backward_prop_impl(    tensor_4& prev_out,
                                tensor_4& prev_delta,
                                tensor_4& curr_out,
                                tensor_4& curr_delta)  {

        /**
         * Number of samples in the previous output
         */
        const size_t samples = prev_out.dimension(0);

        /**
         * Perform back propagation
         *
         * For every sample in the input
         */
        #pragma omp parallel for simd
        for(size_t si = 0; si < samples; ++si) {
            /**
             * For every (input depth, output depth) pair that is connected
             */
            for (size_t id = 0; id < input_dims::d; ++id) {
                for (size_t od = 0; od < output_dims::d; ++od) {
                    if(connections(od, id)) {
                        /* Propagate the current delta to the previous delta through the kernel */
                        for (size_t oy = 0, iny = 0; oy < output_dims::h; ++oy, iny += kernel_params::s_h) {
                            for (size_t ox = 0, inx = 0; ox < output_dims::w; ++ox, inx += kernel_params::s_w) {
                                float_t& grad = curr_delta(si, od, oy, ox);
                                for (size_t wy = 0; wy < weights_dims::h; ++wy) {
                                    for (size_t wx = 0; wx < weights_dims::w; ++wx) {
                                        auto& w_val = w(od, id, wy, wx);
                                        prev_delta(si, id, iny + wy, inx + wx) += w_val * grad;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            for (size_t id = 0; id < input_dims::d; ++id) {
                for (size_t od = 0; od < output_dims::d; ++od) {
                    if(connections(od, id)) {
                        for (size_t wy = 0; wy < weights_dims::h; ++wy) {
                            for (size_t wx = 0; wx < weights_dims::w; ++wx) {
                                float_t delta = 0;
                                for (size_t oy = 0, iny = 0; oy < output_dims::h; ++oy, iny += kernel_params::s_h) {
                                    for (size_t ox = 0, inx = 0; ox < output_dims::w; ++ox, inx += kernel_params::s_w) {
                                        auto& po = prev_out(si, id, oy + wy, ox + wx);
                                        auto& cd = curr_delta(si, od, oy, ox);
                                        delta +=  po * cd;
                                    }
                                }
                                dw(si, od, id, wy, wx) += delta;
                            }
                        }
                    }
                }
            }
            if (biased) {
                for (size_t od = 0; od < output_dims::d; ++od) {
                    tensor_0 sum = curr_delta.chip(si, 0).chip(od, 0).sum();
                    db(si, od) += sum(0);
                }
            }
        }
    }

    /**
     * \brief Weights of the layer.
     */
    weights_type w;

    /**
     * Weights Delta
     */
    weights_delta_type dw;

    /**
     * \brief Bias.
     */
    bias_type b;

    /**
     * Bias Delta.
     */
    bias_delta_type db;

    connectivity_type connections;

};

SP_ALGO_GEN_NAMESPACE_END

#endif	/* SP_ALGO_NN_LAYER_CONV_HPP */


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

#ifndef BOOST_MATH_DIFFERENTIATION_LANCZOS_SMOOTHING_HPP
#define BOOST_MATH_DIFFERENTIATION_LANCZOS_SMOOTHING_HPP
#include <cmath> // for std::abs
#include <cstddef>
#include <limits> // to nan initialize
#include <vector>
#include <string>
#include <stdexcept>
#include <type_traits>
#include <boost/math/tools/assert.hpp>

namespace boost::math::differentiation {

namespace detail {
template <typename Real>
class discrete_legendre {
  public:
    explicit discrete_legendre(std::size_t n, Real x) : m_n{n}, m_r{2}, m_x{x},
                                                        m_qrm2{1}, m_qrm1{x},
                                                        m_qrm2p{0}, m_qrm1p{1},
                                                        m_qrm2pp{0}, m_qrm1pp{0}
    {
        using std::abs;
        BOOST_MATH_ASSERT_MSG(abs(m_x) <= 1, "Three term recurrence is stable only for |x| <=1.");
        // The integer n indexes a family of discrete Legendre polynomials indexed by k <= 2*n
    }

    Real norm_sq(int r) const
    {
        Real prod = Real(2) / Real(2 * r + 1);
        for (int k = -r; k <= r; ++k) {
            prod *= Real(2 * m_n + 1 + k) / Real(2 * m_n);
        }
        return prod;
    }

    Real next()
    {
        Real N = 2 * m_n + 1;
        Real num = (m_r - 1) * (N * N - (m_r - 1) * (m_r - 1)) * m_qrm2;
        Real tmp = (2 * m_r - 1) * m_x * m_qrm1 - num / Real(4 * m_n * m_n);
        m_qrm2 = m_qrm1;
        m_qrm1 = tmp / m_r;
        ++m_r;
        return m_qrm1;
    }

    Real next_prime()
    {
        Real N = 2 * m_n + 1;
        Real s = (m_r - 1) * (N * N - (m_r - 1) * (m_r - 1)) / Real(4 * m_n * m_n);
        Real tmp1 = ((2 * m_r - 1) * m_x * m_qrm1 - s * m_qrm2) / m_r;
        Real tmp2 = ((2 * m_r - 1) * (m_qrm1 + m_x * m_qrm1p) - s * m_qrm2p) / m_r;
        m_qrm2 = m_qrm1;
        m_qrm1 = tmp1;
        m_qrm2p = m_qrm1p;
        m_qrm1p = tmp2;
        ++m_r;
        return m_qrm1p;
    }

    Real next_dbl_prime()
    {
        Real N = 2*m_n + 1;
        Real trm1 = 2*m_r - 1;
        Real s = (m_r - 1) * (N * N - (m_r - 1) * (m_r - 1)) / Real(4 * m_n * m_n);
        Real rqrpp = 2*trm1*m_qrm1p + trm1*m_x*m_qrm1pp - s*m_qrm2pp;
        Real tmp1 = ((2 * m_r - 1) * m_x * m_qrm1 - s * m_qrm2) / m_r;
        Real tmp2 = ((2 * m_r - 1) * (m_qrm1 + m_x * m_qrm1p) - s * m_qrm2p) / m_r;
        m_qrm2 = m_qrm1;
        m_qrm1 = tmp1;
        m_qrm2p = m_qrm1p;
        m_qrm1p = tmp2;
        m_qrm2pp = m_qrm1pp;
        m_qrm1pp = rqrpp/m_r;
        ++m_r;
        return m_qrm1pp;
    }

    Real operator()(Real x, std::size_t k)
    {
        BOOST_MATH_ASSERT_MSG(k <= 2 * m_n, "r <= 2n is required.");
        if (k == 0)
        {
            return 1;
        }
        if (k == 1)
        {
            return x;
        }
        Real qrm2 = 1;
        Real qrm1 = x;
        Real N = 2 * m_n + 1;
        for (std::size_t r = 2; r <= k; ++r) {
            Real num = (r - 1) * (N * N - (r - 1) * (r - 1)) * qrm2;
            Real tmp = (2 * r - 1) * x * qrm1 - num / Real(4 * m_n * m_n);
            qrm2 = qrm1;
            qrm1 = tmp / r;
        }
        return qrm1;
    }

    Real prime(Real x, std::size_t k) {
        BOOST_MATH_ASSERT_MSG(k <= 2 * m_n, "r <= 2n is required.");
        if (k == 0) {
            return 0;
        }
        if (k == 1) {
            return 1;
        }
        Real qrm2 = 1;
        Real qrm1 = x;
        Real qrm2p = 0;
        Real qrm1p = 1;
        Real N = 2 * m_n + 1;
        for (std::size_t r = 2; r <= k; ++r) {
            Real s =
                (r - 1) * (N * N - (r - 1) * (r - 1)) / Real(4 * m_n * m_n);
            Real tmp1 = ((2 * r - 1) * x * qrm1 - s * qrm2) / r;
            Real tmp2 = ((2 * r - 1) * (qrm1 + x * qrm1p) - s * qrm2p) / r;
            qrm2 = qrm1;
            qrm1 = tmp1;
            qrm2p = qrm1p;
            qrm1p = tmp2;
        }
        return qrm1p;
    }

  private:
    std::size_t m_n;
    std::size_t m_r;
    Real m_x;
    Real m_qrm2;
    Real m_qrm1;
    Real m_qrm2p;
    Real m_qrm1p;
    Real m_qrm2pp;
    Real m_qrm1pp;
};

template <class Real>
std::vector<Real> interior_velocity_filter(std::size_t n, std::size_t p) {
    auto dlp = discrete_legendre<Real>(n, 0);
    std::vector<Real> coeffs(p+1);
    coeffs[1] = 1/dlp.norm_sq(1);
    for (std::size_t l = 3; l < p + 1; l += 2)
    {
        dlp.next_prime();
        coeffs[l] = dlp.next_prime()/ dlp.norm_sq(l);
    }

    // We could make the filter length n, as f[0] = 0,
    // but that'd make the indexing awkward when applying the filter.
    std::vector<Real> f(n + 1);
    // This value should never be read, but this is the correct value *if it is read*.
    // Hmm, should it be a nan then? I'm not gonna agonize.
    f[0] = 0;
    for (std::size_t j = 1; j < f.size(); ++j)
    {
        Real arg = Real(j) / Real(n);
        dlp = discrete_legendre<Real>(n, arg);
        f[j] = coeffs[1]*arg;
        for (std::size_t l = 3; l <= p; l += 2)
        {
            dlp.next();
            f[j] += coeffs[l]*dlp.next();
        }
        f[j] /= (n * n);
    }
    return f;
}

template <class Real>
std::vector<Real> boundary_velocity_filter(std::size_t n, std::size_t p, int64_t s)
{
    std::vector<Real> coeffs(p+1, std::numeric_limits<Real>::quiet_NaN());
    Real sn = Real(s) / Real(n);
    auto dlp = discrete_legendre<Real>(n, sn);
    coeffs[0] = 0;
    coeffs[1] = 1/dlp.norm_sq(1);
    for (std::size_t l = 2; l < p + 1; ++l)
    {
        // Calculation of the norms is common to all filters,
        // so it seems like an obvious optimization target.
        // I tried this: The spent in computing the norms time is not negligible,
        // but still a small fraction of the total compute time.
        // Hence I'm not refactoring out these norm calculations.
        coeffs[l] = dlp.next_prime()/ dlp.norm_sq(l);
    }

    std::vector<Real> f(2*n + 1);
    for (std::size_t k = 0; k < f.size(); ++k)
    {
        Real j = Real(k) - Real(n);
        Real arg = j/Real(n);
        dlp = discrete_legendre<Real>(n, arg);
        f[k] = coeffs[1]*arg;
        for (std::size_t l = 2; l <= p; ++l)
        {
            f[k] += coeffs[l]*dlp.next();
        }
        f[k] /= (n * n);
    }
    return f;
}

template <class Real>
std::vector<Real> acceleration_filter(std::size_t n, std::size_t p, int64_t s)
{
    BOOST_MATH_ASSERT_MSG(p <= 2*n, "Approximation order must be <= 2*n");
    BOOST_MATH_ASSERT_MSG(p > 2, "Approximation order must be > 2");

    std::vector<Real> coeffs(p+1, std::numeric_limits<Real>::quiet_NaN());
    Real sn = Real(s) / Real(n);
    auto dlp = discrete_legendre<Real>(n, sn);
    coeffs[0] = 0;
    coeffs[1] = 0;
    for (std::size_t l = 2; l < p + 1; ++l)
    {
        coeffs[l] = dlp.next_dbl_prime()/ dlp.norm_sq(l);
    }

    std::vector<Real> f(2*n + 1, 0);
    for (std::size_t k = 0; k < f.size(); ++k)
    {
        Real j = Real(k) - Real(n);
        Real arg = j/Real(n);
        dlp = discrete_legendre<Real>(n, arg);
        for (std::size_t l = 2; l <= p; ++l)
        {
            f[k] += coeffs[l]*dlp.next();
        }
        f[k] /= (n * n * n);
    }
    return f;
}

} // namespace detail

template <typename Real, std::size_t order = 1>
class discrete_lanczos_derivative {
public:
    discrete_lanczos_derivative(Real const & spacing,
                                std::size_t n = 18,
                                std::size_t approximation_order = 3)
        : m_dt{spacing}
    {
        static_assert(!std::is_integral_v<Real>,
                      "Spacing must be a floating point type.");
        BOOST_MATH_ASSERT_MSG(spacing > 0,
                         "Spacing between samples must be > 0.");

        if constexpr (order == 1)
        {
            BOOST_MATH_ASSERT_MSG(approximation_order <= 2 * n,
                             "The approximation order must be <= 2n");
            BOOST_MATH_ASSERT_MSG(approximation_order >= 2,
                             "The approximation order must be >= 2");

            if constexpr (std::is_same_v<Real, float> || std::is_same_v<Real, double>)
            {
                auto interior = detail::interior_velocity_filter<long double>(n, approximation_order);
                m_f.resize(interior.size());
                for (std::size_t j = 0; j < interior.size(); ++j)
                {
                    m_f[j] = static_cast<Real>(interior[j])/m_dt;
                }
            }
            else
            {
                m_f = detail::interior_velocity_filter<Real>(n, approximation_order);
                for (auto & x : m_f)
                {
                    x /= m_dt;
                }
            }

            m_boundary_filters.resize(n);
            // This for loop is a natural candidate for parallelization.
            // But does it matter? Probably not.
            for (std::size_t i = 0; i < n; ++i)
            {
                if constexpr (std::is_same_v<Real, float> || std::is_same_v<Real, double>)
                {
                    int64_t s = static_cast<int64_t>(i) - static_cast<int64_t>(n);
                    auto bf = detail::boundary_velocity_filter<long double>(n, approximation_order, s);
                    m_boundary_filters[i].resize(bf.size());
                    for (std::size_t j = 0; j < bf.size(); ++j)
                    {
                        m_boundary_filters[i][j] = static_cast<Real>(bf[j])/m_dt;
                    }
                }
                else
                {
                    int64_t s = static_cast<int64_t>(i) - static_cast<int64_t>(n);
                    m_boundary_filters[i] = detail::boundary_velocity_filter<Real>(n, approximation_order, s);
                    for (auto & bf : m_boundary_filters[i])
                    {
                        bf /= m_dt;
                    }
                }
            }
        }
        else if constexpr (order == 2)
        {
            // High precision isn't warranted for small p; only for large p.
            // (The computation appears stable for large n.)
            // But given that the filters are reusable for many vectors,
            // it's better to do a high precision computation and then cast back,
            // since the resulting cost is a factor of 2, and the cost of the filters not working is hours of debugging.
            if constexpr (std::is_same_v<Real, double> || std::is_same_v<Real, float>)
            {
                auto f = detail::acceleration_filter<long double>(n, approximation_order, 0);
                m_f.resize(n+1);
                for (std::size_t i = 0; i < m_f.size(); ++i)
                {
                    m_f[i] = static_cast<Real>(f[i+n])/(m_dt*m_dt);
                }
                m_boundary_filters.resize(n);
                for (std::size_t i = 0; i < n; ++i)
                {
                    int64_t s = static_cast<int64_t>(i) - static_cast<int64_t>(n);
                    auto bf = detail::acceleration_filter<long double>(n, approximation_order, s);
                    m_boundary_filters[i].resize(bf.size());
                    for (std::size_t j = 0; j < bf.size(); ++j)
                    {
                        m_boundary_filters[i][j] = static_cast<Real>(bf[j])/(m_dt*m_dt);
                    }
                }
            }
            else
            {
                // Given that the purpose is denoising, for higher precision calculations,
                // the default precision should be fine.
                auto f = detail::acceleration_filter<Real>(n, approximation_order, 0);
                m_f.resize(n+1);
                for (std::size_t i = 0; i < m_f.size(); ++i)
                {
                    m_f[i] = f[i+n]/(m_dt*m_dt);
                }
                m_boundary_filters.resize(n);
                for (std::size_t i = 0; i < n; ++i)
                {
                    int64_t s = static_cast<int64_t>(i) - static_cast<int64_t>(n);
                    m_boundary_filters[i] = detail::acceleration_filter<Real>(n, approximation_order, s);
                    for (auto & bf : m_boundary_filters[i])
                    {
                        bf /= (m_dt*m_dt);
                    }
                }
            }
        }
        else
        {
            BOOST_MATH_ASSERT_MSG(false, "Derivatives of order 3 and higher are not implemented.");
        }
    }

    Real get_spacing() const
    {
        return m_dt;
    }

    template<class RandomAccessContainer>
    Real operator()(RandomAccessContainer const & v, std::size_t i) const
    {
        static_assert(std::is_same_v<typename RandomAccessContainer::value_type, Real>,
                      "The type of the values in the vector provided does not match the type in the filters.");

        BOOST_MATH_ASSERT_MSG(std::size(v) >= m_boundary_filters[0].size(),
            "Vector must be at least as long as the filter length");

        if constexpr (order==1)
        {
            if (i >= m_f.size() - 1 && i <= std::size(v) - m_f.size())
            {
                // The filter has length >= 1:
                Real dvdt = m_f[1] * (v[i + 1] - v[i - 1]);
                for (std::size_t j = 2; j < m_f.size(); ++j)
                {
                    dvdt += m_f[j] * (v[i + j] - v[i - j]);
                }
                return dvdt;
            }

            // m_f.size() = N+1
            if (i < m_f.size() - 1)
            {
                auto &bf = m_boundary_filters[i];
                Real dvdt = bf[0]*v[0];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    dvdt += bf[j] * v[j];
                }
                return dvdt;
            }

            if (i > std::size(v) - m_f.size() && i < std::size(v))
            {
                int k = std::size(v) - 1 - i;
                auto &bf = m_boundary_filters[k];
                Real dvdt = bf[0]*v[std::size(v)-1];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    dvdt += bf[j] * v[std::size(v) - 1 - j];
                }
                return -dvdt;
            }
        }
        else if constexpr (order==2)
        {
            if (i >= m_f.size() - 1 && i <= std::size(v) - m_f.size())
            {
                Real d2vdt2 = m_f[0]*v[i];
                for (std::size_t j = 1; j < m_f.size(); ++j)
                {
                    d2vdt2 += m_f[j] * (v[i + j] + v[i - j]);
                }
                return d2vdt2;
            }

            // m_f.size() = N+1
            if (i < m_f.size() - 1)
            {
                auto &bf = m_boundary_filters[i];
                Real d2vdt2 = bf[0]*v[0];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    d2vdt2 += bf[j] * v[j];
                }
                return d2vdt2;
            }

            if (i > std::size(v) - m_f.size() && i < std::size(v))
            {
                int k = std::size(v) - 1 - i;
                auto &bf = m_boundary_filters[k];
                Real d2vdt2 = bf[0] * v[std::size(v) - 1];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    d2vdt2 += bf[j] * v[std::size(v) - 1 - j];
                }
                return d2vdt2;
            }
        }

        // OOB access:
        std::string msg = "Out of bounds access in Lanczos derivative.";
        msg += "Input vector has length " + std::to_string(std::size(v)) + ", but user requested access at index " + std::to_string(i) + ".";
        throw std::out_of_range(msg);
        return std::numeric_limits<Real>::quiet_NaN();
    }

    template<class RandomAccessContainer>
    void operator()(RandomAccessContainer const & v, RandomAccessContainer & w) const
    {
        static_assert(std::is_same_v<typename RandomAccessContainer::value_type, Real>,
                      "The type of the values in the vector provided does not match the type in the filters.");
        if (&w[0] == &v[0])
        {
            throw std::logic_error("This transform cannot be performed in-place.");
        }

        if (std::size(v) < m_boundary_filters[0].size())
        {
            std::string msg = "The input vector must be at least as long as the filter length. ";
            msg += "The input vector has length = " + std::to_string(std::size(v)) + ", the filter has length " + std::to_string(m_boundary_filters[0].size());
            throw std::length_error(msg);
        }

        if (std::size(w) < std::size(v))
        {
            std::string msg = "The output vector (containing the derivative) must be at least as long as the input vector.";
            msg += "The output vector has length = " + std::to_string(std::size(w)) + ", the input vector has length " + std::to_string(std::size(v));
            throw std::length_error(msg);
        }

        if constexpr (order==1)
        {
            for (std::size_t i = 0; i < m_f.size() - 1; ++i)
            {
                auto &bf = m_boundary_filters[i];
                Real dvdt = bf[0] * v[0];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    dvdt += bf[j] * v[j];
                }
                w[i] = dvdt;
            }

            for(std::size_t i = m_f.size() - 1; i <= std::size(v) - m_f.size(); ++i)
            {
                Real dvdt = m_f[1] * (v[i + 1] - v[i - 1]);
                for (std::size_t j = 2; j < m_f.size(); ++j)
                {
                    dvdt += m_f[j] *(v[i + j] - v[i - j]);
                }
                w[i] = dvdt;
            }

            for(std::size_t i = std::size(v) - m_f.size() + 1; i < std::size(v); ++i)
            {
                int k = std::size(v) - 1 - i;
                auto &f = m_boundary_filters[k];
                Real dvdt = f[0] * v[std::size(v) - 1];;
                for (std::size_t j = 1; j < f.size(); ++j)
                {
                    dvdt += f[j] * v[std::size(v) - 1 - j];
                }
                w[i] = -dvdt;
            }
        }
        else if constexpr (order==2)
        {
            // m_f.size() = N+1
            for (std::size_t i = 0; i < m_f.size() - 1; ++i)
            {
                auto &bf = m_boundary_filters[i];
                Real d2vdt2 = 0;
                for (std::size_t j = 0; j < bf.size(); ++j)
                {
                    d2vdt2 += bf[j] * v[j];
                }
                w[i] = d2vdt2;
            }

            for (std::size_t i = m_f.size() - 1; i <= std::size(v) - m_f.size(); ++i)
            {
                Real d2vdt2 = m_f[0]*v[i];
                for (std::size_t j = 1; j < m_f.size(); ++j)
                {
                    d2vdt2 += m_f[j] * (v[i + j] + v[i - j]);
                }
                w[i] = d2vdt2;
            }

            for (std::size_t i = std::size(v) - m_f.size() + 1; i < std::size(v); ++i)
            {
                int k = std::size(v) - 1 - i;
                auto &bf = m_boundary_filters[k];
                Real d2vdt2 = bf[0] * v[std::size(v) - 1];
                for (std::size_t j = 1; j < bf.size(); ++j)
                {
                    d2vdt2 += bf[j] * v[std::size(v) - 1 - j];
                }
                w[i] = d2vdt2;
            }
        }
    }

    template<class RandomAccessContainer>
    RandomAccessContainer operator()(RandomAccessContainer const & v) const
    {
        RandomAccessContainer w(std::size(v));
        this->operator()(v, w);
        return w;
    }

    // Don't copy; too big.
    discrete_lanczos_derivative( const discrete_lanczos_derivative & ) = delete;
    discrete_lanczos_derivative& operator=(const discrete_lanczos_derivative&) = delete;

    // Allow moves:
    discrete_lanczos_derivative(discrete_lanczos_derivative&&) = default;
    discrete_lanczos_derivative& operator=(discrete_lanczos_derivative&&) = default;

private:
    std::vector<Real> m_f;
    std::vector<std::vector<Real>> m_boundary_filters;
    Real m_dt;
};

} // namespaces
#endif


/**
 * Conversion routines. Convert from coordinate type to coordinate type
 * via from_type foo = converter<to_type>()(from_type bar)
 *
 * Copyright 2013 Bruce Ide
 *
 *  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 "coordinates.hpp"
#include <Eigen/Core>
#include <type_traits>

#ifndef _HPP_CONVERTS
#define _HPP_CONVERTS

namespace fr {

  namespace coordinates {

    template <typename convert_to>
    struct converter {

    };

    /***************************************************************
     * Put convert to lat_long bits here
     */

    template <>
    struct converter<lat_long> {

      // Converting from lat_long to lat_long is kind of odd, but I'll
      // cover the case anyway
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,lat_long>::value,lat_long>::type
	operator()(const convert_from &c)
      {
	return c;
      }

      // ECEF to Latlong
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef>::value,lat_long>::type
      operator()(const convert_from &xyz, const ellipsoid_parameters &e = WGS84_ELLIPSOID, double tolerance = 0.0000000001 )
      {
	double diff = 2 * tolerance;
	double t = e.ee * xyz.get_z();
	double n = 0.0;
	double nph = 0.0;
	double sinPhi = 0.0;
	double lat;
	double longitude;
	double alt;

	longitude = atan2(xyz.get_y(), xyz.get_x()) * 180 / fr::constants::pi;
	while(diff > tolerance) {
	  double zT = xyz.get_z() + t;
	  nph = sqrt(pow(xyz.get_x(), 2) + pow(xyz.get_y(), 2) + pow(zT, 2));
	  sinPhi = zT / nph;
	  n = e.ae / sqrt(1 - e.ee * sinPhi * sinPhi);
	  double told = t;
	  t = n * e.ee * sinPhi;
	  diff = fabs(t - told);
	}

	lat = asin(sinPhi) * 180 / fr::constants::pi;
	alt = nph - n;
	lat_long retval(lat, longitude, alt);
	return retval;
      }

      // ecef_vel to lat_long (Loses velocity information)

      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef_vel>::value,lat_long>::type
      operator()(const convert_from &c, const ellipsoid_parameters &e = WGS84_ELLIPSOID)
      {
	ecef interim = converter<ecef>()(c);
	lat_long retval = converter<lat_long>()(interim, e);
	return retval;
      }

      // tod_eci_vel to latlong (Loses velocity information)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci_vel>::value,lat_long>::type
      operator()(const convert_from &c, const double &t, const ellipsoid_parameters e = WGS84_ELLIPSOID)
      {
	// Convert from tod_eci to ecef_vel
	ecef_vel interim = converter<ecef_vel>()(c,t);
	// Then use the function before this one to convert to lat/long
	lat_long retval = converter<lat_long>()(interim, e);
	return retval;
      }

    };

    /*********************************************************
     * Put convert to ECEF bits here
     */

    template <>
    struct converter<ecef> {

      // ECEF to ECEF conversion.
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef>::value,ecef>::type
      operator()(const convert_from &c)
      {
	return c;
      }

      // latlong to ecef
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,lat_long>::value,ecef>::type
      operator()(const convert_from &c, const ellipsoid_parameters &e = WGS84_ELLIPSOID)
      {
	double x,y,z;
	const double &pi = fr::constants::pi;
	double slat = sin(c.get_lat() * pi / 180);
	double clat = cos(c.get_lat() * pi / 180);
	double slon = sin(c.get_long() * pi / 180);
	double clon = cos(c.get_long() * pi / 180);
	double n = e.ae / sqrt(1.0 - e.ee * pow(slat, 2));
	x = (n + c.get_alt()) * clat * clon;
	y = (n + c.get_alt()) * clat * slon;
	z = (n * (1.0 - e.ee) + c.get_alt()) * slat;
	ecef retval(x,y,z);
	return retval;
      }

      // ecef_vel to ecef (loses velocity information)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef_vel>::value,ecef>::type
      operator()(const convert_from &c)
      {
	ecef retval(c.get_x(), c.get_y(), c.get_z());
	return retval;
      }

      // tod_eci to ecef (Requires time coordinate was measured)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci>::value,ecef>::type
      operator()(const convert_from &c, const double &at_time)
      {
	eci_to_ecef conversion_matrix(at_time);
	Eigen::Vector3d c_vec = c.get_xyz();
	Eigen::Matrix3d c_mat = conversion_matrix.get();
	Eigen::Vector3d interim = c_mat * c_vec;
	ecef retval(interim(0), interim(1), interim(2));
	return retval;
      }

    };

    /**********************************************************
     * Put tod_eci stuff here
     */

    template<>
    struct converter<tod_eci> {

      // ECI to ECI conversion

      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci>::value,tod_eci>::type
      operator()(const convert_from &c)
      {
	return c;
      }

      // ecef to tod_eci (Requires time coordinate was measured)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef>::value,tod_eci>::type
      operator()(const convert_from &c, const double &time_at)
      {
	ecef_to_eci conversion_matrix(time_at);
	Eigen::Vector3d c_vec = c.get_xyz();
	Eigen::Matrix3d c_mat = conversion_matrix.get();
	Eigen::Vector3d interim = c_mat * c_vec;
	tod_eci retval(interim(0), interim(1), interim(2));
	return retval;
      }

    };

    /***************************************************************
     * Put ecef_vel stuff here
     */

    template<>
    struct converter<ecef_vel> {
      // ecef_vel to ecef_vel conversion
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef_vel>::value,ecef_vel>::type
      operator()(const convert_from &c)
      {
	return c;
      }

      // tod_eci_vel to ecef_vel (requires time component)
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci_vel>::value,ecef_vel>::type
      operator()(const convert_from &c, const double &time_at)
      {
	Eigen::Matrix<double,6,1> vec = c.get_vector();
	eci_to_ecef cm(time_at);
	Eigen::Matrix<double,6,6> c_mat = cm.get_xyz_vel();
	Eigen::Matrix<double,6,1> interim = c_mat * vec;
	ecef_vel retval(interim(0), interim(1), interim(2), interim(3), interim(4), interim(5));
	return retval;
      }

    };

    /********************************************************************
     * Put tod_eci_vel stuff here
     */

    template<>
    struct converter<tod_eci_vel> {

      // From tod_eci_vel to tod_eci_vel
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,tod_eci_vel>::value,tod_eci_vel>::type
      operator()(const convert_from &c)
      {
	return c;
      }

      // from ecef_vel to tod_eci_vel
      template <typename convert_from>
      typename std::enable_if<std::is_same<convert_from,ecef_vel>::value,tod_eci_vel>::type
      operator()(const convert_from &c, const double &t)
      {
	ecef_to_eci cm(t);
	Eigen::Matrix<double,6,1> vec = c.get_vector();
	Eigen::Matrix<double,6,6> c_mat = cm.get_xyz_vel();
	Eigen::Matrix<double,6,1> interim = c_mat * vec;
	tod_eci_vel retval(interim(0), interim(1), interim(2), interim(3), interim(4), interim(5));
	return retval;
      }

    };

  }

}

#endif


#include <cmath>
#include <fstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <complex>
#include <tuple>
#include <algorithm>
#include <boost/range/iterator_range.hpp>

#include "common.hpp"
#include "fft.hpp"
#include "spectrum_image.hpp"
#include "segment_envelope.hpp"

spectrum_image::spectrum_image(
  const window_list_t &window,
  const std::vector< int16_t > &audio,
  int x_,
  uint32_t sample_rate_,
  uint32_t resolution_,
  uint32_t scale_,
  int weight,
  unsigned int interval
) : x( x_ ), resolution( resolution_ ), scale( scale_ ), sample_rate( sample_rate_ ) {
  a = powf( 2.f, float( scale ) + weight );
  b = float( interval ) * float( scale );
  const auto converted = fftref( window, audio, resolution, a, b, x );
  pixels_begin = converted.second.get();
  pixels = converted.second;
  envelope = std::move( converted.first );
  const auto delay_end = std::find_if( envelope.begin(), envelope.end(), []( float v ) { return v != 0.f; } );
  const auto delay_size = std::distance( envelope.begin(), delay_end );
  pixels_begin += delay_size * x;
  envelope.erase( envelope.begin(), delay_end );
  y = envelope.size();
  const auto tail_blank_end = std::find_if( envelope.rbegin(), envelope.rend(), []( float v ) { return v != 0.f; } );
  const auto tail_blank_size = std::distance( envelope.rbegin(), tail_blank_end );
  envelope.resize( envelope.size() - tail_blank_size );
  y = envelope.size();
  std::tie( delay, attack, release ) = segment_envelope( envelope, a, b );
  delay_time = ( a * delay * delay + b * delay ) * tinyfm3::delta;
  attack_time = ( a * attack * attack + b * attack ) * tinyfm3::delta;
  release_time = ( a * release * release + b * release ) * tinyfm3::delta;
  total_time = ( a * envelope.size() * envelope.size() + b * envelope.size() ) * tinyfm3::delta;
  std::cout << __FILE__ << " " << __LINE__ << " " << delay_time << " " << attack_time << " " << release_time << " " << total_time << std::endl;
}
float get_distance(
  const spectrum_image &ref,
  const window_list_t &window,
  const std::vector< int16_t > &audio
) {
  const float a = ref.get_a();
  const float b = ref.get_b();
  const auto converted = fftcomp( ref.get_pixels(), ref.get_height(), window, audio, ref.get_resolution(), a, b, ref.get_width() );
  float delay, attack, release;
  std::tie( delay, attack, release ) = segment_envelope( converted.second, ref.get_a(), ref.get_b() );
  double delay_time = ( a * delay * delay + b * delay ) * tinyfm3::delta;
  double attack_time = ( a * attack * attack + b * attack ) * tinyfm3::delta;
  double release_time = ( a * release * release + b * release ) * tinyfm3::delta;
  double delay_distance = std::abs( ref.get_delay_time() - delay_time );
  double attack_distance = std::abs( ref.get_attack_time() - attack_time );
  double release_distance = std::abs( ref.get_release_time() - release_time );
//  std::cout << delay_distance << " " << attack_distance << " " << release_distance << std::endl;
  return double( converted.first )/ref.get_width()/ref.get_height() * ( delay_distance * 40.f + attack_distance * 40.f + release_distance * 40.f + 1.f );
}


#ifndef CHARGE_EV_PHEM_HPP
#define CHARGE_EV_PHEM_HPP

#include "common/constant_function.hpp"
#include "common/serialization.hpp"

#include "ev/consumption_model.hpp"

#include <boost/algorithm/string.hpp>

#include <cstdint>

namespace charge::ev {
namespace detail {

enum class PHEMVehicleType : std::int32_t {
    PEUGEOT_ION,
    EV_NO_AUX,
    EV_SPRING,
    EV_SUMMER,
    EV_WINTER,
    INVALID = -1
};

class PHEMConsumptionModel : public ConsumptionModel {
  public:
    PHEMConsumptionModel(double speed_parameter, double slope_parameter, double const_parameter)
        : speed_parameter(speed_parameter), slope_parameter(slope_parameter),
          const_parameter(const_parameter) {}

    LimitedTradeoffFunction tradeoff_function(const double length, const double slope,
                                              const double min_speed,
                                              const double max_speed) const override final {
        assert(min_speed <= max_speed);
        assert(min_speed > 0);
        assert(max_speed < 300);

        const double limited_slope = std::max(-10.0, slope*100);
        // Original code has a scaling by 1000, this causes numeric problems.
        // const double a = 1000.0 * length * speed_parameter * (3.6 * 3.6 * length * length);
        // const double b = 0.0;
        // const double c = 1000.0 * length * (slope_parameter * limited_slope + const_parameter);
        const double a = length * speed_parameter * (3.6 * 3.6 * length * length);
        const double b = 0.0;
        const double c = length * (slope_parameter * limited_slope + const_parameter);
        const double min_duration = length * 3.6 / max_speed;
        const double max_duration = length * 3.6 / min_speed;
        assert(min_duration <= max_duration);
        assert(min_duration >= 0);

        assert(std::isfinite(a));
        assert(std::isfinite(b));
        assert(std::isfinite(c));
        assert(std::isfinite(min_duration));
        assert(std::isfinite(max_duration));

        LimitedTradeoffFunction tradeoff;
        if (a > 0)
            tradeoff = {min_duration, max_duration, common::HyperbolicFunction{a, b, c}};
        else
            tradeoff = {min_duration, max_duration, common::ConstantFunction{c}};

        // Only return a tradeoff function of the min and max speed are different
        if (max_speed - min_speed <= 1 || max_duration - min_duration < 1) {
            auto constant_consumption = tradeoff(tradeoff.min_x);
            return LimitedTradeoffFunction{min_duration, min_duration, common::ConstantFunction{constant_consumption}};
        } else {
            return tradeoff;
        }
    }

  private:
    double speed_parameter;
    double slope_parameter;
    double const_parameter;
};

std::unique_ptr<ConsumptionModel> make_consumption_model(PHEMVehicleType type) {
    double speed_parameter, slope_parameter, const_parameter;

    switch (type) {
    case PHEMVehicleType::PEUGEOT_ION:
        speed_parameter = 0.00001084948;
        slope_parameter = 0.02863728;
        const_parameter = 0.08052179;
        break;
    case PHEMVehicleType::EV_NO_AUX:
        speed_parameter = 0.00001129197;
        slope_parameter = 0.05127720;
        const_parameter = 0.1247448;
        break;
    case PHEMVehicleType::EV_SPRING:
        speed_parameter = 0.00001105337;
        slope_parameter = 0.05135052;
        const_parameter = 0.1287824;
        break;
    case PHEMVehicleType::EV_SUMMER:
        speed_parameter = 0.000009192936;
        slope_parameter = 0.05194433;
        const_parameter = 0.1605070;
        break;
    case PHEMVehicleType::EV_WINTER:
        speed_parameter = 0.000004730422;
        slope_parameter = 0.05345339;
        const_parameter = 0.2379125;
        break;
    default:
        throw std::runtime_error("Unknown vehicle type");
    }

    return std::make_unique<PHEMConsumptionModel>(speed_parameter, slope_parameter,
                                                  const_parameter);
}
}

std::unordered_map<std::string, std::unique_ptr<ConsumptionModel>>
make_phem_consumption_models() {
    std::unordered_map<std::string, std::unique_ptr<ConsumptionModel>> models;

    models["Peugeot Ion"] =
        detail::make_consumption_model(detail::PHEMVehicleType::PEUGEOT_ION);
    models["EV No Aux"] =
        detail::make_consumption_model(detail::PHEMVehicleType::EV_NO_AUX);
    models["EV Spring"] =
        detail::make_consumption_model(detail::PHEMVehicleType::EV_SPRING);
    models["EV Summer"] =
        detail::make_consumption_model(detail::PHEMVehicleType::EV_SUMMER);
    models["EV Winter"] =
        detail::make_consumption_model(detail::PHEMVehicleType::EV_WINTER);

    return models;
}
}

#endif


// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2017 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Tobias Leibner  (2017)

#ifndef DUNE_GDT_HYPERBOLIC_PROBLEMS_MOMENTMODELS_BASISFUNCTIONS_SPHERICALHARMONICS_HH
#define DUNE_GDT_HYPERBOLIC_PROBLEMS_MOMENTMODELS_BASISFUNCTIONS_SPHERICALHARMONICS_HH

#include <boost/math/special_functions/legendre.hpp>
#include <boost/math/special_functions/spherical_harmonic.hpp>

#include "base.hh"

namespace Dune {
namespace GDT {
namespace Hyperbolic {
namespace Problems {

// TODO: use complex arithmetic, currently only usable for Pn Models in 2D, test for only_positive = false
template <class DomainFieldType, class RangeFieldType, size_t order, size_t fluxDim, bool only_positive = true>
class SphericalHarmonics
    : public BasisfunctionsInterface<DomainFieldType,
                                     3,
                                     RangeFieldType,
                                     only_positive ? ((order + 1) * (order + 2)) / 2 : (order + 1) * (order + 1),
                                     1,
                                     fluxDim>
{
public:
  static const size_t dimDomain = 3;
  static const size_t dimRange = only_positive ? ((order + 1) * (order + 2)) / 2 : (order + 1) * (order + 1);
  static const size_t dimFlux = fluxDim;

private:
  typedef BasisfunctionsInterface<DomainFieldType, dimDomain, RangeFieldType, dimRange, 1, dimFlux> BaseType;

public:
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::MatrixType;
  template <class DiscreteFunctionType>
  using VisualizerType = typename BaseType::template VisualizerType<DiscreteFunctionType>;

  virtual RangeType evaluate(const DomainType& v) const override
  {
    const auto v_spherical = XT::Common::CoordinateConverter<DomainFieldType>::to_spherical(v);
    return evaluate_in_spherical_coords(v_spherical);
  } // ... evaluate(...)

  RangeType evaluate_in_spherical_coords(const FieldVector<DomainFieldType, 2>& coords) const
  {
    const DomainFieldType theta = coords[0];
    const DomainFieldType phi = coords[1];
    RangeType ret(0);
    // TODO: use complex arithmetic, remove real() call
    for (size_t ll = 0; ll <= order; ++ll)
      for (int mm = only_positive ? 0 : -int(ll); mm <= int(ll); ++mm)
        ret[helper<only_positive>::pos(ll, mm)] = boost::math::spherical_harmonic(ll, mm, theta, phi).real();
    return ret;
  } // ... evaluate(...)

  virtual RangeType integrated() const override
  {
    RangeType ret(0);
    ret[0] = std::sqrt(4. * M_PI);
    return ret;
  }

  virtual MatrixType mass_matrix() const override
  {
    MatrixType M(dimRange, dimRange, 0);
    for (size_t rr = 0; rr < dimRange; ++rr)
      M[rr][rr] = 1;
    return M;
  }

  virtual MatrixType mass_matrix_inverse() const override
  {
    return mass_matrix();
  }

  virtual FieldVector<MatrixType, dimFlux> mass_matrix_with_v() const override
  {
    FieldVector<MatrixType, dimFlux> ret(MatrixType(dimRange, dimRange, 0));
    ret[0] = create_Bx();
    ret[1] = create_Bz();
    //    if (dimFlux == 3)
    //      ret[2] = create_By();
    return ret;
  } // ... mass_matrix_with_v()

  template <class DiscreteFunctionType>
  VisualizerType<DiscreteFunctionType> visualizer() const
  {
    return [](const DiscreteFunctionType& u_n, const std::string& filename_prefix, const size_t ii) {
      component_visualizer<DiscreteFunctionType, dimRange, 0>(u_n, filename_prefix, ii, std::sqrt(4 * M_PI));
    };
  }

  std::pair<RangeType, RangeType> calculate_isotropic_distribution(const RangeType& u) const
  {
    RangeType u_iso(0), alpha_iso(0);
    u_iso[0] = u[0];
    alpha_iso[0] = std::log(u[0] / (4. * M_PI));
    return std::make_pair(u_iso, alpha_iso);
  }

private:
  static RangeFieldType A_lm(const size_t l, const int m)
  {
    return std::sqrt((l + m) * (l - m) / ((2. * l + 1.) * (2. * l - 1.)));
  }

  static RangeFieldType B_lm(const size_t l, const int m)
  {
    return std::sqrt((l + m) * (l + m - 1.) / ((2. * l + 1.) * (2. * l - 1.)));
  }

  static MatrixType create_Bx()
  {
    MatrixType Bx(dimRange, dimRange, 0);
    const auto& pos = helper<only_positive>::pos;
    for (size_t l1 = 0; l1 <= order; ++l1) {
      for (int m1 = only_positive ? 0 : -int(l1); size_t(std::abs(m1)) <= l1; ++m1) {
        for (size_t l2 = 0; l2 <= order; ++l2) {
          for (int m2 = -int(l2); size_t(std::abs(m2)) <= l2; ++m2) {
            size_t row = pos(l1, m1);
            size_t col = pos(l2, only_positive ? std::abs(m2) : m2);
            RangeFieldType factor = !only_positive ? 1. : (m2 < 0 ? std::pow(-1., m2) : 1.);
            if (l1 == l2 + 1 && m1 == m2 + 1)
              Bx[row][col] += -0.5 * factor * B_lm(l2 + 1, m2 + 1);
            if (l1 == l2 - 1 && m1 == m2 + 1)
              Bx[row][col] += 0.5 * factor * B_lm(l2, -m2);
            if (l1 == l2 + 1 && m1 == m2 - 1)
              Bx[row][col] += 0.5 * factor * B_lm(l2 + 1, -m2 - 1);
            if (l1 == l2 - 1 && m1 == m2 - 1)
              Bx[row][col] += -0.5 * factor * B_lm(l2, m2);
          } // m2
        } // l2
      } // m1
    } // l1
    return Bx;
  } // ... create_Bx()

  //    static MatrixType create_By()
  //    {
  //      MatrixType By(dimRange, dimRange, 0);
  //      const auto& pos = helper<only_positive>::pos;
  //      for (size_t l1 = 0; l1 <= order; ++l1) {
  //        for (int m1 = only_positive ? 0 : -l1; size_t(std::abs(m1)) <= l1; ++m1) {
  //          for (size_t l2 = 0; l2 <= order; ++l2) {
  //            for (int m2 = -int(l2); size_t(std::abs(m2)) <= l2; ++m2) {
  //              size_t row = pos(l1, m1);
  //              size_t col = pos(l2, only_positive ? std::abs(m2) : m2);
  //              RangeFieldType factor = !only_positive ? 1. : (m2 < 0 ? std::pow(-1., m2) : 1.);
  //              if (l1 == l2 + 1 && m1 == m2 + 1)
  //                By[row][col] += 0.5 * factor * std::complex<RangeFieldType>(0, 1) * B_lm(l2 + 1, m2 + 1);
  //              if (l1 == l2 - 1 && m1 == m2 + 1)
  //                By[row][col] += -0.5 * factor * std::complex<RangeFieldType>(0, 1) * B_lm(l2, -m2);
  //              if (l1 == l2 + 1 && m1 == m2 - 1)
  //                By[row][col] += 0.5 * factor * std::complex<RangeFieldType>(0, 1) * B_lm(l2 + 1, -m2 - 1);
  //              if (l1 == l2 - 1 && m1 == m2 - 1)
  //                By[row][col] += -0.5 * factor * std::complex<RangeFieldType>(0, 1) * B_lm(l2, m2);
  //            } // m2
  //          } // l2
  //        } // m1
  //      } // l1
  //      return By;
  //    } // ... create_By()

  static MatrixType create_Bz()
  {
    MatrixType Bz(dimRange, dimRange, 0);
    const auto& pos = helper<only_positive>::pos;
    for (size_t l1 = 0; l1 <= order; ++l1) {
      for (int m1 = only_positive ? 0. : -int(l1); size_t(std::abs(m1)) <= l1; ++m1) {
        for (size_t l2 = 0; l2 <= order; ++l2) {
          size_t row = pos(l1, m1);
          size_t col = pos(l2, m1); // m1 == m2, else matrix entry is 0
          if (l1 == l2 + 1)
            Bz[row][col] += A_lm(l2 + 1, m1);
          if (l1 == l2 - 1)
            Bz[row][col] += A_lm(l2, m1);
        } // l2
      } // m1
    } // l1
    return Bz;
  }

  template <bool positive, class anything = void>
  struct helper
  {
    // Converts a pair (l, m) to a vector index. The vector is ordered by l first, then by m.
    // Each l has 2l+1 values of m, so (l, m) has position
    // (\sum_{k=0}^{l-1} (2k+1)) + (m+l) = l^2 + m + l
    static size_t pos(const size_t l, const int m)
    {
      return size_t(l * l + m + l);
    }
  };

  template <class anything>
  struct helper<true, anything>
  {
    // Converts a pair (l, m) to a vector index. The vector is ordered by l first, then by m.
    // Each l has l+1 non-negative values of m, so (l, m) has position
    // (\sum_{k=0}^{l-1} (l+1)) + m = l(l+1)/2 + m
    static size_t pos(const size_t l, const int m)
    {
      return l * (l + 1) / 2 + m;
    }
  };
}; // class SphericalHarmonics<DomainFieldType, 3, ...>

template <class DomainFieldType, class RangeFieldType, size_t order, size_t fluxDim, bool only_even = false>
class RealSphericalHarmonics
    : public BasisfunctionsInterface<DomainFieldType,
                                     3,
                                     RangeFieldType,
                                     only_even ? ((order + 1) * (order + 2)) / 2 : (order + 1) * (order + 1),
                                     1,
                                     fluxDim>
{
public:
  static const size_t dimDomain = 3;
  static const size_t dimFlux = fluxDim;
  static const size_t dimRange = only_even ? ((order + 1) * (order + 2)) / 2 : (order + 1) * (order + 1);

private:
  typedef BasisfunctionsInterface<DomainFieldType, dimDomain, RangeFieldType, dimRange, 1, dimFlux> BaseType;

public:
  typedef typename Dune::QuadratureRule<DomainFieldType, dimDomain> QuadratureType;
  using typename BaseType::DomainType;
  using typename BaseType::RangeType;
  using typename BaseType::MatrixType;
  template <class DiscreteFunctionType>
  using VisualizerType = typename BaseType::template VisualizerType<DiscreteFunctionType>;

  virtual RangeType evaluate(const DomainType& v) const override
  {
    const auto v_spherical = XT::Common::CoordinateConverter<DomainFieldType>::to_spherical(v);
    return evaluate_in_spherical_coords(v_spherical);
  } // ... evaluate(...)

  RangeType evaluate_in_spherical_coords(const FieldVector<DomainFieldType, 2>& coords) const
  {
    const DomainFieldType theta = coords[0];
    const DomainFieldType phi = coords[1];
    RangeType ret(0);
    for (size_t ll = 0; ll <= order; ++ll)
      for (int mm = -int(ll); mm <= int(ll); ++mm)
        if (!only_even || !((mm + ll) % 2))
          ret[helper<only_even>::pos(ll, mm)] = evaluate_lm(theta, phi, int(ll), mm);
    return ret;
  } // ... evaluate(...)

  virtual RangeType integrated() const override
  {
    RangeType ret(0);
    ret[0] = std::sqrt(4. * M_PI);
    return ret;
  }

  virtual MatrixType mass_matrix() const override
  {
    MatrixType M(dimRange, dimRange, 0);
    for (size_t rr = 0; rr < dimRange; ++rr)
      M[rr][rr] = 1;
    return M;
  }

  virtual MatrixType mass_matrix_inverse() const override
  {
    return mass_matrix();
  }

  virtual FieldVector<MatrixType, dimFlux> mass_matrix_with_v() const override
  {
    FieldVector<MatrixType, dimFlux> ret(MatrixType(dimRange, dimRange, 0));
    ret[0] = create_Bx();
    ret[1] = create_By();
    if (dimFlux == 3)
      ret[2] = create_Bz();
    return ret;
  } // ... mass_matrix_with_v()

  std::pair<RangeType, RangeType> calculate_isotropic_distribution(const RangeType& u) const
  {
    RangeType u_iso(0), alpha_iso(0);
    u_iso[0] = u[0];
    alpha_iso[0] = std::log(u[0] / std::sqrt(4. * M_PI)) * std::sqrt(4. * M_PI);
    return std::make_pair(u_iso, alpha_iso);
  }

  template <class DiscreteFunctionType>
  VisualizerType<DiscreteFunctionType> visualizer() const
  {
    return [](const DiscreteFunctionType& u_n, const std::string& filename_prefix, const size_t ii) {
      component_visualizer<DiscreteFunctionType, dimRange, 0>(u_n, filename_prefix, ii, std::sqrt(4 * M_PI));
    };
  }

  RangeFieldType realizability_limiter_max(const RangeType& u, const RangeType& u_bar) const
  {
    return 2 * std::max(u[0], u_bar[0]);
  }

private:
  static RangeFieldType A_lm(const size_t l, const int m)
  {
    return std::sqrt((l + m) * (l - m) / ((2. * l + 1.) * (2. * l - 1.)));
  }

  static RangeFieldType B_lm(const size_t l, const int m)
  {
    return std::sqrt((l + m) * (l + m - 1.) / ((2. * l + 1.) * (2. * l - 1.)));
  }

  static MatrixType create_Bx()
  {
    MatrixType Bx(dimRange, dimRange, 0.);
    const auto& pos = helper<only_even>::pos;
    for (size_t l1 = 0; l1 <= order; ++l1) {
      for (int m1 = -int(l1); size_t(std::abs(m1)) <= l1; ++m1) {
        for (size_t l2 = 0; l2 <= order; ++l2) {
          for (int m2 = -int(l2); size_t(std::abs(m2)) <= l2; ++m2) {
            if (!only_even || (!((m1 + l1) % 2) && !((m2 + l2) % 2))) {
              if (l1 == l2 - 1 && m1 == m2 - 1 && m2 > 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += 0.5 * std::sqrt(1. + (m2 == 1)) * B_lm(l2, m2);
              if (l1 == l2 + 1 && m1 == m2 - 1 && m2 > 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += -0.5 * std::sqrt(1. + (m2 == 1)) * B_lm(l2 + 1, -m2 + 1);
              if (l1 == l2 - 1 && m1 == m2 + 1 && m2 > 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += -0.5 * B_lm(l2, -m2);
              if (l1 == l2 + 1 && m1 == m2 + 1 && m2 > 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += 0.5 * B_lm(l2 + 1, m2 + 1);
              if (l1 == l2 - 1 && m1 == m2 + 1 && m2 < 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += 0.5 * (1. - (-m2 == 1)) * B_lm(l2, -m2);
              if (l1 == l2 + 1 && m1 == m2 + 1 && m2 < 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += -0.5 * (1. - (-m2 == 1)) * B_lm(l2 + 1, m2 + 1);
              if (l1 == l2 - 1 && m1 == m2 - 1 && m2 < 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += -0.5 * B_lm(l2, m2);
              if (l1 == l2 + 1 && m1 == m2 - 1 && m2 < 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += 0.5 * B_lm(l2 + 1, -m2 + 1);
              if (l1 == l2 - 1 && m1 == 1 && m2 == 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += -1. / std::sqrt(2.) * B_lm(l2, 0);
              if (l1 == l2 + 1 && m1 == 1 && m2 == 0)
                Bx[pos(l1, m1)][pos(l2, m2)] += 1. / std::sqrt(2.) * B_lm(l2 + 1, 1);
            }
          } // m2
        } // l2
      } // m1
    } // l1
    return Bx;
  }

  static MatrixType create_By()
  {
    MatrixType By(dimRange, dimRange, 0.);
    const auto& pos = helper<only_even>::pos;
    for (size_t l1 = 0; l1 <= order; ++l1) {
      for (int m1 = -int(l1); size_t(std::abs(m1)) <= l1; ++m1) {
        for (size_t l2 = 0; l2 <= order; ++l2) {
          for (int m2 = -int(l2); size_t(std::abs(m2)) <= l2; ++m2) {
            if (!only_even || (!((m1 + l1) % 2) && !((m2 + l2) % 2))) {
              if (l1 == l2 + 1 && m1 == -m2 + 1 && m2 > 0)
                By[pos(l1, m1)][pos(l2, m2)] += 0.5 * (1. - (m2 == 1)) * B_lm(l2 + 1, -m2 + 1);
              if (l1 == l2 - 1 && m1 == -m2 + 1 && m2 > 0)
                By[pos(l1, m1)][pos(l2, m2)] += -0.5 * (1. - (m2 == 1)) * B_lm(l2, m2);
              if (l1 == l2 - 1 && m1 == -m2 - 1 && m2 > 0)
                By[pos(l1, m1)][pos(l2, m2)] += -0.5 * B_lm(l2, -m2);
              if (l1 == l2 + 1 && m1 == -m2 - 1 && m2 > 0)
                By[pos(l1, m1)][pos(l2, m2)] += 0.5 * B_lm(l2 + 1, m2 + 1);
              if (l1 == l2 - 1 && m1 == -m2 - 1 && m2 < 0)
                By[pos(l1, m1)][pos(l2, m2)] += 0.5 * std::sqrt(1. + (-m2 == 1)) * B_lm(l2, -m2);
              if (l1 == l2 + 1 && m1 == -m2 - 1 && m2 < 0)
                By[pos(l1, m1)][pos(l2, m2)] += -0.5 * std::sqrt(1. + (-m2 == 1)) * B_lm(l2 + 1, m2 + 1);
              if (l1 == l2 - 1 && m1 == -m2 + 1 && m2 < 0)
                By[pos(l1, m1)][pos(l2, m2)] += 0.5 * B_lm(l2, m2);
              if (l1 == l2 + 1 && m1 == -m2 + 1 && m2 < 0)
                By[pos(l1, m1)][pos(l2, m2)] += -0.5 * B_lm(l2 + 1, -m2 + 1);
              if (l1 == l2 - 1 && m1 == -1 && m2 == 0)
                By[pos(l1, m1)][pos(l2, m2)] += -1. / std::sqrt(2.) * B_lm(l2, 0);
              if (l1 == l2 + 1 && m1 == -1 && m2 == 0)
                By[pos(l1, m1)][pos(l2, m2)] += 1. / std::sqrt(2.) * B_lm(l2 + 1, 1);
            }
          } // m2
        } // l2
      } // m1
    } // l1
    return By;
  } // ... create_By()

  static MatrixType create_Bz()
  {
    MatrixType Bz(dimRange, dimRange, 0);
    const auto& pos = helper<only_even>::pos;
    for (size_t l1 = 0; l1 <= order; ++l1) {
      for (int m1 = -int(l1); size_t(std::abs(m1)) <= l1; ++m1) {
        for (size_t l2 = 0; l2 <= order; ++l2) {
          for (int m2 = -int(l2); size_t(std::abs(m2)) <= l2; ++m2) {
            if (!only_even || (!((m1 + l1) % 2) && !((m2 + l2) % 2))) {
              if (m1 == m2 && l1 == l2 + 1)
                Bz[pos(l1, m1)][pos(l2, m2)] += A_lm(l2 + 1, m2);
              if (m1 == m2 && l1 == l2 - 1)
                Bz[pos(l1, m1)][pos(l2, m2)] += A_lm(l2, m2);
            }
          } // m2
        } // l2
      } // m1
    } // l1
    return Bz;
  } // ... create_Bz()

  template <bool even, class anything = void>
  struct helper
  {
    // Converts a pair (l, m) to a vector index. The vector is ordered by l first, then by m.
    // Each l has 2l+1 values of m, so (l, m) has position
    // (\sum_{k=0}^{l-1} (2k+1)) + (m+l) = l^2 + m + l
    static size_t pos(const size_t l, const int m)
    {
      return size_t(l * l + m + l);
    }
  };

  template <class anything>
  struct helper<true, anything>
  {
    // Converts a pair (l, m) to a vector index. The vector is ordered by l first, then by m.
    // Each l has l+1 values of m (as only m s.t. m+l is even are considered), so (l, m) has position
    // (\sum_{k=0}^{l-1} (k+1)) + (m+l)/2 = l(l+1)/2 + (l+m)/2
    static size_t pos(const int l, const int m)
    {
      return size_t(l * (l + 1) / 2 + (m + l) / 2);
    }
  };

  // Notation from Garrett, Hauck, "A Comparison of Moment Closures for Linear Kinetic Transport Equations: The Line
  // Source Benchmark",
  // http://www.tandfonline.com/doi/full/10.1080/00411450.2014.910226?src=recsys&, Section 4.1
  RangeFieldType N_lm(const int l, const int m) const
  {
    assert(l >= 0 && m >= 0 && m <= l);
    return std::sqrt((2. * l + 1.) * XT::Common::factorial(l - m) / (XT::Common::factorial(l + m) * 4. * M_PI));
  }

  RangeFieldType evaluate_lm(const DomainFieldType theta, const DomainFieldType phi, const int l, const int m) const
  {
    const auto cos_theta = std::cos(theta);
    assert(l >= 0 && std::abs(m) <= l);
    if (m < 0)
      return std::sqrt(2) * N_lm(l, -m) * boost::math::legendre_p(l, -m, cos_theta) * std::sin(-m * phi);
    else if (m == 0)
      return N_lm(l, 0) * boost::math::legendre_p(l, 0, cos_theta);
    else
      return std::sqrt(2) * N_lm(l, m) * boost::math::legendre_p(l, m, cos_theta) * std::cos(m * phi);
  }
}; // class RealSphericalHarmonics<DomainFieldType, 3, ...>

} // namespace Problems
} // namespace Hyperbolic
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_HYPERBOLIC_PROBLEMS_MOMENTMODELS_BASISFUNCTIONS_SPHERICALHARMONICS_HH


#include "OGR.h"
#include <boost/math/constants/constants.hpp>
#include <macgyver/Exception.h>
#include <ogr_geometry.h>

// ----------------------------------------------------------------------
/*!
 * \brief Calculate area in m^2 for a geography
 *
 * Refs: Some algorithms for polygons on a sphere
 *       http://hdl.handle.net/2014/40409
 *
 *       https://trac.osgeo.org/openlayers/browser/trunk/openlayers/lib/OpenLayers/Geometry/LinearRing.js
 *
 * Note: The algorithm returns a positive number for clockwise rings.
 */
// ----------------------------------------------------------------------

double geographic_area(const OGRLineString *theGeom)
{
  try
  {
    double area = 0;

    std::size_t npoints = theGeom->getNumPoints();

    for (std::size_t i = 0; i < npoints - 1; i++)
    {
      double x1 = theGeom->getX(i) * boost::math::double_constants::degree;
      double y1 = theGeom->getY(i) * boost::math::double_constants::degree;
      double x2 = theGeom->getX(i + 1) * boost::math::double_constants::degree;
      double y2 = theGeom->getY(i + 1) * boost::math::double_constants::degree;

      area += (x2 - x1) * (2 + sin(y1) + sin(y2));
    }
    area *= 6378137.0 * 6378137.0 / 2.0;

    return std::abs(area);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

double geographic_area(const OGRLinearRing *theGeom)
{
  try
  {
    return geographic_area(static_cast<const OGRLineString *>(theGeom));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief get_Area substitute for OGRLineString copied from OGRLinearRing::get_Area()
 *
 * The area is computed according to Green's Theorem:
 *
 * Area is "Sum(x(i)*(y(i+1) - y(i-1)))/2" for i = 0 to pointCount-1,
 * assuming the last point is a duplicate of the first.
 *
 */
// ----------------------------------------------------------------------

double metric_area(const OGRLineString *theGeom)
{
  try
  {
    std::size_t npoints = theGeom->getNumPoints();
    if (npoints < 2)
      return 0;

    double area = theGeom->getX(0) * (theGeom->getY(1) - theGeom->getY(npoints - 1));

    for (std::size_t i = 1; i < npoints - 1; i++)
      area += theGeom->getX(i) * (theGeom->getY(i + 1) - theGeom->getY(i - 1));

    area += theGeom->getX(npoints - 1) * (theGeom->getY(0) - theGeom->getY(npoints - 2));

    return 0.5 * std::abs(area);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRPolygon *despeckle_polygon(const OGRPolygon *theGeom, double theLimit, bool theGeogFlag)
{
  try
  {
    // Quick exit if the exterior is too small

    const auto *exterior = theGeom->getExteriorRing();
    double area = (theGeogFlag ? geographic_area(exterior) : exterior->get_Area());

    if (area < theLimit)
      return nullptr;

    // We have at least a valid exterior

    auto *out = new OGRPolygon;
    out->addRingDirectly(dynamic_cast<OGRLinearRing *>(exterior->clone()));

    // Remove too small holes too

    for (int i = 0, n = theGeom->getNumInteriorRings(); i < n; ++i)
    {
      const auto *hole = theGeom->getInteriorRing(i);
      area = (theGeogFlag ? geographic_area(hole) : hole->get_Area());
      if (area >= theLimit)
        out->addRingDirectly(dynamic_cast<OGRLinearRing *>(hole->clone()));
    }

    return out;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRLineString *despeckle_linestring(const OGRLineString *theGeom, double theLimit, bool theGeogFlag)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    if (!theGeom->get_IsClosed())
      return dynamic_cast<OGRLineString *>(theGeom->clone());

    // Despeckle closed linestrings only

    // TODO: Old GDAL does not have get_Area for linestrings
    // double area = (theGeogFlag ? geographic_area(theGeom) : geom->get_Area());
    double area = (theGeogFlag ? geographic_area(theGeom) : metric_area(theGeom));

    if (area < theLimit)
      return nullptr;

    return dynamic_cast<OGRLineString *>(theGeom->clone());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRPoint *despeckle_point(const OGRPoint *theGeom)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    return dynamic_cast<OGRPoint *>(theGeom->clone());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRMultiPoint *despeckle_multipoint(const OGRMultiPoint *theGeom)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    return dynamic_cast<OGRMultiPoint *>(theGeom->clone());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRMultiLineString *despeckle_multilinestring(const OGRMultiLineString *theGeom,
                                              double theLimit,
                                              bool theGeogFlag)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;
    ;

    auto *out = new OGRMultiLineString();

    for (int i = 0, n = theGeom->getNumGeometries(); i < n; ++i)
    {
      auto *geom = despeckle_linestring(
          dynamic_cast<const OGRLineString *>(theGeom->getGeometryRef(i)), theLimit, theGeogFlag);
      if (geom != nullptr)
        out->addGeometryDirectly(geom);
    }

    if (out->IsEmpty() == 0)
      return out;

    delete out;
    return nullptr;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

OGRMultiPolygon *despeckle_multipolygon(const OGRMultiPolygon *theGeom,
                                        double theLimit,
                                        bool theGeogFlag)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    auto *out = new OGRMultiPolygon();

    for (int i = 0, n = theGeom->getNumGeometries(); i < n; ++i)
    {
      auto *geom = despeckle_polygon(
          dynamic_cast<const OGRPolygon *>(theGeom->getGeometryRef(i)), theLimit, theGeogFlag);
      if (geom != nullptr)
        out->addGeometryDirectly(geom);
    }

    if (out->IsEmpty() == 0)
      return out;

    delete out;
    return nullptr;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry
 */
// ----------------------------------------------------------------------

// Needed since two functions call each other

OGRGeometry *despeckle_geom(const OGRGeometry *theGeom, double theLimit, bool theGeogFlag);

OGRGeometryCollection *despeckle_geometrycollection(const OGRGeometryCollection *theGeom,
                                                    double theLimit,
                                                    bool theGeogFlag)
{
  try
  {
    if (theGeom == nullptr || theGeom->IsEmpty() != 0)
      return nullptr;

    auto *out = new OGRGeometryCollection;

    for (int i = 0, n = theGeom->getNumGeometries(); i < n; ++i)
    {
      auto *geom = despeckle_geom(theGeom->getGeometryRef(i), theLimit, theGeogFlag);
      if (geom != nullptr)
        out->addGeometryDirectly(geom);
    }

    if (out->IsEmpty() == 0)
      return out;

    delete out;
    return nullptr;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle OGR geometry to output geometry
 */
// ----------------------------------------------------------------------

OGRGeometry *despeckle_geom(const OGRGeometry *theGeom, double theLimit, bool theGeogFlag)
{
  try
  {
    OGRwkbGeometryType id = theGeom->getGeometryType();

    switch (id)
    {
      case wkbPoint:
        return despeckle_point(dynamic_cast<const OGRPoint *>(theGeom));
      case wkbLineString:
        return despeckle_linestring(
            dynamic_cast<const OGRLineString *>(theGeom), theLimit, theGeogFlag);
      case wkbPolygon:
        return despeckle_polygon(dynamic_cast<const OGRPolygon *>(theGeom), theLimit, theGeogFlag);
      case wkbMultiPoint:
        return despeckle_multipoint(dynamic_cast<const OGRMultiPoint *>(theGeom));
      case wkbMultiLineString:
        return despeckle_multilinestring(
            dynamic_cast<const OGRMultiLineString *>(theGeom), theLimit, theGeogFlag);
      case wkbMultiPolygon:
        return despeckle_multipolygon(
            dynamic_cast<const OGRMultiPolygon *>(theGeom), theLimit, theGeogFlag);
      case wkbGeometryCollection:
        return despeckle_geometrycollection(
            dynamic_cast<const OGRGeometryCollection *>(theGeom), theLimit, theGeogFlag);
      case wkbLinearRing:
        throw Fmi::Exception::Trace(BCP, "Direct despeckling of LinearRings is not supported");
      case wkbNone:
        throw Fmi::Exception::Trace(
            BCP, "Encountered a 'none' geometry component when despeckling a geometry");
      default:
        throw Fmi::Exception::Trace(
            BCP, "Encountered an unknown geometry component when clipping polygons");
    }

    // NOT REACHED
    return nullptr;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Despeckle a geometry so that small polygons are removed
 *
 * \return Empty GeometryCollection if the result is empty
 */
// ----------------------------------------------------------------------

OGRGeometry *Fmi::OGR::despeckle(const OGRGeometry &theGeom, double theAreaLimit)
{
  try
  {
    // Area calculations for geographies must be done by ourselves, OGR
    // does it in the native system and hence would produce square degrees.

    OGRSpatialReference *crs = theGeom.getSpatialReference();
    bool geographic = (crs != nullptr ? (crs->IsGeographic() != 0) : false);

    // Actual despeckling

    auto *geom =
        despeckle_geom(&theGeom, theAreaLimit * 1000 * 1000, geographic);  // from m^2 to km^2

    if (geom != nullptr)
      geom->assignSpatialReference(theGeom.getSpatialReference());  // SR is ref. counted

    return geom;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#ifndef __IS_KURATOWSKI_SUBGRAPH_HPP__
#define __IS_KURATOWSKI_SUBGRAPH_HPP__

#include <boost/config.hpp>
#include <boost/utility.hpp> //for next/prior
#include <boost/tuple/tuple.hpp>   //for tie
#include <boost/property_map/property_map.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/isomorphism.hpp>
#include <boost/graph/adjacency_list.hpp>

#include <algorithm>
#include <vector>
#include <set>

namespace boost
{

  namespace detail
  {

    template <typename Graph>
    Graph make_K_5()
    {
      typename graph_traits<Graph>::vertex_iterator vi, vi_end, inner_vi;
      Graph K_5(5);
      for(tie(vi,vi_end) = vertices(K_5); vi != vi_end; ++vi)
        for(inner_vi = next(vi); inner_vi != vi_end; ++inner_vi)
          add_edge(*vi, *inner_vi, K_5);
      return K_5;
    }

    template <typename Graph>
    Graph make_K_3_3()
    {
      typename graph_traits<Graph>::vertex_iterator
        vi, vi_end, bipartition_start, inner_vi;
      Graph K_3_3(6);
      bipartition_start = next(next(next(vertices(K_3_3).first)));
      for(tie(vi, vi_end) = vertices(K_3_3); vi != bipartition_start; ++vi)
        for(inner_vi= bipartition_start; inner_vi != vi_end; ++inner_vi)
          add_edge(*vi, *inner_vi, K_3_3);
      return K_3_3;
    }

    template <typename AdjacencyList, typename Vertex>
    void contract_edge(AdjacencyList& neighbors, Vertex u, Vertex v)
    {
      // Remove u from v's neighbor list
      neighbors[v].erase(std::remove(neighbors[v].begin(),
                                     neighbors[v].end(), u
                                     ),
                         neighbors[v].end()
                         );

      // Replace any references to u with references to v
      typedef typename AdjacencyList::value_type::iterator
        adjacency_iterator_t;

      adjacency_iterator_t u_neighbor_end = neighbors[u].end();
      for(adjacency_iterator_t u_neighbor_itr = neighbors[u].begin();
          u_neighbor_itr != u_neighbor_end; ++u_neighbor_itr
          )
        {
          Vertex u_neighbor(*u_neighbor_itr);
          std::replace(neighbors[u_neighbor].begin(),
                       neighbors[u_neighbor].end(), u, v
                       );
        }

      // Remove v from u's neighbor list
      neighbors[u].erase(std::remove(neighbors[u].begin(),
                                     neighbors[u].end(), v
                                     ),
                         neighbors[u].end()
                         );

      // Add everything in u's neighbor list to v's neighbor list
      std::copy(neighbors[u].begin(),
                neighbors[u].end(),
                std::back_inserter(neighbors[v])
                );

      // Clear u's neighbor list
      neighbors[u].clear();

    }

    enum target_graph_t { tg_k_3_3, tg_k_5};

  } // namespace detail

  template <typename Graph, typename ForwardIterator, typename VertexIndexMap>
  bool is_kuratowski_subgraph(const Graph& g,
                              ForwardIterator begin,
                              ForwardIterator end,
                              VertexIndexMap vm
                              )
  {

    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename graph_traits<Graph>::edges_size_type e_size_t;
    typedef typename graph_traits<Graph>::vertices_size_type v_size_t;
    typedef typename std::vector<vertex_t> v_list_t;
    typedef typename v_list_t::iterator v_list_iterator_t;
    typedef iterator_property_map
      <typename std::vector<v_list_t>::iterator, VertexIndexMap>
      vertex_to_v_list_map_t;

    typedef adjacency_list<vecS, vecS, undirectedS> small_graph_t;

    detail::target_graph_t target_graph = detail::tg_k_3_3; //unless we decide otherwise later

    static small_graph_t K_5(detail::make_K_5<small_graph_t>());

    static small_graph_t K_3_3(detail::make_K_3_3<small_graph_t>());

    v_size_t n_vertices(num_vertices(g));
    v_size_t max_num_edges(3*n_vertices - 5);

    std::vector<v_list_t> neighbors_vector(n_vertices);
    vertex_to_v_list_map_t neighbors(neighbors_vector.begin(), vm);

    e_size_t count = 0;
    for(ForwardIterator itr = begin; itr != end; ++itr)
      {

        if (count++ > max_num_edges)
          return false;

        edge_t e(*itr);
        vertex_t u(source(e,g));
        vertex_t v(target(e,g));

        neighbors[u].push_back(v);
        neighbors[v].push_back(u);

      }

    for(v_size_t max_size = 2; max_size < 5; ++max_size)
      {

        vertex_iterator_t vi, vi_end;
        for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
          {
            vertex_t v(*vi);

            //a hack to make sure we don't contract the middle edge of a path
            //of four degree-3 vertices
            if (max_size == 4 && neighbors[v].size() == 3)
              {
                if (neighbors[neighbors[v][0]].size() +
                    neighbors[neighbors[v][1]].size() +
                    neighbors[neighbors[v][2]].size()
                    < 11 // so, it has two degree-3 neighbors
                    )
                  continue;
              }

            while (neighbors[v].size() > 0 && neighbors[v].size() < max_size)
              {
                // Find one of v's neighbors u such that that v and u
                // have no neighbors in common. We'll look for such a
                // neighbor with a naive cubic-time algorithm since the
                // max size of any of the neighbor sets we'll consider
                // merging is 3

                bool neighbor_sets_intersect = false;

                vertex_t min_u = graph_traits<Graph>::null_vertex();
                vertex_t u;
                v_list_iterator_t v_neighbor_end = neighbors[v].end();
                for(v_list_iterator_t v_neighbor_itr = neighbors[v].begin();
                    v_neighbor_itr != v_neighbor_end;
                    ++v_neighbor_itr
                    )
                  {
                    neighbor_sets_intersect = false;
                    u = *v_neighbor_itr;
                    v_list_iterator_t u_neighbor_end = neighbors[u].end();
                    for(v_list_iterator_t u_neighbor_itr =
                          neighbors[u].begin();
                        u_neighbor_itr != u_neighbor_end &&
                          !neighbor_sets_intersect;
                        ++u_neighbor_itr
                        )
                      {
                        for(v_list_iterator_t inner_v_neighbor_itr =
                              neighbors[v].begin();
                            inner_v_neighbor_itr != v_neighbor_end;
                            ++inner_v_neighbor_itr
                            )
                          {
                            if (*u_neighbor_itr == *inner_v_neighbor_itr)
                              {
                                neighbor_sets_intersect = true;
                                break;
                              }
                          }

                      }
                    if (!neighbor_sets_intersect &&
                        (min_u == graph_traits<Graph>::null_vertex() ||
                         neighbors[u].size() < neighbors[min_u].size())
                        )
                      {
                        min_u = u;
                      }

                  }

                if (min_u == graph_traits<Graph>::null_vertex())
                  // Exited the loop without finding an appropriate neighbor of
                  // v, so v must be a lost cause. Move on to other vertices.
                  break;
                else
                  u = min_u;

                detail::contract_edge(neighbors, u, v);

              }//end iteration over v's neighbors

          }//end iteration through vertices v

        if (max_size == 3)
          {
            // check to see whether we should go on to find a K_5
            for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
              if (neighbors[*vi].size() == 4)
                {
                  target_graph = detail::tg_k_5;
                  break;
                }

            if (target_graph == detail::tg_k_3_3)
              break;
          }

      }//end iteration through max degree 2,3, and 4

    //Now, there should only be 5 or 6 vertices with any neighbors. Find them.

    v_list_t main_vertices;
    vertex_iterator_t vi, vi_end;

    for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
      {
        if (!neighbors[*vi].empty())
          main_vertices.push_back(*vi);
      }

    // create a graph isomorphic to the contracted graph to test
    // against K_5 and K_3_3
    small_graph_t contracted_graph(main_vertices.size());
    std::map<vertex_t,typename graph_traits<small_graph_t>::vertex_descriptor>
      contracted_vertex_map;

    typename v_list_t::iterator itr, itr_end;
    itr_end = main_vertices.end();
    typename graph_traits<small_graph_t>::vertex_iterator
      si = vertices(contracted_graph).first;

    for(itr = main_vertices.begin(); itr != itr_end; ++itr, ++si)
      {
        contracted_vertex_map[*itr] = *si;
      }

    typename v_list_t::iterator jtr, jtr_end;
    for(itr = main_vertices.begin(); itr != itr_end; ++itr)
      {
        jtr_end = neighbors[*itr].end();
        for(jtr = neighbors[*itr].begin(); jtr != jtr_end; ++jtr)
          {
            if (get(vm,*itr) < get(vm,*jtr))
              {
                add_edge(contracted_vertex_map[*itr],
                         contracted_vertex_map[*jtr],
                         contracted_graph
                         );
              }
          }
      }

    if (target_graph == detail::tg_k_5)
      {
        return isomorphism(K_5,contracted_graph);
      }
    else //target_graph == tg_k_3_3
      {
        return isomorphism(K_3_3,contracted_graph);
      }

  }

  template <typename Graph, typename ForwardIterator>
  bool is_kuratowski_subgraph(const Graph& g,
                              ForwardIterator begin,
                              ForwardIterator end
                              )
  {
    return is_kuratowski_subgraph(g, begin, end, get(vertex_index,g));
  }

}

#endif //__IS_KURATOWSKI_SUBGRAPH_HPP__