/**
 * si_function.hpp
 *
 * This file defines functions from a shift invariant space. We call these
 * Shift Invariant Functions (si_functions), which is a slight abuse
 * of naming to mean that they come from shift invariant spaces.
 *
 * @author Joshua Horacsek
 **/

#ifndef _SISL_SI_FUNCTION_H_
#define _SISL_SI_FUNCTION_H_

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sisl/primitives.hpp>
#include <sisl/function/base_function.hpp>

namespace sisl {

	/*! \brief Combines a lattice and a generating function.
	 *
	 */
    template<class L, class BF, int N>
    class si_function : public function {
    public:
        /*! \brief Evaluate the function at a point.
         */
        si_function() : _bForceScale(false), _lattice(nullptr), _dBasisScale(1.), _bUseBasisDerivative(true){
            _mSpaceTransform = Eigen::MatrixXd::Identity(N, N);
        }

        /*! \brief Evaluate the function at a point.
         */
        si_function(L *lattice) : si_function(){
            _lattice = lattice;
        }

        virtual ~si_function() {

        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const double operator()(double d0, ...) const {
            va_list vl;
            vector V(N);
            V[0] = d0;

            va_start(vl, d0);
            for(unsigned int i = 1; i < N; i++) {
                V[i] = va_arg(vl, double);
            }
            va_end(vl);

            return (*this)(V);
        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const double operator()(const vector &p) const {
            if(_lattice == nullptr) return 0;

            if(!_bForceScale) {
                return BF::template convolution_sum<N, L, BF>(
                            _mSpaceTransform*p,
                            (const L*)_lattice);
            }
            return BF::template convolution_sum_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        _dBasisScale);
        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const double d(int component, double d0, ...) const {
            va_list vl;
            vector V(N);
            V[0] = d0;

            va_start(vl, d0);
            for(unsigned int i = 1; i < N; i++) {
                V[i] = va_arg(vl, double);
            }
            va_end(vl);

            return d(component, V);
        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const double d(int component, const vector &p) const {
            if(_bUseBasisDerivative && BF::has_derivative()) {
                if(!_bForceScale)
                    return BF::template convolution_sum_deriv<N, L, BF>(
                            _mSpaceTransform*p,
                            (const L*)_lattice,
                            component);
                return BF::template convolution_sum_deriv_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        component,
                        _dBasisScale);

            }
            if(!_d[component]) return 0;
            if(!_bForceScale)
                return BF::template convolution_sum<N, L, BF>(
                            _mSpaceTransform*p,
                            (const L*)_d[component]);
            return BF::template convolution_sum_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_d[component],
                        _dBasisScale);

        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const vector grad(double d0, ...) const {
            va_list vl;
            vector V(N);
            V[0] = d0;

            va_start(vl, d0);
            for(unsigned int i = 1; i < N; i++) {
                V[i] = va_arg(vl, double);
            }
            va_end(vl);

            return grad(V);
        }

        /*! \brief Evaluate the function at a point.
         */
        virtual const vector grad(const vector &p) const {
            if(_bUseBasisDerivative && BF::has_derivative()) {
                if(!_bForceScale)
                    return BF::template grad_convolution_sum<N, L, BF>(
                            _mSpaceTransform*p,
                            (const L*)_lattice);
                return BF::template grad_convolution_sum_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        _dBasisScale);

            }
            if(!_bForceScale)
                return BF::template grad_convolution_sum<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        (const L**)_d);
            return BF::template grad_convolution_sum_h<N, L, BF>(
                        _mSpaceTransform*p,
                        (const L*)_lattice,
                        (const L**)_d,
                        _dBasisScale);

        }
        /*! \brief Sets a scale for the basis function to use.
        */
        void set_basis_scale(const double &h) {
            _dBasisScale = h;
            _bForceScale = true;
        }

        /*! \brief If the basis has a derivative, this makes the function use
         * that derivative for derivative reconstruction
         */
        void use_basis_gradient() {
            _bUseBasisDerivative = true;
        }

        /*! \brief If the function has derivative lattices, this makes the
         * function use the given basis to reconstruct derivative values.
         */
        void use_derivative_lattice() {
            _bUseBasisDerivative = false;
        }

        /*! \brief Sets the current reconstruction lattice
         */
        void set_lattice(L *new_lat) {
            this->_lattice = new_lat;
        }

        /*! \brief Sets the lattice to be used for derivative reconstruction
         *  use use_derivative_lattice() to use this for reconstruction after
         *  all lattices have been set.
         */
        void set_derivative_lattice(unsigned int component, L *d) {
            if(component < N)
                _d[component] = d;
        }

        /*! \brief Sets the transform for this space,
        */
        void set_transform(const transform &t){
            if(fabs(t.determinant()) > 1e-8)
                _mSpaceTransform = t;
        }

        /*! \bried Gets the transform assiated to this space,
         */
        transform get_transform() const{
            return _mSpaceTransform;
        }

        L *get_lattice() {
            return _lattice;
        }

        void set_scale(const vector &s) {
            m_vUserScale = s;
            _mSpaceTransform *= Eigen::Scaling(s);
        }

        vector get_scale() const {
            return m_vUserScale;
        }
        virtual const int dim() const {
        	return N;
        }

        virtual const double n_d(const int_tuple &order, double d0, ...) const {
        	throw "Not yet implemnented";

        }

        virtual const double n_d(const int_tuple &order, vector &p) const {
        	throw "Not yet implemnented";
        }

    private:
        L *_lattice, *_d[N];
        bool _bUseBasisDerivative;
        bool _bForceScale;
        double _dBasisScale;
        transform _mSpaceTransform;
        sisl::vector m_vUserScale;
    };
}

#endif // _SISL_SI_FUNCTION_H_


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>
#include <chrono>

#define DIRECTLAYER 2
#define PI314 (static_cast<double>(3.1415926535897932384626433))

namespace Laplace2D3D {

using EVec3 = Eigen::Vector3d;

inline double ERFC(double x) { return std::erfc(x); }
inline double ERF(double x) { return std::erf(x); }

// real and wave sum of 2D Laplace kernel Ewald

// xm: target, xn: source
inline double realSum(const double xi, const EVec3 &xn, const EVec3 &xm) {
    EVec3 rmn = xm - xn;
    double rnorm = rmn.norm();
    if (rnorm < 1e-14) {
        return 0;
    }
    return ERFC(rnorm * xi) / rnorm;
}

// xm: target, xn: source
inline double realSum2(const double xi, const EVec3 &xn, const EVec3 &xm) {
    double zmn = xm[2] - xn[2];
    double answer =
        exp(-xi * xi * zmn * zmn) / xi + sqrt(PI314) * zmn * ERF(xi * zmn);
    return answer;
}

inline double gkzxi(const double k, double zmn, double xi) {
    double answer = exp(k * zmn) * ERFC(k / (2 * xi) + xi * zmn) +
                    exp(-k * zmn) * ERFC(k / (2 * xi) - xi * zmn);
    return answer;
}

inline double selfTerm(double xi) { return -2 * xi / sqrt(PI314); }

inline double gKernelEwald(const EVec3 &xm, const EVec3 &xn) {
    const double xi = 1.8; // recommend for box=1 to get machine precision
    EVec3 target = xm;
    EVec3 source = xn;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);

    // real sum
    int rLim = 4;
    double Kreal = 0;
    for (int i = -rLim; i <= rLim; i++) {
        for (int j = -rLim; j <= rLim; j++) {
            EVec3 rmn = target - source + EVec3(i, j, 0);
            if (rmn.norm() < 1e-13) {
                continue;
            }
            Kreal += realSum(xi, EVec3(0, 0, 0), rmn);
        }
    }

    // wave sum
    int wLim = 4;
    double Kwave = 0;
    EVec3 rmn = target - source;
    const double rmnnorm = rmn.norm();
    double zmn = rmn[2];
    rmn[2] = 0;
    for (int i = -wLim; i <= wLim; i++) {
        for (int j = -wLim; j <= wLim; j++) {
            if (i == 0 && j == 0) {
                continue;
            }
            EVec3 kvec = EVec3(i, j, 0) * (2 * PI314);
            double knorm = kvec.norm();
            Kwave += cos(kvec[0] * rmn[0] + kvec[1] * rmn[1]) * (1 / knorm) *
                     gkzxi(knorm, zmn, xi);
        }
    }
    Kwave *= PI314;

    double Kreal2 = 2 * sqrt(PI314) * realSum2(xi, source, target);
    double Kself = rmnnorm < 1e-10 ? -2 * xi / sqrt(PI314) : 0;

    return Kreal + Kwave - Kreal2 + Kself;
}

inline double gKernel(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    return rnorm < 1e-14 ? 0 : 1 / rnorm;
}

// Out of Direct Sum Layer, far field part
inline double gKernelFF(const EVec3 &target, const EVec3 &source) {
    double fEwald = gKernelEwald(target, source);
    const int N = DIRECTLAYER;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            double gFree = gKernel(target, source - EVec3(i, j, 0));
            fEwald -= gFree;
        }
    }

    //   {
    //     std::cout << "source:" << source << std::endl
    //               << "target:" << target << std::endl
    //               << "gKernalFF" << fEwald << std::endl;
    //   }
    return fEwald;
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    // testing Ewald routine
    double Madelung2D =
        gKernelEwald(EVec3(0, 0, 0), EVec3(0.5, 0.5, 0)) * (-1) +
        gKernelEwald(EVec3(0, 0, 0), EVec3(0, 0, 0)) * 1;
    std::cout << std::setprecision(16) << "Madelung2D: " << Madelung2D
              << " Error: " << Madelung2D + 2.2847222932891311 << std::endl;

    //   exit(1);

    std::chrono::high_resolution_clock::time_point t1 =
        std::chrono::high_resolution_clock::now();
    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {
        -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {
        -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    const double scaleLEquiv = 1.05;
    const double scaleLCheck = 2.95;
    const double pCenterLEquiv[3] = {
        -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2};
    const double pCenterLCheck[3] = {
        -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2};

    auto pointMEquiv = surface(
        pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(
        pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(
        pEquiv, (double *)&(pCenterLCheck[0]), scaleLCheck,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(
        pCheck, (double *)&(pCenterLEquiv[0]), scaleLEquiv,
        0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd M2L(equivN, equivN); // Laplace, 1->1

    Eigen::MatrixXd A(1 * checkN, 1 * equivN);
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                               pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l],
                                         pointLEquiv[3 * l + 1],
                                         pointLEquiv[3 * l + 2]);
            A(k, l) = gKernel(Cpoint, Lpoint);
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1],
                                     pointMEquiv[3 * i + 2]);
        //		std::cout << "debug:" << Mpoint << std::endl;

        // assemble linear system
        Eigen::VectorXd f(checkN);
        for (int k = 0; k < checkN; k++) {
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1],
                                   pointLCheck[3 * k + 2]);
            //			std::cout<<"debug:"<<k<<std::endl;
            // sum the images
            f(k) = gKernelFF(Cpoint, Mpoint);
        }
        //		std::cout << "debug:" << f << std::endl;

        M2L.col(i) = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    }
    std::chrono::high_resolution_clock::time_point t2 =
        std::chrono::high_resolution_clock::now();
    auto duration =
        std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
    std::cout << "Precomputing time:" << duration / 1e6 << std::endl;

    // dump M2L
    for (int i = 0; i < equivN; i++) {
        for (int j = 0; j < equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific
                      << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
        chargePoint(2);
    std::vector<double> chargeValue(2);
    chargePoint[0] = Eigen::Vector3d(0.5, 0.5, 0);
    chargeValue[0] = -1;
    chargePoint[1] = Eigen::Vector3d(0, 0, 0);
    chargeValue[1] = 1;

    // solve M
    A.resize(checkN, equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(checkN);
    for (int k = 0; k < checkN; k++) {
        double temp = 0;
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1],
                               pointMCheck[3 * k + 2]);
        for (size_t p = 0; p < chargePoint.size(); p++) {
            temp = temp + gKernel(Cpoint, chargePoint[p]) * (chargeValue[p]);
        }
        f(k) = temp;
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1],
                                   pointMEquiv[3 * l + 2]);
            A(k, l) = gKernel(Mpoint, Cpoint);
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));

    std::cout << "Msource: " << Msource << std::endl;

    Eigen::VectorXd M2Lsource = M2L * (Msource);

    Eigen::Vector3d samplePoint(0, 0, 0);
    double Usample = 0;
    double UsampleSP = 0;

    for (int i = -DIRECTLAYER; i < 1 + DIRECTLAYER; i++) {
        for (int j = -DIRECTLAYER; j < 1 + DIRECTLAYER; j++) {
            for (size_t p = 0; p < chargePoint.size(); p++) {
                Usample +=
                    gKernel(samplePoint, chargePoint[p] + EVec3(i, j, 0)) *
                    chargeValue[p];
            }
        }
    }

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1],
                               pointLEquiv[3 * p + 2]);
        UsampleSP += gKernel(samplePoint, Lpoint) * M2Lsource[p];
    }

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;
    std::cout << "Error : " << UsampleSP + Usample + 2.284722293289131159
              << std::endl;

    samplePoint = EVec3(0.5, 0.5, 0);
    Usample = 0;
    UsampleSP = 0;

    for (int i = -DIRECTLAYER; i < 1 + DIRECTLAYER; i++) {
        for (int j = -DIRECTLAYER; j < 1 + DIRECTLAYER; j++) {
            for (size_t p = 0; p < chargePoint.size(); p++) {
                Usample +=
                    gKernel(samplePoint, chargePoint[p] + EVec3(i, j, 0)) *
                    chargeValue[p];
            }
        }
    }

    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1],
                               pointLEquiv[3 * p + 2]);
        UsampleSP += gKernel(samplePoint, Lpoint) * M2Lsource[p];
    }

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;
    std::cout << "Error : " << UsampleSP + Usample - 2.284722293289131159
              << std::endl;

    return 0;
}

} // namespace Laplace2D3D

#undef DIRECTLAYER
#undef PI314


///////////////////////////////////////////////////////////////////////////////
// main.cpp
//
//  Copyright 2008 Erwann Rogard. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#include <iostream>
#include <algorithm>
#include <iterator>
#include <vector>
#include <functional>
#include <fstream>
#include <boost/mpl/size_t.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/delay.hpp>
#include <boost/accumulators/statistics/acvf_moving_average.hpp>
#include <boost/accumulators/statistics/acvf.hpp>
#include <boost/accumulators/statistics/acf.hpp>
#include <boost/accumulators/statistics/integrated_acf.hpp>
#include <boost/accumulators/statistics/integrated_acvf.hpp>
#include <boost/accumulators/statistics/percentage_effective_sample_size.hpp>
#include <boost/accumulators/statistics/standard_error_autocorrelated.hpp>
#include <boost/accumulators/statistics/standard_error_iid.hpp>
#include <boost/accumulators/statistics/acvf_analysis.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/moving_average.hpp>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/numeric/conversion/converter.hpp>
int main(){

    const char* filepath = "./acvf_output";
    std::ofstream out(filepath);

    using namespace boost::accumulators;
    typedef boost::mt19937                                    urng_type;
    typedef boost::normal_distribution<>                      nd_type;
    typedef boost::variate_generator<urng_type&,nd_type>      gen_nd_type;
    typedef double                                            value_type;
    typedef boost::random::moving_average<value_type>         ma_type;
    typedef std::vector<value_type>                           ma_vals_type;
    //typedef default_delay_discriminator                       delaydisrc;
    typedef default_delay_discriminator                       discr_t;
    typedef accumulator_set<
        value_type, stats<
            tag::acvf<discr_t>,
            tag::acf<discr_t>,
            tag::integrated_acvf<discr_t>,
            tag::percentage_effective_sample_size<discr_t>,
            tag::standard_error_autocorrelated<discr_t>,
            tag::standard_error_iid<discr_t>
            >
    >            acc_type;

    //model parameters and related quantities
    std::vector<value_type>     coeffs;
    std::vector<unsigned int>   lags;
    std::vector<value_type>     true_acfs;
    value_type                  true_integrated_acvf = 0.0;
    std::size_t                 true_ess = 0;
    //with these coeffs, should expect ess% > 100
    {using namespace boost::assign; coeffs+=1.0,-0.5,0.2; lags+=0,1,2;}
    unsigned int  K = coeffs.size()-1;
    transform(lags.begin(),lags.end(),back_inserter(true_acfs),
        make_acvf_moving_average(coeffs));
    true_integrated_acvf
        = 2*std::accumulate(true_acfs.begin(),true_acfs.end(),0.0);
    true_integrated_acvf -=  *true_acfs.begin();
    true_ess = boost::numeric::converter<std::size_t,value_type>::convert(
        100.0*true_acfs[0]/true_integrated_acvf);

    out << "->true_acvf: ";
    copy(true_acfs.begin(),true_acfs.end(),
        std::ostream_iterator<value_type>(out," "));
    out << "<-" << std::endl;
    {   value_type div = 1.0/true_acfs[0];
        transform(true_acfs.begin(),true_acfs.end(),true_acfs.begin(),
            boost::bind(std::multiplies<value_type>(),_1,div));
    }
    out << "->true_acf: ";
    copy(true_acfs.begin(),true_acfs.end(),
        std::ostream_iterator<value_type>(out," ")); out << "<-" << std::endl;
    out << "->true var: " << true_integrated_acvf << "<-" << std::endl;
    out << "->true ess%: " << true_ess << "<-" << std::endl;

    //generation of a Moving Average of order K process
    const unsigned long N = 100000;
    urng_type urng(0);
    gen_nd_type gen_nd(urng,nd_type());
    ma_type ma(boost::make_iterator_range(coeffs.begin(),coeffs.end()));
    ma_vals_type ma_vals(N);
    for(ma_vals_type::iterator i=ma_vals.begin(); i<ma_vals.end(); i++)
    { (*i) = ma(gen_nd); }

    //estimation
    acc_type acc(tag::delay<discr_t>::cache_size=(K+1));
    for_each(ma_vals.begin(),ma_vals.end(),
        boost::bind<void>(boost::ref(acc),_1));
    out << "->sample size: " << N << std::endl;
    out << "->estimated acvf: ";
    copy(begin(acvf<discr_t>(acc)),end(acvf<discr_t>(acc)),
        std::ostream_iterator<value_type>(out," "));
    out<<"<-"<<std::endl;

    out << "->estimated acf: ";
    copy(begin(acf<discr_t>(acc)),end(acf<discr_t>(acc)),
        std::ostream_iterator<value_type>(out," "));
    out<<"<-"<<std::endl;

    out << "->estimated var: "
        << integrated_acvf<discr_t>(acc) << "<-" << std::endl;

    out << "->estimated ess%: "
        << percentage_effective_sample_size<discr_t>(acc)
        << "<-" << std::endl;

    out << "->estimated standard error assuming iid: "
        << standard_error_iid<discr_t>(acc) << "<-" << std::endl;

    out << "->estimated standard error assuming acf is zero after lag "
        << K << ": "
        << standard_error_autocorrelated<discr_t>(acc) << "<-" << std::endl;

    //the above bundled into one class:
    out << " --------- ";
    out << "output from acvf_analysis:" << std::endl;
    const unsigned int offset = 0;
    const unsigned int stride = 1;
    const unsigned int assumed_lag = 3;
    statistics::acvf_analysis<value_type,discr_t> acvf_x(assumed_lag);
    acvf_x(ma_vals,offset,stride);
    acvf_x.print(out);

    std::cout << "output of libs/accumulators/main.cpp was written to"
        << filepath << std::endl;

    return 0;
}


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include "gps_conversion.h"
#include <cmath>
#include <Eigen/Dense>

namespace mars
{
std::ostream& operator<<(std::ostream& out, const GpsCoordinates& coordinates)
{
  out << "Lat:\t" << coordinates.latitude_ << std::endl;
  out << "Long:\t" << coordinates.longitude_ << std::endl;
  out << "Alt:\t" << coordinates.altitude_ << std::endl;

  return out;
}

Eigen::Matrix<double, 3, 1> mars::GpsConversion::get_enu(mars::GpsCoordinates coordinates)
{
  return WGS84ToENU(coordinates);
}

GpsConversion::GpsConversion(mars::GpsCoordinates coordinates)
{
  ecef_ref_orientation_.setIdentity();
  ecef_ref_point_.setZero();

  set_gps_reference(coordinates);
}

GpsCoordinates mars::GpsConversion::get_gps_reference()
{
  return reference_;
}

void GpsConversion::set_gps_reference(mars::GpsCoordinates coordinates)
{
  // set gps reference coordinates
  reference_ = coordinates;

  // set ecef reference, position and orientation
  const double rad_lat = deg2rad(coordinates.latitude_);
  const double rad_long = deg2rad(coordinates.longitude_);

  const double s_lat = sin(rad_lat);
  const double c_lat = cos(rad_lat);

  const double s_long = sin(rad_long);
  const double c_long = cos(rad_long);

  Eigen::Matrix3d R;
  R(0, 0) = -s_long;
  R(0, 1) = c_long;
  R(0, 2) = 0;

  R(1, 0) = -s_lat * c_long;
  R(1, 1) = -s_lat * s_long;
  R(1, 2) = c_lat;

  R(2, 0) = c_lat * c_long;
  R(2, 1) = c_lat * s_long;
  R(2, 2) = s_lat;

  ecef_ref_orientation_ = R;
  ecef_ref_point_ = WGS84ToECEF(coordinates);
}

double GpsConversion::deg2rad(const double& deg)
{
  return (M_PI / 180) * deg;
}

Eigen::Matrix<double, 3, 1> GpsConversion::WGS84ToENU(const mars::GpsCoordinates& coordinates)
{
  return ECEFToENU(WGS84ToECEF(coordinates));
}

Eigen::Matrix<double, 3, 1> GpsConversion::ECEFToENU(const Eigen::Matrix<double, 3, 1>& ecef)
{
  Eigen::Matrix<double, 3, 1> enu = ecef_ref_orientation_ * (ecef - ecef_ref_point_);
  return enu;
}

Eigen::Matrix<double, 3, 1> GpsConversion::WGS84ToECEF(const mars::GpsCoordinates& coordinates)
{
  // WGS84 ellipsoid constants
  constexpr double a = 6378137.0;             // semi-major axis
  constexpr double ecc = 8.1819190842622e-2;  // eccentricity of this ellipsoid
  constexpr double ecc_sq = ecc * ecc;

  const double rad_lat = deg2rad(coordinates.latitude_);
  const double rad_long = deg2rad(coordinates.longitude_);

  const double s_lat = sin(rad_lat);
  const double c_lat = cos(rad_lat);
  const double s_long = sin(rad_long);
  const double c_long = cos(rad_long);

  const double N = a / sqrt(1 - ecc_sq * s_lat * s_lat);

  Eigen::Matrix<double, 3, 1> ecef;
  const double h = coordinates.altitude_;
  ecef(0) = (N + h) * c_lat * c_long;
  ecef(1) = (N + h) * c_lat * s_long;
  ecef(2) = (N * (1 - ecc_sq) + h) * s_lat;

  return ecef;
}
}


/* -*-c++-*--------------------------------------------------------------------
 * 2019 Bernd Pfrommer bernd.pfrommer@gmail.com
 */

#include "tagslam/logging.h"
#include "tagslam/rpp.h"
#include "tagslam/quartic.h"
#include <boost/range/irange.hpp>
#include <iostream>
#include <math.h>

//
// Implementation of tests for checking if tag can be 'flipped',
// and a second valid pose can be obtained. This gives a measure of
// how well the tag-to-camera transform is established.
//
// See paper: "Robust Pose Estimation from a Planar Target"
// by Gerald Schweighofer and Alex Pinz
//
// Notes:
//
// 1) The paper is a bit nebulous about how to compute \tilde{R}_z^-1.
//    What it means concretely: decompose
//
//    \tilde{R}_1 = Rz(gamma) * Ry(beta) * Rz(gamma'),
//
//    and set \tilde{R}_z^{-1} = Rz(gamma'). Then
//
//    \tilde{R}_1 * \tilde{R}_z = Rz(gamma) * Ry(beta),
//
//    i.e. only z and y rotations.
//
// 2) top of page 6, beta_t = tan(1/(2*beta)) is wrong, it should
//    read beta_t = tan(beta/2)
//
// 3) I cannot reproduce their expression for the gradient. I think it's
//    wrong, but not sure. I derived my own, which works. This affects
//    equations (12) and (13) in the paper:
//
//    (12) becomes:
//
//    t_opt = -G * sum_i (I-\tilde{V}_i)^2 R_z(gamma) R_y(beta) \tilde{p}_i
//
//    with G = (sum_i (I-\tilde{V}_i)^2)^{-1}
//
//    (13) is modified accordingly:
//
//    E_os = sum_i |... - G sum_j(I -\tilde{V}_j)^2 R_z  R_y \tilde{p}_i|^2

// #define DEBUG

namespace tagslam {
  namespace rpp {
    using boost::irange;

    static double eval_poly(double x, const double *a, int n) {
      double p = 1.0;
      double sum = 0;
      for (const auto i: irange(0, n)) {
        sum += p * a[i];
        p = p * x;
      }
      return (sum);
    }

    //
    // computes rotation R_t from the paper: it rotates the optical axis
    // to face to the origin (center) of the tag.
    //
    static Transform rotate_to_z(
      const Eigen::Vector3d &translat, double *ang) {
      const double t_norm = translat.norm();
      const Eigen::Vector3d txz = translat.cross(Eigen::Vector3d::UnitZ());
      const double sin_a_t_norm = txz.norm();
      if (std::abs(sin_a_t_norm) < 1e-8) {
        return Eigen::Isometry3d::Identity();
      }
      const double sin_a = sin_a_t_norm / t_norm;
      const Eigen::Vector3d n = txz / sin_a_t_norm;
      //const Transform tf = Eigen::AngleAxisd(std::asin(sin_a), n);
      *ang = std::asin(sin_a);
      const Transform tf = (Transform) Eigen::AngleAxisd(*ang, n);
      return (tf);
    }

    typedef std::vector<Eigen::Matrix3d,
                        Eigen::aligned_allocator<Eigen::Matrix3d> > M33dVec;

    //
    // make normalized matrices \tilde{V}_i from \tilde{v}_i
    //
    static M33dVec V_from_v(const ImgPointsH &v) {
      M33dVec V(v.rows());
      for (int i = 0; i < v.rows(); i++) {
        const double vnsq = v.row(i).squaredNorm();
        if (vnsq > 1e-12) {
          V[i] = v.row(i).transpose()*v.row(i) / vnsq;
        } else {
          V[i] = Eigen::Matrix3d::Zero();
        }
      }
      return (V);
    }

    //
    // computes G from the paper (but using my formula)
    //
    static Eigen::Matrix3d compute_G(const M33dVec &V_tilde) {
      Eigen::Matrix3d ImV_sum = Eigen::Matrix3d::Zero();
      for (const auto &V_i: V_tilde) {
        const auto ImV = Eigen::Matrix3d::Identity() - V_i;
        ImV_sum = ImV_sum + ImV.transpose() * ImV;
      }
      return (ImV_sum.inverse());
    }

    //
    // little helper matrix K that extracts beta_t to various powers
    // from R_y(beta_t)*p
    //
    static Eigen::Matrix3d make_K(const Eigen::Vector3d &p) {
      Eigen::Matrix3d K;
      K <<
        p(0),  2*p(2), -p(0),
        p(1),     0.0,  p(1),
        p(2), -2*p(0), -p(2);
      return (K);
    }

    //
    // compute helper matrix FTF = F^T * F.
    //
    // The error E can be expressed then as:
    //
    // E_os(beta_t) = (1+beta_t^2)^{-2} * mu^T * (F^T * F) * mu
    //
    // where mu = [1, beta_t, beta_t^2]^T
    //

    static Eigen::Matrix3d compute_FTF(const M33dVec &V_tilde,
                                       const Transform &R_z,
                                       const ObjPoints &p_tilde) {
      const int n = p_tilde.rows(); // number of points
      const auto G = compute_G(V_tilde);
      Eigen::Matrix3d C_sum = Eigen::Matrix3d::Zero();
      M33dVec C(n);
      for (const auto i: irange(0ul, V_tilde.size())) {
        const auto ImV_i = Eigen::Matrix3d::Identity() - V_tilde[i];
        C[i] = ImV_i * R_z * make_K(p_tilde.row(i));
        C_sum = C_sum + ImV_i.transpose() * C[i];
      }
      Eigen::Matrix3d FTF = Eigen::Matrix3d::Zero();
      for (const auto i: irange(0ul, V_tilde.size())) {
        const auto ImV_i = Eigen::Matrix3d::Identity() - V_tilde[i];
        const auto F_i   = C[i] - ImV_i * G * C_sum;
        FTF = FTF + F_i.transpose() * F_i;
      }
      return (FTF);
    }

    //
    // Starting from
    //
    // E_os(beta_t) = (1+beta_t^2)^{-2} * mu^T * (F^T * F) * mu
    // where mu = [1, beta_t, beta_t^2]^T
    //
    // now express E_os(beta_t) and derivates as polynomials in beta_t
    //
    // E_os   = (1+beta^2)^{-2} * (sum_{i=0^n} f[i] beta^i)
    // E_os'  = (1+beta^2)^{-3} * (sum_{i=0^n} g[i] beta^i)
    // E_os'' = (1+beta^2)^{-4} * (sum_{i=0^n} h[i] beta^i)
    //
    //
    static void compute_polynomial(const Eigen::Matrix3d &FTF,
                                   double *f, double *g, double *h) {
      // polynomial coefficients f[0] == zeroth order etc
      //
      // E_os = (f[0] + f[1] * beta_t + ... f[4] * beta_t^4) / (1+beta_t^2)^2
      //
      f[0] = FTF(0, 0);
      f[1] = FTF(0, 1) + FTF(1, 0);
      f[2] = FTF(0, 2) + FTF(1, 1) + FTF(2, 0);
      f[3] = FTF(1, 2) + FTF(2, 1);
      f[4] = FTF(2, 2);

      // first derivate:
      g[4] = -f[3];
      g[3] = 4 * f[4] - 2 * f[2];
      g[2] = 3 * f[3] - 3 * f[1];
      g[1] = 2 * f[2] - 4 * f[0];
      g[0] = f[1];

      // second derivative:
      h[0] =  -4*f[0] +  2*f[2];
      h[1] = -12*f[1] +  6*f[3];
      h[2] =  20*f[0] - 16*f[2] + 12 * f[4];
      h[3] =  12*f[1] - 16*f[3];
      h[4] =   6*f[2] - 12*f[4];
      h[5] =   2*f[3];
    }

    //
    // Finds locations beta_t of real minima and value E there
    // Returns number of minima found.
    //

    static int find_minima(
      const double *f, // poly coeff for E_os
      const double *g, // poly coeff first deriv
      const double *h, // poly coeff second deriv
      double *beta_min, double *beta_max, double *E_min, double *E_max) {
      std::complex<double> root[4];
      // find roots of first derivative
      const int nroots = quartic::solve_quartic(g[4],g[3],g[2],g[1],g[0],root);

      int n_min(0);
      *E_min = 1e90;
      *E_max = -1e90;
      // check all real roots, and evaluate second deriv there
      for (const auto i:irange(0, nroots)) {
        if (std::imag(root[i]) < 1e-8) {
          const double beta_t = std::real(root[i]);
          if (eval_poly(beta_t, h, 6) > 0) {
            const double opbs = (1.0 + beta_t * beta_t);
            const double E_os = eval_poly(beta_t, f, 5)/(opbs * opbs);
            if (E_os < *E_min) {
              *E_min = E_os;
              *beta_min = 2.0 * std::atan(beta_t);
            }
            if (E_os > *E_max) {
              *E_max = E_os;
              *beta_max = 2.0 * std::atan(beta_t);
            }
            n_min++;
          }
        }
      }
      return (n_min);
    }

    //
    // computes ratio of error for lowest/(second lowest) minimum error
    // orientation. The lower the ratio, the better is the pose
    // established, the more robust it is to flipping.
    //
    double check_quality(const ImgPoints &ip, const ObjPoints &op,
                         const Transform &T, double *beta_orig,
                         double *beta_min, double *beta_max) {
      const ImgPointsH iph = ip.rowwise().homogeneous();
      // compute R_t, the matrix that rotates z to the optical axis
      double ang;
      const Transform R_t = rotate_to_z(T.translation(), &ang);
      const auto R1_tilde = R_t * T;
      // decompose R1_tilde = Rz * Ry * Rz0
      const auto   euler_angles = R1_tilde.rotation().eulerAngles(2, 1, 2);
      const double gamma  = euler_angles[0]; // z  rotation
      const double beta   = euler_angles[1]; // y  rotation
      *beta_orig = beta;
      const double alpha  = euler_angles[2]; // z0 rotation
      const Transform R_z = (Transform)
        Eigen::AngleAxisd(gamma, Eigen::Vector3d::UnitZ());
      const Transform R_z0 = (Transform)
        Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitZ());
      // compute v_tilde from equation (5)
      const ImgPointsH v_tilde = (R_t.rotation()*iph.transpose()).transpose();
      // compute p_tilde
      const ObjPoints p_tilde  = (R_z0.rotation()*op.transpose()).transpose();
      M33dVec V_tilde = V_from_v(v_tilde);
      const Eigen::Matrix3d FTF = compute_FTF(V_tilde, R_z, p_tilde);
      double f[5], g[5], h[6];
      compute_polynomial(FTF, f, g, h);
      double E_min, E_max;
      int n_min = find_minima(f, g, h, beta_min, beta_max, &E_min, &E_max);
      switch (n_min) {
      case 2:
        return (E_min / E_max); // two minima, the usual case
        break;
      case 1:
        return (0.0); // single minimum, assume all is good
        break;
      default:
        ROS_WARN_STREAM("found bad num minima: " << n_min);
        *beta_min = beta;
        *beta_max = beta;
        return (0.0); // close eyes and hope for the best....
        break;
      }
      return (0.0); // should never reach this
    }
  } // end of namespace rpp
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__INTERNAL__LMPAR_SPARSE_HPP_
#define SMOOTH__INTERNAL__LMPAR_SPARSE_HPP_

#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

namespace smooth::detail {

/**
 * @brief Calculate the value and derivative of the function
 * \f[
 * \phi(\alpha) = \left\| D (J^T J + \alpha D^T D)^{-1} J^T r \right\| - \Delta
 * \f]
 *
 * @param J sparse matrix size MxN
 * @param d vector size N representing diagonal of D
 * @param r vector size M
 * @param Delta scalar
 * @param alpha scalar
 *
 * @return Triplet \f$(x, \phi(\alpha), \phi'(\alpha))\f$ where \f$x\f$ is a solution to \f$ J^T J +
 * \alpha D^T D = -J^T r\f$.
 */
template<int N, int M>
std::tuple<Eigen::Vector<double, N>, double, double> calc_phi(
  const auto & J,
  const Eigen::Vector<double, N> & d,
  const Eigen::Vector<double, M> & r,
  double Delta,
  double alpha)
{
  const auto n = J.cols();

  Eigen::SparseMatrix<double> lhs = J.transpose() * J;

  lhs.reserve(Eigen::Vector<double, N>::Ones(n));
  if (alpha > 0) {
    for (auto i = 0u; i != n; ++i) { lhs.coeffRef(i, i) += alpha * d(i) * d(i); }
  }
  lhs.makeCompressed();

  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> ldlt;
  ldlt.compute(lhs);

  if (ldlt.info()) {
    // computation failed, add small diagonal to ensure positive definiteness
    for (auto i = 0u; i != n; ++i) { lhs.coeffRef(i, i) += Eigen::NumTraits<double>::epsilon(); }
    ldlt.compute(lhs);
  }

  // calculate q
  const Eigen::Vector<double, N> x = ldlt.solve(-J.transpose() * r);
  const Eigen::Vector<double, N> q = -d.cwiseProduct(x);

  // calculate phi
  const double phi = q.stableNorm() - Delta;

  // calculate dphi
  const Eigen::Vector<double, N> d_q = d.cwiseProduct(q);
  const Eigen::Vector<double, N> y   = ldlt.solve(d_q);
  const double dphi                  = -d.cwiseProduct(q.normalized()).dot(y);

  return std::make_tuple(x, phi, dphi);
}

/**
 * @brief Approximate a Levenberg-Marquardt parameter lambda s.t. if x solves
 *
 *   \| [J; sqrt(lambda) * diag(d)] x  +  [r ; 0] \|^2
 *
 * then either
 *  * lambda = 0 AND \|diag(d) * x\| <= 1.1 Delta
 *    OR
 *  * lambda > 0 AND 0.9 Delta <= \|diag(d) * x\| <= 1.1 Delta
 *
 * @param J sparse matrix MxN
 * @param d vector size Nx1
 * @param r vector size Mx1
 * @param Delta scalar
 *
 * @return pair(lambda, x) where x solves the least-squares problem for lambda
 */
template<int N, int M>
std::pair<double, Eigen::Vector<double, N>> lmpar_sparse(
  const auto & J,
  const Eigen::Vector<double, N> & d,
  const Eigen::Matrix<double, M, 1> & r,
  double Delta)
{
  double alpha = 0;

  auto [x, phi, dphi] = calc_phi(J, d, r, Delta, alpha);

  if (phi <= 0.1 * Delta) {
    return std::make_pair(0, std::move(x));  // alpha = 0 solution fulfills condition
  }

  // initialize bounds
  double l = std::max<double>(0, -phi / dphi);
  double u = (d.cwiseInverse().cwiseProduct(J.transpose() * r)).stableNorm() / Delta;

  // it typically converges in 2 or 3 iterations
  for (auto i = 0u; i != 20; ++i) {
    // ensure alpha stays within bounds (and not equal to zero)
    if (!(l < alpha && alpha < u)) { alpha = std::max<double>(0.001 * u, sqrt(l * u)); }

    std::tie(x, phi, dphi) = calc_phi(J, d, r, Delta, alpha);

    if (std::abs(phi) <= 0.1 * Delta) {
      break;  // condition fulfilled
    }

    // update bounds
    l = std::max<double>(l, alpha - phi / dphi);
    if (phi < 0) { u = alpha; }

    // update alpha
    alpha = alpha - ((phi + Delta) / Delta) * (phi / dphi);
  }

  return std::make_pair(alpha, std::move(x));
}

}  // namespace smooth::detail

#endif  // SMOOTH__INTERNAL__LMPAR_SPARSE_HPP_


// Copyright 2017 David Wise
#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/KroneckerProduct>
#include "donorClass.h"

using namespace Eigen;

void Donor::setNucSpin(const double value) {
    Donor::coeffs.clear();
    Donor::nucSpin = value;
    setSpinsMats();
}
double Donor::getNucSpin() {
	return nucSpin;
}
void Donor::setHypCoup(const double value) {
	Donor::A = value;
}
double Donor::getHypCoup() {
	return A;
}
void Donor::setSpinsMats() {
//  Set up Electron spin operators
    Donor::coeffs.clear();
    Donor::Sx << 0,1,1,0;
    Donor::Sx *= (h_bar/2);
    Donor::Sy << 0,-i,i,0;
    Donor::Sy *= h_bar/2;
    Donor::Sz << 1,0,0,-1;
    Donor::Sz *= h_bar/2;
    Donor::IdS = MatrixXcd::Identity(2*nucSpin+1, 2*nucSpin+1);
    Donor::IdI = MatrixXcd::Identity(2, 2);

//  Set up Nuclear spin operators

    for (int inc = 1; inc < (2*nucSpin)+1; ++inc) {
        std::complex<double> cInc = sqrt(2*nucSpin*inc + inc*(1-inc));
//        std::cout << "Coeff "<< inc << " is \n" << cInc << "\n";
        Donor::coeffs.push_back(cInc);
    }

    Donor::Icr.resize(nucSpin*2+1, nucSpin*2+1);
    Donor::Ian.resize(nucSpin*2+1, nucSpin*2+1);
    for (int inc = 0; inc <= (2*nucSpin-1); ++inc) {
        Donor::Icr(inc, inc+1) = Donor::coeffs[inc];
        Donor::Ian(inc+1, inc) = Donor::coeffs[inc];
    }

    Donor::Ix = h_bar*(1.0/2.0)*(Icr+Ian);
    Donor::Iy = h_bar*(-i/(2.0))*(Icr-Ian);
    Donor::Iz = (-i/h_bar)*((Ix*Iy) - (Iy*Ix));

    Donor::Sx_f = kroneckerProduct(Sx, IdS);
    Donor::Sy_f = kroneckerProduct(Sy, IdS);
    Donor::Sz_f = kroneckerProduct(Sz, IdS);
    Donor::Ix_f = kroneckerProduct(IdI, Ix);
    Donor::Iy_f = kroneckerProduct(IdI, Iy);
    Donor::Iz_f = kroneckerProduct(IdI, Iz);

    Donor::S_I = kroneckerProduct(Sx, Ix) + kroneckerProduct(Sy, Iy) + kroneckerProduct(Sz, Iz);

}

MatrixXcd Donor::getEigs(const double B_0) {
    Donor::Ham = (ge*mu_e/h_bar)*B_0*Sz_f - (gn*mu_n/h_bar)*B_0*Iz_f + A/(pow(h_bar,2))*S_I;
    ComplexEigenSolver<MatrixXcd> es(Ham);
    es.compute(Ham);
    return es.eigenvalues();
}

void Donor::initialise(double nucVal, double hypVal) {
    if (floor(2*nucVal) != 2*nucVal) {
        throw std::invalid_argument("Please use integer or half-integer value");
    };
    Donor::nucSpin = nucVal;
    Donor::A = hypVal;
    Donor::setSpinsMats();
}


#include "amr_momentum_solver.h"

#include <cmath>
#include <Eigen/Dense>

#include "../../tools/cppitertools/zip.hpp"
#include "../../definitions.h"

using namespace Eigen;
using iter::zip;

/// Get snapshot current J_i^n+1 from momentum distribution
template< typename T, int D, int V>
void vlv::MomentumSolver<T,D,V>::update_future_current( vlv::Tile<D>& tile, T cfl)
{
  //auto& yee = tile.get_yee();
  tile.jx1.clear();

  auto& step0 = tile.steps.get(0);
  for(auto&& block0 : step0) {

    auto Nx = int(block0.Nx),
         Ny = int(block0.Ny),
         Nz = int(block0.Nz);

    for (int s=0; s<Nz; s++) {
      for(int r=0; r<Ny; r++) {
        for(int q=0; q<Nx; q++) {
          const auto& M   = block0.block(q,r,s);   // f_i

          T qm = 1.0 / block0.qm;  // charge to mass ratio

          // Jx current; chi(u) = u/gamma = v
          //
          // NOTE: needs to be given in units of grid speed
          //       so we scale with dt/dx
          //yee.jx1(q,r,s) += qm*
          tile.jx1(q,r,s) += qm*cfl*
            integrate_moment(
                M,
                [](std::array<T,3> uvel) -> T
                { return uvel[0]/gamma<T,3>(uvel); }
                );
        }
      }
    }

  }// end of loop over species

  }

/*! \brief Solve Vlasov tile contents
 *
 * Exposes the actual momentum mesh from the Vlasov Tile containers
 * and feeds those to the mesh solver.
 */
template< typename T, int D, int V>
void vlv::MomentumSolver<T,D,V>::solve( vlv::Tile<D>& tile, T step_size)
{

  // init lock/mutex for mesh.clear()

  // get reference to the Vlasov fluid that we are solving
  auto& step0 = tile.steps.get(0);
  auto& step1 = tile.steps.get(1);

  // get reference to the Yee grid
  auto& yee = tile.get_yee();

  // timestep
  //auto dt   = (T) tile.dt;
  //auto dx   = (T) tile.dx;
  //T cfl  = step_size*dt/dx;
  auto cfl = step_size*tile.cfl;

  // block limits
  auto mins = tile.mins;
  //auto maxs = tile.maxs;

  /// Now get future current
  update_future_current(tile, cfl);

  // param object for solve_mesh
  vlv::tools::Params<T> params = {};
  params.cfl = cfl;

  // loop over different particle species (zips current [0] and new [1] solutions)
  for(auto&& blocks : zip(step0, step1) ) {

    // loop over the tile's internal grid
    auto& block0 = std::get<0>(blocks);
    auto& block1 = std::get<1>(blocks);

    for(int q=0; q<block0.Nx; q++) {
      for(int r=0; r<block0.Ny; r++) {
        for(int s=0; s<block0.Nz; s++) {
          T qm = 1.0 / block0.qm;  // charge to mass ratio

          // Get local field components
          vec
            B =
            {{
               (T) yee.bx(q,r,s),
               (T) yee.by(q,r,s),
               (T) yee.bz(q,r,s)
            }},

            // E-field interpolated to the middle of the tile
            // E_i = (E_i+1/2 + E_i-1/2)
            // XXX
            E =
            {{
               (T) (0.5*(yee.ex(q,r,s) + yee.ex(q-1,r,   s  ))),
               (T) (0.5*(yee.ey(q,r,s) + yee.ey(q,  r-1, s  ))),
               (T) (0.5*(yee.ez(q,r,s) + yee.ez(q,  r,   s-1)))
            }};

          // Now push E field to future temporarily
          //E[0] -= yee.jx1(q,r,s) * 0.5;

          // dig out velomeshes from blocks
          auto& mesh0 = block0.block(q,r,s);
          auto& mesh1 = block1.block(q,r,s);

          // fmt::print("solving for srq ({},{},{})\n",s,r,q);
          params.qm = qm;
          params.xloc = mins[0] + static_cast<T>(q);

          // then the final call to the actual mesh solver
          solve_mesh( mesh0, mesh1, E, B, params);
        }
      }
    }
  }

  // XXX update jx1 for debug
  //update_future_current(tile, cfl);
  //

  }

//--------------------------------------------------
// explicit template instantiation
template class vlv::MomentumSolver<Realf, 1, 1>;


#include "wave/optimization/ceres/odom_gp/point_to_line_gp.hpp"
#include <Eigen/QR>

namespace wave {

SE3PointToLineGP::SE3PointToLineGP(const double *const p,
                                   const double *const pA,
                                   const double *const pB,
                                   SE3PointToLineGPObjects &objects,
                                   const Mat3 &CovZ,
                                   bool calculate_weight)
    : pt(p), ptA(pA), ptB(pB), objects(objects) {
    this->objects.JP_T.setZero();
    this->objects.JP_T.block<3, 3>(0, 3).setIdentity();

    this->diff[0] = this->ptB[0] - this->ptA[0];
    this->diff[1] = this->ptB[1] - this->ptA[1];
    this->diff[2] = this->ptB[2] - this->ptA[2];
    this->bottom = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2];

    if (this->bottom < 1e-10) {
        // The points defining the line are too close to each other
        throw std::out_of_range("Points defining line are too close!");
    }

    this->objects.Jres_P(0, 0) = 1 - (diff[0] * diff[0] / bottom);
    this->objects.Jres_P(0, 1) = -(diff[0] * diff[1] / bottom);
    this->objects.Jres_P(0, 2) = -(diff[0] * diff[2] / bottom);
    this->objects.Jres_P(1, 0) = -(diff[1] * diff[0] / bottom);
    this->objects.Jres_P(1, 1) = 1 - (diff[1] * diff[1] / bottom);
    this->objects.Jres_P(1, 2) = -(diff[1] * diff[2] / bottom);
    this->objects.Jres_P(2, 0) = -(diff[2] * diff[0] / bottom);
    this->objects.Jres_P(2, 1) = -(diff[2] * diff[1] / bottom);
    this->objects.Jres_P(2, 2) = 1 - (diff[2] * diff[2] / bottom);

    Eigen::Vector3d unitdiff;
    double invlength = 1.0 / sqrt(this->bottom);
    if (this->diff[2] > 0) {
        unitdiff[0] = this->diff[0] * invlength;
        unitdiff[1] = this->diff[1] * invlength;
        unitdiff[2] = this->diff[2] * invlength;
    } else {
        unitdiff[0] = -this->diff[0] * invlength;
        unitdiff[1] = -this->diff[1] * invlength;
        unitdiff[2] = -this->diff[2] * invlength;
    }

    Eigen::Vector3d unitz;
    unitz << 0, 0, 1;

    auto v = unitdiff.cross(unitz);
    auto s = v.norm();
    auto c = unitz.dot(unitdiff);
    auto skew = Transformation<>::skewSymmetric3(v);
    this->objects.rotation = Eigen::Matrix3d::Identity() + skew + skew * skew * ((1 - c) / (s * s));

    this->objects.Jres_P = this->objects.rotation * this->objects.Jres_P;

    if (calculate_weight) {
        auto rotated = this->objects.Jres_P * CovZ * this->objects.Jres_P.transpose();
        this->weight_matrix = rotated.block<2, 2>(0, 0).inverse().sqrt();
    } else {
        this->weight_matrix.setIdentity();
    }
}

bool SE3PointToLineGP::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    Eigen::Map<const Mat34> tk_map(parameters[0], 3, 4);
    Eigen::Map<const Mat34> tkp1_map(parameters[1], 3, 4);

    Transformation<Eigen::Map<const Mat34>, true> Tk(tk_map);
    Transformation<Eigen::Map<const Mat34>, true> Tkp1(tkp1_map);

    Eigen::Map<const Vec6> vel_k(parameters[2], 6, 1);
    Eigen::Map<const Vec6> vel_kp1(parameters[3], 6, 1);

    if (jacobians) {
        Transformation<Mat34, true>::interpolateAndJacobians(Tk,
                                                             Tkp1,
                                                             vel_k,
                                                             vel_kp1,
                                                             this->objects.hat,
                                                             this->objects.candle,
                                                             this->objects.T_current,
                                                             this->objects.JT_Ti,
                                                             this->objects.JT_Tip1,
                                                             this->objects.JT_Wi,
                                                             this->objects.JT_Wip1);
    } else {
        Transformation<Mat34, true>::interpolate(
          Tk, Tkp1, vel_k, vel_kp1, this->objects.hat, this->objects.candle, this->objects.T_current);
    }

    Eigen::Map<const Vec3> PT(this->pt, 3, 1);
    Vec3 point = this->objects.T_current.transform(PT);

    double p_A[3] = {point(0) - this->ptA[0], point(1) - this->ptA[1], point(2) - this->ptA[2]};

    double scaling = ceres::DotProduct(p_A, diff);
    // point on line closest to point
    double p_Tl[3] = {this->ptA[0] + (scaling / bottom) * diff[0],
                      this->ptA[1] + (scaling / bottom) * diff[1],
                      this->ptA[2] + (scaling / bottom) * diff[2]};

    Eigen::Map<const Vec3> pt_Tl(p_Tl, 3, 1);
    Eigen::Map<Eigen::Vector2d> reduced(residuals, 2, 1);

    reduced = this->weight_matrix * (this->objects.rotation * (point - pt_Tl)).block<2, 1>(0, 0);

    if (jacobians != nullptr) {
        this->objects.JP_T(0, 1) = point(2);
        this->objects.JP_T(0, 2) = -point(1);
        this->objects.JP_T(1, 0) = -point(2);
        this->objects.JP_T(1, 2) = point(0);
        this->objects.JP_T(2, 0) = point(1);
        this->objects.JP_T(2, 1) = -point(0);

        // Jres_P already has rotation incorporated during construction
        this->objects.Jr_T = this->objects.Jres_P * this->objects.JP_T;

        if (jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> Jr_Tk(jacobians[0], 2, 12);
            Jr_Tk.block<2, 6>(0, 0) = this->weight_matrix * this->objects.Jr_T.block<2, 6>(0, 0) * this->objects.JT_Ti;
            Jr_Tk.block<2, 6>(0, 6).setZero();
        }
        if (jacobians[1]) {
            Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>> Jr_Tkp1(jacobians[1], 2, 12);
            Jr_Tkp1.block<2, 6>(0, 0) = this->weight_matrix * this->objects.Jr_T.block<2, 6>(0, 0) * this->objects.JT_Tip1;
            Jr_Tkp1.block<2, 6>(0, 6).setZero();
        }
        if (jacobians[2]) {
            Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> jac_map(jacobians[2], 2, 6);
            jac_map = this->weight_matrix * this->objects.Jr_T.block<2, 6>(0, 0) * this->objects.JT_Wi;
        }
        if (jacobians[3]) {
            Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor>> jac_map(jacobians[3], 2, 6);
            jac_map = this->weight_matrix * this->objects.Jr_T.block<2, 6>(0, 0) * this->objects.JT_Wip1;
        }
    }

    return true;
}

}  // namespace wave


#include "Tests_pcp.h"

#include <fstream>
#include <Eigen/Dense>
#include "Tresca.h"
#include "test_material_models.h"

namespace
{
	enum class AnalysisType : unsigned char
	{
		TriaxialDrained = 0,
		TriaxialUndrained = 1,
		Consolidation = 2,
		Specified = 3
	};

	void output_var(
		std::ostream &os,
		double cohesion,
		const double out_strain[3],
		const double out_stress[6],
		const double out_pstrain[3]
		)
	{
		Eigen::Matrix3d s_mat;
		s_mat << out_stress[0], out_stress[3], out_stress[5],
				 out_stress[3], out_stress[1], out_stress[4],
				 out_stress[5], out_stress[4], out_stress[2];
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(s_mat);
		const Eigen::Vector3d& pstress = eigen_solver.eigenvalues();
		double smax = pstress[0];
		if (smax < pstress[1])
			smax = pstress[1];
		if (smax < pstress[2])
			smax = pstress[2];
		double smin = pstress[0];
		if (smin > pstress[1])
			smin = pstress[1];
		if (smin > pstress[2])
			smin = pstress[2];
		os << out_strain[0] << ", "
			<< out_strain[1] << ", "
			<< out_strain[2] << ", "
			<< out_stress[0] << ", "
			<< out_stress[1] << ", "
			<< out_stress[2] << ", "
			<< out_stress[3] << ", "
			<< out_stress[4] << ", "
			<< out_stress[5] << ", "
			<< out_pstrain[0] << ", "
			<< out_pstrain[1] << ", "
			<< out_pstrain[2] << ", "
			<< smax - smin - 2.0 * cohesion << "\n";
	}
}

void test_tresca()
{
	double de11, de22, de33;
	//AnalysisType tp = AnalysisType::Specified;
	AnalysisType tp = AnalysisType::TriaxialDrained;
	// tresca 1
	//de11 = -0.05;
	//de22 = 0.0;
	//de33 = 0.0;
	// tresca 2
	de11 = 0.05;
	de22 = 0.0;
	de33 = 0.0;
	// tresca 3
	//de11 = 0.05;
	//de22 = 0.0;
	//de33 = -0.05;
	size_t inc_num = 5000;
	size_t out_num = 100;

	double ini_stress[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
	MatModel::Tresca tc;
	tc.set_param(1000.0, 0.1, 1.0, ini_stress);

	std::fstream res_file;
	res_file.open("Tresca_res.csv", std::ios::out | std::ios::binary);
	res_file << "e11, e22, e33, s11, s22, s33, s12, s23, s31,"
				"dep11, dep22, dep33, f\n";

	de11 /= double(inc_num);
	de22 /= double(inc_num);
	de33 /= double(inc_num);
	size_t out_inv = inc_num / out_num;
	double out_strain[3] = { 0.0, 0.0, 0.0 };
	const double(*Dep_mat)[6];
	double dstrain[6];
	for (size_t i = 0; i < inc_num; ++i)
	{
		if (i % out_inv == 0) // output
			output_var(res_file, tc.get_cohesion(), out_strain, tc.get_stress(), tc.get_dstrain_p());

		switch (tp)
		{
		case AnalysisType::TriaxialDrained:
			Dep_mat = reinterpret_cast<const double(*)[6]>(tc.get_Dep_mat());
			dstrain[0] = de11;
			dstrain[1] = -Dep_mat[1][0] / (Dep_mat[1][1] + Dep_mat[1][2]) * de11;
			dstrain[2] = -Dep_mat[2][0] / (Dep_mat[2][1] + Dep_mat[2][2]) * de11;
			dstrain[3] = 0.0;
			dstrain[4] = 0.0;
			dstrain[5] = 0.0;
			break;
		case AnalysisType::TriaxialUndrained:
			dstrain[0] = de11;
			dstrain[1] = -0.5 * de11;
			dstrain[2] = -0.5 * de11;
			dstrain[3] = 0.0;
			dstrain[4] = 0.0;
			dstrain[5] = 0.0;
			break;
		case AnalysisType::Consolidation:
			dstrain[0] = de11;
			dstrain[1] = 0.0;
			dstrain[2] = 0.0;
			dstrain[3] = 0.0;
			dstrain[4] = 0.0;
			dstrain[5] = 0.0;
			break;
		case AnalysisType::Specified:
			dstrain[0] = de11;
			dstrain[1] = de22;
			dstrain[2] = de33;
			dstrain[3] = 0.0;
			dstrain[4] = 0.0;
			dstrain[5] = 0.0;
			break;
		default:
			res_file.close();
			return;
		}

		int res = tc.integrate(dstrain);

		out_strain[0] += dstrain[0];
		out_strain[1] += dstrain[1];
		out_strain[2] += dstrain[2];
	}

	output_var(res_file, tc.get_cohesion(), out_strain, tc.get_stress(), tc.get_dstrain_p());
	res_file.close();
}


#include "additional_functions.h"
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

int ipow(int base, int exp)
{
	int result = 1;
	while (exp)
	{
		if (exp & 1)
		{
			result *= base;
		}
		exp >>= 1;
		base *= base;
	}

	return result;
}

bool stationaritycheck(const double n[], int d)
{
	bool indicator = false;
	Eigen::MatrixXd stat_mat(d, d);
	for (int i = 0; i != d; ++i)
	{
		for (int j = 0; j != d; ++j)
			stat_mat(i, j) = n[i * d + j];
	}
	Eigen::EigenSolver<Eigen::MatrixXd> eigsol;
	eigsol.compute(stat_mat, false);
	Eigen::VectorXd eigsol_real = eigsol.eigenvalues().real();
	for (int i = 0; i != d; ++i)
	{
		if (fabs(eigsol_real(i)) >= 1)
		{
			indicator = true;
		}
	}

	return indicator;
}

bool stationaritycheck(const double x[], const double y[], int d)
{
	bool indicator = false;
	Eigen::MatrixXd stat_mat(d, d);
	for (int i = 0; i != d; ++i)
	{
		for (int j = 0; j != d; ++j)
		{
			stat_mat(i, j) = x[i * d + j] / y[i * d + j];
		}
	}
	Eigen::EigenSolver<Eigen::MatrixXd> eigsol;
	eigsol.compute(stat_mat, false);
	Eigen::VectorXd eigsol_real = eigsol.eigenvalues().real();
	for (int i = 0; i != d; ++i)
	{
		if (fabs(eigsol_real(i)) >= 1)
		{
			indicator = true;
		}
	}

	return indicator;
}

double random_check() {
	double random = rand();
	if (random == 0 || random == RAND_MAX)
	{
		return random_check();
	}
	else
	{
		return random;
	}
}


/**
 * Implementation of Rules to eliminate
 * Design points. Check Risk-Base Allocation.pptx
 * for more details.
 */
#include <iostream>
#include <vector>
#include <set>
#include <iterator>
#include <math.h>
#include <chrono>
#include <exception>
#include <queue>
#include <boost/math/distributions/normal.hpp>
#include <sys/time.h>
#include "ptss_dse.hpp"

/* HI - Test Editing */
using namespace std;
using boost::math::normal;

// a == b ?
bool is_eq(const alloc_t &a, const alloc_t &b) {
    return (a == b);
}

// a > b ?
bool is_gt(const alloc_t &a, const alloc_t &b) {
    bool ret = !(is_eq(a,b));
    int idx = 0;
    for(auto it = a.begin();
        it != a.end();
        it++) {
            if (*it >= b[idx++])
                ret = ret && true;
            else
                ret = ret && false;
    }
    return ret;
}

// a < b ?
bool is_lt(const alloc_t &a, const alloc_t &b) {
    bool ret = !(is_eq(a,b));
    int idx = 0;
    for(auto it = a.begin();
        it != a.end();
        it++) {
            if (*it <= b[idx++])
                ret = ret && true;
            else
                ret = ret && false;
    }
    return ret;
}

// a and b are not comparable
bool is_incomparable(const alloc_t &a, const alloc_t &b) {
    bool ret = (!is_gt(a,b)) && \
               (!is_lt(a,b)) && \
               (!is_eq(a,b));
    return ret;
}

// lexicographic comparison of two allocations ( a < b ? )
bool lex_comp(const alloc_t &a, const alloc_t &b) {
    auto it2 = b.begin();
    for (auto it = a.begin(); it != a.end(); it++, it2++) {
        if (*it2 < *it)
            return false;
    }
    return true;
}

// just for the sample -- print the data sets
std::ostream& operator<<(std::ostream& os, const alloc_t& vi) {
  os << "(|";
  std::copy(vi.begin(), vi.end(), std::ostream_iterator<int>(os, "|"));
  os << ")";
  return os;
}

std::ostream& operator<<(std::ostream& os, const all_alloc_t& vvi) {
  os << "{\n";
  for(auto it = vvi.begin();
      it != vvi.end();
      it++) {
      os << "  " << *it << "\n";
  }
  os << "}";
  return os;
}

// Profiled Data
static double mua  [] = {269.568837,	141.985532, 99.508883, 79.006262, 66.454092, 58.649068, 52.804705, 48.960141, 45.73406, 43.455542, 41.386889, 40.046129, 38.695398, 37.750183, 36.840822, 36.41925, 35.715777, 35.350802, 34.767695, 34.493228, 34.033093, 33.944396, 33.676262, 33.871417, 33.654013, 33.737325, 33.825452, 34.021759, 34.223725, 34.479067, 34.694037, 35.157394};
static double stda [] = {0.676473946, 0.335842225, 0.208458629, 0.319806191, 0.313754681, 0.340024999, 0.340440891, 0.441634464, 0.424892928, 0.45393722, 0.470011702, 0.479161768, 0.496199557, 0.493329504, 0.524060111, 0.523248507, 0.564837145, 0.590188106, 0.584813646, 0.602546264, 0.597015913, 0.600979201, 0.59552246, 0.595876665, 0.587283577, 0.645952011, 0.666749578, 0.732030737, 0.836999403, 0.942281274, 1.046115194, 1.103043517};

double compute_risk(const alloc_t &x) {
    double mu = 0.0, var = 0.0;
    for(auto jt = x.begin();
        jt != x.end();
        jt++) {
                mu  += mua[*jt-1];
                var += (stda[*jt-1])*(stda[*jt-1]);
    }
    /* Create a normal distribution and evaluate the risk */
    double sd = sqrt(var);
    normal dist(mu,sd);

    double risk = 1 - cdf(dist,D);
    return risk;
}

double compute_execution_time(const alloc_t &x) {
    double sum = 0.0;
    for(auto jt = x.begin(); jt != x.end(); jt++) {
        sum += mua[*jt-1];
    }
    return sum;
}

double compute_estimated_util(const alloc_t &x) {
    double util = 0;
    for(auto jt = x.begin();
        jt != x.end();
        jt++) {
            util += (*jt) * mua[*jt-1];
    }
    return util;
}

/* recursively construct and add allocations */
void construct_alloc(all_alloc_t &vvi, int ph) {
    alloc_t vi2;
    static long unsigned int cnt = 0;
    long unsigned int r   = M;
    long unsigned int dec;
    long unsigned int j   = 0;

    if (ph == NPH) {
        //cout << "ph : " << ph << ", invoc : " << cnt << endl;
        dec = cnt++;

        /* Resolve cnt into M-radix number */
        for (j = 0; j < NPH; j++) {
            //cout << dec%r + 1 << ",";
            vi2.push_back(dec%r + 1);
            dec = dec/r;
        }
        vvi.insert(vi2);
        //cout << "}\n";
        return;
    }
    for (int m = 1; m <= M; m++) {
        construct_alloc(vvi,ph+1);
    }
}

void ptss_DSE::display() {
    cout << this->search_space << "\n";
    cout << "Lower : ";
    cout << this->lower << "\n";
    cout << "Upper : ";
    cout << this->upper << "\n";
}

// Create All points
ptss_DSE::ptss_DSE() {
    construct_alloc(this->search_space,0);
    this->is_initialized = true;
    this->dmr = 1.0;
}

ptss_DSE::ptss_DSE(double dmr) {
    construct_alloc(this->search_space,0);
    this->init_point(dmr);
    this->is_initialized = true;
    this->dmr = dmr;
}

// Evaluate all points
void ptss_DSE::evaluate_all() {
    this->opt_point = this->lower;
    this->opt_util  = 10e9;
    this->opt_risk  = 1;

    // cout << "Search space size " << search_space.size();
    for(auto it = search_space.begin();
        it != search_space.end();
        it++) {

        double risk = compute_risk(*it);
        double util = compute_estimated_util(*it);

        if (risk <= this->dmr && util <= this->opt_util) {
            this->opt_point = *it;
            this->opt_util  = util;
            this->opt_risk  = risk;
        }
        // cout << *it << "," << risk << "," << util << "\n";
        usleep(50);
    }
    cout << this->opt_point << "," << this->opt_risk << "," << this->opt_util << "\n";
}

void ptss_DSE::init_point(double dmr) {
    alloc_t tmp;
    double risk;
    int m;

    for (m = 1; m <= M; m++) {
        tmp.clear();

        // Create a uniform allocation
        for (int ph = 0; ph < NPH; ph++)
            tmp.push_back(m);

        // Compute the risk
        risk = compute_risk(tmp);

        if (risk <= dmr) {
            this->upper = tmp;
            break;
        }
    }

    tmp.clear();
    for (int ph = 0; ph < NPH; ph++)
        tmp.push_back(m-1);
    this->lower = tmp;

    cout << "lower" << this->lower << ", dmr = " << compute_risk(this->lower) << endl;
    cout << "upper" << this->upper << ", dmr = " << compute_risk(this->upper) << endl;
}

/*
 * The action to generate a
 * set of all (child) points is to
 * select a src and dst from
 * startpoint and
 * transfer a single core
 * from one src to destitation.
 */
void epsilon_move2_rule1(all_alloc_t &children,\
                         const alloc_t startpoint,\
                         const all_alloc_t fbidden) {
    /* Expand all the elements of the frontier */
    for (int idx = 0; idx < NPH; idx++) {
        alloc_t tmp2  = startpoint;
        if (tmp2[idx] > 1) {
            tmp2[idx]--;

            /* Inert only when it doesn't exist in forbidden set */
            // if (fbidden.find(tmp2) == fbidden.end())
                children.insert(tmp2);
        }
    }
}

void epsilon_move2_rule2(all_alloc_t &children,\
                         const alloc_t startpoint,\
                         const all_alloc_t fbidden) {
     /* Expand all the elements of the frontier */
    for (int idx = 0; idx < NPH; idx++) {
        alloc_t tmp2  = startpoint;
        if (tmp2[idx] < M) {
            tmp2[idx]++;

            /* Inert only when it doesn't exist in forbidden set */
            // if (fbidden.find(tmp2) == fbidden.end())
                children.insert(tmp2);
        }
    }
}

void epsilon_move2_rule3(all_alloc_t &children,\
                         const alloc_t startpoint,\
                         const all_alloc_t fbidden) {
    int i, j;

    for (i = 0; i < NPH; i++) {
        for (j = 0; j < NPH; j++) {
            if (i != j) {
                if (startpoint[i] > 1) {
                    /* Transfer a core from i to j */
                    alloc_t new_point = startpoint;
                    new_point[i]--;
                    new_point[j]++;

                    /* Check Execution Time before insert */
                    if (compute_execution_time(new_point) > compute_execution_time(startpoint)) {
                        // if (fbidden.find(new_point) == fbidden.end())
                            children.insert(new_point);

                        /* Also insert other points derived from it */
                        alloc_t new_point2 = new_point;
                        while (--new_point2[i] > 0) {
                            if (++new_point2[j] <= NPH)
                                // if (fbidden.find(new_point2) == fbidden.end())
                                    children.insert(new_point2);
                        }
                    }
                }
            }
        }
    }
}

void epsilon_move2_test() {
    alloc_t startpoint = {5,5,6,4,4};
    all_alloc_t fbidden;
    all_alloc_t children = {};
    epsilon_move2_rule3(children, startpoint,fbidden);
    cout << "Start Point" << startpoint << endl;
    cout << "Rule-34 children" << children << endl;
    // cout << "Number of children " << children.size() << "\n\n";
    // cout << "Valid Actions " << valid_actions << "\n\n";
}

/* Comparison based elimination */
// void ptss_DSE::eliminate_points_rule12() {
//     // if (!this->is_initialized) {
//     //     throw domain_error("Object not initialized correctly");
//     // }
//     // /* Apply recursively */
//     // all_alloc_t children;
//     // set<alloc_t> fset1 = {this->upper};
//     // set<alloc_t> fset2 = {this->upper};
//     // set<alloc_t> aset;

//     // expand_rule1(children,fset1,aset,0);
//     // aset.clear();
//     // expand_rule2(children,fset2,aset,0);
//     // cout << children << endl;

//     // cout << "Eliminated " << children.size() << endl;
// }

/*
 * Whenever the DMR constraint is violated
 * by a point say "init",
 * Rule1 and Rule3 will (recursively) be expanded
 * to create, sibling solutions which are guaranteed
 * to violate the DMR (Check the slides and the [PAPER])
 *
 * fbidden : A set that holds all the discarded points.
 * aset    : Children of "init" point expanded according to rule 1 and rule 3
 * aset2   : An auxiliary aset.
 *
 */
void apply_action_rule13(all_alloc_t &fbidden,\
                         all_alloc_t &fset,\
                         all_alloc_t &aset,\
                         int actv_id) {
    bool no_child = true;
    int i, j;

    /* Expand the children and create the new frontier */
    for (set<alloc_t>::iterator it = fset.begin(); it != fset.end(); it++) {

        // cout << "Inserting to fbidden set (activ-"<<actv_id<<"): " << *it << endl;
        // fbidden.insert(*it); // Insert all the elements of frontier set into fbidden set.

        /* Expand all the elements of the frontier */
        // epsilon_move2_rule1(aset,*it,fbidden);
        // epsilon_move2_rule3(aset,*it,fbidden);

        alloc_t tmp = *it;
        fbidden.insert(tmp);

        /* Rule 1 Expansion */
        for (int idx = 0; idx < NPH; idx++) {
            alloc_t tmp2  = tmp;
            if (tmp2[idx] > 1) {
                tmp2[idx]--;
                if (fbidden.count(tmp2) < 1)
                    aset.insert(tmp2);
            }
        }

        /* Rule 3 Expansion */
        for (i = 0; i < NPH; i++) {
            for (j = 0; j < NPH; j++) {
                // cout << "Expr : ("<<i<<","<<j<<") " << ((i != j) && (tmp[i] > 1) && (tmp[j] < M));
                if ((i != j) && (tmp[i] > 1) && (tmp[j] < M)) {
                    /* Transfer a core from i to j */
                    alloc_t new_point = tmp;
                    new_point[i]--;
                    new_point[j]++;
                    /* Check Execution Time before insert */
                    if (compute_execution_time(new_point) > compute_execution_time(tmp)) {
                        if (fbidden.count(new_point) < 1)
                            aset.insert(new_point);
                        /* Also insert other points derived from it */
                        alloc_t new_point2 = new_point;
                        while ((new_point2[i] > 1) && (new_point2[j] < M)) {
                                new_point2[i]--;
                                new_point2[j]++;
                                if (fbidden.count(new_point) < 1)
                                    aset.insert(new_point2);
                        }
                    }
                    // cout << "Blah : "<< "i = "<< i << ",j = " << j << ", tmp[i] = " << tmp[i] << ", tmp[j] = " << tmp[j] << endl;
                } else {
                    // cout << "i = "<< i << ",j = " << j << ", tmp[i] = " << tmp[i] << ", tmp[j] = " << tmp[j] << endl;
                }
            }
        }
    }
    fset.clear();
    no_child = no_child && (aset.empty()?true:false);

    if (!no_child) {
        apply_action_rule13(fbidden,aset,fset,++actv_id);
    }

}

void ptss_DSE::explore() {
    all_alloc_t tmp1 = {this->lower}, tmp2;
    apply_action_rule13(this->discarded_space,tmp1,tmp2,0);
    cout << "Search points discarded : "<<this->discarded_space.size()<<endl;
}
/********************************************************************/
int main () {

    struct timeval t1, t2;
    gettimeofday(&t1,NULL);
    ptss_DSE obj(0.25);
    // cout << obj;
    // obj.display();
    // obj.explore();
    obj.evaluate_all();
    gettimeofday(&t2,NULL);
    double elapsed  = (t2.tv_sec-t1.tv_sec)*1000000+(t2.tv_usec-t1.tv_usec);

    cout << elapsed << "us\n";

    // alloc_t a = {1,2,3,4,5};
    // alloc_t b = {1,1,3,4,2};
    // cout << is_lt(b,a) << "\n";
}


#include <iostream>
#include <cmath>
#include <boost/random.hpp>

double function(double argument)
{
   return sin(argument);
}

int main(int argc, char** argv)
{
   // random number setup
   //
   typedef boost::mt19937 base_generator_type;

   base_generator_type generator(42u);
   boost::uniform_real<> uniform_distribution(0,1);
   boost::variate_generator<base_generator_type&, boost::uniform_real<> >
      random_uniform(generator, uniform_distribution);

   // integration now from 0 to 1
   //
   long number_of_samples = 1000;

   double sum(0);

   for (long i(0); i < number_of_samples; ++i)
   {
      double value = function(random_uniform());
      sum += value;
   }

   std::cout << "the approximate result is: " << sum / number_of_samples << std::endl;

   return 0;
}


/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2) ICP algorithms in C++
 * Copyright (C) 2018-2019 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
/**
 * @file   optimal_tf_olae.cpp
 * @brief  OLAE algorithm to find the SE(3) optimal transformation
 * @author Jose Luis Blanco Claraco
 * @date   Jun 16, 2019
 */

#include <mp2p_icp/optimal_tf_olae.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/tfest/se3.h>
#include <Eigen/Dense>
#include "visit_correspondences.h"

using namespace mp2p_icp;

// Convert to quaternion by normalizing q=[1, optim_rot], then to rot. matrix:
static mrpt::poses::CPose3D gibbs2pose(const Eigen::Vector3d& v)
{
    auto       x = v[0], y = v[1], z = v[2];
    const auto r = 1.0 / std::sqrt(1.0 + x * x + y * y + z * z);
    x *= r;
    y *= r;
    z *= r;
    auto q = mrpt::math::CQuaternionDouble(r, -x, -y, -z);

    // Quaternion to 3x3 rot matrix:
    return mrpt::poses::CPose3D(q, .0, .0, .0);
}

/** The systems built by olae_build_linear_system.
 * The system is: "M g = v".
 *
 * However, if the solution is near the Gibbs vector singularity (|Phi|~= \pi)
 * we may need to use the alternative systems built by the
 * "sequential rotation method" [shuster1981attitude].
 *
 * (Refer to technical report for details)
 */
struct OLAE_LinearSystems
{
    Eigen::Matrix3d M, Mx, My, Mz;
    Eigen::Vector3d v, vx, vy, vz;

    /** Attitude profile matrix */
    Eigen::Matrix3d B;
};

/** Core of the OLAE algorithm  */
static OLAE_LinearSystems olae_build_linear_system(
    const WeightedPairings& in, const mrpt::math::TPoint3D& ct_other,
    const mrpt::math::TPoint3D& ct_this, OutlierIndices& in_out_outliers)
{
    MRPT_START

    using mrpt::math::TPoint3D;
    using mrpt::math::TVector3D;

    OLAE_LinearSystems res;

    // Build the linear system: M g = v
    res.M = Eigen::Matrix3d::Zero();
    res.v = Eigen::Vector3d::Zero();

    // Attitude profile matrix:
    res.B = Eigen::Matrix3d::Zero();

    // Lambda: process each pairing:
    auto lambda_each_pair = [&](const mrpt::math::TVector3D& bi,
                                const mrpt::math::TVector3D& ri,
                                const double                 wi) {
// We will evaluate M from an alternative expression below from the
// attitude profile matrix B instead, since it seems to be slightly more
// stable, numerically. The original code for M is left here for
// reference, though.
#if 0
    // M+=(1/2)* ([s_i]_{x})^2
    // with: s_i = b_i + r_i
    const double sx = bi.x + ri.x, sy = bi.y + ri.y, sz = bi.z + ri.z;

    /* ([s_i]_{x})^2 is:
     *
     *  ⎡    2     2                          ⎤
     *  ⎢- sy  - sz      sx⋅sy        sx⋅sz   ⎥
     *  ⎢                                     ⎥
     *  ⎢                 2     2             ⎥
     *  ⎢   sx⋅sy     - sx  - sz      sy⋅sz   ⎥
     *  ⎢                                     ⎥
     *  ⎢                              2     2⎥
     *  ⎣   sx⋅sz        sy⋅sz     - sx  - sy ⎦
     */
    const double c00 = -sy * sy - sz * sz;
    const double c11 = -sx * sx - sz * sz;
    const double c22 = -sx * sx - sy * sy;
    const double c01 = sx * sy;
    const double c02 = sx * sz;
    const double c12 = sy * sz;

    // clang-format off
    const auto dM = (Eigen::Matrix3d() <<
       c00, c01, c02,
       c01, c11, c12,
       c02, c12, c22 ).finished();
    // clang-format on

    // res.M += wi * dM;

    // The missing (1/2) from the formulas above:
    res.M *= 0.5;
#endif
        /* v-= weight *  [b_i]_{x}  r_i
         *  Each term is:
         *  ⎡by⋅rz - bz⋅ry ⎤   ⎡ B23 - B32 ⎤
         *  ⎢              ⎥   |           ⎥
         *  ⎢-bx⋅rz + bz⋅rx⎥ = | B31 - B13 ⎥
         *  ⎢              ⎥   |           ⎥
         *  ⎣bx⋅ry - by⋅rx ⎦   ⎣ B12 - B21 ⎦
         *
         * B (attitude profile matrix):
         *
         * B+= weight * (b_i * r_i')
         *
         */

        // clang-format off
    const auto dV = (Eigen::Vector3d() <<
       (bi.y * ri.z - bi.z * ri.y),
       (-bi.x * ri.z + bi.z * ri.x),
       (bi.x * ri.y - bi.y * ri.x) ).finished();
        // clang-format on

        res.v -= wi * dV;

        // clang-format off
    const auto dB = (Eigen::Matrix3d() <<
       bi.x * ri.x, bi.x * ri.y, bi.x * ri.z,
       bi.y * ri.x, bi.y * ri.y, bi.y * ri.z,
       bi.z * ri.x, bi.z * ri.y, bi.z * ri.z).finished();
        // clang-format on
        res.B += wi * dB;
    };  // end lambda for visit_correspondences()

    // Lambda for the final stage after visiting all corres:
    auto lambda_final = [&](const double w_sum) {
        // Normalize weights. OLAE assumes \sum(w_i) = 1.0
        if (w_sum > .0)
        {
            const auto f = (1.0 / w_sum);
            // res.M *= f;
            res.v *= f;
            res.B *= f;
        }
        else
        {
            // We either had NO input correspondences, or ALL were detected
            // as outliers... What to do in this case?
        }
    };

    visit_correspondences(
        in, ct_other, ct_this, in_out_outliers, lambda_each_pair, lambda_final,
        true /* DO make unit point vectors for OLAE */);

    // Now, compute the other three sets of linear systems, corresponding
    // to the "sequential rotation method" [shuster1981attitude], so we can
    // later keep the best one (i.e. the one with the largest |M|).
    {
        const Eigen::Matrix3d S = res.B + res.B.transpose();
        const double          p = res.B.trace() + 1;
        const double          m = res.B.trace() - 1;
        // Short cut:
        const auto& v = res.v;

        // Set #0: M g=v, without further rotations (the system built above).
        // clang-format off
        res.M = (Eigen::Matrix3d() <<
           S(0,0)-p,  S(0,1), S(0,2),
           S(0,1),   S(1,1)-p, S(1,2),
           S(0,2),   S(1,2),  S(2,2)-p ).finished();
        // clang-format on

        const auto&  M0 = res.M;  // shortcut
        const double z1 = v[0], z2 = v[1], z3 = v[2];

        // Set #1: rotating 180 deg around "x":
        // clang-format off
        res.Mx = (Eigen::Matrix3d() <<
           m     ,      -z3  ,     z2,
           -z3   ,  M0(2,2),     -S(1,2),
           z2    ,  -S(1,2),    M0(1,1)).finished();
        res.vx = (Eigen::Vector3d() <<
            -z1, S(0,2), -S(0,1)
            ).finished();
        // clang-format on

        // Set #2: rotating 180 deg around "y":
        // clang-format off
        res.My = (Eigen::Matrix3d() <<
           M0(2,2),     z3  ,     -S(0,2),
           z3     ,       m ,     -z1,
         -S(0,2)  ,     -z1 ,   M0(0,0)).finished();
        res.vy = (Eigen::Vector3d() <<
            -S(1,2), -z2, S(0,1)
            ).finished();
        // clang-format on

        // Set #3: rotating 180 deg around "z":
        // clang-format off
        res.Mz = (Eigen::Matrix3d() <<
           M0(1,1),  -S(0,1),     -z2,
          -S(0,1) ,  M0(0,0),      z1,
             -z2  ,      z1 ,      m).finished();
        res.vz = (Eigen::Vector3d() <<
            S(1,2), -S(0,2), -z3
            ).finished();
        // clang-format on
    }

    return res;

    MRPT_END
}

// See .h docs, and associated technical report.
void mp2p_icp::optimal_tf_olae(const WeightedPairings& in, OptimalTF_Result& result)
{
    MRPT_START

    using mrpt::math::TPoint3D;
    using mrpt::math::TVector3D;

    // Note on notation: we are search the relative transformation of
    // the "other" frame wrt to "this", i.e. "this"="global",
    // "other"="local":
    //   p_this = pose \oplus p_other
    //   p_A    = pose \oplus p_B      --> pB = p_A \ominus pose

    // Reset output to defaults:
    result = OptimalTF_Result();

    // Normalize weights for each feature type and for each target (attitude
    // / translation):
    ASSERT_(in.attitude_weights.pt2pt >= .0);
    ASSERT_(in.attitude_weights.l2l >= .0);
    ASSERT_(in.attitude_weights.pl2pl >= .0);

    // Compute the centroids:
    auto [ct_other, ct_this] =
        eval_centroids_robust(in, result.outliers /* empty for now  */);

    // Build the linear system: M g = v
    OLAE_LinearSystems linsys = olae_build_linear_system(
        in, ct_other, ct_this, result.outliers /* empty for now  */);

    MRPT_TODO("Refactor to avoid duplicated code? Is it possible?");

    // Re-evaluate the centroids, now that we have a guess on outliers.
    if (!result.outliers.empty())
    {
        // Re-evaluate the centroids:
        const auto [new_ct_other, new_ct_this] =
            eval_centroids_robust(in, result.outliers);

        ct_other = new_ct_other;
        ct_this  = new_ct_this;

        // And rebuild the linear system with the new values:
        linsys =
            olae_build_linear_system(in, ct_other, ct_this, result.outliers);
    }

    // We are finding the optimal rotation "g", as a Gibbs vector.
    // Solve linear system for optimal rotation: M g = v

    const double detM_orig = std::abs(linsys.M.determinant()),
                 detMx     = std::abs(linsys.Mx.determinant()),
                 detMy     = std::abs(linsys.My.determinant()),
                 detMz     = std::abs(linsys.Mz.determinant());

#if 0
    // clang-format off
    std::cout << " |M_orig|= " << detM_orig << "\n"
                 " |M_x|   = " << detMx << "\n"
                 " |M_t|   = " << detMy << "\n"
                 " |M_z|   = " << detMz << "\n";
    // clang-format on
#endif

    if (detM_orig > mrpt::max3(detMx, detMy, detMz))
    {
        // original rotation is the best numerically-determined problem:
        const auto sol0 =
            gibbs2pose(linsys.M.colPivHouseholderQr().solve(linsys.v));
        result.optimal_pose = sol0;
#if 0
        std::cout << "M   : |M|="
                  << mrpt::format("%16.07f", linsys.M.determinant())
                  << " sol: " << sol0.asString() << "\n";
#endif
    }
    else if (detMx > mrpt::max3(detM_orig, detMy, detMz))
    {
        // rotation wrt X is the best choice:
        auto sol1 =
            gibbs2pose(linsys.Mx.colPivHouseholderQr().solve(linsys.vx));
        sol1                = mrpt::poses::CPose3D(0, 0, 0, 0, 0, M_PI) + sol1;
        result.optimal_pose = sol1;
#if 0
        std::cout << "M_x : |M|="
                  << mrpt::format("%16.07f", linsys.Mx.determinant())
                  << " sol: " << sol1.asString() << "\n";
#endif
    }
    else if (detMy > mrpt::max3(detM_orig, detMx, detMz))
    {
        // rotation wrt Y is the best choice:
        auto sol2 =
            gibbs2pose(linsys.My.colPivHouseholderQr().solve(linsys.vy));
        sol2                = mrpt::poses::CPose3D(0, 0, 0, 0, M_PI, 0) + sol2;
        result.optimal_pose = sol2;
#if 0
        std::cout << "M_y : |M|="
                  << mrpt::format("%16.07f", linsys.My.determinant())
                  << " sol: " << sol2.asString() << "\n";
#endif
    }
    else
    {
        // rotation wrt Z is the best choice:
        auto sol3 =
            gibbs2pose(linsys.Mz.colPivHouseholderQr().solve(linsys.vz));
        sol3                = mrpt::poses::CPose3D(0, 0, 0, M_PI, 0, 0) + sol3;
        result.optimal_pose = sol3;
#if 0
        std::cout << "M_z : |M|="
                  << mrpt::format("%16.07f", linsys.Mz.determinant())
                  << " sol: " << sol3.asString() << "\n";
#endif
    }

    // Use centroids to solve for optimal translation:
    mrpt::math::TPoint3D pp;
    result.optimal_pose.composePoint(
        ct_other.x, ct_other.y, ct_other.z, pp.x, pp.y, pp.z);
    // Scale, if used, was: pp *= s;

    result.optimal_pose.x(ct_this.x - pp.x);
    result.optimal_pose.y(ct_this.y - pp.y);
    result.optimal_pose.z(ct_this.z - pp.z);

    MRPT_END
}


/**
 * @brief Partial Least Squares Regression with Eigen
 *
 * Same source as LDA, \cite friedman2001elements
 *
 * @file pls-eigen.hpp
 * @author François-David Collin <Francois-David.Collin@umontpellier.fr>
 * @brief
 * @version 0.1
 * @date 2018-11-08
 *
 * @copyright Copyright (c) 2018
 *
 */
#pragma once

#include "various.hpp"
#include <Eigen/Dense>
#include <list>
#include <algorithm>
#include <range/v3/all.hpp>
#include "tqdm.hpp"

using namespace Eigen;
using namespace std;
using namespace ranges;

/**
 * @brief filters out constant variables
 *
 * @tparam Derived
 * @param xr table to filter
 * @return std::vector<size_t> indexes of valid vars
 */
template<class Derived>
std::vector<size_t> filterConstantVars(const MatrixBase<Derived>& xr) {
    RowVectorXd meanr = xr.colwise().mean();
    VectorXd stdr = ((xr.rowwise() - meanr).array().square().colwise().sum() / (xr.rows() - 1)).sqrt();;
    std::vector<size_t> validvars(xr.cols());
    size_t m = 0;
    for(size_t i = 0; i< xr.cols(); i++) {
        if (stdr(i) >= 1.0e-8) validvars[m++] = i;
    }
    validvars.resize(m);
    return validvars;
}

/**
 * @brief Apply PLS on x regarding y
 *
 * @tparam Derived
 * @tparam OtherDerived
 * @param x input
 * @param y output
 * @param ncomp number of components expected
 * @param Projection the projection matrix
 * @param mean mean of variables in x
 * @param std standard deviation of variables in x
 * @param stopping elbow heuristic enabled
 * @return VectorXd explained variance for each computed components
 */
template<class Derived, class OtherDerived>
VectorXd pls(const MatrixBase<Derived>& x,
         const MatrixBase<OtherDerived>& y,
         size_t ncomp,
         MatrixXd& Projection,
         RowVectorXd& mean,
         RowVectorXd& std,
         bool stopping = false)
{
    size_t n = x.rows();
    size_t p = x.cols();
    mean = x.colwise().mean();
    ncomp = std::min(std::min(n,p),ncomp);
    std = ((x.rowwise() - mean).array().square().colwise().sum() / (x.rows() - 1)).sqrt();;
    MatrixXd X = (x.rowwise() - mean).array().rowwise() / std.array();
    MatrixXd X0 = X;
    MatrixXd Ptilde(ncomp,p);
    MatrixXd Wstar(p,ncomp);
    VectorXd res(ncomp);
    size_t window_size = std::max(2_z,ncomp/10_z);

    std::list<unsigned char> stopping_criterium(window_size,0);
    double ymean = y.mean();
    MatrixXd w_k, t_k, p_k, y_k;
    y_k = y;
    double SSTO = (y.array() - ymean).array().square().sum();
    int m = 0;
    tqdm bar;

    while (m < ncomp)
    {
        bar.progress(m,ncomp);
        // $w_{k}=\frac{X_{k-1}^{T} y_{k-1}}{\left\|X_{k-1}^{T} y_{k-1}\right\|}$
        w_k = X.transpose() * y;   //  (p)
        w_k /= sqrt((w_k.transpose()*w_k)(0,0));
        // $\mathbf{W}^{*}_{p \times K} = [w_1, \ldots, w_K]$
        Wstar.col(m) = w_k;
        // $t_{k}=X_{k-1}w_{k}$
        t_k = X * w_k; // (n)
        double t_k_s = (t_k.transpose() * t_k)(0,0);
        // $p_{k}=\frac{X_{k-1}^{T} t_{k}}{t_{k}^{T} t_{k}}$
        p_k = (X.transpose() * t_k) / t_k_s; // (p)
        // $\widetilde{\mathbf{P}}_{K \times p}=\mathbf{t}\left[p_{1}, \ldots, p_{K}\right]$
        Ptilde.row(m) = p_k.transpose();
        // $q_{k}=\frac{y_{k-1}^{T} t_{k}}{t_{k}^{T} t_{k}}$
        double q_k = (y_k.transpose() * t_k)(0,0) / t_k_s; //(n,n)
        // $y_{k}=y_{k-1}-q_{k} t_{k}$
        y_k -= q_k * t_k;
        // $X_{k}=X_{k-1}-t_{k} p_{k}^{T}$
        X -= (t_k * p_k.transpose());

        // $$Yvar^m = \frac{\sum_{i=1}^{N}{(\hat{y}^{m}_{i}-\bar{y})^2}}{\sum_{i=1}^{N}{(y_{i}-\hat{y})^2}}$$
        res(m) = 1 - (y_k.array() - ymean).array().square().sum() / SSTO;

        // Elbow heuristic
        // $$Yvar^m = \frac{\sum_{i=1}^{N}{(\hat{y}^{m}_{i}-\bar{y})^2}}{\sum_{i=1}^{N}{(y_{i}-\hat{y})^2}}$$
        if ((m >= 2) && stopping) {
            auto lastdiff = res(m) - res(m-1);
            auto lastmean = (res(m) + res(m-1))/2.0;
            size_t remains = ncomp - m;
            stopping_criterium.pop_front();
            stopping_criterium.push_back(lastmean >= 0.99 * remains * lastdiff);
            auto wcrit = ranges::accumulate(stopping_criterium,0);
            if (wcrit == window_size) break;
        }

        m++;
    }
    if (m < ncomp) {
        m--;
        res = res(seq(0,m)).eval();
    }

    // $\mathbf{W}=\mathbf{W}^{*}\left(\widetilde{\mathbf{P}} \mathbf{W}^{*}\right)^{-1}$
    auto solver = (Ptilde*Wstar).completeOrthogonalDecomposition();
    Projection = Wstar*solver.pseudoInverse();
    return res;
}

// Tenenhaus, M. L’approche PLS. Revue de statistique appliquée 47, 5–40 (1999).
// Vancolen, S. La Régression PLS. Mémoire Postgrade en Statistiques, University of Neuchâtel (Switzerland) 1--28 (2004).
// Wold, S., Sjöström, M. & Eriksson, L. PLS-regression: a basic tool of chemometrics. Chemometrics and intelligent laboratory systems 58, 109–130 (2001).
// Mémoire m2 de ghislain : https://plmbox.math.cnrs.fr/f/1192b14f90ea44a1b26a/
// Krämer, N. An overview on the shrinkage properties of partial least squares regression. Computational Statistics 22, 249–273 (2007).


// Copyright 2019 Xanadu Quantum Technologies Inc.

// 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.
/**
 * @file
 * Contains functions for approximating the hafnian of a matrix
 * in a classically efficient manner, for certains classes of matrices.
 */

#pragma once
#include <stdafx.h>
#include <numeric>
#include <random>
#include <cmath>

#ifdef LAPACKE
#define EIGEN_SUPERLU_SUPPORT
#define EIGEN_USE_BLAS
#define EIGEN_USE_LAPACKE

#define LAPACK_COMPLEX_CUSTOM
#define lapack_complex_float std::complex<float>
#define lapack_complex_double std::complex<double>
#endif

#include <Eigen/Eigenvalues>

namespace libwalrus {

/**
* Returns the approximation to the hafnian of a matrix with non-negative entries.
*
* The approximation follows an stochastic algorithm according to which the hafnian
* can be approximated as the sum of determinants of matrices.
* The accuracy of the approximation increases with increasing number of iterations.
*
* @param mat vector representing the flattened matrix
* @param nsamples positive integer representing the number of samples to perform
* @return the approximate hafnian
*/
template <typename T>
inline long double hafnian_nonneg(std::vector<T> &mat, int &nsamples) {
    int n = std::sqrt(static_cast<double>(mat.size()));

    long double mean = 0;
    long double stdev = 1;

    namespace eg = Eigen;
    eg::Matrix<T, eg::Dynamic, eg::Dynamic> A = eg::Map<eg::Matrix<T, eg::Dynamic, eg::Dynamic>, eg::Unaligned>(mat.data(), n, n);

#ifdef _OPENMP
    int nthreads = omp_get_max_threads();
    omp_set_num_threads(nthreads);
#else
    int nthreads = 1;
#endif

    std::vector<int> threadbound_low(nthreads);
    std::vector<int> threadbound_hi(nthreads);

    std::default_random_engine generator;
    std::normal_distribution<double> distribution(mean, stdev);

    std::vector<long double> determinants(nsamples);

    #pragma omp parallel for shared(determinants)
    for (int k = 0; k < nsamples; k++) {
        std::vector<T> matrand(n * n, 0);
        std::vector<T> g(n * n, 0);
        std::vector<T> gt(n * n, 0);
        eg::Matrix<T, eg::Dynamic, eg::Dynamic> W;
        W.resize(n, n);

        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                long double randnum = distribution(generator);
                matrand[i * n + j] = static_cast<T>(randnum);
                g[i * n + j] = 0.0;
                gt[i * n + j] = 0.0;
            }
        }

        for (int i = 0; i < n; i++) {
            for (int j = i; j < n; j++) {
                g[i * n + j] = matrand[i * n + j];
                gt[j * n + i] = matrand[i * n + j];
            }
        }

        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                int id = i * n + j;
                W(i, j) = (g[id] - gt[id]) * std::sqrt(std::abs(A(i, j)));
            }
        }

        long double det = std::real(W.determinant());

        determinants[k] = det;
    }

    long double final = 0.0;

    for (int i = 0; i < nsamples; i++) {
        final += determinants[i];
    }

    final = final / (static_cast<long double>(nsamples));

    return final;

}

/**
* Returns the approximation to the hafnian of a matrix with non-negative entries.
*
* The approximation follows an stochastic algorithm according to which the hafnian
* can be approximated as the sum of determinants of matrices.
* The accuracy of the approximation increases with increasing number of iterations.
*
* This is a wrapper around the templated function `libwalrus::hafnian_nonneg` for Python
* integration. It accepts and returns double numeric types, and
* returns sensible values for empty and non-even matrices.
*
* In addition, this wrapper function automatically casts all matrices
* to type `long double`, allowing for greater precision than supported
* by Python and NumPy.
*
* @param mat vector representing the flattened matrix
* @param nsamples positive integer representing the number of samples to perform
* @return the approximate hafnian
*/
double hafnian_approx(std::vector<double> &mat, int &nsamples) {
    std::vector<long double> matq(mat.begin(), mat.end());
    int n = std::sqrt(static_cast<double>(mat.size()));
    long double haf;

    if (n == 0)
        haf = 1.0;
    else if (n % 2 != 0)
        haf = 0.0;
    else
        haf = hafnian_nonneg(matq, nsamples);

    return static_cast<double>(haf);
}

}