#pragma once

#include <Eigen/Sparse>
#include <cilantro/space_transformations.hpp>
#include <cilantro/nearest_neighbors.hpp>
#include <cilantro/correspondence.hpp>

namespace cilantro {
    // Values interpreted as weights
    template <typename ScalarT, ptrdiff_t EigenDim>
    void resampleTransformations(const RigidTransformationSet<ScalarT,EigenDim> &old_transforms,
                                 const std::vector<NeighborSet<ScalarT>> &new_to_old_map,
                                 RigidTransformationSet<ScalarT,EigenDim> &new_transforms)
    {
        new_transforms.resize(new_to_old_map.size());

        ScalarT total_weight;

#pragma omp parallel for shared (new_transforms) private (total_weight)
        for (size_t i = 0; i < new_transforms.size(); i++) {
            total_weight = (ScalarT)0.0;
            new_transforms[i].linear().setZero();
            new_transforms[i].translation().setZero();
            for (size_t j = 0; j < new_to_old_map[i].size(); j++) {
                total_weight += new_to_old_map[i][j].value;
                new_transforms[i].linear() += new_to_old_map[i][j].value*old_transforms[new_to_old_map[i][j].index].linear();
                new_transforms[i].translation() += new_to_old_map[i][j].value*old_transforms[new_to_old_map[i][j].index].translation();
            }

            if (total_weight == (ScalarT)0.0) {
                new_transforms[i].setIdentity();
            } else {
                total_weight = (ScalarT)(1.0)/total_weight;
                new_transforms[i].linear() *= total_weight;
                new_transforms[i].linear() = new_transforms[i].rotation();
                new_transforms[i].translation() *= total_weight;
            }
        }
    }

    // Values interpreted as weights
    template <typename ScalarT, ptrdiff_t EigenDim>
    inline RigidTransformationSet<ScalarT,EigenDim> resampleTransformations(const RigidTransformationSet<ScalarT,EigenDim> &old_transforms,
                                                                            const std::vector<NeighborSet<ScalarT>> &new_to_old_map)
    {
        RigidTransformationSet<ScalarT,EigenDim> new_transforms;
        resampleTransformations<ScalarT,EigenDim>(old_transforms, new_to_old_map, new_transforms);
        return new_transforms;
    }

    // Values interpreted as distances
    template <typename ScalarT, ptrdiff_t EigenDim>
    void resampleTransformations(const RigidTransformationSet<ScalarT,EigenDim> &old_transforms,
                                 const std::vector<NeighborSet<ScalarT>> &new_to_old_map,
                                 ScalarT distance_sigma,
                                 RigidTransformationSet<ScalarT,EigenDim> &new_transforms)
    {
        new_transforms.resize(new_to_old_map.size());

        const ScalarT sigma_inv_sq = (ScalarT)(1.0)/(distance_sigma*distance_sigma);
        ScalarT curr_weight, total_weight;

#pragma omp parallel for shared (new_transforms) private (curr_weight, total_weight)
        for (size_t i = 0; i < new_transforms.size(); i++) {
            total_weight = (ScalarT)0.0;
            new_transforms[i].linear().setZero();
            new_transforms[i].translation().setZero();
            for (size_t j = 0; j < new_to_old_map[i].size(); j++) {
                curr_weight = std::exp(-(ScalarT)(0.5)*new_to_old_map[i][j].value*sigma_inv_sq);
                total_weight += curr_weight;
                new_transforms[i].linear() += curr_weight*old_transforms[new_to_old_map[i][j].index].linear();
                new_transforms[i].translation() += curr_weight*old_transforms[new_to_old_map[i][j].index].translation();
            }

            if (total_weight == (ScalarT)0.0) {
                new_transforms[i].setIdentity();
            } else {
                total_weight = (ScalarT)(1.0)/total_weight;
                new_transforms[i].linear() *= total_weight;
                new_transforms[i].linear() = new_transforms[i].rotation();
                new_transforms[i].translation() *= total_weight;
            }
        }
    }

    // Values interpreted as distances
    template <typename ScalarT, ptrdiff_t EigenDim>
    inline RigidTransformationSet<ScalarT,EigenDim> resampleTransformations(const RigidTransformationSet<ScalarT,EigenDim> &old_transforms,
                                                                            const std::vector<NeighborSet<ScalarT>> &new_to_old_map,
                                                                            ScalarT distance_sigma)
    {
        RigidTransformationSet<ScalarT,EigenDim> new_transforms;
        resampleTransformations<ScalarT,EigenDim>(old_transforms, new_to_old_map, distance_sigma, new_transforms);
        return new_transforms;
    }

    template <typename ScalarT, ptrdiff_t EigenDim, NeighborhoodType NT>
    void resampleTransformations(const KDTree<ScalarT,EigenDim,KDTreeDistanceAdaptors::L2> &old_support_kd_tree,
                                 const RigidTransformationSet<ScalarT,EigenDim> &old_transforms,
                                 const ConstVectorSetMatrixMap<ScalarT,EigenDim> &new_support,
                                 const NeighborhoodSpecification<ScalarT> &nh,
                                 ScalarT distance_sigma,
                                 RigidTransformationSet<ScalarT,EigenDim> &new_transforms)
    {
        new_transforms.resize(new_support.cols());
        const ScalarT sigma_inv_sq = (ScalarT)(1.0)/(distance_sigma*distance_sigma);

        NeighborSet<ScalarT> nn;
        ScalarT curr_weight, total_weight;

#pragma omp parallel for shared (new_transforms) private (nn, curr_weight, total_weight)
        for (size_t i = 0; i < new_transforms.size(); i++) {
            old_support_kd_tree.template search<NT>(new_support.col(i), nh, nn);

            total_weight = (ScalarT)0.0;
            new_transforms[i].linear().setZero();
            new_transforms[i].translation().setZero();
            for (size_t j = 0; j < nn.size(); j++) {
                curr_weight = std::exp(-(ScalarT)(0.5)*nn[j].value*sigma_inv_sq);
                total_weight += curr_weight;
                new_transforms[i].linear() += curr_weight*old_transforms[nn[j].index].linear();
                new_transforms[i].translation() += curr_weight*old_transforms[nn[j].index].translation();
            }

            if (total_weight == (ScalarT)0.0) {
                new_transforms[i].setIdentity();
            } else {
                total_weight = (ScalarT)(1.0)/total_weight;
                new_transforms[i].linear() *= total_weight;
                new_transforms[i].linear() = new_transforms[i].rotation();
                new_transforms[i].translation() *= total_weight;
            }
        }
    }

    template <typename ScalarT, ptrdiff_t EigenDim, NeighborhoodType NT>
    inline RigidTransformationSet<ScalarT,EigenDim> resampleTransformations(const KDTree<ScalarT,EigenDim,KDTreeDistanceAdaptors::L2> &old_support_kd_tree,
                                                                            const RigidTransformationSet<ScalarT,EigenDim> &old_transforms,
                                                                            const ConstVectorSetMatrixMap<ScalarT,EigenDim> &new_support,
                                                                            const NeighborhoodSpecification<ScalarT> &nh,
                                                                            ScalarT distance_sigma)
    {
        RigidTransformationSet<ScalarT,EigenDim> new_transforms;
        resampleTransformations<ScalarT,EigenDim,NT>(old_support_kd_tree, old_transforms, new_support, nh, distance_sigma, new_transforms);
        return new_transforms;
    }

    template <typename ScalarT, ptrdiff_t EigenDim>
    void resampleTransformations(const KDTree<ScalarT,EigenDim,KDTreeDistanceAdaptors::L2> &old_support_kd_tree,
                                 const RigidTransformationSet<ScalarT,EigenDim> &old_transforms,
                                 const ConstVectorSetMatrixMap<ScalarT,EigenDim> &new_support,
                                 const NeighborhoodSpecification<ScalarT> &nh,
                                 ScalarT distance_sigma,
                                 RigidTransformationSet<ScalarT,EigenDim> &new_transforms)
    {
        switch (nh.type) {
            case NeighborhoodType::KNN:
                resampleTransformations<ScalarT,EigenDim,NeighborhoodType::KNN>(old_support_kd_tree, old_transforms, new_support, nh, distance_sigma, new_transforms);
                break;
            case NeighborhoodType::RADIUS:
                resampleTransformations<ScalarT,EigenDim,NeighborhoodType::RADIUS>(old_support_kd_tree, old_transforms, new_support, nh, distance_sigma, new_transforms);
                break;
            case NeighborhoodType::KNN_IN_RADIUS:
                resampleTransformations<ScalarT,EigenDim,NeighborhoodType::KNN_IN_RADIUS>(old_support_kd_tree, old_transforms, new_support, nh, distance_sigma, new_transforms);
                break;
        }
    }

    template <typename ScalarT, ptrdiff_t EigenDim>
    inline RigidTransformationSet<ScalarT,EigenDim> resampleTransformations(const KDTree<ScalarT,EigenDim,KDTreeDistanceAdaptors::L2> &old_support_kd_tree,
                                                                            const RigidTransformationSet<ScalarT,EigenDim> &old_transforms,
                                                                            const ConstVectorSetMatrixMap<ScalarT,EigenDim> &new_support,
                                                                            const NeighborhoodSpecification<ScalarT> &nh,
                                                                            ScalarT distance_sigma)
    {
        RigidTransformationSet<ScalarT,EigenDim> new_transforms;
        resampleTransformations<ScalarT,EigenDim>(old_support_kd_tree, old_transforms, new_support, nh, distance_sigma, new_transforms);
        return new_transforms;
    }

    template <typename ScalarT>
    inline ScalarT sqrtHuberLoss(ScalarT x, ScalarT delta = (ScalarT)1.0) {
        const ScalarT x_abs = std::abs(x);
        if (x_abs > delta) {
            return std::sqrt(delta*(x_abs - (ScalarT)(0.5)*delta));
        } else {
            return std::sqrt((ScalarT)(0.5))*x_abs;
        }
    }

    template <typename ScalarT>
    inline ScalarT sqrtHuberLossDerivative(ScalarT x, ScalarT delta = (ScalarT)1.0) {
        const ScalarT x_abs = std::abs(x);
        if (x < (ScalarT)0.0) {
            if (x_abs > delta) {
                return -delta/((ScalarT)(2.0)*std::sqrt(delta*(x_abs - (ScalarT)(0.5)*delta)));
            } else {
                return -std::sqrt((ScalarT)(0.5));
            }
        } else {
            if (x_abs > delta) {
                return delta/((ScalarT)(2.0)*std::sqrt(delta*(x_abs - (ScalarT)(0.5)*delta)));
            } else {
                return std::sqrt((ScalarT)0.5);
            }
        }
    }

    template <typename ScalarT>
    void computeRotationTerms(ScalarT a, ScalarT b, ScalarT c,
                              Eigen::Matrix<ScalarT,3,3> &rot_coeffs,
                              Eigen::Matrix<ScalarT,3,3> &d_rot_coeffs_da,
                              Eigen::Matrix<ScalarT,3,3> &d_rot_coeffs_db,
                              Eigen::Matrix<ScalarT,3,3> &d_rot_coeffs_dc)
    {
        const ScalarT sina = std::sin(a);
        const ScalarT cosa = std::cos(a);
        const ScalarT sinb = std::sin(b);
        const ScalarT cosb = std::cos(b);
        const ScalarT sinc = std::sin(c);
        const ScalarT cosc = std::cos(c);

        rot_coeffs(0,0) = cosc*cosb;
        rot_coeffs(1,0) = -sinc*cosa + cosc*sinb*sina;
        rot_coeffs(2,0) = sinc*sina + cosc*sinb*cosa;
        rot_coeffs(0,1) = sinc*cosb;
        rot_coeffs(1,1) = cosc*cosa + sinc*sinb*sina;
        rot_coeffs(2,1) = -cosc*sina + sinc*sinb*cosa;
        rot_coeffs(0,2) = -sinb;
        rot_coeffs(1,2) = cosb*sina;
        rot_coeffs(2,2) = cosb*cosa;

        d_rot_coeffs_da(0,0) = (ScalarT)0.0;
        d_rot_coeffs_da(1,0) = sinc*sina + cosc*sinb*cosa;
        d_rot_coeffs_da(2,0) = sinc*cosa - cosc*sinb*sina;
        d_rot_coeffs_da(0,1) = (ScalarT)0.0;
        d_rot_coeffs_da(1,1) = -cosc*sina + sinc*sinb*cosa;
        d_rot_coeffs_da(2,1) = -cosc*cosa - sinc*sinb*sina;
        d_rot_coeffs_da(0,2) = (ScalarT)0.0;
        d_rot_coeffs_da(1,2) = cosb*cosa;
        d_rot_coeffs_da(2,2) = -cosb*sina;

        d_rot_coeffs_db(0,0) = -cosc*sinb;
        d_rot_coeffs_db(1,0) = cosc*cosb*sina;
        d_rot_coeffs_db(2,0) = cosc*cosb*cosa;
        d_rot_coeffs_db(0,1) = -sinc*sinb;
        d_rot_coeffs_db(1,1) = sinc*cosb*sina;
        d_rot_coeffs_db(2,1) = sinc*cosb*cosa;
        d_rot_coeffs_db(0,2) = -cosb;
        d_rot_coeffs_db(1,2) = -sinb*sina;
        d_rot_coeffs_db(2,2) = -sinb*cosa;

        d_rot_coeffs_dc(0,0) = -sinc*cosb;
        d_rot_coeffs_dc(1,0) = -cosc*cosa - sinc*sinb*sina;
        d_rot_coeffs_dc(2,0) = cosc*sina - sinc*sinb*cosa;
        d_rot_coeffs_dc(0,1) = cosc*cosb;
        d_rot_coeffs_dc(1,1) = -sinc*cosa + cosc*sinb*sina;
        d_rot_coeffs_dc(2,1) = sinc*sina + cosc*sinb*cosa;
        d_rot_coeffs_dc(0,2) = (ScalarT)0.0;
        d_rot_coeffs_dc(1,2) = (ScalarT)0.0;
        d_rot_coeffs_dc(2,2) = (ScalarT)0.0;
    }

    template <typename ScalarT, typename CorrValueT = ScalarT>
    bool estimateDenseWarpFieldCombinedMetric3(const ConstVectorSetMatrixMap<ScalarT,3> &dst_p,
                                               const ConstVectorSetMatrixMap<ScalarT,3> &dst_n,
                                               const ConstVectorSetMatrixMap<ScalarT,3> &src_p,
                                               const CorrespondenceSet<CorrValueT> &correspondences,
                                               const std::vector<NeighborSet<ScalarT>> &regularization_neighborhoods,
                                               RigidTransformationSet<ScalarT,3> &transforms,
                                               ScalarT point_to_point_weight,
                                               ScalarT point_to_plane_weight,
                                               ScalarT stiffness_weight,
                                               ScalarT huber_boundary = (ScalarT)(1e-6),
                                               size_t max_gn_iter = 10,
                                               ScalarT gn_conv_tol = (ScalarT)1e-5,
                                               size_t max_cg_iter = 1000,
                                               ScalarT cg_conv_tol = (ScalarT)1e-5)
    {
        if (dst_p.cols() != dst_n.cols() || (point_to_point_weight == (ScalarT)0.0 && point_to_plane_weight == (ScalarT)0.0)) {
            transforms.resize(src_p.cols());
            transforms.setIdentity();
            return false;
        }

//        if (point_to_point_weight == (ScalarT)0.0) {
//            // Do point-to-plane
//            return estimateWarpFieldDensePointToPlane3D<ScalarT,CorrValueT>(dst_p, dst_n, src_p, correspondences, regularization_neighborhoods, transforms, stiffness_weight/point_to_plane_weight, max_iter, convergence_tol);
//        }
//
//        if (point_to_plane_weight == (ScalarT)0.0) {
//            // Do point-to-point
//            return estimateWarpFieldDensePointToPoint3D<ScalarT,CorrValueT>(dst_p, src_p, correspondences, regularization_neighborhoods, transforms, stiffness_weight/point_to_point_weight, max_iter, convergence_tol);
//        }

        const ScalarT point_to_point_weight_sqrt = std::sqrt(point_to_point_weight);
        const ScalarT point_to_plane_weight_sqrt = std::sqrt(point_to_plane_weight);
        const ScalarT stiffness_weight_sqrt = std::sqrt(stiffness_weight);
        const ScalarT gn_conv_tol_sq = gn_conv_tol*gn_conv_tol;

        // Compute number of equations and unknowns
        const size_t num_unknowns = 6*src_p.cols();
        const size_t num_data_term_equations = 4*correspondences.size();

        std::vector<size_t> reg_eq_ind(regularization_neighborhoods.size());
        size_t num_reg_arcs = 0;
        if (!regularization_neighborhoods.empty()) {
            reg_eq_ind[0] = 0;
            num_reg_arcs = std::max((size_t)0, regularization_neighborhoods[0].size() - 1);
        }
        for (size_t i = 1; i < regularization_neighborhoods.size(); i++) {
            reg_eq_ind[i] = reg_eq_ind[i-1] + 6*std::max((size_t)0, regularization_neighborhoods[i-1].size() - 1);
            num_reg_arcs += std::max((size_t)0, regularization_neighborhoods[i].size() - 1);
        }

        const size_t num_regularization_equations = 6*num_reg_arcs;
        const size_t num_equations = num_data_term_equations + num_regularization_equations;
        const size_t num_non_zeros = 6*num_data_term_equations + 2*num_regularization_equations;

        // Jacobian
        Eigen::SparseMatrix<ScalarT> At(num_unknowns,num_equations);
        At.reserve(num_non_zeros);
        // Values
        ScalarT * const values = At.valuePtr();
        // Outer pointers
        typename Eigen::SparseMatrix<ScalarT>::StorageIndex * const outer_ptr = At.outerIndexPtr();
#pragma omp parallel for
        for (size_t i = 0; i < num_data_term_equations + 1; i++) {
            outer_ptr[i] = 6*i;
        }
#pragma omp parallel for
        for (size_t i = 1; i < num_regularization_equations + 1; i++) {
            outer_ptr[num_data_term_equations + i] = 6*num_data_term_equations + 2*i;
        }
        // Inner indices
        typename Eigen::SparseMatrix<ScalarT>::StorageIndex * const inner_ind = At.innerIndexPtr();

        // Vector of (negative) residuals
        Eigen::Matrix<ScalarT,Eigen::Dynamic,1> b(num_equations);

        // Vector of unknowns (Euler angles and translation offsets per point)
        Eigen::Matrix<ScalarT,Eigen::Dynamic,1> tforms_vec(num_unknowns);
        tforms_vec.setZero();

        Eigen::SparseMatrix<ScalarT> AtA;
        Eigen::Matrix<ScalarT,Eigen::Dynamic,1> Atb;

        // Conjugate Gradient solver
//        Eigen::ConjugateGradient<Eigen::SparseMatrix<ScalarT>,Eigen::Lower|Eigen::Upper,Eigen::IncompleteCholesky<ScalarT,Eigen::Lower|Eigen::Upper>> solver;
//        Eigen::ConjugateGradient<Eigen::SparseMatrix<ScalarT>,Eigen::Lower|Eigen::Upper,Eigen::IdentityPreconditioner> solver;
        Eigen::ConjugateGradient<Eigen::SparseMatrix<ScalarT>,Eigen::Lower|Eigen::Upper,Eigen::DiagonalPreconditioner<ScalarT>> solver;
//        Eigen::ConjugateGradient<Eigen::SparseMatrix<ScalarT>,Eigen::Lower|Eigen::Upper,BlockDiagonalPreconditioner<ScalarT,6>> solver;
        solver.setMaxIterations(max_cg_iter);
        solver.setTolerance(cg_conv_tol);

        // Temporaries
        Eigen::Matrix<ScalarT,3,3> rot_coeffs, d_rot_coeffs_da, d_rot_coeffs_db, d_rot_coeffs_dc;
        Eigen::Matrix<ScalarT,3,1> trans_s, d_rot_da_s, d_rot_db_s, d_rot_dc_s;
        Eigen::Matrix<ScalarT,Eigen::Dynamic,1> delta;
        ScalarT weight, diff, d_sqrt_huber_loss, curr_delta_sq, max_delta_sq;
        size_t eq_ind, nz_ind;

        bool has_converged = false;
        size_t iter = 0;
        while (iter < max_gn_iter) {
            // Data term
#pragma omp parallel for shared (At, b) private (eq_ind, nz_ind, rot_coeffs, d_rot_coeffs_da, d_rot_coeffs_db, d_rot_coeffs_dc, trans_s, d_rot_da_s, d_rot_db_s, d_rot_dc_s)
            for (size_t i = 0; i < correspondences.size(); i++) {
                const auto d = dst_p.col(correspondences[i].indexInFirst);
                const auto n = dst_n.col(correspondences[i].indexInFirst);
                const auto s = src_p.col(correspondences[i].indexInSecond);
                const size_t offset = 6*correspondences[i].indexInSecond;

                computeRotationTerms(tforms_vec[offset], tforms_vec[offset + 1], tforms_vec[offset + 2], rot_coeffs, d_rot_coeffs_da, d_rot_coeffs_db, d_rot_coeffs_dc);
                const auto trans_coeffs = tforms_vec.template segment<3>(offset + 3);

                trans_s = rot_coeffs.transpose()*s + trans_coeffs - d;
                d_rot_da_s = d_rot_coeffs_da.transpose()*s;
                d_rot_db_s = d_rot_coeffs_db.transpose()*s;
                d_rot_dc_s = d_rot_coeffs_dc.transpose()*s;

                eq_ind = 4*i;
                nz_ind = 24*i;

                // Point to plane
                values[nz_ind] = (n.dot(d_rot_da_s))*point_to_plane_weight_sqrt;
                inner_ind[nz_ind++] = offset;
                values[nz_ind] = (n.dot(d_rot_db_s))*point_to_plane_weight_sqrt;
                inner_ind[nz_ind++] = offset + 1;
                values[nz_ind] = (n.dot(d_rot_dc_s))*point_to_plane_weight_sqrt;
                inner_ind[nz_ind++] = offset + 2;
                values[nz_ind] = n[0]*point_to_plane_weight_sqrt;
                inner_ind[nz_ind++] = offset + 3;
                values[nz_ind] = n[1]*point_to_plane_weight_sqrt;
                inner_ind[nz_ind++] = offset + 4;
                values[nz_ind] = n[2]*point_to_plane_weight_sqrt;
                inner_ind[nz_ind++] = offset + 5;
                b[eq_ind++] = -(n.dot(trans_s))*point_to_plane_weight_sqrt;

                // Point to point
                values[nz_ind] = d_rot_da_s[0]*point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset;
                values[nz_ind] = d_rot_db_s[0]*point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset + 1;
                values[nz_ind] = d_rot_dc_s[0]*point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset + 2;
                values[nz_ind] = point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset + 3;
                values[nz_ind] = (ScalarT)0.0;
                inner_ind[nz_ind++] = offset + 4;
                values[nz_ind] = (ScalarT)0.0;
                inner_ind[nz_ind++] = offset + 5;
                b[eq_ind++] = -(trans_s[0])*point_to_point_weight_sqrt;

                values[nz_ind] = d_rot_da_s[1]*point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset;
                values[nz_ind] = d_rot_db_s[1]*point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset + 1;
                values[nz_ind] = d_rot_dc_s[1]*point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset + 2;
                values[nz_ind] = (ScalarT)0.0;
                inner_ind[nz_ind++] = offset + 3;
                values[nz_ind] = point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset + 4;
                values[nz_ind] = (ScalarT)0.0;
                inner_ind[nz_ind++] = offset + 5;
                b[eq_ind++] = -(trans_s[1])*point_to_point_weight_sqrt;

                values[nz_ind] = d_rot_da_s[2]*point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset;
                values[nz_ind] = d_rot_db_s[2]*point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset + 1;
                values[nz_ind] = d_rot_dc_s[2]*point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset + 2;
                values[nz_ind] = (ScalarT)0.0;
                inner_ind[nz_ind++] = offset + 3;
                values[nz_ind] = (ScalarT)0.0;
                inner_ind[nz_ind++] = offset + 4;
                values[nz_ind] = point_to_point_weight_sqrt;
                inner_ind[nz_ind++] = offset + 5;
                b[eq_ind++] = -(trans_s[2])*point_to_point_weight_sqrt;
            }

            // Regularization term
#pragma omp parallel for shared (At, b) private (eq_ind, nz_ind, weight, diff, d_sqrt_huber_loss)
            for (size_t i = 0; i < regularization_neighborhoods.size(); i++) {
                eq_ind = num_data_term_equations + reg_eq_ind[i];
                nz_ind = 6*num_data_term_equations + 2*reg_eq_ind[i];

                for (size_t j = 1; j < regularization_neighborhoods[i].size(); j++) {
                    size_t s_offset = 6*regularization_neighborhoods[i][0].index;
                    size_t n_offset = 6*regularization_neighborhoods[i][j].index;
                    weight = stiffness_weight_sqrt*std::sqrt(regularization_neighborhoods[i][j].value);

                    if (n_offset < s_offset) std::swap(s_offset, n_offset);

                    diff = tforms_vec[s_offset + 0] - tforms_vec[n_offset + 0];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 1] - tforms_vec[n_offset + 1];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 1;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 1;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 2] - tforms_vec[n_offset + 2];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 2;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 2;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 3] - tforms_vec[n_offset + 3];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 3;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 3;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 4] - tforms_vec[n_offset + 4];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 4;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 4;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 5] - tforms_vec[n_offset + 5];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 5;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 5;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);
                }
            }

            // Solve linear system using CG
            AtA = At*At.transpose();
            Atb = At*b;

//            solver.compute(AtA);
            if (iter == 0) solver.analyzePattern(AtA);
            solver.factorize(AtA);
            delta = solver.solve(Atb);
            tforms_vec += delta;

            iter++;

            // Check for convergence
            max_delta_sq = (ScalarT)0.0;
#pragma omp parallel for private (curr_delta_sq) reduction (max: max_delta_sq)
            for (size_t i = 0; i < src_p.cols(); i++) {
                curr_delta_sq = delta.template segment<6>(6*i).squaredNorm();
                if (curr_delta_sq > max_delta_sq) max_delta_sq = curr_delta_sq;
            }

//            std::cout << iter << ": " << std::sqrt(max_delta_sq) << std::endl;

            if (max_delta_sq < gn_conv_tol_sq) {
                has_converged = true;
                break;
            }
        }

        // Convert to output format
        transforms.resize(src_p.cols());
#pragma omp parallel for
        for (size_t i = 0; i < transforms.size(); i++) {
            transforms[i].linear() = (Eigen::AngleAxis<ScalarT>(tforms_vec[6*i + 2],Eigen::Matrix<ScalarT,3,1>::UnitZ()) *
                                      Eigen::AngleAxis<ScalarT>(tforms_vec[6*i + 1],Eigen::Matrix<ScalarT,3,1>::UnitY()) *
                                      Eigen::AngleAxis<ScalarT>(tforms_vec[6*i + 0],Eigen::Matrix<ScalarT,3,1>::UnitX())).matrix();
            transforms[i].linear() = transforms[i].rotation();
            transforms[i].translation() = tforms_vec.template segment<3>(6*i + 3);
        }

        return has_converged;
    }

    template <typename ScalarT>
    bool estimateSparseWarpFieldCombinedMetric3(const ConstVectorSetMatrixMap<ScalarT,3> &dst_p,
                                                const ConstVectorSetMatrixMap<ScalarT,3> &dst_n,
                                                const ConstVectorSetMatrixMap<ScalarT,3> &src_p,
                                                size_t num_ctrl_points,
                                                const std::vector<NeighborSet<ScalarT>> &src_to_ctrl_neighborhoods,
                                                const std::vector<NeighborSet<ScalarT>> &regularization_neighborhoods,
                                                RigidTransformationSet<ScalarT,3> &transforms,
                                                ScalarT point_to_point_weight,
                                                ScalarT point_to_plane_weight,
                                                ScalarT stiffness_weight,
                                                ScalarT huber_boundary = (ScalarT)(1e-6),
                                                size_t max_gn_iter = 10,
                                                ScalarT gn_conv_tol = (ScalarT)1e-5,
                                                size_t max_cg_iter = 1000,
                                                ScalarT cg_conv_tol = (ScalarT)1e-5)
    {
        if (dst_p.cols() != dst_n.cols() || dst_p.cols() != src_p.cols() || src_to_ctrl_neighborhoods.size() != src_p.cols() || (point_to_point_weight == (ScalarT)0.0 && point_to_plane_weight == (ScalarT)0.0)) {
            transforms.resize(num_ctrl_points);
            transforms.setIdentity();
            return false;
        }

//        if (point_to_point_weight == (ScalarT)0.0) {
//            // Do point-to-plane
//            return estimateWarpFieldSparsePointToPlane3D<ScalarT>(dst_p, dst_n, src_p, src_to_ctrl_neighborhoods, ctrl_regularization_neighborhoods, ctrl_transforms, stiffness_weight/point_to_plane_weight, max_iter, convergence_tol);
//        }
//
//        if (point_to_plane_weight == (ScalarT)0.0) {
//            // Do point-to-point
//            return estimateWarpFieldSparsePointToPoint3D<ScalarT>(dst_p, src_p, src_to_ctrl_neighborhoods, ctrl_regularization_neighborhoods, ctrl_transforms, stiffness_weight/point_to_point_weight, max_iter, convergence_tol);
//        }

        const ScalarT point_to_point_weight_sqrt = std::sqrt(point_to_point_weight);
        const ScalarT point_to_plane_weight_sqrt = std::sqrt(point_to_plane_weight);
        const ScalarT stiffness_weight_sqrt = std::sqrt(stiffness_weight);
        const ScalarT gn_conv_tol_sq = gn_conv_tol*gn_conv_tol;

        // Compute number of equations and unknowns
        const size_t num_unknowns = 6*num_ctrl_points;
        const size_t num_data_term_equations = 4*src_p.cols();

        std::vector<size_t> reg_eq_ind(regularization_neighborhoods.size());
        size_t num_reg_arcs = 0;
        if (!regularization_neighborhoods.empty()) {
            reg_eq_ind[0] = 0;
            num_reg_arcs = std::max((size_t)0, regularization_neighborhoods[0].size() - 1);
        }
        for (size_t i = 1; i < regularization_neighborhoods.size(); i++) {
            reg_eq_ind[i] = reg_eq_ind[i-1] + 6*std::max((size_t)0, regularization_neighborhoods[i-1].size() - 1);
            num_reg_arcs += std::max((size_t)0, regularization_neighborhoods[i].size() - 1);
        }

        std::vector<size_t> nz_coeff_ind(src_to_ctrl_neighborhoods.size() + 1);
        nz_coeff_ind[0] = 0;
        for (size_t i = 1; i < src_to_ctrl_neighborhoods.size() + 1; i++) {
            nz_coeff_ind[i] = nz_coeff_ind[i-1] + 24*src_to_ctrl_neighborhoods[i-1].size();
        }

        const size_t num_regularization_equations = 6*num_reg_arcs;
        const size_t num_equations = num_data_term_equations + num_regularization_equations;
        const size_t num_non_zeros = nz_coeff_ind.back() + 2*num_regularization_equations;

        // Jacobian
        Eigen::SparseMatrix<ScalarT> At(num_unknowns,num_equations);
        At.reserve(num_non_zeros);
        // Values
        ScalarT * const values = At.valuePtr();
        // Outer pointers
        std::vector<NeighborSet<ScalarT>> src_to_ctrl_sorted(src_to_ctrl_neighborhoods);
        typename Eigen::SparseMatrix<ScalarT>::StorageIndex * const outer_ptr = At.outerIndexPtr();
#pragma omp parallel for
        for (size_t i = 0; i < src_to_ctrl_neighborhoods.size(); i++) {
            std::sort(src_to_ctrl_sorted[i].begin(), src_to_ctrl_sorted[i].end(), typename Neighbor<ScalarT>::IndexLessComparator());
            const size_t offset = 6*src_to_ctrl_neighborhoods[i].size();
            outer_ptr[4*i] = nz_coeff_ind[i];
            outer_ptr[4*i + 1] = nz_coeff_ind[i] + offset;
            outer_ptr[4*i + 2] = nz_coeff_ind[i] + offset + offset;
            outer_ptr[4*i + 3] = nz_coeff_ind[i] + offset + offset + offset;
        }
        outer_ptr[num_data_term_equations] = nz_coeff_ind.back();
#pragma omp parallel for
        for (size_t i = 1; i < num_regularization_equations + 1; i++) {
            outer_ptr[num_data_term_equations + i] = nz_coeff_ind.back() + 2*i;
        }
        // Inner indices
        typename Eigen::SparseMatrix<ScalarT>::StorageIndex * const inner_ind = At.innerIndexPtr();

        // Vector of (negative) residuals
        Eigen::Matrix<ScalarT,Eigen::Dynamic,1> b(num_equations);

        // Vector of unknowns (Euler angles and translation offsets per point)
        Eigen::Matrix<ScalarT,Eigen::Dynamic,1> tforms_vec(num_unknowns);
        tforms_vec.setZero();

        // Sum of control point influences
        std::vector<ScalarT> total_weight(src_to_ctrl_sorted.size());
#pragma omp parallel for shared (total_weight)
        for (size_t i = 0; i < src_to_ctrl_sorted.size(); i++) {
            total_weight[i] = (ScalarT)0.0;
            for (size_t j = 0; j < src_to_ctrl_sorted[i].size(); j++) {
                total_weight[i] += src_to_ctrl_sorted[i][j].value;
            }
        }

        Eigen::SparseMatrix<ScalarT> AtA;
        Eigen::Matrix<ScalarT,Eigen::Dynamic,1> Atb;

        // Conjugate Gradient solver
//        Eigen::ConjugateGradient<Eigen::SparseMatrix<ScalarT>,Eigen::Lower|Eigen::Upper,Eigen::IncompleteCholesky<ScalarT,Eigen::Lower|Eigen::Upper>> solver;
//        Eigen::ConjugateGradient<Eigen::SparseMatrix<ScalarT>,Eigen::Lower|Eigen::Upper,Eigen::IdentityPreconditioner> solver;
        Eigen::ConjugateGradient<Eigen::SparseMatrix<ScalarT>,Eigen::Lower|Eigen::Upper,Eigen::DiagonalPreconditioner<ScalarT>> solver;
//        Eigen::ConjugateGradient<Eigen::SparseMatrix<ScalarT>,Eigen::Lower|Eigen::Upper,BlockDiagonalPreconditioner<ScalarT,6>> solver;
        solver.setMaxIterations(max_cg_iter);
        solver.setTolerance(cg_conv_tol);

        // Temporaries
        Eigen::Matrix<ScalarT,3,3> rot_coeffs, d_rot_coeffs_da, d_rot_coeffs_db, d_rot_coeffs_dc;
        Eigen::Matrix<ScalarT,3,1> trans_s, d_rot_da_s, d_rot_db_s, d_rot_dc_s;
        Eigen::Matrix<ScalarT,3,1> angles_curr, trans_curr;
        Eigen::Matrix<ScalarT,Eigen::Dynamic,1> delta;
        ScalarT weight, diff, d_sqrt_huber_loss, curr_delta_sq, max_delta_sq;
        size_t eq_ind, nz_ind;

        bool has_converged = false;
        size_t iter = 0;
        while (iter < max_gn_iter) {
            // Data term
#pragma omp parallel for shared (At, b) private (eq_ind, nz_ind, weight, angles_curr, trans_curr, rot_coeffs, d_rot_coeffs_da, d_rot_coeffs_db, d_rot_coeffs_dc, trans_s, d_rot_da_s, d_rot_db_s, d_rot_dc_s)
            for (size_t i = 0; i < src_p.cols(); i++) {
                // Compute weighted influence from control nodes
                angles_curr.setZero();
                trans_curr.setZero();
                for (size_t j = 0; j < src_to_ctrl_sorted[i].size(); j++) {
                    const size_t offset = 6*src_to_ctrl_sorted[i][j].index;
                    angles_curr += src_to_ctrl_sorted[i][j].value*tforms_vec.template segment<3>(offset);
                    trans_curr += src_to_ctrl_sorted[i][j].value*tforms_vec.template segment<3>(offset + 3);
                }
                if (total_weight[i] != (ScalarT)0.0) {
                    weight = (ScalarT)(1.0)/total_weight[i];
                    angles_curr *= weight;
                    trans_curr *= weight;
                }

                const auto d = dst_p.col(i);
                const auto n = dst_n.col(i);
                const auto s = src_p.col(i);

                computeRotationTerms(angles_curr[0], angles_curr[1], angles_curr[2], rot_coeffs, d_rot_coeffs_da, d_rot_coeffs_db, d_rot_coeffs_dc);

                trans_s = rot_coeffs.transpose()*s + trans_curr - d;
                d_rot_da_s = d_rot_coeffs_da.transpose()*s;
                d_rot_db_s = d_rot_coeffs_db.transpose()*s;
                d_rot_dc_s = d_rot_coeffs_dc.transpose()*s;

                eq_ind = 4*i;

                for (size_t j = 0; j < src_to_ctrl_sorted[i].size(); j++) {
                    const size_t offset = 6*src_to_ctrl_sorted[i][j].index;
                    weight = (total_weight[i] == (ScalarT)0.0) ? (ScalarT)0.0 : src_to_ctrl_sorted[i][j].value/total_weight[i];
//                    weight = src_to_ctrl_sorted[i][j].value/total_weight[i];

                    // Point to plane
                    nz_ind = outer_ptr[eq_ind] + 6*j;
                    values[nz_ind] = (n.dot(d_rot_da_s))*weight*point_to_plane_weight_sqrt;
                    inner_ind[nz_ind++] = offset;
                    values[nz_ind] = (n.dot(d_rot_db_s))*weight*point_to_plane_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 1;
                    values[nz_ind] = (n.dot(d_rot_dc_s))*weight*point_to_plane_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 2;
                    values[nz_ind] = n[0]*weight*point_to_plane_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 3;
                    values[nz_ind] = n[1]*weight*point_to_plane_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 4;
                    values[nz_ind] = n[2]*weight*point_to_plane_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 5;

                    // Point to point
                    nz_ind = outer_ptr[eq_ind + 1] + 6*j;
                    values[nz_ind] = d_rot_da_s[0]*weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset;
                    values[nz_ind] = d_rot_db_s[0]*weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 1;
                    values[nz_ind] = d_rot_dc_s[0]*weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 2;
                    values[nz_ind] = weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 3;
                    values[nz_ind] = (ScalarT)0.0;
                    inner_ind[nz_ind++] = offset + 4;
                    values[nz_ind] = (ScalarT)0.0;
                    inner_ind[nz_ind++] = offset + 5;

                    nz_ind = outer_ptr[eq_ind + 2] + 6*j;
                    values[nz_ind] = d_rot_da_s[1]*weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset;
                    values[nz_ind] = d_rot_db_s[1]*weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 1;
                    values[nz_ind] = d_rot_dc_s[1]*weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 2;
                    values[nz_ind] = (ScalarT)0.0;
                    inner_ind[nz_ind++] = offset + 3;
                    values[nz_ind] = weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 4;
                    values[nz_ind] = (ScalarT)0.0;
                    inner_ind[nz_ind++] = offset + 5;

                    nz_ind = outer_ptr[eq_ind + 3] + 6*j;
                    values[nz_ind] = d_rot_da_s[2]*weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset;
                    values[nz_ind] = d_rot_db_s[2]*weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 1;
                    values[nz_ind] = d_rot_dc_s[2]*weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 2;
                    values[nz_ind] = (ScalarT)0.0;
                    inner_ind[nz_ind++] = offset + 3;
                    values[nz_ind] = (ScalarT)0.0;
                    inner_ind[nz_ind++] = offset + 4;
                    values[nz_ind] = weight*point_to_point_weight_sqrt;
                    inner_ind[nz_ind++] = offset + 5;
                }

                weight = (total_weight[i] == (ScalarT)0.0) ? (ScalarT)0.0 : (ScalarT)1.0;
//                weight = (ScalarT)1.0;

                // Point to plane
                b[eq_ind] = -(n.dot(trans_s))*weight*point_to_plane_weight_sqrt;
                // Point to point
                b[eq_ind + 1] = -(trans_s[0])*weight*point_to_point_weight_sqrt;
                b[eq_ind + 2] = -(trans_s[1])*weight*point_to_point_weight_sqrt;
                b[eq_ind + 3] = -(trans_s[2])*weight*point_to_point_weight_sqrt;
            }

            // Regularization term
#pragma omp parallel for shared (At, b) private (eq_ind, nz_ind, weight, diff, d_sqrt_huber_loss)
            for (size_t i = 0; i < regularization_neighborhoods.size(); i++) {
                eq_ind = num_data_term_equations + reg_eq_ind[i];
                nz_ind = nz_coeff_ind.back() + 2*reg_eq_ind[i];

                for (size_t j = 1; j < regularization_neighborhoods[i].size(); j++) {
                    size_t s_offset = 6*regularization_neighborhoods[i][0].index;
                    size_t n_offset = 6*regularization_neighborhoods[i][j].index;
                    weight = stiffness_weight_sqrt*std::sqrt(regularization_neighborhoods[i][j].value);

                    if (n_offset < s_offset) std::swap(s_offset, n_offset);

                    diff = tforms_vec[s_offset + 0] - tforms_vec[n_offset + 0];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 1] - tforms_vec[n_offset + 1];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 1;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 1;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 2] - tforms_vec[n_offset + 2];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 2;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 2;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 3] - tforms_vec[n_offset + 3];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 3;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 3;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 4] - tforms_vec[n_offset + 4];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 4;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 4;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);

                    diff = tforms_vec[s_offset + 5] - tforms_vec[n_offset + 5];
                    d_sqrt_huber_loss = weight*sqrtHuberLossDerivative<ScalarT>(diff, huber_boundary);
                    values[nz_ind] = d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = s_offset + 5;
                    values[nz_ind] = -d_sqrt_huber_loss;
                    inner_ind[nz_ind++] = n_offset + 5;
                    b[eq_ind++] = -weight*sqrtHuberLoss<ScalarT>(diff, huber_boundary);
                }
            }

//            Eigen::SparseMatrix<double> AtA = (At*At.transpose()).template cast<double>();
//            Eigen::VectorXd Atb = (At*b).template cast<double>();
//
//            ScalarT shift = std::sqrt(std::numeric_limits<ScalarT>::epsilon());
//            Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> solver;
//            solver.compute(AtA);
//            while (solver.info() != Eigen::Success) {
//                solver.setShift(shift);
//                solver.compute(AtA);
//                shift *= 5.0;
//            }
//            delta = solver.solve(Atb).template cast<ScalarT>();
//            tforms_vec += delta;

            // Solve linear system using CG
            AtA = At*At.transpose();
            Atb = At*b;

//            solver.compute(AtA);
            if (iter == 0) solver.analyzePattern(AtA);
            solver.factorize(AtA);
            delta = solver.solve(Atb);
            tforms_vec += delta;

            iter++;

            // Check for convergence
            max_delta_sq = (ScalarT)0.0;
#pragma omp parallel for private (curr_delta_sq) reduction (max: max_delta_sq)
            for (size_t i = 0; i < num_ctrl_points; i++) {
                curr_delta_sq = delta.template segment<6>(6*i).squaredNorm();
                if (curr_delta_sq > max_delta_sq) max_delta_sq = curr_delta_sq;
            }

//            std::cout << iter << ": " << std::sqrt(max_delta_sq) << std::endl;

            if (max_delta_sq < gn_conv_tol_sq) {
                has_converged = true;
                break;
            }

        }

        // Convert to output format
        transforms.resize(num_ctrl_points);
#pragma omp parallel for
        for (size_t i = 0; i < transforms.size(); i++) {
            transforms[i].linear() = (Eigen::AngleAxis<ScalarT>(tforms_vec[6*i + 2],Eigen::Matrix<ScalarT,3,1>::UnitZ()) *
                                      Eigen::AngleAxis<ScalarT>(tforms_vec[6*i + 1],Eigen::Matrix<ScalarT,3,1>::UnitY()) *
                                      Eigen::AngleAxis<ScalarT>(tforms_vec[6*i + 0],Eigen::Matrix<ScalarT,3,1>::UnitX())).matrix();
            transforms[i].linear() = transforms[i].rotation();
            transforms[i].translation() = tforms_vec.template segment<3>(6*i + 3);
        }

        return has_converged;
    }

    template <typename ScalarT, typename CorrValueT = ScalarT>
    bool estimateSparseWarpFieldCombinedMetric3(const ConstVectorSetMatrixMap<ScalarT,3> &dst_p,
                                                const ConstVectorSetMatrixMap<ScalarT,3> &dst_n,
                                                const ConstVectorSetMatrixMap<ScalarT,3> &src_p,
                                                const CorrespondenceSet<CorrValueT> &corr,
                                                size_t num_ctrl_points,
                                                const std::vector<NeighborSet<ScalarT>> &src_to_ctrl_neighborhoods,
                                                const std::vector<NeighborSet<ScalarT>> &regularization_neighborhoods,
                                                RigidTransformationSet<ScalarT,3> &transforms,
                                                ScalarT point_to_point_weight,
                                                ScalarT point_to_plane_weight,
                                                ScalarT stiffness_weight,
                                                ScalarT huber_boundary = (ScalarT)(1e-6),
                                                size_t max_gn_iter = 10,
                                                ScalarT gn_conv_tol = (ScalarT)1e-5,
                                                size_t max_cg_iter = 1000,
                                                ScalarT cg_conv_tol = (ScalarT)1e-5)
    {
        VectorSet<ScalarT,3> dst_p_corr(3, corr.size());
        VectorSet<ScalarT,3> dst_n_corr(3, corr.size());
        VectorSet<ScalarT,3> src_p_corr(3, corr.size());
        std::vector<NeighborSet<ScalarT>> src_to_ctrl_neighborhoods_corr(corr.size());
#pragma omp parallel for
        for (size_t i = 0; i < corr.size(); i++) {
            dst_p_corr.col(i) = dst_p.col(corr[i].indexInFirst);
            dst_n_corr.col(i) = dst_n.col(corr[i].indexInFirst);
            src_p_corr.col(i) = src_p.col(corr[i].indexInSecond);
            src_to_ctrl_neighborhoods_corr[i] = src_to_ctrl_neighborhoods[corr[i].indexInSecond];
        }
        return estimateSparseWarpFieldCombinedMetric3<ScalarT>(dst_p_corr, dst_n_corr, src_p_corr, num_ctrl_points,
                                                               src_to_ctrl_neighborhoods_corr,
                                                               regularization_neighborhoods,
                                                               transforms,
                                                               point_to_point_weight, point_to_plane_weight,
                                                               stiffness_weight, huber_boundary,
                                                               max_gn_iter, gn_conv_tol,
                                                               max_cg_iter, cg_conv_tol);
    }
}


#include "gaussian.h"
#include "bvnl.h"
#include <algorithm>
#include <boost/math/special_functions/erf.hpp>

const I epsilon_interval(-std::numeric_limits<double>::epsilon(),
						 std::numeric_limits<double>::epsilon());

// Add a certain number of epsilons as error to an interval
I nudge(const I& x, int epsilons) {
  I eps = (double)epsilons * epsilon_interval;
  return x * (1.0 + eps) + eps;
}

I erf(const I& x) {
	// Built in erf should have an error of <= epsilon.
	// We overestimate with 10 epsilons.
	return nudge(I(erf(x.lower()), erf(x.upper())), 10);
}

I erf_inv(const I& x) {
	// erf_inv has an error of <= 2 epsilons according to Boost docs.
	// We overestimate with 20 epsilons.
	return nudge(I(boost::math::erf_inv(x.lower()),
				   boost::math::erf_inv(x.upper())), 20);
}

const I sqrt2 = sqrt(I(2.0));

I Phi(const I& x) {
  return (1.0 + erf(x/sqrt2))/2.0;
}

I Phi_inv(const I& x) {
  return sqrt2*erf_inv(2.0*x-1.0);
}

I bvnl(const I& dh, const I& dk, const I& r) {
  if (empty(dh) || empty(dk) || empty(r)) return I::empty();
  // Assumes Fact: bvnl is monotone in all three parameters
  I ans = I(bvnl_down(dh.lower(), dk.lower(), r.lower()),
			bvnl_up(dh.upper(), dk.upper(), r.upper()));
  return ans;
}

I Gamma(const I& q1, const I& q2, const I& rho) {
  return bvnl(Phi_inv(q1), Phi_inv(q2), rho);
}

double Gamma_up(double q1, double q2, double rho) {
	// Assumes Fact: Gamma is monotone in all three parameters
	return bvnl_up(Phi_inv(q1).upper(), Phi_inv(q2).upper(), rho);
}

// Naive implementation of Lambda_{\trho}(r1, r2)
// Has unnecessary loss of precision due to repeated occurrences of r1 and r2
I Lambda_naive(const I& r1, const I &r2, const I& trho) {
  return 2.0*Gamma((1.0-r1)/2.0, (1.0-r2)/2.0, trho) + (r1+r2)/2.0;
}

double Lambda_up(double r1, double r2, double trho) {
	// TODO assumes something about error
	return 2.0*Gamma_up((1.0-r1)/2.0 + 1e-15, (1.0-r2)/2.0 + 1e-15, trho) + (r1+r2)/2.0 + 1e-15;
}

// Upper bound on Lambda which gives a good approximation for trho close to 0.
// Precondition: trho.upper() >= 0
double Lambda_up_near_zero(const I& r1, const I& r2, const I& trho) {
	// Assumes Lemma 2.7, which implies Lambda_trho(r1, r2) <= (1+r1*r2)/2 + 4*|trho|
	return ((1.0+r1*r2) / 2.0 + 4.0 * trho).upper();
}

// The "g" function from Lemma 5.5 of the paper
I Lambda_g(const I& r, const I& trho) {
  return 1.0 - 2.0*Phi(Phi_inv((1.0-r)/2.0) / trho);
}

// More accurate implementation of Lambda_{\trho}(r1, r2).
// Uses Lemma 5.5 of paper which characterizes the extreme points of
// Lambda_{\trho}(I_1, I_2).  In fact for performance reasons we only
// use it for the upper bound, which is what we need a good estimate
// on in order to get a good lower bound on alpha.  For the lower
// bound on Lambda we just use the naive bound.
I Lambda_precise(const I& r1, const I &r2, const I& trho) {
  I ans = Lambda_naive(r1, r2, trho);

  double r1_lo = r1.lower(), r1_hi = r1.upper();
  double r2_lo = r2.lower(), r2_hi = r2.upper();

  // The four combinations of extreme points for r1, r2.
  // Assumes Fact: Lambda is monotone in trho
  double ub = std::max(std::max(Lambda_up(r1_lo, r2_lo, trho.upper()),
								Lambda_up(r1_lo, r2_hi, trho.upper())),
					   std::max(Lambda_up(r1_hi, r2_lo, trho.upper()),
								Lambda_up(r1_hi, r2_hi, trho.upper())));

  if (posgt(trho, 0.0)) {
	  // When trho is (possibly) positive, Lambda is convex and there
	  // are five more possibilities for the upper bound.
	  I z;

	  // r1 at extreme point, r2 = g(r1)
	  z = hull(z, Lambda_naive(r1_lo,
							   intersect(r2, Lambda_g(r1_lo, trho.upper())),
							   trho.upper()));
	  z = hull(z, Lambda_naive(r1_hi,
							   intersect(r2, Lambda_g(r1_hi, trho.upper())),
							   trho.upper()));

	  // r2 at extreme point, r1 = g(r2)
	  z = hull(z, Lambda_naive(intersect(r1, Lambda_g(r2_lo, trho.upper())),
							   r2_lo,
							   trho.upper()));
	  z = hull(z, Lambda_naive(intersect(r1, Lambda_g(r2_hi, trho.upper())),
							   r2_hi,
							   trho.upper()));

	  // (0, 0)
	  if (poseq(r1, 0.0) && poseq(r2, 0.0))
		  z = hull(z, Lambda_up(0.0, 0.0, trho.upper()));

	  // If trho is close to zero, the computation of the "g" function
	  // of Lemma 5.5 is quite unstable and sometimes gives poor
	  // bounds.  To safeguard against these cases we also use the
	  // upper bound provided by Lemma 2.7 which gives good bounds for
	  // trho close to 0.
	  ub = std::max(ub, std::min(z.upper(), Lambda_up_near_zero(r1, r2, trho)));
  }

  return intersect(ans, I(0.0, std::min(ub, 1.0)));
}

I Lambda(const I& r1, const I& r2, const I& trho) {
	return Lambda_precise(r1, r2, trho);
}


#ifdef _DEBUG
#include "../../../library/src/debug_template.hpp"
#define DMP(...) dump(#__VA_ARGS__, __VA_ARGS__)
#else
#define DMP(...) ((void)0)
#endif

#include <cassert>
#include <cstdio>
#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <set>
#include <map>
#include <unordered_map>
#include <queue>
#include <numeric>
#include <algorithm>
#include <bitset>
#include <functional>

#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;

using namespace std;
using lint = long long;
constexpr int INF = 1010101010;
constexpr lint LINF = 1LL << 60;

struct init {
    init() {
        cin.tie(nullptr);
        ios::sync_with_stdio(false);
        cout << fixed << setprecision(10);
    }
} init_;

template<class T, class F>
T bisearch(T OK, T NG, F f) {
    T ok = OK;
    T ng = NG;

    while (abs(ok - ng) > 1) {
        auto mid = (ok + ng) / 2;

        if (f(mid)) ok = mid;
        else ng = mid;
    }

    return ok;
}

int main() {

    string X;
    lint M;
    cin >> X >> M;

    if (X.size() == 1) {
        cout << (X[0] - '0' <= M) << '\n';
        return 0;
    }

    reverse(X.begin(), X.end());

    auto check = [&](const lint &k) {
        mp::cpp_int now = 0;
        mp::cpp_int x = 1;
        for (const auto &c : X) {
            now += (c - '0') * x;
            x *= k;
        }
        return now <= M;
    };

    lint d = *max_element(X.begin(), X.end()) - '0';
    cout << bisearch(d, M + 1, check) - d << '\n';

    return 0;
}


#pragma once

#include <boost/multi_array.hpp>
#include <cmath>
#include <iostream>
#include <vector>

// ------------------------------------------------------------
#include "base/hash_specializations.hpp"
#include "laguerren_impl.hpp"

template <typename NUMERIC>
class LaguerreNW
{
 public:
  typedef NUMERIC numeric_t;

 public:
  LaguerreNW(int K)
      : Y_(K + 1)
      , K_(K)
  {
  }

  void compute(const std::vector<numeric_t> &x);
  void compute(const numeric_t *x, unsigned int n);

  unsigned int get_npoints() const { return Y_[0].shape()[1]; }

 public:
  typedef boost::multi_array<numeric_t, 2> array_t;

 public:
  const NUMERIC *get(unsigned int k, unsigned int alpha) const;
  void info() const;

 private:
  std::vector<array_t> Y_;
  unsigned int K_;
};

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreNW<NUMERIC>::compute(const std::vector<numeric_t> &x)
{
  compute(x.data(), x.size());
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreNW<NUMERIC>::compute(const numeric_t *x, unsigned int n)
{
  // L_n-1
  std::vector<numeric_t> Lnm1(n);
  // L_n-2
  std::vector<numeric_t> Lnm2(n);

  for (unsigned int alpha = 0; alpha <= K_; ++alpha) {
    Y_[alpha].resize(boost::extents[K_ / 2 + 1][n]);
// init
#pragma omp parallel for
    for (size_t xi = 0; xi < n; ++xi) {
      numeric_t expw = ::math::exp(-0.5 * x[xi]);
      Y_[alpha][0][xi] = boost::math::laguerren(0, alpha, x[xi]) * expw;
      Y_[alpha][1][xi] = boost::math::laguerren(1, alpha, x[xi]) * expw;
    }

    for (unsigned int k = 2; k <= K_ / 2; ++k) {
#pragma omp parallel for
      for (size_t xi = 0; xi < n; ++xi) {
        Y_[alpha][k][xi] = boost::math::laguerren_next(
            k - 1, alpha, x[xi], Y_[alpha][k - 1][xi], Y_[alpha][k - 2][xi]);
      }
    }
  }
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
const NUMERIC *
LaguerreNW<NUMERIC>::get(unsigned int k, unsigned int alpha) const
{
  assert(alpha < Y_.size());
  assert(k < Y_[alpha].shape()[0]);
  return Y_[alpha][k].origin();
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreNW<NUMERIC>::info() const
{
  unsigned long long int nentries = 0;
  for (unsigned int alpha = 0; alpha < Y_.size(); ++alpha) {
    nentries += Y_[alpha].shape()[0] * Y_[alpha].shape()[1];
  }

  std::cout << " LaguerreNW uses " << nentries * sizeof(NUMERIC) / 1e6 << " MB" << std::endl;
}


// Wheel-NeuralNetwork.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include <iostream>
#include <Eigen/Dense>
#include "NeuralNetwork.h"
#include "CSVParser.h"
#define PRINT(x) std::cout << x << std::endl
int main()
{
	DataPreprocessing::CSVParser data_parser("data.csv");
	auto data = data_parser.GetParsedResult();

	//Optimal layers for XOR problem: 2 3 2

	std::vector<Layer> layers;
	layers.emplace_back(2);
	layers.emplace_back(4);
	//layers.emplace_back(2);

	layers.emplace_back(1);

	//std::vector<float> inputs{1,1};
	//std::vector<float> target{ 0 };
	NeuralNetwork nn(layers);
	srand((unsigned int)time(NULL));
	for (int i = 0; i < 50000; i++) {
		std::vector<float> inputs;
		std::vector<float> target;
		int x2 = ((double)rand() / (RAND_MAX)) > 0.5 ? 1 : 0;
		int x1 = ((double)rand() / (RAND_MAX)) > 0.5 ? 1 : 0;

		int y = 1;

		//XOR
		if (x1 == x2)
			y = 1;
		else
		{
			y = 0;
		}

		////OR
		//if (x1 == 1 || x2 == 1)
		//	y = 1;
		//else
		//{
		//	y = 0;
		//}

		////AND
		//if (x1 == 1 && x2 == 1)
		//	y = 1;
		//else
		//{
		//	y = 0;
		//}

		/*PRINT("Inputs:");
		PRINT(x1);
		PRINT(x2);
		PRINT("LABEL");
		PRINT(y);*/
		inputs.emplace_back(x1);
		inputs.emplace_back(x2);

		target.emplace_back(y);

		nn.SetInput(inputs);
		nn.Train(inputs, target);
	}

	/*nn.SetInput(inputs);
	nn.Train(inputs, target);*/

	/*for (int j = 0; j < 300; j++) {
		for (int i = 1; i < data.size(); ++i)
		{
			std::vector<float> inputs;
			std::vector<float> target;
			inputs.emplace_back(std::atof(data[i][0].c_str()));
			inputs.emplace_back(std::atof(data[i][1].c_str()));

			int label = std::atoi(data[i][2].c_str());
			target.emplace_back(label);
			std::cout << "training... " << std::endl;
			nn.SetInput(inputs);
			nn.Train(inputs, target);

		}
	}*/

	std::vector<std::vector<float>> all_data_to_predict;
	all_data_to_predict.emplace_back(std::vector<float>{ 0, 1 });
	all_data_to_predict.emplace_back(std::vector<float>{ 1, 0 });
	all_data_to_predict.emplace_back(std::vector<float>{ 1, 1 });
	all_data_to_predict.emplace_back(std::vector<float>{ 0, 0 });

	for (int i = 0; i < all_data_to_predict.size(); ++i)
	{
		std::cout << "prediction: " << nn.predict(all_data_to_predict[i])[0] << std::endl;
	}
}


#include <boost/random.hpp>
#include <boost/random/random_device.hpp>

int
main ()
{
  boost::random::random_device dev;
  boost::random::uniform_int_distribution dist (10, 20);

  for (int i (0); i != 100; ++i)
  {
    if (dist (dev) < 10 || dist (dev) > 20)
      return 1;
  }

  return 0;
}


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.

#include <boost/hana.hpp>

#include <cstddef>
#include <tuple>
#include <type_traits>
#include <utility>
namespace hana = boost::hana;
using namespace boost::hana::literals;

template <typename Tuples, std::size_t ...i, std::size_t ...j,
  typename Res = std::tuple<
    std::tuple_element_t<j, std::tuple_element_t<i,
      std::remove_reference_t<Tuples>>>...
  >
>
constexpr Res tuple_cat_impl(Tuples&& tuples,
                             std::index_sequence<i...>,
                             std::index_sequence<j...>)
{
  return Res{
      std::get<j>(std::get<i>(std::forward<Tuples>(tuples)))...
  };
}

template <typename ...Tuples>
constexpr auto tuple_cat(Tuples&& ...tuples) {
  constexpr std::size_t N = sizeof...(Tuples);
  hana::tuple<Tuples&&...> xs{std::forward<Tuples>(tuples)...};

  constexpr auto indices = hana::to<hana::tuple_tag>(hana::range_c<int, 0, N>);
  auto inner = hana::fill(tuples, indices)...;
  auto outer = hana::make_range(0_c, hana::length(tuples))...;

  return tuple_cat_impl(
      std::move(xs),
      hana::to<hana::ext::std::integer_sequence_tag<std::size_t>>(inner),
      hana::to<hana::ext::std::integer_sequence_tag<std::size_t>>(outer)
  );
}

int main() { }

// constexpr std::size_t N = sizeof...(Tuples);
// hana::_tuple<Tuples&&...> xs{std::forward<Tuples>(tuples)...};
// auto inner = hana::flatten(hana::zip.with(hana::fill, xs, hana::to<hana::Tuple>(hana::range_c<int, 0, N>)));
// auto outer = flatten(tuple(to<Tuple>(range(0_c, length(tuples)))...));
// return tuple_cat_impl<Res>(
//     std::move(xs),
//     to<ext::std::IntegerSequence<std::size_t>>(inner),
//     to<ext::std::IntegerSequence<std::size_t>>(outer)
// );


/*
 * Copyright 2018 Esref Ozdemir
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <algorithm>
#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/point.hpp>

#include "convex_stats.hpp"
#include <utils.hpp>

using namespace feature;

namespace bg = boost::geometry;
typedef bg::model::point<double, 2, bg::cs::cartesian> point;
typedef bg::model::multi_point<point> multi_point;

/**
 * Find the convex hull of Player sequence between [begin, end) and return the
 * indices of Player objects that are on the convex hull.
 */
/**
 * @brief Find the convex hull of a Player range [begin, end) and return the
 * indices of Player objects that are on the convex hull.
 *
 * This function computes the convex hull of Player objects given in [begin,
 * end) and then finds the indices of points on the hull by doing an \f$O(NK)\f$
 * time worst-case search where \f$K\f$ is the size of the convex hull and
 * \f$N\f$ is the number of total players.
 *
 * @param begin Beginning of the player range [begin, end).
 * @param end End of the player range [begin, end).
 *
 * @return Indices of the players on the convex hull.
 *
 * @todo Reduce index finding time complexity to \f$O(KlogN)\f$ by sorting the
 * points. We should do this if \f$NK \gg KlogN\f$ which is not the case for
 * small \f$N\f$.
 */
static std::vector<int> convex_indices(player_cit begin, player_cit end) {
    // construct points
    multi_point points;
    for (auto it = begin; it != end; ++it) {
        bg::append(points, point(it->x, it->y));
    }
    // compute convex hull
    multi_point hull;
    bg::convex_hull(points, hull);

    // find the index of each point on the hull by searching for it in the
    // original range.
    std::vector<int> indices;
    // bg::convex_hull puts the first point twice. Don't count it in the end.
    for (auto it = boost::begin(hull); it != std::prev(boost::end(hull));
         ++it) {
        auto point_it = std::find_if(
            boost::begin(points), boost::end(points), [it](const point& p) {
                return close(p.get<0>(), it->get<0>()) &&
                       close(p.get<1>(), it->get<1>());
            });
        indices.push_back(std::distance(boost::begin(points), point_it));
    }

    return indices;
}

/**
 * Constructs a sequence of Point objects from Player objects at the given
 * indices of the given Player sequence starting at begin.
 */

/**
 * @brief Construct a vector of point types at the given indices and return it.
 *
 * This function filters the Players at the given indices in the range that
 * starts at begin and returns them as a vector of point types.
 *
 * @param indices Indices of points to return as a separate vector.
 * @param begin Beginning of a Player range.
 */
static std::vector<point> points_from_indices(const std::vector<int>& indices,
                                              player_cit begin) {
    std::vector<point> res(indices.size());
    std::transform(indices.begin(), indices.end(), res.begin(),
                   [begin](const int i) {
                       auto player = std::next(begin, i);
                       return point(player->x, player->y);
                   });

    return res;
}

namespace feature {
namespace details {

void convex_stats(player_cit begin, player_cit end,
                  std::vector<double>::iterator speed_begin,
                  const std::string& prefix, std::vector<double>& features) {
    // initialize features with default values
    double min_x = feature::default_value();
    double min_y = feature::default_value();
    double max_x = feature::default_value();
    double max_y = feature::default_value();
    double max_dist = feature::default_value();
    double min_dist = feature::default_value();
    double max_speed = feature::default_value();
    point center(feature::default_value(), feature::default_value());

    // if there are at least 3 points (no convex hull of 2 or less points)
    if (std::distance(begin, end) > 2) {
        // get convex indices and corresponding points
        std::vector<int> indices = convex_indices(begin, end);
        std::vector<point> convex_points = points_from_indices(indices, begin);

        min_x = std::numeric_limits<double>::max();
        min_y = std::numeric_limits<double>::max();
        max_x = std::numeric_limits<double>::lowest();
        max_y = std::numeric_limits<double>::lowest();
        center = point(0, 0);
        double size = static_cast<double>(convex_points.size());

        // calculate min/max x/y and center
        for (const auto& point : convex_points) {
            min_x = std::min(min_x, point.get<0>());
            min_y = std::min(min_y, point.get<1>());
            max_x = std::max(max_x, point.get<0>());
            max_y = std::max(max_y, point.get<1>());

            center.set<0>(center.get<0>() + point.get<0>() / size);
            center.set<1>(center.get<1>() + point.get<1>() / size);
        }

        // farDistance, closestDistance
        max_dist = std::numeric_limits<double>::lowest();
        min_dist = std::numeric_limits<double>::max();
        for (const auto& point : convex_points) {
            double distance = dist(point.get<0>(), point.get<1>(),
                                   center.get<0>(), center.get<1>());
            max_dist = std::max(max_dist, distance);
            min_dist = std::min(min_dist, distance);
        }

        // maxSpeed
        max_speed = std::numeric_limits<double>::lowest();
        for (int i : indices) {
            double speed = *std::next(speed_begin, i);
            max_speed = std::max(max_speed, speed);
        }
    }
    // write the results
    features[name_to_index(prefix + "ConvexMaxX")] = max_x;
    features[name_to_index(prefix + "ConvexMinX")] = min_x;
    features[name_to_index(prefix + "ConvexMaxY")] = max_y;
    features[name_to_index(prefix + "ConvexMinY")] = min_y;
    features[name_to_index(prefix + "ConvexCenterX")] = center.get<0>();
    features[name_to_index(prefix + "ConvexCenterY")] = center.get<1>();
    features[name_to_index(prefix + "ConvexMaxSpeed")] = max_speed;
    features[name_to_index(prefix + "ConvexFarDistance")] = max_dist;
    features[name_to_index(prefix + "ConvexClosestDistance")] = min_dist;
}

}; // namespace details
}; // namespace feature


#include <iostream>
#include <vector>
#include <math.h>
#include <string>

#include "Input_Reader.h"
#include "Materials.h"
#include "Fem_Quadrature.h"
#include "Quadrule_New.h"
#include "Cell_Data.h"
#include "Angular_Quadrature.h"
#include "Time_Data.h"
#include "Temperature_Data.h"

#include <Eigen/Dense>
#include "Diffusion_Matrix_Creator_Grey.h"

#include "Dark_Arts_Exception.h"

/**
  Goal of this unit test is to check source moment formation, and reaction matrix for SLXS Lobatto scheme
  -This is a MMS problem with a spatially varying sigma_a, constant cv , zero sig_s
*/

int main(int argc, char** argv)
{
  int val = 0;
  const double tol = 1.0E-6;

  Input_Reader input_reader;
  try
  {
    input_reader.read_xml(argv[1]);
  }
  catch(const Dark_Arts_Exception& da_exception )
  {
    da_exception.message() ;
    val = -1;
  }

  /// Initialize a Quadrule object to be able to get all of the quadrature we need
  Quadrule_New quad_fun;
  Fem_Quadrature fem_quadrature( input_reader , quad_fun);
  Cell_Data cell_data( input_reader );
  Angular_Quadrature angular_quadrature( input_reader , quad_fun );

  const int n_dfem_p = fem_quadrature.get_number_of_interpolation_points();
  const double sn_w = angular_quadrature.get_sum_w();

  /// Create a Materials object that contains all opacity, heat capacity, and source objects
  Materials materials( input_reader, fem_quadrature , cell_data, angular_quadrature );
  Time_Data time_data(input_reader);
  Temperature_Data t_old(fem_quadrature, input_reader, cell_data);

  Eigen::MatrixXd r_sig_a_reg1 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd r_sig_a_reg2 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);

  Eigen::MatrixXd r_sig_s_reg1 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd r_sig_s_reg2 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);

  Eigen::MatrixXd dimless_mass = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd r_cv_1 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd r_cv_2 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);

  Eigen::MatrixXd r_sig_tau_1 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd r_sig_tau_2 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);

  Eigen::MatrixXd d_1 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd d_2 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);

  const double sig_a_1 = 0.7;
  const double sig_a_2 = 0.9;
  const double sig_s_1 = 1.0;
  const double sig_s_2 = 0.2;
  const double cv_1 = 1.0;
  const double cv_2 = 1.2;
  const double dx_1 = 4./3.;
  const double dx_2 = 1./3.;

  const double w_1 = 1.;
  const double w_2 = 1.;

  const double dt = time_data.get_dt_min();
  const double c = 1.0;
  const double rk_a = 1.0;

  const double temp_1 = 0.5;
  const double temp_2 = 0.4;

  dimless_mass(0,0) = w_1;
  dimless_mass(1,1) = w_2;

  r_sig_a_reg1 = dx_1/2.*sig_a_1*dimless_mass;
  r_sig_a_reg2 = dx_2/2.*sig_a_2*dimless_mass;

  r_sig_s_reg1 = dx_1/2.*sig_s_1*dimless_mass;
  r_sig_s_reg2 = dx_2/2.*sig_s_2*dimless_mass;

  r_cv_1 = dx_1/2.*cv_1*dimless_mass;
  r_cv_2 = dx_2/2.*cv_2*dimless_mass;

  std::cout << "r_sig_a_1: \n" << r_sig_a_reg1 << std::endl;
  std::cout << "r_sig_a_2: \n" << r_sig_a_reg2 << std::endl;
  std::cout << "r_sig_s_1: \n" << r_sig_s_reg1 << std::endl;
  std::cout << "r_sig_s_2: \n" << r_sig_s_reg2 << std::endl;
  std::cout << "r_cv_1: \n" << r_cv_1 << std::endl;
  std::cout << "r_cv_2: \n" << r_cv_2 << std::endl;

  r_sig_tau_1 = r_sig_a_reg1+r_sig_s_reg1+(1./(c*dt*1.0))*dx_1/2.*dimless_mass;
  r_sig_tau_2 = r_sig_a_reg2+r_sig_s_reg2+(1./(c*dt*1.0))*dx_2/2.*dimless_mass;

  d_1(0,0) = 4.*pow(temp_1,3)/sn_w; d_1(1,1) = 4.*pow(temp_1,3)/sn_w;
  d_2(0,0) = 4.*pow(temp_2,3)/sn_w; d_2(1,1) = 4.*pow(temp_2,3)/sn_w;

  Eigen::MatrixXd r_pseudo_s_1 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd r_pseudo_s_2 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd r_pseudo_a_1 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd r_pseudo_a_2 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);

  int i = 0;
  r_pseudo_s_1(i,i) = r_sig_a_reg1(i,i)*d_1(i,i)/( 1.+sn_w*dt*rk_a/r_cv_1(i,i)*r_sig_a_reg1(i,i)*d_1(i,i) )/r_cv_1(i,i)*r_sig_a_reg1(i,i) ;
  i = 1;
  r_pseudo_s_1(i,i) = r_sig_a_reg1(i,i)*d_1(i,i)/( 1.+sn_w*dt*rk_a/r_cv_1(i,i)*r_sig_a_reg1(i,i)*d_1(i,i) )/r_cv_1(i,i)*r_sig_a_reg1(i,i) ;
  r_pseudo_s_1 *= sn_w*dt*rk_a;
  r_pseudo_s_1 += r_sig_s_reg1;

  i=0;
  r_pseudo_s_2(i,i) = r_sig_a_reg2(i,i)*d_2(i,i)/( 1.+sn_w*dt*rk_a/r_cv_2(i,i)*r_sig_a_reg2(i,i)*d_2(i,i) )/r_cv_2(i,i)*r_sig_a_reg2(i,i) ;
  i = 1;
  r_pseudo_s_2(i,i) = r_sig_a_reg2(i,i)*d_2(i,i)/( 1.+sn_w*dt*rk_a/r_cv_2(i,i)*r_sig_a_reg2(i,i)*d_2(i,i) )/r_cv_2(i,i)*r_sig_a_reg2(i,i) ;
  r_pseudo_s_2 *= sn_w*dt*rk_a;
  r_pseudo_s_2 += r_sig_s_reg2;

  r_pseudo_a_1 = r_sig_tau_1 - r_pseudo_s_1 ;
  r_pseudo_a_2 = r_sig_tau_2 - r_pseudo_s_2 ;

  Eigen::MatrixXd unitless_s_matrix = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
  Eigen::MatrixXd calculated_s_matrix_1 = unitless_s_matrix;
  Eigen::MatrixXd calculated_s_matrix_2 = unitless_s_matrix;
  Eigen::MatrixXd s_matrix_1 = unitless_s_matrix;
  Eigen::MatrixXd s_matrix_2 = unitless_s_matrix;
  unitless_s_matrix(0,0) = 0.5;
  unitless_s_matrix(1,1) = 0.5;
  unitless_s_matrix(0,1) = -0.5;
  unitless_s_matrix(1,0) = -0.5;

  const double diff_co_1 = 1./(3.*(sig_a_1 + sig_s_1 + 1./(c*dt*rk_a)));
  const double diff_co_2 = 1./(3.*(sig_a_2 + sig_s_2 + 1./(c*dt*rk_a)));

  std::cout << "D_1 = " << diff_co_1 << std::endl;
  std::cout << "D_2 = " << diff_co_2 << std::endl;

  /**
    \f{eqnarray}{
      \frac{\partial b_0}{\partial s} &=& -\frac{1}{2} \\
      \frac{\partial b_1}{\partial s} &=& \frac{1}{2} \\
      \int_{x_{i-1/2}}^{x_{i+1/2}}{ \frac{\partial b_0}{\partial x} \frac{\partial b_0}{\partial x} ~dx} &=&
        \frac{2}{\Delta x} \frac{2}{\Delta x} \frac{\Delta x}{2}
        \int_{-1}^1{ \frac{\partial b_0}{\partial s} \frac{\partial b_0}{\partial s}~ds} \\
        &=& \frac{4}{\Delta x}
    \f}
  */

  s_matrix_1 = 2./dx_1*diff_co_1*unitless_s_matrix;
  s_matrix_2 = 2./dx_2*diff_co_2*unitless_s_matrix;

  try{
    /// test matrix integrations
    std::shared_ptr<V_Diffusion_Matrix_Creator> matrix_creator;
    matrix_creator = std::make_shared<Diffusion_Matrix_Creator_Grey>
      (fem_quadrature,materials,angular_quadrature,t_old, cell_data.get_total_number_of_cells(),input_reader );

    const double t_stage = time_data.get_t_start();

    double d_cm1_r , d_c_l, d_c_r , d_cp1_l;

    Eigen::MatrixXd calc_r_sig_a_1 = Eigen::MatrixXd::Zero(n_dfem_p,n_dfem_p);
    Eigen::MatrixXd calc_r_sig_a_2 = calc_r_sig_a_1;
    Eigen::MatrixXd calc_r_sig_s_1 = calc_r_sig_a_1;
    Eigen::MatrixXd calc_r_sig_s_2 = calc_r_sig_a_1;

    matrix_creator->set_time_data(dt,t_stage,rk_a);
    matrix_creator->set_cell_group_information(1,0,dx_1);
    matrix_creator->calculate_pseudo_r_sig_a_and_pseudo_r_sig_s(calc_r_sig_a_1,calc_r_sig_s_1);
    matrix_creator->calculate_d_dependent_quantities(d_cm1_r , d_c_l, d_c_r , d_cp1_l, calculated_s_matrix_1);

    matrix_creator->set_cell_group_information(4,0,dx_2);
    matrix_creator->calculate_pseudo_r_sig_a_and_pseudo_r_sig_s(calc_r_sig_a_2,calc_r_sig_s_2);
    matrix_creator->calculate_d_dependent_quantities(d_cm1_r , d_c_l, d_c_r , d_cp1_l, calculated_s_matrix_2);

    std::cout << "Region 0\n";
    std::cout << "Calculated Pseudo r_sig_a: \n" << calc_r_sig_a_1 << "\n Expected: \n" << r_pseudo_a_1 << std::endl;
    std::cout << "Calculated Pseudo r_sig_s: \n" << calc_r_sig_s_1 << "\n Expected: \n" << r_pseudo_s_1 << std::endl;
    std::cout << "Calculated S_matrix: \n" << calculated_s_matrix_1 << "\n Expected: \n" << s_matrix_1 << std::endl;

    std::cout << "\nRegion 1\n";
    std::cout << "Calculated Pseudo r_sig_a: \n" << calc_r_sig_a_2 << "\n Expected: \n" << r_pseudo_a_2 << std::endl;
    std::cout << "Calculated Pseudo r_sig_s: \n" << calc_r_sig_s_2 << "\n Expected: \n" << r_pseudo_s_2 << std::endl;
    std::cout << "Calculated S_matrix: \n" << calculated_s_matrix_2 << "\n Expected: \n" << s_matrix_2 << std::endl;

    for(int i=0; i < n_dfem_p ; i++)
    {
      for(int j=0; j < n_dfem_p ; j++)
      {
        if( fabs(calc_r_sig_a_1(i,j) - r_pseudo_a_1(i,j) ) > tol)
          throw Dark_Arts_Exception(MIP, "Not calculating region 0 grey pseudo r_sig_a correctly");

        if( fabs(calc_r_sig_s_1(i,j) - r_pseudo_s_1(i,j) ) > tol)
          throw Dark_Arts_Exception(MIP, "Not calculating region 0 grey pseudo r_sig_s correctly");

        if( fabs(calc_r_sig_a_2(i,j) - r_pseudo_a_2(i,j) ) > tol)
          throw Dark_Arts_Exception(MIP, "Not calculating region 1 grey pseudo r_sig_a correctly");

        if( fabs(calc_r_sig_s_2(i,j) - r_pseudo_s_2(i,j) ) > tol)
          throw Dark_Arts_Exception(MIP, "Not calculating region 1 grey pseudo r_sig_s correctly");

        if( fabs(calculated_s_matrix_1(i,j) - s_matrix_1(i,j) ) > tol)
          throw Dark_Arts_Exception(MIP, "Not calculating region 1 grey pseudo S matrix correctly");

        if( fabs(calculated_s_matrix_2(i,j) - s_matrix_2(i,j) ) > tol)
          throw Dark_Arts_Exception(MIP, "Not calculating region 2 grey pseudo S matrix correctly");
      }
    }

    /// test edge diffusion coefficent evaluations
    /// first test, interior of region 0

    matrix_creator->set_cell_group_information(1,0,dx_1);
    matrix_creator->calculate_d_dependent_quantities(d_cm1_r , d_c_l, d_c_r , d_cp1_l, calculated_s_matrix_2);

    std::cout << "Interior of region 0 D coeff\n";
    std::cout << "Expected d_cm1_r: " << diff_co_1 << " Calculated: " << d_cm1_r << std::endl;
    std::cout << "Expected d_c_l: " << diff_co_1 << " Calculated: " << d_c_l << std::endl;
    std::cout << "Expected d_c_r: " << diff_co_1 << " Calculated: " << d_c_r << std::endl;
    std::cout << "Expected d_cp1_l: " << diff_co_1 << " Calculated: " << d_cp1_l << std::endl;
    if( fabs(d_cm1_r - diff_co_1) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c minus 1 right edge D");

    if( fabs(d_c_l - diff_co_1) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c left edge D");

    if( fabs(d_c_r - diff_co_1) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c right edge D");

    if( fabs(d_cp1_l - diff_co_1) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c plus 1 left edge D");

    /// interior of region 1
    matrix_creator->set_cell_group_information(4,0,dx_2);
    matrix_creator->calculate_d_dependent_quantities(d_cm1_r , d_c_l, d_c_r , d_cp1_l, calculated_s_matrix_2);
    std::cout << "Interior of region 1 D coeff\n";
    std::cout << "Expected d_cm1_r: " << diff_co_2 << " Calculated: " << d_cm1_r << std::endl;
    std::cout << "Expected d_c_l: " << diff_co_2 << " Calculated: " << d_c_l << std::endl;
    std::cout << "Expected d_c_r: " << diff_co_2 << " Calculated: " << d_c_r << std::endl;
    std::cout << "Expected d_cp1_l: " << diff_co_2 << " Calculated: " << d_cp1_l << std::endl;
    if( fabs(d_cm1_r - diff_co_2) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c minus 1 right edge D");

    if( fabs(d_c_l - diff_co_2) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c left edge D");

    if( fabs(d_c_r - diff_co_2) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c right edge D");

    if( fabs(d_cp1_l - diff_co_2) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c plus 1 left edge D");

    /// rightmost cell of region 0
    matrix_creator->set_cell_group_information(2,0,dx_1);
    matrix_creator->calculate_d_dependent_quantities(d_cm1_r , d_c_l, d_c_r , d_cp1_l, calculated_s_matrix_2);
    std::cout << "Righmost edge of region 0 D coeff\n";
    std::cout << "Expected d_cm1_r: " << diff_co_1 << " Calculated: " << d_cm1_r << std::endl;
    std::cout << "Expected d_c_l: " << diff_co_1 << " Calculated: " << d_c_l << std::endl;
    std::cout << "Expected d_c_r: " << diff_co_1 << " Calculated: " << d_c_r << std::endl;
    std::cout << "Expected d_cp1_l: " << diff_co_2 << " Calculated: " << d_cp1_l << std::endl;
    if( fabs(d_cm1_r - diff_co_1) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c minus 1 right edge D");

    if( fabs(d_c_l - diff_co_1) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c left edge D");

    if( fabs(d_c_r - diff_co_1) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c right edge D");

    if( fabs(d_cp1_l - diff_co_2) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c plus 1 left edge D");

    /// leftmost cell of region 1
    matrix_creator->set_cell_group_information(3,0,dx_2);
    matrix_creator->calculate_d_dependent_quantities(d_cm1_r , d_c_l, d_c_r , d_cp1_l, calculated_s_matrix_2);
    std::cout << "Leftmost cell of region 1 D coeff\n";
    std::cout << "Expected d_cm1_r: " << diff_co_1 << " Calculated: " << d_cm1_r << std::endl;
    std::cout << "Expected d_c_l: " << diff_co_2 << " Calculated: " << d_c_l << std::endl;
    std::cout << "Expected d_c_r: " << diff_co_2 << " Calculated: " << d_c_r << std::endl;
    std::cout << "Expected d_cp1_l: " << diff_co_2 << " Calculated: " << d_cp1_l << std::endl;
    if( fabs(d_cm1_r - diff_co_1) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c minus 1 right edge D");

    if( fabs(d_c_l - diff_co_2) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c left edge D");

    if( fabs(d_c_r - diff_co_2) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c right edge D");

    if( fabs(d_cp1_l - diff_co_2) > tol )
      throw Dark_Arts_Exception(MIP, "Wrong cell c plus 1 left edge D");

  }
  catch(const Dark_Arts_Exception& da)
  {
    val = -1;
    da.testing_message();
  }

  return val;
}


/* boost random/inversive_congruential.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_INVERSIVE_CONGRUENTIAL_HPP
#define BOOST_RANDOM_INVERSIVE_CONGRUENTIAL_HPP

#include <iosfwd>
#include <stdexcept>
#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/integer/static_log2.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/const_mod.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/detail/seed_impl.hpp>

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

namespace boost {
namespace random {

// Eichenauer and Lehn 1986
/**
 * Instantiations of class template @c inversive_congruential_engine model a
 * \pseudo_random_number_generator. It uses the inversive congruential
 * algorithm (ICG) described in
 *
 *  @blockquote
 *  "Inversive pseudorandom number generators: concepts, results and links",
 *  Peter Hellekalek, In: "Proceedings of the 1995 Winter Simulation
 *  Conference", C. Alexopoulos, K. Kang, W.R. Lilegdon, and D. Goldsman
 *  (editors), 1995, pp. 255-262. ftp://random.mat.sbg.ac.at/pub/data/wsc95.ps
 *  @endblockquote
 *
 * The output sequence is defined by x(n+1) = (a*inv(x(n)) - b) (mod p),
 * where x(0), a, b, and the prime number p are parameters of the generator.
 * The expression inv(k) denotes the multiplicative inverse of k in the
 * field of integer numbers modulo p, with inv(0) := 0.
 *
 * The template parameter IntType shall denote a signed integral type large
 * enough to hold p; a, b, and p are the parameters of the generators. The
 * template parameter val is the validation value checked by validation.
 *
 * @xmlnote
 * The implementation currently uses the Euclidian Algorithm to compute
 * the multiplicative inverse. Therefore, the inversive generators are about
 * 10-20 times slower than the others (see section"performance"). However,
 * the paper talks of only 3x slowdown, so the Euclidian Algorithm is probably
 * not optimal for calculating the multiplicative inverse.
 * @endxmlnote
 */
template<class IntType, IntType a, IntType b, IntType p>
class inversive_congruential_engine
{
public:
    typedef IntType result_type;
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

    BOOST_STATIC_CONSTANT(result_type, multiplier = a);
    BOOST_STATIC_CONSTANT(result_type, increment = b);
    BOOST_STATIC_CONSTANT(result_type, modulus = p);
    BOOST_STATIC_CONSTANT(IntType, default_seed = 1);

    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () { return b == 0 ? 1 : 0; }
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () { return p-1; }

    /**
     * Constructs an @c inversive_congruential_engine, seeding it with
     * the default seed.
     */
    inversive_congruential_engine() { seed(); }

    /**
     * Constructs an @c inversive_congruential_engine, seeding it with @c x0.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(inversive_congruential_engine,
                                               IntType, x0)
    { seed(x0); }

    /**
     * Constructs an @c inversive_congruential_engine, seeding it with values
     * produced by a call to @c seq.generate().
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(inversive_congruential_engine,
                                             SeedSeq, seq)
    { seed(seq); }

    /**
     * Constructs an @c inversive_congruential_engine, seeds it
     * with values taken from the itrator range [first, last),
     * and adjusts first to point to the element after the last one
     * used.  If there are not enough elements, throws @c std::invalid_argument.
     *
     * first and last must be input iterators.
     */
    template<class It> inversive_congruential_engine(It& first, It last)
    { seed(first, last); }

    /**
     * Calls seed(default_seed)
     */
    void seed() { seed(default_seed); }

    /**
     * If c mod m is zero and x0 mod m is zero, changes the current value of
     * the generator to 1. Otherwise, changes it to x0 mod m. If c is zero,
     * distinct seeds in the range [1,m) will leave the generator in distinct
     * states. If c is not zero, the range is [0,m).
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(inversive_congruential_engine, IntType, x0)
    {
        // wrap _x if it doesn't fit in the destination
        if(modulus == 0) {
            _value = x0;
        } else {
            _value = x0 % modulus;
        }
        // handle negative seeds
        if(_value <= 0 && _value != 0) {
            _value += modulus;
        }
        // adjust to the correct range
        if(increment == 0 && _value == 0) {
            _value = 1;
        }
        BOOST_ASSERT(_value >= (min)());
        BOOST_ASSERT(_value <= (max)());
    }

    /**
     * Seeds an @c inversive_congruential_engine using values from a SeedSeq.
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(inversive_congruential_engine, SeedSeq, seq)
    { seed(detail::seed_one_int<IntType, modulus>(seq)); }

    /**
     * seeds an @c inversive_congruential_engine with values taken
     * from the itrator range [first, last) and adjusts @c first to
     * point to the element after the last one used.  If there are
     * not enough elements, throws @c std::invalid_argument.
     *
     * @c first and @c last must be input iterators.
     */
    template<class It> void seed(It& first, It last)
    { seed(detail::get_one_int<IntType, modulus>(first, last)); }

    /** Returns the next output of the generator. */
    IntType operator()()
    {
        typedef const_mod<IntType, p> do_mod;
        _value = do_mod::mult_add(a, do_mod::invert(_value), b);
        return _value;
    }

    /** Fills a range with random values */
    template<class Iter>
    void generate(Iter first, Iter last)
    { detail::generate_from_int(*this, first, last); }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    {
        for(boost::uintmax_t j = 0; j < z; ++j) {
            (*this)();
        }
    }

    /**
     * Writes the textual representation of the generator to a @c std::ostream.
     */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, inversive_congruential_engine, x)
    {
        os << x._value;
        return os;
    }

    /**
     * Reads the textual representation of the generator from a @c std::istream.
     */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, inversive_congruential_engine, x)
    {
        is >> x._value;
        return is;
    }

    /**
     * Returns true if the two generators will produce identical
     * sequences of outputs.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(inversive_congruential_engine, x, y)
    { return x._value == y._value; }

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

private:
    IntType _value;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class IntType, IntType a, IntType b, IntType p>
const bool inversive_congruential_engine<IntType, a, b, p>::has_fixed_range;
template<class IntType, IntType a, IntType b, IntType p>
const typename inversive_congruential_engine<IntType, a, b, p>::result_type inversive_congruential_engine<IntType, a, b, p>::multiplier;
template<class IntType, IntType a, IntType b, IntType p>
const typename inversive_congruential_engine<IntType, a, b, p>::result_type inversive_congruential_engine<IntType, a, b, p>::increment;
template<class IntType, IntType a, IntType b, IntType p>
const typename inversive_congruential_engine<IntType, a, b, p>::result_type inversive_congruential_engine<IntType, a, b, p>::modulus;
template<class IntType, IntType a, IntType b, IntType p>
const typename inversive_congruential_engine<IntType, a, b, p>::result_type inversive_congruential_engine<IntType, a, b, p>::default_seed;
#endif

/// \cond show_deprecated

// provided for backwards compatibility
template<class IntType, IntType a, IntType b, IntType p, IntType val = 0>
class inversive_congruential : public inversive_congruential_engine<IntType, a, b, p>
{
    typedef inversive_congruential_engine<IntType, a, b, p> base_type;
public:
    inversive_congruential(IntType x0 = 1) : base_type(x0) {}
    template<class It>
    inversive_congruential(It& first, It last) : base_type(first, last) {}
};

/// \endcond

/**
 * The specialization hellekalek1995 was suggested in
 *
 *  @blockquote
 *  "Inversive pseudorandom number generators: concepts, results and links",
 *  Peter Hellekalek, In: "Proceedings of the 1995 Winter Simulation
 *  Conference", C. Alexopoulos, K. Kang, W.R. Lilegdon, and D. Goldsman
 *  (editors), 1995, pp. 255-262. ftp://random.mat.sbg.ac.at/pub/data/wsc95.ps
 *  @endblockquote
 */
typedef inversive_congruential_engine<uint32_t, 9102, 2147483647-36884165,
  2147483647> hellekalek1995;

} // namespace random

using random::hellekalek1995;

} // namespace boost

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

#endif // BOOST_RANDOM_INVERSIVE_CONGRUENTIAL_HPP


#pragma once

#include <Eigen/Geometry>

#include "geometry.hpp"

class Quaternion {
public:
  Quaternion() { eigenQuaternion_ = Eigen::Quaternion<float>::Identity(); }

  Quaternion(const Eigen::Quaternion<float> &initialQuaternion) {
    eigenQuaternion_ = initialQuaternion;
  }

  // note: expected euler angle format is: [z_axis_angle, y_axis_angle,
  // x_axis_angle]
  Quaternion(const std::array<float, 3> &eulerAngle) {
    eigenQuaternion_ = Eigen::Quaternion<float>::Identity();

    Eigen::Quaternion<float> zAxisRotation(
        cos(degreesToRadians(eulerAngle[0]) / 2), 0.0f, 0.0f,
        sin(degreesToRadians(eulerAngle[0]) / 2));
    Eigen::Quaternion<float> yAxisRotation(
        cos(degreesToRadians(eulerAngle[1]) / 2), 0.0f,
        sin(degreesToRadians(eulerAngle[1]) / 2), 0.0f);
    Eigen::Quaternion<float> xAxisRotation(
        cos(degreesToRadians(eulerAngle[2]) / 2),
        sin(degreesToRadians(eulerAngle[2]) / 2), 0.0f, 0.0f);

    eigenQuaternion_ =
        zAxisRotation * yAxisRotation * xAxisRotation * eigenQuaternion_;
    eigenQuaternion_.normalize();
  }

  // CC
  Quaternion(const Quaternion &otherQuaterion) {
    eigenQuaternion_ = otherQuaterion.eigenQuaternion_;
  }

  // AO
  Quaternion &operator=(const Quaternion &otherQuaterion) {
    eigenQuaternion_ = otherQuaterion.eigenQuaternion_;
  }

  Eigen::Quaternion<float> getEigenQuaternion() const {
    return eigenQuaternion_;
  }

private:
  Eigen::Quaternion<float> eigenQuaternion_;
};