// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// 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 BOOST_GEOMETRY_PROJECTIONS_GOODE_HPP
#define BOOST_GEOMETRY_PROJECTIONS_GOODE_HPP

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/proj/gn_sinu.hpp>
#include <boost/geometry/srs/projections/proj/moll.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct goode {}; // Goode Homolosine

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace goode
    {

            static const double Y_COR = 0.05280;
            static const double PHI_LIM = .71093078197902358062;

            // TODO: consider storing references to Parameters instead of copies
            template <typename T, typename Par>
            struct par_goode
            {
                sinu_spheroid<T, Par>    sinu;
                moll_spheroid<T, Par>    moll;

                par_goode(Par const& par) : sinu(par), moll(par) {}
            };

            template <typename T, typename Par>
            inline void s_forward(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y,
                                  Par const& par, par_goode<T, Par> const& proj_par)
            {
                if (fabs(lp_lat) <= PHI_LIM)
                    proj_par.sinu.fwd(lp_lon, lp_lat, xy_x, xy_y);
                else {
                    proj_par.moll.fwd(lp_lon, lp_lat, xy_x, xy_y);
                    xy_y -= lp_lat >= 0.0 ? Y_COR : -Y_COR;
                }
            }

            template <typename T, typename Par>
            inline void s_inverse(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat,
                                  Par const& par, par_goode<T, Par> const& proj_par)
            {
                if (fabs(xy_y) <= PHI_LIM)
                    proj_par.sinu.inv(xy_x, xy_y, lp_lon, lp_lat);
                else {
                    xy_y += xy_y >= 0.0 ? Y_COR : -Y_COR;
                    proj_par.moll.inv(xy_x, xy_y, lp_lon, lp_lat);
                }
            }

            // Goode Homolosine
            template <typename Par>
            inline void setup_goode(Par& par)
            {
                par.es = 0.;

                // NOTE: The following explicit initialization of sinu projection
                // is not needed because setup_goode() is called before proj_par.sinu
                // is constructed and m_par of parent projection is used.

                //proj_par.sinu.m_par.es = 0.;
                //detail::gn_sinu::setup_sinu(proj_par.sinu.m_par, proj_par.sinu.m_proj_parm);
            }

    }} // namespace detail::goode
    #endif // doxygen

    /*!
        \brief Goode Homolosine projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Example
        \image html ex_goode.gif
    */
    template <typename T, typename Parameters>
    struct goode_spheroid : public detail::base_t_fi<goode_spheroid<T, Parameters>, T, Parameters>
    {
        detail::goode::par_goode<T, Parameters> m_proj_parm;

        inline goode_spheroid(const Parameters& par)
            : detail::base_t_fi<goode_spheroid<T, Parameters>, T, Parameters>(*this, par)
            , m_proj_parm(setup(this->m_par))
        {}

        // FORWARD(s_forward)  spheroid
        // Project coordinates from geographic (lon, lat) to cartesian (x, y)
        inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
        {
            detail::goode::s_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
        }

        // INVERSE(s_inverse)  spheroid
        // Project coordinates from cartesian (x, y) to geographic (lon, lat)
        inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
        {
            detail::goode::s_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
        }

        static inline std::string get_name()
        {
            return "goode_spheroid";
        }

    private:
        static Parameters& setup(Parameters& par)
        {
            detail::goode::setup_goode(par);
            return par;
        }

    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::goode, goode_spheroid, goode_spheroid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class goode_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<goode_spheroid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void goode_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("goode", new goode_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_GOODE_HPP


#define ARMA_NO_DEBUG

#include <armadillo>
#include <RcppArmadillo.h>
// [[Rcpp::depends(RcppArmadillo, BH, bigmemory)]]

#include <Rcpp.h>
#include <vector>

using namespace Rcpp;
using namespace arma;

#include <bigmemory/BigMatrix.h>
// [[Rcpp::plugins(cpp11)]]

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
Rcpp::List calcT0Wsim1 (arma::mat& CvSqrt,arma::mat& weight, arma::vec powV, int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int k = CvSqrt.n_rows;
    // containers
    arma::mat T0s1(nperm,npow);
    T0s1.fill(0);

    for (int i = 0; i < nperm; i++) {
        arma::mat U00 = arma::randn(k,1);
        arma::mat U0 = CvSqrt * U00;
        arma::mat U0tmp = weight % U0;

        for (int j = 0; j < npow; j++) {
            if (powV[j] == 0) {
                arma::mat tmpU01 = abs(U0tmp);
                T0s1(i,j) = tmpU01.max();
            } else {
                T0s1(i,j) = accu(pow(U0tmp,powV[j]));
            }
        }
    }
    Rcpp::List res;
    res["T0"] =T0s1;

    return(res);
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
arma::vec avg_rank(arma::vec x) {

    arma::uvec w  = arma::stable_sort_index(x,"descend");
    R_xlen_t sz = x.size();
    arma::vec r(sz);

    for (R_xlen_t n, i = 0; i < sz; i += n) {
        n = 1;
        while (i + n < sz && x[w[i]] == x[w[i + n]]) ++n;
        for (R_xlen_t k = 0; k < n; k++) {
            r[w[i + k]] = i + (n + 1) / 2.;
        }
    }

    return r;

}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
Rcpp::List calcT0Wsim3 (arma::mat& CvSqrt,arma::mat& weight, arma::vec powV, arma::vec Tsabs, SEXP pBigMat, int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int k = CvSqrt.n_rows;

    // containers
    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T0s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    for (int i = 0; i < nperm; i++) {
        arma::mat U00 = arma::randn(k,1);
        arma::mat U0 = CvSqrt * U00;
        arma::mat U0tmp = weight % U0;

        for (int j = 0; j < npow; j++) {
            if (powV[j] == 0) {
                arma::mat tmpU01 = abs(U0tmp);
                T0s(i,j) = tmpU01.max();
            } else {
                T0s(i,j) = accu(pow(U0tmp,powV[j]));
            }
        }
    }

    T0s = arma::abs(T0s);

    arma::vec minp0(nperm);
    arma::vec P0s(nperm);
    arma::vec pPerm0(npow);

    for (int i = 0; i < npow; i++) {

        // Calculate p-value
        arma::vec T0stmp = T0s.col(i);
        int tmp3 = 0;
        arma::vec rankarma  = avg_rank(T0stmp);

        for( int tt=0 ; tt < nperm ; tt++) {
            if( Tsabs(i)  <= T0stmp(tt) ) {
                tmp3++;
            }

            P0s(tt) = (double) rankarma(tt) / (double) nperm;
        }

        if(i == 0) {
            minp0 = P0s;
        } else {
            for( int ii = 0; ii < nperm; ii++) {
                if( minp0(ii) > P0s(ii) ) {
                    minp0(ii) = P0s(ii);
                }
            }
        }
        pPerm0(i) = (double) tmp3/ (double) nperm;

    }

    return Rcpp::List::create(Rcpp::Named("minp0") = minp0,
                              Rcpp::Named("pPerm0") = pPerm0 );

}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
Rcpp::List calcT0Wsim4 (arma::mat& CvSqrt,arma::mat& weight, arma::vec Tsabs, arma::vec powV, SEXP pBigMat, SEXP pBigMat3, int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int nweight = weight.n_cols;
    const int k = CvSqrt.n_rows;
    // containers
    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T0s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    XPtr<BigMatrix> xpMat3(pBigMat3);
    arma::mat minp0_sign = arma::Mat<double> ( (double *)xpMat3->matrix(), xpMat3->nrow(), xpMat3->ncol(), false);

    for (int i = 0; i < nperm; i++) {
        arma::mat U00 = arma::randn(k,1);
        arma::mat U0 = CvSqrt * U00;

        for (int k = 0; k < nweight; k++) {
            arma::mat weight_tmp = weight.col(k);
            arma::mat U0tmp = weight_tmp % U0;

            for (int j = 0; j < npow; j++) {
                if (powV[j] == 0) {
                    arma::mat tmpU01 = abs(U0tmp);
                    T0s(i,j * nweight + k) = tmpU01.max();
                } else {
                    T0s(i,j * nweight + k) = accu(pow(U0tmp,powV[j]));
                }
            }
        }
    }

    for (int k =0; k<nweight; k++) {
        minp0_sign.col(k) = T0s.col(k);
    }

    int npw = npow * nweight;
    arma::mat cov_res(npw,npw);
    cov_res.fill(0);
    cov_res = cov(T0s);

    T0s = arma::abs(T0s);

    arma::vec minp0(nperm);
    arma::vec P0s(nperm);
    arma::mat pPerm0(npow + 1,nweight);
    pPerm0.fill(-1);

    for(int k = 0; k < nweight; k++) {

        double minP_tmp = 1.0;

        for (int j = 0; j < npow; j++) {

            // Calculate p-value
            arma::vec T0stmp = T0s.col(j * nweight + k);
            int tmp3 = 0;
            arma::vec rankarma  = avg_rank(T0stmp);

            for( int tt=0 ; tt < nperm ; tt++) {
                if( Tsabs(j * nweight + k)  <= T0stmp(tt) ) {
                    tmp3++;
                }

                P0s(tt) = (double) rankarma(tt) / (double) nperm;
            }

            if(j == 0) {
                minp0 = P0s;
            } else {
                for( int ii = 0; ii < nperm; ii++) {
                    if( minp0(ii) > P0s(ii) ) {
                        minp0(ii) = P0s(ii);
                    }
                }
            }
            double tmp_pvalue = (double) tmp3 / (double) nperm;
            pPerm0(j,k) = tmp_pvalue;

            if (tmp_pvalue <= minP_tmp) {
                minP_tmp = tmp_pvalue;
            }
        }

        int count_pvalue = 0;
        for(int j=0; j < nperm; j++) {
            if( minP_tmp > minp0(j) ) {
                count_pvalue = count_pvalue + 1;
            }
        }

        pPerm0(npow,k) = (double) (count_pvalue + 1) / (double) (nperm + 1);

    }

    return Rcpp::List::create(Rcpp::Named("pPerm0") = pPerm0,
                              Rcpp::Named("cov") = cov_res );

}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
Rcpp::List calcT0WsimV3 (arma::mat& CvSqrt,arma::mat& weight, arma::vec powV, SEXP pBigMat, SEXP pBigMat3,int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int nweight = weight.n_cols;
    const int k = CvSqrt.n_rows;
    // containers
    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T0s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    XPtr<BigMatrix> xpMat3(pBigMat3);
    arma::mat minp0_sign = arma::Mat<double> ( (double *)xpMat3->matrix(), xpMat3->nrow(), xpMat3->ncol(), false);

    for (int i = 0; i < nperm; i++) {
        arma::mat U00 = arma::randn(k,1);
        arma::mat U0 = CvSqrt * U00;

        for (int k = 0; k < nweight; k++) {
            arma::mat weight_tmp = weight.col(k);
            arma::mat U0tmp = weight_tmp % U0;

            for (int j = 0; j < npow; j++) {
                if (powV[j] == 0) {
                    arma::mat tmpU01 = abs(U0tmp);
                    T0s(i,j * nweight + k) = tmpU01.max();
                } else {
                    T0s(i,j * nweight + k) = accu(pow(U0tmp,powV[j]));
                }
            }
        }
    }

    for (int k =0; k<nweight; k++) {
        minp0_sign.col(k) = T0s.col(k);
    }

    int npw = npow * nweight;
    arma::mat cov_res(npw,npw);
    cov_res.fill(0);
    cov_res = arma::cov(T0s);

    arma::mat mean_res(npw,1);
    mean_res.fill(0);
    mean_res = arma::mean(T0s,0);

    return Rcpp::List::create(Rcpp::Named("cov") = cov_res,
                              Rcpp::Named("mean") = mean_res );

}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
void calc_test_ch(arma::mat& cov, arma::mat& weight, arma::vec powV, SEXP pBigMat, SEXP pBigMat2,int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int nweight = weight.n_cols;

    // containers
    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T0s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    XPtr<BigMatrix> xpMat2(pBigMat2);
    arma::mat T1s = arma::Mat<double> ( (double *)xpMat2->matrix(), xpMat2->nrow(), xpMat2->ncol(), false);

    arma::vec pPerm0(npow);

    for (int i = 0; i < npow; i++) {

        arma::mat subcov(nweight,nweight);
        subcov.fill(0);

        subcov = cov.submat(i*nweight,i*nweight,((i+1)*nweight -1),((i+1)*nweight -1));
        subcov = subcov.i();

        for (int j =0; j < nperm; j++) {
            arma::vec tmp_ts = T0s.row(j);
            arma::mat tmp_ts2(1,nweight);
            tmp_ts2 = tmp_ts.subvec(i*nweight,((i+1) * nweight -1));
            arma::mat tmp = tmp_ts2.t() * subcov * tmp_ts2;
            T1s(j,i) = tmp(0,0);
        }
    }
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
void calc_test_ch_V3(arma::mat& cov,arma::vec & mean, arma::mat& weight, arma::vec powV, SEXP pBigMat, SEXP pBigMat2,int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int nweight = weight.n_cols;

    // containers
    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T0s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    XPtr<BigMatrix> xpMat2(pBigMat2);
    arma::mat T1s = arma::Mat<double> ( (double *)xpMat2->matrix(), xpMat2->nrow(), xpMat2->ncol(), false);

    arma::vec pPerm0(npow);

    for (int i = 0; i < npow; i++) {

        arma::mat subcov(nweight,nweight);
        subcov.fill(0);

        subcov = cov.submat(i*nweight,i*nweight,((i+1)*nweight -1),((i+1)*nweight -1));
        subcov = subcov.i();

        arma::mat tmp_mean(1,nweight);
        tmp_mean = mean.subvec(i*nweight,((i+1) * nweight -1));
        for (int j =0; j < nperm; j++) {
            arma::vec tmp_ts = T0s.row(j);
            arma::mat tmp_ts2(1,nweight);

            tmp_ts2 = tmp_ts.subvec(i*nweight,((i+1) * nweight -1));
            tmp_ts2 = tmp_ts2 - tmp_mean;
            arma::mat tmp = tmp_ts2.t() * subcov * tmp_ts2;
            T1s(j,i) = tmp(0,0);
        }
    }
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
void calc_test_pan(arma::mat& cov, arma::mat& weight, arma::vec powV, SEXP pBigMat,SEXP pBigMat2, int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int nweight = weight.n_cols;

    // containers
    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T1s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    XPtr<BigMatrix> xpMat2(pBigMat2);
    arma::mat T2s = arma::Mat<double> ( (double *)xpMat2->matrix(), xpMat2->nrow(), xpMat2->ncol(), false);

    arma::vec pPerm0(npow);

    for (int i = 0; i < npow; i++) {

        arma::mat subcov(nweight,nweight);
        subcov.fill(0);

        subcov = cov.submat(i*nweight,i*nweight,((i+1)*nweight -1),((i+1)*nweight -1));

        for (int j =0; j < nperm; j++) {
            arma::vec tmp_ts = T2s.row(j);
            arma::mat tmp_ts2(1,nweight);
            tmp_ts2 = tmp_ts.subvec(i*nweight,((i+1) * nweight -1));
            arma::mat tmp = tmp_ts2.t() * subcov * tmp_ts2;
            T1s(j,i) = tmp(0,0);
        }
    }
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
arma::mat calc_test_ts(arma::mat& cov, arma::mat& weight, arma::vec powV, arma::vec Ts) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int nweight = weight.n_cols;

    arma::mat Ts2(npow,1);
    Ts2.fill(0);

    for (int i = 0; i < npow; i++) {
        arma::mat subcov(nweight,nweight);
        subcov.fill(0);

        subcov = cov.submat(i*nweight,i*nweight,((i+1)*nweight -1),((i+1)*nweight -1));

        arma::mat tmp_ts2(1,nweight);
        tmp_ts2 = Ts.subvec(i*nweight,((i+1)*nweight -1));
        arma::mat tmp =  tmp_ts2.t() * subcov * tmp_ts2;
        Ts2(i,0) = tmp(0,0);
    }

    return Ts2;
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
arma::mat calc_test_ts_V3(arma::mat& cov, arma::vec & mean, arma::mat& weight, arma::vec powV, arma::vec Ts) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int nweight = weight.n_cols;

    arma::mat Ts2(npow,1);
    Ts2.fill(0);

    for (int i = 0; i < npow; i++) {
        arma::mat subcov(nweight,nweight);
        subcov.fill(0);

        subcov = cov.submat(i*nweight,i*nweight,((i+1)*nweight -1),((i+1)*nweight -1));
        subcov = subcov.i();

        arma::mat tmp_mean(1,nweight);
        tmp_mean = mean.subvec(i*nweight,((i+1) * nweight -1));

        arma::mat tmp_ts2(1,nweight);
        tmp_ts2 = Ts.subvec(i*nweight,((i+1)*nweight -1));
        tmp_ts2 = tmp_ts2 - tmp_mean;

        arma::mat tmp =  tmp_ts2.t() * subcov * tmp_ts2;
        Ts2(i,0) = tmp(0,0);
    }

    return Ts2;
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
Rcpp::List calc_p_ch (arma::vec powV, arma::vec Tsabs, SEXP pBigMat2, int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;

    // containers
    XPtr<BigMatrix> xpMat2(pBigMat2);
    arma::mat T1s = arma::Mat<double> ( (double *)xpMat2->matrix(), xpMat2->nrow(), xpMat2->ncol(), false);

    arma::vec minp0(nperm);
    arma::vec P0s(nperm);
    arma::vec pPerm0(npow);

    for (int i = 0; i < npow; i++) {

        // Calculate p-value
        arma::vec T1stmp = T1s.col(i);
        int tmp3 = 0;

        arma::vec rankarma  = avg_rank(T1stmp);

        for( int tt=0 ; tt < nperm ; tt++) {
            if( Tsabs(i)  <= T1stmp(tt) ) {
                tmp3++;
            }
            P0s(tt) = (double) rankarma(tt) / (double) nperm;
        }

        if(i == 0) {
            minp0 = P0s;
        } else {
            for( int ii = 0; ii < nperm; ii++) {
                if( minp0(ii) > P0s(ii) ) {
                    minp0(ii) = P0s(ii);
                }
            }
        }
        pPerm0(i) = (double) tmp3 / (double) nperm;

    }

    return Rcpp::List::create(Rcpp::Named("minp0") = minp0,
                              Rcpp::Named("pPerm0") = pPerm0 );

}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
Rcpp::List calc_p_ch_pan (arma::vec powV, arma::vec Tsabs, SEXP pBigMat, int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;

    // containers
    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T1s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    arma::vec minp0(nperm);
    arma::vec P0s(nperm);
    arma::vec pPerm0(npow);

    for (int i = 0; i < npow; i++) {

        // Calculate p-value
        arma::vec T1stmp = T1s.col(i);
        int tmp3 = 0;

        arma::vec rankarma  = avg_rank(T1stmp);

        for( int tt=0 ; tt < nperm ; tt++) {
            if( Tsabs(i)  <= T1stmp(tt) ) {
                tmp3++;
            }
            P0s(tt) = (double) rankarma(tt) / (double) nperm;
        }

        if(i == 0) {
            minp0 = P0s;
        } else {
            for( int ii = 0; ii < nperm; ii++) {
                if( minp0(ii) > P0s(ii) ) {
                    minp0(ii) = P0s(ii);
                }
            }
        }
        pPerm0(i) = (double) tmp3 / (double) nperm;

    }

    return Rcpp::List::create(Rcpp::Named("minp0") = minp0,
                              Rcpp::Named("pPerm0") = pPerm0 );

}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
Rcpp::List calc_p_pan (arma::vec Tsabs,SEXP pBigMat,SEXP pBigMat2,SEXP pBigMat3, int nTs,int nweight, int nperm) {

    // containers
    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T0s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    XPtr<BigMatrix> xpMat2(pBigMat2);
    arma::mat T2s = arma::Mat<double> ( (double *)xpMat2->matrix(), xpMat2->nrow(), xpMat2->ncol(), false);

    XPtr<BigMatrix> xpMat3(pBigMat3);
    arma::mat minp0_sign = arma::Mat<double> ( (double *)xpMat3->matrix(), xpMat3->nrow(), xpMat3->ncol(), false);

    // T0s = arma::abs(T0s);

    arma::vec minp0(nperm);
    arma::vec P0s(nperm);
    arma::vec pPerm0(nTs);

    for (int i = 0; i < nTs; i++) {

        // Calculate p-value
        arma::vec T1stmp = T0s.col(i);
        int tmp3 = 0;

        arma::vec rankarma  = avg_rank(T1stmp);

        int minp0_index = i - floor(i/nweight) * nweight;

        for( int tt=0 ; tt < nperm ; tt++) {
            if( Tsabs(i)  <= T1stmp(tt) ) {
                tmp3++;
            }

            NumericVector tmp_res;
            tmp_res = (double) rankarma(tt) / (double) nperm;

            int p0_sign = -1;
            if (minp0_sign(tt,minp0_index) >0) {
                p0_sign = 1;
            }

            tmp_res = qnorm(1 - tmp_res/2) * p0_sign;
            T2s(tt,i) = tmp_res[0];
        }

        pPerm0(i) = (double) tmp3 / (double) nperm;
    }

    arma::mat cov_res(nTs,nTs);
    cov_res.fill(0);
    cov_res = arma::cov(T2s);

    return Rcpp::List::create(Rcpp::Named("pPerm0") = pPerm0,
                              Rcpp::Named("cov") = cov_res );

}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
arma::vec calc_aSPU (arma::mat p_cov, SEXP pBigMat2, int nperm) {

    // containers
    XPtr<BigMatrix> xpMat2(pBigMat2);
    arma::mat minp0 = arma::Mat<double> ( (double *)xpMat2->matrix(), xpMat2->nrow(), xpMat2->ncol(), false);

    arma::vec res(nperm);

    for (int i = 0; i < nperm; i++) {
        arma::mat tmp =  minp0.row(i) * p_cov * minp0.row(i).t();
        res(i) = tmp(0,0);
    }

    return res;
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
Rcpp::List daSPU_calcT0Wsim4 (arma::mat& CvSqrt,arma::mat& weight, arma::vec powV, arma::vec Tsabs, SEXP pBigMat, SEXP pBigMat2,SEXP pBigMat3,int nperm) {

    //const int n = X1.n_rows;
    const int npow = powV.n_rows;
    const int nweight = weight.n_cols;
    const int k = CvSqrt.n_rows;
    // containers
    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T0s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    // containers
    XPtr<BigMatrix> xpMat2(pBigMat2);
    arma::mat minp0 = arma::Mat<double> ( (double *)xpMat2->matrix(), xpMat2->nrow(), xpMat2->ncol(), false);

    // containers
    XPtr<BigMatrix> xpMat3(pBigMat3);
    arma::mat minp0_sign = arma::Mat<double> ( (double *)xpMat3->matrix(), xpMat3->nrow(), xpMat3->ncol(), false);

    for (int i = 0; i < nperm; i++) {
        arma::mat U00 = arma::randn(k,1);
        arma::mat U0 = CvSqrt * U00;

        for (int k = 0; k < nweight; k++) {
            arma::mat weight_tmp = weight.col(k);
            arma::mat U0tmp = weight_tmp % U0;

            for (int j = 0; j < npow; j++) {
                if (powV[j] == 0) {
                    arma::mat tmpU01 = abs(U0tmp);
                    T0s(i,k * npow + j) = tmpU01.max();
                } else {
                    T0s(i,k * npow + j) = accu(pow(U0tmp,powV[j]));
                }
            }
        }
    }

    for (int k =0; k<nweight; k++) {
        minp0_sign.col(k) = T0s.col(k * npow);
    }

    T0s = arma::abs(T0s);

    arma::vec minp0_tmp(nperm);

    arma::vec P0s(nperm);
    arma::vec pPerm0(npow* nweight);

    for (int k = 0; k < nweight; k++) {

        for (int i = (npow*k); i < (npow* (k + 1)); i++) {

            // Calculate p-value
            arma::vec T0stmp = T0s.col(i);
            int tmp3 = 0;
            arma::vec rankarma  = avg_rank(T0stmp);

            for( int tt=0 ; tt < nperm ; tt++) {
                if( Tsabs(i)  <= T0stmp(tt) ) {
                    tmp3++;
                }

                P0s(tt) = (double) rankarma(tt) / (double) nperm;
            }

            if(i == 0) {
                minp0_tmp = P0s;
            } else {
                for( int ii = 0; ii < nperm; ii++) {
                    if( minp0_tmp(ii) > P0s(ii) ) {
                        minp0_tmp(ii) = P0s(ii);
                    }
                }
            }
            pPerm0(i) = (double) tmp3/ (double) nperm;
        }

        minp0.col(k) = minp0_tmp;
    }

    for (int k = 0; k < nweight; k++) {
        for (int i = 0; i <nperm; i++) {
            NumericVector tmp_res;

            tmp_res = minp0(i,k);
            int p0_sign = -1;
            if (minp0_sign(i,k) >0) {
                p0_sign = 1;
            }
            tmp_res = qnorm(1 - tmp_res/2) * p0_sign;
            minp0(i,k) = tmp_res[0];
        }
    }

    arma::mat cov_res(nweight,nweight);
    cov_res.fill(0);
    cov_res = arma::cov(minp0);

    return Rcpp::List::create(Rcpp::Named("pPerm0") = pPerm0,
                              Rcpp::Named("cov") = cov_res );

}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
arma::vec daSPU_calc_aSPU (arma::mat p_cov, SEXP pBigMat2, int nperm) {

    // containers
    XPtr<BigMatrix> xpMat2(pBigMat2);
    arma::mat minp0 = arma::Mat<double> ( (double *)xpMat2->matrix(), xpMat2->nrow(), xpMat2->ncol(), false);

    arma::vec res(nperm);

    for (int i = 0; i < nperm; i++) {
        arma::mat tmp =  minp0.row(i) * p_cov * minp0.row(i).t();
        res(i) = tmp(0,0);
    }

    return res;
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
Rcpp::List calcT0Wsim2 (arma::mat& CvSqrt,arma::mat& weight, arma::vec pow1, arma::vec pow2, int nperm) {

    //const int n = X1.n_rows;
    const int n_pow1 = pow1.size();
    const int n_pow2 = pow2.size();
    const int n_weight = weight.n_cols;

    const int k = CvSqrt.n_rows;
    // containers
    arma::mat T0s1(n_pow1,n_weight);
    T0s1.fill(0);
    arma::mat T0s(nperm,n_pow1 * n_pow2);
    T0s.fill(0);

    for (int i = 0; i < nperm; i++) {
        arma::mat U00 = arma::randn(k,1);
        arma::mat U0 = CvSqrt * U00;

        for (int i2 = 0; i2 < n_weight; i2 ++) {
            arma::mat U0tmp = weight.col(i2) % U0;
            for (int j1 = 0; j1 < n_pow1; j1++) {
                if( pow1[j1] > 0) {
                    arma::vec tmp1 = pow(U0tmp,pow1[j1]);
                    double tmp2 = sum(tmp1);

                    if( tmp2 > 0 ) {
                        T0s1(j1,i2) = pow(std::abs(tmp2), 1/pow1[j1]);
                    } else {
                        T0s1(j1,i2) = -pow(std::abs(tmp2), 1/pow1[j1]);
                    }

                } else {
                    arma::vec tmp1 = abs(U0tmp);
                    T0s1(j1,i2) = max(tmp1);
                }
            }
        }

        for (int j1 = 0; j1 < n_pow1; j1++) {
            for (int j2 = 0; j2 < n_pow2; j2++) {
                if (pow2[j2] > 0) {
                    arma::mat tmp3 = pow(T0s1.row(j1),pow2[j2]);
                    T0s(i, j2 * n_pow1 + j1) = accu(tmp3);
                } else {
                    T0s(i, j2 * n_pow1 + j1) = max(abs(T0s1.row(j1)));
                }
            }
        }
    }

    Rcpp::List res;
    res["T0s"] =T0s;

    return(res);
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export()]]
arma::vec daSPU_calcT0 (arma::mat& CvSqrt,arma::mat& weight, arma::vec pow1, arma::vec pow2,  arma::vec Tsabs,SEXP pBigMat, int nperm) {

    //const int n = X1.n_rows;
    const int n_pow1 = pow1.size();
    const int n_pow2 = pow2.size();
    const int n_pow = n_pow1 * n_pow2;
    const int n_weight = weight.n_cols;

    const int k = CvSqrt.n_rows;
    // containers
    arma::mat T0s1(n_pow1,n_weight);
    T0s1.fill(0);

    XPtr<BigMatrix> xpMat(pBigMat);
    arma::mat T0s = arma::Mat<double> ( (double *)xpMat->matrix(), xpMat->nrow(), xpMat->ncol(), false);

    for (int i = 0; i < nperm; i++) {
        arma::mat U00 = arma::randn(k,1);
        arma::mat U0 = CvSqrt * U00;

        for (int i2 = 0; i2 < n_weight; i2 ++) {
            arma::mat U0tmp = weight.col(i2) % U0;
            for (int j1 = 0; j1 < n_pow1; j1++) {
                if( pow1[j1] > 0) {
                    arma::vec tmp1 = pow(U0tmp,pow1[j1]);
                    double tmp2 = sum(tmp1);

                    if( tmp2 > 0 ) {
                        T0s1(j1,i2) = pow(std::abs(tmp2), 1/pow1[j1]);
                    } else {
                        T0s1(j1,i2) = -pow(std::abs(tmp2), 1/pow1[j1]);
                    }

                } else {
                    arma::vec tmp1 = abs(U0tmp);
                    T0s1(j1,i2) = max(tmp1);
                }
            }
        }

        for (int j1 = 0; j1 < n_pow1; j1++) {
            for (int j2 = 0; j2 < n_pow2; j2++) {
                if (pow2[j2] > 0) {
                    arma::mat tmp3 = pow(T0s1.row(j1),pow2[j2]);
                    T0s(i, j2 * n_pow1 + j1) = accu(tmp3);
                } else {
                    T0s(i, j2 * n_pow1 + j1) = max(abs(T0s1.row(j1)));
                }
            }
        }
    }

    //
    T0s = arma::abs(T0s);

    arma::vec minp0(nperm);
    arma::vec P0s(nperm);
    arma::vec pPerm0(n_pow + 1);
    double minP_tmp = 1.0;

    for(int k = 0; k < n_pow; k++) {

        // Calculate p-value
        arma::vec T0stmp = T0s.col(k);
        int tmp3 = 0;
        arma::vec rankarma  = avg_rank(T0stmp);

        for( int tt=0 ; tt < nperm ; tt++) {
            if( Tsabs(k)  <= T0stmp(tt) ) {
                tmp3++;
            }

            P0s(tt) = (double) rankarma(tt) / (double) nperm;
        }

        if(k == 0) {
            minp0 = P0s;
        } else {
            for( int ii = 0; ii < nperm; ii++) {
                if( minp0(ii) > P0s(ii) ) {
                    minp0(ii) = P0s(ii);
                }
            }
        }
        double tmp_pvalue = (double) tmp3 / (double) nperm;
        pPerm0(k) = tmp_pvalue;

        if (tmp_pvalue <= minP_tmp) {
            minP_tmp = tmp_pvalue;
        }

    }

    int count_pvalue = 0;
    for(int j=0; j < nperm; j++) {
        if( minP_tmp >= minp0(j) ) {
            count_pvalue = count_pvalue + 1;
        }
    }

    pPerm0(n_pow) = (double) (count_pvalue + 1) / (double) (nperm + 1);

    return pPerm0;
}

// [[Rcpp::export]]
void set_seed(unsigned int seed) {
    Rcpp::Environment base_env("package:base");
    Rcpp::Function set_seed_r = base_env["set.seed"];
    set_seed_r(seed);
}

// This function is taken from http://stackoverflow.com/questions/39153082/rcpp-rank-function-that-does-average-ties
class Comparator {
    private:
    const Rcpp::NumericVector& ref;

    bool is_na(double x) const
    {
        return Rcpp::traits::is_na<REALSXP>(x);
    }

    public:
    Comparator(const Rcpp::NumericVector& ref_)
    : ref(ref_)
    {}

    bool operator()(const int ilhs, const int irhs) const
    {
        double lhs = ref[ilhs], rhs = ref[irhs];
        if (is_na(lhs)) return false;
        if (is_na(rhs)) return true;
        return lhs < rhs;
    }
};

// [[Rcpp::export]]
Rcpp::NumericVector avg_rank2(Rcpp::NumericVector x)
{
    R_xlen_t sz = x.size();
    Rcpp::IntegerVector w = Rcpp::seq(0, sz - 1);
    std::sort(w.begin(), w.end(), Comparator(x));

    Rcpp::NumericVector r = Rcpp::no_init_vector(sz);
    for (R_xlen_t n, i = 0; i < sz; i += n) {
        n = 1;
        while (i + n < sz && x[w[i]] == x[w[i + n]]) ++n;
        for (R_xlen_t k = 0; k < n; k++) {
            r[w[i + k]] = i + (n + 1) / 2.;
        }
    }

    return r;
}

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::export]]
Rcpp::List aSPUsPathEngine2(Rcpp::List CH, Rcpp::List CHcovSq, arma::vec pow1, arma::vec pow2, int nGenes, int n_perm, int k, arma::vec nSNPs0, arma::vec nChrom0, arma::vec Ts2, int s) {
    const int n_ch = CH.size();
    const int n_pow1 = pow1.size();
    const int n_pow2 = pow2.size();

    arma::vec T0s(n_perm);
    arma::vec T0st(nGenes*n_pow1);
    arma::vec Ts2t(n_pow1*n_pow2);
    arma::vec pPerm0(n_pow1*n_pow2);
    arma::vec minp0(n_perm);
    arma::vec P0s(n_perm);

    // iterate for pow2
    for(int j2=0 ; j2 < n_pow2; j2++) {
        // iterate for pow1
        for(int j=0 ; j < n_pow1; j++) {

            // set seed to use same random numbers for same b's
            // This is necessary to use efficient memory
            set_seed(s);
            for(int b=0; b < n_perm; b++) {

                // Generate Score from null distribution
                Rcpp::NumericVector U00 = rnorm(k,0,1);
                arma::vec u0 = as<arma::vec>(U00);
                arma::vec U0(1);

                int SNPstart = 0;
                // iterate for chromosome
                for(int b2 = 0; b2 < n_ch ; b2++) {

                    // itreate for chromosome
                    if( b2 != 0) {
                        SNPstart = sum( nChrom0.subvec(0,b2-1) ) ;
                    }
                    int idx1 = SNPstart;
                    int idx2 = SNPstart+nChrom0(b2)-1;

                    arma::vec TT = as<arma::mat>(CHcovSq[b2])*u0.subvec(idx1, idx2) ;
                    U0 = join_cols(U0,TT) ;
                }

                arma::vec UU2 = U0.subvec(1,U0.size()-1);

                // iterate for genes
                SNPstart = 0;
                for(int iGene = 0 ; iGene < nGenes ; iGene++ ) {

                    // calculate starting and ending position of each gene from nSNPs0 vector
                    if( iGene != 0) {
                        SNPstart = sum( nSNPs0.subvec(0,iGene-1) ) ;
                    }
                    int idx1 = SNPstart;
                    int idx2 = SNPstart+nSNPs0(iGene)-1;

                    // calculate 1st level test statistic
                    if( pow1[j] > 0) {
                        arma::vec tmp1 = pow(UU2.subvec(idx1, idx2),pow1[j]);
                        double tmp2 = sum(tmp1);

                        if( tmp2 > 0 ) {
                            T0st(j*nGenes+iGene) = pow(std::abs(tmp2)/nSNPs0[iGene] , 1/pow1[j]);
                        } else {
                            T0st(j*nGenes+iGene) = -pow(std::abs(tmp2)/nSNPs0[iGene] , 1/pow1[j]);
                        }

                    } else {
                        arma::vec T0tp = abs(UU2.subvec(idx1, idx2));
                        T0st(j*nGenes+iGene) = max(abs(T0tp));
                    }
                }

                // calculate 2nd level test statistics
                if( pow2[j2] > 0) {
                    arma::vec tmp3 = pow(T0st.subvec(j*nGenes,(j+1)*nGenes-1), pow2[j2]);
                    double tmp4 = sum(tmp3);
                    T0s(b) = tmp4;
                } else {
                    T0s(b) = max( arma::abs(T0st.subvec(j*nGenes,(j+1)*nGenes-1)) ) ;
                }
            }

            // Calculate P-values
            int tmp3 = 0;
            arma::vec T0sabs = arma::abs(T0s);
            Rcpp::NumericVector a( T0sabs.begin(), T0sabs.end() );
            Rcpp::NumericVector ranka = avg_rank2(a);
            arma::vec rankarma = as<arma::vec>(ranka);

            for( int tt=0 ; tt < n_perm ; tt++) {
                if( std::abs(Ts2(j2*n_pow1 + j) ) <= std::abs(T0s(tt))) {
                    tmp3++;
                }

                P0s(tt) = (double) (n_perm - rankarma(tt) + 1) / (double) n_perm;
            }

            minp0 = P0s;

            if(j == 1) {
                for( int ii=0; ii < n_perm; ii++) {
                    minp0(ii) = P0s(ii);
                }
            } else {
                for( int ii=0; ii < n_perm; ii++) {
                    if( minp0(ii) > P0s(ii) ) {
                        minp0(ii) = P0s(ii);
                    }
                }
            }

            pPerm0(j2*n_pow1 + j) = (double) tmp3 / (double) n_perm;

        }
    }
    return Rcpp::List::create(Rcpp::Named("minp0") = minp0,
                              Rcpp::Named("pPerm0") = pPerm0,
                              Rcpp::Named("P0s") = P0s);
}


/******************************************************************************

  Markov random field model for regular 2D lattice.

  Copyright (c) 2009 - 2012
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

*******************************************************************************/

#ifndef MRF_2D_HPP_1140ED81_1E3E_4AEB_AFBB_6CA70FE3EF9B_
#define MRF_2D_HPP_1140ED81_1E3E_4AEB_AFBB_6CA70FE3EF9B_

#include <cstddef>
#include <cmath>
#include <stdexcept>
#include <boost/random.hpp>
#include <boost/assert.hpp>
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>

#include "bo/core/raw_image_2d.hpp"

namespace bo {
namespace mrf {

// A class representing a Markov random field on a regular 2D lattice. Operates on
// random field configuration using given observation and clique functions. Only first
// order (pairwise) neighbourhood is supproted. No copies of an instance are allowed.
template <typename NodeType, typename DataType, typename RealType>
class MRF2D: public boost::noncopyable
{
public:
    typedef NodeType& reference;
    typedef const NodeType& const_reference;

    typedef RawImage2D<NodeType> RandomLattice;
    typedef RawImage2D<DataType> DataLattice;
    typedef boost::function<RealType (DataType, NodeType)> LikelihoodEnergy;
    typedef boost::function<RealType (NodeType, NodeType)> PriorEnergy;

    MRF2D(const RandomLattice& initial_configuration, const DataLattice& observation,
          LikelihoodEnergy likelihood, PriorEnergy prior);

    RealType compute_full_energy() const;
    RealType compute_local_energy(NodeType val, std::size_t col, std::size_t row) const;
    RealType compute_local_likelihood(NodeType val, std::size_t col, std::size_t row) const;

    const_reference operator()(std::size_t col, std::size_t row) const;
    reference operator()(std::size_t col, std::size_t row);

    std::size_t width() const;
    std::size_t height() const;

private:
    RealType right_clique_(NodeType val, std::size_t col, std::size_t row) const;
    RealType down_clique_(NodeType val, std::size_t col, std::size_t row) const;
    RealType left_clique_(NodeType val, std::size_t col, std::size_t row) const;
    RealType up_clique_(NodeType val, std::size_t col, std::size_t row) const;

    RealType prior_fun_(NodeType val, std::size_t neigh_col, std::size_t neigh_row) const;
    RealType likelihood_fun_(NodeType val, std::size_t col, std::size_t row) const;

private:
    RandomLattice configuration_;
    DataLattice observation_;
    LikelihoodEnergy likelihood_;
    PriorEnergy prior_;
};

template <typename NodeType, typename DataType, typename RealType>
MRF2D<NodeType, DataType, RealType>::MRF2D(const RandomLattice& initial_configuration,
    const DataLattice& observation, LikelihoodEnergy likelihood, PriorEnergy prior):
    configuration_(initial_configuration),
    observation_(observation),
    likelihood_(likelihood),
    prior_(prior)
{
    // Check if the dimensions of observation and configuration are the same.
    if ((configuration_.width() != observation_.width()) ||
        (configuration_.height() != configuration_.height()))
    {
        BOOST_ASSERT(false && "Dimensions of the configuration and observation do not coincide.");
        throw std::logic_error("MRF2D: dimensions of the random field lattice and the "
                               "observation data must be the same.");
    }
}

// Computes full energy of the current MRF state. It takes sum over all zero order
// (single node) and first order (every two adjacent nodes) cliques using corresponding
// clique functions. In order to iterate over all first order cliques, for every
// node we can consider only right and down neighbour except for some border nodes.
template <typename NodeType, typename DataType, typename RealType>
RealType MRF2D<NodeType, DataType, RealType>::compute_full_energy() const
{
    RealType energy(0);

    for (std::size_t col = 0; col < configuration_.width(); ++col) {
        for (std::size_t row = 0; row < configuration_.height(); ++row) {
            NodeType value = configuration_(col, row);
            energy += (likelihood_fun_(value, col, row) +
                       right_clique_(value, col, row) +
                       down_clique_(value, col, row));
    }   }

    return energy;
}

// Computes a local energy of the neighbourhood of the given node with provided value.
// Border checks are done inside corresponding clique functions.
template <typename NodeType, typename DataType, typename RealType>
RealType MRF2D<NodeType, DataType, RealType>::compute_local_energy(NodeType val,
    std::size_t col, std::size_t row) const
{
    // Compute likelihood energy for the given value at the given position.
    RealType energy = likelihood_fun_(val, col, row);

    // Compute prior energy of the given node given its neighbours.
    energy += (right_clique_(val, col, row) + down_clique_(val, col, row) +
               left_clique_(val, col, row) + up_clique_(val, col, row));

    return energy;
}

// Computes a likelihood function for the given node with provided value.
template <typename NodeType, typename DataType, typename RealType>
RealType MRF2D<NodeType, DataType, RealType>::compute_local_likelihood(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        likelihood_fun_(val, col, row);
}

template <typename NodeType, typename DataType, typename RealType> inline
typename MRF2D<NodeType, DataType, RealType>::const_reference
MRF2D<NodeType, DataType, RealType>::operator()(std::size_t col, std::size_t row) const
{
    return configuration_(col, row);
}

template <typename NodeType, typename DataType, typename RealType> inline
typename MRF2D<NodeType, DataType, RealType>::reference
MRF2D<NodeType, DataType, RealType>::operator()(std::size_t col, std::size_t row)
{
    return configuration_(col, row);
}

template <typename NodeType, typename DataType, typename RealType> inline
std::size_t MRF2D<NodeType, DataType, RealType>::width() const
{
    return configuration_.width();
}

template <typename NodeType, typename DataType, typename RealType> inline
std::size_t MRF2D<NodeType, DataType, RealType>::height() const
{
    return configuration_.height();
}

// Next four functions represent cliques and neighbourhood relations in the model.
// They check for border overrun and call the associated clique function.
template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::right_clique_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        (col < configuration_.width() - 1) ? prior_fun_(val, col + 1, row) : RealType(0);
}

template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::down_clique_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        (row < configuration_.height() - 1) ? prior_fun_(val, col, row + 1) : RealType(0);
}

template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::left_clique_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        (col > 0) ? prior_fun_(val, col - 1, row) : RealType(0);
}

template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::up_clique_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return
        (row > 0) ? prior_fun_(val, col, row - 1) : RealType(0);
}

// Computes prior energy for two nodes.
template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::prior_fun_(NodeType val,
    std::size_t neigh_col, std::size_t neigh_row) const
{
    return prior_(val, configuration_(neigh_col, neigh_row));
}

// Computes likelihood energy for the given node.
template <typename NodeType, typename DataType, typename RealType> inline
RealType MRF2D<NodeType, DataType, RealType>::likelihood_fun_(NodeType val,
    std::size_t col, std::size_t row) const
{
    return likelihood_(observation_(col, row), val);
}

} // namespace mrf
} // namespace bo

#endif // MRF_2D_HPP_1140ED81_1E3E_4AEB_AFBB_6CA70FE3EF9B_


#ifndef EDGE_PLANE_PARALLEL_HPP
#define EDGE_PLANE_PARALLEL_HPP

#include <Eigen/Dense>
#include <g2o/core/base_binary_edge.h>
#include <g2o/types/slam3d_addons/vertex_plane.h>

namespace g2o {

class EdgePlaneParallel : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgePlaneParallel() : BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane>() {
    _information.setIdentity();
    _error.setZero();
  }

  void computeError() override {
    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);

    Eigen::Vector3d normal1 = v1->estimate().normal();
    Eigen::Vector3d normal2 = v2->estimate().normal();

    if (normal1.dot(normal2) < 0.0) {
      normal2 = -normal2;
    }

    _error = (normal2 - normal1) - _measurement;
  }
  virtual bool read(std::istream& is) override {
    Eigen::Vector3d v;
    for (int i = 0; i < 3; ++i) {
      is >> v[i];
    }

    setMeasurement(v);
    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        is >> information()(i, j);
        if(i != j) {
          information()(j, i) = information()(i, j);
        }
      }
    }

    return true;
  }

  virtual bool write(std::ostream& os) const override {
    for(int i = 0; i < 3; ++i) {
      os << _measurement[i] << " ";
    }

    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        os << " " << information()(i, j);
      };
    }
    return os.good();
  }

  virtual void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; }

  virtual int measurementDimension() const override { return 3; }
};

class EdgePlanePerpendicular : public BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgePlanePerpendicular() : BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane>() {
    _information.setIdentity();
    _error.setZero();
  }

  void computeError() override {
    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);

    Eigen::Vector3d normal1 = v1->estimate().normal().normalized();
    Eigen::Vector3d normal2 = v2->estimate().normal().normalized();

    _error[0] = normal1.dot(normal2);
  }
  virtual bool read(std::istream& is) override {
    Eigen::Vector3d v;
    for (int i = 0; i < 3; ++i) {
      is >> v[i];
    }

    setMeasurement(v);
    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        is >> information()(i, j);
        if (i != j) {
          information()(j, i) = information()(i, j);
        }
      }
    }

    return true;
  }

  virtual bool write(std::ostream& os) const override {
    for (int i = 0; i < 3; ++i) {
      os << _measurement[i] << " ";
    }

    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        os << " " << information()(i, j);
      };
    }
    return os.good();
  }

  virtual void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; }

  virtual int measurementDimension() const override { return 3; }
};

}  // namespace g2o

#endif  // EDGE_PLANE_PARALLEL_HPP


// Copyright (C) 2008-today The SG++ project
// This file is part of the SG++ project. For conditions of distribution and
// use, please see the copyright notice provided with SG++ or at
// sgpp.sparsegrids.org

/**
 * \page example_optimization_cpp optimization.cpp
 *
 * On this page, we look at an example application of the sgpp::optimization module.
 * Versions of the example are given in all languages
 * currently supported by SG++: C++, Python, Java, and MATLAB.
 *
 * The example interpolates a bivariate test function with B-splines instead
 * of piecewise linear basis functions to obtain a smoother interpolant.
 * The resulting sparse grid function is then minimized with the method of steepest descent.
 * For comparison, we also minimize the objective function with Nelder-Mead's method.
 */

/**
 * First, we include all the necessary headers, including those of the sgpp::base and
 * sgpp::optimization module.
 */
#include <sgpp_base.hpp>
#include <sgpp/base/grid/Grid.hpp>
#include <sgpp/base/datatypes/DataMatrix.hpp>
#include <sgpp/datadriven/DatadrivenOpFactory.hpp>
#include <sgpp/datadriven/application/KernelDensityEstimator.hpp>
#include <sgpp/datadriven/application/LearnerSGDE.hpp>
#include <sgpp/datadriven/configuration/RegularizationConfiguration.hpp>
#include <sgpp/datadriven/tools/ARFFTools.hpp>
#include <sgpp_optimization.hpp>
#include <sgpp/globaldef.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>

#include <algorithm>
#include <iostream>
#include <iterator>
#include <random>
#include <string>

using sgpp::base::DataMatrix;
using sgpp::base::DataVector;
using sgpp::base::Grid;
using sgpp::base::GridGenerator;
using sgpp::base::GridStorage;
using boost::property_tree::ptree;

/**
 * The function \f$f\colon [0, 1]^d \to \mathbb{R}\f$ to be minimized
 * is called <i>objective function</i> and has to derive from
 * sgpp::optimization::ScalarFunction.
 * In the constructor, we give the dimensionality of the domain
 * (in this case \f$d = 2\f$).
 * The eval method evaluates the objective function and returns the function
 * value \f$f(\vec{x})\f$ for a given point \f$\vec{x} \in [0, 1]^d\f$.
 * The clone method returns a std::unique_ptr to a clone of the object
 * and is used for parallelization (in case eval is not thread-safe).
 */
class ExampleFunction : public sgpp::optimization::ScalarFunction {
 public:
  sgpp::base::DataVector featureVector;
  sgpp::datadriven::LearnerSGDE& learner;

  // our optimization problem is 3dim
  ExampleFunction(sgpp::datadriven::LearnerSGDE& _learner) : sgpp::optimization::ScalarFunction(3), learner(_learner) {}

  // x is 3dim, we need to supply the rest (feature vector)
  double eval(const sgpp::base::DataVector& x) {

        sgpp::base::DataVector xNew(x);

        //xNew.append(featureVector.get(0));
        //xNew.append(featureVector.get(1));
        //xNew.append(featureVector.get(2));
        //xNew.append(featureVector.get(3));

        xNew.append(featureVector.get(0));
        xNew.append(featureVector.get(1));
        xNew.append(featureVector.get(2));
        xNew.append(featureVector.get(3));

        return learner.pdf(xNew);
  }

  virtual void clone(std::unique_ptr<sgpp::optimization::ScalarFunction>& clone) const {
    clone = std::unique_ptr<sgpp::optimization::ScalarFunction>(new ExampleFunction(*this));
  }

  void setParamters(const sgpp::base::DataVector& reqFeatureVector){
    featureVector=reqFeatureVector;
  }
};

/**
 * Now, we can start with the \c main function.
 */
void printLine() {
  std::cout << "----------------------------------------"
               "----------------------------------------\n";
}

int main(int argc, const char* argv[]) {
  (void)argc;
  (void)argv;

  double featureVectorCoeff_0 = atof(argv[3]);
  double featureVectorCoeff_1 = atof(argv[4]);
  double featureVectorCoeff_2 = atof(argv[5]);
  double featureVectorCoeff_3 = atof(argv[6]);

  std::string filename = "../" + std::string(argv[1]);
  std::string ininame = "../" + std::string(argv[2]);

  std::cout << "# loading file: " << filename << std::endl;
  sgpp::datadriven::Dataset dataset =
    sgpp::datadriven::ARFFTools::readARFFFromFile(filename);
  sgpp::base::DataMatrix& samples = dataset.getData();

  /**
   * Configure the sparse grid of level 3 with linear basis functions and the same dimension as the
   * given test data.
   * Alternatively load a sparse grid that has been saved to a file, see the commented line.
   */
  std::cout << "# create grid config" << std::endl;
  sgpp::base::RegularGridConfiguration gridConfig;
  gridConfig.dim_ = dataset.getDimension();
  gridConfig.level_ = 3;
  gridConfig.type_ = sgpp::base::GridType::Linear; //TODO: BSplines not available, but does not matter
  //  gridConfig.filename_ = "/tmp/sgde-grid-4391dc6e-54cd-4ca2-9510-a9c02a2889ec.grid";

  /**
   * Configure the adaptive refinement. Therefore the number of refinements and the number of points
   * are specified.
   */
  std::cout << "# create adaptive refinement config" << std::endl;
  sgpp::base::AdaptivityConfiguration adaptConfig;
  adaptConfig.numRefinements_ = 0;
  adaptConfig.noPoints_ = 10;

  /**
   * Configure the solver. The solver type is set to the conjugent gradient method and the maximum
   * number of iterations, the tolerance epsilon and the threshold are specified.
   */
  std::cout << "# create solver config" << std::endl;
  sgpp::solver::SLESolverConfiguration solverConfig;
  solverConfig.type_ = sgpp::solver::SLESolverType::CG;
  solverConfig.maxIterations_ = 1000;
  solverConfig.eps_ = 1e-14;
  solverConfig.threshold_ = 1e-14;

  /**
   * Configure the regularization for the laplacian operator.
   */
  std::cout << "# create regularization config" << std::endl;
  sgpp::datadriven::RegularizationConfiguration regularizationConfig;
  regularizationConfig.type_ = sgpp::datadriven::RegularizationType::Laplace;

  /**
   * Configure the learner by specifying:
   * - an initial value for the lagrangian multiplier \f$\lambda\f$ and the interval
   *   \f$ [\lambda_{Start} , \lambda_{End}] \f$ in which \f$\lambda\f$ will be searched,
   * - whether a logarithmic scale is used,
   * - the parameters shuffle and an initial seed for the random value generation,
   * - whether parts of the output shall be kept off.
   */
  std::cout << "# create learner config" << std::endl;
  sgpp::datadriven::CrossvalidationConfiguration crossvalidationConfig;
  crossvalidationConfig.enable_ = false;
  crossvalidationConfig.kfold_ = 3;
  crossvalidationConfig.lambda_ = 3.16228e-06;
  crossvalidationConfig.lambdaStart_ = 1e-1;
  crossvalidationConfig.lambdaEnd_ = 1e-10;
  crossvalidationConfig.lambdaSteps_ = 3;
  crossvalidationConfig.logScale_ = true;
  crossvalidationConfig.shuffle_ = true;
  crossvalidationConfig.seed_ = 1234567;
  crossvalidationConfig.silent_ = false;

  /**
   * Create the learner using the configuratons set above. Then initialize it with the data read
   * from the file in the first step and train the learner.
   */
  std::cout << "# creating the learner" << std::endl;
  sgpp::datadriven::LearnerSGDE learner(gridConfig, adaptConfig, solverConfig, regularizationConfig,
                                        crossvalidationConfig);
  learner.initialize(samples);
  learner.train();
  /**
   * Estimate the probability density function (pdf) via a gaussian kernel density estimation (KDE)
   * and print the corresponding values.
   */
  sgpp::datadriven::KernelDensityEstimator kde(samples);
  sgpp::base::DataVector x(learner.getDim());
  // samples.getRow(samples.getNrows()/2, x);
  samples.getRow(1, x);

  std::cout << "--------------------------------------------------------\n";
  std::cout << "x= " << x.toString() << "\n";
  std::cout << "SGDE dimensions " << learner.getDim() << "\n";
  std::cout << "SGDE sample count " << learner.getNsamples() << "\n";
  std::cout << learner.getSurpluses()->getSize() << " -> " << learner.getSurpluses()->sum() << "\n";
  std::cout << "pdf_SGDE(x) = " << learner.pdf(x) << " ~ " << kde.pdf(x) << " =pdf_KDE(x)\n";
  std::cout << "mean_SGDE(x) = " << learner.mean() << " ~ " << kde.mean() << " = mean_KDE(x)\n";
  std::cout << "var_SGDE(x) = " << learner.variance() << " ~ " << kde.variance() << "=var_KDE(x)\n";

  std::cout << "sgpp::optimization example program started.\n\n";
  // increase verbosity of the output
  sgpp::optimization::Printer::getInstance().setVerbosity(2);

  /**
   * Here, we define some parameters: objective function, dimensionality,
   * B-spline degree, maximal number of grid points, and adaptivity.
   */
  // objective function
  ExampleFunction f(learner);
  sgpp::base::DataVector testFV;
  testFV.append(featureVectorCoeff_0);
  testFV.append(featureVectorCoeff_1);
  testFV.append(featureVectorCoeff_2);
  testFV.append(featureVectorCoeff_3);

  f.setParamters(testFV);
  // dimension of domain
  const size_t d = f.getNumberOfParameters();
  // B-spline degree
  const size_t p = 3;
  // maximal number of grid points
  const size_t N = 30;
  // adaptivity of grid generation
  const double gamma = 0.95;

  /**
   * First, we define a grid with modified B-spline basis functions and
   * an iterative grid generator, which can generate the grid adaptively.
   */
  sgpp::base::ModBsplineGrid grid(d, p);
  sgpp::optimization::IterativeGridGeneratorRitterNovak gridGen(f, grid, N, gamma);

  /**
   * With the iterative grid generator, we generate adaptively a sparse grid.
   */
  printLine();
  std::cout << "Generating grid...\n\n";

  if (!gridGen.generate()) {
    std::cout << "Grid generation failed, exiting.\n";
    return 1;
  }

  /**
   * Then, we hierarchize the function values to get hierarchical B-spline
   * coefficients of the B-spline sparse grid interpolant
   * \f$\tilde{f}\colon [0, 1]^d \to \mathbb{R}\f$.
   */
  printLine();
  std::cout << "Hierarchizing...\n\n";
  sgpp::base::DataVector functionValues(gridGen.getFunctionValues());
  sgpp::base::DataVector coeffs(functionValues.getSize());
  sgpp::optimization::HierarchisationSLE hierSLE(grid);
  sgpp::optimization::sle_solver::Auto sleSolver;

  // solve linear system
  if (!sleSolver.solve(hierSLE, functionValues, coeffs)) {
    std::cout << "Solving failed, exiting.\n";
    return 1;
  }

  /**
   * We define the interpolant \f$\tilde{f}\f$ and its gradient
   * \f$\nabla\tilde{f}\f$ for use with the gradient method (steepest descent).
   * Of course, one can also use other optimization algorithms from
   * sgpp::optimization::optimizer.
   */
  printLine();
  std::cout << "Optimizing smooth interpolant...\n\n";
  sgpp::optimization::InterpolantScalarFunction ft(grid, coeffs);
  sgpp::optimization::InterpolantScalarFunctionGradient ftGradient(grid, coeffs);
  sgpp::optimization::optimizer::GradientDescent gradientDescent(ft, ftGradient);
  sgpp::base::DataVector x0(d);
  double fX0;
  double ftX0;

  /**
   * The gradient method needs a starting point.
   * We use a point of our adaptively generated sparse grid as starting point.
   * More specifically, we use the point with the smallest
   * (most promising) function value and save it in x0.
   */
  {
    sgpp::base::GridStorage& gridStorage = grid.getStorage();

    // index of grid point with minimal function value
    size_t x0Index =
        std::distance(functionValues.getPointer(),
                      std::min_element(functionValues.getPointer(),
                                       functionValues.getPointer() + functionValues.getSize()));

    //x0 = gridStorage.getCoordinates(gridStorage[x0Index]);

    x0.set(0, samples.get(0,0)/2);
    x0.set(1, samples.get(0,1));
    x0.set(2, samples.get(0,2));

    fX0 = functionValues[x0Index];
    ftX0 = ft.eval(x0);
  }

  std::cout << "Gradient starting point:\n";
  std::cout << "x0 = " << x0.toString() << "\n";
  std::cout << "f(x0) = " << fX0 << ", ft(x0) = " << ftX0 << "\n\n";

  /**
   * We apply the gradient method and print the results.
   */
  gradientDescent.setStartingPoint(x0);
  gradientDescent.optimize();
  const sgpp::base::DataVector& xOpt = gradientDescent.getOptimalPoint();
  const double ftXOpt = gradientDescent.getOptimalValue();
  const double fXOpt = f.eval(xOpt);

  std::cout << "\noptimal control point:\n";
  std::cout << "for feature vector" << testFV.toString() << "\n";
  std::cout << "xOpt = " << xOpt.toString() << "\n";
  std::cout << "f(xOpt) = " << fXOpt << ", ft(xOpt) = " << ftXOpt << "\n\n";

  /**
   * We modify the control point inside the .ini file
   */

   ptree pt;

   boost::property_tree::ini_parser::read_ini( ininame, pt );

   pt.put("Simulation.cpX", xOpt.get(0));
   pt.put("Simulation.cpY", xOpt.get(1));
   pt.put("Simulation.cpZ", xOpt.get(2));

   boost::property_tree::write_ini( ininame, pt );

  return 0;
}


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

//
// This is not a complete header file, it is included by beta.hpp
// after it has defined it's definitions.  This inverts the incomplete
// beta functions ibeta and ibetac on the first parameters "a"
// and "b" using a generic root finding algorithm (TOMS Algorithm 748).
//

#ifndef BOOST_MATH_SP_DETAIL_BETA_INV_AB
#define BOOST_MATH_SP_DETAIL_BETA_INV_AB

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/toms748_solve.hpp>
#include <boost/cstdint.hpp>

namespace boost{ namespace math{ namespace detail{

template <class T, class Policy>
struct beta_inv_ab_t
{
   beta_inv_ab_t(T b_, T z_, T p_, bool invert_, bool swap_ab_) : b(b_), z(z_), p(p_), invert(invert_), swap_ab(swap_ab_) {}
   T operator()(T a)
   {
      return invert ?
         p - boost::math::ibetac(swap_ab ? b : a, swap_ab ? a : b, z, Policy())
         : boost::math::ibeta(swap_ab ? b : a, swap_ab ? a : b, z, Policy()) - p;
   }
private:
   T b, z, p;
   bool invert, swap_ab;
};

template <class T, class Policy>
T inverse_negative_binomial_cornish_fisher(T n, T sf, T sfc, T p, T q, const Policy& pol)
{
   BOOST_MATH_STD_USING
   // mean:
   T m = n * (sfc) / sf;
   T t = sqrt(n * (sfc));
   // standard deviation:
   T sigma = t / sf;
   // skewness
   T sk = (1 + sfc) / t;
   // kurtosis:
   T k = (6 - sf * (5+sfc)) / (n * (sfc));
   // Get the inverse of a std normal distribution:
   T x = boost::math::erfc_inv(p > q ? 2 * q : 2 * p, pol) * constants::root_two<T>();
   // Set the sign:
   if(p < 0.5)
      x = -x;
   T x2 = x * x;
   // w is correction term due to skewness
   T w = x + sk * (x2 - 1) / 6;
   //
   // Add on correction due to kurtosis.
   //
   if(n >= 10)
      w += k * x * (x2 - 3) / 24 + sk * sk * x * (2 * x2 - 5) / -36;

   w = m + sigma * w;
   if(w < tools::min_value<T>())
      return tools::min_value<T>();
   return w;
}

template <class T, class Policy>
T ibeta_inv_ab_imp(const T& b, const T& z, const T& p, const T& q, bool swap_ab, const Policy& pol)
{
   BOOST_MATH_STD_USING  // for ADL of std lib math functions
   //
   // Special cases first:
   //
   BOOST_MATH_INSTRUMENT_CODE("b = " << b << " z = " << z << " p = " << p << " q = " << " swap = " << swap_ab);
   if(p == 0)
   {
      return swap_ab ? tools::min_value<T>() : tools::max_value<T>();
   }
   if(q == 0)
   {
      return swap_ab ? tools::max_value<T>() : tools::min_value<T>();
   }
   //
   // Function object, this is the functor whose root
   // we have to solve:
   //
   beta_inv_ab_t<T, Policy> f(b, z, (p < q) ? p : q, (p < q) ? false : true, swap_ab);
   //
   // Tolerance: full precision.
   //
   tools::eps_tolerance<T> tol(policies::digits<T, Policy>());
   //
   // Now figure out a starting guess for what a may be,
   // we'll start out with a value that'll put p or q
   // right bang in the middle of their range, the functions
   // are quite sensitive so we should need too many steps
   // to bracket the root from there:
   //
   T guess = 0;
   T factor = 5;
   //
   // Convert variables to parameters of a negative binomial distribution:
   //
   T n = b;
   T sf = swap_ab ? z : 1-z;
   T sfc = swap_ab ? 1-z : z;
   T u = swap_ab ? p : q;
   T v = swap_ab ? q : p;
   if(u <= pow(sf, n))
   {
      //
      // Result is less than 1, negative binomial approximation
      // is useless....
      //
      if((p < q) != swap_ab)
      {
         guess = (std::min)(T(b * 2), T(1));
      }
      else
      {
         guess = (std::min)(T(b / 2), T(1));
      }
   }
   if(n * n * n * u * sf > 0.005)
      guess = 1 + inverse_negative_binomial_cornish_fisher(n, sf, sfc, u, v, pol);

   if(guess < 10)
   {
      //
      // Negative binomial approximation not accurate in this area:
      //
      if((p < q) != swap_ab)
      {
         guess = (std::min)(T(b * 2), T(10));
      }
      else
      {
         guess = (std::min)(T(b / 2), T(10));
      }
   }
   else
      factor = (v < sqrt(tools::epsilon<T>())) ? 2 : (guess < 20 ? 1.2f : 1.1f);
   BOOST_MATH_INSTRUMENT_CODE("guess = " << guess);
   //
   // Max iterations permitted:
   //
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   std::pair<T, T> r = bracket_and_solve_root(f, guess, factor, swap_ab ? true : false, tol, max_iter, pol);
   if(max_iter >= policies::get_max_root_iterations<Policy>())
      return policies::raise_evaluation_error<T>("boost::math::ibeta_invab_imp<%1%>(%1%,%1%,%1%)", "Unable to locate the root within a reasonable number of iterations, closest approximation so far was %1%", r.first, pol);
   return (r.first + r.second) / 2;
}

} // namespace detail

template <class RT1, class RT2, class RT3, class Policy>
typename tools::promote_args<RT1, RT2, RT3>::type
      ibeta_inva(RT1 b, RT2 x, RT3 p, const Policy& pol)
{
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::ibeta_inva<%1%>(%1%,%1%,%1%)";
   if(p == 0)
   {
      return policies::raise_overflow_error<result_type>(function, 0, Policy());
   }
   if(p == 1)
   {
      return tools::min_value<result_type>();
   }

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(
      detail::ibeta_inv_ab_imp(
         static_cast<value_type>(b),
         static_cast<value_type>(x),
         static_cast<value_type>(p),
         static_cast<value_type>(1 - static_cast<value_type>(p)),
         false, pol),
      function);
}

template <class RT1, class RT2, class RT3, class Policy>
typename tools::promote_args<RT1, RT2, RT3>::type
      ibetac_inva(RT1 b, RT2 x, RT3 q, const Policy& pol)
{
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::ibetac_inva<%1%>(%1%,%1%,%1%)";
   if(q == 1)
   {
      return policies::raise_overflow_error<result_type>(function, 0, Policy());
   }
   if(q == 0)
   {
      return tools::min_value<result_type>();
   }

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(
      detail::ibeta_inv_ab_imp(
         static_cast<value_type>(b),
         static_cast<value_type>(x),
         static_cast<value_type>(1 - static_cast<value_type>(q)),
         static_cast<value_type>(q),
         false, pol),
      function);
}

template <class RT1, class RT2, class RT3, class Policy>
typename tools::promote_args<RT1, RT2, RT3>::type
      ibeta_invb(RT1 a, RT2 x, RT3 p, const Policy& pol)
{
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   static const char* function = "boost::math::ibeta_invb<%1%>(%1%,%1%,%1%)";
   if(p == 0)
   {
      return tools::min_value<result_type>();
   }
   if(p == 1)
   {
      return policies::raise_overflow_error<result_type>(function, 0, Policy());
   }

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(
      detail::ibeta_inv_ab_imp(
         static_cast<value_type>(a),
         static_cast<value_type>(x),
         static_cast<value_type>(p),
         static_cast<value_type>(1 - static_cast<value_type>(p)),
         true, pol),
      function);
}

template <class RT1, class RT2, class RT3, class Policy>
typename tools::promote_args<RT1, RT2, RT3>::type
      ibetac_invb(RT1 a, RT2 x, RT3 q, const Policy& pol)
{
   static const char* function = "boost::math::ibeta_invb<%1%>(%1%, %1%, %1%)";
   typedef typename tools::promote_args<RT1, RT2, RT3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   if(q == 1)
   {
      return tools::min_value<result_type>();
   }
   if(q == 0)
   {
      return policies::raise_overflow_error<result_type>(function, 0, Policy());
   }

   return policies::checked_narrowing_cast<result_type, forwarding_policy>(
      detail::ibeta_inv_ab_imp(
         static_cast<value_type>(a),
         static_cast<value_type>(x),
         static_cast<value_type>(1 - static_cast<value_type>(q)),
         static_cast<value_type>(q),
         true, pol),
         function);
}

template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
         ibeta_inva(RT1 b, RT2 x, RT3 p)
{
   return boost::math::ibeta_inva(b, x, p, policies::policy<>());
}

template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
         ibetac_inva(RT1 b, RT2 x, RT3 q)
{
   return boost::math::ibetac_inva(b, x, q, policies::policy<>());
}

template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
         ibeta_invb(RT1 a, RT2 x, RT3 p)
{
   return boost::math::ibeta_invb(a, x, p, policies::policy<>());
}

template <class RT1, class RT2, class RT3>
inline typename tools::promote_args<RT1, RT2, RT3>::type
         ibetac_invb(RT1 a, RT2 x, RT3 q)
{
   return boost::math::ibetac_invb(a, x, q, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SP_DETAIL_BETA_INV_AB


/**
 * @Author: Julien Vial-Detambel <l3ninj>
 * @Date:   2018-06-12T11:23:27+01:00
 * @Email: julien.vial-detambel@epitech.eu
 * @Project: CUDA-Based Simulator of Quantum Systems
 * @Filename: Simulator.cpp
 * @Last modified by:   l3ninj
 * @Last modified time: 2018-08-23T12:13:08+02:00
 * @License: MIT License
 */

#include <iostream>
#include <cmath>
#include <random>

#include <boost/foreach.hpp>

#include "MatrixStore.hpp"
#include "Worker/Simulator.hpp"
#include "Logger.hpp"

using namespace TaskGraph;
using namespace MeasurementResultsTree;

Simulator::Simulator(SimulateCircuitTask &task, IMeasurementResultsTree &measurementsTree, Matrix const &state)
: m_task(task), m_measurementsTree(measurementsTree), m_state(state) {
  // Computing the offsets for each qregisters,
  // and total qubits number of the system.
  m_size = 0;
  for (auto &reg: m_task.circuit.qreg) {
    m_qRegOffsets.insert(make_pair(reg.name, m_size));
    m_size += reg.size;
  }
}

Simulator::StepVisitor::StepVisitor(Simulator& simulator) :
m_simulator(simulator) {}

void Simulator::StepVisitor::operator()(const Circuit::UGate& value) {
  // Computing the offset of the target qubit
  Circuit::Qubit target = value.target;
  int id = m_simulator.m_qRegOffsets.find(target.registerName)->second;
  id += target.element;

  // Creating a gate according to the phi, theta and lambda parameters and
  // setting it as the target qubit transformation gate.
  using namespace std::complex_literals;
  m_simulator.m_gates[id] = Matrix(std::shared_ptr<Tvcplxd>(new Tvcplxd({exp(-1i * (value.phi + value.lambda) / 2.0)
    * cos(value.theta / 2.0),
    -exp(-1i * (value.phi - value.lambda) / 2.0) * sin(value.theta / 2.0),
    exp(1i * (value.phi - value.lambda) / 2.0) * sin(value.theta / 2.0),
    exp(1i * (value.phi + value.lambda) / 2.0) * cos(value.theta / 2.0)
  })), 2, 2);

  // Debug logs
  LOG(Logger::DEBUG, "Applying a U Gate:" << "\nU Gate:\n\ttheta: "
    << value.theta << ", phi: " << value.phi << ", lambda: " << value.lambda
    << "\n\ttarget: " << value.target.registerName << "["
    << value.target.element << "]\n" << std::endl);
}

void Simulator::StepVisitor::operator()(const Circuit::CXGate& value) {
  // Apparently this method requires us to normalize the state afterward
  m_simulator.m_shouldNormalize = true;
  m_simulator.m_shouldAddSecondMatrix = true;

  // Computing the offset of the control qubit.
  Circuit::Qubit control = value.control;
  int controlId = m_simulator.m_qRegOffsets.find(control.registerName)->second;
  controlId += control.element;

  // Computing the offset of the target qubit.
  Circuit::Qubit target = value.target;
  int targetId = m_simulator.m_qRegOffsets.find(target.registerName)->second;
  targetId += target.element;

  m_simulator.m_gates[controlId] = MatrixStore::pk0;
  m_simulator.m_extraGates[controlId] = MatrixStore::pk1;
  m_simulator.m_extraGates[targetId] = MatrixStore::x;

  LOG(Logger::DEBUG, "Applying a CX Gate:" << "\nCX Gate:\n\tcontrol: "
    << value.control.registerName << "[" << value.control.element
    << "]\n\ttarget: " << value.target.registerName << "["
    << value.target.element << "]\n" << std::endl);
}

void Simulator::StepVisitor::operator()(const Circuit::Measurement& value) {
  (void)(value);
  LOG(Logger::ERROR, "Measurement should not be present in the circuit anymore");
  assert(true == false);
}

void Simulator::StepVisitor::operator()(const Circuit::Barrier& __attribute__((unused)) value) {
  // Nothing to be done for a barrier. It is the same as an identity, computationaly-wise
  (void)value;
}

void Simulator::StepVisitor::operator()(const Circuit::Reset& value) {
  m_simulator.m_shouldNormalize = true;
  // Computing the offset of the target qubit
  Circuit::Qubit target = value.target;
  int id = m_simulator.m_qRegOffsets.find(target.registerName)->second;
  id += target.element;

  m_simulator.m_gates[id] = MatrixStore::pk0;
}
void Simulator::StepVisitor::operator()(const Circuit::ConditionalGate& cGate) {
  const auto value = m_simulator.m_measurementsTree.getCregValueAtNode(cGate.testedRegister, m_simulator.m_task.measurementNodeId);
  if (value == cGate.expectedValue) { cGate.gate.apply_visitor(*this); }
}

Matrix Simulator::simulate() {
  auto visitor = StepVisitor(*this);
  // Looping through each steps of the circuit.
  for (std::vector<Circuit::Step>::iterator it = m_task.circuit.steps.begin();
    it != m_task.circuit.steps.end(); ++it) {
      // LOG(Logger::DEBUG, "State before step:" << m_state);
      m_shouldNormalize = false;
      m_shouldAddSecondMatrix = false;
    // Initializing the gate vectors
    m_gates = std::vector<Matrix>(m_size, MatrixStore::i2);
    m_extraGates = std::vector<Matrix>(m_size, MatrixStore::i2);
    for (auto &substep: *it) {
      // Applying defined tranformations in the visitor.
      boost::apply_visitor(visitor, substep);
    }
    // Computing the new state as the dot product between the kroenecker product
    // of the transformation gates for each qubits and the actual simulator state.
    Matrix op = Matrix::kron(m_gates);
    if (m_shouldAddSecondMatrix) op = op + Matrix::kron(m_extraGates);
    // for (auto const &g : m_extraGates) {
    //   LOG(Logger::ERROR, g);
    // }
    // LOG(Logger::DEBUG, "Applying matrix:" << op);
    // LOG(Logger::DEBUG, "Submatrices:" << Matrix::kron(m_gates) << Matrix::kron(m_extraGates));
    m_state = op * m_state;

    if (m_shouldNormalize) m_state = m_state.normalize();
      // LOG(Logger::DEBUG, "State after step:" << m_state);
  }
  return m_state;
}