// This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

/* least_squares.cc
   Jeremy Barnes, 15 June 2003
   Copyright (c) 2003 Jeremy Barnes.  All rights reserved.

   Least squares solution of problems.  Various versions.
*/

#include "least_squares.h"

#include "mldb/plugins/jml/algebra/matrix_ops.h"
#include "mldb/base/exc_assert.h"
#include "svd.h"
#include <boost/timer.hpp>
#include "mldb/arch/timers.h"
#include "lapack.h"
#include <cmath>
#include "mldb/utils/string_functions.h"
#include "mldb/arch/simd_vector.h"
#include "mldb/base/parallel.h"

using namespace std;

namespace ML {

#if 0
template<class Float>
distribution<Float>
least_squares_impl(const boost::multi_array<Float, 2> & A,
                   const distribution<Float> & c,
                   const boost::multi_array<Float, 2> & B,
                   const distribution<Float> & d)
{
    using namespace LAPack;

    size_t m = A.shape()[0];
    size_t n = A.shape()[1];
    size_t p = B.shape()[0];

    //cerr << "A: (mxn) " << A.shape()[0] << " x " << A.shape()[1] << endl;
    //cerr << "B: (pxn) " << B.shape()[0] << " x " << B.shape()[1] << endl;
    //cerr << "c: m     " << c.size() << endl;
    //cerr << "d: p     " << d.size() << endl;

    if (c.size() != m || B.shape()[1] != n || d.size() != p)
        throw Exception("least_squares: sizes didn't match");

    if (p > n || n > (m + p))
        throw Exception("least_squares: overconstrained system");

    // todo: check that B has full row rank p and that the matrix (A B)' has
    // full column rank n.

    distribution<Float> result(n);

    /* We need to transpose them for Fortran, but since they are destroyed
       anyway it's no big deal since they would have been copied. */
    boost::multi_array<Float, 2> AF = fortran(A);
    boost::multi_array<Float, 2> BF = fortran(B);
    distribution<Float> c2 = c;
    distribution<Float> d2 = d;

    int res = gglse(m, n, p,
                    AF.data_begin(), AF.shape()[0],
                    BF.data_begin(), BF.shape()[1],
                    &c2[0], &d2[0], &result[0]);

    if (res != 0)
        throw Exception(format("least_squares(): gglse returned error in arg "
                               "%d", res));

    return result;
}

distribution<float>
least_squares(const boost::multi_array<float, 2> & A,
              const distribution<float> & c,
              const boost::multi_array<float, 2> & B,
              const distribution<float> & d)
{
    return least_squares_impl(A, c, B, d);
}

distribution<double>
least_squares(const boost::multi_array<double, 2> & A,
              const distribution<double> & c,
              const boost::multi_array<double, 2> & B,
              const distribution<double> & d)
{
    return least_squares_impl(A, c, B, d);
}
#endif

template<class Float>
distribution<Float>
least_squares_impl(const boost::multi_array<Float, 2> & A, const distribution<Float> & b)
{
    using namespace std;

    //boost::timer t;

    if (A.shape()[0] != b.size()) {
        cerr << "A.shape()[0] = " << A.shape()[0] << endl;
        cerr << "A.shape()[1] = " << A.shape()[1] << endl;
        cerr << "b.size() = " << b.size() << endl;
        throw Exception("incompatible dimensions for least_squares");
    }

    using namespace LAPack;

    int m = A.shape()[0];
    int n = A.shape()[1];

    distribution<Float> x = b;
    x.resize(std::max<size_t>(m, n));

    boost::multi_array<Float, 2> A2 = A;

#if 0
    using namespace std;
    cerr << "m = " << m << " n = " << n << " A2.shape()[0] = " << A2.shape()[0]
         << " A2.shape()[1] = " << A2.shape()[1] << endl;
    cerr << "A2 = " << endl << A2 << endl;
    cerr << "b = " << b << endl;
#endif
    int res = gels('T', n, m, 1, A2.data(), n, &x[0],
                   x.size());

    if (res < 0)
        throw Exception(format("least_squares(): gels returned error in arg "
                               "%d", -res));

    //if (debug_irls) {
    //    (*debug_irls) << "gels returned " << res << endl;
    //    (*debug_irls) << "x = " << x << endl;
    //}

    if (res > 0) {
        //if (debug_irls)
        //      (*debug_irls) << "retrying; " << res << " are too small" << endl;

        /* Rank-deficient matrix.  Use the more efficient routine. */
        int rank;
        Float rcond = -1.0;
        Float sv[std::min(m, n)];
        std::fill(sv, sv + std::min(m, n), 0.0);

        // Rebuild A2, transposed this time
        A2.resize(boost::extents[n][m]);
        A2 = transpose(A);

        // Rebuild x as it was previously overwritten
        x = b;
        x.resize(std::max<size_t>(m, n));

        res = gelsd(m, n, 1, A2.data(), m, &x[0], x.size(), sv, rcond, rank);

        //if (debug_irls)
        //    (*debug_irls) << "rcond: " << rcond << " rank: "
        //                  << rank << endl;
    }

    if (res < 0) {
        throw Exception(format("least_squares(): gelsy returned error in arg "
                               "%d", -res));
    }

    x.resize(n);

    //using namespace std;
    //cerr << "least_squares: took " << t.elapsed() << "s" << endl;

    return x;
    //cerr << "least squares: gels returned " << x2 << endl;
    //cerr << "least squares: A2 = " << endl << A2 << endl;

    //cerr << "least_squares: " << t.elapsed() << "s" << endl;
    //distribution<Float> x3
    //    = least_squares(A, b, boost::multi_array<Float, 2>(0, n), distribution<Float>());

    //cerr << "least squares: gglse returned " << x3 << endl;

}

distribution<float>
least_squares(const boost::multi_array<float, 2> & A,
              const distribution<float> & b)
{
    return least_squares_impl(A, b);
}

distribution<double>
least_squares(const boost::multi_array<double, 2> & A,
              const distribution<double> & b)
{
    return least_squares_impl(A, b);
}

template<typename Float>
void doDiagMultColumn(const boost::multi_array<Float, 2> & U,
                      const distribution<Float> & d,
                      const boost::multi_array<Float, 2> & V,
                      boost::multi_array<Float, 2> & result,
                      int j)
{
    size_t m = U.shape()[0], x = d.size();

    Float Vj_values[x];
    for (unsigned k = 0;  k < x;  ++k) {
        Vj_values[k] = V[k][j];
    }
    for (unsigned i = 0;  i < m;  ++i) {
        result[i][j] = MLDB::SIMD::vec_accum_prod3(&U[i][0], &d[0], Vj_values, x);
    }
}

template<class Float>
boost::multi_array<Float, 2>
diag_mult_impl(const boost::multi_array<Float, 2> & U,
               const distribution<Float> & d,
               const boost::multi_array<Float, 2> & V,
               bool parallel)
{
    size_t m = U.shape()[0], n = V.shape()[1], x = d.size();

    boost::multi_array<Float, 2> result(boost::extents[m][n]);

    if (U.shape()[1] != x || V.shape()[0] != x)
        throw Exception("diag_mult(): wrong shape");

    auto doColumn = std::bind(&doDiagMultColumn<Float>,
                              std::cref(U),
                              std::cref(d),
                              std::cref(V),
                              std::ref(result),
                              std::placeholders::_1);

    if (parallel)
        MLDB::parallelMap(0, n, doColumn);
    else {
        for (unsigned j = 0;  j < n;  ++j)
            doColumn(j);
    }

    return result;
}

boost::multi_array<float, 2>
diag_mult(const boost::multi_array<float, 2> & U,
          const distribution<float> & d,
          const boost::multi_array<float, 2> & V,
          bool parallel)
{
    return diag_mult_impl<float>(U, d, V, parallel);
}

boost::multi_array<double, 2>
diag_mult(const boost::multi_array<double, 2> & U,
          const distribution<double> & d,
          const boost::multi_array<double, 2> & V,
          bool parallel)
{
    return diag_mult_impl<double>(U, d, V, parallel);
}

template<typename Float>
struct RidgeRegressionIteration {
    double lambda;
    double current_lambda;
    double total_mse_unbiased;
    distribution<Float> x;

    void run(const distribution<Float> & singular_values,
             const boost::multi_array<Float, 2> & A,
             const distribution<Float> & b,
             const boost::multi_array<Float, 2> & VT,
             const boost::multi_array<Float, 2> & U,
             const boost::multi_array<Float, 2> & GK,
             bool debug)
    {
        int m = A.shape()[0];
        int n = A.shape()[1];

        Timer t(debug);

        auto doneStep = [&] (const std::string & where)
        {
            if (!debug)
                return;
            cerr << "      " << where << ": " << t.elapsed() << endl;
            t.restart();
        };

        //cerr << "i = " << i << " current_lambda = " << current_lambda << endl;
        // Adjust the singular values for the new lambda
        distribution<Float> my_singular = singular_values;
        if (current_lambda != lambda)
            my_singular += (current_lambda - lambda);

        //boost::multi_array<Float, 2> GK_pinv
        //    = U * diag((Float)1.0 / my_singular) * VT;

        boost::multi_array<Float, 2> GK_pinv
            = diag_mult(U, (Float)1.0 / my_singular, VT, true /* parallel */);

        doneStep("diag_mult");

        // TODO: reduce GK by removing those basis vectors where the singular
        // values are too close to lambda

        if (debug && false) {
            cerr << "GK_pinv = " << endl << GK_pinv
                 << endl;
            cerr << "prod = " << endl << (GK * GK_pinv * GK) << endl;
            cerr << "prod2 = " << endl << (GK_pinv * GK * GK) << endl;
        }

        boost::multi_array<Float, 2> A_pinv
            = (m < n ? GK_pinv * A : A * GK_pinv);

        doneStep("A_pinv");

        if (debug && false)
            cerr << "A_pinv = " << endl << A_pinv << endl;

        x = b * A_pinv;

        if (debug)
            cerr << "x = " << x << endl;

        distribution<Float> predictions = A * x;

        //cerr << "A: " << A.shape()[0] << "x" << A.shape()[1] << endl;
        //cerr << "A_pinv: " << A_pinv.shape()[0] << "x" << A_pinv.shape()[1]
        //     << endl;

        //boost::multi_array<Float, 2> A_A_pinv
        //    = A * transpose(A_pinv);

        doneStep("predictions");

#if 0
        boost::multi_array<Float, 2> A_A_pinv
        = multiply_transposed(A, A_pinv);

        cerr << "A_A_pinv: " << A_A_pinv.shape()[0] << "x"
        << A_A_pinv.shape()[1] << " m = " << m << endl;

        if (debug && false)
            cerr << "A_A_pinv = " << endl << A_A_pinv << endl;
#else
        // We only need the diagonal of A * A_pinv

        distribution<Float> A_A_pinv_diag(m);
        for (unsigned j = 0;  j < m;  ++j)
            A_A_pinv_diag[j] = SIMD::vec_dotprod_dp(&A[j][0], &A_pinv[j][0], n);
#endif

        doneStep("A_A_pinv");

        // Now figure out the performance
        double total_mse_biased = 0.0;
        for (unsigned j = 0;  j < m;  ++j) {

            if (j < 10 && false)
                cerr << "j = " << j << " b[j] = " << b[j]
                     << " predictions[j] = " << predictions[j]
                     << endl;

            double resid = b[j] - predictions[j];

            // Adjust for the bias cause by training on this example.  This is
            // A * pinv(A), which is A *

            double factor = 1.0 - A_A_pinv_diag[j];

            double resid_unbiased = resid / factor;

            total_mse_biased += (1.0 / m) * resid * resid;
            total_mse_unbiased += (1.0 / m) * resid_unbiased * resid_unbiased;
        }

        doneStep("mse");

        //cerr << "lambda " << current_lambda
        //     << " rmse_biased = " << sqrt(total_mse_biased)
        //     << " rmse_unbiased = " << sqrt(total_mse_unbiased)
        //     << endl;

        //if (sqrt(total_mse_biased) > 1.0) {
        //    cerr << "rmse_biased: x = " << x << endl;
        //}

#if 0
        cerr << "m = " << m << endl;
        cerr << "total_mse_biased   = " << total_mse_biased << endl;
        cerr << "total_mse_unbiased = " << total_mse_unbiased << endl;
        cerr << "best_error = " << best_error << endl;
        cerr << "x = " << x << endl;
#endif
    };

};

// NOTE: this cumbersome construction is to keep clang 3.4 from
// ICEing
template<typename Float>
struct RidgeRegressionIterations: public std::vector<RidgeRegressionIteration<Float> > {
    void run(const distribution<Float> & singular_values,
             const boost::multi_array<Float, 2> & A,
             const distribution<Float> & b,
             const boost::multi_array<Float, 2> & VT,
             const boost::multi_array<Float, 2> & U,
             const boost::multi_array<Float, 2> & GK,
             bool debug,
             int n)
    {
        this->at(n).run(singular_values, A, b, VT, U, GK, debug);
    }
};

template<class Float>
distribution<Float>
ridge_regression_impl(const boost::multi_array<Float, 2> & A,
                      const distribution<Float> & b,
                      float& lambda)
{
    using namespace std;
    float initialLambda = lambda < 0 ? 1e-5 : lambda;
    //cerr << "ridge_regression: A = " << A.shape()[0] << "x" << A.shape()[1]
    //     << " b = " << b.size() << endl;

    //cerr << "b = " << b << endl;
    //cerr << "A = " << A << endl;

    bool debug = false;
    //debug = true;

    Timer t(debug);

    auto doneStep = [&] (const std::string & where)
        {
            if (!debug)
                return;
            cerr << where << ": " << t.elapsed() << endl;
            t.restart();
        };

    // Step 1: SVD

    if (A.shape()[0] != b.size())
        throw Exception("incompatible dimensions for least_squares");

    using namespace LAPack;

    int m = A.shape()[0];
    int n = A.shape()[1];

    int minmn = std::min(m, n);

    // See http://www.clopinet.com/isabelle/Projects/ETH/KernelRidge.pdf

    // The matrix to decompose is square
    boost::multi_array<Float, 2> GK(boost::extents[minmn][minmn]);

    //cerr << "m = " << m << " n = " << n << endl;

    // Take either A * transpose(A) or (A transpose) * A, whichever is smaller
    if (m < n) {
        for (unsigned i1 = 0;  i1 < m;  ++i1)
            for (unsigned i2 = 0;  i2 < m;  ++i2)
                GK[i1][i2] = SIMD::vec_dotprod_dp(&A[i1][0], &A[i2][0], n);

        //for (unsigned i1 = 0;  i1 < m;  ++i1)
        //    for (unsigned i2 = 0;  i2 < m;  ++i2)
        //        for (unsigned j = 0;  j < n;  ++j)
        //            GK[i1][i2] += A[i1][j] * A[i2][j];
    } else {
        // TODO: vectorize and look at loop order
        for (unsigned i = 0;  i < m;  ++i)
            for (unsigned j1 = 0;  j1 < n;  ++j1)
                for (unsigned j2 = 0;  j2 < n;  ++j2)
                    GK[j1][j2] += A[i][j1] * A[i][j2];
    }

    doneStep("    square");

    if (debug)
        cerr << "GK = " << endl << GK << endl;

    //cerr << "GK.shape()[0] = " << GK.shape()[0] << endl;
    //cerr << "GK.shape()[1] = " << GK.shape()[1] << endl;

    // Add in the ridge
    for (unsigned i = 0;  i < minmn;  ++i)
        GK[i][i] += initialLambda;

    if (debug)
        cerr << "GK with ridge = " << endl << GK << endl;

    // Decompose to get the pseudoinverse
    distribution<Float> svalues(minmn);
    boost::multi_array<Float, 2> VT(boost::extents[minmn][minmn]);
    boost::multi_array<Float, 2> U(boost::extents[minmn][minmn]);

    svd_square(GK, VT, U, svalues);

    distribution<Float> singular_values
        (svalues.begin(), svalues.begin() + minmn);

    if (debug)
        cerr << "singular values = " << singular_values << endl;

    if (debug) {
        // Multiply decomposition back to make sure that we get the original
        // matrix
        boost::multi_array<Float, 2> D = diag(singular_values);

        boost::multi_array<Float, 2> GK_test
            = U * D * VT;

        cerr << "GK_test = " << endl << GK_test << endl;
        //cerr << "errors = " << endl << (GK_test - GK) << endl;
    }

    // Figure out the optimal value of lambda based upon leave-one-out cross
    // validation
    distribution<Float> x_best;
    Float best_lambda = initialLambda;
    typedef RidgeRegressionIteration<Float> Iteration;
    if (lambda < 0) {
        double current_lambda = 10.0;

        RidgeRegressionIterations<Float> iterations;

        for (; current_lambda >= 1e-14; current_lambda /= 10.0) {
            Iteration iter;
            iter.lambda = initialLambda;
            iter.current_lambda = current_lambda;
            iterations.push_back(iter);
        };

        MLDB::parallelMap(0, iterations.size(),
                        std::bind(std::mem_fn(&RidgeRegressionIterations<Float>::run),
                                  std::ref(iterations),
                                  std::cref(singular_values),
                                  std::cref(A), std::cref(b),
                                  std::cref(VT), std::cref(U),
                                  std::cref(GK), debug,
                                  std::placeholders::_1));

        //double best_lambda = -1000;
        double best_error = 1000000;

        for (unsigned i = 0;  i < iterations.size();  ++i) {

            if (iterations[i].total_mse_unbiased < best_error || i == 0) {

                //cerr << "best_lambda " << iterations[i].current_lambda << " error : " << iterations[i].total_mse_unbiased << endl;

                x_best = iterations[i].x;
                best_lambda = iterations[i].current_lambda;
                best_error = iterations[i].total_mse_unbiased;
            }
        }
    }
    else {
        Iteration iter;
        iter.lambda = initialLambda;
        iter.current_lambda = initialLambda;
        iter.run(singular_values, A, b, VT, U, GK, debug);
        x_best = iter.x;
    }

    doneStep("    lambda");

    //cerr << "total: " << t.elapsed() << endl;

    lambda = best_lambda; //return the lambda we ended up using
    return x_best;
}

distribution<float>
ridge_regression(const boost::multi_array<float, 2> & A,
                 const distribution<float> & b,
                 float lambda)
{
    return ridge_regression_impl(A, b, lambda);
}

distribution<double>
ridge_regression(const boost::multi_array<double, 2> & A,
                 const distribution<double> & b,
                 float lambda)
{
    return ridge_regression_impl(A, b, lambda);
}

template<class Float>
distribution<Float>
lasso_regression_impl(const boost::multi_array<Float, 2> & A,
                      const distribution<Float> & b,
                      float lambda,
                      int maxIter,
                      float epsilon)
{
    // cerr << "lasso_regression: A = " << A.shape()[0] << "x" << A.shape()[1]
    //     << " b = " << b.size() << " lambda = " << lambda <<" maxIter = "
    //     << maxIter << " epsilon = " << epsilon << endl;

    //  ref: https://www.coursera.org/learn/ml-regression/lecture/AsCvQ/coordinate-descent-for-lasso-unnormalized-features
    //
    //  on why standardisation might be required before using LASSO:
    //  http://stats.stackexchange.com/q/86434/22296

    int n = A.shape()[0];   //Number of samples
    int p = A.shape()[1];   //Number of variables

     distribution<Float> x(p, 0.); //our solution vector

    if (lambda <= 0) {
        x = ridge_regression_impl(A, b, lambda); //Use the ridge regression to determine lambda and use the result as initialization
    }

    Float halflambda = lambda / 2.0f;
    distribution<Float> Atb(p, 0.);                        //Correlation of each variable with the target vector
    boost::multi_array<Float, 2> AtA(boost::extents[p][p]); //Correlation betwen each variables

    //Precompute Atb and AtA
    for (int j = 0; j < p; ++j) {
        for (int i = 0; i < n; ++i) {
            Atb[j] += A[i][j] * b[i];
        }
        //each column dot each column
        for (int i = 0; i < p; ++i) { //todo: optimise for triangular matrix
            Float dotprod = 0.;
            for (int r = 0; r < n; ++r) {
                dotprod += A[r][i] * A[r][j];
            }
            AtA[j][i] = dotprod;
        }
    }

    //Main lasso loop, which is really a coordinate descent where we optimize each variable independently.
    //We do this in a roundrobin fashion.
    int iter = 0;
    do {
        Float max_step = 0.;
        distribution<Float> oldX(p);
        oldX = x;

        for (int j = 0; j < p; ++j) { //for each column / variable

            Float rho = Atb[j];

            for (int i = 0; i < p; ++i){    //scales with the number of variables
                if (i != j)
                    rho -= AtA[j][i] * x[i];
            }

            if (rho > halflambda) {
                x[j] = (rho - halflambda) / (AtA[j][j]);
            }
            else if (rho < -halflambda) {
                x[j] = (rho + halflambda) / (AtA[j][j]);
            }
            else {
                x[j] = 0;
            }

	        //cerr << "x[j]: " << x[j] << endl;
            Float step = fabs(x[j] - oldX[j]);
            if (step > max_step)
                max_step = step;
        }

        // cerr << "max_step: " << max_step << endl;

        // if the biggest step we took was smaller than our threshold, let's
        // stop there and assume we have converged
        if (max_step < epsilon) {
            break;
        }
        if (iter >= maxIter) {
            // if we stopped after max iteration and not because we have
            // converge, let's issue a warning
            cerr << MLDB::format("LASSO did not converge in %i iterations, last "
                               " max_step > eps (%f > %f)",
                               iter, max_step, epsilon);
            break;
        }
        iter++;
    } while (true); //until convergence

    return x;
}

distribution<float>
lasso_regression(const boost::multi_array<float, 2> & A,
                 const distribution<float> & b,
                 float lambda,
                 int maxIter,
                 float epsilon)
{
    return lasso_regression_impl(A, b, lambda, maxIter, epsilon);
}

distribution<double>
lasso_regression(const boost::multi_array<double, 2> & A,
                 const distribution<double> & b,
                 float lambda,
                 int maxIter,
                 float epsilon)
{
    return lasso_regression_impl(A, b, lambda, maxIter, epsilon);
}

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

template<class Float>
void doWeightedSquareRow(const boost::multi_array<Float, 2> & XT,
                         const distribution<Float> & d,
                         boost::multi_array<Float, 2> & result,
                         int i)
{
    int chunk_size = 2048;  // ensure we fit in the cache

    size_t nx = XT.shape()[1];
    size_t nv = XT.shape()[0];

    int x = 0;
    while (x < nx) {
        int nxc = std::min<size_t>(chunk_size, nx - x);
        distribution<Float> Xid(chunk_size);
        SIMD::vec_prod(&XT[i][x], &d[x], &Xid[0], nxc);

        for (unsigned j = 0;  j < nv;  ++j) {
            result[i][j] += SIMD::vec_dotprod_dp(&XT[j][x], &Xid[0], nxc);
        }

        x += nxc;
    }

}

template<class Float>
boost::multi_array<Float, 2>
weighted_square_impl(const boost::multi_array<Float, 2> & XT,
                     const distribution<Float> & d)
{
    if (XT.shape()[1] != d.size())
        throw Exception("Incompatible matrix sizes for weighted_square");

    size_t nx = XT.shape()[1];
    size_t nv = XT.shape()[0];

    //cerr << "nx = " << nx << " nv = " << nv << endl;

    boost::multi_array<Float, 2> result(boost::extents[nv][nv]);

    if (false) {
        int chunk_size = 2048;  // ensure we fit in the cache
        distribution<Float> Xid(chunk_size);

        int x = 0;
        while (x < nx) {
            int nxc = std::min<size_t>(chunk_size, nx - x);

            for (unsigned i = 0;  i < nv;  ++i) {
                SIMD::vec_prod(&XT[i][x], &d[x], &Xid[0], nxc);

                for (unsigned j = 0;  j < nv;  ++j) {
                    result[i][j] += SIMD::vec_dotprod_dp(&XT[j][x], &Xid[0], nxc);
                }
            }

            x += nxc;
        }
    } else {
        MLDB::parallelMap(0, nv, std::bind(doWeightedSquareRow<Float>,
                                                 std::cref(XT),
                                                 std::cref(d),
                                                 std::ref(result),
                                                 std::placeholders::_1));
    }

    return result;
}

boost::multi_array<float, 2>
weighted_square(const boost::multi_array<float, 2> & XT,
                const distribution<float> & d)
{
    return weighted_square_impl(XT, d);
}

boost::multi_array<double, 2>
weighted_square(const boost::multi_array<double, 2> & XT,
                const distribution<double> & d)
{
    return weighted_square_impl(XT, d);
}

template<typename Float>
void svd_square_impl(boost::multi_array<Float, 2> & X,
                     boost::multi_array<Float, 2> & VT,
                     boost::multi_array<Float, 2> & U,
                     distribution<Float> & svalues)
{
    size_t minmn = X.shape()[0];
    ExcAssertEqual(minmn, X.shape()[1]);

    // Decompose to get the pseudoinverse
    svalues.clear();
    svalues.resize(minmn);

    VT.resize(boost::extents[minmn][minmn]);
    U.resize(boost::extents[minmn][minmn]);

    // SVD
    int result = LAPack::gesdd("S", minmn, minmn,
                               X.data(), minmn,
                               &svalues[0],
                               &VT[0][0], minmn,
                               &U[0][0], minmn);

    if (result != 0)
        throw Exception("gesdd returned non-zero");
}

void svd_square(boost::multi_array<float, 2> & X,
                boost::multi_array<float, 2> & VT,
                boost::multi_array<float, 2> & U,
                distribution<float> & svalues)
{
    return svd_square_impl(X, VT, U, svalues);
}

void svd_square(boost::multi_array<double, 2> & X,
                boost::multi_array<double, 2> & VT,
                boost::multi_array<double, 2> & U,
                distribution<double> & svalues)
{
    return svd_square_impl(X, VT, U, svalues);
}

} // namespace ML


// All content Copyright (C) 2018 Genomics plc
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "caller/diploid/genotypeUtils.hpp"
#include "utils/indexedProduct.hpp"

namespace wecall
{
namespace caller
{
    namespace model
    {
        std::vector< double > computeGenotypeLikelihoods( const variant::GenotypeVector & genotypes,
                                                          const utils::matrix_t & probReadsGivenHaplotypes,
                                                          const variant::HaplotypeVector & haplotypes )
        {
            std::vector< double > genotypeLikelihoods( genotypes.size() );
            const auto nReads = probReadsGivenHaplotypes.size1();
            WECALL_ASSERT( nReads > 0, "Only makes sense to compute likelihoods if there is read-data" );

            for ( std::size_t genotypeIndex = 0; genotypeIndex < genotypes.size(); ++genotypeIndex )
            {
                double thisGenotypeLogLikelihood = 0.0;
                const auto hapIndicies = genotypes.getHaplotypeIndices( genotypeIndex );

                // prior probability of any haplotype (given genotype).  Assumes uniform distribution across haplotypes
                const auto haplotypePriorProbability = 1.0 / static_cast< double >( hapIndicies.size() );

                for ( std::size_t readIndex = 0; readIndex < nReads; ++readIndex )
                {
                    double sumReadLikelihoodsOverHaplotypes = 0.0;
                    for ( const auto & hapIndex : hapIndicies )
                    {
                        sumReadLikelihoodsOverHaplotypes += probReadsGivenHaplotypes( readIndex, hapIndex );
                    }
                    thisGenotypeLogLikelihood +=
                        std::log( sumReadLikelihoodsOverHaplotypes * haplotypePriorProbability );
                }

                genotypeLikelihoods[genotypeIndex] = thisGenotypeLogLikelihood;
            }

            // Now re-scale all the genotype likelihoods for this individual, to prevent underflows, and
            rescaleLogLikelihoods( genotypeLikelihoods, nReads > 0 );

            // store the likelihoods as non-log values
            std::transform( genotypeLikelihoods.begin(), genotypeLikelihoods.end(), genotypeLikelihoods.begin(), exp );

            if ( false )
            {
                // print likelihoods of genotypes
                WECALL_LOG( SUPER_DEBUG, "Genotype likelihoods:-" );
                for ( std::size_t genotypeIndex = 0; genotypeIndex < genotypes.size(); ++genotypeIndex )
                {
                    if ( genotypeLikelihoods[genotypeIndex] > 1e-6 )
                    {
                        WECALL_LOG( SUPER_DEBUG, std::to_string( genotypeLikelihoods[genotypeIndex] ) + " for " +
                                                      genotypes[genotypeIndex]->toString( haplotypes ) );
                    }
                }
            }

            return genotypeLikelihoods;
        }
    }
}
}


//  Copyright (c) 2006 Xiaogang Zhang
//  Copyright (c) 2006 John Maddock
//  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)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to correctly
//  handle the various corner cases.
//

#ifndef BOOST_MATH_ELLINT_3_HPP
#define BOOST_MATH_ELLINT_3_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/ellint_rf.hpp>
#include <boost/math/special_functions/ellint_rj.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/atanh.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/workaround.hpp>
#include <boost/math/special_functions/round.hpp>

// Elliptic integrals (complete and incomplete) of the third kind
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math {

namespace detail{

template <typename T, typename Policy>
T ellint_pi_imp(T v, T k, T vc, const Policy& pol);

// Elliptic integral (Legendre form) of the third kind
template <typename T, typename Policy>
T ellint_pi_imp(T v, T phi, T k, T vc, const Policy& pol)
{
   // Note vc = 1-v presumably without cancellation error.
   BOOST_MATH_STD_USING

   static const char* function = "boost::math::ellint_3<%1%>(%1%,%1%,%1%)";

   T sphi = sin(fabs(phi));
   T result = 0;

   if (k * k * sphi * sphi > 1)
   {
      return policies::raise_domain_error<T>(function,
         "Got k = %1%, function requires |k| <= 1", k, pol);
   }
   // Special cases first:
   if(v == 0)
   {
      // A&S 17.7.18 & 19
      return (k == 0) ? phi : ellint_f_imp(phi, k, pol);
   }
   if((v > 0) && (1 / v < (sphi * sphi)))
   {
      // Complex result is a domain error:
      return policies::raise_domain_error<T>(function,
         "Got v = %1%, but result is complex for v > 1 / sin^2(phi)", v, pol);
   }

   if(v == 1)
   {
      if (k == 0)
         return tan(phi);

      // http://functions.wolfram.com/08.06.03.0008.01
      T m = k * k;
      result = sqrt(1 - m * sphi * sphi) * tan(phi) - ellint_e_imp(phi, k, pol);
      result /= 1 - m;
      result += ellint_f_imp(phi, k, pol);
      return result;
   }
   if(phi == constants::half_pi<T>())
   {
      // Have to filter this case out before the next
      // special case, otherwise we might get an infinity from
      // tan(phi).
      // Also note that since we can't represent PI/2 exactly
      // in a T, this is a bit of a guess as to the users true
      // intent...
      //
      return ellint_pi_imp(v, k, vc, pol);
   }
   if((phi > constants::half_pi<T>()) || (phi < 0))
   {
      // Carlson's algorithm works only for |phi| <= pi/2,
      // use the integrand's periodicity to normalize phi
      //
      // Xiaogang's original code used a cast to long long here
      // but that fails if T has more digits than a long long,
      // so rewritten to use fmod instead:
      //
      // See http://functions.wolfram.com/08.06.16.0002.01
      //
      if(fabs(phi) > 1 / tools::epsilon<T>())
      {
         if(v > 1)
            return policies::raise_domain_error<T>(
            function,
            "Got v = %1%, but this is only supported for 0 <= phi <= pi/2", v, pol);
         //
         // Phi is so large that phi%pi is necessarily zero (or garbage),
         // just return the second part of the duplication formula:
         //
         result = 2 * fabs(phi) * ellint_pi_imp(v, k, vc, pol) / constants::pi<T>();
      }
      else
      {
         T rphi = boost::math::tools::fmod_workaround(T(fabs(phi)), T(constants::half_pi<T>()));
         T m = boost::math::round((fabs(phi) - rphi) / constants::half_pi<T>());
         int sign = 1;
         if((m != 0) && (k >= 1))
         {
            return policies::raise_domain_error<T>(function, "Got k=1 and phi=%1% but the result is complex in that domain", phi, pol);
         }
         if(boost::math::tools::fmod_workaround(m, T(2)) > 0.5)
         {
            m += 1;
            sign = -1;
            rphi = constants::half_pi<T>() - rphi;
         }
         result = sign * ellint_pi_imp(v, rphi, k, vc, pol);
         if((m > 0) && (vc > 0))
            result += m * ellint_pi_imp(v, k, vc, pol);
      }
      return phi < 0 ? T(-result) : result;
   }
   if(k == 0)
   {
      // A&S 17.7.20:
      if(v < 1)
      {
         T vcr = sqrt(vc);
         return atan(vcr * tan(phi)) / vcr;
      }
      else
      {
         // v > 1:
         T vcr = sqrt(-vc);
         T arg = vcr * tan(phi);
         return (boost::math::log1p(arg, pol) - boost::math::log1p(-arg, pol)) / (2 * vcr);
      }
   }
   if((v < 0) && fabs(k) <= 1)
   {
      //
      // If we don't shift to 0 <= v <= 1 we get
      // cancellation errors later on.  Use
      // A&S 17.7.15/16 to shift to v > 0.
      //
      // Mathematica simplifies the expressions
      // given in A&S as follows (with thanks to
      // Rocco Romeo for figuring these out!):
      //
      // V = (k2 - n)/(1 - n)
      // Assuming[(k2 >= 0 && k2 <= 1) && n < 0, FullSimplify[Sqrt[(1 - V)*(1 - k2 / V)] / Sqrt[((1 - n)*(1 - k2 / n))]]]
      // Result: ((-1 + k2) n) / ((-1 + n) (-k2 + n))
      //
      // Assuming[(k2 >= 0 && k2 <= 1) && n < 0, FullSimplify[k2 / (Sqrt[-n*(k2 - n) / (1 - n)] * Sqrt[(1 - n)*(1 - k2 / n)])]]
      // Result : k2 / (k2 - n)
      //
      // Assuming[(k2 >= 0 && k2 <= 1) && n < 0, FullSimplify[Sqrt[1 / ((1 - n)*(1 - k2 / n))]]]
      // Result : Sqrt[n / ((k2 - n) (-1 + n))]
      //
      T k2 = k * k;
      T N = (k2 - v) / (1 - v);
      T Nm1 = (1 - k2) / (1 - v);
      T p2 = -v * N;
      T t;
      if(p2 <= tools::min_value<T>())
         p2 = sqrt(-v) * sqrt(N);
      else
         p2 = sqrt(p2);
      T delta = sqrt(1 - k2 * sphi * sphi);
      if(N > k2)
      {
         result = ellint_pi_imp(N, phi, k, Nm1, pol);
         result *= v / (v - 1);
         result *= (k2 - 1) / (v - k2);
      }

      if(k != 0)
      {
         t = ellint_f_imp(phi, k, pol);
         t *= k2 / (k2 - v);
         result += t;
      }
      t = v / ((k2 - v) * (v - 1));
      if(t > tools::min_value<T>())
      {
         result += atan((p2 / 2) * sin(2 * phi) / delta) * sqrt(t);
      }
      else
      {
         result += atan((p2 / 2) * sin(2 * phi) / delta) * sqrt(fabs(1 / (k2 - v))) * sqrt(fabs(v / (v - 1)));
      }
      return result;
   }
   if(k == 1)
   {
      // See http://functions.wolfram.com/08.06.03.0013.01
      result = sqrt(v) * atanh(sqrt(v) * sin(phi)) - log(1 / cos(phi) + tan(phi));
      result /= v - 1;
      return result;
   }
#if 0  // disabled but retained for future reference: see below.
   if(v > 1)
   {
      //
      // If v > 1 we can use the identity in A&S 17.7.7/8
      // to shift to 0 <= v <= 1.  In contrast to previous
      // revisions of this header, this identity does now work
      // but appears not to produce better error rates in
      // practice.  Archived here for future reference...
      //
      T k2 = k * k;
      T N = k2 / v;
      T Nm1 = (v - k2) / v;
      T p1 = sqrt((-vc) * (1 - k2 / v));
      T delta = sqrt(1 - k2 * sphi * sphi);
      //
      // These next two terms have a large amount of cancellation
      // so it's not clear if this relation is useable even if
      // the issues with phi > pi/2 can be fixed:
      //
      result = -ellint_pi_imp(N, phi, k, Nm1, pol);
      result += ellint_f_imp(phi, k, pol);
      //
      // This log term gives the complex result when
      //     n > 1/sin^2(phi)
      // However that case is dealt with as an error above,
      // so we should always get a real result here:
      //
      result += log((delta + p1 * tan(phi)) / (delta - p1 * tan(phi))) / (2 * p1);
      return result;
   }
#endif
   //
   // Carlson's algorithm works only for |phi| <= pi/2,
   // by the time we get here phi should already have been
   // normalised above.
   //
   BOOST_ASSERT(fabs(phi) < constants::half_pi<T>());
   BOOST_ASSERT(phi >= 0);
   T x, y, z, p, t;
   T cosp = cos(phi);
   x = cosp * cosp;
   t = sphi * sphi;
   y = 1 - k * k * t;
   z = 1;
   if(v * t < 0.5)
      p = 1 - v * t;
   else
      p = x + vc * t;
   result = sphi * (ellint_rf_imp(x, y, z, pol) + v * t * ellint_rj_imp(x, y, z, p, pol) / 3);

   return result;
}

// Complete elliptic integral (Legendre form) of the third kind
template <typename T, typename Policy>
T ellint_pi_imp(T v, T k, T vc, const Policy& pol)
{
    // Note arg vc = 1-v, possibly without cancellation errors
    BOOST_MATH_STD_USING
    using namespace boost::math::tools;

    static const char* function = "boost::math::ellint_pi<%1%>(%1%,%1%)";

    if (abs(k) >= 1)
    {
       return policies::raise_domain_error<T>(function,
            "Got k = %1%, function requires |k| <= 1", k, pol);
    }
    if(vc <= 0)
    {
       // Result is complex:
       return policies::raise_domain_error<T>(function,
            "Got v = %1%, function requires v < 1", v, pol);
    }

    if(v == 0)
    {
       return (k == 0) ? boost::math::constants::pi<T>() / 2 : ellint_k_imp(k, pol);
    }

    if(v < 0)
    {
       // Apply A&S 17.7.17:
       T k2 = k * k;
       T N = (k2 - v) / (1 - v);
       T Nm1 = (1 - k2) / (1 - v);
       T result = 0;
       result = boost::math::detail::ellint_pi_imp(N, k, Nm1, pol);
       // This next part is split in two to avoid spurious over/underflow:
       result *= -v / (1 - v);
       result *= (1 - k2) / (k2 - v);
       result += ellint_k_imp(k, pol) * k2 / (k2 - v);
       return result;
    }

    T x = 0;
    T y = 1 - k * k;
    T z = 1;
    T p = vc;
    T value = ellint_rf_imp(x, y, z, pol) + v * ellint_rj_imp(x, y, z, p, pol) / 3;

    return value;
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type ellint_3(T1 k, T2 v, T3 phi, const boost::false_type&)
{
   return boost::math::ellint_3(k, v, phi, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type ellint_3(T1 k, T2 v, const Policy& pol, const boost::true_type&)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_pi_imp(
         static_cast<value_type>(v),
         static_cast<value_type>(k),
         static_cast<value_type>(1-v),
         pol), "boost::math::ellint_3<%1%>(%1%,%1%)");
}

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type ellint_3(T1 k, T2 v, T3 phi, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_pi_imp(
         static_cast<value_type>(v),
         static_cast<value_type>(phi),
         static_cast<value_type>(k),
         static_cast<value_type>(1-v),
         pol), "boost::math::ellint_3<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
typename detail::ellint_3_result<T1, T2, T3>::type ellint_3(T1 k, T2 v, T3 phi)
{
   typedef typename policies::is_policy<T3>::type tag_type;
   return detail::ellint_3(k, v, phi, tag_type());
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type ellint_3(T1 k, T2 v)
{
   return ellint_3(k, v, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_3_HPP


/*****************************************************************************
 *
 * ALPS DMFT Project
 *
 * Copyright (C) 2005 - 2009 by Emanuel Gull <gull@phys.columbia.edu>
 *                              Philipp Werner <werner@itp.phys.ethz.ch>,
 *                              Sebastian Fuchs <fuchs@theorie.physik.uni-goettingen.de>
 *                              Matthias Troyer <troyer@comp-phys.org>
 *
 *
 * This software is part of the ALPS Applications, published under the ALPS
 * Application License; you can use, redistribute it and/or modify it under
 * the terms of the license, either version 1 or (at your option) any later
 * version.
 *
 * You should have received a copy of the ALPS Application License along with
 * the ALPS Applications; see the file LICENSE.txt. If not, the license is also
 * available from http://alps.comp-phys.org/.
 *
 * 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
 * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 *
 *****************************************************************************/

#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/bindings/lapack/driver/gesv.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <math.h>
#include "fouriertransform.h"

using namespace Eigen;

typedef boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major> dense_matrix;

FourierTransformer::FourierTransformer(const double beta,
                                       const VectorXd &ftail)
    : beta_(beta), c_(ftail) {
  if (c_.size() < 3)
    throw std::length_error("The tail should contains up to at least the third order");
}

// backward_ft G(iwn)->G(tau) works for cluster Green's functions
void FourierTransformer::backward_ft(const VectorXcd &G_omega,
                                     VectorXd &G_tau, const int N_tau) const {
  unsigned int N_omega = G_omega.size();
  VectorXcd G_omega_no_model(G_omega);
  double dt = beta_/N_tau;

  G_tau.resize(N_tau+1);
  if (c_(0) == 0 && c_(1) == 0 && c_(2) == 0) {
    //nothing happening in this gf.
    for (int i = 0; i <= N_tau; i++) {
      G_tau(i)=0.;
    }
  } else {
    for (int k = 0; k < N_omega; k++) {
      std::complex<double> iw(0,(2*k+1)*M_PI/beta_);
      G_omega_no_model(k) -= f_omega(iw, c_(0),c_(1), c_(2));
    }
    for (int i=0; i<N_tau; i++) {
      G_tau(i) = f_tau(i*dt, beta_, c_(0), c_(1), c_(2));
      for (int k=0; k<N_omega; k++) {
        double wt((2*k+1)*i*M_PI/N_tau);
        G_tau(i) += 2/beta_*(cos(wt)*G_omega_no_model(k).real()+
                             sin(wt)*G_omega_no_model(k).imag());
      }
    }
    G_tau(N_tau) = -c_[0];
    G_tau(N_tau)-= G_tau(0);
  }
}

void generate_spline_matrix(dense_matrix & spline_matrix, double dt) {
  // spline_matrix has dimension (N+1)x(N+1)
  int Np1 = spline_matrix.size1();
  //std::cout<<"spline matrix size is: "<<Np1<<std::endl;
  // A is the matrix whose inverse defines spline_matrix
  //
  //      6                   6
  //      1  4  1
  //         1  4  1
  // A =        ...
  //
  //                    1  4  1
  //     -2 -1     0       1  2
  spline_matrix.clear();
  dense_matrix A = 4*dt/6.*boost::numeric::ublas::identity_matrix<double>(Np1);

  for (int i=1; i<Np1-1; i++) {
    A(i,i-1) = dt/6.;
    A(i,i+1) = dt/6.;
  }
  A(0,0) = 1.;
  A(0, Np1-1) = 1.;
  A(Np1-1, 0) = -2.*dt/6.;
  A(Np1-1, 1) = -1.*dt/6.;
  A(Np1-1, Np1-2) = 1*dt/6.;
  A(Np1-1, Np1-1) = 2*dt/6.;

  // solve A*spline_matrix=I
  // gesv solves A*X=B, input for B is I, output (=solution X) is spline_matrix
  spline_matrix = boost::numeric::ublas::identity_matrix<double>(Np1);
  boost::numeric::ublas::vector<fortran_int_t> ipivot(A.size1());
  boost::numeric::bindings::lapack::gesv(A, ipivot,spline_matrix);
}

void evaluate_second_derivatives(double dt, dense_matrix & spline_matrix,
    std::vector<double> & g, std::vector<double> & second_derivatives,
    const double c1g, const double c2g, const double c3g) {
  // g, rhs and second_derivatives have dimension N+1
  int Np1 = spline_matrix.size1();
  //assert(c1g==1);
  // rhs is the vector containing the data of the curve y = g(tau), which allows to
  // compute the vector of second derivatives y'' at times tau_n by evaluating
  // y'' = spline_matrix * rhs(y)
  //
  //                         0
  //                         y0 - 2*y1 + y2
  //                         y1 - 2*y2 + y3
  // rhs = 6/(delta_tau)^2 * ...
  //
  //                         yNp1-3 - 2*yNp1-2 + yNp1-1
  //                         y0 - y1 + yNp1-2 - yNp1-1

  std::vector<double> rhs(Np1, 0);
  std::cout<<"constants: "<<c1g<<" "<<c2g<<" "<<c3g<<std::endl;
  rhs[0] = -c3g; //G''(0)+G''(beta)=-c3
  for (int i=1; i<Np1-1; i++) {
    rhs[i] = (g[i-1]-2*g[i]+g[i+1])/dt;
  }
  rhs[Np1-1] = c2g -1./dt*(-g[0] + g[1] -g[Np1-2] + g[Np1-1]);

  for (int i=0; i<Np1; i++) {
    second_derivatives[i]=0;
    for (int j=0; j<Np1; j++) {
      second_derivatives[i] += spline_matrix(i,j)*rhs[j];
    }
  }
}

//
// calculations following Armin Comadac's thesis (see the thesis for details)
// S1 = G'(0) + G'(beta) = y'(0) + y'(L)
// S2 = G''(0) + G''(beta) = y''(0) + y''(L)
// remember that S1 and S2 can be different from the Green's function's tail
// in details: S1 = G2
//             S2 = -G3
//
VectorXd ArminGetSecondDerivative(const VectorXd &y,
    const double beta, const double S1, const double S2) {
  const int L = y.size()-1;
  double dx = beta/L;

  VectorXd X(L+1), Y(L), Z(L);
  // coefficients for L, U matrices (Armin's notations)
  VectorXd a(L), b(L), d(L);
  Y.setZero();
  Z.setZero();
  Y(0) = 6. * ((y[1]-y[0]+y[L]-y[L-1]-S1*dx) / (dx*dx) + S2/3.);
  Z(0) = Y(0);
  a(0) = 4.;
  b(0) = -1./a(0);
  d(0) = -1.;
  for (int i = 1; i < L-1; ++i) {
    a(i) = 4. - 1./a(i-1);
    b(i) = -b(i-1)/a(i);
    d(i) = -d(i-1)/a(i-1);
    Y(i) = 6. * (y(i+1) - 2*y(i) + y(i-1)) / (dx*dx);
    Z(i) = Y(i) - Z(i-1)/a(i-1);
  }
  Y(L-1) = 6. * ((y(L) - 2*y(L-1) + y(L-2)) / (dx*dx) - S2 / 6.);
  a(L-1) = 4.;
  b(L-2) = (1.-b(L-3))/a(L-2);
  d(L-2) = 1. - d(L-3)/a(L-3);
  for (int i = 0; i < L-1; ++i)
    a(L-1) -= b(i)*d(i);
  Z(L-1) = Y(L-1);
  for (int i = 0; i < L-1; ++i)
    Z(L-1) -= b(i)*Z(i);

  X(L-1) = Z(L-1) / a(L-1);
  X(L-2) = (Z(L-2)-d(L-2)*X(L-1)) / a(L-2);
  for (int i = L-3; i >= 0; --i)
    X(i) = (Z(i) - X(i+1) - d(i)*X(L-1))/a(i);
  X(L) = S2 - X(0);

  return X;
}

// forward_ft G(tau)->G(iwn) works for cluster Green's functions
void FourierTransformer::Armin_forward_ft(const VectorXd &gtau, VectorXcd &gomega,
                                          const int num_matsubara) const {
  int N = gtau.size()-1;
  double dt = beta_/N;
  VectorXcd v_omega(num_matsubara);
  gomega.resize(num_matsubara);
  VectorXd v2(ArminGetSecondDerivative(gtau, beta_, c_(1), -c_(2)));

  // DEBUG
//  const VectorXd &v(gtau);
//  std::cout<<"c1 is: "<<c_(0)<<" computed: "<<-v[0]-v[N]<<std::endl;
//  std::cout<<"c2 is: "<<c_(1)<<" computed: "<<(v[1]-v[0]+v[N]-v[N-1])/dt<<std::endl;
//  std::cout<<"c3 is: "<<c_(2)<<" computed: "<<-(v[2]-2*v[1]+v[0]+v[N-2]-2*v[N-1]+v[N])/(dt*dt)<<std::endl;

  v_omega.setZero();
  for (int k = 0; k < num_matsubara; ++k) {
    std::complex<double> iw(0, M_PI*(2*k+1)/beta_);
    for (int n = 1; n < N; n++) {
      //partial integration, four times.
      //Then approximate the fourth derivative by finite differences
      v_omega(k) += exp(iw*(n*dt))*(v2(n+1)-2*v2(n)+v2(n-1));
    }
    // the third derivative, on the boundary
    v_omega(k) += (v2(1) - v2(0) + v2(N) - v2(N-1));
    v_omega(k) *= 1./(dt*iw*iw*iw*iw);
    // the boundary terms of the first, second, and third partial integration.
    v_omega(k) += f_omega(iw, c_(0), c_(1), c_(2));
    //std::cout<<"derivative at boundary: "<<-v2[1] + v2[0] + v2[N] - v2[N-1]<<" divisor: "<< 1./(dt*iw*iw*iw*iw)<<std::endl;
    // the proper convention for the self consistency loop here.
    gomega(k)=v_omega(k);
  }
}

// forward_ft G(tau)->G(iwn) works for cluster Green's functions
void FourierTransformer::forward_ft(const VectorXd &gtau, VectorXcd &gomega,
                                    const int num_matsubara) const {
  std::vector<double> v(gtau.size());
  std::vector<std::complex<double> > v_omega(num_matsubara);
  int Np1 = v.size();
  int N = Np1-1;
  int N_omega = v_omega.size();
  double dt = beta_/N;

  gomega.resize(num_matsubara);
  for(int tau=0;tau<Np1;++tau) {
    v[tau]=gtau(tau);
  }

  dense_matrix spline_matrix(Np1, Np1);
  generate_spline_matrix(spline_matrix, dt);
  // matrix containing the second derivatives y''
  // of interpolated y=v[tau] at points tau_n
  std::vector<double> v2(Np1, 0);
  evaluate_second_derivatives(dt/*,beta_*/, spline_matrix, v, v2,
      c_(0), c_(1), c_(2));
  v_omega.assign(N_omega, 0);
  for (int k=0; k<N_omega; k++) {
    std::complex<double> iw(0, M_PI*(2*k+1)/beta_);
    for (int n=1; n<N; n++) {
      //partial integration, four times.
      //Then approximate the fourth derivative by finite differences
      v_omega[k] += exp(iw*(n*dt))*(v2[n+1]-2*v2[n]+v2[n-1]);
    }
    // the third derivative, on the boundary
    v_omega[k] += (v2[1] - v2[0] + v2[N] - v2[N-1]);
    v_omega[k] *= 1./(dt*iw*iw*iw*iw);
    // the boundary terms of the first, second, and third partial integration.
    v_omega[k] += f_omega(iw, c_(0), c_(1), c_(2));
    //std::cout<<"derivative at boundary: "<<-v2[1] + v2[0] + v2[N] - v2[N-1]<<" divisor: "<< 1./(dt*iw*iw*iw*iw)<<std::endl;
    // the proper convention for the self consistency loop here.
    gomega(k)=v_omega[k];
  }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file KalmanFilter.cpp
 *
 * @brief Simple linear Kalman filter.
 * Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
 *
 * @date Sep 3, 2011
 * @author Stephen Williams
 * @author Frank Dellaert
 */

#include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/Testable.h>

#include <boost/make_shared.hpp>
#include <boost/assign/list_of.hpp>

using namespace boost::assign;
using namespace std;

namespace gtsam {

/* ************************************************************************* */
// Auxiliary function to solve factor graph and return pointer to root conditional
KalmanFilter::State //
KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {

  // Eliminate the graph using the provided Eliminate function
  Ordering ordering(factorGraph.keys());
  GaussianBayesNet::shared_ptr bayesNet = //
      factorGraph.eliminateSequential(ordering, function_);

  // As this is a filter, all we need is the posterior P(x_t).
  // This is the last GaussianConditional in the resulting BayesNet
  GaussianConditional::shared_ptr posterior = *(--bayesNet->end());
  return boost::make_shared<GaussianDensity>(*posterior);
}

/* ************************************************************************* */
// Auxiliary function to create a small graph for predict or update and solve
KalmanFilter::State //
KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {

  // Create a factor graph
  GaussianFactorGraph factorGraph;
  factorGraph += p, newFactor;

  // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
  return solve(factorGraph);
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x0,
    const SharedDiagonal& P0) const {

  // Create a factor graph f(x0), eliminate it into P(x0)
  GaussianFactorGraph factorGraph;
  factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma
  return solve(factorGraph);
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const {

  // Create a factor graph f(x0), eliminate it into P(x0)
  GaussianFactorGraph factorGraph;
  factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
  return solve(factorGraph);
}

/* ************************************************************************* */
void KalmanFilter::print(const string& s) const {
  cout << "KalmanFilter " << s << ", dim = " << n_ << endl;
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
    const Matrix& B, const Vector& u, const SharedDiagonal& model) const {

  // The factor related to the motion model is defined as
  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
  Key k = step(p);
  return fuse(p,
      boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
    const Matrix& B, const Vector& u, const Matrix& Q) const {

#ifndef NDEBUG
  DenseIndex n = F.cols();
  assert(F.rows() == n);
  assert(B.rows() == n);
  assert(B.cols() == u.size());
  assert(Q.rows() == n);
  assert(Q.cols() == n);
#endif

  // The factor related to the motion model is defined as
  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
  // See documentation in HessianFactor, we have A1 = -F,  A2 = I_, b = B*u:
  // TODO: starts to seem more elaborate than straight-up KF equations?
  Matrix M = Q.inverse(), Ft = trans(F);
  Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
  Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
  double f = dot(b, g2);
  Key k = step(p);
  return fuse(p,
      boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
    const Matrix& A1, const Vector& b, const SharedDiagonal& model) const {
  // Nhe factor related to the motion model is defined as
  // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
  Key k = step(p);
  return fuse(p, boost::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
    const Vector& z, const SharedDiagonal& model) const {
  // The factor related to the measurements would be defined as
  // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
  //    = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
  Key k = step(p);
  return fuse(p, boost::make_shared<JacobianFactor>(k, H, z, model));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
    const Vector& z, const Matrix& Q) const {
  Key k = step(p);
  Matrix M = Q.inverse(), Ht = trans(H);
  Matrix G = Ht * M * H;
  Vector g = Ht * M * z;
  double f = dot(z, M * z);
  return fuse(p, boost::make_shared<HessianFactor>(k, G, g, f));
}

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

} // \namespace gtsam


//   Copyright (c) 2018 Shahrzad Shirzad
//
//   Distributed under the Boost Software License, Version 1.0.0. (See accompanying
//   file LICENSE_1_0.0.txt or copy at http://www.boost.org/LICENSE_1_0.0.txt)

#include <phylanx/phylanx.hpp>
#include <hpx/hpx_init.hpp>

#include <iostream>

#include <blaze/Math.h>
#include <boost/program_options.hpp>

#include <hpx/include/agas.hpp>
#include <hpx/runtime_fwd.hpp>

#include <cstddef>
#include <cstdint>
#include <iostream>
#include <map>
#include <vector>
#include <string>
#include <utility>

///////////////////////////////////////////////////////////////////////////////
char const* const read_x_code = R"(
    //
    // Read input-data from given CSV file
    //
    define(read_x, filepath, row_start, row_stop, col_start, col_stop,
        slice(file_read_csv(filepath), make_list(row_start , row_stop),
              make_list(col_start , col_stop))
    )
    read_x
)";

char const* const als_explicit = R"(
    //
    // Alternating Least squares algorithm (ALS)
    //
    define(als_explicit, ratings, regularization, num_factors, iterations, alpha,
        enable_output,
        block(
            define(num_users, shape(ratings, 0)),
            define(num_items, shape(ratings, 1)),
            define(conf, alpha * ratings),

            define(conf_u, constant(0.0, make_list(num_items))),
            define(conf_i, constant(0.0, make_list(num_users))),

            define(c_u, constant(0.0, make_list(num_items, num_items))),
            define(c_i, constant(0.0, make_list(num_users, num_users))),
            define(p_u, constant(0.0, make_list(num_items))),
            define(p_i, constant(0.0, make_list(num_users))),

            set_seed(0),
            define(X, random(make_list(num_users, num_factors))),
            define(Y, random(make_list(num_items, num_factors))),
            define(I_f, identity(num_factors)),
            define(I_i, identity(num_items)),
            define(I_u, identity(num_users)),
            define(k, 0),
            define(i, 0),
            define(u, 0),

            define(XtX, constant(0.0, make_list(num_factors, num_factors))),
            define(YtY, constant(0.0, make_list(num_factors, num_factors))),
            define(A, constant(0.0, make_list(num_factors, num_factors))),
            define(b, constant(0.0, make_list(num_factors))),

            while(k < iterations,
                block(
                    if(enable_output,
                            block(
                                    cout("iteration ", k, u),
                                    cout("X: ",X),
                                    cout("Y: ",Y)
                            )
                    ),
                    store(YtY, dot(transpose(Y), Y) + regularization * I_f),
                    store(XtX, dot(transpose(X), X) + regularization * I_f),

                    while(u < num_users,
                        block(
                            store(conf_u, slice_row(conf, u)),
                            store(c_u, diag(conf_u)),
                            store(p_u, __ne(conf_u, 0.0, true)),
                            store(A, dot(dot(transpose(Y), c_u), Y)+ YtY),
                            store(b, dot(dot(transpose(Y),(c_u + I_i)), transpose(p_u))),
                            store(slice(X, list(u, u + 1, 1),nil), dot(inverse(A), b)),
                            store(u, u + 1)
                        )
                    ),
                    store(u, 0),
                    while(i < num_items,
                        block(
                            store(conf_i, slice_column(conf, i)),
                            store(c_i, diag(conf_i)),
                            store(p_i, __ne(conf_i, 0.0, true)),
                            store(A, dot(dot(transpose(X),c_i), X) + XtX),
                            store(b, dot(dot(transpose(X),(c_i + I_u)), transpose(p_i))),
                            store(slice(Y, list(i, i + 1, 1),nil), dot(inverse(A), b)),
                            store(i, i + 1)
                        )
                    ),
                    store(i, 0),
                    store(k, k + 1)
                )
            ),
            list(X, Y)
        )
    )
    als_explicit
)";

std::string const als_direct = R"(
    //
    // Alternating Least squares algorithm (ALS) (direct implementation)
    //
    define(als_direct, ratings, regularization, num_factors, iterations, alpha,
        enable_output, als(ratings, regularization, num_factors, iterations, alpha,
        enable_output)
    )
    als_direct
)";

///////////////////////////////////////////////////////////////////////////////
// Find the line/column position in the source code from a given iterator
// pointing into it.
//
std::pair<std::size_t, std::size_t> get_pos(std::string const& code,
    std::tuple<std::size_t, std::size_t, std::int64_t> const& tags)
{
    // Column might be given directly, in that case line is given as well
    if (std::get<2>(tags) != -1)
    {
        return std::make_pair(std::get<1>(tags), std::get<2>(tags));
    }

    // Otherwise the given value is the offset into the code
    std::size_t pos = std::get<1>(tags);
    std::size_t line = 1;
    std::size_t column = 1;

    for (std::int64_t i = 0; i != pos && i != code.size(); ++i)
    {
        if (code[i] == '\r' || code[i] == '\n')    // CR/LF
        {
            ++line;
            column = 1;
        }
        else
        {
            ++column;
        }
    }

    return std::make_pair(line, column);
}

///////////////////////////////////////////////////////////////////////////////
// Find offset into code as given by the tags argument
//
std::size_t get_offset(std::string const& code,
    std::tuple<std::size_t, std::size_t, std::size_t> const& tags)
{
    // Offset might be given directly
    if (std::get<2>(tags) == -1)
    {
        return std::get<1>(tags);
    }

    // Otherwise the given value is the line/column position in the code
    std::size_t offset = 0;
    std::size_t line = 1;
    std::size_t column = 0;

    for (std::int64_t i = 0; i != code.size(); ++i, ++offset)
    {
        if (code[i] == '\r' || code[i] == '\n')    // CR/LF
        {
            ++line;
            column = 0;
        }
        else
        {
            ++column;
        }

        if (std::get<1>(tags) == line && std::get<2>(tags) == column)
        {
            break;
        }
    }

    return offset;
}

// Extract the compile_id/tag pair from a given primitive instance name.
//
// The compile_id is a sequence number tracking invocations of the
// function phylanx::execution_tree::compile (needed to link back to the
// concrete source code compiled).
//
// The tag is an index into the array of iterators filled by
// phylanx::ast::generate_ast. It allows to find the iterator referring
// to the construct in the source code a particular primitive instance was
// created by.
//
std::tuple<std::size_t, std::size_t, std::size_t> extract_tags(
    std::string const& name)
{
    auto data = phylanx::execution_tree::compiler::parse_primitive_name(name);
    return std::make_tuple(data.compile_id, data.tag1, data.tag2);
}

// The symbolic names registered in AGAS that identify the created
// primitive instances have the following structure:
//
// /phylanx/<primitive>$<sequence-nr>[$<instance>]/<compile_id>$<tag1>[$<tag2>]
//
//  where:
//      <primitive>:   the name of primitive type representing the given
//                     node in the expression tree
//      <sequence-nr>: the sequence number of the corresponding instance
//                     of type <primitive>
//      <instance>:    (optional), some primitives have additional instance
//                     names, for instance references to function arguments
//                     have the name of the argument as their <instance>
//      <compile_id>:  the sequence number of the invocation of the
//                     function phylanx::execution_tree::compile
//      <tag1>:        if <tag2> == -1: the position inside the compiled code
//                     block where the referring to the point of usage of the
//                     primitive in the compiled source code
//                     if <tag2> != -1: the line number in the compiled code
//                     block where the referring to the point of usage of the
//                     primitive in the compiled source code
//      <tag2>:        (optional) if <tag2> != -1 or not given: the column
//                      offset in the given line (default: -1)
//
void print_instrumentation(char const* const name, int compile_id,
    std::string const& code,
    phylanx::execution_tree::compiler::function const& func,
    std::map<std::string, hpx::id_type> const& entries)
{
    std::cout << "Instrumentation information for function: " << name << "\n";

    for (auto const& e : entries)
    {
        // Extract compile_id and iterator index (tag) from the symbolic name
        auto tags = extract_tags(e.first);
        if (std::get<0>(tags) != compile_id)
            continue;

        // Find real position of given symbol in source code
        if (std::get<1>(tags) != std::size_t(-1))
        {
            auto pos = get_pos(code, tags);
            std::cout << e.first << ": " << name << "(" << pos.first << ", "
                      << pos.second << "): ";

            // Show the next (at max) 20 characters
            auto offset = get_offset(code, tags);
            auto end = code.begin() + offset;
            for (int i = 0; end != code.end() && i != 20; ++end, ++i)
            {
                if (*end == '\n' || *end == '\r')
                    break;
            }
            std::cout << std::string(code.begin() + offset, end) << " ...\n";
        }
        else
        {
            std::cout << e.first << "\n";
        }
    }

    std::cout << "\n";

    std::cout << "Tree information for function: " << name << "\n";
    std::cout << phylanx::execution_tree::newick_tree(
                     name, func.get_expression_topology())
              << "\n\n";

    std::cout << phylanx::execution_tree::dot_tree(
                     name, func.get_expression_topology())
              << "\n\n";
}

void print_performance_counter_data_csv(
    std::vector<std::string> const& existing_primitive_instances)
{
    std::cout << std::endl << "Primitive Performance Counter Data in CSV:";

    // CSV Header
    std::cout << "\nprimitive_instance,display_name,count,time,eval_direct\n";

    // Print performance data
    for (auto const& entry :
        phylanx::util::retrieve_counter_data(existing_primitive_instances))
    {
        std::cout << "\"" << entry.first << "\",\""
                  << phylanx::execution_tree::compiler::primitive_display_name(
                         entry.first)
                  << "\"";
        for (auto const& counter_value : entry.second)
        {
            std::cout << "," << counter_value;
        }
        std::cout << std::endl;
    }

    std::cout << std::endl;
}

int hpx_main(boost::program_options::variables_map& vm)
{
    if (vm.count("data_csv") == 0)
    {
        std::cerr << "Please specify '--data_csv=data-file'";
        return hpx::finalize();
    }

    // compile the given code
    phylanx::execution_tree::compiler::function_list snippets_read_x;
    auto const& code_read_x =
        phylanx::execution_tree::compile("read_x", read_x_code, snippets_read_x);

    phylanx::execution_tree::compiler::function_list snippets_als;
    auto const& code_als = phylanx::execution_tree::compile(
        vm.count("direct") != 0 ? als_direct : als_explicit, snippets_als);

    // Enable collection of performance data for all existing primitives
    auto primitives = phylanx::util::enable_measurements();

    auto read_x = code_read_x.run();
    auto als = code_als.run();

    // Print instrumentation information, if enabled
    if (vm.count("instrument") != 0)
    {
        auto entries = hpx::agas::find_symbols(hpx::launch::sync, "/phylanx/*");

        print_instrumentation("als", 0,
            vm.count("direct") != 0 ? als_direct : als_explicit, als, entries);
    }

    // evaluate generated execution tree
    auto row_start = static_cast<int64_t>(0);
    auto col_start = static_cast<int64_t>(0);
    auto row_stop = vm["row_stop"].as<std::int64_t>();
    auto col_stop = vm["col_stop"].as<std::int64_t>();

    auto regularization = vm["regularization"].as<double>();
    auto iterations = vm["iterations"].as<int64_t>();
    auto num_factors = vm["factors"].as<int64_t>();
    auto alpha = vm["alpha"].as<double>();
    auto filepath = vm["data_csv"].as<std::string>();

    bool enable_output = vm.count("enable_output") != 0;

    // Read the data from the files
    auto ratings = read_x(filepath, row_start, row_stop, col_start, col_stop);

    // Measure execution time
    hpx::util::high_resolution_timer t;

    // Evaluate ALS using the read data
    auto result =
        als(ratings, regularization, num_factors, iterations, alpha, enable_output);
    auto elapsed = t.elapsed();

    // Print performance counter data in CSV
    if (vm.count("instrument") != 0)
    {
        print_performance_counter_data_csv(primitives);
    }

    // Make sure all counters are properly initialized, don't reset current
    // counter values
    hpx::reinit_active_counters(false);

    auto result_r = phylanx::execution_tree::extract_list_value(result);
    auto it = result_r.begin();
    std::cout << "X: \n"
              << phylanx::execution_tree::extract_numeric_value(*it++)
              << "\nY: \n"
              << phylanx::execution_tree::extract_numeric_value(*it)
              << std::endl;
    std::cout << "time: " << t.elapsed() << std::endl;

    return hpx::finalize();
}

int main(int argc, char* argv[])
{
    // command line handling
    boost::program_options::options_description desc("usage: als [options]");
    desc.add_options()("enable_output,e",
        "enable progress output (default: false)")("instrument,i",
        "print instrumentation information (default: false)")("direct,d",
        "use direct implementation of ALS (default: false)")("iterations,n",
        boost::program_options::value<std::int64_t>()->default_value(3),
        "number of iterations (default: 10.0)")("factors,f",
        boost::program_options::value<std::int64_t>()->default_value(10),
        "number of factors (default: 10)")("alpha,a",
        boost::program_options::value<double>()->default_value(40),
        "alpha (default: 40)")("regularization,r",
        boost::program_options::value<double>()->default_value(0.1),
        "regularization (default: 0.1)")("data_csv",
        boost::program_options::value<std::string>(),
        "file name for reading data")("row_stop",
        boost::program_options::value<std::int64_t>()->default_value(10),
        "row_stop (default: 10)")("col_stop",
        boost::program_options::value<std::int64_t>()->default_value(100),
        "col_stop (default: 100)");
    return hpx::init(desc, argc, argv);
}


#define _USE_MATH_DEFINES

#include "fastRect.h"

#include <Eigen/Core>
#include <Eigen/Dense>

std::vector<std::vector<fast::calPoint>> fast::rectROI(int center_x, int center_y, double angle, int range, int distance, bool direction, int skip_pixels) {

    if (range == 0) {
        HVERROR(error, "Invalid box roi range");
    }

    if (distance == 0) {
        HVERROR(error, "Invalid box distance");
    }

    if (skip_pixels == 0) {
        HVERROR(error, "Invalid search step");
    }

    int dimension = range * distance;

    int roi_mid_width = range / 2;
    int roi_mid_height = distance / 2;

    int start_x1 = 0;
    int start_y1 = 0;
    int end_x1 = 0;
    int end_y1 = 0;

    int start_x2 = 0;
    int start_y2 = 0;
    int end_x2 = 0;
    int end_y2 = 0;

    if (direction == false) {
        start_x1 = center_x - roi_mid_width;
        start_y1 = center_y + roi_mid_height;

        start_x2 = center_x + roi_mid_width;
        start_y2 = center_y + roi_mid_height;

        end_x1 = center_x - roi_mid_width;
        end_y1 = center_y - roi_mid_height;

        end_x2 = center_x + roi_mid_width;
        end_y2 = center_y - roi_mid_height;
    }
    else {
        start_x1 = center_x - roi_mid_width;
        start_y1 = center_y - roi_mid_height;

        start_x2 = center_x + roi_mid_width;
        start_y2 = center_y - roi_mid_height;

        end_x1 = center_x - roi_mid_width;
        end_y1 = center_y + roi_mid_height;

        end_x2 = center_x + roi_mid_width;
        end_y2 = center_y + roi_mid_height;
    }

    int size = 4;

    Eigen::MatrixXd trigonometric_matrix(2, 2);

    //trigonometric_matrix
    trigonometric_matrix(0, 0) = cos(angle * M_PI / 180);
    trigonometric_matrix(0, 1) = -sin(angle * M_PI / 180);
    trigonometric_matrix(1, 0) = sin(angle * M_PI / 180);
    trigonometric_matrix(1, 1) = cos(angle * M_PI / 180);

    //rotation_matrix
    Eigen::MatrixXd rotation_matrix(2, size);

    //base_matrix
    Eigen::MatrixXd base_matrix(2, size);

    rotation_matrix(0, 0) = start_x1 - center_x; // x           Start X1
    rotation_matrix(1, 0) = start_y1 - center_y; // y

    rotation_matrix(0, 1) = start_x2 - center_x; // x
    rotation_matrix(1, 1) = start_y2 - center_y; // y

    rotation_matrix(0, 2) = end_x1 - center_x;// x
    rotation_matrix(1, 2) = end_y1 - center_y; // y

    rotation_matrix(0, 3) = end_x2 - center_x; // x
    rotation_matrix(1, 3) = end_y2 - center_y; /// y

    // No problem
    for (int index = 0; index < 4; index++) {
        base_matrix(0, index) = center_x;
        base_matrix(1, index) = center_y;
    }

    Eigen::MatrixXd result = trigonometric_matrix * rotation_matrix + base_matrix;

    double rotated_start_x1 = result(0, 0);
    double rotated_start_y1 = result(1, 0);

    double rotated_start_x2 = result(0, 1);
    double rotated_start_y2 = result(1, 1);

    double rotated_end_x1 = result(0, 2);
    double rotated_end_y1 = result(1, 2);

    double rotated_end_x2 = result(0, 3);
    double rotated_end_y2 = result(1, 3);

    //startx1 = rotated_start_x1;
    //starty1 = rotated_start_y1;
    //startx2 = rotated_start_x2;
    //starty2 = rotated_start_y2;
    //endx1 = rotated_end_x1;
    //endy1 = rotated_end_y1;
    //endx2 = rotated_end_x2;
    //endy2 = rotated_end_y2;

    double diff_start_x = rotated_start_x2 - rotated_start_x1;
    double diff_start_y = rotated_start_y2 - rotated_start_y1;

    double increase_rate_x = diff_start_x / range;
    double increase_rate_y = diff_start_y / range;

    //Start Line Points
    //Start Line Points
    //Start Line Points
    //Start Line Points
    int range_align_size = (range)+(range % 4);
    std::vector<double> range_vec(range_align_size * 2);
    for (int index = 0; index < range * 2; index += 2) {
        int current_index = index;
        range_vec[index] = current_index / 2;
        range_vec[index + 1] = current_index / 2;
    }

    std::vector<fast::calPoint> start_line_xy(range_align_size);
    std::vector<fast::calPoint> end_line_xy(range_align_size);

    std::vector<double> increase_rate_xy_vec = { increase_rate_x, increase_rate_y,increase_rate_x, increase_rate_y };
    const __m256d simd_increase_rate_xy = _mm256_load_pd(increase_rate_xy_vec.data());

    std::vector<double> simd_start_xy_vec = { rotated_start_x1 , rotated_start_y1, rotated_start_x1 , rotated_start_y1 };
    const __m256d simd_start_xy = _mm256_load_pd(simd_start_xy_vec.data());

    std::vector<double> simd_end_xy_vec = { rotated_end_x1 , rotated_end_y1, rotated_end_x1 , rotated_end_y1 };
    const __m256d simd_end_xy = _mm256_load_pd(simd_end_xy_vec.data());

    const double* range_ptr = &range_vec[0];

    const fast::calPoint* start_result_xy_ptr = start_line_xy.data();
    const fast::calPoint* end_result_xy_ptr = end_line_xy.data();

    int chunk_size = sizeof(double) * 4;
    for (int index = 0; index < range * 2; index += 4) {

        __m256d range_chunk = _mm256_load_pd(range_ptr + index);

        __m256d chunk_mul = _mm256_mul_pd(simd_increase_rate_xy, range_chunk);

        // Start Line
        __m256d start_result = _mm256_add_pd(chunk_mul, simd_start_xy);

        // End Line
        __m256d end_result = _mm256_add_pd(chunk_mul, simd_end_xy);
        memcpy(((double*)start_result_xy_ptr + index), &start_result, chunk_size);
        memcpy(((double*)end_result_xy_ptr + index), &end_result, chunk_size);
    }

    //Start Line Points
    //Start Line Points
    //Start Line Points
    //Start Line Points

    int distance_align_size = (distance)+(distance % 4);
    std::vector<double> distance_vec(distance_align_size * 2);
    for (int index = 0; index < distance * 2; index += 2) {
        int current_index = index;
        distance_vec[index] = current_index / 2;
        distance_vec[index + 1] = current_index / 2;
    }

    std::vector<std::vector<fast::calPoint>> combine_vertical_xy;

    double diff_virtical_start_x = rotated_end_x1 - rotated_start_x1;
    double diff_virtical_start_y = rotated_end_y1 - rotated_start_y1;

    double increase_virtical_rate_x = diff_virtical_start_x / distance;
    double increase_virtical_rate_y = diff_virtical_start_y / distance;

    std::vector<double> increase_vertical_rate_xy_vec = { increase_virtical_rate_x, increase_virtical_rate_y,increase_virtical_rate_x, increase_virtical_rate_y };
    const __m256d simd_increase_vertical_rate_xy = _mm256_load_pd(increase_vertical_rate_xy_vec.data());

    for (int range_index = 0; range_index < range; range_index += skip_pixels) {

        auto start_vertical_point = start_line_xy[range_index];
        std::vector<double> simd_vertical_xy_vec = { start_vertical_point.x , start_vertical_point.y, start_vertical_point.x , start_vertical_point.y };
        const __m256d simd_vertical_xy = _mm256_load_pd(simd_vertical_xy_vec.data());

        const double* distance_ptr = &distance_vec[0];

        std::vector<fast::calPoint> vertical_xy;
        vertical_xy.resize(distance_align_size);
        const fast::calPoint* start_vertical_xy_ptr = vertical_xy.data();

        for (int index = 0; index < distance * 2; index += 4) {
            __m256d distance_chunk = _mm256_load_pd(distance_ptr + index);
            __m256d chunk_mul = _mm256_mul_pd(simd_increase_vertical_rate_xy, distance_chunk);
            __m256d start_result = _mm256_add_pd(chunk_mul, simd_vertical_xy);
            memcpy((((double*)start_vertical_xy_ptr) + index), &start_result, chunk_size);
        }

        combine_vertical_xy.push_back(vertical_xy);
    }

    return combine_vertical_xy;
}


//
//  Copyright Markus Rickert 2008
//
// 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 <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/blas/level3/gemm.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/std/valarray.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

namespace bindings = boost::numeric::bindings;

int
main(int argc, char** argv)
{
  {
    // a * b' = C ; a' * b = d

    boost::numeric::ublas::vector<double> a(3);
    for(std::size_t i = 0; i < a.size(); ++i) a(i) = i;
    std::cout << "a=" << a << std::endl;

    boost::numeric::ublas::vector<double> b(3);
    for(std::size_t i = 0; i < b.size(); ++i) b(i) = i;
    std::cout << "b=" << b << std::endl;

    boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
    boost::numeric::bindings::blas::gemm(
      1.0, a, bindings::trans(b), 0.0, c
    );
    std::cout << "c=" << c << std::endl;

    boost::numeric::ublas::vector<double> d(1);
    boost::numeric::bindings::blas::gemm(
      1.0, bindings::trans(a), b, 0.0, d
    );
    std::cout << "d=" << d << std::endl;
  }

  std::cout << std::endl;

  {
    // a * b' = C ; a' * b = d

    std::vector<double> a(3);
    for(std::size_t i = 0; i < a.size(); ++i) a[i] = i;
    std::cout << "a=[" << a.size() << "](";
    for(std::size_t i = 0; i < a.size(); ++i) std::cout << (i > 0 ? "," : "") << a[i];
    std::cout << ")" << std::endl;

    std::valarray<double> b(3);
    for(std::size_t i = 0; i < b.size(); ++i) b[i] = i;
    std::cout << "b=[" << b.size() << "](";
    for(std::size_t i = 0; i < b.size(); ++i) std::cout << (i > 0 ? "," : "") << b[i];
    std::cout << ")" << std::endl;

    boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
    boost::numeric::bindings::blas::gemm(
      1.0, a, bindings::trans(b), 0.0, c
    );
    std::cout << "c=" << c << std::endl;

    std::vector<double> d(1);
    boost::numeric::bindings::blas::gemm(
      1.0, bindings::trans(a), b, 0.0, d
    );
    std::cout << "d=[" << d.size() << "](";
    for(std::size_t i = 0; i < d.size(); ++i) std::cout << (i > 0 ? "," : "") << d[i];
    std::cout << ")" << std::endl;
  }

  std::cout << std::endl;

  {
    // a * b' = C ; a' * b = d

    double a[3];
    for(std::size_t i = 0; i < 3; ++i) a[i] = i;
    std::cout << "a=[" << 3 << "](";
    for(std::size_t i = 0; i < 3; ++i) std::cout << (i > 0 ? "," : "") << a[i];
    std::cout << ")" << std::endl;

    std::vector<double> b(3);
    for(std::size_t i = 0; i < b.size(); ++i) b[i] = i;
    std::cout << "b=[" << b.size() << "](";
    for(std::size_t i = 0; i < b.size(); ++i) std::cout << (i > 0 ? "," : "") << b[i];
    std::cout << ")" << std::endl;

    boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
    boost::numeric::bindings::blas::gemm(
      1.0, a, bindings::trans(b), 0.0, c
    );
    std::cout << "c=" << c << std::endl;

    std::valarray<double> d(1);
    boost::numeric::bindings::blas::gemm(
      1.0, bindings::trans(a), b, 0.0, d
    );
    std::cout << "d=[" << d.size() << "](";
    for(std::size_t i = 0; i < d.size(); ++i) std::cout << (i > 0 ? "," : "") << d[i];
    std::cout << ")" << std::endl;
  }

  return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_POWER_EXPLICIT_INCLUDE
#define MTL_POWER_EXPLICIT_INCLUDE

#include <boost/numeric/linear_algebra/algebraic_concepts.hpp>
#include <boost/numeric/linear_algebra/concepts.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>

namespace mtl {

template <typename Op, typename Element, typename Exponent>
  _GLIBCXX_WHERE( std::Integral<Exponent>
	    && std::Callable2<Op, Element, Element>
	    && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>)
inline Element power(const Element& base, Exponent n, Op op)
{
    if (n < 1) throw "In power: exponent must be greater than 0";
    // std::cout << "[Magma] ";

    Element value= base;
    for (; n > 1; --n)
	value= op(value, base);
    return value;
}

# ifndef __GXX_CONCEPTS__
#   ifdef LA_SHOW_WARNINGS
#     warning "Automatic dispatching only works with concept compiler"
#     warning "If structure is a Monoid you can call square_and_multiply directly"
#   endif
# else

template <typename Op, typename Element, typename Exponent>
    where algebra::SemiGroup<Op, Element> && std::Integral<Exponent>
          && std::Callable2<Op, Element, Element>
          && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>
inline Element power(const Element& base, Exponent n, Op op)
{
    // std::cout << "[SemiGroup] ";

    if (n <= 0) throw "In recursive_multiply_and_square: exponent must greater than 0";

    Exponent half= n >> 1;

    // If halt is 0 then n must be 1 and the result is base
    if (half == 0)
	return base;

    // compute power of downward rounded exponent and square the result
    Element value= power(base, half, op);
    value= op(value, value);

    // if odd another multiplication with base is needed
    if (n & 1)
	value= op(value, base);
    return value;
}

// {Op, Element} must be a Monoid
template <typename Op, typename Element, typename Exponent>
    where algebra::Monoid<Op, Element>
          && std::Integral<Exponent>
          && std::Callable2<Op, Element, Element>
          && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>
// && std::Assignable<Element, algebdra::Monoid<Op, Element>::identity_result_type>
          && std::Assignable<Element, Element>
inline Element multiply_and_square(const Element& base, Exponent n, Op op)
{
    // Same as the simpler form except that the first multiplication is made before
    // the loop and one squaring is saved this way
    if (n < 0) throw "In multiply_and_square: negative exponent";

    using math::identity;
    Element value= identity(op, base), square= identity(op, base);

    if (n & 1)
        value= base;

    for (n>>= 1; n > 0; n>>= 1) {
	square= op(square, square);
	if (n & 1)
	    value= op(value, square);
    }
    return value;
}

template <typename Op, typename Element, typename Exponent>
    where algebra::Monoid<Op, Element> && std::Integral<Exponent>
          && std::Callable2<Op, Element, Element>
          && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>
          && std::Assignable<Element, Element>
inline Element power(const Element& base, Exponent n, Op op)
{
    return multiply_and_square(base, n, op);
}

template <typename Op, typename Element, typename Exponent>
    where algebra::Group<Op, Element> && std::SignedIntegral<Exponent>
          && std::Callable2<Op, Element, Element>
          && std::Assignable<Element, std::Callable2<Op, Element, Element>::result_type>
          && std::Assignable<Element, Element>
inline Element power(const Element& base, Exponent n, Op op)
{
    using math::inverse;

    return n >= 0 ? multiply_and_square(base, n, op)
	          : multiply_and_square(inverse(op, base), -n, op);
}

# endif   // __GXX_CONCEPTS__

} // namespace mtl

#endif // MTL_POWER_EXPLICIT_INCLUDE


// BSD 3-Clause License

// Copyright (c) 2020, Chenyu
// 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.

// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived from
//    this software without specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 COPYRIGHT HOLDER 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.

#include "clustering/spectral_cluster.h"

#include <Spectra/SymEigsSolver.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <algorithm>

#include "clustering/kmeans.h"
// #include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
// #include <Spectra/MatOp/SparseGenMatProd.h>
#include <glog/logging.h>

#include "util/timer.h"

namespace DAGSfM {

std::unordered_map<int, int> SpectralCluster::ComputeCluster(
    const std::vector<std::pair<int, int>>& edges,
    const std::vector<int>& weights, const int num_partitions) {
  if (num_partitions == 1) {
    for (auto node : nodes_) {
      labels_[node] = 0;
    }
    return labels_;
  }

  colmap::Timer timer;
  std::vector<Eigen::Triplet<double>> s_triplets;
  std::unordered_map<int, int> degrees;
  const int k = num_partitions;
  cluster_num_ = num_partitions;

  // 1. Compute similarity graph.
  for (uint i = 0; i < nodes_.size(); i++) {
    node_mapper_[nodes_[i]] = i;
  }

  timer.Start();
  const int N = nodes_.size();
  Eigen::SparseMatrix<double> S(N, N);
  for (uint i = 0; i < edges.size(); i++) {
    int src = node_mapper_[edges[i].first], dst = node_mapper_[edges[i].second];
    s_triplets.push_back(Eigen::Triplet<double>(src, dst, weights[i]));
    s_triplets.push_back(Eigen::Triplet<double>(dst, src, weights[i]));
    degrees[src] += 1;
    degrees[dst] += 1;
  }

  S.setFromTriplets(s_triplets.begin(), s_triplets.end());
  S.makeCompressed();
  timer.Pause();
  LOG(INFO) << "1. Similarity Graph Computation Time: "
            << timer.ElapsedSeconds();

  // 2. Compute Laplacian matrix.
  timer.Start();
  Eigen::SparseMatrix<double> L = ComputeLaplacian(S, degrees);
  L.makeCompressed();
  timer.Pause();
  LOG(INFO) << "2. Laplacian Matrix Computation Time: "
            << timer.ElapsedSeconds();

  // 3. Compute the top-k smallest eigen values and corresponding eigen vectors.
  timer.Start();
  Spectra::SparseSymMatProd<double> op(L);
  Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE,
                         Spectra::SparseSymMatProd<double>>
      eigs(&op, k, std::min(2 * k, N));
  eigs.init();
  int nconv = eigs.compute();

  Eigen::VectorXd eigen_values;
  Eigen::MatrixXd eigen_vectors;
  if (eigs.info() == Spectra::SUCCESSFUL) {
    eigen_values = eigs.eigenvalues();
    eigen_vectors = eigs.eigenvectors();
  }

  timer.Pause();
  LOG(INFO) << "3. EigenValue Computation Time: " << timer.ElapsedSeconds();

  // 4. Reverse original eigen vectors(as it stored in descending order)
  timer.Start();
  uint i = 0, j = k - 1;
  while (i < j) {
    const Eigen::VectorXd tmp = eigen_vectors.col(i);
    eigen_vectors.col(i) = eigen_vectors.col(j);
    eigen_vectors.col(j) = tmp;
    i++;
    j--;
  }

  std::vector<Eigen::VectorXd> source_data;
  source_data.reserve(eigen_vectors.rows());
  for (uint i = 0; i < eigen_vectors.rows(); i++) {
    source_data.push_back(eigen_vectors.row(i));
  }
  timer.Pause();
  LOG(INFO) << "4. Eigen Vectors Reverse Time: " << timer.ElapsedSeconds();

  // 5. Invert K-Means for clustering.
  timer.Start();
  std::vector<uint32_t> cluster_assignment;
  std::vector<Eigen::VectorXd> centers;
  KMeans(source_data, cluster_assignment, centers, k);
  timer.Pause();
  LOG(INFO) << "5. KMeans Time: " << timer.ElapsedSeconds();

  for (uint i = 0; i < cluster_assignment.size(); i++) {
    labels_[nodes_[i]] = cluster_assignment[i];
  }
  return labels_;
}

Eigen::SparseMatrix<double> SpectralCluster::ComputeLaplacian(
    const Eigen::SparseMatrix<double>& S,
    const std::unordered_map<int, int>& degrees) const {
  // Compute degree matrix.
  const int N = degrees.size();
  Eigen::SparseMatrix<double> D(N, N);
  // Eigen::SparseMatrix<double> D_inv(N, N);
  // Eigen::SparseMatrix<double> D_sqrt(N, N);
  for (auto it = degrees.begin(); it != degrees.end(); ++it) {
    int id = it->first;
    D.insert(id, id) = it->second;
    // D.insert(node_mapper_.at(id), node_mapper_.at(id)) = it->second;
    // D_inv.insert(node_mapper_.at(id), node_mapper_.at(id)) = 1.0 /
    // it->second; D_sqrt.insert(node_mapper_.at(id), node_mapper_.at(id)) =
    // -1.0 / sqrt(it->second);
  }
  D.makeCompressed();
  // D_inv.makeCompressed();
  // D_sqrt.makeCompressed();

  // Compute Laplacian matrix.
  Eigen::SparseMatrix<double> L = D - S;
  // Eigen::MatrixXd L_random = D_inv * L;
  // Eigen::MatrixXd L_sym = D_sqrt * L * D_sqrt;

  return L;
}

}  // namespace DAGSfM