#pragma once

#include <algorithm>

#include <Eigen/Core>
#include <Eigen/CXX11/Tensor>

#include "../piecewise_polynomial.hpp"
#include "spline.hpp"

namespace irlib {
    //template<typename T>
    //reutrn
    template<typename T> T const_pi();

    template<>
    inline mpfr::mpreal const_pi<mpfr::mpreal>() {
        return mpfr::const_pi();
    }

    template<>
    inline double const_pi<double>() {
        return M_PI;
    }

    inline std::complex<mpreal> my_exp(const mpreal& z_img) {
        return std::complex<mpreal>(
                mpfr::cos(z_img), mpfr::sin(z_img)
        );
    }

    template<typename mp_type>
    std::vector<std::pair<mp_type, mp_type> >
    composite_gauss_legendre_nodes(
            const std::vector<mp_type> &section_edges,
            const std::vector<std::pair<mp_type, mp_type> > &nodes
    ) {
        int num_sec = section_edges.size() - 1;
        int num_local_nodes = nodes.size();

        std::vector<std::pair<mp_type, mp_type> > all_nodes(num_sec * num_local_nodes);
        for (int s = 0; s < num_sec; ++s) {
            auto a = section_edges[s];
            auto b = section_edges[s + 1];
            for (int n = 0; n < num_local_nodes; ++n) {
                mp_type x = a + ((b - a) / mp_type(2)) * (nodes[n].first + mp_type(1));
                mp_type w = ((b - a) / mp_type(2)) * nodes[n].second;
                all_nodes[s * num_local_nodes + n] = std::make_pair(x, w);
            }
        }
        return all_nodes;
    };

    template<typename Tx, typename Ty, typename F>
    Ty integrate_gauss_legendre(const std::vector<Tx>& section_edges, const F& f, int num_local_nodes) {
        std::vector<std::pair<Tx, Tx>> nodes = detail::gauss_legendre_nodes<Tx>(num_local_nodes);
        auto nodes_x = composite_gauss_legendre_nodes(section_edges, nodes);
        Ty r = 0;
        for (int n=0; n<nodes_x.size(); ++n) {
            r += f(static_cast<Ty>(nodes_x[n].first)) * static_cast<Ty>(nodes_x[n].second);
        }
        return r;
    };

    /**
     * Compute Matrix representation of a given Kernel
     * @tparam Scalar
     * @tparam K
     * @param kernel
     * @param section_edges_x
     * @param section_edges_y
     * @param num_local_nodes
     * @param num_local_poly
     * @return Matrix representation
     */
    template<typename Scalar, typename K>
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
    matrix_rep(const K &kernel,
               const std::vector<mpreal> &section_edges_x,
               const std::vector<mpreal> &section_edges_y,
               int num_local_nodes,
               int num_local_poly) {

        using mpreal_matrix_type = Eigen::Matrix<mpreal, Eigen::Dynamic, Eigen::Dynamic>;
        using matrix_type = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

        int num_sec_x = section_edges_x.size() - 1;
        int num_sec_y = section_edges_y.size() - 1;

        // nodes for Gauss-Legendre integration
        std::vector<std::pair<mpreal, mpreal >> nodes = detail::gauss_legendre_nodes<mpreal>(num_local_nodes);
        auto nodes_x = composite_gauss_legendre_nodes(section_edges_x, nodes);
        auto nodes_y = composite_gauss_legendre_nodes(section_edges_y, nodes);

        std::vector<mpreal_matrix_type> phi_x(num_sec_x);
        for (int s = 0; s < num_sec_x; ++s) {
            phi_x[s] = mpreal_matrix_type(num_local_poly, num_local_nodes);
            for (int n = 0; n < num_local_nodes; ++n) {
                for (int l = 0; l < num_local_poly; ++l) {
                    auto leg_val = normalized_legendre_p(l, nodes[n].first);
                    phi_x[s](l, n) = detail::sqrt<mpreal>(mpreal(2) / (section_edges_x[s + 1] - section_edges_x[s])) * leg_val *
                                     nodes_x[s * num_local_nodes + n].second;
                }
            }
        }

        std::vector<mpreal_matrix_type> phi_y(num_sec_y);
        for (int s = 0; s < num_sec_y; ++s) {
            phi_y[s] = mpreal_matrix_type(num_local_poly, num_local_nodes);
            for (int n = 0; n < num_local_nodes; ++n) {
                for (int l = 0; l < num_local_poly; ++l) {
                    auto leg_val = normalized_legendre_p(l, nodes[n].first);
                    phi_y[s](l, n) = detail::sqrt<mpreal>(mpreal("2") / (section_edges_y[s + 1] - section_edges_y[s])) * leg_val *
                                     nodes_y[s * num_local_nodes + n].second;
                }
            }
        }

        matrix_type K_mat(num_sec_x * num_local_poly, num_sec_y * num_local_poly);
        for (int s2 = 0; s2 < num_sec_y; ++s2) {
            for (int s = 0; s < num_sec_x; ++s) {

                mpreal_matrix_type K_nn(num_local_nodes, num_local_nodes);
                for (int n = 0; n < num_local_nodes; ++n) {
                    for (int n2 = 0; n2 < num_local_nodes; ++n2) {
                        K_nn(n, n2) = kernel(static_cast<Scalar>(nodes_x[s * num_local_nodes + n].first),
                                             static_cast<Scalar>(nodes_y[s2 * num_local_nodes + n2].first)
                        );
                    }
                }

                // phi_x(l, n) * K_nn(n, n2) * phi_y(l2, n2)^T
                mpreal_matrix_type r = phi_x[s] * K_nn * phi_y[s2].transpose();

                for (int l2 = 0; l2 < num_local_poly; ++l2) {
                    for (int l = 0; l < num_local_poly; ++l) {
                        K_mat(num_local_poly * s + l, num_local_poly * s2 + l2) = static_cast<Scalar>(r(l, l2));
                    }
                }
            }
        }

        return K_mat;
    }

    /**
     * Estimate absolute errors in ulx and vly by computing the residual of the integral equation
     *   r(x) = u(x) - s^{-1} int_0^1 K(x,y) v(y) dy
     *   This returns an estimate of max_x |r(x)|
     * @tparam K
     * @param ux defined on [0, 1]
     * @param vy defined on [0, 1]
     * @param kernel a function of (x, y)
     * @return residual for ux
     */
    template<typename T, typename K>
    double estimate_residual(const piecewise_polynomial<T,T>& ux, const piecewise_polynomial<T,T>& vy, T s, const K& kernel, int num_local_nodes) {
        auto section_edges_x = ux.section_edges();
        auto section_edges_y = vy.section_edges();

        auto local_nodes = detail::gauss_legendre_nodes<mpfr::mpreal>(num_local_nodes);
        auto nodes_y = composite_gauss_legendre_nodes(section_edges_y, local_nodes);

        std::vector<T> sampling_points(section_edges_x);
        for (int i=0; i < section_edges_x.size()-1; ++i) {
            auto dx = section_edges_x[i+1]-section_edges_x[i];
            sampling_points.push_back(0.25*dx + section_edges_x[i]);
            sampling_points.push_back(0.50*dx + section_edges_x[i]);
            sampling_points.push_back(0.75*dx + section_edges_x[i]);
        }

        // Now we compute residual for u_l(x)
        double residual_x = 0.0;
        for (auto i = 0; i < section_edges_x.size()-1; ++i) {
            auto x = (section_edges_x[i+1] + section_edges_x[i])/2;
            mpfr::mpreal sum(0);
            for (int n=0; n < nodes_y.size(); ++n) {
                auto y = nodes_y[n].first;
                auto w = nodes_y[n].second;
                sum += w * kernel(x,y) * vy.compute_value(y);
            }
            auto diff = mpfr::abs(sum/s - ux.compute_value(x));
            residual_x = std::max(residual_x, static_cast<double>(diff));
        }

        return residual_x;
    }

    template<typename SVD>
    void check_SVD(const MatrixXmp& Kmat_even, const SVD& svd_even) {
        MatrixXmp S(svd_even.matrixU().cols(), svd_even.matrixV().cols());
        MatrixXmp invS(svd_even.matrixU().cols(), svd_even.matrixV().cols());
        S.setZero();
        invS.setZero();
        for (int l=0; l<svd_even.singularValues().rows(); ++l) {
            S(l,l) = svd_even.singularValues()[l];
            invS(l,l) = 1/svd_even.singularValues()[l];
        }
        for (int l=0; l<svd_even.matrixV().cols(); ++l) {
            MatrixXmp diff = (Kmat_even * svd_even.matrixV().col(l))/svd_even.singularValues()[l] - svd_even.matrixU().col(l);
            std::cout << "Residual of SVD at l = " << l << " " << diff.squaredNorm() << " " << svd_even.singularValues()[l] << std::endl;
        }
        /*
         * MatrixXmp diff = Kmat_even - svd_even.matrixU() * S * svd_even.matrixV().transpose();
        for (int l=0; l<std::min(diff.rows(), diff.cols()); ++l) {
            std::cout << "Residual of SVD at l = " << l << " " << diff(l,l) << std::endl;
        }
         */
    }

    /**
     *
     * @tparam ScalarType
     * @tparam KernelType
     * @r_int_eq absolute errors in ulx and vly estimated by the residual of integral equations. This estimate may be too big
     *    for very small singular values because the residual contains the inverse of singular values.
     */
    template<typename ScalarType, typename KernelType>
    std::tuple<
            std::vector<mpreal>,
            std::vector<piecewise_polynomial<mpreal,mpreal>>,
            std::vector<piecewise_polynomial<mpreal,mpreal>>
    >
    generate_ir_basis_functions_impl(
            const KernelType &kernel,
            int max_dim,
            double sv_cutoff,
            int num_local_poly,
            int num_nodes_gauss_legendre,
            const std::vector<mpreal> &section_edges_x,
            const std::vector<mpreal> &section_edges_y,
            std::vector<double> &residual_x,
            std::vector<double> &residual_y,
            std::pair<double,double>& r_int_eq,
            bool verbose
    ) throw(std::runtime_error) {
        using vector_t = Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>;
        using matrix_t = Eigen::Matrix<ScalarType, Eigen::Dynamic, Eigen::Dynamic>;
        using mp_vector_t = Eigen::Matrix<mpreal, Eigen::Dynamic, 1>;
        using mp_matrix_t = Eigen::Matrix<mpreal, Eigen::Dynamic, Eigen::Dynamic>;

        if (num_local_poly < 2) {
            throw std::runtime_error("num_local_poly < 2! : " + std::to_string(num_local_poly));
        }

        // Compute Kernel matrix and do SVD for even/odd sector
        if (verbose) {
            std::cout << "  Constructing kernel matrix for even sector ... "  << std::flush;
        }
        auto kernel_even = [&](const mpreal &x, const mpreal &y) { return kernel(x, y) + kernel(x, -y); };
        auto Kmat_even = irlib::matrix_rep<ScalarType>(
                kernel_even, section_edges_x, section_edges_y, num_nodes_gauss_legendre, num_local_poly
        );
        if (verbose) {
            std::cout << " done " << std::endl;
            std::cout << "  SVD kernel matrix for even sector ... " << std::flush;
        }
        Eigen::BDCSVD<matrix_t> svd_even(Kmat_even, Eigen::ComputeThinU | Eigen::ComputeThinV);
        if (verbose) {
            std::cout << " done " << std::endl;
            std::cout << "  Constructing kernel matrix for odd sector ... " << std::flush;
        }
        auto kernel_odd = [&](const mpreal &x, const mpreal &y) { return kernel(x, y) - kernel(x, -y); };
        auto Kmat_odd = irlib::matrix_rep<ScalarType>(
                kernel_odd, section_edges_x, section_edges_y, num_nodes_gauss_legendre, num_local_poly
        );
        if (verbose) {
            std::cout << " done " << std::endl;
            std::cout << "  SVD kernel matrix for odd sector ... " << std::flush;
        }
        Eigen::BDCSVD<matrix_t> svd_odd(Kmat_odd, Eigen::ComputeThinU | Eigen::ComputeThinV);
        if (verbose) {
            std::cout << " done " << std::endl;
        }

        // Pick up singular values and basis functions larger than cutoff
        std::vector<mpfr::mpreal> sv;
        std::vector<vector_t> Uvec, Vvec;
        auto s0 = svd_even.singularValues()[0];
        for (int i = 0; i < svd_even.singularValues().size(); ++i) {
            if (sv.size() == max_dim || svd_even.singularValues()[i] / s0 < sv_cutoff) {
                break;
            }
            sv.push_back(svd_even.singularValues()[i]);
            Uvec.push_back(svd_even.matrixU().col(i));
            Vvec.push_back(svd_even.matrixV().col(i));
            if (sv.size() == max_dim || svd_odd.singularValues()[i] / s0 < sv_cutoff) {
                break;
            }
            sv.push_back(svd_odd.singularValues()[i]);
            Uvec.push_back(svd_odd.matrixU().col(i));
            Vvec.push_back(svd_odd.matrixV().col(i));
        }
        assert(sv.size() <= max_dim);

        // Check if singular values are in decreasing order
        for (int l = 0; l < sv.size() - 1; ++l) {
            if (sv[l] < sv[l + 1]) {
                throw std::runtime_error(
                        "Singular values are not in decreasing order. This may be due to numerical round erros. You may ask for fewer basis functions!");
            }
        }

        // Construct basis functions
        std::vector<std::vector<mpreal>> deriv_xm1 = normalized_legendre_p_derivatives(num_local_poly, mpreal("-1"));
        std::vector<mpreal> inv_factorial;
        inv_factorial.push_back(mpreal(1));
        for (int l = 1; l < num_local_poly; ++l) {
            inv_factorial.push_back(inv_factorial.back() / mpreal(l));
        }

        auto gen_pp = [&](const std::vector<mpreal> &section_edges, const std::vector<vector_t> &vectors) {
            std::vector<piecewise_polynomial<mpreal,mpreal>> pp;

            int ns_pp = section_edges.size() - 1;
            for (int v = 0; v < vectors.size(); ++v) {
                Eigen::Matrix<mpreal,Eigen::Dynamic,Eigen::Dynamic> coeff(ns_pp, num_local_poly);
                coeff.setZero();
                int parity = v % 2 == 0 ? 1 : -1;
                // loop over sections in [0, 1]
                for (int s = 0; s < section_edges.size() - 1; ++s) {
                    // loop over normalized Legendre polynomials
                    for (int l = 0; l < num_local_poly; ++l) {
                        mpreal coeff2 = mpreal(1)/mpfr::sqrt(section_edges[s + 1] - section_edges[s]);
                        // loop over the orders of derivatives
                        for (int d = 0; d < num_local_poly; ++d) {
                            mpreal tmp = inv_factorial[d] * coeff2 * vectors[v][s * num_local_poly + l] * deriv_xm1[l][d];
                            coeff(s, d) += tmp;
                            coeff2 *= mpreal(2)/(section_edges[s + 1] - section_edges[s]);
                        }
                    }
                }
                pp.push_back(piecewise_polynomial<mpreal,mpreal>(section_edges.size() - 1, section_edges, coeff));
            }

            return pp;
        };

        auto u_basis_pp = gen_pp(section_edges_x, Uvec);
        auto v_basis_pp = gen_pp(section_edges_y, Vvec);

        for (int i = 0; i < u_basis_pp.size(); ++i) {
            if (u_basis_pp[i].compute_value(1) < 0) {
                u_basis_pp[i] = mpreal(-1.0) * u_basis_pp[i];
                v_basis_pp[i] = mpreal(-1.0) * v_basis_pp[i];
            }
        }

        if (u_basis_pp.size()%2 == 1) {
            r_int_eq.first = estimate_residual(u_basis_pp.back(), v_basis_pp.back(), sv.back(), kernel_even, num_nodes_gauss_legendre);
            auto k_yx = [&](mpreal y, mpreal x) {return kernel_even(x,y);};
            r_int_eq.second = estimate_residual(v_basis_pp.back(), u_basis_pp.back(), sv.back(), k_yx, num_nodes_gauss_legendre);
        } else {
            r_int_eq.first = estimate_residual(u_basis_pp.back(), v_basis_pp.back(), sv.back(), kernel_odd, num_nodes_gauss_legendre);
            auto k_yx = [&](mpreal y, mpreal x) {return kernel_odd(x,y);};
            r_int_eq.second = estimate_residual(v_basis_pp.back(), u_basis_pp.back(), sv.back(), k_yx, num_nodes_gauss_legendre);
        }

        residual_x.resize(section_edges_x.size() - 1);
        residual_y.resize(section_edges_y.size() - 1);
        //std::fill(residual_x.begin(), residual_x.end(), 0.0);
        ////std::fill(residual_y.begin(), residual_y.end(), 0.0);

        {
            auto l = Uvec.size() -1;
            for (int s = 0; s < residual_x.size(); ++s) {
                double dx = static_cast<double>(section_edges_x[s+1]-section_edges_x[s]);
                double a_diff = static_cast<double>(Uvec[l](s * num_local_poly + num_local_poly - 1)) * std::sqrt((2.*l+1)/dx);
                residual_x[s] = std::abs(a_diff);
            }
            for (int s = 0; s < residual_y.size(); ++s) {
                double dy = static_cast<double>(section_edges_y[s+1]-section_edges_y[s]);
                double a_diff = static_cast<double>(Vvec[l](s * num_local_poly + num_local_poly - 1)) * std::sqrt((2.*l+1)/dy);
                residual_y[s] = std::abs(a_diff);
            }
        }

        return std::make_tuple(sv, u_basis_pp, v_basis_pp);
    }

    template<typename ScalarType, typename KernelType>
    std::tuple<
            std::vector<mpreal>,
            std::vector<piecewise_polynomial<mpreal,mpreal>>,
            std::vector<piecewise_polynomial<mpreal,mpreal>>
    >
    generate_ir_basis_functions(
            const KernelType &kernel,
            int max_dim,
            double sv_cutoff = 1e-12,
            bool verbose = false,
            double r_tol = 1e-6,
            int num_local_poly = 10,
            int num_nodes_gauss_legendre = 24
    ) throw(std::runtime_error) {
        using vector_t = Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>;
        // Compute approximate positions of nodes of the highest basis function in the even sector
        std::vector<double> nodes_x, nodes_y;
        if (verbose){
            std::cout << "Computing approximate positions of zeros... ";
            std::tie(nodes_x, nodes_y) = compute_approximate_nodes_even_sector(kernel, 500, std::max(1e-12, sv_cutoff));
            std::cout << "Done" << std::endl;
        }

        auto gen_section_edges = [](const std::vector<double> &nodes) {
            std::vector<mpreal> section_edges;
            section_edges.push_back(0);
            for (int i = 0; i < nodes.size(); ++i) {
                section_edges.push_back(static_cast<mpreal>(nodes[i]));
            }
            section_edges.push_back(1);
            return section_edges;
        };

        auto u = [](const std::vector<mpreal> &section_edges,
                    std::vector<double> &residual, double eps) {
            std::vector<mpreal> section_edges_new(section_edges);
            for (int s = 0; s < section_edges.size() - 1; ++s) {
                if (residual[s] > eps) {
                    section_edges_new.push_back(
                            (section_edges[s] + section_edges[s + 1]) / 2
                    );
                }
            }
            std::sort(section_edges_new.begin(), section_edges_new.end());
            return section_edges_new;
        };

        std::vector<mpreal> section_edges_x = gen_section_edges(nodes_x);
        std::vector<mpreal> section_edges_y = gen_section_edges(nodes_y);

        int ite = 0;

        // Sections are split recursively until convergence is reached.
        while (true) {
            if (verbose) {
                std::cout << "Iteration " << ite+1 << " : " << section_edges_x.size()-1 << " sections for x, " << section_edges_y.size()-1 << " sections for y." << std::endl;
            }
            std::vector<double> residual_x, residual_y;
            std::pair<double,double> r_int_eq;
            auto r = generate_ir_basis_functions_impl<ScalarType>(kernel, max_dim, sv_cutoff, num_local_poly, num_nodes_gauss_legendre,
                    section_edges_x,
                    section_edges_y,
                    residual_x,
                    residual_y,
                    r_int_eq,
                    verbose
            );
            int ns = section_edges_x.size() + section_edges_y.size();

            int dim = std::get<1>(r).size();

            auto a_tol_x = r_tol * std::abs(
                    static_cast<double>(std::get<1>(r).back().compute_value(1))
            );
            auto a_tol_y = r_tol * std::max(
                    std::abs(static_cast<double>(std::get<2>(r)[2*(dim/2)-1].compute_value(1))),
                    std::abs(static_cast<double>(std::get<2>(r)[2*(dim/2)-1].compute_value(0)))
            );

            section_edges_x = u(section_edges_x, residual_x, a_tol_x);
            section_edges_y = u(section_edges_y, residual_y, a_tol_y);

            if (verbose) {
                std::cout << "Iteration " << ite+1 << " : found " << std::get<1>(r).size() <<  " basis functions. " << std::endl;
                std::cout << "Iteration " << ite+1 << " : max_x |u_l(x) - s_l^{-1} dy int_{-1}^1 K(x,y) v_l(y)| = " << r_int_eq.first << " for largest l." << std::endl;
                std::cout << "Iteration " << ite+1 << " : max_y |v_l(y) - s_l^{-1} dx int_{-1}^1 K(x,y) u_l(x)| = " << r_int_eq.second << " for largest l." << std::endl;
                std::cout << "Iteration " << ite+1 << " : residual estimated by expansion coefficients for x = " << *std::max_element(residual_x.begin(),residual_x.end()) << std::endl;
                std::cout << "Iteration " << ite+1 << " : residual estimated by expansion coefficients for y = " << *std::max_element(residual_y.begin(),residual_y.end()) << std::endl;
            }

            if (section_edges_x.size() + section_edges_y.size() == ns) {
                return r;
            }

            ite += 1;
        }

    };

    template<typename T>
    inline std::vector<T> linspace(T minval, T maxval, int N, bool include_last_point = true) {
        int end = include_last_point ? N : N-1;
        std::vector<T> r(end);
        for (int i = 0; i < end; ++i) {
            r[i] = i * (maxval - minval) / (N - T(1)) + minval;
        }
        return r;
    }

    template<typename T>
    piecewise_polynomial<T,T> construct_piecewise_polynomial_cspline(
            const std::vector<T> &x_array, const std::vector<T> &y_array) {
        const int n_points = x_array.size();
        const int n_section = n_points - 1;

        Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> coeff(n_section, 4);

        // Cubic spline interpolation
        tk::spline<T> spline;
        spline.set_points(x_array, y_array);

        // Construct piecewise_polynomial
        for (int s = 0; s < n_section; ++s) {
            for (int p = 0; p < 4; ++p) {
                coeff(s, p) = spline.get_coeff(s, p);
            }
        }
        return piecewise_polynomial<T,T>(n_section, x_array, coeff);
    };

    /**
     * Find all zeros of a piecewise polynomial defined on (0,1)
     * @tparam T
     * @tparam Tx
     * @param p
     * @param delta tolerance
     * @return
     */
    template<typename T, typename Tx>
    std::vector<Tx> find_zeros(
            const piecewise_polynomial<T,Tx>& p,
            Tx delta
    ) {
        int N = 10000;
        Tx de_cutoff = 3.0;

        std::vector<Tx> tx_vec = linspace<Tx>(0.0, de_cutoff, N);
        std::vector<Tx> x_vec(N), zeros;
        for (int i = 0; i < N; ++i) {
            x_vec[i] = tanh(0.5 * const_pi<Tx>() * sinh(tx_vec[i]));
        }

        for (int i = 0; i < N-1; ++i) {
            if (p.compute_value(x_vec[i]) * p.compute_value(x_vec[i+1]) < 0) {
                Tx x_left = x_vec[i];
                T p_left = p.compute_value(x_vec[i]);

                Tx x_right = x_vec[i+1];
                T p_right = p.compute_value(x_vec[i+1]);

                while (x_right-x_left > delta) {
                    Tx x_mid = (x_left+x_right)/2;
                    if (p_left * p.compute_value(x_mid) > 0) {
                        x_left = x_mid;
                    } else {
                        x_right = x_mid;
                    }
                }
                zeros.push_back((x_left+x_right)/2);
            }
        }

        return zeros;
    };

    template<typename T, typename Tx>
    std::vector<piecewise_polynomial<T,Tx> > cspline_approximation(
            const std::vector<piecewise_polynomial<T,Tx> >& basis_vectors,
            double r_tol
    ) {
        std::vector<piecewise_polynomial<T,Tx> > basis_vectors_cspline;

        // Determine sampling points
        std::set<Tx> x_set{0.0, 1.0};
        int largest_even_l = 2*((basis_vectors.size()-1)/2);
        std::vector<Tx> zeros = find_zeros(basis_vectors[largest_even_l], static_cast<Tx>(1e-10));
        std::copy(zeros.begin(), zeros.end(), std::inserter(x_set, x_set.begin()));
        std::vector<Tx> x(x_set.size());
        std::vector<Tx> y(x_set.size());
        while (true) {
            // construct cubic spline interpolation of the highest-l basis function
            int index = 0;
            x.resize(x_set.size());
            y.resize(x_set.size());
            for (auto it=x_set.begin(); it != x_set.end(); ++it) {
                x[index] = *it;
                y[index] = basis_vectors.back().compute_value(*it);
                ++ index;
            }
            auto cspline = construct_piecewise_polynomial_cspline<T>(x, y);

            // check convergence
            for (int i=0; i < x.size()-1; ++i) {
                auto x_mid = (x[i]+x[i+1])/2;
                auto diff = std::abs(static_cast<double>(
                                             (cspline.compute_value(x_mid)-basis_vectors.back().compute_value(x_mid))/
                                                     basis_vectors.back().compute_value(1)
                                     ));
                if (diff > r_tol) {
                    //std::cout << "add " << x_mid << " " << diff << std::endl;
                    x_set.insert(x_mid);
                }
            }

            if (x_set.size() == x.size()) {
                break;
            }
        }

        int Nx = x.size();

        // construct cspline approximation one by one
        for (int l=0; l < basis_vectors.size(); ++l) {
            for (int ix=0; ix<Nx; ++ix) {
                //std::cout << " final x " << std::setprecision(20) << x[ix] << std::endl;
                y[ix] = basis_vectors[l].compute_value(x[ix]);
            }
            basis_vectors_cspline.push_back(construct_piecewise_polynomial_cspline<T>(x, y));
        }

        return basis_vectors_cspline;
    };

    //Compute nodes (zeros) of Legendre polynomials
    inline std::vector<double> compute_legendre_nodes(int l) {
        double eps = 1e-10;
        if (l > 200) {
            throw std::runtime_error("l > 200 in compute_legendre_nodes");
        }

        std::vector<double> nodes;

        auto leg_diff = [](int l, double x) {
            return l * (x * legendre_p(l, x) - legendre_p(l - 1, x)) / (x * x - 1);
        };

        //i-th zero
        for (int i = 0; i < l / 2; i++) {
            //initial guess
            double x = std::cos(M_PI * (i + 1 - 0.25) / (l + 0.5));

            //Newton-Raphson iteration
            while (true) {
                double leg = legendre_p(l, x);
                double x_new = x - 0.1 * leg / leg_diff(l, x);
                if (std::abs(x_new - x) < eps && std::abs(leg) < eps) {
                    break;
                }
                x = x_new;
            }

            nodes.push_back(x);
            nodes.push_back(-x);
        }

        if (l % 2 == 1) {
            nodes.push_back(0.0);
        }

        std::sort(nodes.begin(), nodes.end());

        return nodes;
    }

    //AVOID USING BOOST_TYPEOF
    template<class T1, class T2>
    struct result_of_overlap {
        typedef std::complex<double> value;
    };

    template<>
    struct result_of_overlap<double, double> {
        typedef double value;
    };

    /// Construct piecewise polynomials representing exponential functions: exp(i w_i x)
    template<class T, typename Tx>
    void construct_exp_functions_coeff(
            const std::vector<T> &w,
            const std::vector<Tx> &section_edges,
            int k,
            Eigen::Tensor<std::complex<T>, 3> &coeffs) {
        const int N = section_edges.size() - 1;

        std::complex<T> z;
        coeffs = Eigen::Tensor<std::complex<T>,3>(w.size(), N, k + 1);

        std::vector<T> pre_factor(k + 1);
        pre_factor[0] = 1.0;
        for (int j = 1; j < k + 1; ++j) {
            pre_factor[j] = pre_factor[j - 1] / j;
        }

        for (int n = 0; n < w.size(); ++n) {
            auto z = std::complex<T>(0.0, w[n]);
            for (int section = 0; section < N; ++section) {
                auto x = section_edges[section];
                std::complex<T> exp0 = std::exp(z * (x + 1));
                std::complex<T> z_power = T(1);
                for (int j = 0; j < k + 1; ++j) {
                    coeffs(n, section, j) = exp0 * z_power * pre_factor[j];
                    z_power *= z;
                }
            }
        }
    }

/**
 *  Compute \int_x0^x1 dx exp(i w (x+1)) (x-x0)^k
 *      for k=0, 1, ..., K+1. x1 = x0+dx.
**/
    template<typename T>
    void compute_Ik(T x0, T dx, T w, int K, std::vector<std::complex<T> >& Ik) {
        auto x1 = x0 + dx;
        auto iw = std::complex<T>(0.0, w);
        auto exp0 = exp(iw*(x0+1));
        auto exp1 = exp(iw*(x1+1));
        auto exp10 = exp(iw*(x1-x0));
        Ik[0] = (exp1 - exp0)/iw;
        //Ik[0] = (exp10 - 1) * exp0 /iw;

        auto dx_k = dx;
        for (int k=1; k<K+1; ++k) {
            Ik[k] = (dx_k * exp1 - static_cast<T>(k) * Ik[k-1])/iw;
            dx_k *= dx;
        }
    }

/**
 * Compute integral of exponential functions and given piecewise polynomials
 *           \int dx exp(i w_i (x+1)) p_j(x),
 *           where w_i are given real double objects and p_j are piecewise polynomials.
 * @tparam T  scalar type of piecewise polynomials
 * @param w vector of w_i in ascending order
 * @param statis Statistics (fermion or boson)
 * @param p vector of piecewise polynomials.
 * @param results  computed results
 */
    template<typename T, typename Tx>
    void compute_integral_with_exp(
            const std::vector<T> &w,
            const std::vector<piecewise_polynomial<T,Tx> > &pp_func,
            Eigen::Tensor<std::complex<T>, 2> &Tnl
    ) {
        typedef std::complex<T> dcomplex;
        typedef piecewise_polynomial<std::complex<T>,Tx> pp_type;
        typedef Eigen::Matrix<std::complex<T>, Eigen::Dynamic, Eigen::Dynamic> ex_matrix_t;
        typedef Eigen::Tensor<std::complex<T>, 2> tensor_t;

        T pi = const_pi<T>();

        //order of polynomials used for representing exponential functions internally.
        const int k_iw = 16;//for debug
        const int k = pp_func[0].order();
        const int n_section = pp_func[0].num_sections();

        std::vector<std::complex<T> > Ik(k+1);

        for (int l = 0; l < pp_func.size(); ++l) {
            if (k != pp_func[l].order()) {
                throw std::runtime_error(
                        "Error in compute_transformation_matrix_to_matsubara: basis functions must be pieacewise polynomials of the same order");
            }
            if (pp_func[l].section_edge(0) != 0 || pp_func[l].section_edge(n_section) != 1) {
                throw std::runtime_error("Piecewise polynomials must be defined on [0,1]");
            }
        }

        const int n_iw = w.size();
        const int n_max = n_iw - 1;

        for (int i = 0; i < w.size() - 1; ++i) {
            if (w[i] > w[i + 1]) {
                throw std::runtime_error("w must be give in ascending order.");
            }
        }

        //Use Taylor expansion for exp(i w_n tau) for w_n*dx < cutoff*M_PI
        const double cutoff = 0.1;

        Eigen::Tensor<std::complex<T>,3> exp_coeffs(w.size(), n_section, k_iw + 1);
        construct_exp_functions_coeff(w, pp_func[0].section_edges(), k_iw, exp_coeffs);

        ex_matrix_t left_mid_matrix(n_iw, k + 1);
        ex_matrix_t left_matrix(n_iw, k_iw + 1);
        ex_matrix_t mid_matrix(k_iw + 1, k + 1);
        ex_matrix_t right_matrix(k + 1, pp_func.size());
        ex_matrix_t r(n_iw, pp_func.size());
        r.setZero();

        std::vector<T> dx_power(k + k_iw + 2);

        for (int s = 0; s < n_section; ++s) {
            auto x0 = pp_func[0].section_edge(s);
            T dx = static_cast<T>(pp_func[0].section_edge(s + 1) - pp_func[0].section_edge(s));
            left_mid_matrix.setZero();

            dx_power[0] = 1.0;
            for (int p = 1; p < dx_power.size(); ++p) {
                dx_power[p] = dx * dx_power[p - 1];
            }

            //Use Taylor expansion for exp(i w_n tau) for w_n*dx < cutoff*M_PI
            auto w_max_cs = cutoff * pi / dx;
            int n_max_cs = -1;
            for (int i = 0; i < w.size(); ++i) {
                if (w[i] <= w_max_cs) {
                    n_max_cs = i;
                }
            }

            //Use Taylor expansion
            if (n_max_cs >= 0) {
                for (int p = 0; p < k_iw + 1; ++p) {
                    for (int p2 = 0; p2 < k + 1; ++p2) {
                        mid_matrix(p, p2) = dx_power[p + p2 + 1] / (p + p2 + 1.0);
                    }
                }

                for (int n = 0; n < n_max_cs + 1; ++n) {
                    for (int p = 0; p < k_iw + 1; ++p) {
                        left_matrix(n, p) = exp_coeffs(n,s,p);
                    }
                }

                left_mid_matrix.block(0, 0, n_max_cs + 1, k + 1) =
                        left_matrix.block(0, 0, n_max_cs + 1, k_iw + 1) * mid_matrix;
            }

            //Otherwise, compute the overlap exactly
            for (int n = std::max(n_max_cs + 1, 0); n <= n_max; ++n) {
                compute_Ik(x0, dx, w[n], k, Ik);
                for (int i=0; i<k+1; ++i) {
                    left_mid_matrix(n, i) = Ik[i];
                }
            }

            for (int l = 0; l < pp_func.size(); ++l) {
                for (int p2 = 0; p2 < k + 1; ++p2) {
                    right_matrix(p2, l) = static_cast<T>(pp_func[l].coefficient(s, p2));
                }
            }

            r += left_mid_matrix * right_matrix;
        }

        Tnl = tensor_t(n_iw, pp_func.size());
        for (int n = 0; n < n_iw; ++n) {
            for (int l = 0; l < pp_func.size(); ++l) {
                Tnl(n, l) = r(n, l);
            }
        }
    }

    /**
    * Compute a transformation matrix from a give orthogonal basis set to Matsubara freq.
    * @tparam T  scalar type
    * @tparam Tx  scalar type for x in [-1,1]
    * @param n_vec indices of Matsubara frequqneices for which matrix elements will be computed (in strictly ascending order).
    *          The Matsubara basis functions look like exp(i PI * (n[i]+1/2)) for fermions, exp(i PI * n[i]) for bosons.
    * @param bf_src orthogonal basis functions on [-1,1]. They must be piecewise polynomials of the same order. Piecewise polynomial representations on [0,1] must be provided.
    *               Basis functions u_l(x) are assumed to be even or odd for even l and odd l, respectively.
    * @param Tnl  computed transformation matrix, results are cast into double
    */
    template<typename T, typename Tx>
    void compute_transformation_matrix_to_matsubara(
            const std::vector<long> &n_vec,
            irlib::statistics::statistics_type statis,
            const std::vector<piecewise_polynomial<T,Tx> > &bf_src,
            Eigen::Tensor<std::complex<double>, 2> &Tnl
    ) {
        typedef std::complex<double> dcomplex;
        typedef Eigen::Matrix<std::complex<T>, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
        typedef Eigen::Tensor<std::complex<T>, 2> tensor_t;

        int Nl = bf_src.size();
        auto pi = const_pi<T>();

        if (n_vec.size() == 0) {
            return;
        }

        for (int i = 0; i < n_vec.size() - 1; ++i) {
            if (n_vec[i] > n_vec[i + 1]) {
                throw std::runtime_error("n_vec must be in strictly ascending order!");
            }
        }

        if (n_vec[0] < 0) {
            throw std::runtime_error("n_vec cannot be negative!");
        }

        long offset = (statis == statistics::FERMIONIC ? 1 : 0);

        // compute tails
        int sign_s = (statis == statistics::FERMIONIC ? -1 : 1);
        // even number close to (bf_src[0].order()/2
        int num_tail = std::min(2*(bf_src[0].order()/2), 4);
        if (num_tail < 4) {
            throw std::runtime_error("num_tail < 4.");
        }
        Eigen::Matrix<std::complex<T>,Eigen::Dynamic,Eigen::Dynamic> tails(bf_src.size(), num_tail);
        const std::complex<T> zi(0.0, 1.0);
        for (int l=0; l<bf_src.size(); ++l) {
            auto ztmp = zi;
            for (int m=0; m<num_tail; ++m) {
                int sign_lm = (l+m)%2==0 ? 1 : -1;
                tails(l,m) = - sqrt(T(2.0)) * pow(T(2), m) * ztmp * static_cast<T>((sign_s - sign_lm) * bf_src[l].derivative(1, m));
                ztmp *= zi;
            }
        }

        // Determine for which Matsubara frequencies tail is used
        // Store those indices in ovec
        std::vector<int> num_low_freq(Nl);
        double eps = 1e-8;
        for (int l=0; l<Nl; ++l) {
            int m_low = (l+offset-1)%2==0 ?  0 : 1;
            int m_high = (l+offset-1)%2==0 ?  num_tail-2 : num_tail-1;
            auto wn_limit = pow(eps * abs(tails(l,m_low)/tails(l,m_high)), 1.0/(m_low-m_high) );
            auto n_limit = 0.5*(wn_limit/pi-offset);

            num_low_freq[l] = std::count_if(n_vec.begin(), n_vec.end(), [&](long n){return n < n_limit;});
        }
        auto max_num_low_freq = *std::max_element(num_low_freq.begin(), num_low_freq.end());
        auto last = n_vec.begin();
        std::advance(last, max_num_low_freq);
        std::vector<long> ovec;
        std::transform(n_vec.begin(), last, std::back_inserter(ovec), [&](long n){return 2*n+offset;});

        // Compute Tnl
        Eigen::Tensor<std::complex<T>, 2> Tnl_low_freq;
        compute_Tbar_ol(ovec, bf_src, Tnl_low_freq);
        Tnl = Eigen::Tensor<std::complex<double>,2>(n_vec.size(), bf_src.size());
        Tnl.setZero();
        for(int l=0; l<Nl; ++l) {
            for(int i=0; i<max_num_low_freq; ++i) {
                Tnl(i,l) = to_dcomplex(Tnl_low_freq(i,l));
            }
        }

        // Relace with tail
        for (int l=0; l<Nl; ++l) {
            for (int i=num_low_freq[l]; i<n_vec.size(); ++i) {
                double wn = (2*n_vec[i]+offset) * M_PI;
                Tnl(i, l) = 0.0;
                for (int m=0; m<num_tail; ++m) {
                    Tnl(i, l) += to_dcomplex(tails(l,m)/pow(wn, m+1));
                }
            }
        }

    }

    /**
    * Compute a transformation matrix (\bar{T}_{nl}) from a give orthogonal basis set to Matsubara freq.
    * @tparam T  scalar type
    * @tparam Tx  scalar type for x \in [-1, 1]
    * @param n indices of Matsubara frequqneices for which matrix elements will be computed (in strictly ascending order).
    *          The Matsubara basis functions look like exp(i PI * (n[i]/2) * (x+1)).
    * @param bf_src orthogonal basis functions on [-1,1]. They must be piecewise polynomials of the same order. Piecewise polynomial representations on [0,1] must be provided.
    *               Basis functions u_l(x) are assumed to be even or odd for even l and odd l, respectively.
    * @param Tnl  computed transformation matrix
    */
    template<typename T, typename Tx>
    void compute_Tbar_ol(
            const std::vector<long> &o_vec,
            const std::vector<irlib::piecewise_polynomial<T,Tx>> &bf_src,
            Eigen::Tensor<std::complex<T>, 2> &Tbar_ol
    ) {
        typedef std::complex<T> dcomplex;
        typedef Eigen::Matrix<std::complex<T>, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
        typedef Eigen::Tensor<std::complex<T>, 2> tensor_t;

        if (o_vec.size() == 0) {
            return;
        }

        for (int i = 0; i < o_vec.size() - 1; ++i) {
            if (o_vec[i] > o_vec[i + 1]) {
                throw std::runtime_error("o must be given in strictly ascending order!");
            }
        }

        std::vector<T> w;
        std::transform(o_vec.begin(), o_vec.end(), std::back_inserter(w), [](long o) { return 0.5 * M_PI * o; });

        compute_integral_with_exp(w, bf_src, Tbar_ol);

        for (int l=0; l<bf_src.size(); ++l) {
            for (int i=0; i<o_vec.size(); ++i) {
                if ( (l+o_vec[i])%2 == 0) {
                    Tbar_ol(i,l) = 2 * Tbar_ol(i,l).real();
                } else {
                    Tbar_ol(i,l) = std::complex<T>(0.0, 2 * Tbar_ol(i,l).imag());
                }
            }
        }

        std::vector<T> inv_norm(bf_src.size());
        for (int l = 0; l < bf_src.size(); ++l) {
            inv_norm[l] = 1. / sqrt(2*bf_src[l].overlap(bf_src[l]));
        }
        for (int n = 0; n < w.size(); ++n) {
            for (int l = 0; l < bf_src.size(); ++l) {
                Tbar_ol(n, l) *= inv_norm[l] * sqrt(static_cast<T>(0.5));
            }
        }
    }

    /**
     * Find approximate positions of nodes for the even singular vectors with the lowest singular value larger than a cutoff
     * @tparam Kernel kernel type
     * @param knl Kernel object
     * @param N Number of points for discretizing the kernel
     * @param cutoff_singular_values smallest relative singular value
     * @return positions of nodes
     */
    template<typename Kernel>
    std::pair<std::vector<double>,std::vector<double>>
    compute_approximate_nodes_even_sector(const Kernel &knl, int N, double cutoff_singular_values) {
        typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_t;

        double de_cutoff = 2.5;

        //DE mesh for x
        std::vector<double> tx_vec = linspace<double>(0.0, de_cutoff, N);
        std::vector<double> weight_x(N), x_vec(N);
        for (int i = 0; i < N; ++i) {
            x_vec[i] = std::tanh(0.5 * M_PI * std::sinh(tx_vec[i]));
            //sqrt of the weight of DE formula
            weight_x[i] =
                    std::sqrt(0.5 * M_PI * std::cosh(tx_vec[i])) / std::cosh(0.5 * M_PI * std::sinh(tx_vec[i]));
        }

        //DE mesh for y
        std::vector<double> ty_vec = linspace<double>(-de_cutoff, 0.0, N);
        std::vector<double> y_vec(N), weight_y(N);
        for (int i = 0; i < N; ++i) {
            y_vec[i] = std::tanh(0.5 * M_PI * std::sinh(ty_vec[i])) + 1.0;
            //sqrt of the weight of DE formula
            weight_y[i] =
                    std::sqrt(0.5 * M_PI * std::cosh(ty_vec[i])) / std::cosh(0.5 * M_PI * std::sinh(ty_vec[i]));
        }

        matrix_t K(N, N);
        for (int i = 0; i < N; ++i) {
            for (int j = 0; j < N; ++j) {
                K(i, j) = weight_x[i] * static_cast<double>(knl(x_vec[i], y_vec[j]) + knl(x_vec[i], -y_vec[j])) *
                          weight_y[j];
            }
        }

        //Perform SVD
        Eigen::BDCSVD<matrix_t> svd(K, Eigen::ComputeFullU | Eigen::ComputeFullV);
        const Eigen::VectorXd& svalues = svd.singularValues();
        const matrix_t& U = svd.matrixU();
        const matrix_t& V = svd.matrixV();

        //Count non-zero SV
        int dim = N;
        for (int i = 1; i < N; ++i) {
            if (std::abs(svalues(i) / svalues(0)) < cutoff_singular_values) {
                dim = i;
                break;
            }
        }

        //find nodes
        std::vector<double> nodes_x, nodes_y;
        for (int i = 0; i < N - 1; ++i) {
            if (U(i, dim - 1) * U(i + 1, dim - 1) < 0.0) {
                nodes_x.push_back(0.5 * (x_vec[i] + x_vec[i + 1]));
            }
            if (V(i, dim - 1) * V(i+1, dim - 1) < 0.0) {
                nodes_y.push_back(0.5 * (y_vec[i] + y_vec[i + 1]));
            }
        }

        if (nodes_x.size() != dim - 1 || nodes_y.size() != dim - 1) {
            std::cerr << "The number of nodes for x is " << nodes_x.size() << " , which is different from l " << dim-1 << std::endl;
            std::cerr << "The number of nodes for y is " << nodes_y.size() << " , which is different from l " << dim-1 << std::endl;
            for (auto n : nodes_y) {
                std::cout << n << std::endl;
            }
            throw std::runtime_error("The number of nodes is wrong.");
        }

        return std::make_pair(nodes_x, nodes_y);
    }

/**
 *  Compute \int_{-1}^1 dx exp(i w x) p(x)
**/
/*
    inline std::vector<std::complex<mpfr::mpreal>>
    compute_Tnl_impl(const std::vector<piecewise_polynomial<mpreal,mpreal>>& p,
                                                       std::vector<mpreal>& w_vec,
                                                       int digits) {
        auto prec_bak = mpfr::mpreal::get_default_prec();

        mpfr::mpreal::set_default_prec(mpfr::digits2bits(digits));

        int num_local_nodes = 24;
        auto local_nodes = detail::gauss_legendre_nodes<mpreal>(num_local_nodes);
        std::vector<mpreal> section_edges = p.section_edges();
        auto global_nodes = composite_gauss_legendre_nodes(section_edges, local_nodes);
        auto n_local_nodes = local_nodes.size();

        std::vector<mpreal> values(global_nodes.size());
        for (int n=0; n<global_nodes.(); ++n) {
            values[n] = p.compute_value(global_nodes[n].first);
        }

        auto num_sections = p.num_sections();
        auto n_poly = p.order()+1;
        std::vector<mpreal> deriv_values_left(num_sections*n_poly), deriv_values_left(num_sections*n_poly);
        for (int s=0; s<num_sections; s++) {
            for (int k=0; k<n_poly; ++k) {
                deriv_values_left[k + s*n_poly] = p.derivative(p.section_edge(s), k, s);
                deriv_values_right[k + s*n_poly] = p.derivative(p.section_edge(s+1), k, s);
            }
        }

        std::vector<std::complex<mpreal>> result(w_vec.size());
        for (int i=0; i < w_vec.size(); ++i) {
            result[i] = 0;

            for (int s = 0; s < p.num_sections(); ++s) {
                mpreal x0 = p.section_edge(s);
                mpreal x1 = p.section_edge(s+1);

                if (w * (x1-x0) < 0.1 * const_pi<mpreal>()){
                    std::complex<mpreal> tmp(0);
                    for (int n = 0; n < n_local_nodes; ++n) {
                        tmp += values(s*n_local_nodes + n) * my_exp(w*x_smpl) * global_nodes[s*n_local_nodes + n].second;
                    }
                    result[i] += tmp;
                } else {
                    std::complex<mpreal> Jk(0, 0);
                    std::complex<mpreal> iw(0, w);
                    std::complex<mpreal> exp0 = my_exp(w*x0);
                    std::complex<mpreal> exp_tmp = my_exp(w*(x1-x0));

                    //p contains x^0, x^1, xj^2, ..., x^K (K = p.order())
                    for (int k=p.order(); k >= 0; --k) {
                        mpreal f0 = deriv_values_left[k + s*n_poly];
                        mpreal f1 = deriv_values_right[k + s*n_poly];
                        Jk = ((exp_tmp * f1 - f0) * exp0 - Jk)/iw;
                    }
                    result[i] += Jk;
                }
            }

            if (even) {
                result[i] = mpfr::sqrt(2) * result[i].real() * my_exp(w);
            } else {
                result[i] = mpfr::sqrt(2) * std::complex<mpreal>(0, result[i].imag()) * my_exp(w);
            }
        }

        mpfr::mpreal::set_default_prec(prec_bak);

        return result;
    }
    */

    inline std::complex<mpreal> compute_Tnl_tail(const piecewise_polynomial<mpreal,mpreal>& p,
                                   mpreal w,
                                   bool l_even,
                                   irlib::statistics::statistics_type s,
                                   int num_deriv) {
        int sign_s = (s == irlib::statistics::BOSONIC ? 1 : -1);

        if (w == 0.0) {
            throw std::runtime_error("Error zero frequency");
        }

        std::complex<mpreal> result(0);
        std::complex<mpreal> fact(0, 1/w);
        std::complex<mpreal> coeff(fact);
        int sign_l = l_even ? 1 : -1;
        for (int m = 0; m < num_deriv; ++m) {
            int sign_m = m%2 == 0 ? 1 : -1;
            int sign_lm = sign_l * sign_m;
            result += -static_cast<mpreal>(sign_s) * coeff * static_cast<mpreal>(1 - sign_s * sign_lm) * p.derivative(1, m);
            coeff *= fact;
        }

        return result/mpfr::sqrt(2);
    }

/**
 *  Compute \int_{-1}^1 dx exp(i w x) p(x)
**/
    inline std::complex<mpfr::mpreal> compute_Tnl_impl(const piecewise_polynomial<mpreal,mpreal>& p, bool even,
                                                       irlib::statistics::statistics_type s, mpreal w,
                                                       int digits_A = 30, int digits_B = 30) {
        auto prec_bak = mpfr::mpreal::get_default_prec();

        int num_local_nodes = 24;
        auto local_nodes = detail::gauss_legendre_nodes<mpreal>(num_local_nodes);
        std::vector<mpreal> section_edges = p.section_edges();
        auto global_nodes = composite_gauss_legendre_nodes(section_edges, local_nodes);
        auto n_local_nodes = local_nodes.size();

        std::complex<mpreal> result(0);
        for (int s = 0; s < p.num_sections(); ++s) {
            mpreal x0 = p.section_edge(s);
            mpreal x1 = p.section_edge(s+1);

            if (w * (x1-x0) < 0.1 * const_pi<mpreal>()){
                // using low-frequency formula (Gauss-Legendre quadrature)
                mpfr::mpreal::set_default_prec(mpfr::digits2bits(digits_A));
                std::complex<mpreal> tmp(0);
                for (int n = 0; n < n_local_nodes; ++n) {
                    auto x_smpl = global_nodes[s*n_local_nodes + n].first;
                    tmp += p.compute_value(x_smpl) * my_exp(w*x_smpl) * global_nodes[s*n_local_nodes + n].second;
                }
                result += tmp;
            } else {
                // using low-frequency formula (Gauss-Legendre quadrature)
                mpfr::mpreal::set_default_prec(mpfr::digits2bits(digits_B));
                std::complex<mpreal> Jk(0, 0);
                std::complex<mpreal> iw(0, w);
                std::complex<mpreal> exp0 = my_exp(w*x0);
                std::complex<mpreal> exp_tmp = my_exp(w*(x1-x0));

                //p contains x^0, x^1, xj^2, ..., x^K (K = p.order())
                for (int k=p.order(); k >= 0; --k) {
                    mpreal f0 = p.derivative(x0, k, s);
                    mpreal f1 = p.derivative(x1, k, s);
                    Jk = ((exp_tmp * f1 - f0) * exp0 - Jk)/iw;
                }
                result += Jk;
            }
        }

        if (even) {
            result = mpfr::sqrt(2) * result.real() * my_exp(w);
        } else {
            result = mpfr::sqrt(2) * std::complex<mpreal>(0, result.imag()) * my_exp(w);
        }

        // replace with tail
        if (w != 0.0) {
            int num_deriv = p.order()+1;
            auto tail_full = compute_Tnl_tail(p, w, even, s, num_deriv);
            auto tail_two_less = compute_Tnl_tail(p, w, even, s, num_deriv-2);
            if (std::abs((tail_full-tail_two_less)/tail_full) < 1e-12) {
                result = tail_full;
            }
        }

        mpfr::mpreal::set_default_prec(prec_bak);

        return result;
    }

}


#include <elasty/constraint.hpp>
#include <elasty/particle.hpp>
#include <algorithm>
#include <cstring>
#include <Eigen/Geometry>

namespace
{
    inline Eigen::Matrix3d convert_vector_to_cross_operator(const Eigen::Vector3d& vec)
    {
        Eigen::Matrix3d mat = Eigen::Matrix3d::Zero();
        mat(0, 1) = + vec(2);
        mat(0, 2) = - vec(1);
        mat(1, 0) = - vec(2);
        mat(1, 2) = + vec(0);
        mat(2, 0) = + vec(1);
        mat(2, 1) = - vec(0);
        return mat;
    };

    inline double calculateCotTheta(const Eigen::Vector3d& x, const Eigen::Vector3d& y)
    {
        const double cos_theta = x.dot(y);
        const double sin_theta = x.cross(y).norm();
        return cos_theta / sin_theta;
    }

    template<int Num>
    void projectPositions(const double C,
                          const Eigen::Matrix<double, Num * 3, 1>& grad_C,
                          const Eigen::Matrix<double, Num * 3, 1>& inv_M,
                          const double stiffness,
                          std::vector<std::shared_ptr<elasty::Particle>>& particles)
    {
        // Skip if the gradient is sufficiently small
        if (grad_C.isApprox(Eigen::Matrix<double, Num * 3, 1>::Zero())) { return; }

        // Calculate $s$
        const double s = C / (grad_C.transpose() * inv_M.asDiagonal() * grad_C);

        // Calculate $\Delta x$
        const Eigen::Matrix<double, Num * 3, 1> delta_x = - s * inv_M.asDiagonal() * grad_C;
        assert(!delta_x.hasNaN());

        // Update predicted positions
        for (unsigned int j = 0; j < Num; ++ j)
        {
            particles[j]->p += stiffness * delta_x.segment(3 * j, 3);
        }
    }

    template<int Num>
    Eigen::Matrix<double, Num * 3, 1> constructInverseMassMatrix(const std::vector<std::shared_ptr<elasty::Particle>>& particles)
    {
        Eigen::Matrix<double, Num * 3, 1> inv_M;
        for (unsigned int j = 0; j < Num; ++ j)
        {
            inv_M(j * 3 + 0) = particles[j]->w;
            inv_M(j * 3 + 1) = particles[j]->w;
            inv_M(j * 3 + 2) = particles[j]->w;
        }
        return inv_M;
    }
}

elasty::BendingConstraint::BendingConstraint(const std::shared_ptr<Particle> p_0,
                                             const std::shared_ptr<Particle> p_1,
                                             const std::shared_ptr<Particle> p_2,
                                             const std::shared_ptr<Particle> p_3,
                                             const double stiffness,
                                             const double dihedral_angle) :
Constraint(std::vector<std::shared_ptr<Particle>>{ p_0, p_1, p_2, p_3 }, stiffness),
m_dihedral_angle(dihedral_angle)
{
}

void elasty::BendingConstraint::projectParticles()
{
    const double C = calculateValue();

    Eigen::Matrix<double, 12, 1> grad_C;
    calculateGrad(grad_C.data());

    projectPositions<4>(C, grad_C, m_inv_M, m_stiffness, m_particles);
}

double elasty::BendingConstraint::calculateValue()
{
    const Eigen::Vector3d& x_0 = m_particles[0]->p;
    const Eigen::Vector3d& x_1 = m_particles[1]->p;
    const Eigen::Vector3d& x_2 = m_particles[2]->p;
    const Eigen::Vector3d& x_3 = m_particles[3]->p;

    const Eigen::Vector3d p_10 = x_1 - x_0;
    const Eigen::Vector3d p_20 = x_2 - x_0;
    const Eigen::Vector3d p_30 = x_3 - x_0;

    const Eigen::Vector3d n_0 = p_10.cross(p_20).normalized();
    const Eigen::Vector3d n_1 = p_10.cross(p_30).normalized();

    assert(!n_0.hasNaN());
    assert(!n_1.hasNaN());

    const double current_dihedral_angle = std::acos(std::min(+ 1.0, std::max(- 1.0, n_0.dot(n_1))));

    assert(!std::isnan(current_dihedral_angle));

    return current_dihedral_angle - m_dihedral_angle;
}

void elasty::BendingConstraint::calculateGrad(double* grad_C)
{
    const Eigen::Vector3d& x_0 = m_particles[0]->p;
    const Eigen::Vector3d& x_1 = m_particles[1]->p;
    const Eigen::Vector3d& x_2 = m_particles[2]->p;
    const Eigen::Vector3d& x_3 = m_particles[3]->p;

    // Assuming that p_0 = [ 0, 0, 0 ]^T without loss of generality
    const Eigen::Vector3d p_1 = x_1 - x_0;
    const Eigen::Vector3d p_2 = x_2 - x_0;
    const Eigen::Vector3d p_3 = x_3 - x_0;

    const Eigen::Vector3d p_1_cross_p_2 = p_1.cross(p_2);
    const Eigen::Vector3d p_1_cross_p_3 = p_1.cross(p_3);

    const Eigen::Vector3d n_0 = p_1_cross_p_2.normalized();
    const Eigen::Vector3d n_1 = p_1_cross_p_3.normalized();

    const double d = n_0.dot(n_1);

    // If the dihedral angle is sufficiently small, return zeros
    constexpr double epsilon = 1e-12;
    if (std::abs(d) - 1.0 < epsilon)
    {
        std::fill(grad_C, grad_C + 12, 0.0);
        return;
    }

    const double common_coeff = - 1.0 / std::sqrt(1.0 - d * d);

    auto calculate_gradient_of_normalized_cross_product_wrt_p_1 = [](const Eigen::Vector3d& p_1,
                                                                     const Eigen::Vector3d& p_2,
                                                                     const Eigen::Vector3d& n)
    -> Eigen::Matrix3d
    {
        return + (1.0 / p_1.cross(p_2).norm()) * (- convert_vector_to_cross_operator(p_2) + n * (n.cross(p_2)).transpose());
    };

    auto calculate_gradient_of_normalized_cross_product_wrt_p_2 = [](const Eigen::Vector3d& p_1,
                                                                     const Eigen::Vector3d& p_2,
                                                                     const Eigen::Vector3d& n)
    -> Eigen::Matrix3d
    {
        return - (1.0 / p_1.cross(p_2).norm()) * (- convert_vector_to_cross_operator(p_1) + n * (n.cross(p_1)).transpose());
    };

    const Eigen::Vector3d grad_C_wrt_p_1 = common_coeff * (calculate_gradient_of_normalized_cross_product_wrt_p_1(n_0, p_1, p_2).transpose() * n_1 + calculate_gradient_of_normalized_cross_product_wrt_p_1(n_1, p_1, p_3).transpose() * n_0);
    const Eigen::Vector3d grad_C_wrt_p_2 = common_coeff * calculate_gradient_of_normalized_cross_product_wrt_p_2(n_0, p_1, p_2).transpose() * n_1;
    const Eigen::Vector3d grad_C_wrt_p_3 = common_coeff * calculate_gradient_of_normalized_cross_product_wrt_p_2(n_1, p_1, p_3).transpose() * n_0;
    const Eigen::Vector3d grad_C_wrt_p_0 = - grad_C_wrt_p_1 - grad_C_wrt_p_2 - grad_C_wrt_p_3;

    std::memcpy(grad_C + (3 * 0), grad_C_wrt_p_0.data(), sizeof(double) * 3);
    std::memcpy(grad_C + (3 * 1), grad_C_wrt_p_1.data(), sizeof(double) * 3);
    std::memcpy(grad_C + (3 * 2), grad_C_wrt_p_2.data(), sizeof(double) * 3);
    std::memcpy(grad_C + (3 * 3), grad_C_wrt_p_3.data(), sizeof(double) * 3);
}

elasty::DistanceConstraint::DistanceConstraint(const std::shared_ptr<Particle> p_0,
                                               const std::shared_ptr<Particle> p_1,
                                               const double stiffness,
                                               const double d) :
Constraint(std::vector<std::shared_ptr<Particle>>{ p_0, p_1 }, stiffness),
m_inv_M(constructInverseMassMatrix<2>(m_particles)),
m_d(d)
{
    assert(d >= 0.0);
}

void elasty::DistanceConstraint::projectParticles()
{
    const double C = calculateValue();

    Eigen::Matrix<double, 6, 1> grad_C;
    calculateGrad(grad_C.data());

    projectPositions<2>(C, grad_C, m_inv_M, m_stiffness, m_particles);
}

double elasty::DistanceConstraint::calculateValue()
{
    const Eigen::Vector3d& x_0 = m_particles[0]->p;
    const Eigen::Vector3d& x_1 = m_particles[1]->p;

    return (x_0 - x_1).norm() - m_d;
}

void elasty::DistanceConstraint::calculateGrad(double* grad_C)
{
    const Eigen::Vector3d& x_0 = m_particles[0]->p;
    const Eigen::Vector3d& x_1 = m_particles[1]->p;

    Eigen::Vector3d n = (x_0 - x_1).normalized();

    if (n.hasNaN()) { n = Eigen::Vector3d::Random(3).normalized(); }

    grad_C[0] = + n(0);
    grad_C[1] = + n(1);
    grad_C[2] = + n(2);
    grad_C[3] = - n(0);
    grad_C[4] = - n(1);
    grad_C[5] = - n(2);
}

elasty::EnvironmentalCollisionConstraint::EnvironmentalCollisionConstraint(const std::shared_ptr<Particle> p_0,
                                                                           const double stiffness,
                                                                           const Eigen::Vector3d& n,
                                                                           const double d) :
Constraint(std::vector<std::shared_ptr<Particle>>{ p_0 }, stiffness),
m_inv_M(constructInverseMassMatrix<1>(m_particles)),
m_n(n),
m_d(d)
{
}

void elasty::EnvironmentalCollisionConstraint::projectParticles()
{
    const double C = calculateValue();

    if (C >= 0.0) { return; }

    Eigen::Vector3d grad_C;
    calculateGrad(grad_C.data());

    projectPositions<1>(C, grad_C, m_inv_M, m_stiffness, m_particles);
}

double elasty::EnvironmentalCollisionConstraint::calculateValue()
{
    const Eigen::Vector3d& x = m_particles[0]->p;
    return m_n.transpose() * x - m_d;
}

void elasty::EnvironmentalCollisionConstraint::calculateGrad(double* grad_C)
{
    std::memcpy(grad_C, m_n.data(), sizeof(double) * 3);
}

elasty::FixedPointConstraint::FixedPointConstraint(const std::shared_ptr<Particle> p_0,
                                                   const double stiffness,
                                                   const Eigen::Vector3d& point) :
Constraint(std::vector<std::shared_ptr<Particle>>{ p_0 }, stiffness),
m_inv_M(constructInverseMassMatrix<1>(m_particles)),
m_point(point)
{
}

void elasty::FixedPointConstraint::projectParticles()
{
    const double C = calculateValue();

    Eigen::Vector3d grad_C;
    calculateGrad(grad_C.data());

    projectPositions<1>(C, grad_C, m_inv_M, m_stiffness, m_particles);
}

double elasty::FixedPointConstraint::calculateValue()
{
    const Eigen::Vector3d& x = m_particles[0]->p;
    return (x - m_point).norm();
}

void elasty::FixedPointConstraint::calculateGrad(double* grad_C)
{
    const Eigen::Vector3d& x = m_particles[0]->p;
    const Eigen::Vector3d n = (x - m_point).normalized();

    if (n.hasNaN()) { std::fill(grad_C, grad_C + 3, 0.0); }

    std::memcpy(grad_C, n.data(), sizeof(double) * 3);
}

elasty::IsometricBendingConstraint::IsometricBendingConstraint(const std::shared_ptr<Particle> p_0,
                                                               const std::shared_ptr<Particle> p_1,
                                                               const std::shared_ptr<Particle> p_2,
                                                               const std::shared_ptr<Particle> p_3,
                                                               const double stiffness) :
Constraint(std::vector<std::shared_ptr<Particle>>{ p_0, p_1, p_2, p_3 }, stiffness),
m_inv_M(constructInverseMassMatrix<4>(m_particles))
{
    const Eigen::Vector3d& x_0 = p_0->x;
    const Eigen::Vector3d& x_1 = p_1->x;
    const Eigen::Vector3d& x_2 = p_2->x;
    const Eigen::Vector3d& x_3 = p_3->x;

    const Eigen::Vector3d e0 = x_1 - x_0;
    const Eigen::Vector3d e1 = x_2 - x_1;
    const Eigen::Vector3d e2 = x_0 - x_2;
    const Eigen::Vector3d e3 = x_3 - x_0;
    const Eigen::Vector3d e4 = x_1 - x_3;

    const double cot_01 = calculateCotTheta(e0, - e1);
    const double cot_02 = calculateCotTheta(e0, - e2);
    const double cot_03 = calculateCotTheta(e0, e3);
    const double cot_04 = calculateCotTheta(e0, e4);

    const Eigen::Vector4d K = Eigen::Vector4d(cot_01 + cot_04, cot_02 + cot_03, - cot_01 - cot_02, - cot_03 - cot_04);

    const double A_0 = 0.5 * e0.cross(e1).norm();
    const double A_1 = 0.5 * e0.cross(e3).norm();

    m_Q = (3.0 / (A_0 + A_1)) * K * K.transpose();
}

void elasty::IsometricBendingConstraint::projectParticles()
{
    const double C = calculateValue();

    Eigen::Matrix<double, 12, 1> grad_C;
    calculateGrad(grad_C.data());

    projectPositions<4>(C, grad_C, m_inv_M, m_stiffness, m_particles);
}

double elasty::IsometricBendingConstraint::calculateValue()
{
    double sum = 0.0;
    for (unsigned int i = 0; i < 4; ++ i)
    {
        for (unsigned int j = 0; j < 4; ++ j)
        {
            sum += m_Q(i, j) * double(m_particles[i]->p.transpose() * m_particles[j]->p);
        }
    }
    return 0.5 * sum;
}

void elasty::IsometricBendingConstraint::calculateGrad(double* grad_C)
{
    for (unsigned int i = 0; i < 4; ++ i)
    {
        Eigen::Vector3d sum = Eigen::Vector3d::Zero();
        for (unsigned int j = 0; j < 4; ++ j)
        {
            sum += m_Q(i, j) * m_particles[j]->p;
        }
        std::memcpy(grad_C + (3 * i), sum.data(), sizeof(double) * 3);
    }
}


#include "problem_generator.h"

#include <Eigen/Dense>
#include <iostream>
#include <random>
#include <vector>

namespace poselib {

static const double kPI = 3.14159265358979323846;

double CalibPoseValidator::compute_pose_error(const AbsolutePoseProblemInstance &instance, const CameraPose &pose,
                                              double scale) {
    return (instance.pose_gt.R() - pose.R()).norm() + (instance.pose_gt.t - pose.t).norm() +
           std::abs(instance.scale_gt - scale);
}
double CalibPoseValidator::compute_pose_error(const RelativePoseProblemInstance &instance, const CameraPose &pose) {
    return (instance.pose_gt.R() - pose.R()).norm() + (instance.pose_gt.t - pose.t).norm();
}

bool CalibPoseValidator::is_valid(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale,
                                  double tol) {

    // Point to point correspondences
    // alpha * p + lambda*x = R*X + t
    for (int i = 0; i < instance.x_point_.size(); ++i) {
        double err = 1.0 - std::abs(instance.x_point_[i].dot(
                               (pose.R() * instance.X_point_[i] + pose.t - scale * instance.p_point_[i]).normalized()));
        if (err > tol)
            return false;
    }

    // Point to Line correspondences
    // alpha * p + lambda * x = R*(X + mu*V) + t
    for (int i = 0; i < instance.x_line_.size(); ++i) {
        // lambda * x - mu * R*V = R*X + t - alpha * p
        // x.cross(R*V).dot(R*X+t-alpha.p) = 0
        Eigen::Vector3d X = pose.R() * instance.X_line_[i] + pose.t - scale * instance.p_line_[i];
        double err = instance.x_line_[i].cross(pose.R() * instance.V_line_[i]).normalized().dot(X);

        if (err > tol)
            return false;
    }

    // Line to point correspondences
    // l'*(R*X + t - alpha*p) = 0
    for (int i = 0; i < instance.l_line_point_.size(); ++i) {

        Eigen::Vector3d X = pose.R() * instance.X_line_point_[i] + pose.t - scale * instance.p_line_point_[i];

        double err = std::abs(instance.l_line_point_[i].dot(X.normalized()));
        if (err > tol)
            return false;
    }

    // Line to line correspondences
    // l'*(R*(X + mu*V) + t - alpha*p) = 0
    for (int i = 0; i < instance.l_line_line_.size(); ++i) {

        Eigen::Vector3d X = pose.R() * instance.X_line_line_[i] + pose.t - scale * instance.p_line_line_[i];
        Eigen::Vector3d V = pose.R() * instance.V_line_line_[i];

        double err = std::abs(instance.l_line_line_[i].dot(X.normalized())) +
                     std::abs(instance.l_line_line_[i].dot(V.normalized()));
        if (err > tol)
            return false;
    }

    return true;
}

bool CalibPoseValidator::is_valid(const RelativePoseProblemInstance &instance, const CameraPose &pose, double tol) {
    if ((pose.R().transpose() * pose.R() - Eigen::Matrix3d::Identity()).norm() > tol)
        return false;

    // Point to point correspondences
    // R * (alpha * p1 + lambda1 * x1) + t = alpha * p2 + lambda2 * x2
    //
    // cross(R*x1, x2)' * (alpha * p2 - t - alpha * R*p1) = 0
    for (int i = 0; i < instance.x1_.size(); ++i) {
        double err = std::abs(instance.x2_[i]
                                  .cross(pose.R() * instance.x1_[i])
                                  .normalized()
                                  .dot(pose.R() * instance.p1_[i] + pose.t - instance.p2_[i]));
        if (err > tol)
            return false;
    }

    return true;
}

double HomographyValidator::compute_pose_error(const RelativePoseProblemInstance &instance, const Eigen::Matrix3d &H) {
    double err1 = (H.normalized() - instance.H_gt.normalized()).norm();
    double err2 = (H.normalized() + instance.H_gt.normalized()).norm();
    return std::min(err1, err2);
}

bool HomographyValidator::is_valid(const RelativePoseProblemInstance &instance, const Eigen::Matrix3d &H, double tol) {

    for (int i = 0; i < instance.x1_.size(); ++i) {
        Eigen::Vector3d z = H * instance.x1_[i];
        double err = 1.0 - std::abs(z.normalized().dot(instance.x2_[i].normalized()));
        if (err > tol)
            return false;
    }

    return true;
}

double UnknownFocalValidator::compute_pose_error(const AbsolutePoseProblemInstance &instance, const CameraPose &pose,
                                                 double focal) {
    return (instance.pose_gt.R() - pose.R()).norm() + (instance.pose_gt.t - pose.t).norm() +
           std::abs(instance.focal_gt - focal);
}

bool UnknownFocalValidator::is_valid(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double focal,
                                     double tol) {
    if ((pose.R().transpose() * pose.R() - Eigen::Matrix3d::Identity()).norm() > tol)
        return false;

    if (focal < 0)
        return false;

    Eigen::Matrix3d Kinv;
    Kinv.setIdentity();
    Kinv(2, 2) = focal;
    // lambda*diag(1,1,alpha)*x = R*X + t
    for (int i = 0; i < instance.x_point_.size(); ++i) {
        double err = 1.0 - std::abs((Kinv * instance.x_point_[i])
                                        .normalized()
                                        .dot((pose.R() * instance.X_point_[i] + pose.t).normalized()));
        if (err > tol)
            return false;
    }

    return true;
}

double RadialPoseValidator::compute_pose_error(const AbsolutePoseProblemInstance &instance, const CameraPose &pose,
                                               double scale) {
    // Only compute up to sign for radial cameras

    double err1 = (instance.pose_gt.R().topRows(2) - pose.R().topRows(2)).norm() +
                  (instance.pose_gt.t.topRows(2) - pose.t.topRows(2)).norm();
    double err2 = (instance.pose_gt.R().topRows(2) + pose.R().topRows(2)).norm() +
                  (instance.pose_gt.t.topRows(2) + pose.t.topRows(2)).norm();

    return std::min(err1, err2);
}

bool RadialPoseValidator::is_valid(const AbsolutePoseProblemInstance &instance, const CameraPose &pose, double scale,
                                   double tol) {
    if ((pose.R().transpose() * pose.R() - Eigen::Matrix3d::Identity()).norm() > tol)
        return false;

    // Point to point correspondences -- Convert these to line correspondences
    // alpha * p + lambda*x = R*X + t
    for (int i = 0; i < instance.x_point_.size(); ++i) {
        Eigen::Vector3d radial_line{-instance.x_point_[i](1), instance.x_point_[i](0), 0.0};
        Eigen::Vector3d X = pose.R() * instance.X_point_[i] + pose.t;
        double err = std::abs(radial_line.dot(X.normalized()));
        if (err > tol)
            return false;
    }

    // Line to point correspondences
    // l'*(R*X + t) = 0
    for (int i = 0; i < instance.l_line_point_.size(); ++i) {
        Eigen::Vector3d X = pose.R() * instance.X_line_point_[i] + pose.t;

        double err = std::abs(instance.l_line_point_[i].dot(X.normalized()));
        if (err > tol)
            return false;
    }

    return true;
}

void set_random_pose(CameraPose &pose, bool upright, bool planar) {
    if (upright) {
        Eigen::Vector2d r;
        r.setRandom().normalize();
        Eigen::Matrix3d R;
        R << r(0), 0.0, r(1), 0.0, 1.0, 0.0, -r(1), 0.0, r(0); // y-gravity
        // pose.R << r(0), r(1), 0.0, -r(1), r(0), 0.0, 0.0, 0.0, 1.0; // z-gravity
        pose.q = rotmat_to_quat(R);
    } else {
        pose.q = Eigen::Quaternion<double>::UnitRandom().coeffs();
    }
    pose.t.setRandom();
    if (planar) {
        pose.t.y() = 0;
    }
}

void generate_abspose_problems(int n_problems, std::vector<AbsolutePoseProblemInstance> *problem_instances,
                               const ProblemOptions &options) {
    problem_instances->clear();
    problem_instances->reserve(n_problems);

    double fov_scale = std::tan(options.camera_fov_ / 2.0 * kPI / 180.0);

    // Random generators
    std::default_random_engine random_engine;
    std::uniform_real_distribution<double> depth_gen(options.min_depth_, options.max_depth_);
    std::uniform_real_distribution<double> coord_gen(-fov_scale, fov_scale);
    std::uniform_real_distribution<double> scale_gen(options.min_scale_, options.max_scale_);
    std::uniform_real_distribution<double> focal_gen(options.min_focal_, options.max_focal_);
    std::normal_distribution<double> direction_gen(0.0, 1.0);
    std::normal_distribution<double> offset_gen(0.0, 1.0);

    for (int i = 0; i < n_problems; ++i) {
        AbsolutePoseProblemInstance instance;
        set_random_pose(instance.pose_gt, options.upright_, options.planar_);

        if (options.unknown_scale_) {
            instance.scale_gt = scale_gen(random_engine);
        }
        if (options.unknown_focal_) {
            instance.focal_gt = focal_gen(random_engine);
        }

        // Point to point correspondences
        instance.x_point_.reserve(options.n_point_point_);
        instance.X_point_.reserve(options.n_point_point_);
        instance.p_point_.reserve(options.n_point_point_);
        for (int j = 0; j < options.n_point_point_; ++j) {

            Eigen::Vector3d p{0.0, 0.0, 0.0};
            Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0};
            x.normalize();
            Eigen::Vector3d X;

            if (options.generalized_) {
                p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);
            }

            X = instance.scale_gt * p + x * depth_gen(random_engine);

            X = instance.pose_gt.R().transpose() * (X - instance.pose_gt.t);

            if (options.unknown_focal_) {
                x.block<2, 1>(0, 0) *= instance.focal_gt;
                x.normalize();
            }

            instance.x_point_.push_back(x);
            instance.X_point_.push_back(X);
            instance.p_point_.push_back(p);
        }

        // This generates instances where the same 3D point is observed twice in a generalized camera
        // This is degenerate case for the 3Q3 based gp3p/gp4ps solver unless specifically handled.
        if (options.generalized_ && options.generalized_duplicate_obs_) {
            std::vector<int> ind = {0, 1, 2, 3};
            assert(options.n_point_point_ >= 4);

            std::random_shuffle(ind.begin(), ind.end());
            instance.X_point_[ind[1]] = instance.X_point_[ind[0]];
            instance.x_point_[ind[1]] = (instance.pose_gt.R() * instance.X_point_[ind[0]] + instance.pose_gt.t -
                                         instance.scale_gt * instance.p_point_[ind[1]])
                                            .normalized();
        }

        // Point to line correspondences
        instance.x_line_.reserve(options.n_point_line_);
        instance.X_line_.reserve(options.n_point_line_);
        instance.V_line_.reserve(options.n_point_line_);
        instance.p_line_.reserve(options.n_point_line_);
        for (int j = 0; j < options.n_point_line_; ++j) {
            Eigen::Vector3d p{0.0, 0.0, 0.0};
            Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0};
            x.normalize();
            Eigen::Vector3d X;

            if (options.generalized_) {
                p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);
            }
            X = instance.scale_gt * p + x * depth_gen(random_engine);
            X = instance.pose_gt.R().transpose() * (X - instance.pose_gt.t);

            Eigen::Vector3d V{direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine)};
            V.normalize();

            // Translate X such that X.dot(V) = 0
            X = X - V.dot(X) * V;

            if (options.unknown_focal_) {
                // TODO implement this.
            }

            instance.x_line_.push_back(x);
            instance.X_line_.push_back(X);
            instance.V_line_.push_back(V);
            instance.p_line_.push_back(p);
        }

        // Line to point correspondences
        instance.l_line_point_.reserve(options.n_line_point_);
        instance.X_line_point_.reserve(options.n_line_point_);
        instance.p_line_point_.reserve(options.n_line_point_);
        for (int j = 0; j < options.n_line_point_; ++j) {
            Eigen::Vector3d p{0.0, 0.0, 0.0};
            Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0};
            x.normalize();
            Eigen::Vector3d X;

            if (options.generalized_) {
                p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);
            }
            X = instance.scale_gt * p + x * depth_gen(random_engine);
            X = instance.pose_gt.R().transpose() * (X - instance.pose_gt.t);

            // Cross product with random vector to generate line
            Eigen::Vector3d l;
            if (options.radial_lines_) {
                // Line passing through image center
                l = x.cross(Eigen::Vector3d{0.0, 0.0, 1.0});
            } else {
                // Random line
                l = x.cross(Eigen::Vector3d(direction_gen(random_engine), direction_gen(random_engine),
                                            direction_gen(random_engine)));
            }

            l.normalize();

            if (options.unknown_focal_) {
                // TODO implement this.
            }

            instance.l_line_point_.push_back(l);
            instance.X_line_point_.push_back(X);
            instance.p_line_point_.push_back(p);
        }

        // Line to line correspondences
        instance.l_line_line_.reserve(options.n_line_line_);
        instance.X_line_line_.reserve(options.n_line_line_);
        instance.V_line_line_.reserve(options.n_line_line_);
        instance.p_line_line_.reserve(options.n_line_line_);
        for (int j = 0; j < options.n_line_line_; ++j) {
            Eigen::Vector3d p{0.0, 0.0, 0.0};
            Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0};
            x.normalize();
            Eigen::Vector3d X;

            if (options.generalized_) {
                p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);
            }
            X = instance.scale_gt * p + x * depth_gen(random_engine);
            X = instance.pose_gt.R().transpose() * (X - instance.pose_gt.t);

            Eigen::Vector3d V{direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine)};
            V.normalize();

            // Translate X such that X.dot(V) = 0
            X = X - V.dot(X) * V;

            Eigen::Vector3d l = x.cross(instance.pose_gt.R() * V);
            l.normalize();

            if (options.unknown_focal_) {
                // TODO implement this.
            }

            instance.l_line_line_.push_back(l);
            instance.X_line_line_.push_back(X);
            instance.V_line_line_.push_back(V);
            instance.p_line_line_.push_back(p);
        }

        problem_instances->push_back(instance);
    }
}

void generate_relpose_problems(int n_problems, std::vector<RelativePoseProblemInstance> *problem_instances,
                               const ProblemOptions &options) {
    problem_instances->clear();
    problem_instances->reserve(n_problems);

    double fov_scale = std::tan(options.camera_fov_ / 2.0 * kPI / 180.0);

    // Random generators
    std::default_random_engine random_engine;
    std::uniform_real_distribution<double> depth_gen(options.min_depth_, options.max_depth_);
    std::uniform_real_distribution<double> coord_gen(-fov_scale, fov_scale);
    std::uniform_real_distribution<double> scale_gen(options.min_scale_, options.max_scale_);
    std::uniform_real_distribution<double> focal_gen(options.min_focal_, options.max_focal_);
    std::normal_distribution<double> direction_gen(0.0, 1.0);
    std::normal_distribution<double> offset_gen(0.0, 1.0);

    for (int i = 0; i < n_problems; ++i) {
        RelativePoseProblemInstance instance;
        set_random_pose(instance.pose_gt, options.upright_, options.planar_);

        if (options.unknown_scale_) {
            instance.scale_gt = scale_gen(random_engine);
        }
        if (options.unknown_focal_) {
            instance.focal_gt = focal_gen(random_engine);
        }

        if (!options.generalized_) {
            instance.pose_gt.t.normalize();
        }

        // Point to point correspondences
        instance.p1_.reserve(options.n_point_point_);
        instance.x1_.reserve(options.n_point_point_);
        instance.p2_.reserve(options.n_point_point_);
        instance.x2_.reserve(options.n_point_point_);

        for (int j = 0; j < options.n_point_point_; ++j) {

            Eigen::Vector3d p1{0.0, 0.0, 0.0};
            Eigen::Vector3d p2{0.0, 0.0, 0.0};
            Eigen::Vector3d x1{coord_gen(random_engine), coord_gen(random_engine), 1.0};
            x1.normalize();
            Eigen::Vector3d X;

            if (options.generalized_) {
                p1 << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);
                p2 << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);

                if (j > 0 && j < options.generalized_first_cam_obs_) {
                    p1 = instance.p1_[0];
                    p2 = instance.p2_[0];
                }
            }

            X = instance.scale_gt * p1 + x1 * depth_gen(random_engine);
            // Map into second image
            X = instance.pose_gt.R() * X + instance.pose_gt.t;

            Eigen::Vector3d x2 = (X - instance.scale_gt * p2).normalized();

            if (options.unknown_focal_) {
                // NYI
                assert(false);
            }

            // TODO: ensure FoV of second cameras as well...

            instance.p1_.push_back(p1);
            instance.x1_.push_back(x1);
            instance.p2_.push_back(p2);
            instance.x2_.push_back(x2);
        }

        problem_instances->push_back(instance);
    }
}

void generate_homography_problems(int n_problems, std::vector<RelativePoseProblemInstance> *problem_instances,
                                  const ProblemOptions &options) {
    problem_instances->clear();
    problem_instances->reserve(n_problems);

    double fov_scale = std::tan(options.camera_fov_ / 2.0 * kPI / 180.0);

    // Random generators
    std::default_random_engine random_engine;
    std::uniform_real_distribution<double> depth_gen(options.min_depth_, options.max_depth_);
    std::uniform_real_distribution<double> coord_gen(-fov_scale, fov_scale);
    std::uniform_real_distribution<double> scale_gen(options.min_scale_, options.max_scale_);
    std::uniform_real_distribution<double> focal_gen(options.min_focal_, options.max_focal_);
    std::normal_distribution<double> direction_gen(0.0, 1.0);
    std::normal_distribution<double> offset_gen(0.0, 1.0);

    while (problem_instances->size() < n_problems) {
        RelativePoseProblemInstance instance;
        set_random_pose(instance.pose_gt, options.upright_, options.planar_);

        if (options.unknown_scale_) {
            instance.scale_gt = scale_gen(random_engine);
        }
        if (options.unknown_focal_) {
            instance.focal_gt = focal_gen(random_engine);
        }

        if (!options.generalized_) {
            instance.pose_gt.t.normalize();
        }

        // Point to point correspondences
        instance.x1_.reserve(options.n_point_point_);
        instance.x2_.reserve(options.n_point_point_);

        // Generate plane
        Eigen::Vector3d n;
        n << direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine);
        n.normalize();

        // Choose depth of plane such that center point of image 1 is at depth d
        double d_center = depth_gen(random_engine);
        double alpha = d_center / n(2);
        // plane is n'*X = alpha

        // ground truth homography
        instance.H_gt = alpha * instance.pose_gt.R() + instance.pose_gt.t * n.transpose();

        bool failed_instance = false;
        for (int j = 0; j < options.n_point_point_; ++j) {
            bool point_okay = false;
            for (int trials = 0; trials < 10; ++trials) {
                Eigen::Vector3d x1{coord_gen(random_engine), coord_gen(random_engine), 1.0};
                x1.normalize();
                Eigen::Vector3d X;

                // compute depth
                double lambda = alpha / n.dot(x1);
                X = x1 * lambda;
                // Map into second image
                X = instance.pose_gt.R() * X + instance.pose_gt.t;

                Eigen::Vector3d x2 = X.normalized();

                // Check cheirality
                if (x2(2) < 0 || lambda < 0) {
                    // try to generate another point
                    continue;
                }

                // Check FoV of second camera
                Eigen::Vector2d x2h = x2.hnormalized();
                if (x2h(0) < -fov_scale || x2h(0) > fov_scale || x2h(1) < -fov_scale || x2h(1) > fov_scale) {
                    // try to generate another point
                    continue;
                }

                if (options.generalized_) {
                    // NYI
                    assert(false);
                }
                if (options.unknown_focal_) {
                    // NYI
                    assert(false);
                }

                instance.x1_.push_back(x1);
                instance.x2_.push_back(x2);
                point_okay = true;
                break;
            }
            if (!point_okay) {
                failed_instance = true;
                break;
            }
        }
        if (failed_instance) {
            continue;
        }

        problem_instances->push_back(instance);
    }
}

}; // namespace poselib


#define BOOST_TEST_MODULE "test_aabb"
#include <boost/test/included/unit_test.hpp>
#include <afmize/collision.hpp>
#include <random>

BOOST_AUTO_TEST_CASE(aabb_sphere)
{
    using sphere = afmize::sphere<double>;
    using aabb   = afmize::aabb<double>;
    using point  = mave::vector<double, 3>;

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<double> uni(-100.0, 100.0);

    for(std::size_t i=0; i<1000; ++i)
    {
        const sphere s{uni(mt), point{uni(mt), uni(mt), uni(mt)}};
        const aabb box = afmize::make_aabb(s);

        BOOST_TEST(box.lower[0] == s.center[0] - s.radius,
                   boost::test_tools::tolerance(1e-6));
        BOOST_TEST(box.lower[1] == s.center[1] - s.radius,
                   boost::test_tools::tolerance(1e-6));
        BOOST_TEST(box.lower[2] == s.center[2] - s.radius,
                   boost::test_tools::tolerance(1e-6));

        BOOST_TEST(box.upper[0] == s.center[0] + s.radius,
                   boost::test_tools::tolerance(1e-6));
        BOOST_TEST(box.upper[1] == s.center[1] + s.radius,
                   boost::test_tools::tolerance(1e-6));
        BOOST_TEST(box.upper[2] == s.center[2] + s.radius,
                   boost::test_tools::tolerance(1e-6));
    }
}

BOOST_AUTO_TEST_CASE(merge_aabb)
{
    using sphere = afmize::sphere<double>;
    using aabb   = afmize::aabb<double>;
    using point  = mave::vector<double, 3>;

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<double> uni(-100.0, 100.0);

    for(std::size_t i=0; i<1000; ++i)
    {
        const aabb box1{point{uni(mt), uni(mt), uni(mt)},
                        point{uni(mt), uni(mt), uni(mt)}};
        const aabb box2{point{uni(mt), uni(mt), uni(mt)},
                        point{uni(mt), uni(mt), uni(mt)}};

        const aabb box3 = afmize::merge_aabb(box1, box2);

        BOOST_TEST(box3.lower[0] == std::min(box1.lower[0], box2.lower[0]),
                   boost::test_tools::tolerance(1e-6));
        BOOST_TEST(box3.lower[1] == std::min(box1.lower[1], box2.lower[1]),
                   boost::test_tools::tolerance(1e-6));
        BOOST_TEST(box3.lower[2] == std::min(box1.lower[2], box2.lower[2]),
                   boost::test_tools::tolerance(1e-6));

        BOOST_TEST(box3.upper[0] == std::max(box1.upper[0], box2.upper[0]),
                   boost::test_tools::tolerance(1e-6));
        BOOST_TEST(box3.upper[1] == std::max(box1.upper[1], box2.upper[1]),
                   boost::test_tools::tolerance(1e-6));
        BOOST_TEST(box3.upper[2] == std::max(box1.upper[2], box2.upper[2]),
                   boost::test_tools::tolerance(1e-6));
    }
}


#ifndef HOPS_DIKINPROPOSAL_HPP
#define HOPS_DIKINPROPOSAL_HPP

#include <Eigen/LU>
#include "DikinEllipsoidCalculator.hpp"
#include "../../RandomNumberGenerator/RandomNumberGenerator.hpp"
#include <random>

namespace hops {
    template<typename MatrixType, typename VectorType>
    class DikinProposal {
    public:
        using StateType = VectorType;

        /**
         * @brief Constructs Gaussian Dikin proposal mechanism on polytope defined as Ax<b.
         * @param A
         * @param b
         * @param currentState
         */
        DikinProposal(MatrixType A, VectorType b, VectorType currentState);

        void propose(RandomNumberGenerator &randomNumberGenerator);

        void acceptProposal();

        typename MatrixType::Scalar computeLogAcceptanceProbability();

        StateType getState() const;

        void setState(StateType newState);

        StateType getProposal() const;

        typename MatrixType::Scalar getStepSize() const;

        void setStepSize(typename MatrixType::Scalar newStepSize);

        std::string getName();

    private:
        MatrixType A;
        VectorType b;

        StateType state;
        StateType proposal;
        typename MatrixType::Scalar stateLogSqrtDeterminant = 0;
        typename MatrixType::Scalar proposalLogSqrtDeterminant = 0;
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> stateCholeskyOfDikinEllipsoid;
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> proposalCholeskyOfDikinEllipsoid;

        typename MatrixType::Scalar stepSize = 0.075; // value  from dikin walk publication
        typename MatrixType::Scalar geometricFactor;
        typename MatrixType::Scalar covarianceFactor;
        constexpr static typename MatrixType::Scalar boundaryCushion = 0;

        std::normal_distribution<typename MatrixType::Scalar> normalDistribution{0., 1.};
        DikinEllipsoidCalculator<MatrixType, VectorType> dikinEllipsoidCalculator;
    };

    template<typename MatrixType, typename VectorType>
    DikinProposal<MatrixType, VectorType>::DikinProposal(MatrixType A,
                                                         VectorType b,
                                                         VectorType currentState) :
            A(std::move(A)),
            b(std::move(b)),
            dikinEllipsoidCalculator(this->A, this->b) {
        setStepSize(1.);
        setState(std::move(currentState));
        proposal = state;
    }

    template<typename MatrixType, typename VectorType>
    void DikinProposal<MatrixType, VectorType>::propose(RandomNumberGenerator &randomNumberGenerator) {
        for (long i = 0; i < proposal.rows(); ++i) {
            proposal(i) = normalDistribution(randomNumberGenerator);
        }
        proposal = state + covarianceFactor *
                           stateCholeskyOfDikinEllipsoid.template triangularView<Eigen::Lower>().solve(proposal);
    }

    template<typename MatrixType, typename VectorType>
    void DikinProposal<MatrixType, VectorType>::acceptProposal() {
        state.swap(proposal);
        stateCholeskyOfDikinEllipsoid = std::move(proposalCholeskyOfDikinEllipsoid);
        stateLogSqrtDeterminant = proposalLogSqrtDeterminant;
    }

    template<typename MatrixType, typename VectorType>
    typename MatrixType::Scalar
    DikinProposal<MatrixType, VectorType>::computeLogAcceptanceProbability() {
        bool isProposalInteriorPoint = ((A * proposal - b).array() < -boundaryCushion).all();
        if (!isProposalInteriorPoint) {
            return -std::numeric_limits<typename MatrixType::Scalar>::infinity();
        }

        auto choleskyResult = dikinEllipsoidCalculator.computeCholeskyFactorOfDikinEllipsoid(proposal);
        if (!choleskyResult.first) {
            return -std::numeric_limits<typename MatrixType::Scalar>::infinity();
        }
        proposalCholeskyOfDikinEllipsoid = std::move(choleskyResult.second);

        proposalLogSqrtDeterminant = proposalCholeskyOfDikinEllipsoid.diagonal().array().log().sum();
        VectorType stateDifference = state - proposal;

        return proposalLogSqrtDeterminant
               - stateLogSqrtDeterminant
               + geometricFactor * ((stateCholeskyOfDikinEllipsoid * stateDifference).squaredNorm()
                                    - (proposalCholeskyOfDikinEllipsoid * stateDifference).squaredNorm()
        );
    }

    template<typename MatrixType, typename VectorType>
    typename DikinProposal<MatrixType, VectorType>::StateType
    DikinProposal<MatrixType, VectorType>::getState() const {
        return state;
    }

    template<typename MatrixType, typename VectorType>
    void DikinProposal<MatrixType, VectorType>::setState(StateType newState) {
        state.swap(newState);
        auto choleskyResult = dikinEllipsoidCalculator.computeCholeskyFactorOfDikinEllipsoid(state);
        if (!choleskyResult.first) {
            throw std::runtime_error("Could not compute cholesky factorization for newState.");
        }
        stateCholeskyOfDikinEllipsoid = std::move(choleskyResult.second);
        stateLogSqrtDeterminant = stateCholeskyOfDikinEllipsoid.diagonal().array().log().sum();
    }

    template<typename MatrixType, typename VectorType>
    typename DikinProposal<MatrixType, VectorType>::StateType
    DikinProposal<MatrixType, VectorType>::getProposal() const {
        return proposal;
    }

    template<typename MatrixType, typename VectorType>
    typename MatrixType::Scalar DikinProposal<MatrixType, VectorType>::getStepSize() const {
        return stepSize;
    }

    template<typename MatrixType, typename VectorType>
    void DikinProposal<MatrixType, VectorType>::setStepSize(typename MatrixType::Scalar newStepSize) {
        stepSize = newStepSize;
        geometricFactor = A.cols() / (2 * stepSize);
        covarianceFactor = std::sqrt(stepSize / A.cols());
    }

    template<typename MatrixType, typename VectorType>
    std::string DikinProposal<MatrixType, VectorType>::getName() {
        return "Dikin Walk";
    }
}

#endif //HOPS_DIKINPROPOSAL_HPP