#include <iostream>
#include <cstdlib>
#include <cstdio>
#include <armadillo>
#include <random>

#include <mpi.h>

#include "transactions.hh"
#include "histograms.hh"

using namespace std;
using namespace arma;

int main(int argc, char **argv) {
  int mpiSize, mpiRank;

  MPI_Init(&argc, &argv);
  MPI_Comm_size(MPI_COMM_WORLD, &mpiSize);
  MPI_Comm_rank(MPI_COMM_WORLD, &mpiRank);

  if(argc <= 6) {
    if(mpiRank == 0) {
      fprintf(stderr, "Usage: %s OUTPUT-FILE NUM-AGENTS NUM-TRANSACTIONS NUM-RUNS INITIAL-MONEY [LAMBDA] [ALPHA] [S-FACTOR]\n", argv[0]);
    }
    MPI_Finalize();
    exit(0);
  }

  // read parameters
  const char *filename;
  int N;      // number of agents
  int K;      // number of transactions
  int R;      // number of runs
  double m0;  // initial money for each agent
  double l;   // lambda parameter
  double a;   // alpha parameter
  double S;   // "S-factor", affects accuracy and slowness of simulations with α≠0 or γ≠0 (default: 1.0)
  if(mpiRank == 0) {
    // mandatory arguments
    filename = argv[1];
    N = atoi(argv[2]);
    K = atoi(argv[3]);
    R = atoi(argv[4]);
    m0 = atof(argv[5]);

    // optional arguments
    l  = (argc > 6 ? atof(argv[6]) : 0.0);
    a  = (argc > 7 ? atof(argv[7]) : 0.0);
    S  = (argc > 8 ? atof(argv[8]) : 1.0);
  }

  if(mpiRank == 0) {
    fprintf(stderr, "Running with parameters:\n");
    fprintf(stderr, "  N = %d\n", N);
    fprintf(stderr, "  K = %d\n", K);
    fprintf(stderr, "  R = %d\n", R);
    fprintf(stderr, " m0 = %.3f\n", m0);
    fprintf(stderr, "  λ = %.3f\n", l);
    fprintf(stderr, "  α = %.3f\n", a);
    fprintf(stderr, "  S = %.1f\n", S);
  }

  // broadcast parameters to all nodes
  MPI_Bcast(&N, 1, MPI_INT, 0, MPI_COMM_WORLD);
  MPI_Bcast(&K, 1, MPI_INT, 0, MPI_COMM_WORLD);
  MPI_Bcast(&R, 1, MPI_INT, 0, MPI_COMM_WORLD);
  MPI_Bcast(&m0, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
  MPI_Bcast(&l, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
  MPI_Bcast(&a, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);
  MPI_Bcast(&S, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD);

  // reseed simulation with true random seed
  // (so that the different processes don't make the exact same simulations)
  seed_simulation();

  // list of all agents after each run
  vector<double> m(N * R, 1);

  // find start and end indices for range of runs to be done
  // by this process
  size_t runsPerProcess = R / mpiSize;
  size_t startRun = mpiRank * runsPerProcess;
  size_t endRun = startRun + runsPerProcess;
  if(endRun > R) endRun = R;
  if(mpiRank == mpiSize - 1 && endRun < R) endRun = R;

  // do all runs (parallelized)
  for(size_t r = startRun; r < endRun; r++) {
    // simulate transactions
    // (store resulting money distribution in a subvector)
    vector<double> m_dist = simulate_transactions(N, K, m0, l, S, a);

    copy(begin(m_dist), end(m_dist), begin(m) + (r * N));
  }

  // send all data to process #0
  double *mptr = m.data();
  if(mpiRank == 0) {
    for(int p = 1; p < mpiSize; p++) {
      size_t pStartRun = p * runsPerProcess;
      size_t pEndRun   = pStartRun + runsPerProcess;
      if(pEndRun > R) pEndRun = R;
      if(p == mpiSize - 1 && pEndRun < R) pEndRun = R;

      MPI_Recv(&mptr[N * pStartRun], N * (pEndRun - pStartRun), MPI_DOUBLE, p, 0, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
    }
  } else {
    MPI_Send(&mptr[N * startRun], N * (endRun - startRun), MPI_DOUBLE, 0, 0, MPI_COMM_WORLD);
  }

  // save histogram data to file (for plotting)
  if(mpiRank == 0)
    save_histogram(m, 0.01, filename);

  MPI_Finalize();
  return 0;
}


#pragma once

#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include "../Config/config.hh"
#include "../Math/math.hh"

namespace bold
{
  typedef std::function<Eigen::Vector2d(Eigen::Vector3d const&)> Projector;

  class CameraModel
  {
  public:
    /// Initialises a CameraModel using parameters defined in configuration.
    CameraModel()
    : CameraModel(Config::getStaticValue<int>("camera.image-width"),
                  Config::getStaticValue<int>("camera.image-height"),
                  Config::getStaticValue<double>("camera.field-of-view.vertical-degrees"),
                  Config::getStaticValue<double>("camera.field-of-view.horizontal-degrees"))
    {}

    /// Initialises a CameraModel using the specified parameters.
    CameraModel(ushort imageWidth, ushort imageHeight, double rangeVerticalDegs, double rangeHorizontalDegs);

    ushort imageWidth() const { return d_imageWidth; }
    ushort imageHeight() const { return d_imageHeight; }

    double focalLength() const { return d_focalLength; }
    double rangeVerticalDegs() const { return d_rangeVerticalDegs; }
    double rangeVerticalRads() const { return Math::degToRad(d_rangeVerticalDegs); }
    double rangeHorizontalDegs() const { return d_rangeHorizontalDegs; }
    double rangeHorizontalRads() const { return Math::degToRad(d_rangeHorizontalDegs); }

    /** Gets the direction, in camera coordinates, of the specified pixel.
     * Returns a unit vector.
     */
    Eigen::Vector3d directionForPixel(Eigen::Vector2d const& pixel) const;

    Maybe<Eigen::Vector2d> pixelForDirection(Eigen::Vector3d const& direction) const;

    /** Gets a projection matrix
     *
     * This matrix projects from camera frame onto image frame, up to
     * a scaling factor, which is given in the z element of the
     * transformed vector. i.e to get the pixel coordinate p of a
     * point v with projection matrix T: p' = Tv, p = p'/p'_z.
     */
    Eigen::Affine3d getImageCameraTransform() const { return d_projectionTransform; }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  private:
    ushort d_imageWidth;
    ushort d_imageHeight;
    double d_focalLength;
    double d_rangeVerticalDegs;
    double d_rangeHorizontalDegs;
    Eigen::Affine3d d_projectionTransform;
  };
}


// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * 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.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill 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 HOLDERS 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.
//
// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)

#include "base/polynomial.h"

#include <Eigen/Eigenvalues>

#include "util/logging.h"

namespace colmap {
namespace {

// Remove leading zero coefficients.
Eigen::VectorXd RemoveLeadingZeros(const Eigen::VectorXd& coeffs) {
  Eigen::VectorXd::Index num_zeros = 0;
  for (; num_zeros < coeffs.size(); ++num_zeros) {
    if (coeffs(num_zeros) != 0) {
      break;
    }
  }
  return coeffs.tail(coeffs.size() - num_zeros);
}

// Remove trailing zero coefficients.
Eigen::VectorXd RemoveTrailingZeros(const Eigen::VectorXd& coeffs) {
  Eigen::VectorXd::Index num_zeros = 0;
  for (; num_zeros < coeffs.size(); ++num_zeros) {
    if (coeffs(coeffs.size() - 1 - num_zeros) != 0) {
      break;
    }
  }
  return coeffs.head(coeffs.size() - num_zeros);
}

}  // namespace

bool FindLinearPolynomialRoots(const Eigen::VectorXd& coeffs,
                               Eigen::VectorXd* real, Eigen::VectorXd* imag) {
  CHECK_EQ(coeffs.size(), 2);

  if (coeffs(0) == 0) {
    return false;
  }

  if (real != nullptr) {
    real->resize(1);
    (*real)(0) = -coeffs(1) / coeffs(0);
  }

  if (imag != nullptr) {
    imag->resize(1);
    (*imag)(0) = 0;
  }

  return true;
}

bool FindQuadraticPolynomialRoots(const Eigen::VectorXd& coeffs,
                                  Eigen::VectorXd* real,
                                  Eigen::VectorXd* imag) {
  CHECK_EQ(coeffs.size(), 3);

  const double a = coeffs(0);
  if (a == 0) {
    return FindLinearPolynomialRoots(coeffs.tail(2), real, imag);
  }

  const double b = coeffs(1);
  const double c = coeffs(2);
  if (b == 0 && c == 0) {
    if (real != nullptr) {
      real->resize(1);
      (*real)(0) = 0;
    }
    if (imag != nullptr) {
      imag->resize(1);
      (*imag)(0) = 0;
    }
    return true;
  }

  const double d = b * b - 4 * a * c;

  if (d >= 0) {
    const double sqrt_d = std::sqrt(d);
    if (real != nullptr) {
      real->resize(2);
      if (b >= 0) {
        (*real)(0) = (-b - sqrt_d) / (2 * a);
        (*real)(1) = (2 * c) / (-b - sqrt_d);
      } else {
        (*real)(0) = (2 * c) / (-b + sqrt_d);
        (*real)(1) = (-b + sqrt_d) / (2 * a);
      }
    }
    if (imag != nullptr) {
      imag->resize(2);
      imag->setZero();
    }
  } else {
    if (real != nullptr) {
      real->resize(2);
      real->setConstant(-b / (2 * a));
    }
    if (imag != nullptr) {
      imag->resize(2);
      (*imag)(0) = std::sqrt(-d) / (2 * a);
      (*imag)(1) = -(*imag)(0);
    }
  }

  return true;
}

bool FindPolynomialRootsDurandKerner(const Eigen::VectorXd& coeffs_all,
                                     Eigen::VectorXd* real,
                                     Eigen::VectorXd* imag) {
  CHECK_GE(coeffs_all.size(), 2);

  const Eigen::VectorXd coeffs = RemoveLeadingZeros(coeffs_all);

  const int degree = coeffs.size() - 1;

  if (degree <= 0) {
    return false;
  } else if (degree == 1) {
    return FindLinearPolynomialRoots(coeffs, real, imag);
  } else if (degree == 2) {
    return FindQuadraticPolynomialRoots(coeffs, real, imag);
  }

  // Initialize roots.
  Eigen::VectorXcd roots(degree);
  roots(degree - 1) = std::complex<double>(1, 0);
  for (int i = degree - 2; i >= 0; --i) {
    roots(i) = roots(i + 1) * std::complex<double>(1, 1);
  }

  // Iterative solver.
  const int kMaxNumIterations = 100;
  const double kMaxRootChange = 1e-10;
  for (int iter = 0; iter < kMaxNumIterations; ++iter) {
    double max_root_change = 0.0;
    for (int i = 0; i < degree; ++i) {
      const std::complex<double> root_i = roots(i);
      std::complex<double> numerator = coeffs[0];
      std::complex<double> denominator = coeffs[0];
      for (int j = 0; j < degree; ++j) {
        numerator = numerator * root_i + coeffs[j + 1];
        if (i != j) {
          denominator = denominator * (root_i - roots(j));
        }
      }
      const std::complex<double> root_i_change = numerator / denominator;
      roots(i) = root_i - root_i_change;
      max_root_change =
          std::max(max_root_change, std::abs(root_i_change.real()));
      max_root_change =
          std::max(max_root_change, std::abs(root_i_change.imag()));
    }

    // Break, if roots do not change anymore.
    if (max_root_change < kMaxRootChange) {
      break;
    }
  }

  if (real != nullptr) {
    real->resize(degree);
    *real = roots.real();
  }
  if (imag != nullptr) {
    imag->resize(degree);
    *imag = roots.imag();
  }

  return true;
}

bool FindPolynomialRootsCompanionMatrix(const Eigen::VectorXd& coeffs_all,
                                        Eigen::VectorXd* real,
                                        Eigen::VectorXd* imag) {
  CHECK_GE(coeffs_all.size(), 2);

  Eigen::VectorXd coeffs = RemoveLeadingZeros(coeffs_all);

  const int degree = coeffs.size() - 1;

  if (degree <= 0) {
    return false;
  } else if (degree == 1) {
    return FindLinearPolynomialRoots(coeffs, real, imag);
  } else if (degree == 2) {
    return FindQuadraticPolynomialRoots(coeffs, real, imag);
  }

  // Remove the coefficients where zero is a solution.
  coeffs = RemoveTrailingZeros(coeffs);

  // Check if only zero is a solution.
  if (coeffs.size() == 1) {
    if (real != nullptr) {
      real->resize(1);
      (*real)(0) = 0;
    }
    if (imag != nullptr) {
      imag->resize(1);
      (*imag)(0) = 0;
    }
    return true;
  }

  // Fill the companion matrix.
  Eigen::MatrixXd C(coeffs.size() - 1, coeffs.size() - 1);
  C.setZero();
  for (Eigen::MatrixXd::Index i = 1; i < C.rows(); ++i) {
    C(i, i - 1) = 1;
  }
  C.row(0) = -coeffs.tail(coeffs.size() - 1) / coeffs(0);

  // Solve for the roots of the polynomial.
  Eigen::EigenSolver<Eigen::MatrixXd> solver(C, false);
  if (solver.info() != Eigen::Success) {
    return false;
  }

  // If there are trailing zeros, we must add zero as a solution.
  const int effective_degree =
      coeffs.size() - 1 < degree ? coeffs.size() : coeffs.size() - 1;

  if (real != nullptr) {
    real->resize(effective_degree);
    real->head(coeffs.size() - 1) = solver.eigenvalues().real();
    if (effective_degree > coeffs.size() - 1) {
      (*real)(real->size() - 1) = 0;
    }
  }
  if (imag != nullptr) {
    imag->resize(effective_degree);
    imag->head(coeffs.size() - 1) = solver.eigenvalues().imag();
    if (effective_degree > coeffs.size() - 1) {
      (*imag)(imag->size() - 1) = 0;
    }
  }

  return true;
}

// Stolen from PoseLib implementation
/* Solves the quadratic equation a*x^2 + b*x + c = 0 */
inline double sign(const double z) { return z < 0 ? -1.0 : 1.0; }

int SolveQuadraticReal(double a, double b, double c, double roots[2]) {
  double b2m4ac = b * b - 4 * a * c;
  if (b2m4ac < 0) return 0;

  double sq = std::sqrt(b2m4ac);

  // Choose sign to avoid cancellations
  roots[0] = (b > 0) ? (2 * c) / (-b - sq) : (2 * c) / (-b + sq);
  roots[1] = c / (a * roots[0]);

  return 2;
}
void SolveCubicRealSingleRoot(double c2, double c1, double c0, double& root) {
  double a = c1 - c2 * c2 / 3.0;
  double b = (2.0 * c2 * c2 * c2 - 9.0 * c2 * c1) / 27.0 + c0;
  double c = b * b / 4.0 + a * a * a / 27.0;
  if (c > 0) {
    c = std::sqrt(c);
    b *= -0.5;
    root = std::cbrt(b + c) + std::cbrt(b - c) - c2 / 3.0;
  } else {
    c = 3.0 * b / (2.0 * a) * std::sqrt(-3.0 / a);
    root = 2.0 * std::sqrt(-a / 3.0) * std::cos(std::acos(c) / 3.0) - c2 / 3.0;
  }
}
/* Solves the quartic equation x^4 + b*x^3 + c*x^2 + d*x + e = 0 */
int SolveQuarticReal(double b, double c, double d, double e, double roots[4]) {
  // Find depressed quartic
  double p = c - 3.0 * b * b / 8.0;
  double q = b * b * b / 8.0 - 0.5 * b * c + d;
  double r =
      (-3.0 * b * b * b * b + 256.0 * e - 64.0 * b * d + 16.0 * b * b * c) /
      256.0;

  // Resolvent cubic is now
  // U^3 + 2*p U^2 + (p^2 - 4*r) * U - q^2
  double bb = 2.0 * p;
  double cc = p * p - 4.0 * r;
  double dd = -q * q;

  // Solve resolvent cubic
  double u2;
  SolveCubicRealSingleRoot(bb, cc, dd, u2);

  if (u2 < 0) return 0;

  double u = sqrt(u2);

  double s = -u;
  double t = (p + u * u + q / u) / 2.0;
  double v = (p + u * u - q / u) / 2.0;

  int sols = 0;
  double disc = u * u - 4.0 * v;
  if (disc > 0) {
    roots[0] = (-u - sign(u) * std::sqrt(disc)) / 2.0;
    roots[1] = v / roots[0];
    sols += 2;
  }
  disc = s * s - 4.0 * t;
  if (disc > 0) {
    roots[sols] = (-s - sign(s) * std::sqrt(disc)) / 2.0;
    roots[sols + 1] = t / roots[sols];
    sols += 2;
  }

  for (int i = 0; i < sols; i++) {
    roots[i] = roots[i] - b / 4.0;

    // do one step of newton refinement
    double x = roots[i];
    double x2 = x * x;
    double x3 = x * x2;
    double dx = -(x2 * x2 + b * x3 + c * x2 + d * x + e) /
                (4.0 * x3 + 3.0 * b * x2 + 2.0 * c * x + d);
    roots[i] = x + dx;
  }
  return sols;
}

}  // namespace colmap


#ifndef EXPSUM_REDUCTION_CHOLESKY_CAUCHY_HPP
#define EXPSUM_REDUCTION_CHOLESKY_CAUCHY_HPP

#include <armadillo>
#include <cassert>

namespace expsum
{
//
// Rank-revealing Cholesky decomposition for a positive-definite quasi-Cauchy
// matrix.
//
// This class computes the Cholesky decomposition of ``$n \times n$``
// quasi-Cauchy matrix defined as
//
// ``` math
//  C_{ij} = \frac{a_{i}^{} b_{j}^{}}{x_{i}^{} + y_{i}^{}}.
// ```
//
template <typename T>
struct cholesky_quasi_cauchy
{
public:
    using value_type = T;
    using real_type  = typename arma::get_pod_type<T>::result;
    using size_type  = arma::uword;

    using vector_type      = arma::Col<value_type>;
    using matrix_type      = arma::Mat<value_type>;
    using real_vector_type = arma::Col<real_type>;
    //
    // Pre-compute pivot order for the Cholesky factorization of Cauchy matrix.
    //
    static size_type pivot_order(vector_type& a, vector_type& b, vector_type& x,
                                 vector_type& y, real_type delta,
                                 arma::uvec& ipiv, vector_type& g);
    //
    // Compute Cholesky factors (`X` and diagonal part of `D`).
    //
    // The arrays `a,b,x,y` must be properly reordered by calling `pivot_order`
    // beforehand, so that the diagonal part of Cholesky factors appear in
    // decesnding order.
    //
    // @a vector of length ``$n$`` defining quasi-Cauchy matrix (reordered)
    // @b vector of length ``$n$`` defining quasi-Cauchy matrix (reordered)
    // @x vector of length ``$n$`` defining quasi-Cauchy matrix (reordered)
    // @y vector of length ``$n$`` defining quasi-Cauchy matrix (reordered)
    // @L Cholesky factor (lower triangular matrix)
    // @d diagonal elements of Cholesky factor ``$D$``
    // @alpha vector of length ``$n$`` as working space
    // @beta  vector of length ``$n$`` as working space
    //
    static void factorize(const vector_type& a, const vector_type& b,
                          const vector_type& x, const vector_type& y,
                          matrix_type& L, real_vector_type& d,
                          vector_type& alpha, vector_type& beta);
    //
    // Apply permutation matrix generated by previous decomposition *in-place.*
    //
    // @X $n \times k$ matrix that the row-permutation matrix is applied.
    // @ipiv vector of index with size $n$. Permutation index obtained by
    //       `pivot_order` or `pivot_order_sym`.
    // @work working space of size $n$.
    //
    template <typename MatX>
    static void apply_row_permutation(MatX& X, const arma::uvec& ipiv,
                                      vector_type& work)
    {
        const size_type n = ipiv.size();
        assert(X.n_rows == n && work.n_elem == n);

        for (size_type j = 0; j < X.n_cols; ++j)
        {
            for (size_type i = 0; i < n; ++i)
            {
                work(ipiv(i)) = X(i, j);
            }
            X.col(j) = work;
        }
    }
    //
    // Reconstruct matrix from Cholesky factor
    //
    // @X Cholesky factor computed by `cholesky_quasi_cauchy::run`.
    // @d Cholesky factor computed by `cholesky_quasi_cauchy::run`.
    //
    static matrix_type reconstruct(const matrix_type& X, const arma::uvec& ipiv,
                                   const real_vector_type& d)
    {
        matrix_type PX(X);
        vector_type work(X.n_rows);
        apply_row_permutation(PX, ipiv, work);

        return matrix_type(PX * arma::diagmat(arma::square(d)) * PX.t());
    }
};

//------------------------------------------------------------------------------
// Implementation of member functions
//------------------------------------------------------------------------------
template <typename T>
typename cholesky_quasi_cauchy<T>::size_type
cholesky_quasi_cauchy<T>::pivot_order(vector_type& a, vector_type& b,
                                      vector_type& x, vector_type& y,
                                      real_type delta, arma::uvec& ipiv,
                                      vector_type& g)
{
    const size_type n = a.size();
    assert(b.size() == n);
    assert(x.size() == n);
    assert(y.size() == n);
    assert(ipiv.size() == n);
    assert(g.size() == n);
    //
    // Set cutoff for GECP termination
    //
    const auto eta = arma::Datum<real_type>::eps * delta * delta;
    //
    // Form vector g(i) = a(i) * b(i) / (x(i) + y(i))
    //
    g = (a % b) / (x + y);
    //
    // Initialize permutation matrix
    //
    ipiv = arma::linspace<arma::uvec>(0, n - 1, n);

    size_type m = 0;
    while (m < n)
    {
        //
        // Find m <= l < n such that |g(l)| = max_{m<=k<n}|g(k)|
        //
        const auto l    = arma::abs(g.tail(n - m)).index_max() + m;
        const auto gmax = std::abs(g(l));

        if (gmax < eta)
        {
            break;
        }

        if (l != m)
        {
            // Swap elements
            std::swap(g(l), g(m));
            std::swap(a(l), a(m));
            std::swap(b(l), b(m));
            std::swap(x(l), x(m));
            std::swap(y(l), y(m));
            // Swap _rows_ of permutation matrix
            std::swap(ipiv(l), ipiv(m));
        }

        // Update diagonal of Schur complement
        const auto xm = x(m);
        const auto ym = y(m);
        for (size_type k = m + 1; k < n; ++k)
        {
            g(k) *= (x(k) - xm) * (y(k) - ym) / ((x(k) + ym) * (y(k) + xm));
        }
        ++m;
    }
    //
    // Returns the truncation size
    //
    return m;
}

template <typename T>
void cholesky_quasi_cauchy<T>::factorize(const vector_type& a,
                                         const vector_type& b,
                                         const vector_type& x,
                                         const vector_type& y, matrix_type& L,
                                         real_vector_type& d,
                                         vector_type& alpha, vector_type& beta)
{
    const auto n = L.n_rows;
    const auto m = L.n_cols;
    assert(a.size() == n);
    assert(b.size() == n);
    assert(x.size() == n);
    assert(y.size() == n);
    assert(d.size() == m);
    assert(alpha.size() == n);
    assert(beta.size() == n);

    alpha = a;
    beta  = b;

    L.zeros();
    for (size_type l = 0; l < n; ++l)
    {
        L(l, 0) = alpha(l) * beta(0) / (x(l) + y(0));
    }

    for (size_type k = 1; k < m; ++k)
    {
        // Upgrade generators
        const auto xkm1 = x(k - 1);
        const auto ykm1 = y(k - 1);
        for (size_type l = k; l < n; ++l)
        {
            alpha(l) *= (x(l) - xkm1) / (x(l) + ykm1);
            beta(l) *= (y(l) - ykm1) / (y(l) + xkm1);
        }
        // Extract k-th column for Cholesky factors
        for (size_type l = k; l < n; ++l)
        {
            L(l, k) = alpha(l) * beta(k) / (x(l) + y(k));
        }
    }
    //
    // Scale strictly lower triangular part of G
    //   - diagonal part of G contains D**2
    //   - L = tril(G) * D^{-2} + I
    //
    for (size_type j = 0; j < m; ++j)
    {
        const auto djj   = std::real(L(j, j));
        const auto scale = real_type(1) / djj;
        d(j)             = std::sqrt(djj);
        L(j, j) = real_type(1);
        for (size_type i = j + 1; i < n; ++i)
        {
            L(i, j) *= scale;
        }
    }

    return;
}

} // namespace: expsum

#endif /* EXPSUM_REDUCTION_CHOLESKY_CAUCHY_HPP */


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2009 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef SCHUR_SOLVER_HH
#define SCHUR_SOLVER_HH

/**
 * @file
 * @brief  Routines for the solution of (sparse) linear systems
 * @author Anton Schiela
 */

#include <iostream>
#include <memory> // std::unique_ptr

#include <boost/timer/timer.hpp>

#include "dune/common/fmatrix.hh"
#include "dune/istl/matrix.hh"

#include "linalg/umfpack_solve.hh"
#include "linalg/linearsystem.hh"
#include "linalg/simpleLAPmatrix.hh"
#include "algorithm/opt_interface.hh"
#include "algorithm/newton_bridge.hh"

namespace Kaskade
{

/**
 * \ingroup linalg
 * \brief Adapter class for DUNE::IterativeSolver
 */

void printVec(std::vector<double> const&v, int vend=1000000);

template<class Factorization=UMFFactorization<double> >
class BlockLUFactorization
{
public:
/// needs a matrix
  static const bool needMatrix = true;
template<class Sys>
BlockLUFactorization(Sys const& lin, int start2, int end2, int start3, int end3)
  {
    MatrixAsTriplet<double> matL;
    lin.getMatrixBlocks(matL,start3,end3,start2,end2);
    matA.resize(0);
    lin.getMatrixBlocks(matA,start2,end2,start2,end2);
    boost::timer::cpu_timer timer;
    factoredL.reset(new Factorization(matL.nrows(),
                                      2,
                                      matL.ridx,
                                      matL.cidx,
                                      matL.data,
                                      MatrixProperties::GENERAL));
  }

  void resetBlock22(MatrixAsTriplet<double>const & matA_) { matA = matA_; };

  void solve(std::vector<double>const& rhs, std::vector<double>& sol, int nr=1);
  MatrixAsTriplet<double> matA;
  std::unique_ptr<Factorization> factoredL;
};

void printVec(std::vector<double> const&v, int vend)
{
  int endv=v.size();
  if(vend < v.size()) endv=vend;
  for(int i=0; i< endv; ++i)
  {
    if(i%5 == 0)  std::cout << ".  " << v[i] << std::endl;
    else std::cout << "   " << v[i] << std::endl;

  }
}

template<class Factorization>
void BlockLUFactorization<Factorization>::solve(std::vector<double>const& rhs, std::vector<double>& sol, int nr)
{
  int r=rhs.size()/2/nr;
  std::vector<double> rhs1(r*nr);
  std::vector<double> rhs2(r*nr);
  std::vector<double> sol1(r*nr);
  std::vector<double> sol2(r*nr);
  for(int i=0; i<nr; ++i)
  for(int j=0; j<r; ++j)
  {
    rhs1[i*r+j]=rhs[i*2*r+j];
    rhs2[i*r+j]=-rhs[i*2*r+r+j];
  }
  factoredL->solve(rhs2,sol2,false);  // solve system
  //  factoredL->solve(rhs2,sol2,nr,false);  // solve system
  matA.axpy(rhs1,sol2,1.0,nr);
  factoredL->solve(rhs1,sol1,true);  //solve transposed system
  // factoredL->solve(rhs1,sol1,nr,true);  //solve transposed system
  sol.resize(2*r*nr);
  for(int i=0; i<nr; ++i)
  for(int j=0; j<r; ++j)
  {
    sol[i*2*r+j]=-sol2[i*r+j];
    sol[i*2*r+r+j]=sol1[i*r+j];
  }
};

struct BlockSchurParameters
{
  BlockSchurParameters(bool reg_)
    : refactorizeInner(true), refactorizeOuter(true)
  {
    if(reg_) regularizationMethod=AddId; else regularizationMethod=None;
  }

  BlockSchurParameters(bool reg_, bool innerf_, bool outerf_)
    : refactorizeInner(innerf_), refactorizeOuter(outerf_)
  {
    if(reg_) regularizationMethod=AddId; else regularizationMethod=None;
  }

  bool refactorizeInner, refactorizeOuter;
  typedef enum { None=0, AddId=1, IterateType::CG = 2} RegularizationMethod;

  RegularizationMethod regularizationMethod;
};

template<class Factorization, class VariableSet>
class DirectBlockSchurSolver : public AbstractNormalDirection, public Dune::Preconditioner<typename VariableSet::Descriptions::template CoefficientVectorRepresentation<>::type,
 typename VariableSet::Descriptions::template CoefficientVectorRepresentation<>::type>
{
public:
  typedef typename VariableSet::Descriptions::template CoefficientVectorRepresentation<>::type Domain;
  typedef Domain Range;

  int start1, end1, start2, end2, start3, end3;

/// needs a matrix
  static const bool needMatrix = true;

  DirectBlockSchurSolver(bool doregularize = false) :
    start1(0), end1(1),
    start2(1), end2(2),
    start3(2), end3(3),
    report(false), paras(doregularize)
  {}

  virtual void pre(Domain &x, Range &b) {}

  virtual void apply (Domain &v, const Range &d) {

    std::vector<double> r(rows1),s(rows2),t(rows3), sol1(rows1+rows2+rows3);

    d.write(sol1.begin());

    for(int i=0; i<rows1; ++i) r[i]=sol1[i];
    for(int i=0; i<rows2; ++i) s[i]=sol1[i+rows1];
    for(int i=0; i<rows3; ++i) t[i]=sol1[i+rows1+rows2];

    resolveN(sol1,r,s,t);

    v.read(sol1.begin());
  }
  virtual void post (Domain &x) {}

  void onChangedLinearization() {flushFactorization(); }

  void flushFactorization()
  {
    if(factorization.get() && paras.refactorizeInner) factorization.reset();
    mC.setSize(0,0);
    B.resize(0);
    AinvB.resize(0);
    matANormal.flush();
  }

  bool report;

  void resetParameters(BlockSchurParameters const& p_) { paras=p_; }

  virtual void computeCorrectionAndAdjointCorrection(AbstractVector& correction, AbstractVector& adjointCorrection, AbstractLinearization& linearization)
  {
    SparseLinearSystem& lins=dynamic_cast<SparseLinearSystem &>(linearization);
    flushFactorization();
    buildNewSchurComplement(lins);

    std::vector<double> r,s,t, sol1, sol2;

    lins.getRHSBlocks(r,start1,end1);
    lins.getRHSBlocks(s,start2,end2);
    lins.getRHSBlocks(t,start3,end3);

    std::vector<double> r0(r.size(),0.0),s0(s.size(),0.0),t0(t.size(),0.0);
    resolveN(sol1,r0,s0,t);
    resolveN(sol2,r,s,t0);

    dynamic_cast<Bridge::Vector<VariableSet>& >(correction).read(sol1);
    dynamic_cast<Bridge::Vector<VariableSet>& >(adjointCorrection).read(sol2);
    correction *= -1.0;
    adjointCorrection *= -1.0;
  }

  virtual void computeSimplifiedCorrection(AbstractVector& correction, AbstractLinearization const& lin) const
  {
    SparseLinearSystem const& lins=dynamic_cast<SparseLinearSystem const&>(lin);

    std::vector<double> t, sol1;
    lins.getRHSBlocks(t,start3,end3);
    std::vector<double> r0(rows1,0.0),s0(rows2,0.0);
    resolveN(sol1,r0,s0,t);
    dynamic_cast<Bridge::Vector<VariableSet>& >(correction).read(sol1);
    correction *= -1.0;
  }

private:

  void resolveN(std::vector<double>& sol, std::vector<double>const &rhs0,std::vector<double>const &rhs1, std::vector<double>const &rhs2) const;

  void fwd(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const;
  void bwd(std::vector<double>& sol, std::vector<double>const &x2,std::vector<double>const &s,std::vector<double>const &t) const;

  void buildNewSchurComplement(SparseLinearSystem const& lin,int task);

  std::unique_ptr<BlockLUFactorization<Factorization> > factorization; //
  MatrixAsTriplet<double> matANormal;
  Dune::Matrix<Dune::FieldMatrix<double,1,1> > mC,mCNormal;
  std::vector<double> B,AinvB;

  int rowsB, colsBC, rowsC;
  int rows1, rows2, rows3;

  BlockSchurParameters paras;

};

//  UU UY BU* ...  C B* B*
//  YU YY AY* ...  B A  A
//  BU AY 00  ...  B A  A
//  .. .. ..  ...
template<class Factorization, class VariableSet>
void DirectBlockSchurSolver<Factorization, VariableSet>::buildNewSchurComplement(SparseLinearSystem const& lin,int task=0)
{
    boost::timer::cpu_timer timer;
    if(report) std::cout << "Schur Complement: " << std::flush;

    MatrixAsTriplet<double> matB, matC;
    rowsB=lin.rows(start2,end3);
    colsBC=lin.cols(start1,end1);
    rowsC=lin.rows(start1,end1);
    if(task==0)
    {
      rows1=lin.rows(start1,end1);
      rows2=lin.rows(start2,end2);
      rows3=lin.rows(start3,end3);
    }

    if(paras.refactorizeOuter || !factorization.get() || B.size()==0)
    {
      if(report) std::cout << "Outer, " << std::flush;
      if(task==0)
      {
        flushFactorization();
      }

      if((task==0 && paras.refactorizeInner) || !factorization.get())
      {
        factorization.reset(new BlockLUFactorization<Factorization>(lin,start2,end2,start3,end3));
        lin.getMatrixBlocks(matANormal,start2,end2, start2, end2);
      }
      else
      {
        MatrixAsTriplet<double> matA;
        lin.getMatrixBlocks(matA,start2,end2, start2, end2);
        factorization->resetBlock22(matA);
      }

// Matrix = [C B^T; B A]; rhs=[r2, r1]; sol=[x_2,x_1]
// Compute sol via the Schur complement
// B is stored in column-first formal in a vector B(i,j)=B[i+j*rowsB]
      if(task==0 || B.size()==0)
      {
        matB.resize(0);
        lin.getMatrixBlocks(matB,start2,end3, start1, end1);
        B.resize(0);
        matB.toVector(B);
      }

// Compute A^{-1}B
      AinvB.resize(0);
      factorization->solve(B,AinvB,colsBC);

    }

// Compute C as a full matrix
    matC.resize(0);
    lin.getMatrixBlocks(matC,start1,end1, start1, end1);
    mC.setSize(rowsC, colsBC);
    mC=0.0;
    matC.addToMatrix(mC);

// Compute S := B^T A^{-1}B-C
    for(int i=0; i<rowsC; ++i)
      for(int j=0; j<colsBC; ++j)
      {
        mC[i][j] *= -1.0;
        for(int k=0; k<rowsB; ++k) mC[i][j] += B[i*rowsB+k]*AinvB[j*rowsB+k];
      }
    if(task==0)
    {
      mCNormal.setSize(rowsC, colsBC);
      mCNormal = mC;
    }
    if(report) std::cout << "Finished: " << (double)(timer.elapsed().user)/1e9 << " sec." << std::endl;
}

template<class Factorization, class VariableSet>
void DirectBlockSchurSolver<Factorization,VariableSet>::resolveN(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const
{
  std::vector<double> xx2;

  fwd(xx2,r,s,t);

  std::vector<double> x2(xx2.size(),0.0);

  // Compute x2 := S^{-1}xx_2

  LeastSquares(SLAPMatrix<double>(mCNormal), xx2, x2);

  bwd(sol,x2,s,t);
}

template<class Factorization, class VariableSet>
void DirectBlockSchurSolver<Factorization,VariableSet>::fwd(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const
{
    std::vector<double> xx1, r1;

    r1.reserve(s.size()+t.size());

    for(int i=0; i<s.size();++i) r1.push_back(s[i]);
    for(int i=0; i<t.size();++i) r1.push_back(t[i]);
// Compute xx1=A^{-1}r1

    xx1.resize(r1.size());
    factorization->solve(r1,xx1);

// Compute sol := -r2+B^T xx1

    sol.resize(r.size());
    for(int i=0; i<r.size(); ++i)
    {
      sol[i]=-r[i];
      for(int k=0; k<xx1.size(); ++k)
        sol[i]+=B[i*rowsB+k]*xx1[k];
    }
}

template<class Factorization, class VariableSet>
void DirectBlockSchurSolver<Factorization,VariableSet>::bwd
(std::vector<double>& sol, std::vector<double>const &x2,std::vector<double>const &s,std::vector<double>const &t) const
{
  std::vector<double> xx1,x1;

   xx1.reserve(s.size()+t.size());

    for(int i=0; i<s.size();++i) xx1.push_back(s[i]);
    for(int i=0; i<t.size();++i) xx1.push_back(t[i]);

// Compute xx1 := r1-B x2
    for(int i=0; i<xx1.size(); ++i)
      for(int k=0; k<x2.size(); ++k)
        xx1[i]-=B[k*rowsB+i]*x2[k];

// Compute x1 = A^{-1}xx1
    factorization->solve(xx1,x1);

// sol=[x2;x1]
    sol.reserve(x2.size()+x1.size());//x2.size()+x1.size());
    sol.resize(x2.size()+x1.size(),0.0);
    int k=0;
    for(int i=0; i< x2.size(); ++i)
    {
      sol[k]=x2[i];
      ++k;
    }
    for(int i=0; i< x1.size(); ++i)
    {
      sol[k]=x1[i];
      ++k;
    }
}

}  // namespace Kaskade
#endif


#ifndef BAYES_CLASSIFIER_HPP
#define BAYES_CLASSIFIER_HPP

#include <cmath>

#include <utility>
#include <vector>

#include <boost/math/constants/constants.hpp>

namespace osrm
{
namespace engine
{
namespace map_matching
{

struct NormalDistribution
{
    NormalDistribution(const double mean, const double standard_deviation)
        : mean(mean), standard_deviation(standard_deviation)
    {
    }

    // FIXME implement log-probability version since it's faster
    double Density(const double val) const
    {
        using namespace boost::math::constants;

        const double x = val - mean;
        return 1.0 / (std::sqrt(two_pi<double>()) * standard_deviation) *
               std::exp(-x * x / (standard_deviation * standard_deviation));
    }

    double mean;
    double standard_deviation;
};

struct LaplaceDistribution
{
    LaplaceDistribution(const double location, const double scale)
        : location(location), scale(scale)
    {
    }

    // FIXME implement log-probability version since it's faster
    double Density(const double val) const
    {
        const double x = std::abs(val - location);
        return 1.0 / (2. * scale) * std::exp(-x / scale);
    }

    double location;
    double scale;
};

template <typename PositiveDistributionT, typename NegativeDistributionT, typename ValueT>
class BayesClassifier
{
  public:
    enum class ClassLabel : unsigned
    {
        NEGATIVE,
        POSITIVE
    };
    using ClassificationT = std::pair<ClassLabel, double>;

    BayesClassifier(PositiveDistributionT positive_distribution,
                    NegativeDistributionT negative_distribution,
                    const double positive_apriori_probability)
        : positive_distribution(std::move(positive_distribution)),
          negative_distribution(std::move(negative_distribution)),
          positive_apriori_probability(positive_apriori_probability),
          negative_apriori_probability(1. - positive_apriori_probability)
    {
    }

    // Returns label and the probability of the label.
    ClassificationT classify(const ValueT &v) const
    {
        const double positive_postpriori =
            positive_apriori_probability * positive_distribution.Density(v);
        const double negative_postpriori =
            negative_apriori_probability * negative_distribution.Density(v);
        const double norm = positive_postpriori + negative_postpriori;

        if (positive_postpriori > negative_postpriori)
        {
            return std::make_pair(ClassLabel::POSITIVE, positive_postpriori / norm);
        }

        return std::make_pair(ClassLabel::NEGATIVE, negative_postpriori / norm);
    }

  private:
    PositiveDistributionT positive_distribution;
    NegativeDistributionT negative_distribution;
    double positive_apriori_probability;
    double negative_apriori_probability;
};
}
}
}

#endif // BAYES_CLASSIFIER_HPP


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2020 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* SVFMM <-> Python data type conversions                                   *
***************************************************************************/

#ifndef PYSVFMM_HPP
#define PYSVFMM_HPP

#define cimg_display 0
#include <CImg.h>
#include <Eigen/Dense>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>
#include <SpatiallyVariantFiniteMixtureModel.hpp>
#include <Distributions/MultivariateNormal.hpp>
#include <Distributions/MultivariateTStudent.hpp>
#include <Distributions/Gamma.hpp>
#include <MarkovRandomFields/MarkovRandomField.hpp>
#include <MarkovRandomFields/FrequentistGaussMarkovRandomField.hpp>
#include <MarkovRandomFields/FrequentistTStudentMarkovRandomField.hpp>
#include <MarkovRandomFields/FrequentistNonLocalMarkovRandomField.hpp>
#include <MarkovRandomFields/BayesianGaussMarkovRandomField.hpp>
#include <MarkovRandomFields/BayesianTStudentMarkovRandomField.hpp>
#include <MarkovRandomFields/BayesianNonLocalMarkovRandomField.hpp>
#include <PyCImg.hpp>
#include <PySTL.hpp>
#include <cstdint>
#include <stdexcept>
#include <sstream>
#include <string>
#include <omp.h>

namespace py = pybind11;
using namespace cimg_library;
using namespace Eigen;

template<typename T>
using ndarray = py::array_t<T, py::array::c_style | py::array::forcecast>;

template<typename Distribution, int Dimensions>
ndarray<double> muToNumpy(const SpatiallyVariantFiniteMixtureModel<Distribution, Dimensions> *svfmm)
{
    const int K = svfmm->components();
    const int D = svfmm->spectrum();

    ndarray<double> output({K, D});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        for (int j = 0; j < D; ++j)
        {
            ptr[j * K + i] = (*svfmm)[i].mu()(j);
        }
    }

    return output;
}

template<typename Distribution, int Dimensions>
ndarray<double> sigmaToNumpy(const SpatiallyVariantFiniteMixtureModel<Distribution, Dimensions> *svfmm)
{
    const int K = svfmm->components();
    const int D = svfmm->spectrum();

    ndarray<double> output({D, D, K});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        for (int j = 0; j < D; ++j)
        {
            for (int k = 0; k < D; ++k)
            {
                ptr[(i * D * D) + (k * D) + j] = (*svfmm)[i].sigma()(j, k);
            }
        }
    }
    return output;
}

template<int Dimensions>
ndarray<double> nuToNumpy(const SpatiallyVariantFiniteMixtureModel<MultivariateTStudent, Dimensions> *svfmm)
{
    const int K = svfmm->components();

    ndarray<double> output({K});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        ptr[i] = (*svfmm)[i].nu();
    }

    return output;
}

template<int Dimensions>
ndarray<double> kToNumpy(const SpatiallyVariantFiniteMixtureModel<Gamma, Dimensions> *svfmm)
{
    const int K = svfmm->components();

    ndarray<double> output({K});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        ptr[i] = (*svfmm)[i].k();
    }

    return output;
}

template<int Dimensions>
ndarray<double> thetaToNumpy(const SpatiallyVariantFiniteMixtureModel<Gamma, Dimensions> *svfmm)
{
    const int K = svfmm->components();

    ndarray<double> output({K});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        ptr[i] = (*svfmm)[i].theta();
    }

    return output;
}

/***************************** Python SVFMM Base *****************************/

class PySVFMMBase
{
public:
    enum Mode { SEGMENTATION, MIXTURE, COMPLETE };
};

/***************************** Python SVFMM *****************************/

template<typename Distribution, int Dimensions>
class PySVFMM : public PySVFMMBase
{
public:
    static py::dict toPython(const SpatiallyVariantFiniteMixtureModel<Distribution, Dimensions> *svfmm, const CImg<bool> &mask, const PySVFMMBase::Mode mode);
};

template<typename Distribution, int Dimensions>
py::dict PySVFMM<Distribution, Dimensions>::toPython(const SpatiallyVariantFiniteMixtureModel<Distribution, Dimensions> *svfmm, const CImg<bool> &mask, const PySVFMMBase::Mode mode)
{
    std::stringstream s;
    s << "In function " << __PRETTY_FUNCTION__ << " ==> Function must be specialized for each distribution type." << std::endl;
    throw std::runtime_error(s.str());

    return py::dict();
}

/***************************** Template Specialization Multivariate Normal *****************************/

template<int Dimensions>
class PySVFMM<MultivariateNormal, Dimensions> : public PySVFMMBase
{
public:
    static py::dict toPython(const SpatiallyVariantFiniteMixtureModel<MultivariateNormal, Dimensions> *svfmm, const CImg<bool> &mask, const PySVFMMBase::Mode mode);
};

template<int Dimensions>
py::dict PySVFMM<MultivariateNormal, Dimensions>::toPython(const SpatiallyVariantFiniteMixtureModel<MultivariateNormal, Dimensions> *svfmm, const CImg<bool> &mask, const PySVFMMBase::Mode mode)
{
    py::dict output = py::dict();

    if (mode == SEGMENTATION || mode == MIXTURE || mode == COMPLETE)
    {
        output["segmentation"] = PyCImg::toNumpy(EigenCImg::toCImg(svfmm->labels().array() + 1, mask));
    }

    if (mode == MIXTURE || mode == COMPLETE)
    {
        output["mu"] = muToNumpy(svfmm);
        output["sigma"] = sigmaToNumpy(svfmm);
        output["priors"] = PyCImg::toNumpy(EigenCImg::toCImg(svfmm->coefficients(), mask));
        output["posteriors"] = PyCImg::toNumpy(EigenCImg::toCImg(svfmm->posteriorProbabilities(), mask));
        output["loglikelihood"] = PySTL::toNumpy(svfmm->logLikelihoodHistory());

    }

    if (mode == COMPLETE)
    {
        py::dict mrf = py::dict();

        const MarkovRandomFieldBase::Model model = svfmm->spatialCoefficients().get()->model();
        const MarkovRandomFieldBase::Tropism tropism = svfmm->spatialCoefficients().get()->tropism();
        const MarkovRandomFieldBase::Topology topology = svfmm->spatialCoefficients().get()->topology();
        const MarkovRandomFieldBase::Estimation estimation = svfmm->spatialCoefficients().get()->estimation();

        mrf["cliques"] = svfmm->spatialCoefficients().get()->cliques();
        mrf["connectivity"] = svfmm->spatialCoefficients().get()->connectivity();
        mrf["nodes"] = svfmm->spatialCoefficients().get()->nodes();
        mrf["classes"] = svfmm->spatialCoefficients().get()->classes();
        mrf["model"] = model == MarkovRandomFieldBase::GAUSS ? "gaussian" : (MarkovRandomFieldBase::TSTUDENT ? "tstudent" : "nonlocal");
        mrf["tropism"] = tropism == MarkovRandomFieldBase::ISOTROPIC ? "isotropic" : "anisotropic";
        mrf["topology"] = topology == MarkovRandomFieldBase::ORTHOGONAL ? "orthogonal" : "complete";
        mrf["estimation"] = estimation == MarkovRandomFieldBase::FREQUENTIST ? "frequentist" : "bayesian";

        const MarkovRandomField<Dimensions> *ptr_ = svfmm->spatialCoefficients().get();

        // Gauss
        if (model == MarkovRandomFieldBase::GAUSS && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianIsotropicGaussMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianIsotropicGaussMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
        }
        if (model == MarkovRandomFieldBase::GAUSS && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistIsotropicGaussMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistIsotropicGaussMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
        }
        if (model == MarkovRandomFieldBase::GAUSS && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianAnisotropicGaussMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianAnisotropicGaussMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
        }
        if (model == MarkovRandomFieldBase::GAUSS && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistAnisotropicGaussMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistAnisotropicGaussMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
        }
        // T-student
        if (model == MarkovRandomFieldBase::TSTUDENT && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianIsotropicTStudentMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianIsotropicTStudentMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
        }
        if (model == MarkovRandomFieldBase::TSTUDENT && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistIsotropicTStudentMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistIsotropicTStudentMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
        }
        if (model == MarkovRandomFieldBase::TSTUDENT && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianAnisotropicTStudentMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianAnisotropicTStudentMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
        }
        if (model == MarkovRandomFieldBase::TSTUDENT && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistAnisotropicTStudentMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistAnisotropicTStudentMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
        }
        // Non-Local
        if (model == MarkovRandomFieldBase::NONLOCAL && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianIsotropicNonLocalMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianIsotropicNonLocalMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
            mrf["patch_size"] = ptr->patchSize();
            mrf["chi2_sigma"] = ptr->chi2Sigma();
        }
        if (model == MarkovRandomFieldBase::NONLOCAL && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistIsotropicNonLocalMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistIsotropicNonLocalMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
            mrf["patch_size"] = ptr->patchSize();
            mrf["chi2_sigma"] = ptr->chi2Sigma();
        }
        if (model == MarkovRandomFieldBase::NONLOCAL && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianAnisotropicNonLocalMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianAnisotropicNonLocalMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
            mrf["patch_size"] = ptr->patchSize();
            mrf["chi2_sigma"] = ptr->chi2Sigma();
        }
        if (model == MarkovRandomFieldBase::NONLOCAL && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistAnisotropicNonLocalMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistAnisotropicNonLocalMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
            mrf["patch_size"] = ptr->patchSize();
            mrf["chi2_sigma"] = ptr->chi2Sigma();
        }

        output["mrf"] = mrf;
    }

    return output;
}

#endif


#include <boost/lexical_cast.hpp>
#include <dai/alldai.h>
#include <iostream>
#include <fstream>
#include <string>
#include "dai/emrun.h"
#include <dai/util.h>
#include <string>
#include <sys/stat.h>
#include <time.h>
#include <boost/tokenizer.hpp>
#include <iomanip>      // std::setprecision
#include <sstream>
#include <string>
#include <cstdlib>
#include<math.h>
using namespace std;
using namespace dai;
using namespace boost;
std::string& trim(std::string& s, const char* t = " \t\n\r\f\v")
{
	s.erase(0, s.find_first_not_of(t));
	s.erase(s.find_last_not_of(t) + 1);
	return s;
}
double euclideanDist(double v1[],double v2[],int length)
{
    double distance=0;
    for(int i=0;i<length;i++)
    {
        distance = distance+ pow((v1[i]-v2[i]),2); //Euclidean distance
        //cout<<v1[i]<<" "<<v2[i]<<" "<<distance;
    }
   // cout<<"**** "<<distance<<endl;
    distance = sqrt(distance);
    return distance;
}
double klDist(double v1[],double v2[],int length)
{
    double distance=0;
    for(int i=0;i<length;i++)
    {
        distance = distance + (v1[i]* log0( v1[i]/v2[i])); // KL distance
       // cout<<( v1[i]/v2[i])<<" "<<(v1[i]* std::log( v1[i]/v2[i]))<<endl;
       // cout<<distance<<"+";
    }
    //cout<<"="<<distance<<endl;
    return distance;
}
void calculateDistance(string child, string child1)
{

        const char* childfgfile = &child[0];
        const char* childfgfile1 = &child1[0];
        //cout<<"child file "<< childfgfile<<endl;
        string line1,line2;
        ifstream p1 (childfgfile);
        ifstream c1 (childfgfile1);
        getline(p1,line1);
        getline(c1,line2);

        int factor =0;
        int no_factors = 0;
        int* factor_names;
        int factor_pos=0;
        int* states_arr;
        int no_states = 1;
        int arr_size =1;
        int factor_states =0;
        int tot_arrays =0;
        int arr_set =0;
        int arr_set_states =0;
        int no_arr_arrset =0;
        int stat =0;
        int p_c = 0;
        double pmut=0,pm=0,distval=0;
        while(getline(p1,line1)&&getline(c1,line2))
        {
            line1 = trim(line1);
            line2 = trim(line2);
            //cout<<p_c<<" "<<pmut<<" "<<pm<<" "<<line1<<endl;
            stringstream ss(line1);
            stringstream ss1(line2);
            if(line1.empty())
            {
                p_c = 0;
                stat =0;
                //cout<<"factor variable "<<factor<<endl;
                //seed();
                //pmut = unifRand(); // generates random a value between 0 and 1 for child1
                //cout<<"Mutation probability "<<pmut<<endl;
                no_states = 1;
                arr_size =1;
                factor++;
            }
            if(pmut>pm)
            {
                //c1<<line1<<endl;
                //cout<<"No mutation"<<endl;
                //p_c++;
                //continue;
            }
            if(pmut<=pm) //do mutation handling the constraint that the states of the random variable sum to 1.
            {
                //cout<<"Mutation happening "<<endl;
                double sumo=0,sumo1=0;
                if(p_c == 0)
                {
                    //c1<<line1<<endl;
                }
                if(p_c == 1)
                {
                    ss>>no_factors;
                    //cout<<"no. of factors "<<no_factors<<endl;
                    //c1<<no_factors<<endl;
                }
                if(p_c == 2)
                {
                    factor_names = new int[no_factors];
                    std::vector<std::string> fields;
                    fields = tokenizeString( line1, true,"  ");
                    for( size_t i = 0; i < fields.size(); ++i )
                    {
                        stringstream n;
                        n << fields[i];
                        string s = n.str();
                        if( s.find_first_not_of(" ")!= std::string::npos)
                        {
                            //c1<<s<<" ";
                            n >> factor_names[i];
                            if(factor_names[i] == (factor-1))
                                factor_pos =i;
                        }
                    }
                    //c1<<endl;
                  //  cout<<"factor position "<<factor_pos<<endl;
                }
                if(p_c == 3)
                {
                    states_arr = new int[no_factors];
                    std::vector<std::string> fields1;
                    fields1 = tokenizeString( line1, true,"  ");
                    for( size_t j = 0; j < fields1.size(); ++j )
                    {
                        stringstream n1;
                        n1 << fields1[j];
                        string state = n1.str();
                        if( state.find_first_not_of(" ")!= std::string::npos)
                        {
                            //c1<<state<<" ";
                            n1 >> states_arr[j];
                            no_states = no_states * states_arr[j];
                            if(j < factor_pos)
                                arr_size = arr_size * states_arr[j];
                            if(j == factor_pos)
                            {
                                factor_states = states_arr[j];
                                arr_set_states = arr_size*states_arr[j];
                            }
                        }
                    }
                    //c1<<endl;
                    tot_arrays = no_states/arr_size;
                    arr_set = no_states/arr_set_states;
                    no_arr_arrset = factor_states;
                    //cout<<"array size "<<arr_size<<endl;
                   // cout<<no_states<<"\t"<<"no. of states "<<no_states<<"\t"<<"total arrays "<<tot_arrays<<"\t"<<"array set "<<arr_set<<"\t"<<"no. arrays "<<no_arr_arrset<<"\t"<<"factor states "<<factor_states<<endl;
                    //c1<<no_states<<endl;
                }
                if(p_c >= 5)
                {
                    int k=0;
                    while(k<arr_set)
                    {
                        std::vector< double * > Arrays, Arrays1;
                        for(int l=0; l<no_arr_arrset; l++)
                        {
                            Arrays.push_back( new double[arr_size]);
                            Arrays1.push_back( new double[arr_size]);
                        }
                        vector<double*>::iterator it2,it21;
                        it2=Arrays.begin();
                        it21=Arrays1.begin();
                        //get the current cpt values and copy them in the array
                        int l=0;
                        while(l<no_arr_arrset)
                        {
                            double* arr = *it2;
                            double* arr1 = *it21;
                           // cout<<"new arr"<<endl;
                            int m=0;
                            while(m<arr_size)
                            {
                                line1 = trim(line1);
                                line2 = trim(line2);
                                //cout<<line1 <<" "<<line2<<endl;
                                std::vector<std::string> values,values1;
//                                values = tokenizeString( line1, true," +");
//                                values1 = tokenizeString( line2, true," +");
                                char_separator<char> sep("                 ");
                                tokenizer<char_separator<char>> tokens(line1, sep);
                                for (const auto& t : tokens) {
                                    //cout<<t;
                                    if(t!=" ")
                                    {
                                        values.push_back(t);
                                        // cout<<"pushed "<<t<<endl;
                                    }
                                }
                                //cout<<endl;
                                tokenizer<char_separator<char>> tokens1(line2, sep);
                                for (const auto& t : tokens1) {
                                    //cout<<t;
                                    if(t!=" ")
                                    {
                                        values1.push_back(t);
                                        //cout<<"pushed "<<t<<endl;
                                    }
                                }
                                int ii=0;
                                //cout<<values.size()<<" "<<values1.size()<<endl;
                                for( size_t j = 0; j < values.size(); ++j )
                                {
                                    stringstream n1,n2;
                                    n1 << values[j];
                                    n2 << values1[j];
                                    string state = n1.str();
                                    string state1 = n2.str();
                                    //cout<<state<<" !! "<<state1<<" !! "<<endl;
                                    if( state.find_first_not_of(" ")!= std::string::npos && state1.find_first_not_of(" ")!= std::string::npos)
                                    {
                                        if(ii==1)
                                        {
                                            n1 >> arr[m];
                                            n2 >> arr1[m];
                                            //cout<<p_c<<" "<<line1<<" "<<"arrval = "<<values[0]<<"\tarr[m]"<<arr[m]<<" m = "<<m<<endl;
                                            m++;
                                            if(m<arr_size)
                                            {
                                             getline(p1,line1);
                                             getline(c1,line2);
                                            }
                                            ii=0;
                                        }
                                        else
                                            ii=1;
                                    } //end of if
                                } // end of for
                            } // end of while -m
                            l++;
                            if(l<no_arr_arrset)
                            {
                                getline(p1,line1);
                                getline(c1,line2);
                            }
                            ++it2; ++it21;
                        } //end of while -l
                       // cout<<"new val"<<endl;
                        vector<double*>::iterator it,it1;
                        double sumarr[arr_size];
                        for(int m=0; m<arr_size; m++)
                        {
                            it=Arrays.begin();
                            it1=Arrays1.begin();
                            double valarr[no_arr_arrset],valarr1[no_arr_arrset];
                            for(int l=0; l<no_arr_arrset; l++) // stdnormal rand values are generated and sum is found
                            {
                                double* arr = *it;
                                double* arr1 = *it1;
                                double val = arr[m];
                                double val1 =  arr1[m];
                                valarr[l]=val;
                                valarr1[l]=val1;
                                cout<<val<<" "<<val1<<endl;
                                ++it; ++it1;
                            }

                            sumo+=euclideanDist(valarr,valarr1,no_arr_arrset);
                            sumo1+=klDist(valarr,valarr1,no_arr_arrset);
                           // cout<<endl;
                        }
                        k++;
                        if(k<arr_set)
                         {
                             getline(p1,line1);
                             getline(c1,line2);
                         }
                    }// end of while -k
                    cout<<"******"<<no_states<<" "<<factor<<" "<<sumo<<" "<<sumo1<<" "<<endl;
                    distval = distval+sumo;
                }// end of if p_c>=5

            } // end of if pmut<p_m
            p_c++;
        } // end of while loop
        c1.close();
    p1.close();
    cout<<distval<<endl;
}
// Type for storing a joint state of all variables
int main(int argc, char *argv[]) {
time_t timer1, timer2;
time(&timer1);
clock_t tStart = clock();
    string child,child1;
//    if(argc.length!=2){
//     child =  "/Applications/Utilities/libDAI-0.3.1/alarmnew_factor_graphs/alarm1l.fg";
//     child1 =  "/Applications/Utilities/libDAI-0.3.1/alarmnew_factor_graphs/alarm2l.fg";
//    }
//    else
    {
        stringstream ss2(argv[1]);
        child = ss2.str();
        stringstream ss3(argv[2]);
        child1 = ss3.str();
    }
    calculateDistance(child,child1);

time(&timer2);
double t = difftime(timer2,timer1);
(clock() - tStart)/CLOCKS_PER_SEC;
double t_c = (clock() - tStart)/CLOCKS_PER_SEC;
//cout<<"Time taken: "<<t<<"s"<<endl;
//cout<<" Processor Time taken: "<<t_c<<"s"<<endl;

    return 0;
}


#pragma once

#include <set>

#include <Eigen/Eigenvalues>

#include "CompressedPauliString.hpp"
#include "Operator.hpp"

#include "../utils.hpp"

namespace yavque
{

namespace detail
{

	bool commute(const std::map<uint32_t, Pauli>& p1,
	             const std::map<uint32_t, Pauli>& p2);
	std::string extract_pauli_string(const std::map<uint32_t, Pauli>& pmap);

	class SumPauliStringImpl
	{
	public:
		using PauliString = std::map<uint32_t, Pauli>;

	private:
		const uint32_t num_qubits_;
		std::vector<PauliString> pauli_strings_;
		mutable std::vector<std::shared_ptr<detail::CompressedPauliString>> cps_;

		void update_cps() const
		{
			for(std::size_t n = cps_.size(); n < pauli_strings_.size(); ++n)
			{
				cps_.emplace_back(CPSFactory::get_instance().get_pauli_string_for(
					extract_pauli_string(pauli_strings_[n])));
			}
		}

	public:
		explicit SumPauliStringImpl(uint32_t num_qubits) : num_qubits_{num_qubits} { }

		explicit SumPauliStringImpl(uint32_t num_qubits,
		                            std::vector<PauliString> pauli_strings)
			: num_qubits_{num_qubits}, pauli_strings_{std::move(pauli_strings)}
		{
			// check sites pauli strings applied < num_qubits
		}

		explicit SumPauliStringImpl(uint32_t num_qubits,
		                            std::vector<PauliString>&& pauli_strings)
			: num_qubits_{num_qubits}, pauli_strings_{std::move(pauli_strings)}
		{
		}

		uint32_t num_qubits() const { return num_qubits_; }

		void add(const std::map<uint32_t, Pauli>& rhs)
		{
			// check sites pauli strings applied < num_qubits
			pauli_strings_.push_back(rhs);
		}

		void add(std::map<uint32_t, Pauli>&& rhs)
		{
			// check sites pauli strings applied < num_qubits
			pauli_strings_.emplace_back(std::move(rhs));
		}

		bool mutually_commuting() const
		{
			for(uint32_t i = 0; i < pauli_strings_.size() - 1; ++i)
			{
				for(uint32_t j = i + 1; j < pauli_strings_.size(); ++j)
				{
					if(!commute(pauli_strings_[i], pauli_strings_[j]))
					{
						return false;
					}
				}
			}
			return true;
		}

		Eigen::VectorXcd apply(const Eigen::VectorXcd& vec) const
		{
			assert(vec.size() == (1U << num_qubits_));

			if(cps_.size() < pauli_strings_.size())
			{
				update_cps();
			}

			Eigen::VectorXcd res = Eigen::VectorXcd::Zero(vec.size());
			for(std::size_t n = 0; n < pauli_strings_.size(); ++n)
			{
				const auto& pauli_string = pauli_strings_[n];
				std::vector<uint32_t> indices;
				std::transform(pauli_string.cbegin(), pauli_string.cend(),
				               std::back_inserter(indices),
				               [](const auto& p) { return p.first; });
				res += cps_[n]->apply(indices, vec);
			}
			return res;
		}

		/* this function makes sense only when operators are mutually commuting
		 *
		 * */
		Eigen::VectorXcd apply_exp(cx_double t, const Eigen::VectorXcd& vec) const
		{
			assert(vec.size() == (1U << num_qubits_));

			if(cps_.size() < pauli_strings_.size())
			{
				update_cps();
			}

			Eigen::VectorXcd res = vec;
			for(std::size_t n = 0; n < pauli_strings_.size(); ++n)
			{
				const auto& pauli_string = pauli_strings_[n];
				std::vector<uint32_t> indices;
				std::transform(pauli_string.cbegin(), pauli_string.cend(),
				               std::back_inserter(indices),
				               [](const auto& p) { return p.first; });
				res = cps_[n]->apply_exp(t, indices, res);
			}
			return res;
		}
	};
} // namespace detail

class SumPauliString final : public Operator
{
public:
	using PauliString = std::map<uint32_t, Pauli>;

private:
	std::shared_ptr<const detail::SumPauliStringImpl> p_;
	cx_double constant_ = 1.0;

	void dagger_in_place_impl() override { constant_ = std::conj(constant_); }

public:
	explicit SumPauliString(const uint32_t num_qubits, const std::string& name = {})
		: Operator(1U << num_qubits, name),
		  p_{std::make_shared<detail::SumPauliStringImpl>(num_qubits)}
	{
	}

	explicit SumPauliString(const uint32_t num_qubits,
	                        const std::vector<std::map<uint32_t, Pauli>>& pauli_strings,
	                        const std::string& name = {})
		: Operator(1U << num_qubits, name),
		  p_{std::make_shared<detail::SumPauliStringImpl>(num_qubits, pauli_strings)}
	{
	}

	explicit SumPauliString(std::shared_ptr<const detail::SumPauliStringImpl> p,
	                        const std::string& name = {}, cx_double constant = 1.0)
		: Operator(1U << p->num_qubits(), name), p_{std::move(p)}, constant_{constant}
	{
	}

	[[nodiscard]] bool mutually_commuting() const { return p_->mutually_commuting(); }

	[[nodiscard]] std::shared_ptr<const detail::SumPauliStringImpl> get_impl() const
	{
		return p_;
	}
	/* This function might be slow.
	 */
	SumPauliString& operator+=(const PauliString& str)
	{
		auto p = std::make_shared<detail::SumPauliStringImpl>(*p_);
		p->add(str);
		p_ = std::move(p);
		return *this;
	}

	SumPauliString& operator+=(PauliString&& str)
	{
		auto p = std::make_shared<detail::SumPauliStringImpl>(*p_);
		p->add(std::move(str));
		p_ = std::move(p);
		return *this;
	}

	SumPauliString(const SumPauliString&) = default;
	SumPauliString(SumPauliString&&) = default;

	SumPauliString& operator=(const SumPauliString&) = delete;
	SumPauliString& operator=(SumPauliString&&) = delete;

	~SumPauliString() override = default;

	[[nodiscard]] std::unique_ptr<Operator> clone() const override
	{
		auto cloned = std::make_unique<SumPauliString>(*this);
		return cloned;
	}

	[[nodiscard]] Eigen::VectorXcd apply_right(const Eigen::VectorXcd& st) const override
	{
		return constant_ * p_->apply(st);
	}
};

} // namespace yavque


/// My typedefs
#include <inttypes.h>
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>

typedef int32_t    node_t;
typedef int32_t    edge_t;
typedef int64_t    cost_t;

/// From STL library
#include <vector>
using std::vector;

#include <string>

using std::pair;
using std::make_pair;

/// Label for the labeling and/or dijkstra algorithm
enum Label { UNREACHED, LABELED, SCANNED };

/// Data structure to store Key-Value pairs in a
/// PriorityQueue (as a Fibonacci heap)
struct ValueKey {
   cost_t d;
   node_t u;
   ValueKey(cost_t _d, node_t _u)
      : d(_d), u(_u) {}
   /// The relation establishes the order in the PriorityQueue
   inline bool operator<(ValueKey const & rhs) const { return d < rhs.d; }
};

#include <boost/heap/fibonacci_heap.hpp>
typedef boost::heap::fibonacci_heap<ValueKey>  FibonacciHeap;

#include <boost/heap/d_ary_heap.hpp>
typedef boost::heap::d_ary_heap<ValueKey, boost::heap::arity<2>, boost::heap::mutable_<true> >  BinaryHeap;

#include <boost/heap/d_ary_heap.hpp>
typedef boost::heap::d_ary_heap<ValueKey, boost::heap::arity<3>, boost::heap::mutable_<true> >  TernaryHeap;

#include <boost/heap/skew_heap.hpp>
typedef boost::heap::skew_heap<ValueKey, boost::heap::mutable_<true> >  SkewHeap;

#include <boost/heap/pairing_heap.hpp>
typedef boost::heap::pairing_heap<ValueKey>  PairingHeap;

#include <boost/heap/binomial_heap.hpp>
typedef boost::heap::binomial_heap<ValueKey>  BinomialHeap;

/// Simple Arc class: store tuple (i,j,c)
class Arc {
   public:
      node_t    w;  /// Target node
      cost_t    c;  /// Cost of the arc
      /// Standard constructor
      Arc ( node_t _w, cost_t _c )
         : w(_w), c(_c) {}
};

/// Forward and Backward star: intrusive list
typedef std::vector<Arc>                 FSArcList;
typedef FSArcList::iterator              FSArcIter;

///--------------------------------------------------------------------------------
/// Class of graph to compute RCSP with superadditive cost
class Digraph {
   private:
      node_t  n;
      edge_t  m;

      vector<FSArcList>  Nc;   /// Nodes container

      /// Initialize distance vector with Infinity
      /// Maybe it is better to intialize with an upper bound on the optimal path (optimal rcsp path)
      const cost_t Inf;

   public:
      ///Standard constructor
      Digraph( node_t _n, edge_t _m )
         : n(_n), m(_m), Inf(std::numeric_limits<cost_t>::max())
      {
         assert( n < Inf && m < Inf );
         Nc.reserve(n);
         /// Reserve memory for the set of arcs
         int avg_degree = m/n+1;
         for ( int i = 0; i < n; ++i ) {
            FSArcList tmp;
            tmp.reserve(avg_degree);
            Nc.push_back(tmp);
         }
      }

      void addArc( node_t i, node_t j, cost_t c ) {
         Nc[i].push_back( Arc(j, c) );
      }

      ///--------------------------------------------------
      /// Shortest Path for a graph with positive weights
      /// With a Fibonacci Heap, as given in the book "Algorithms" by Vazirani et all.
      /// NOTE: since 'increase' is O(1), while 'decrease' is O(log n)
      /// We use negative distances in the heap, i.e., we start with distance labels set to -\infinity
      /// However, the distance labels are kept with the correct value
      template <typename PriorityQueue>
      cost_t spp ( node_t S, node_t T, vector<node_t>& P ) {
         typedef typename PriorityQueue::handle_type     handle_t;

         PriorityQueue     H;
         vector<handle_t>  K(n);
         vector<Label>     Q(n,UNREACHED);  /// true if it is in the heap

         /// Initialize the source distance
         //D[S] = 0;
         K[S] = H.push( ValueKey(0,S) );
         while ( !H.empty() ) {
            /// u = deleteMin(H)
            ValueKey p = H.top();
            H.pop();
            node_t u  = p.u;
            Q[u] = SCANNED;
            cost_t Du = -(*K[u]).d;
            if ( u == T ) { break; }
            /// for all edges (u, v) \in E
            for ( FSArcIter it = Nc[u].begin(), it_end = Nc[u].end(); it != it_end; ++it ) {
               node_t v   = it->w;
               if ( Q[v] != SCANNED ) {
                  cost_t Duv = it->c;
                  cost_t Dv  = Du + Duv;
                  if ( Q[v] == UNREACHED ) {
                     P[v] = u;
                     Q[v] = LABELED;
                     K[v] = H.push( ValueKey(-Dv,v) );
                  } else {
                     if ( -(*K[v]).d > Dv ) {
                        P[v] = u;
                        H.increase( K[v], ValueKey(-Dv,v) );
                     }
                  }
               }
            }
         }
         assert( R[T] == SCANNED );
         return -(*K[T]).d;
      }
};

/// Boost Timer
#include <boost/progress.hpp>
using boost::timer;

#include <fstream>

using namespace boost;

/// Read input data, build graph, and run Dijkstra
cost_t runDijkstra( char* argv[] ) {
   /// Read instance from the OR-lib
   std::ifstream infile(argv[1]);
   if (!infile)
      exit ( EXIT_FAILURE );

   int n;     /// Number of variables
   int m;     /// Number of constraints

   // reads file of the form
   // #nodes #edges
   // e_1 = v_i v_j cost[e_m]
   // ..
   // e_m = v_i v_j cost[e_m]

   /// Read the first line
   infile >> n >> m;
   fprintf(stdout,"n %d, m %d\n", n, m);
   /// Build the graph
   Digraph G (n, m);

   int v, w;
   cost_t c;
   for ( int i = 0; i < m; i++ ) {
      infile >> v >> w >> c;
      G.addArc(v-1, w-1, c);
   }

   vector<node_t> P(n);
   cost_t T_dist;

   timer TIMER;
   for ( int i = 0; i < 50; ++i ) {
      double t0 = TIMER.elapsed();
      node_t S = i;
      node_t T = n-1-i;
      T_dist = G.spp<BinaryHeap>(S, T, P);
      fprintf(stdout,"Time %.4f Cost %"PRId64"\n", TIMER.elapsed()-t0, T_dist);
   }
   fprintf(stdout,"Tot %.4f\n", TIMER.elapsed());

   return T_dist;
}

///------------------------------------------------------------------------------------------
/// Main function
int
main (int argc, char **argv)
{
   if ( argc != 2 ) {
      fprintf(stdout, "usage: ./dijkstra <filename>\n");
      exit ( EXIT_FAILURE );
   }
   /// Measure overall time
   timer TIMER;
   /// Invoke the different Dijkstra algorithm implementations
   cost_t T_dist = runDijkstra(argv);
   /// Print basic figures
   fprintf(stdout,"Cost %"PRId64" - Time %.3f\n", T_dist, TIMER.elapsed());

   return 0;
}


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include <ctime>
#include "bayesopt/bayesopt.h"                 // For the C API
#include "bayesopt/bayesopt.hpp"               // For the C++ API
#include <boost/numeric/ublas/assignment.hpp> // <<= op assigment

/* Function to be used for C-API testing */
double testFunction(unsigned int n, const double *x,
		    double *gradient, /* NULL if not needed */
		    void *func_data)
{
  double f = 10.;
  for (unsigned int i = 0; i < n; ++i)
    {
      f += (x[i] - .53) * (x[i] - .53);
    }
  return f;
}

/* Class to be used for C++-API testing */
class ExampleQuadratic: public bayesopt::ContinuousModel
{
 public:

  ExampleQuadratic(size_t dim,bayesopt::Parameters param):
    ContinuousModel(dim,param) {}

  double evaluateSample( const vectord &Xi )
  {
    double x[100];
    for (size_t i = 0; i < Xi.size(); ++i)
      {
	x[i] = Xi(i);
      }
    return testFunction(Xi.size(),x,NULL,NULL);
  };

  bool checkReachability( const vectord &query )
  { return true; };

};

int main(int nargs, char *args[])
{
  int n = 10;                   // Number of dimensions
  clock_t start, end;
  double diff,diff2;

  // Common configuration
  // See parameters.h for the available options.
  // Some parameters did not need to be changed for default, but we have done it for
  // illustrative purpose.
  bayesopt::Parameters par = initialize_parameters_to_default();

  par.kernel.name = "kSum(kSEISO,kConst)";
  par.kernel.hp_mean <<= 1.0, 1.0;
  par.kernel.hp_std <<= 1.0, 1.0;

  par.mean.name = "mConst";
  par.mean.coef_mean <<= 1.0;
  par.mean.coef_std <<= 1.0;

  par.surr_name = "sStudentTProcessJef";
  par.noise = 1e-10;

  par.sc_type = SC_MAP;
  par.l_type = L_EMPIRICAL;

  par.n_iterations = 100;    // Number of iterations
  par.random_seed = 0;
  par.n_init_samples = 15;
  par.n_iter_relearn = 0;

  /*******************************************/
  std::cout << "Running C++ interface" << std::endl;

  ExampleQuadratic opt(n,par);
  vectord result(n);

  // Run C++ interface
  start = clock();
  opt.optimize(result);
  end = clock();
  diff = (double)(end-start) / (double)CLOCKS_PER_SEC;

  /*******************************************/
  std::cout << "Running C inferface" << std::endl;

  // Prepare C interface
  double low[128], up[128], xmin[128], fmin[128];

  // Lower and upper bounds
  for (int i = 0; i < n; ++i)
    {
      low[i] = 0.;
      up[i] = 1.;
    }

  // Run C interface
  start = clock();
  bayes_optimization(n,&testFunction,NULL,low,up,xmin,fmin,par.generate_bopt_params());
  end = clock();
  diff2 = (double)(end-start) / (double)CLOCKS_PER_SEC;
  /*******************************************/

  // Results
  std::cout << "Final result C++: " << result << std::endl;
  std::cout << "Elapsed time in C++: " << diff << " seconds" << std::endl;

  std::cout << "Final result C: [" << n <<"](" << xmin[0];
  for (int i = 1; i < n; ++i )
    {
      std::cout << "," << xmin[i];
    }
  std::cout << ")" << std::endl;
  std::cout << "Elapsed time in C: " << diff2 << " seconds" << std::endl;

}


#include <NTL/mat_lzz_pE.h>

#include <NTL/new.h>

NTL_START_IMPL

void add(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

void sub(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix sub: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         sub(X(i,j), A(i,j), B(i,j));
}

void negate(mat_zz_pE& X, const mat_zz_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         negate(X(i,j), A(i,j));
}

void mul_aux(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   long i, j, k;
   zz_pX acc, tmp;

   for (i = 1; i <= n; i++) {
      for (j = 1; j <= m; j++) {
         clear(acc);
         for(k = 1; k <= l; k++) {
            mul(tmp, rep(A(i,k)), rep(B(k,j)));
            add(acc, acc, tmp);
         }
         conv(X(i,j), acc);
      }
   }
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   if (&X == &A || &X == &B) {
      mat_zz_pE tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

static
void mul_aux(vec_zz_pE& x, const mat_zz_pE& A, const vec_zz_pE& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i, k;
   zz_pX acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, rep(A(i,k)), rep(b(k)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_zz_pE& x, const mat_zz_pE& A, const vec_zz_pE& b)
{
   if (&b == &x || A.position1(x) != -1) {
      vec_zz_pE tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_zz_pE& x, const vec_zz_pE& a, const mat_zz_pE& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   zz_pX acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, rep(a(k)), rep(B(k,i)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_zz_pE& x, const vec_zz_pE& a, const mat_zz_pE& B)
{
   if (&a == &x) {
      vec_zz_pE tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);

}

void ident(mat_zz_pE& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

void determinant(zz_pE& d, const mat_zz_pE& M_in)
{
   long k, n;
   long i, j;
   long pos;
   zz_pX t1, t2;
   zz_pX *x, *y;

   const zz_pXModulus& p = zz_pE::modulus();

   n = M_in.NumRows();

   if (M_in.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   UniqueArray<vec_zz_pX> M_store;
   M_store.SetLength(n);
   vec_zz_pX *M = M_store.get();

   for (i = 0; i < n; i++) {
      M[i].SetLength(n);
      for (j = 0; j < n; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   zz_pX det;
   set(det);

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1))
            pos = i;
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], p);

         // make M[k, k] == -1 mod p, and make row k reduced

         InvMod(t1, M[k][k], p);
         negate(t1, t1);
         for (j = k+1; j < n; j++) {
            rem(t2, M[k][j], p);
            MulMod(M[k][j], t2, t1, p);
         }

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            for (j = k+1; j < n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   conv(d, det);
}

long IsIdent(const mat_zz_pE& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_zz_pE& X, const mat_zz_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_zz_pE tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

void solve(zz_pE& d, vec_zz_pE& X,
           const mat_zz_pE& A, const vec_zz_pE& b)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      set(d);
      X.SetLength(0);
      return;
   }

   long i, j, k, pos;
   zz_pX t1, t2;
   zz_pX *x, *y;

   const zz_pXModulus& p = zz_pE::modulus();

   UniqueArray<vec_zz_pX> M_store;
   M_store.SetLength(n);
   vec_zz_pX *M = M_store.get();

   for (i = 0; i < n; i++) {
      M[i].SetLength(n+1);
      for (j = 0; j < n; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(A[j][i]);
      }
      M[i][n].rep.SetMaxLength(2*deg(p)-1);
      M[i][n] = rep(b[i]);
   }

   zz_pX det;
   set(det);

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], p);

         // make M[k, k] == -1 mod p, and make row k reduced

         InvMod(t1, M[k][k], p);
         negate(t1, t1);
         for (j = k+1; j <= n; j++) {
            rem(t2, M[k][j], p);
            MulMod(M[k][j], t2, t1, p);
         }

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            for (j = k+1; j <= n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   X.SetLength(n);
   for (i = n-1; i >= 0; i--) {
      clear(t1);
      for (j = i+1; j < n; j++) {
         mul(t2, rep(X[j]), M[i][j]);
         add(t1, t1, t2);
      }
      sub(t1, t1, M[i][n]);
      conv(X[i], t1);
   }

   conv(d, det);
}

void inv(zz_pE& d, mat_zz_pE& X, const mat_zz_pE& A)
{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("inv: nonsquare matrix");

   if (n == 0) {
      set(d);
      X.SetDims(0, 0);
      return;
   }

   long i, j, k, pos;
   zz_pX t1, t2;
   zz_pX *x, *y;

   const zz_pXModulus& p = zz_pE::modulus();

   UniqueArray<vec_zz_pX> M_store;
   M_store.SetLength(n);
   vec_zz_pX *M = M_store.get();

   for (i = 0; i < n; i++) {
      M[i].SetLength(2*n);
      for (j = 0; j < n; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(A[i][j]);
         M[i][n+j].rep.SetMaxLength(2*deg(p)-1);
         clear(M[i][n+j]);
      }
      set(M[i][n+i]);
   }

   zz_pX det;
   set(det);

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], p);

         // make M[k, k] == -1 mod p, and make row k reduced

         InvMod(t1, M[k][k], p);
         negate(t1, t1);
         for (j = k+1; j < 2*n; j++) {
            rem(t2, M[k][j], p);
            MulMod(M[k][j], t2, t1, p);
         }

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            for (j = k+1; j < 2*n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   X.SetDims(n, n);
   for (k = 0; k < n; k++) {
      for (i = n-1; i >= 0; i--) {
         clear(t1);
         for (j = i+1; j < n; j++) {
            mul(t2, rep(X[j][k]), M[i][j]);
            add(t1, t1, t2);
         }
         sub(t1, t1, M[i][n+k]);
         conv(X[i][k], t1);
      }
   }

   conv(d, det);
}

long gauss(mat_zz_pE& M_in, long w)
{
   long k, l;
   long i, j;
   long pos;
   zz_pX t1, t2, t3;
   zz_pX *x, *y;

   long n = M_in.NumRows();
   long m = M_in.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   const zz_pXModulus& p = zz_pE::modulus();

   UniqueArray<vec_zz_pX> M_store;
   M_store.SetLength(n);
   vec_zz_pX *M = M_store.get();

   for (i = 0; i < n; i++) {
      M[i].SetLength(m);
      for (j = 0; j < m; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   l = 0;
   for (k = 0; k < w && l < n; k++) {

      pos = -1;
      for (i = l; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         swap(M[pos], M[l]);

         InvMod(t3, M[l][k], p);
         negate(t3, t3);

         for (j = k+1; j < m; j++) {
            rem(M[l][j], M[l][j], p);
         }

         for (i = l+1; i < n; i++) {
            // M[i] = M[i] + M[l]*M[i,k]*t3

            MulMod(t1, M[i][k], t3, p);

            clear(M[i][k]);

            x = M[i].elts() + (k+1);
            y = M[l].elts() + (k+1);

            for (j = k+1; j < m; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(t2, t2, *x);
               *x = t2;
            }
         }

         l++;
      }
   }

   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         conv(M_in[i][j], M[i][j]);

   return l;
}

long gauss(mat_zz_pE& M)
{
   return gauss(M, M.NumCols());
}

void image(mat_zz_pE& X, const mat_zz_pE& A)
{
   mat_zz_pE M;
   M = A;
   long r = gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

void kernel(mat_zz_pE& X, const mat_zz_pE& A)
{
   long m = A.NumRows();
   long n = A.NumCols();

   mat_zz_pE M;
   long r;

   transpose(M, A);
   r = gauss(M);

   X.SetDims(m-r, m);

   long i, j, k, s;
   zz_pX t1, t2;

   zz_pE T3;

   vec_long D;
   D.SetLength(m);
   for (j = 0; j < m; j++) D[j] = -1;

   vec_zz_pE inverses;
   inverses.SetLength(m);

   j = -1;
   for (i = 0; i < r; i++) {
      do {
         j++;
      } while (IsZero(M[i][j]));

      D[j] = i;
      inv(inverses[j], M[i][j]);
   }

   for (k = 0; k < m-r; k++) {
      vec_zz_pE& v = X[k];
      long pos = 0;
      for (j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k)
               set(v[j]);
            else
               clear(v[j]);
            pos++;
         }
         else {
            i = D[j];

            clear(t1);

            for (s = j+1; s < m; s++) {
               mul(t2, rep(v[s]), rep(M[i][s]));
               add(t1, t1, t2);
            }

            conv(T3, t1);
            mul(T3, T3, inverses[j]);
            negate(v[j], T3);
         }
      }
   }
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, const zz_pE& b_in)
{
   zz_pE b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, const zz_p& b_in)
{
   NTL_zz_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, long b_in)
{
   NTL_zz_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void diag(mat_zz_pE& X, long n, const zz_pE& d_in)
{
   zz_pE d = d_in;
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_zz_pE& A, long n, const zz_pE& d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

long IsZero(const mat_zz_pE& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_zz_pE& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_zz_pE operator+(const mat_zz_pE& a, const mat_zz_pE& b)
{
   mat_zz_pE res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_zz_pE, res);
}

mat_zz_pE operator*(const mat_zz_pE& a, const mat_zz_pE& b)
{
   mat_zz_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_zz_pE, res);
}

mat_zz_pE operator-(const mat_zz_pE& a, const mat_zz_pE& b)
{
   mat_zz_pE res;
   sub(res, a, b);
   NTL_OPT_RETURN(mat_zz_pE, res);
}

mat_zz_pE operator-(const mat_zz_pE& a)
{
   mat_zz_pE res;
   negate(res, a);
   NTL_OPT_RETURN(mat_zz_pE, res);
}

vec_zz_pE operator*(const mat_zz_pE& a, const vec_zz_pE& b)
{
   vec_zz_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_zz_pE, res);
}

vec_zz_pE operator*(const vec_zz_pE& a, const mat_zz_pE& b)
{
   vec_zz_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_zz_pE, res);
}

void inv(mat_zz_pE& X, const mat_zz_pE& A)
{
   zz_pE d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

void power(mat_zz_pE& X, const mat_zz_pE& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_zz_pE T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

NTL_END_IMPL