#ifndef SPARSEMATRIX_HPP_1BWMMLC8
#define SPARSEMATRIX_HPP_1BWMMLC8

#include <algorithm>
#include <cassert>
#include <fstream>
#include <iterator>
#include <map>
#include <stdexcept>
#include <tuple>
#include <unordered_map>
#include <vector>
#include <vector>
#include <iostream>

#include <Eigen/Sparse>

namespace cask {
  namespace sparse {

    using EigenSparseMatrix = Eigen::SparseMatrix<double, Eigen::RowMajor, int32_t>;

    template<typename value_type>
  class SparkCooMatrix {

        public:
        using CoordType = std::tuple<int, int, value_type>;

        int n, m;
        std::vector<CoordType> data;

        SparkCooMatrix(int _n, int _m) : n(_n), m(_m) {}
      };
  }

// A dense vector representation
class Vector {
public:
  std::vector<double> data;

  Vector(int n) : data(n, 0) {}
  Vector(std::initializer_list<double> l) : data(l) {}
  Vector(const std::vector<double>& v) : data(v.begin(), v.end()) {}

  Vector operator-(const Vector& other) const {
    int n = size();
    if (other.size() != data.size()) {
      throw std::invalid_argument("Attempt to subtract vectors of different lengths: " +
                                  std::to_string(other.size()) + " != " + std::to_string(n));
    }
    Vector v(n);
    for (int i = 0; i < n; i++) {
      v[i] = data[i] - other[i];
    }
    return v;
  }

  int size() const {
    return data.size();
  }

  bool operator==(const Vector& other) const {
    return data == other.data;
  }

  const double& operator[](int i) const {
    return data[i];
  }

  double& operator[](int i) {
    return data[i];
  }

  void print(std::string label="") const {
    std::cout << label;
    for (int i : data)
      std::cout << i << " ";
    std::cout << std::endl;
  }

  double norm() const {
    double residual;
    for (double d : data)
      residual += d * d;
    return std::sqrt(residual);
  }

  double distance(const Vector& exp) const {
    return (*this - exp).norm();
  }

  void writeToFile(std::string path) {
    std::ofstream f{path};
    if (!f)
      throw std::invalid_argument("Could not open file for writing");
    for (auto v : data)
      f << v << std::endl;
  }
};

/**
 * Sparse matrix representations for some common storage formats:
 * - DoK (Dictionary of Keys) -- the de facto format for all/many construction tasks; random read/write access is
 * efficient, but memory footprint is large
 * - CSR -- an efficient format for when only row slicing operations are required; random read access is supported but inefficient,
 * random write access is only supported in update mode (cannot insert new nonzeros), memory footprint is minimal
 *
 * Some underlying assumptions that are respected by all matrix formats:
 *
 * - all storage is 0 indexed; it is incredibly awkward and error-prone to support 1-based indexing in a language like C++;
 * - for interfacing with certain codes that do not support 0 based indexing (e.g. if using BLAS from Fortran), some
 * formats provide a getIndexed() method to obtain the corresponding representation in 1 based indexing. Read the specific
 * of each method to understand the storage format and prevent bugs when interfacing with low-level C/Fortran APIs
 *
 * TODOs
 * - a consistent, safe mechanism for handling symmetric matrices?
 */
class DokMatrix {

 public:
  int n, m;
  int nnzs;

  // use of std::map important: values are sorted by column index internaly
  std::unordered_map<int, std::map<int, double>> dok;

  DokMatrix() : n(0), m(0), nnzs(0) {}

  DokMatrix(int _n, int m) : n(_n), m(m), nnzs(0) {}

  DokMatrix(int _n, int m, int _nnzs) : n(_n), m(m), nnzs(_nnzs) {}

  // Initialize the matrix as a dense matrix, with the given values, in order;
  // matrix is assumed square with n = sqrt(pattern.size()); 0 entries must be
  // entered explicitly
  DokMatrix(const std::initializer_list<double>& pattern) :
      DokMatrix(floor((sqrt(pattern.size()))), pattern) { }

  // Initialize the matrix as a dense matrix, with the given values, in order;
  // matrix is assumed of size n rows by pattern.size() / n columns;
  // 0 entries must be entered explicitly
  DokMatrix(int n, const std::initializer_list<double>& pattern) : n(n) {
    nnzs = std::count_if(pattern.begin(), pattern.end(), [](double x){return x != 0;});
    auto it = pattern.begin();
    m = pattern.size() / n;
    for (int i = 0; i < n; i++)
      for (int j = 0; j < m; j++) {
        double value = *it;
        if (value != 0) {
          dok[i][j] = value;
        }
        it++;
      }
  }

  DokMatrix explicitSymmetric() {
    int newNnzs = 0;
    DokMatrix m(n, this->m);
    for (auto &&s : dok) {
      for (auto &&p : s.second) {
        int i = s.first;
        int j = p.first;
        double value = p.second;
        m.dok[i][j] = value;
        newNnzs++;

        // check the transpose entry does not exist
        // TODO replace dok.find with dok.at
        if (i == j)
          continue;
        if (dok.find(j) != dok.end()) {
          if (dok[j].find(i) != dok[j].end()) {
            if (dok[j][i] != p.second) {
              throw std::invalid_argument("Matrix is not symmetric");
            } else {
              std::cout << "Warning! Matrix already contains transpose entry for "
                        << s.first << " " << p.first << std::endl;
            }
          }
        }

        // add the transpose entry
        m.dok[j][i] = value;
        newNnzs++;
      }
    }
    m.nnzs = newNnzs;
    return m;
  }

  bool operator==(const DokMatrix& other) const {
    return n == other.n && m == other.m && nnzs == other.nnzs && dok == other.dok;
  }

  void pretty_print() const {
    for (int i = 0; i < n; i++) {
      for (int j = 0; j < m; j++) {
        std::cout << at(i, j) << " ";
      }
      std::cout << "\n";
    }
  }

  double at(int i, int j) const {
    assert(i < n && j < m);
    if (dok.count(i) == 0)
      return 0;
    if (dok.at(i).count(j) == 0)
      return 0;
    return dok.at(i).at(j);
  }

  void set(int i, int j, double val) {
    assert(i < n && j < m);
    dok[i][j] = val;
    nnzs++;
  }

  bool isNnz(int i, int j) const {
    if (dok.count(i) == 0)
      return false;
    if (dok.at(i).count(j) == 0)
        return false;
    return dok.at(i).at(j) != 0;
  }

  DokMatrix getLowerTriangular() const {
    DokMatrix lowerTriangular(n, m);
    for (auto &e : dok) {
      for (auto &ee : e.second) {
        int i = e.first;
        int j = ee.first;
        double value = ee.second;
        if (j <= i)
          lowerTriangular.set(i, j, value);
      }
    }
    return lowerTriangular;
  }

  DokMatrix getUpperTriangular() const {
    DokMatrix lowerTriangular(n, m);
    for (auto &e : dok) {
      for (auto &ee : e.second) {
        int i = e.first;
        int j = ee.first;
        double value = ee.second;
        if (i <= j)
          lowerTriangular.set(i, j, value);
      }
    }
    return lowerTriangular;
  }

  Vector dot(const Vector& b) const {
    Vector result(b.size());
    for (auto &p : dok) {
      int row = p.first;
      for (auto &e : p.second) {
        result[row] += b[e.first] * e.second;
      }
    }
    return result;
  }

};

// TODO verify preconditions:
// with 1 based indexing
// lower triangular
// row entries in increasing column order
class CsrMatrix {
  // TODO move to include/, make API
 public:
  int n, m;
  int nnzs;
  std::vector<double> values;
  std::vector<int> col_ind;
  std::vector<int> row_ptr;

  CsrMatrix() : n(0), m(0), nnzs(0) {}

  CsrMatrix(std::initializer_list<double> mat) : CsrMatrix(DokMatrix(mat)){
  }

  CsrMatrix(int n, std::initializer_list<double> mat) : CsrMatrix(DokMatrix(n, mat)){
  }

  CsrMatrix(const DokMatrix &m) {
    nnzs = m.nnzs;
    this->n = m.n;
    this->m = m.m;
    int pos = 0;
    for (int i = 0; i < n; i++) {
      row_ptr.push_back(pos);
      if (m.dok.count(i) != 0) {
        for (auto &entries : m.dok.at(i)) {
          col_ind.push_back(entries.first);
          values.push_back(entries.second);
          pos++;
        }
      }
    }
    row_ptr.push_back(nnzs);
  }

  CsrMatrix(int _n, int _m, int _nnzs, double *_values, int *_col_ind, int *_row_ptr) :
      n(_n), m(_m),
      nnzs(_nnzs) {
    values.assign(_values, _values + _nnzs);
    col_ind.assign(_col_ind, _col_ind + _nnzs);
    row_ptr.assign(_row_ptr, _row_ptr + n + 1);
  }

  CsrMatrix(int n, int m, int nnzs,
            const std::vector<double> &values,
            const std::vector<int> &col_ind,
            const std::vector<int> &row_ptr) :
      n(n), m(m), nnzs(nnzs), values(values), col_ind(col_ind), row_ptr(row_ptr) {}

  // Prints all matrix values
  void pretty_print() const {
    for (int i = 0; i < n; i++) {
      int col_ptr = row_ptr[i];
      for (int j = 0; j < n; j++) {
        if (col_ptr < row_ptr[i + 1] && col_ind[col_ptr] == j) {
          std::cout << values[col_ptr] << " ";
          col_ptr++;
          continue;
        }
        std::cout << "0 ";
      }
      std::cout << "\n";
    }
  }

  void print() {
    std::cout << "CSRMatrix( n= " << n << " nnzs= " << nnzs << ")" << std::endl;
    std::cout << "values = ";
    std::copy(values.begin(), values.end(), std::ostream_iterator<double>{std::cout, " "});
    std::cout << std::endl;
    std::cout << "col_ind = ";
    std::copy(col_ind.begin(), col_ind.end(), std::ostream_iterator<double>{std::cout, " "});
    std::cout << std::endl;
    std::cout << "row_ptr = ";
    std::copy(row_ptr.begin(), row_ptr.end(), std::ostream_iterator<double>{std::cout, " "});
    std::cout << std::endl;
  }

  double &get(int i, int j) {
    for (int k = row_ptr[i]; k < row_ptr[i + 1]; k++) {
      if (col_ind[k] == j) {
        return values[k];
      }
    }

    throw std::invalid_argument("No nonzero at row col:"
                                    + std::to_string(i) + " "
                                    + std::to_string(j));
  }

  bool isNnz(int i, int j) {
    // TODO efficiency could be improved
    try {
      get(i, j);
    } catch (std::invalid_argument) {
      return false;
    }
    return true;
  }

  bool isSymmetric() const {
    return true;
  }

  DokMatrix toDok() const {
    DokMatrix m(n, this->m, nnzs);
    for (int i = 0; i < n; i++) {
      for (int k = row_ptr[i]; k < row_ptr[i + 1]; k++) {
        m.dok[i][col_ind[k]] = values[k];
      }
    }
    return m;
  }

  bool operator==(const CsrMatrix& o) const {
    return n == o.n &&
        m == o.m &&
        nnzs == o.nnzs &&
        values == o.values &&
        row_ptr == o.row_ptr &&
        col_ind == o.col_ind;
  }

  std::vector<int> getRowPtrWithOneBasedIndex() const {
    std::vector<int> rowIndexed;
    std::transform(
        row_ptr.begin(), row_ptr.end(), std::back_inserter(rowIndexed),
        [](int x){return x + 1;}
    );
    return rowIndexed;
  }

  std::vector<int> getColIndWithOneBasedIndex() const {
    std::vector<int> colIndexed;
    std::transform(
        col_ind.begin(), col_ind.end(), std::back_inserter(colIndexed),
        [](int x){return x + 1;}
    );
    return colIndexed;

  }

  CsrMatrix getLowerTriangular() const {
    return CsrMatrix(toDok().getLowerTriangular());
  }

  CsrMatrix getUpperTriangular() const {
    return CsrMatrix(toDok().getUpperTriangular());
  }

  Vector dot(const Vector& b) const {
    return toDok().dot(b);
  }

  CsrMatrix sliceRows(int startRow, int nRows) const {
    //startPos = row_ptr[startRow];
    std::vector<int> newRowPtr, newColInd;
    std::vector<double> newValues;
    int nnzs = 0;
    int offset = row_ptr[startRow];
    for (int i = startRow; i < startRow + nRows; i++) {
      int r = row_ptr[i];
      newRowPtr.push_back(r - offset);
      for (int j = r; j < row_ptr[i + 1]; j++) {
        newValues.push_back(values[j]);
        newColInd.push_back(col_ind[j]);
        nnzs++;
      }
    }
    newRowPtr.push_back(newValues.size());
    return CsrMatrix(nRows, this->m, nnzs, newValues, newColInd, newRowPtr);
  }

  /** Slices columns from this matrix into blocks. Each block has as
      many rows as this matrix but at most blockSize columns.  All
      blocks except the last one have exactly blockSize columns.

      Example:

      This matrix:
      1 2 3 4
      5 6 7 8
      sliceColumns(3) returns:
      1 2 3 | 4
      5 6 7 | 8
  */
  std::vector<cask::CsrMatrix> sliceColumns(int blockSize) const {
    // XXX constructing a CSR matrix this way is not safe because m and n are not updated
    int nBlocks = m / blockSize + (m % blockSize == 0 ? 0 : 1);
    std::vector<cask::CsrMatrix> partitions(nBlocks);
    for (int i = 0; i < n; i++) {
      for (int j = 0; j < nBlocks; j++) {
        auto& p = partitions[j].row_ptr;
        if (p.size() == 0)
          p.push_back(0);
        else
          p.push_back(p.back());
      }
      //std::cout << "i = " << i << std::endl;
      //std::cout << "colptr" << colptr[i] << std::endl;
      for (int j = row_ptr[i]; j < row_ptr[i+1]; j++) {
        auto& p = partitions[col_ind[j] / blockSize];
        int idxInPartition = col_ind[j] - (col_ind[j] / blockSize ) * blockSize;
        p.col_ind.push_back(idxInPartition);
        p.values.push_back(values[j]);
        p.row_ptr.back()++;
      }
    }
    return partitions;
  }

};

/** A symmetric matrix for which only the lower triangle is stored explicitly, in CSR format */
class SymCsrMatrix {
 public:
  int n, m;
  int nnzs;
  CsrMatrix matrix;

  // Construct a symmetric matrix from a lower triangular matrix in DoK format
  explicit SymCsrMatrix(const DokMatrix& l) : n(l.n), m(l.m), matrix(l) {
    int diagNnzs = 0;
    for (int i = 0; i < l.n; i++)
      if (l.at(i, i) != 0)
        diagNnzs++;
    nnzs = 2 * (l.nnzs - diagNnzs) + diagNnzs;
  }

  void print() {
    // matrix.print();
  }

  void pretty_print() {
    std::cout << "Stored matrix: " << std::endl;
    matrix.pretty_print();
    std::cout << "Implicit values: " << std::endl;
    matrix.toDok().explicitSymmetric().pretty_print();
  }

  Vector dot(const Vector& b) const {
    return matrix.toDok().explicitSymmetric().dot(b);
  }

};

}

#endif /* end of include guard: SPARSEMATRIX_HPP_1BWMMLC8 */


// Copyright (c) 2022, 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 "estimators/fundamental_matrix.h"

#include <cfloat>
#include <complex>
#include <vector>

#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

#include "base/polynomial.h"
#include "estimators/utils.h"
#include "util/logging.h"

namespace colmap {

std::vector<FundamentalMatrixSevenPointEstimator::M_t>
FundamentalMatrixSevenPointEstimator::Estimate(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2) {
  CHECK_EQ(points1.size(), 7);
  CHECK_EQ(points2.size(), 7);

  // Note that no normalization of the points is necessary here.

  // Setup system of equations: [points2(i,:), 1]' * F * [points1(i,:), 1]'.
  Eigen::Matrix<double, 7, 9> A;
  for (size_t i = 0; i < 7; ++i) {
    const double x0 = points1[i](0);
    const double y0 = points1[i](1);
    const double x1 = points2[i](0);
    const double y1 = points2[i](1);
    A(i, 0) = x1 * x0;
    A(i, 1) = x1 * y0;
    A(i, 2) = x1;
    A(i, 3) = y1 * x0;
    A(i, 4) = y1 * y0;
    A(i, 5) = y1;
    A(i, 6) = x0;
    A(i, 7) = y0;
    A(i, 8) = 1;
  }

  // 9 unknowns with 7 equations, so we have 2D null space.
  Eigen::JacobiSVD<Eigen::Matrix<double, 7, 9>> svd(A, Eigen::ComputeFullV);
  const Eigen::Matrix<double, 9, 9> f = svd.matrixV();
  Eigen::Matrix<double, 1, 9> f1 = f.col(7);
  Eigen::Matrix<double, 1, 9> f2 = f.col(8);

  f1 -= f2;

  // Normalize, such that lambda + mu = 1
  // and add constraint det(F) = det(lambda * f1 + (1 - lambda) * f2).

  const double t0 = f1(4) * f1(8) - f1(5) * f1(7);
  const double t1 = f1(3) * f1(8) - f1(5) * f1(6);
  const double t2 = f1(3) * f1(7) - f1(4) * f1(6);
  const double t3 = f2(4) * f2(8) - f2(5) * f2(7);
  const double t4 = f2(3) * f2(8) - f2(5) * f2(6);
  const double t5 = f2(3) * f2(7) - f2(4) * f2(6);

  Eigen::Vector4d coeffs;
  coeffs(0) = f1(0) * t0 - f1(1) * t1 + f1(2) * t2;
  coeffs(1) = f2(0) * t0 - f2(1) * t1 + f2(2) * t2 -
              f2(3) * (f1(1) * f1(8) - f1(2) * f1(7)) +
              f2(4) * (f1(0) * f1(8) - f1(2) * f1(6)) -
              f2(5) * (f1(0) * f1(7) - f1(1) * f1(6)) +
              f2(6) * (f1(1) * f1(5) - f1(2) * f1(4)) -
              f2(7) * (f1(0) * f1(5) - f1(2) * f1(3)) +
              f2(8) * (f1(0) * f1(4) - f1(1) * f1(3));
  coeffs(2) = f1(0) * t3 - f1(1) * t4 + f1(2) * t5 -
              f1(3) * (f2(1) * f2(8) - f2(2) * f2(7)) +
              f1(4) * (f2(0) * f2(8) - f2(2) * f2(6)) -
              f1(5) * (f2(0) * f2(7) - f2(1) * f2(6)) +
              f1(6) * (f2(1) * f2(5) - f2(2) * f2(4)) -
              f1(7) * (f2(0) * f2(5) - f2(2) * f2(3)) +
              f1(8) * (f2(0) * f2(4) - f2(1) * f2(3));
  coeffs(3) = f2(0) * t3 - f2(1) * t4 + f2(2) * t5;

  Eigen::VectorXd roots_real;
  Eigen::VectorXd roots_imag;
  if (!FindPolynomialRootsCompanionMatrix(coeffs, &roots_real, &roots_imag)) {
    return {};
  }

  std::vector<M_t> models;
  models.reserve(roots_real.size());

  for (Eigen::VectorXd::Index i = 0; i < roots_real.size(); ++i) {
    const double kMaxRootImag = 1e-10;
    if (std::abs(roots_imag(i)) > kMaxRootImag) {
      continue;
    }

    const double lambda = roots_real(i);
    const double mu = 1;

    Eigen::MatrixXd F = lambda * f1 + mu * f2;

    F.resize(3, 3);

    const double kEps = 1e-10;
    if (std::abs(F(2, 2)) < kEps) {
      continue;
    }

    F /= F(2, 2);

    models.push_back(F.transpose());
  }

  return models;
}

void FundamentalMatrixSevenPointEstimator::Residuals(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2,
    const M_t& F, std::vector<double>* residuals) {
  ComputeSquaredSampsonError(points1, points2, F, residuals);
}

std::vector<FundamentalMatrixEightPointEstimator::M_t>
FundamentalMatrixEightPointEstimator::Estimate(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2) {
  CHECK_EQ(points1.size(), points2.size());

  // Center and normalize image points for better numerical stability.
  std::vector<X_t> normed_points1;
  std::vector<Y_t> normed_points2;
  Eigen::Matrix3d points1_norm_matrix;
  Eigen::Matrix3d points2_norm_matrix;
  CenterAndNormalizeImagePoints(points1, &normed_points1, &points1_norm_matrix);
  CenterAndNormalizeImagePoints(points2, &normed_points2, &points2_norm_matrix);

  // Setup homogeneous linear equation as x2' * F * x1 = 0.
  Eigen::Matrix<double, Eigen::Dynamic, 9> cmatrix(points1.size(), 9);
  for (size_t i = 0; i < points1.size(); ++i) {
    cmatrix.block<1, 3>(i, 0) = normed_points1[i].homogeneous();
    cmatrix.block<1, 3>(i, 0) *= normed_points2[i].x();
    cmatrix.block<1, 3>(i, 3) = normed_points1[i].homogeneous();
    cmatrix.block<1, 3>(i, 3) *= normed_points2[i].y();
    cmatrix.block<1, 3>(i, 6) = normed_points1[i].homogeneous();
  }

  // Solve for the nullspace of the constraint matrix.
  Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> cmatrix_svd(
      cmatrix, Eigen::ComputeFullV);
  const Eigen::VectorXd cmatrix_nullspace = cmatrix_svd.matrixV().col(8);
  const Eigen::Map<const Eigen::Matrix3d> ematrix_t(cmatrix_nullspace.data());

  // Enforcing the internal constraint that two singular values must non-zero
  // and one must be zero.
  Eigen::JacobiSVD<Eigen::Matrix3d> fmatrix_svd(
      ematrix_t.transpose(), Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Vector3d singular_values = fmatrix_svd.singularValues();
  singular_values(2) = 0.0;
  const Eigen::Matrix3d F = fmatrix_svd.matrixU() *
                            singular_values.asDiagonal() *
                            fmatrix_svd.matrixV().transpose();

  const std::vector<M_t> models = {points2_norm_matrix.transpose() * F *
                                   points1_norm_matrix};
  return models;
}

void FundamentalMatrixEightPointEstimator::Residuals(
    const std::vector<X_t>& points1, const std::vector<Y_t>& points2,
    const M_t& E, std::vector<double>* residuals) {
  ComputeSquaredSampsonError(points1, points2, E, residuals);
}

}  // namespace colmap


// std includes
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <cmath>
#include <ctime>
#include <memory>

// Armadillo includes and preprocessors
#include <armadillo>
#include <viennacl/ocl/forwards.h>
// ViennaCL includes
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/ilu.hpp"
//#include "viennacl/linalg/ilu/chow_patel_ilu.hpp"
#include "viennacl/linalg/lu.hpp"
#include "viennacl/linalg/amg.hpp"
#include "viennacl/linalg/sum.hpp"
#include "viennacl/linalg/maxmin.hpp"
#include "viennacl/tools/timer.hpp"
#include "viennacl/forwards.h"

// Main CoMFi include
#include "comfi.hpp"

using namespace std;

int main(int argc, char** argv) {

  // Create intermediate variable but use constant for safety
	comfi::types::Settings __set;
	if (argc > 1) {
		comfi::util::interpret_arguments(__set, argc, argv);
	}
  const comfi::types::Settings settings = __set;

  // Create context
  comfi::types::Context ctx(1,
                            400,
                            comfi::types::NEUMANN,
                            comfi::types::NEUMANN,
                            comfi::types::NEUMANN,
                            comfi::types::NEUMANN);
  unique_ptr<vcl_mat> xn_vcl(new vcl_mat(comfi::util::shock_tube_ic(ctx)));
  //unique_ptr<vcl_mat> xn_vcl(new vcl_mat(comfi::util::ot_vortex_ic(ctx)));
  unique_ptr<vcl_mat> xn1_vcl(new vcl_mat(*xn_vcl));
  comfi::util::save_solution(*xn_vcl, ctx);

  // Initial std output
  cout << "Begin CoMFi Simulation" << endl;
  cout << "======================" << endl
       << endl;
  #ifdef VIENNACL_WITH_OPENCL
    cout << "Device Info:" << endl
         << "------------" << endl
         << viennacl::ocl::current_device().info()
         << endl;
  #endif
  #ifdef _OPENMP
    cout << "OpenMP Info:" << endl
         << "------------" << endl
         << "- OMP Max threads: " << omp_get_max_threads() << endl
         << "- OMP Devices: " << omp_get_default_device() << "/" << omp_get_num_devices() << endl
         << endl;
  #endif
  cout << "Parameters:" << endl;
  cout << "-----------" << endl;
  cout << "- Grid size: (" << ctx.nz << ", " << ctx.nx << ") " << endl
       << "- Normalization constants:" << endl
       << " - " << "n_0: " << ctx.n_0 << " m^-3" << endl
       << " - " << "L_0: " << ctx.l_0 << " m" << endl
       << " - " << "t_0: " << ctx.t_0 << " s" << endl
       << " - " << "V_0: " << ctx.V_0 << " m/s" << endl
       << " - " << "B_0: " << ctx.B_0/0.1e-3 << " G" << endl
       << " - " << "e_0: " << ctx.e_0 << " C" << endl
       << " - " << "T_0: " << ctx.T_0 << " K" << endl
       << " - " << "p_0: "<< ctx.p_0 << " Pa" << endl
       << " - " << "Width: " << ctx.width << " m" << endl
       << " - " << "Height: " << ctx.height << " m" << endl
       << " - " << "dx: " << ctx.dx*ctx.l_0 << " m | " << "dz: " << ctx.dz*ctx.l_0 << " m | "
                << "ds: " << ctx.ds*ctx.l_0 << " m" << endl
       << endl;

  //Initiate
  viennacl::tools::timer vcl_timer[2]; // timers
  //auto init = mhdsim::util::calcInitialCondition(op);
  //auto init = mhdsim::util::calcReconnectionIC(op);
  //auto init = comfi::util::calcShockTubeIC(op);
  //auto init = comfi::util::calcSolerIC(op);
  //arma::vec x0 = std::get<0>(init);
  //const comfi::types::BgData bg;

  cout << "Compiling kernels ... ";
  std::ifstream phi_file("kernels_ocl/phi.c");
  string phi_code((std::istreambuf_iterator<char>(phi_file)),
                   std::istreambuf_iterator<char>()
                 );
  viennacl::ocl::program & phi_prog = viennacl::ocl::current_context().add_program(phi_code.c_str(), "fluxl"); // compile flux opencl kernel
  std::ifstream eig_file("kernels_ocl/element_max.c");
  string eig_code((std::istreambuf_iterator<char>(eig_file)),
                   std::istreambuf_iterator<char>()
                 );
  viennacl::ocl::program & eig_prog = viennacl::ocl::current_context().add_program(eig_code.c_str(), "element_max"); // compile eigenvalue opencl kernel
  cout << "built." << endl;

  // Full execution timer
  vcl_timer[0].start();

  // Begin solving/advancing
  while (((ctx.time_step() < settings.max_time_steps) || (settings.max_time_steps < 0)) &&
         ((ctx.time_elapsed() < settings.max_time) || (settings.max_time < 0.0))) {

    double solve_time=0.0, build_time=0.0;

    // Figure out time step
    const double V = comfi::util::getmaxV(*xn_vcl, ctx);
    cout << "Char speed: "  << V << " V_0" << endl;
    const double ds = (ctx.dx>ctx.dz)*ctx.dz + (ctx.dz>=ctx.dx)*ctx.dx;
    ctx.set_dt(0.8*0.5*ds/V);
    cout << "dt: " << ctx.dt() << " t_0";
    cout << "\t| Time: " << ctx.time_elapsed() << " t_0" << endl;

    /* //Update loop stuff */
    /* t(ctx.time_step()) = ctx.time_elapsed(); */
    /* dt_n(ctx.time_step()) = ctx.dt(); */

    cout << ctx.time_step() << ": " ;

    vcl_timer[1].start();
    //const arma::sp_mat Ri_cpu = mhdsim::routines::computeRi(*xn_vcl, op);
    //static const arma::sp_mat one = arma::speye(num_of_elem, num_of_elem);
    //const arma::sp_mat LHS_cpu = one + Ri_cpu*dt; // Euler
    //vcl_sp_mat LHS;
    //viennacl::copy(LHS_cpu, LHS);
    build_time = vcl_timer[1].get();

    vcl_timer[1].start();
    //GMRES
    // solve (e.g. using GMRES solver)
    // create and compute preconditioner:
    //viennacl::linalg::ilu0_tag ilu0_config(true);
    //viennacl::linalg::block_ilu_precond<vcl_sp_mat, viennacl::linalg::ilu0_tag> vcl_precond(LHS, ilu0_config);
    //viennacl::linalg::ilu0_precond< vcl_sp_mat > vcl_precond(LHS, viennacl::linalg::ilu0_tag());
    //viennacl::linalg::gmres_tag my_solver_tag(tolerance, 100, 20);
    //viennacl::linalg::bicgstab_tag my_solver_tag(tolerance, 100, 20);
    //unique_ptr<vcl_vec> x0_vcl(new vcl_vec(viennacl::linalg::solve(LHS, RHS, my_solver_tag, vcl_precond)));
    /* unique_ptr<vcl_mat> x0_vcl(new vcl_mat(comfi::routines::computeRHS_Euler(*xn_vcl, ctx))); */
    unique_ptr<vcl_mat> x0_vcl(new vcl_mat(comfi::routines::computeRHS_RK4(*xn_vcl, ctx)));
    ctx.advance();
    solve_time = vcl_timer[1].get();

//    cout << " GMRES(" << my_solver_tag.iters();
//    cout << "," << my_solver_tag.error();
//    cout << ") | Build Time:" << build_time;
//    cout << "s\t| Sol Time:" << solve_time << "s" << endl;
//    const double sol_error = my_solver_tag.error();
    /* double sol_error = 0; */

    //mhdsim::util::save_solution(x0_vcl, -1, op);
    //mhdsim::util::save_solution(xn_vcl, -2, op);
    //mhdsim::util::save_solution(xn1_vcl, -3, op);

    //Save solution every save_dt time
    static double time_since_last_save = 0.0;
    time_since_last_save += ctx.dt();
    if ((time_since_last_save > settings.save_dt) && (settings.save_dt > 0.0)) {
      comfi::util::save_solution(*x0_vcl, ctx);
      time_since_last_save = 0.0;
    }

    //Save solution every save_dn steps
    if ((ctx.time_step()%settings.save_dn == 0) && (settings.save_dn > 0)) {
      comfi::util::save_solution(*x0_vcl, ctx);
    }

    // Move pointers before starting next step
    xn1_vcl = std::move(xn_vcl);
    xn_vcl = std::move(x0_vcl);

  }

  cout << "Total exec time: " << vcl_timer[0].get() << endl;

  return 0;
}

/*
vim: tabstop=2
vim: shiftwidth=2
vim: smarttab
vim: expandtab
*/


//sensor_msgs/Imu

#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <vector>
#include <iostream>
#include <boost/regex.hpp>
#include <boost/filesystem.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include "sensor_msgs/Imu.h"
#include "nav_msgs/Odometry.h"
#include "std_msgs/Bool.h"

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

using namespace std;
using namespace cv;
using namespace Eigen;

static void toEulerAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
// roll (x-axis rotation)
double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
roll = atan2(sinr_cosp, cosr_cosp);

// pitch (y-axis rotation)
double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
if (fabs(sinp) >= 1)
pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
pitch = asin(sinp);

// yaw (z-axis rotation)
double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
yaw = atan2(siny_cosp, cosy_cosp);
}
static float toAngle(double& r)
{
    return 180 * r / M_PI;
}
class message_sync_ros_node
{
private:
    ros::NodeHandle node_;
    ros::Subscriber init_sub,yt_sub,imu_sub;
    typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::Imu> slamSyncPolicy;
    message_filters::Subscriber<nav_msgs::Odometry> *yt_sub_;
    message_filters::Subscriber<sensor_msgs::Imu> *imu_sub_;
    message_filters::Synchronizer<slamSyncPolicy> *sync_;
    std::string yt_topic;
    std::string imu_topic;
    std::string root_path;
    bool is_record,is_init;
    ofstream map_points;
    stringstream gss;
    float init_yt_x,init_yt_y,init_yt_z,init_imu_x,init_imu_y,init_imu_z;

public:
    message_sync_ros_node();
    ~message_sync_ros_node();
    void callback(const nav_msgs::Odometry::ConstPtr &yt_data,const sensor_msgs::Imu::ConstPtr &imu_data);
    void update();
    void initCB(const std_msgs::Bool::ConstPtr &msg);
    void ytCB(const nav_msgs::Odometry::ConstPtr &yt_data);
    void imuCB(const sensor_msgs::Imu::ConstPtr &imu_data);
    void isDirectory(string path);
};

message_sync_ros_node::message_sync_ros_node()
{
    is_record = false;
    is_init = false;
    //map_points.open("/home/li/capture/0919/record.txt", ios_base::app);
    ros::param::get("/sync_imu/yt_topic", yt_topic);
    ros::param::get("/sync_imu/imu_topic", imu_topic);
    ros::param::get("/sync_imu/root_path", root_path);
    std::cout << "yt_topic:" << yt_topic << std::endl;
    std::cout << "imu_topic:" << imu_topic << std::endl;
    std::cout << "root_path:" << root_path << std::endl;

    isDirectory(root_path);
    string record_path = root_path + "/record.txt";
    map_points.open(record_path, ios_base::app);
    init_sub = node_.subscribe<std_msgs::Bool>("sync_imu/init", 1, &message_sync_ros_node::initCB, this);
    yt_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(node_, yt_topic, 1);
    imu_sub_ = new message_filters::Subscriber<sensor_msgs::Imu>(node_, imu_topic, 1);
    sync_ = new message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(20), *yt_sub_, *imu_sub_);
    sync_->registerCallback(boost::bind(&message_sync_ros_node::callback, this, _1, _2));

    //sub
    yt_sub = node_.subscribe<nav_msgs::Odometry>(yt_topic, 1, &message_sync_ros_node::ytCB, this);
    imu_sub = node_.subscribe<sensor_msgs::Imu>(imu_topic, 1, &message_sync_ros_node::imuCB, this);
}

void message_sync_ros_node::initCB(const std_msgs::Bool::ConstPtr &msg){
    std::cout << "initCB " << std::endl;
     is_init = true;
}

message_sync_ros_node::~message_sync_ros_node()
{
    map_points.close();
}

void message_sync_ros_node::update()
{
   std::cout << "update " << std::endl;
}

void message_sync_ros_node::isDirectory(string path)
{
    //判断根目录下是否存在子目录，不存在创建
    if (!boost::filesystem::exists(path))
    {
        boost::filesystem::create_directory(path);
    }
}

void message_sync_ros_node::callback(const nav_msgs::Odometry::ConstPtr &yt_data,const sensor_msgs::Imu::ConstPtr &imu_data)
{
    std::cout << "sync message" << std::endl;
    // nav_msgs::Odometry current_pose;
    // current_pose = *odom_data;
    float angle_x = yt_data->pose.pose.position.x;
    float angle_y = yt_data->pose.pose.position.y;
    float angle_z = yt_data->pose.pose.position.z;
    //std::cout << "angle_x:" << angle_x<< " angle_y:" << angle_y<< " angle_z:" << angle_z << std::endl;
    //
    //float imu_x = imu_data->orientation.x;
    geometry_msgs::Quaternion qua = imu_data->orientation;
    Eigen::Quaterniond q(qua.w, qua.x, qua.y, qua.z);
    double roll,pitch,yaw;
    toEulerAngle(q,roll,pitch,yaw);
    //std::cout << "roll:" << roll<< " pitch:" << pitch<< " yaw:" << yaw << std::endl;
    if(is_init){
        std::cout << "init:" << std::endl;
        init_yt_x = angle_x;
        init_yt_y = angle_y;
        init_yt_z = angle_z;
        init_imu_x = toAngle(roll);
        init_imu_y = toAngle(pitch);
        init_imu_z = toAngle(yaw);
        is_init = false;
    }
    float dx = abs(angle_x - init_yt_x)/100;
    float dy = abs(angle_y - init_yt_y)/100;
    float dz = abs(angle_z - init_yt_z)/100;
    std::cout << "ptz dx:" << dx<< " dy:" << dy << " dz:" << dz << std::endl;
    map_points << "ptz dx:" << dx<< " dy:" << dy << " dz:" << dz << std::endl;
    dx = abs(toAngle(roll) - init_imu_x);
    dy = abs(toAngle(pitch) - init_imu_y);
    dz = abs(toAngle(yaw) - init_imu_z);
    //std::cout << "roll:" << toAngle(roll)<< " pitch:" << toAngle(pitch) << " yaw:" << toAngle(yaw) << std::endl;
    std::cout << "imu dx:" << dx<< " dy:" << dy << " dz:" << dz << std::endl;
    map_points << "imu dx:" << dx<< " dy:" << dy << " dz:" << dz << std::endl;
}

void message_sync_ros_node::ytCB(const nav_msgs::Odometry::ConstPtr &yt_data)
{
    float angle_x = abs(yt_data->pose.pose.position.x)/100;
    float angle_y = abs(yt_data->pose.pose.position.y)/100;
    float angle_z = abs(yt_data->pose.pose.position.z)/100;
    std::cout << "PTZ data receive time:" << yt_data->header.stamp << " angle_x:" << angle_x<< " angle_y:" << angle_y << " angle_z:" << angle_z << std::endl;
    map_points << "PTZ data receive time:" << yt_data->header.stamp << " angle_x:" << angle_x<< " angle_y:" << angle_y << " angle_z:" << angle_z << std::endl;
}

void message_sync_ros_node::imuCB(const sensor_msgs::Imu::ConstPtr &imu_data)
{
    geometry_msgs::Quaternion qua = imu_data->orientation;
    Eigen::Quaterniond q(qua.w, qua.x, qua.y, qua.z);
    double roll,pitch,yaw;
    toEulerAngle(q,roll,pitch,yaw);
    if(is_init){
        std::cout << "init:" << std::endl;
        init_imu_x = toAngle(roll);
        init_imu_y = toAngle(pitch);
        init_imu_z = toAngle(yaw);
        is_init = false;
    }
    float dx = abs(toAngle(roll) - init_imu_x);
    float dy = abs(toAngle(pitch) - init_imu_y);
    float dz = abs(toAngle(yaw) - init_imu_z);
    std::cout << "IMU data receive time:" << imu_data->header.stamp << " roll:" << dx<< " pitch:" << dy << " yaw:" << dz << std::endl;
    map_points << "IMU data receive time:" << imu_data->header.stamp << " roll:" << dx<< " pitch:" << dy << " yaw:" << dz << std::endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "message_sync_node");
    message_sync_ros_node node;

    ROS_INFO("message_sync_node node started...");
    ros::Rate rate(10);

    while (ros::ok())
    {
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}


// Copyright (c) Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of differential evolution using on the DE/best/1 strategy.
// Uses two deviations from the standard DE algorithm:
// a) temporal locality introduced in
// https://www.researchgate.net/publication/309179699_Differential_evolution_for_protein_folding_optimization_based_on_a_three-dimensional_AB_off-lattice_model
// b) reinitialization of individuals based on their age.
// requires https://github.com/imneme/pcg-cpp
//
// Supports parallel fitness function evaluation.
//
// You may keep parameters F and CR at their defaults since this implementation works well with the given settings for most problems,
// since the algorithm oscillates between different F and CR settings.
//
// For expensive objective functions (e.g. machine learning parameter optimization) use the workers
// parameter to parallelize objective function evaluation. The workers parameter is limited by the
// population size.
//
// The ints parameter is a boolean array indicating which parameters are discrete integer values. This
// parameter was introduced after observing non optimal DE-results for the ESP2 benchmark problem:
// https://github.com/AlgTUDelft/ExpensiveOptimBenchmark/blob/master/expensiveoptimbenchmark/problems/DockerCFDBenchmark.py
// If defined it causes a "special treatment" for discrete variables: They are rounded to the next integer value and
// there is an additional mutation to avoid getting stuck at local minima.

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <stdint.h>
#include <ctime>
#include <random>
#include <queue>
#include <tuple>
#include "pcg_random.hpp"
#include "evaluator.h"

using namespace std;

namespace differential_evolution {

class DeOptimizer {

public:

    DeOptimizer(long runid_, Fitness *fitfun_, int dim_, int seed_,
            int popsize_, int maxEvaluations_, double keep_,
            double stopfitness_, double F_, double CR_, bool *isInt_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        popsize = popsize_ > 0 ? popsize_ : 15 * dim;
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_ > 0 ? maxEvaluations_ : 50000;
        // keep best young after each iteration.
        keep = keep_ > 0 ? keep_ : 30;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        F = F0 = F_ > 0 ? F_ : 0.5;
        CR = CR0 = CR_ > 0 ? CR_ : 0.9;
        // Number of iterations already performed.
        iterations = 0;
        bestY = DBL_MAX;
        // stop criteria
        stop = 0;
        pos = 0;
        //std::random_device rd;
        rs = new pcg64(seed_);
        // Indicating which parameters are discrete integer values. If defined these parameters will be
        // rounded to the next integer and some additional mutation of discrete parameters are performed.
        isInt = isInt_;
        init();
    }

    ~DeOptimizer() {
        delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    int rndInt(int max) {
        return (int) (max * distr_01(*rs));
    }

    vec nextX(int p, const vec &xp, const vec &xb) {
        if (p == 0) {
            iterations++;
            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;
        }
        int r1, r2;
        do {
            r1 = rndInt(popsize);
        } while (r1 == p || r1 == bestI);
        do {
            r2 = rndInt(popsize);
        } while (r2 == p || r2 == bestI || r2 == r1);
        vec x1 = popX.col(r1);
        vec x2 = popX.col(r2);
        vec x = xb + (x1 - x2) * F;
        int r = rndInt(dim);
        for (int j = 0; j < dim; j++)
            if (j != r && rnd01() > CR)
                x[j] = xp[j];
        vec nextx = fitfun->getClosestFeasible(x);
        modify(nextx);
        return nextx;
    }

    vec next_improve(const vec &xb, const vec &x, const vec &xi) {
        vec nextx = fitfun->getClosestFeasible(xb + ((x - xi) * 0.5));
        modify(nextx);
        return nextx;
    }

    void modify(vec &x) {
        if (isInt == NULL)
            return;
        double n_ints = 0;
        for (int i = 0; i < dim; i++)
            if (isInt[i]) n_ints++;
        double min_mutate = 0.5;
        double max_mutate = std::max(1.0, n_ints/20.0);
        double to_mutate = min_mutate + rnd01()*(max_mutate - min_mutate);
        for (int i = 0; i < dim; i++) {
            if (isInt[i]) {
                if (rnd01() < to_mutate/n_ints)
                    x[i] = fitfun->sample_i(i, *rs); // resample
                x[i] = std::round(x[i]);
            }
        }
    }

    vec ask(int &p) {
        // ask for one new argument vector.
        if (improvesX.empty()) {
            p = pos;
            vec x = nextX(p, popX.col(p), popX.col(bestI));
            pos = (pos + 1) % popsize;
            return x;
        } else {
            p = improvesP.front();
            vec x = improvesX.front();
            improvesP.pop();
            improvesX.pop();
            return x;
        }
    }

    int tell(double y, const vec &x, int p) {
        //tell function value for a argument list retrieved by ask_one().
        if (isfinite(y) && y < popY[p]) {
            if (iterations > 1) {
                // temporal locality
                improvesP.push(p);
                improvesX.push(next_improve(popX.col(bestI), x, popX0.col(p)));
            }
            popX0.col(p) = popX.col(p);
            popX.col(p) = x;
            popY[p] = y;
            popIter[p] = iterations;
            if (y < popY[bestI]) {
                bestI = p;
                if (y < bestY) {
                    bestY = y;
                    bestX = x;
                    if (isfinite(stopfitness) && bestY < stopfitness)
                        stop = 1;
                }
            }
        } else {
            // reinitialize individual
            if (keep * rnd01() < iterations - popIter[p]) {
                popX.col(p) = fitfun->sample(*rs);
                popY[p] = DBL_MAX;
            }
        }
        return stop;
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------
        for (iterations = 1; fitfun->evaluations() < maxEvaluations
        		&& !fitfun->terminate(); iterations++) {

            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;

            for (int p = 0; p < popsize; p++) {
                vec xp = popX.col(p);
                vec xb = popX.col(bestI);
                int r1, r2;
                do {
                    r1 = rndInt(popsize);
                } while (r1 == p || r1 == bestI);
                do {
                    r2 = rndInt(popsize);
                } while (r2 == p || r2 == bestI || r2 == r1);
                vec x1 = popX.col(r1);
                vec x2 = popX.col(r2);
                int r = rndInt(dim);
                vec x = vec(xp);
                for (int j = 0; j < dim; j++) {
                    if (j == r || rnd01() < CR) {
                        x[j] = xb[j] + F * (x1[j] - x2[j]);
                        if (!fitfun->feasible(j, x[j]))
                            x[j] = fitfun->sample_i(j, *rs);
                    }
                }
                modify(x);
                double y = fitfun->eval(x)(0);
                if (isfinite(y) && y < popY[p]) {
                    // temporal locality
                    vec x2 = next_improve(xb, x, xp);
                    double y2 = fitfun->eval(x2)(0);
                    if (isfinite(y2) && y2 < y) {
                        y = y2;
                        x = x2;
                    }
                    popX.col(p) = x;
                    popY(p) = y;
                    popIter[p] = iterations;
                    if (y < popY[bestI]) {
                        bestI = p;
                        if (y < bestY) {
                            bestY = y;
                            bestX = x;
                            if (isfinite(stopfitness) && bestY < stopfitness) {
                                stop = 1;
                                return;
                            }
                        }
                    }
                } else {
                    // reinitialize individual
                    if (keep * rnd01() < iterations - popIter[p]) {
                        popX.col(p) = fitfun->sample(*rs);
                        popY[p] = DBL_MAX;
                    }
                }
            }
        }
    }

    void do_optimize_delayed_update(int workers) {
    	 iterations = 0;
    	 fitfun->resetEvaluations();
         workers = std::min(workers, popsize); // workers <= popsize
    	 evaluator eval(fitfun, 1, workers);
         int evals_size = popsize*10;
    	 vec evals_x[evals_size];
   	     int evals_p[evals_size];
         int cp = 0;

	     // fill eval queue with initial population
    	 for (int i = 0; i < workers; i++) {
    		 int p;
    		 vec x = ask(p);
    		 eval.evaluate(x, cp);
    		 evals_x[cp] = x;
    		 evals_p[cp] = p;
             cp = (cp + 1) % evals_size;
    	 }
    	 while (fitfun->evaluations() < maxEvaluations && !fitfun->terminate()) {
    		 vec_id* vid = eval.result();
    		 vec y = vec(vid->_v);
    		 int id = vid->_id;
    		 delete vid;
    		 vec x = evals_x[id];
             int p = evals_p[id];
    		 tell(y(0), x, p); // tell evaluated x
    		 if (fitfun->evaluations() >= maxEvaluations)
    			 break;
    		 x = ask(p);
    		 eval.evaluate(x, cp);
    		 evals_x[cp] = x;
    		 evals_p[cp] = p;
             cp = (cp + 1) % evals_size;
    	 }
	}

    void init() {
        popX = mat(dim, popsize);
        popX0 = mat(dim, popsize);
        popY = vec(popsize);
        for (int p = 0; p < popsize; p++) {
            popX0.col(p) = popX.col(p) = fitfun->sample(*rs);
            popY[p] = DBL_MAX; // compute fitness
        }
        bestI = 0;
        bestX = popX.col(bestI);
        popIter = zeros(popsize);
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

    Fitness* getFitfun() {
        return fitfun;
    }

    int getDim() {
        return dim;
    }

private:
    long runid;
    Fitness *fitfun;
    int popsize; // population size
    int dim;
    int maxEvaluations;
    double keep;
    double stopfitness;
    int iterations;
    double bestY;
    vec bestX;
    int bestI;
    int stop;
    double F0;
    double CR0;
    double F;
    double CR;
    pcg64 *rs;
    mat popX;
    mat popX0;
    vec popY;
    vec popIter;
    queue<vec> improvesX;
    queue<int> improvesP;
    int pos;
    bool *isInt;
};

}

using namespace differential_evolution;

extern "C" {
void optimizeDE_C(long runid, callback_type func, int dim, int seed,
        double *lower, double *upper, bool *ints,
        int maxEvals, double keep,
        double stopfitness, int popsize, double F, double CR, int workers, double* res) {
    vec lower_limit(dim), upper_limit(dim);
    bool isInt[dim];
    bool useIsInt = false;
    for (int i = 0; i < dim; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
        isInt[i] = ints[i];
        useIsInt |= ints[i];
        if (isInt[i]) {
            // adjust bounds because ints are rounded
            lower_limit[i] -= .499999999;
            upper_limit[i] += .499999999;
        }
    }
    Fitness fitfun(func, dim, 1, lower_limit, upper_limit);
    DeOptimizer opt(runid, &fitfun, dim, seed, popsize, maxEvals, keep,
            stopfitness, F, CR, useIsInt ? isInt : NULL);
    try {
        if (workers <= 1)
            opt.doOptimize();
        else
            opt.do_optimize_delayed_update(workers);
        vec bestX = opt.getBestX();
        double bestY = opt.getBestValue();
        for (int i = 0; i < dim; i++)
            res[i] = bestX[i];
        res[dim] = bestY;
        res[dim + 1] = fitfun.evaluations();
        res[dim + 2] = opt.getIterations();
        res[dim + 3] = opt.getStop();
    } catch (std::exception &e) {
        cout << e.what() << endl;
    }
}
}


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

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>

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

namespace Laplace1D3D {

using EVec3 = Eigen::Vector3d;

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

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

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

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

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

double directSum(const EVec3 &target, const EVec3 &source, const int directTerm = 500000) {
    // use asymptotic
    const double L3 = 1.0;
    double potentialDirect = 0;
    for (int t = DIRECTLAYER + 1; t < directTerm; t++) {
        potentialDirect +=
            gKernel(target, source + EVec3(t * L3, 0, 0)) + gKernel(target, source - EVec3(t * L3, 0, 0));
    }

    return potentialDirect;
}

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

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

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

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

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

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

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

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

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

        M2L.col(i) = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    }

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

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> chargePoint(4);
    std::vector<double> chargeValue(4);
    chargePoint[0] = Eigen::Vector3d(0.125, 0.5, 0.5);
    chargeValue[0] = 1;
    chargePoint[1] = Eigen::Vector3d(0.375, 0.5, 0.5);
    chargeValue[1] = -1;
    chargePoint[2] = Eigen::Vector3d(0.625, 0.5, 0.5);
    chargeValue[2] = 1;
    chargePoint[3] = Eigen::Vector3d(0.875, 0.5, 0.5);
    chargeValue[3] = -1;

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

    // impose zero sum
    double fx = 0;
    for (int i = 0; i < equivN; i++) {
        fx += Msource[i];
    }
    fx /= equivN;
    double test = 0;
    for (int i = 0; i < equivN; i++) {
        Msource[i] -= fx;
        test += Msource[i];
    }

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

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

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

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

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

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;
    std::cout << "error:" << UsampleSP + Usample + 8 * log(2) << std::endl;

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

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

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

    std::cout << "samplePoint:" << samplePoint << std::endl;
    std::cout << "Usample NF:" << Usample << std::endl;
    std::cout << "Usample FF:" << UsampleSP << std::endl;
    std::cout << "Usample FF+NF total:" << UsampleSP + Usample << std::endl;
    std::cout << "error:" << UsampleSP + Usample + 8 * log(2) << std::endl;

    return 0;
}

} // namespace Laplace1D3D

#undef DIRECTLAYER
#undef PI314


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

/// \file exponential_sum.hpp

#ifndef MXPFIT_EXPONENTIAL_SUM_HPP
#define MXPFIT_EXPONENTIAL_SUM_HPP

#include <algorithm>
#include <iosfwd>
#include <random>

#include <Eigen/Core>

namespace mxpfit
{
// --- Forward declarations
template <typename ExpScalarT, typename WScalarT = ExpScalarT>
class ExponentialSum;

template <typename ExponentsArrayT, typename WeightsArrayT>
class ExponentialSumWrapper;

namespace detail
{

template <typename T>
struct ExponentialSumTraits;

//
// Generate random real number between [0,1]
//
template <typename T>
struct random_real_scalar
{
    random_real_scalar() : distr_(T(), T(1))
    {
        std::random_device rd;
        std::seed_seq seeds({rd(), rd(), rd(), rd(), rd(), rd(), rd(), rd()});
        engine_.seed(seeds);
    }

    T operator()()
    {
        return distr_(engine_);
    }

    std::mt19937 engine_;
    std::uniform_real_distribution<T> distr_;
};

template <typename T>
struct random_exponent_impl
{
    T operator()()
    {
        return m_rng(); // distributed on [0, 1]
    }

private:
    random_real_scalar<T> m_rng;
};

template <typename T>
struct random_exponent_impl<std::complex<T>>
{
    std::complex<T> operator()()
    {
        static const T pi = EIGEN_PI;
        // zi distributed on unit disk
        const auto zi = std::polar(m_rng(), pi * (m_rng() - T(0.5)));
        return -Eigen::numext::log(zi);
    }

private:
    random_real_scalar<T> m_rng;
};

template <typename T>
struct random_weight_impl
{
    T operator()()
    {
        return m_rng(); // distributed on [0, 1]
    }

private:
    random_real_scalar<T> m_rng;
};

template <typename T>
struct random_weight_impl<std::complex<T>>
{
    std::complex<T> operator()()
    {
        // distributed on [-1, 1] + i[-1,1]
        return std::complex<T>(T(2) * m_rng() - 1, T(2) * m_rng() - T(1));
    }

private:
    random_real_scalar<T> m_rng;
};

} // namespace detail

///
/// ### ExponentialSumBase
///
/// Base class for expressions of exponential sum function
///
template <typename Derived>
class ExponentialSumBase
{
public:
    using Index  = Eigen::Index;
    using Traits = detail::ExponentialSumTraits<Derived>;

    using ExponentsArray = typename Traits::ExponentsArray;
    using WeightsArray   = typename Traits::WeightsArray;

    using ExponentScalar = typename ExponentsArray::Scalar;
    using WeightScalar   = typename WeightsArray::Scalar;

    using ExponentsArrayNested =
        typename Eigen::internal::ref_selector<ExponentsArray>::type;
    using WeightsArrayNested =
        typename Eigen::internal::ref_selector<WeightsArray>::type;

    using PlainExponentsArray =
        Eigen::Array<typename ExponentsArray::Scalar, Eigen::Dynamic, 1>;
    using PlainWeightsArray =
        Eigen::Array<typename WeightsArray::Scalar, Eigen::Dynamic, 1>;

    /// \return reference to the derived object
    Derived& derived()
    {
        return *static_cast<Derived*>(this);
    }
    /// \return const reference to the derived object
    const Derived& derived() const
    {
        return *static_cast<const Derived*>(this);
    }

    /// \return the number of exponential terms
    Index size() const
    {
        assert(exponents().size() == weights().size());
        return exponents().size();
    }

    /// \return value of the multi-exponential function at given argument x
    template <typename ArgT>
    auto operator()(const ArgT& x) const
        -> decltype(ExponentScalar() * WeightScalar())
    {
        // return derived().evalAt(x);
        return ((-x * exponents()).exp() * weights()).sum();
    }

    /// \return const reference to the array of exponents
    ExponentsArrayNested exponents() const
    {
        return derived().exponents();
    }

    /// \return reference to the array of weights
    WeightsArrayNested weights() const
    {
        return derived().weights();
    }

    /// \return value or reference of `i`-th exponents
    typename ExponentsArray::CoeffReturnType exponent(Index i) const
    {
        assert(Index() <= i && i < size());
        return exponents().coeff(i);
    }

    /// \return value or reference of `i`-th weight
    typename WeightsArray::CoeffReturnType weight(Index i) const
    {
        assert(Index() <= i && i < size());
        return weights().coeff(i);
    }
};

/// Output stream operator for `ExponentialSumBase`
template <typename Ch, typename Tr, typename Derived>
std::basic_ostream<Ch, Tr>&
operator<<(std::basic_ostream<Ch, Tr>& os,
           const ExponentialSumBase<Derived>& expsum)
{
    auto n = expsum.size();
    os << n << '\n';
    for (decltype(n) i = 0; i < n; ++i)
    {
        os << expsum.exponent(i) << '\t' << expsum.weight(i) << '\n';
    }

    return os;
}

///
/// Remove terms in exponential sum satisfying the specific criteria
///
/// \param[in] esum original exponential sum,
///   \f$f(t)=\sum_{j=1}^{n}c_{j}e^{-a_{j}t}.\f$
/// \param[in] pred unary predicate which returns `true` if the term should
///   be removed. The signature of the predicate function should be equivalent
///   to the following:
///   ``` c++
///     bool pred(const Scalar& aj, const Scalar& cj);
///   ```
///   where `aj` and `cj` are j-th exponent, \f$a_{j},\$ and coefficient
///   \f$c_{j},\f$ respectively, and `Scalar` is the scalar type of original
///   exponential sum, `esum`. The signature does not need to have `const &`,
///   but the function must not modify the objects passed to it.
///
/// \return an object of ExponentialSum with same scalar types for `esum`.
///
template <typename Derived, typename Predicate>
ExponentialSum<typename ExponentialSumBase<Derived>::ExponentScalar,
               typename ExponentialSumBase<Derived>::WeightScalar>
removeIf(const ExponentialSumBase<Derived>& esum, Predicate pred)
{
    using Index      = Eigen::Index;
    using IndexArray = Eigen::Array<Index, Eigen::Dynamic, 1>;
    using ResultType =
        ExponentialSum<typename ExponentialSumBase<Derived>::ExponentScalar,
                       typename ExponentialSumBase<Derived>::WeightScalar>;
    using Eigen::numext::abs;
    using Eigen::numext::real;

    if (esum.size() == Index())
    {
        return ResultType();
    }

    IndexArray index(IndexArray::LinSpaced(esum.size(), 0, esum.size() - 1));

    auto* last =
        std::remove_if(index.data(), index.data() + index.size(), [&](Index x) {
            return pred(esum.exponent(x), esum.weight(x));
        });

    Index n = static_cast<Index>(last - index.data());
    ResultType ret(n);
    for (Index i = 0; i < n; ++i)
    {
        ret.exponent(i) = esum.exponent(index(i));
        ret.weight(i)   = esum.weight(index(i));
    }

    return ret;
}

//==============================================================================
// ExponentialSum class
//==============================================================================
namespace detail
{

template <typename ExpScalarT, typename WScalarT>
struct ExponentialSumTraits<ExponentialSum<ExpScalarT, WScalarT>>
{
    using ExponentsArray = Eigen::Array<ExpScalarT, Eigen::Dynamic, 1>;
    using WeightsArray   = Eigen::Array<WScalarT, Eigen::Dynamic, 1>;
};

} // namespace detail

///
/// ### ExponentialSum
///
/// Representation of an exponential sum function with its storage.
///
/// \tparam T  the scalar type for parameters
/// \tparam Size_ size of internal arrays to store exponents and weights. Set
///     Eigen::Dynamic for changing the size dynamically. Default is
///     Eigen::Dynamic.
/// \tparam MaxSize_ maximum size of internal arrays. Default is `Size_`.
///
template <typename ExpScalarT, typename WScalarT>
class ExponentialSum
    : public ExponentialSumBase<ExponentialSum<ExpScalarT, WScalarT>>
{
    using Base = ExponentialSumBase<ExponentialSum<ExpScalarT, WScalarT>>;

public:
    using Index = Eigen::Index;

    using ExponentsArray = typename Base::ExponentsArray;
    using WeightsArray   = typename Base::WeightsArray;
    using ExponentScalar = typename ExponentsArray::Scalar;
    using WeightScalar   = typename WeightsArray::Scalar;

protected:
    using IndexArray = Eigen::Array<Index, Eigen::Dynamic, 1>;

    ExponentsArray m_exponents;
    WeightsArray m_weights;

public:
    /// Default constructor
    ExponentialSum() = default;

    /// Create an exponential sum function with number of terms
    explicit ExponentialSum(Index n) : m_exponents(n), m_weights(n)
    {
    }

    /// Create an exponential sum function from other expression
    template <typename Derived>
    explicit ExponentialSum(const ExponentialSumBase<Derived>& other)
        : m_exponents(other.exponents()), m_weights(other.weights())
    {
    }

    /// Copy constructor
    ExponentialSum(const ExponentialSum&) = default;

    /// Move constructor
    ExponentialSum(ExponentialSum&&) = default;

    /// Destuctor
    ~ExponentialSum() = default;

    /// Copy assignment operator
    ExponentialSum& operator=(const ExponentialSum&) = default;

    /// Move assignment operator
    ExponentialSum& operator=(ExponentialSum&&) = default;

    /// Create an exponential sum function from expressions of arrays
    template <typename Derived1, typename Derived2>
    explicit ExponentialSum(const Eigen::DenseBase<Derived1>& exponents_,
                            const Eigen::DenseBase<Derived2>& weights_)
        : m_exponents(exponents_), m_weights(weights_)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived1);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived2);
        eigen_assert(exponents_.size() == weights_.size() &&
                     "Size of exponents and weights array must be the same");
    }

    /// \return const reference to the array of exponents
    const ExponentsArray& exponents() const
    {
        return m_exponents;
    }

    /// \return  reference to the array of exponents
    ExponentsArray& exponents()
    {
        return m_exponents;
    }

    /// \return const reference to the array of exponents
    const WeightsArray& weights() const
    {
        return m_weights;
    }

    /// \return  reference to the array of exponents
    WeightsArray& weights()
    {
        return m_weights;
    }

    /// Set the number of exponential terms
    void resize(Index n)
    {
        m_exponents.resize(n);
        m_weights.resize(n);
    }

    void swap(ExponentialSum& other)
    {
        m_exponents.swap(other.m_exponents);
        m_weights.swap(other.m_weights);
    }

    using Base::size;
    using Base::exponent;
    using Base::weight;
    using Base::operator();

    ExponentScalar& exponent(Index i)
    {
        return m_exponents(i);
    }

    WeightScalar& weight(Index i)
    {
        return m_weights(i);
    }

    /// sort exponents \f$ \xi_i \f$ and weights \f$ w_i \f$ by the ratio
    /// \f$|w_i| / |\mathrm{Re}(\xi)|\f$
    void sortByDominanceRatio()
    {
        using Eigen::numext::abs;
        using Eigen::numext::real;
        using std::swap;

        if (size() <= Index(1))
        {
            return;
        }

        IndexArray order(IndexArray::LinSpaced(size(), 0, size() - 1));

        std::sort(order.data(), order.data() + order.size(),
                  [this](Index x, Index y) {
                      return abs(m_weights(x)) / abs(real(m_exponents(x))) >
                             abs(m_weights(y)) / abs(real(m_exponents(y)));
                  });

        ExponentsArray e_tmp(m_exponents);
        WeightsArray w_tmp(m_weights);
        for (Index i = 0; i < size(); ++i)
        {
            m_exponents(i) = e_tmp(order(i));
            m_weights(i)   = w_tmp(order(i));
        }
    }

    ///
    /// Set exponents and weights to a random number.
    ///
    /// Exponents \f$a_{i}\f$ are distributed on the right half-plane, i.e.,
    /// \f$Re(a_{i})>0,\f$ while weights are located at the region \f$w_{i}\in
    /// [0,1] + i[-1,1].\f$
    ///
    void setRandom()
    {
        static detail::random_exponent_impl<ExponentScalar> rnd_c;
        static detail::random_weight_impl<WeightScalar> rnd_w;

        for (Index i = 0; i < size(); ++i)
        {
            m_exponents(i) = rnd_c();
            m_weights(i)   = rnd_w();
        }
    }

    ///
    /// Merge the terms with same exponents on an exponential sum.
    ///
    /// \param[in] pred binary predicate which returns `true` if two
    ///   exponents can be regarded as the same. The signature of the predicate
    ///   function should be equivalent to the following:
    ///
    ///   ``` c++
    ///     bool pred(const Scalar& AI, const Scalar& aj);
    ///   ```
    ///
    ///   where `ai` and `aj` are the i-th and j-th exponentand `Scalar` is the
    ///   scalar type of original exponential sum, `esum`. The signature does
    ///   not need to have `const &`, but the function must not modify the
    ///   objects passed to it.
    ///
    void
    uniqueExponents(typename Eigen::NumTraits<ExponentScalar>::Real tolerance)
    {
        if (size() <= Index(1))
        {
            return;
        }
        IndexArray order(IndexArray::LinSpaced(size(), 0, size() - 1));
        std::sort(order.data(), order.data() + order.size(),
                  [this](Index x, Index y) {
                      return std::make_tuple(std::abs(m_exponents(x)),
                                             std::arg(m_exponents(x))) >
                             std::make_tuple(std::abs(m_exponents(y)),
                                             std::arg(m_exponents(y)));
                  });

        Index first = 0;
        Index ret   = first;
        Index last  = size();
        while (++first != last)
        {
            const auto lhs = m_exponents(order(ret));
            const auto rhs = m_exponents(order(first));
            // if (pred(m_exponents(order(ret)), m_exponents(order(first))))
            if (std::abs(rhs - lhs) <= tolerance * std::abs(rhs) ||
                std::abs(rhs - lhs) <= tolerance * std::abs(lhs))
            {
                m_weights(order(first)) += m_weights(order(ret));
            }
            else if (++ret != first)
            {
                m_exponents(order(ret)) = m_exponents(order(first));
                m_weights(order(ret))   = m_weights(order(first));
            }
        }
        ++ret;

        m_exponents.conservativeResize(ret);
        m_weights.conservativeResize(ret);

        return;
    }
};

//==============================================================================
// ExponentialSumWrapper class
//==============================================================================
namespace detail
{

template <typename ExpArrayT, typename WArrayT>
struct ExponentialSumTraits<ExponentialSumWrapper<ExpArrayT, WArrayT>>
{
    using ExponentsArray = ExpArrayT;
    using WeightsArray   = WArrayT;
};

} // namespace: detail

///
/// ### ExponentialSumWrapper
///
/// Expression of an exponential sum function formed by wrapping existing array
/// expressions.
///
template <typename ExpArrayT, typename WArrayT>
class ExponentialSumWrapper
    : public ExponentialSumBase<ExponentialSumWrapper<ExpArrayT, WArrayT>>
{
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(ExpArrayT);
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(WArrayT);
    using Base = ExponentialSumBase<ExponentialSumWrapper<ExpArrayT, WArrayT>>;

public:
    using Index = typename Base::Index;

    using ExponentsArray = typename Base::ExponentsArray;
    using WeightsArray   = typename Base::WeightsArray;
    using ExponentsArrayNested =
        typename Eigen::internal::ref_selector<ExponentsArray>::type;
    using WeightsArrayNested =
        typename Eigen::internal::ref_selector<WeightsArray>::type;

protected:
    ExponentsArrayNested m_exponents;
    WeightsArrayNested m_weights;

public:
    /// Create an exponential sum function from expressions of arrays
    explicit ExponentialSumWrapper(const ExponentsArray& exponents_,
                                   const WeightsArray& weights_)
        : m_exponents(exponents_), m_weights(weights_)
    {
        eigen_assert(exponents_.size() == weights_.size() &&
                     "Size of exponents and weights array must be the same");
    }

    /// \return const reference to the array of exponents
    const ExponentsArray& exponents() const
    {
        return m_exponents;
    }

    /// \return const reference to the array of exponents
    const WeightsArray& weights() const
    {
        return m_weights;
    }

    using Base::size;
    using Base::exponent;
    using Base::weight;
    using Base::operator();
};

///
/// Create an instance of `ExponentialSumWrapper` from given arrays.
///
template <typename ExpArrayT, typename WArrayT>
ExponentialSumWrapper<ExpArrayT, WArrayT>
makeExponentialSum(const Eigen::ArrayBase<ExpArrayT>& exponents,
                   const Eigen::ArrayBase<WArrayT>& weights)
{
    return ExponentialSumWrapper<ExpArrayT, WArrayT>(exponents.derived(),
                                                     weights.derived());
}

} // namespace: mxpfit

#endif /* MXPFIT_EXPONENTIAL_SUM_HPP */


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"
#include "graph_properties.hh"
#include "graph_selectors.hh"

#include <boost/python.hpp>

#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct do_all_pairs_search
{
    template <class Graph, class VertexIndexMap, class DistMap, class WeightMap>
    void operator()(const Graph& g, VertexIndexMap vertex_index,
                    DistMap dist_map, WeightMap weight, bool dense) const
    {
        typedef typename property_traits<DistMap>::value_type::value_type
            dist_t;

        parallel_vertex_loop
            (g,
             [&](auto& v)
             {
                 dist_map[v].clear();
                 dist_map[v].resize(num_vertices(g), 0);
             });

        if (dense)
        {
            floyd_warshall_all_pairs_shortest_paths
                (g, dist_map,
                 weight_map(ConvertedPropertyMap<WeightMap,dist_t>(weight)).
                 vertex_index_map(vertex_index));
        }
        else
        {
            johnson_all_pairs_shortest_paths
                (g, dist_map,
                 weight_map(ConvertedPropertyMap<WeightMap,dist_t>(weight)).
                 vertex_index_map(vertex_index));
        }
    }
};

struct do_all_pairs_search_unweighted
{
    template <class DistMap, class PredMap>
    class bfs_visitor: public boost::bfs_visitor<null_visitor>
    {
    public:
        bfs_visitor(DistMap& dist_map, PredMap& pred, size_t source)
        : _dist_map(dist_map), _pred(pred), _source(source) {}

        template <class Graph>
        void initialize_vertex(typename graph_traits<Graph>::vertex_descriptor v,
                               Graph&)
        {
            typedef typename DistMap::value_type dist_t;
            dist_t inf = std::is_floating_point<dist_t>::value ?
                numeric_limits<dist_t>::infinity() :
                numeric_limits<dist_t>::max();
            _dist_map[v] = (v == _source) ? 0 : inf;
            _pred[v] = v;
        }

        template <class Graph>
        void tree_edge(const typename graph_traits<Graph>::edge_descriptor& e,
                       Graph& g)
        {
            _pred[target(e,g)] = source(e,g);
        }

        template <class Graph>
        void discover_vertex(typename graph_traits<Graph>::vertex_descriptor v,
                             Graph&)
        {
            if (size_t(_pred[v]) == v)
                return;
            _dist_map[v] = _dist_map[_pred[v]] + 1;
        }

    private:
        DistMap& _dist_map;
        PredMap& _pred;
        size_t _source;
    };

    template <class Graph, class DistMap>
    void operator()(const Graph& g, DistMap dist_map) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
        typedef typename property_traits<DistMap>::value_type dist_t;

        vector<vertex_t> pred_map(num_vertices(g));
        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            firstprivate(pred_map)
        parallel_vertex_loop_no_spawn
                (g,
                 [&](auto v)
                 {
                     dist_map[v].resize(num_vertices(g), 0);
                     bfs_visitor<dist_t,vector<size_t>>
                         vis(dist_map[v], pred_map, v);
                     breadth_first_search(g, v, visitor(vis));
                 });
    }
};

void get_all_dists(GraphInterface& gi, boost::any dist_map, boost::any weight,
                   bool dense)
{
    if (weight.empty())
    {
        run_action<>()
            (gi, std::bind(do_all_pairs_search_unweighted(),
                           std::placeholders::_1, std::placeholders::_2),
             vertex_scalar_vector_properties())
            (dist_map);
    }
    else
    {
        run_action<>()
            (gi, std::bind(do_all_pairs_search(), std::placeholders::_1,
                           gi.get_vertex_index(), std::placeholders::_2,
                           std::placeholders::_3, dense),
             vertex_scalar_vector_properties(),
             edge_scalar_properties())
            (dist_map, weight);
    }
}

void export_all_dists()
{
    python::def("get_all_dists", &get_all_dists);
};


/*!
  @file   PCA.cpp
  @author David Hirvonen
  @brief  Internal PCA class implementation.

  \copyright Copyright 2014-2016 Elucideye, Inc. All rights reserved.
  \license{This project is released under the 3 Clause BSD License.}

*/

#include "drishti/ml/PCA.h"
#include "drishti/core/make_unique.h"

#include <Eigen/Dense>

DRISHTI_ML_NAMESPACE_BEGIN

// ########## Scaling params ############

StandardizedPCA::Standardizer::Standardizer() = default;
StandardizedPCA::Standardizer::~Standardizer() = default;
StandardizedPCA::Standardizer::Standardizer(int size, int type)
{
    create(size, type);
}

void StandardizedPCA::Standardizer::create(int size, int type)
{
    mu.create(1, size, type);
    sigma.create(1, size, type);
}

void StandardizedPCA::Standardizer::compute(const cv::Mat& src)
{
    create(src.cols, CV_32FC1);
    float *pMu = mu.ptr<float>(), *pSigma = sigma.ptr<float>();
    for (int i = 0; i < src.cols; i++, pMu++, pSigma++)
    {
        cv::Scalar mu, sigma;
        cv::Mat x = src.col(i);
        cv::meanStdDev(x, mu, sigma);
        pMu[0] = mu[0];
        pSigma[0] = sigma[0];
    }
}

cv::Mat StandardizedPCA::Standardizer::standardize(const cv::Mat& data) const
{
    cv::Mat tmp_sigma = cv::repeat(sigma, data.rows / sigma.rows, data.cols / sigma.cols);
    cv::Mat tmp_mean = cv::repeat(mu, data.rows / mu.rows, data.cols / mu.cols), tmp_data;
    int ctype = mu.type();
    if (data.type() != ctype || tmp_mean.data == mu.data)
    {
        data.convertTo(tmp_data, ctype);
        cv::subtract(tmp_data, tmp_mean, tmp_data);
        cv::divide(tmp_data, tmp_sigma, tmp_data);
    }
    else
    {
        cv::subtract(data, tmp_mean, tmp_mean);
        cv::divide(tmp_mean, tmp_sigma, tmp_data);
    }
    return tmp_data;
}

cv::Mat StandardizedPCA::Standardizer::unstandardize(const cv::Mat& data) const
{
    cv::Mat tmp_sigma = cv::repeat(sigma, data.rows / sigma.rows, data.cols / sigma.cols);
    cv::Mat tmp_mean = cv::repeat(mu, data.rows / mu.rows, data.cols / mu.cols), tmp_data;
    int ctype = mu.type();
    if (data.type() != ctype || tmp_mean.data == mu.data)
    {
        data.convertTo(tmp_data, ctype);
        cv::multiply(tmp_data, tmp_sigma, tmp_data);
        cv::add(tmp_data, tmp_mean, tmp_data);
    }
    else
    {
        cv::multiply(data, tmp_sigma, tmp_data);
        cv::add(tmp_data, tmp_mean, tmp_data);
    }
    return tmp_data;
}

// ########### ScalePCA #############

StandardizedPCA::StandardizedPCA() = default;
StandardizedPCA::~StandardizedPCA() = default;

size_t StandardizedPCA::getNumComponents() const
{
    return m_transform.mu.cols;
}

void StandardizedPCA::compute(const cv::Mat& data, cv::Mat& projection, float retainedVariance)
{
    cv::Mat mu;
    m_transform.compute(data);
    cv::Mat data_ = m_transform.standardize(data);
    m_pca = drishti::core::make_unique<cv::PCA>(data_, mu, cv::PCA::DATA_AS_ROW, retainedVariance);
    m_pca->project(data_, projection);

    init();
}

void StandardizedPCA::compute(const cv::Mat& data, cv::Mat& projection, int maxComponents)
{
    cv::Mat mu;
    m_transform.compute(data);
    cv::Mat data_ = m_transform.standardize(data);
    m_pca = drishti::core::make_unique<cv::PCA>(data_, mu, cv::PCA::DATA_AS_ROW, maxComponents);
    m_pca->project(data_, projection);

    init();
}

void StandardizedPCA::init()
{
    // Cache transposed vectors for faster multiplication:
    if (!m_pca->eigenvectors.empty())
    {
        m_eT = m_pca->eigenvectors.t();
    }
}

cv::Mat StandardizedPCA::project(const cv::Mat& samples, int n) const
{
    cv::Mat samples_ = m_transform.standardize(samples), projection;
    if (n > 0)
    {
        // Construct partial eigenvectors
        cv::Mat eigenvectors = m_pca->eigenvectors({ 0, n }, cv::Range::all());
        cv::Mat eigenvalues = m_pca->eigenvalues({ 0, n }, cv::Range::all());
        cv::Mat mean = m_pca->mean;

        cv::Mat data = samples_;
        CV_Assert(!mean.empty() && !eigenvectors.empty() && ((mean.rows == 1 && mean.cols == data.cols) || (mean.cols == 1 && mean.rows == data.rows)));
        cv::Mat tmp_data, tmp_mean = cv::repeat(mean, data.rows / mean.rows, data.cols / mean.cols);
        int ctype = mean.type();
        if (data.type() != ctype || tmp_mean.data == mean.data)
        {
            data.convertTo(tmp_data, ctype);
            cv::subtract(tmp_data, tmp_mean, tmp_data);
        }
        else
        {
            cv::subtract(data, tmp_mean, tmp_mean);
            tmp_data = tmp_mean;
        }
        if (mean.rows == 1)
        {
            cv::gemm(tmp_data, eigenvectors, 1, {}, 0, projection, cv::GEMM_2_T);
        }
        else
        {
            cv::gemm(eigenvectors, tmp_data, 1, {}, 0, projection, 0);
        }
    }
    else
    {
        m_pca->project(samples_, projection);
    }
    return projection;
}

void StandardizedPCA::gemm_transpose(const cv::Mat& A, const cv::Mat& Bt, cv::Mat& result)
{
    assert(A.type() == CV_32F);
    assert(A.isContinuous());
    assert(A.cols == Bt.cols);
    assert(Bt.type() == CV_32F);

    using DynamicStride = Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>;
    using MatrixRowMajor = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    using MatrixColMajor = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
    using MapMatrixRowMajor = Eigen::Map<MatrixRowMajor, Eigen::Unaligned>;
    using MapMatrixColMajor = Eigen::Map<MatrixColMajor, Eigen::Unaligned, DynamicStride>;

    result.create(A.rows, Bt.rows, CV_32F);
    MapMatrixRowMajor C1(result.ptr<float>(), result.rows, result.cols);
    MapMatrixRowMajor A1(const_cast<float*>(A.ptr<float>()), A.rows, A.cols);
    MapMatrixColMajor B1t(const_cast<float*>(Bt.ptr<float>()), /*n*/ Bt.cols, Bt.rows, DynamicStride(Bt.step1(), 1));
    C1 = A1 * B1t;
}

cv::Mat StandardizedPCA::backProject(const cv::Mat& projection) const
{
    cv::Mat result;
    int n = projection.cols;
    if (n != m_pca->eigenvectors.cols)
    {
        // Construct partial eigenvectors
        cv::Mat eigenvectors = m_pca->eigenvectors({ 0, n }, cv::Range::all());
        cv::Mat eigenvalues = m_pca->eigenvalues({ 0, n }, cv::Range::all());
        cv::Mat mean = m_pca->mean;

        cv::Mat data = projection;
        CV_Assert(!mean.empty() && !eigenvectors.empty() && ((mean.rows == 1 && eigenvectors.rows == data.cols) || (mean.cols == 1 && eigenvectors.rows == data.rows)));

        cv::Mat tmp_data, tmp_mean;
        if (data.type() != tmp_data.type())
        {
            data.convertTo(tmp_data, mean.type()); // 12%
        }
        else
        {
            tmp_data = data;
        }
        if (mean.rows == 1)
        {
            tmp_mean = cv::repeat(mean, data.rows, 1);

            // Eigen multiplication ( no copy )
            const cv::Mat& A = tmp_data;
            const cv::Mat& Bt = m_eT;
            gemm_transpose(A, Bt({ 0, Bt.rows }, { 0, n }), result);
        }
        else
        {
            tmp_mean = cv::repeat(mean, 1, data.cols);
            cv::gemm(eigenvectors, tmp_data, 1, tmp_mean, 1, result, cv::GEMM_1_T);
        }
    }
    else
    {
        result = m_pca->backProject(projection);
    }

    result = m_transform.unstandardize(result);

    return result;
}

DRISHTI_ML_NAMESPACE_END


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file k_cut.hpp
 * @brief
 * @author Piotr Smulewicz, Piotr Godlewski
 * @version 1.0
 * @date 2013-09-25
 */
#ifndef PAAL_K_CUT_HPP
#define PAAL_K_CUT_HPP

#include "paal/utils/functors.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/utils/irange.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/graph/subgraph.hpp>
#include <boost/range/as_array.hpp>

#include <queue>

namespace paal {
namespace greedy {

/**
 * @brief this is solve k_cut problem
 * and return cut_cost
 * example:
 *  \snippet k_cut_example.cpp K Cut Example
 *
 * example file is k_cut_example.cpp
 * @param graph
 * @param number_of_parts
 * @param result pairs of vertex_descriptor and number form (1,2,
* ... ,k) id of part
 * @param index_map
 * @param weight_map
 * @tparam InGraph
 * @tparam OutputIterator
 * @tparam VertexIndexMap
 * @tparam EdgeWeightMap
 */
template<typename InGraph, class OutputIterator, typename VertexIndexMap, typename EdgeWeightMap>
auto k_cut(const InGraph& graph, unsigned int number_of_parts,OutputIterator result,
            VertexIndexMap index_map, EdgeWeightMap weight_map) ->
            typename boost::property_traits<EdgeWeightMap>::value_type{
    using cost_t = typename boost::property_traits<EdgeWeightMap>::value_type;
    using Vertex = typename boost::graph_traits<InGraph>::vertex_descriptor;

    using Graph = boost::adjacency_list<
        boost::vecS, boost::vecS, boost::undirectedS, boost::no_property,
        boost::property<boost::edge_weight_t, cost_t,
                        boost::property<boost::edge_index_t, int>>>;

    assert(num_vertices(graph) >= number_of_parts);

    std::vector<int> vertex_to_part(num_vertices(graph));
    using VertexIndexToVertex = typename std::vector<Vertex>;
    using VertexIndexToVertexIndex = std::vector<int>;
    VertexIndexToVertex vertex_in_subgraph_to_vertex(num_vertices(graph));
    VertexIndexToVertexIndex vertex_to_vertex_in_subgraph(num_vertices(graph));
    int vertex_in_part;
    int parts = 1;
    // cuts contain pair(x,y)
    // x is the cost of the cut
    // y and y+1 are index parts of graph after make a cut
    std::priority_queue<
            std::pair<cost_t,int>,
            std::vector<std::pair<cost_t,int> >
            ,utils::greater> cuts;

    int id_part = 0;

    //get part id and compute minimum cost of cut of that part and add it to queue
    auto make_cut = [&](int id) {
        vertex_in_part=0;
        for (auto v: boost::as_array(vertices(graph))) {
            if (vertex_to_part[get(index_map, v)] == id) {
                vertex_in_subgraph_to_vertex[vertex_in_part] = v;
                vertex_to_vertex_in_subgraph[get(index_map, v)] = vertex_in_part;
                ++vertex_in_part;
            }
        }
        Graph part(vertex_in_part);
        for (auto edge : boost::as_array(edges(graph))) {
            auto sour = get(index_map, source(edge,graph));
            auto targ = get(index_map, target(edge,graph));
            if (vertex_to_part[sour] == id &&
                    vertex_to_part[targ] == id &&
                    sour != targ) {
                add_edge(vertex_to_vertex_in_subgraph[sour],
                         vertex_to_vertex_in_subgraph[targ],
                         get(weight_map, edge),
                         part);
            }
        }
        if (vertex_in_part < 2) {
            ++id_part;
            *result = std::make_pair(vertex_in_subgraph_to_vertex[0], id_part);
            ++result;
            return;
        }
        auto parities = boost::make_one_bit_color_map(num_vertices(part),
                                            get(boost::vertex_index, part));
        auto cut_cost = boost::stoer_wagner_min_cut(part,
                                          get(boost::edge_weight, part),
                                          boost::parity_map(parities));

        for (auto i : irange(num_vertices(part))) {
            vertex_to_part[get(index_map, vertex_in_subgraph_to_vertex[i])] =
                    parts + get(parities, i); //return value convertable to 0/1
        }
        cuts.push(std::make_pair(cut_cost, parts));
        parts += 2;
    };

    make_cut(0);
    cost_t k_cut_cost = cost_t();
    while (--number_of_parts) {
        auto cut = cuts.top();
        cuts.pop();
        k_cut_cost += cut.first;
        make_cut(cut.second);
        make_cut(cut.second + 1);
    }

    while (!cuts.empty()) {
        auto cut = cuts.top();
        cuts.pop();
        ++id_part;
        for (auto v: boost::as_array(vertices(graph))) {
            if (vertex_to_part[get(index_map, v)] == cut.second ||
                    vertex_to_part[get(index_map, v)] == cut.second + 1) {
                *result = std::make_pair(v, id_part);
                ++result;
            }
        }
    }
    return k_cut_cost;
}

/**
 * @brief this is solve k_cut problem
 * and return cut_cost
 * example:
 *  \snippet k_cut_example.cpp K Cut Example
 *
 * example file is k_cut_example.cpp
 * @param graph
 * @param number_of_parts
 * @param result pairs of vertex_descriptor and number form (1,2, ... ,k) id of part
 * @param params
 * @tparam InGraph
 * @tparam OutputIterator
 * @tparam T
 * @tparam P
 * @tparam R
 */
template<typename InGraph
        ,class OutputIterator
        ,typename T
        ,typename P
        ,typename R>
auto k_cut(const InGraph& graph, unsigned int number_of_parts,
    OutputIterator result, const boost::bgl_named_params<P, T, R>& params) ->
        typename boost::property_traits<
            puretype(boost::choose_const_pmap(get_param(params, boost::edge_weight), graph, boost::edge_weight))
            >::value_type {
    return k_cut(graph, number_of_parts, result,
        boost::choose_const_pmap(get_param(params, boost::vertex_index), graph,boost::vertex_index),
        boost::choose_const_pmap(get_param(params, boost::edge_weight), graph,boost::edge_weight)
    );
}

/**
 * @brief this is solve k_cut problem
 * and return cut_cost
 * example:
 *  \snippet k_cut_example.cpp K Cut Example
 *
 * example file is k_cut_example.cpp
 * @param graph
 * @param number_of_parts
 * @param result pairs of vertex_descriptor and number form (1,2, ... ,k) id of part
 * @tparam InGraph
 * @tparam OutputIterator
 */
template<typename InGraph, class OutputIterator>
auto k_cut(const InGraph& graph, unsigned int number_of_parts, OutputIterator result) ->
        typename boost::property_traits<puretype(get(boost::edge_weight,graph))>::value_type{
    return k_cut(graph, number_of_parts, result, boost::no_named_parameters());
}

} //!greedy
} //!paal

#endif // PAAL_K_CUT_HPP