#include <hdf5.h>
#include <omp.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <Eigen/Sparse>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <functional>
#include <iomanip>
#include <iostream>
#include <stdexcept>

#include "bte_config.h"
#include "aux/filtered_range.hpp"
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "base/numbers.hpp"
#include "collision_tensor/assembly/gain.hpp"
#include "collision_tensor/collision_tensor_factory.hpp"
#include "collision_tensor/collision_tensor_galerkin.hpp"
#include "collision_tensor/time_stepping/rk4.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "post_processing/energy.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/utility/utility.hpp"

using namespace std;
using namespace boltzmann;
namespace po = boost::program_options;

typedef SpectralBasisFactoryKS basis_factory_t;

int main(int argc, char* argv[])
{
  boltzmann::Timer<> timer;
  double beta, dt;
  int nsteps;
  string tensor_file;
  bool adaptiveL, verbose;

  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")
      ("beta,b", po::value<double>(&beta)->default_value(2), "beta")
      ("dt,t", po::value<double>(&dt)->default_value(0.001), "delta t")
      ("nsteps,n", po::value<int>(&nsteps)->default_value(200), "#timesteps")
      ("adapt,a", po::value<bool>(&adaptiveL)->default_value(false), "use adaptive L-range")
      ("ct,T", po::value<string>(&tensor_file),
      "path to tensor file \n the files `spectral_basis.desc` and \n `spectral_basis_test.desc` "
      "must reside in the same directory")
      ("init,i", po::value<string>()->required(), "coefficients hdf5 in Polar Laguerre basis")
      ("ifdata", po::value<string>()->default_value("coeffs"), " path to data in HDF5 file `if`")
      ("verbose,v", po::value<bool>(&verbose)->default_value(false), "verbose output");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  std::string version_id = GIT_SHA1;
  cout << "VersionID: " << version_id << "@" << GIT_BNAME << std::endl;
  cout << "CMD:\n";
  for (int i = 0; i < argc; ++i) {
    cout << argv[i] << "\t";
  }
  cout << "\n\n\n";

  cout << "Command line parameters::\n"
       << right << setw(15) << "beta" << setw(12) << beta << endl
       << right << setw(15) << "dt" << setw(12) << dt << "\n\n\n";

  typedef typename basis_factory_t::basis_type basis_type;
  typedef boost::filesystem::path path_t;

  // load spectral basis
  basis_type trial_basis;
  path_t tensor_fpath(tensor_file.c_str());
  string trial_basis_fname = (tensor_fpath.parent_path() / path_t("spectral_basis.desc")).string();
  basis_factory_t::create(trial_basis, trial_basis_fname);

  // read tensor from file
  timer.start();
  CollisionTensorGalerkin ct(trial_basis);
  ct.read_hdf5(tensor_file.c_str());
  print_timer(timer.stop(), "read collision tensor from file");
  cout << "------------------------------\n";

  const int N = trial_basis.n_dofs();
  Eigen::VectorXd coeffs(N);
  string fname = vm["init"].as<string>();
  hid_t h5_init = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
  eigen2hdf::load(h5_init, vm["ifdata"].as<string>(), coeffs);

  cout << "---------- MOMENTSs ----------\n";
  Mass mass;
  mass.init(trial_basis);
  double m0 = mass.compute(coeffs.data());
  cout << "\t mass = " << m0 << endl;

  Energy energy;
  energy.init(trial_basis);
  energy.compute(coeffs.data());

  Momentum momentum;
  momentum.init(trial_basis);
  momentum.compute(coeffs.data());
  cout << "\t energy = " << energy.compute(coeffs.data()) << endl;

  // ------------------------------------------------------------
  // TIME STEPPING
  typedef Eigen::VectorXd vec_t;
  vec_t out(N);
  hg::RK4<> rk4(N);
  const double tol = 1e-10;
  auto find_relevant_range = [&](const double* solution) {
    const int L = spectral::get_max_l(trial_basis);
    typedef typename basis_type::elem_t elem_t;
    typedef typename basis_factory_t::fa_type angular_elem_t;
    // start from l=Lmax until  coefficients are above threshold
    const int llower_bound = 2;
    int lupper_bound = 2;
    typename elem_t::Acc::template get<angular_elem_t> get_xir;
    for (int l = L; l > llower_bound; --l) {
      std::function<bool(const elem_t&)> pred = [&](const elem_t& e) {
        return (get_xir(e).get_id().l == l);
      };
      // get current l-range of spectral basis
      auto range = filtered_range(trial_basis.begin(), trial_basis.end(), pred);
      bool is_below_tre = true;
      for (auto it = std::get<0>(range); it != std::get<1>(range); ++it) {
        unsigned int idx = trial_basis.get_dof_index(it->get_id());
        if (std::abs(solution[idx]) > tol) {
          is_below_tre = false;
          break;
        }
      }
      if (!is_below_tre) {
        lupper_bound = l;
        break;
      }
    }
    // look for the upper-bound iterator in trial_basis
    std::function<bool(int, const elem_t& e)> comp = [&](int l, const elem_t& e) {
      return l < get_xir(e).get_id().l;
    };
    auto it_max = std::upper_bound(trial_basis.begin(), trial_basis.end(), lupper_bound, comp);
    if (verbose) cout << "Adpative Basis: L_max = " << lupper_bound << endl;
    // returns nmax => relevant range is (0, nmax)
    return (it_max - trial_basis.begin());
  };

  hid_t file, gdata;
  file = H5Fcreate("coefficients.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  gdata = H5Gcreate1(file, "data", 0);
  eigen2hdf::save(gdata, "0", coeffs);
  H5Fflush(file, H5F_SCOPE_GLOBAL);

  double t = 0;
  for (int i = 0; i < nsteps; ++i) {
    auto coeffsn = coeffs;
    timer.start();
    auto f = [&](double* dst, const double* src) {
      if (adaptiveL) {
        int nmax = find_relevant_range(coeffs.data());
        if (verbose) cout << "Adaptive nmax = " << nmax << endl;
        ct.apply_adaptive(dst, src, nmax);
      } else {
        ct.apply(dst, src);
      }
    };

    rk4.apply(coeffsn.data(), coeffs.data(), f, dt);
    ct.project(coeffsn.data(), coeffs.data());
    print_timer(timer.stop(), "RK4");
    t += dt;
    timer.start();
    coeffs = coeffsn;
    double m = mass.compute(coeffs.data());
    if(std::isnan(m)) {
      H5Gclose(gdata);
      H5Fclose(file);
      throw std::runtime_error("blow up...");
    }
    double e = energy.compute(coeffs.data());
    auto mom = momentum.compute(coeffs.data());
    print_timer(timer.stop(), "compute moments");

    cout << "::MOMENTS::\t" << setw(8) << i + 1 << setw(20) << setprecision(10) << t << setw(30)
         << setprecision(20) << scientific << m << setw(30) << setprecision(20) << scientific << e
         << setw(30) << setprecision(20) << scientific << mom[0] << setw(30) << setprecision(20)
         << scientific << mom[1] << endl;

    // save results to HDF5
    eigen2hdf::save(gdata, boost::lexical_cast<string>(i + 1), coeffs);
    H5Fflush(file, H5F_SCOPE_GLOBAL);
  }

  H5Gclose(gdata);
  H5Fclose(file);

  return 0;
}


#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <string>
#include <chrono>
#include <queue>

using namespace Eigen;

typedef struct {
  long n;
  long m;
  unsigned int *rowOffsets;
  unsigned int *adj;
  long n_coarse;
  long m_coarse;
  unsigned int *rowOffsetsCoarse;
  unsigned int *adjCoarse;
  int *coarseID;
  double *eweights;
} graph_t;

template <typename T>
std::string to_string_with_precision(const T a_value, const int n = 8) {

  std::ostringstream out;
  out << std::setprecision(n) << a_value;
  return out.str();
}

static int
vu_cmpfn_inc(const void *a, const void *b) {

  int *av = ((int *) a);
  int *bv = ((int *) b);
  if (*av > *bv)
    return 1;
  if (*av < *bv)
    return -1;
  if (*av == *bv) {
    if (av[1] > bv[1])
      return 1;
    if (av[1] < bv[1])
      return -1;
  }
  return 0;
}

static int
simpleCoarsening(graph_t *g, int coarseningType) {

  if (coarseningType == 0)
    return 0;

  int num_coarsening_rounds_max = 100;
  int coarse_graph_nmax = 1000;

  int *cID = (int *) malloc((g->n)*sizeof(int));
  assert(cID != NULL);
  int *toMatch = (int *) malloc((g->n)*sizeof(int));
  assert(toMatch != NULL);

#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (long i=0; i<g->n; i++) {
    cID[i] = i;
    toMatch[i] = 1;
  }

  int coarse_vert_count = g->n;
  int num_rounds = 0;
  while ((coarse_vert_count > coarse_graph_nmax) &&
         (num_rounds < num_coarsening_rounds_max)) {
    num_rounds++;
    int num_matched = 0;
    for (int i=0; i<g->n; i++) {
      int u = i;
      while (cID[u] != u) {
        cID[u] = cID[cID[u]];
        u = cID[u];
      }
      if (toMatch[u] == 1) {
        for (unsigned int j=g->rowOffsets[u];
                          j<g->rowOffsets[u+1]; j++) {
          int v = g->adj[j];
          while (v != cID[v]) {
            cID[v] = cID[cID[v]];
            v = cID[v];
          }
          if (v == u) {
            continue;
          }
          if (toMatch[v] == 1) {
            if (u < v) {
              cID[u] = u;
              cID[v] = u;
            } else {
              cID[u] = v;
              cID[v] = v;
            }
            toMatch[u] = toMatch[v] = 0;
            num_matched += 2;
            break;
          }
        }
      }
    }

    int num_unmatched = coarse_vert_count - num_matched;
    int new_coarse_vert_count = num_matched/2 + num_unmatched;
    fprintf(stderr, "prev count: %d, matched: %d, new count: %d\n",
             coarse_vert_count, num_matched, new_coarse_vert_count);
             coarse_vert_count = new_coarse_vert_count;

    for (int i=0; i<g->n; i++) {
      toMatch[i] = 1;
    }
  }
  fprintf(stderr, "num rounds: %d\n", num_rounds);

  int *coarse_edges = (int *) malloc(2 * g->m * sizeof(int));
  assert(coarse_edges != NULL);

  /* set correct IDs */
  for (int i=0; i<g->n; i++) {
    int u = cID[i];
    while (u != cID[u]) {
      u = cID[cID[u]];
    }
    cID[i] = u;
  }

  int *vertIDs = (int *) malloc(g->n * sizeof(int));
  assert(vertIDs != NULL);
  for (int i=0; i<g->n; i++) {
    vertIDs[i] = -1;
  }

  int new_id = 0;
  for (int i=0; i<g->n; i++) {
    if (cID[i] == i) {
      vertIDs[i] = new_id++;
    }
  }
  assert(new_id == coarse_vert_count);
  for (int i=0; i<g->n; i++) {
    if (vertIDs[i] == -1) {
      vertIDs[i] = vertIDs[cID[i]];
    }
  }

  long ecount = 0;
  for (int i=0; i<g->n; i++) {
    int u = vertIDs[i];
    for (unsigned int j=g->rowOffsets[i]; j<g->rowOffsets[i+1]; j++) {
      int v = vertIDs[g->adj[j]];
      coarse_edges[ecount++] = u;
      coarse_edges[ecount++] = v;
      // fprintf(stderr, "%d %d\n", u, v);
    }
  }
  ecount = ecount/2;
  qsort(coarse_edges, g->m, 2*sizeof(int), vu_cmpfn_inc);

  /* count the number of coarse edges */
  int m_coarse = 1;
  int prev_u = coarse_edges[0];
  int prev_v = coarse_edges[1];

  for (int i=1; i<ecount; i++) {
    int curr_u = coarse_edges[2*i];
    int curr_v = coarse_edges[2*i+1];
    if ((curr_u != prev_u) || (curr_v != prev_v)) {
      m_coarse++;
      prev_u = curr_u;
      prev_v = curr_v;
    }
  }

  fprintf(stderr, "m_coarse %d\n", m_coarse);

  /* Allocate coarse edge weights array */
  double *eweights;
  eweights = (double *) malloc(m_coarse * sizeof(double));
  assert(eweights != NULL);
  for (int i=0; i<m_coarse; i++) {
    eweights[i] = 1.0;
  }

  unsigned int *rowOffsetsCoarse;
  rowOffsetsCoarse = (unsigned int *)
    malloc((coarse_vert_count+1)*sizeof(unsigned int));
  assert(rowOffsetsCoarse != NULL);
  for (int i=0; i<coarse_vert_count+1; i++) {
    rowOffsetsCoarse[i] = 0;
  }

  unsigned int *adjCoarse;
  adjCoarse = (unsigned int *) malloc(m_coarse*sizeof(unsigned int));
  assert(adjCoarse != NULL);

  /* update coarse edge weights */
  m_coarse = 1;
  // eweights[0] = 1.0;
  prev_u = coarse_edges[0];
  prev_v = coarse_edges[1];
  adjCoarse[0] = prev_v;
  rowOffsetsCoarse[prev_u+1]++;

  for (int i=1; i<ecount; i++) {
    int curr_u = coarse_edges[2*i];
    int curr_v = coarse_edges[2*i+1];
    if ((curr_u != prev_u) || (curr_v != prev_v)) {
      m_coarse++;
      adjCoarse[m_coarse-1] = curr_v;
      // eweights[m_coarse] = 1.0;
      rowOffsetsCoarse[curr_u+1]++;
      prev_u = curr_u;
      prev_v = curr_v;
    } else {
      eweights[m_coarse-1] += 1.0;
    }
  }

  for (int i=1; i<=coarse_vert_count; i++) {
    rowOffsetsCoarse[i] += rowOffsetsCoarse[i-1];
    // fprintf(stderr, "%u ", rowOffsetsCoarse[i]);
  }

  /*
  fprintf(stderr, "printing coarse graph:\n");
  for (int i=0; i<coarse_vert_count; i++) {
    for (int j=rowOffsetsCoarse[i]; j<rowOffsetsCoarse[i+1]; j++) {
      fprintf(stderr, "[%u %u %lf] ", i, adjCoarse[j], eweights[j]);
    }
  }
  */

  /*
  for (int i=0; i<ecount/2; i++) {
    fprintf(stderr, "%d %d\n", coarse_edges[2*i], coarse_edges[2*i+1]);
  }
  */

  free(cID);
  free(toMatch);
  free(coarse_edges);

  g->coarseID = vertIDs;
  g->n_coarse = coarse_vert_count;
  g->m_coarse = m_coarse;
  g->eweights = eweights;
  g->adjCoarse = adjCoarse;
  g->rowOffsetsCoarse = rowOffsetsCoarse;

  if (coarseningType == 1)
    return 0;

  // Optionally write to CSR and MTX files
  std::cout << "Writing csr and mtx files to current directory\n";
  FILE *writeBinaryPtr = fopen( "graph_coarse.csr", "wb");
  if (writeBinaryPtr == NULL) {
    fprintf(stderr, "could not open csr file for writing\n");
    exit(1);
  }

  FILE *outfp_mtx = fopen( "graph_coarse.mtx", "w");
  if (outfp_mtx == NULL) {
    fprintf(stderr, "could not open mtx file for writing\n");
    exit(1);
  }

  long N = g->n_coarse;
  unsigned int *rowOffsetsCoarse_noloops = (unsigned int *)
  malloc((N+1)*sizeof(unsigned int));
  assert(rowOffsetsCoarse_noloops != NULL);
  for (long i=0; i<N+1; i++) {
    rowOffsetsCoarse_noloops[i] = 0;
  }

  long num_self_loops = 0;
  for (long i=0; i<g->n_coarse; i++) {
    for (unsigned int j=g->rowOffsetsCoarse[i];
                      j<g->rowOffsetsCoarse[i+1]; j++) {
      unsigned int v = g->adjCoarse[j];
      if (((unsigned int) i) == v) {
        num_self_loops++;
      } else {
        rowOffsetsCoarse_noloops[i+1]++;
      }
    }
  }

  for (long i=1; i<N+1; i++) {
    rowOffsetsCoarse_noloops[i] += rowOffsetsCoarse_noloops[i-1];
  }

  long M = g->m_coarse - num_self_loops;
  std::cout << "edge count after loop removal: " << M << std::endl;

  fprintf(outfp_mtx, "%%%%MatrixMarket matrix coordinate pattern symmetric\n");
  fprintf(outfp_mtx, "%ld %ld %ld\n", N, N, M);

  unsigned int *adjCoarse_noloops = (unsigned int *)
    malloc(M*sizeof(unsigned int));
  assert(adjCoarse_noloops != NULL);

  long ec = 0;
  for (long i=0; i<g->n_coarse; i++) {
    for (unsigned int j=g->rowOffsetsCoarse[i];
                      j<g->rowOffsetsCoarse[i+1]; j++) {
      unsigned int v = g->adjCoarse[j];
      if (((unsigned int) i) == v) {
        num_self_loops++;
      } else {
        adjCoarse_noloops[ec++] = v;
        fprintf(outfp_mtx, "%ld %u\n", i+1, v+1);
      }
    }
  }
  assert(ec == M);
  fclose(outfp_mtx);

  long undirected = 1;
  long graph_type = 0;
  long one_indexed = 0;
  long verification_graph = 0;

  fwrite ( &N, sizeof(long), 1, writeBinaryPtr );
  fwrite ( &M, sizeof(long), 1, writeBinaryPtr );
  fwrite ( &undirected, sizeof(long), 1, writeBinaryPtr );
  fwrite ( &graph_type , sizeof(long), 1, writeBinaryPtr );
  fwrite ( &one_indexed , sizeof(long), 1, writeBinaryPtr );
  fwrite ( &verification_graph , sizeof(long), 1, writeBinaryPtr );

  fwrite ( rowOffsetsCoarse_noloops, sizeof(unsigned int), (N+1), writeBinaryPtr );
  fwrite ( adjCoarse_noloops, sizeof(unsigned int), M, writeBinaryPtr );

  fclose( writeBinaryPtr );

  free(rowOffsetsCoarse_noloops);
  free(adjCoarse_noloops);

  return 0;
}

static int
loadToMatrix(SparseMatrix<double,RowMajor>& M, VectorXd& degrees,
    graph_t *g, int coarseningType) {

  typedef Triplet<double> T;
  std::vector<T> tripletList;

  if (coarseningType == 0) {
    tripletList.reserve(g->m);

    for (int i=0; i<g->n; i++) {
      tripletList.push_back(T(i,i,0.5));
      degrees(i) = g->rowOffsets[i+1]-g->rowOffsets[i];
      double nzv = 1/(2.0*(g->rowOffsets[i+1]-g->rowOffsets[i]));
      for (unsigned int j=g->rowOffsets[i]; j<g->rowOffsets[i+1]; j++) {
        unsigned int v = g->adj[j];
        tripletList.push_back(T(i, v, nzv));
      }
    }
    M.setFromTriplets(tripletList.begin(), tripletList.end());
  } else {

    for (int i=0; i<g->n_coarse; i++) {
      double degree_i = 0;
      for (unsigned int j=g->rowOffsetsCoarse[i]; j<g->rowOffsetsCoarse[i+1]; j++) {
      degree_i += g->eweights[j];
    }
    degrees(i) = degree_i;
    // std::cout << degrees(i) << " ";
    }

    tripletList.reserve(g->m_coarse);

    for (long i=0; i<g->n_coarse; i++) {
      double diag_val = 0;
      double inv_2deg = 1/(2.0*degrees(i));
      for (unsigned int j=g->rowOffsetsCoarse[i]; j<g->rowOffsetsCoarse[i+1]; j++) {
        unsigned int v = g->adjCoarse[j];
        if (v == ((unsigned int) i)) {
          diag_val = g->eweights[j]*inv_2deg;
        } else {
          tripletList.push_back(T(i, v, g->eweights[j]*inv_2deg));
        }
      }
      tripletList.push_back(T(i,i,diag_val+0.5));
    }
    M.setFromTriplets(tripletList.begin(), tripletList.end());
  }

  return 0;
}

static VectorXd
bfs(unsigned int *row,
    unsigned int *col,
    long N, long M,
    unsigned int start) {
  VectorXd columnOfMatrix(N);

  unsigned int s = start;
  int *visited = (int *) malloc (sizeof(int) * N);
  memset(visited, 0, sizeof(unsigned int) * N);
  std::queue<unsigned int> Q;
  Q.push(s);
  visited[s] = 1;
  columnOfMatrix(s) = 0;

  while(!Q.empty()) {
    unsigned int h = Q.front();
    Q.pop();
    for (unsigned int j=row[h]; j<row[h+1]; j++) {
      s = col[j];
      if (!visited[s]) {
        visited[s] = 1;
        Q.push(s);
        columnOfMatrix(s) = columnOfMatrix(h) + 1;
      }
    }
  }

  free(visited);
  return columnOfMatrix;
}

static int
HDE(SparseMatrix<double,RowMajor>& M, graph_t *g,
    VectorXd& degrees,
    VectorXd& secondVec, VectorXd& thirdVec) {

  auto startTimerPart = std::chrono::high_resolution_clock::now();

  // Create Laplacian
  long n = g->n;
  long m = g->m;
  typedef Triplet<double> T;
  std::vector<T> LTripletList;
  LTripletList.reserve(g->m);
  for (int i=0; i<g->n; i++) {
    LTripletList.push_back(T(i,i,degrees(i)));
    for (unsigned int j=g->rowOffsets[i]; j<g->rowOffsets[i+1]; j++) {
      unsigned int v = g->adj[j];
      LTripletList.push_back(T(i,v, -1.0));
    }
  }
  SparseMatrix<double,RowMajor> L(n,n);
  L.setFromTriplets(LTripletList.begin(), LTripletList.end());
  auto endTimerPart = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elt = endTimerPart - startTimerPart;
  std::cout << "Laplacian load time: " << elt.count() << " s." << std::endl;

  // HDE Initialize
  startTimerPart = std::chrono::high_resolution_clock::now();
  VectorXi min_dist(g->n);
  min_dist.setOnes();
  min_dist = min_dist * INT_MAX;

  int maxM = 50;
  VectorXd tmp(n);
  tmp.setOnes();
  tmp.normalize();
  MatrixXd dist(n,maxM+1);
  MatrixXd dist_bak(n,maxM);
  dist.col(0) = tmp;

  int start_idx = 0;
  for (int run_count=1; run_count<=maxM; run_count++) {
    dist.col(run_count) = bfs(g->rowOffsets, g->adj, n, m, start_idx);

    int max = -1;
    for (long i=0; i<n; i++) {
      if (dist.col(run_count)(i) < min_dist[i]) {
        min_dist[i] = dist.col(run_count)(i);
      }
      if (min_dist[i] > max) {
        max = min_dist[i];
        start_idx = i;
      }
    }
    dist.col(run_count).normalize();
  }

  int j = 1;
  for (int run_count=0; run_count<maxM; run_count++) {

    for (int k=0; k<j; k++) {
      // D-orthogonalize
#if 1
      VectorXd dnormvec(n);
      dnormvec = dist.col(k).cwiseProduct(degrees);
      double multplr_denom = dist.col(k).dot(dnormvec);
      double multplr_num = dist.col(j).dot(dnormvec);
      dist.col(j) = dist.col(j) - (multplr_num * dist.col(k))/multplr_denom;
#endif
#if 0
      double multplr = dist.col(j).dot(dist.col(k));
      dist.col(j) = dist.col(j) - multplr * dist.col(k);
#endif
    }

    double normdist = dist.col(j).norm();
    if (normdist < 0.001) {
      std::cout << "discarding vec " << j << ", normdist " << normdist << std::endl;
      j--;
    } else {
      // std::cout << "j " << j << ", normdist " << normdist << std::endl;
      dist.col(j).normalize();
    }

    dist_bak.col(j-1) = dist.col(j);
    j++;
  }

  MatrixXd LX(n, maxM);
  LX = L * dist_bak;

  MatrixXd XtLX(maxM, maxM);
  XtLX = dist_bak.transpose() * LX;

  SelfAdjointEigenSolver<MatrixXd> es(XtLX);
  MatrixXd init_vecs(n, 2);
  init_vecs = dist_bak * es.eigenvectors().leftCols(2).real();

  endTimerPart = std::chrono::high_resolution_clock::now();
  elt = endTimerPart - startTimerPart;
  std::cout << "HDE Initialization time " << elt.count() << " s." << std::endl;
  secondVec = init_vecs.col(0);
  thirdVec  = init_vecs.col(1);

  return 0;
}

static int
powerIterationKoren(SparseMatrix<double,RowMajor>& M,
    VectorXd& degrees, double eps, VectorXd& firstVec,
    VectorXd& secondVec, VectorXd& thirdVec,
    int coarseningType, char *inputFilename) {

  // double eps = 1e-9;
  std::cout << "Using eps " << eps << " for second eigenvector" << std::endl;

  if (coarseningType > 0) {
    std::cout << "Using coarsened graph" << std::endl;
  }

  int n = M.rows();

  VectorXd uk_hat(n);

  // Intialized vectors are passed to function
  uk_hat = secondVec;
  // uk_hat.setRandom();
  // uk_hat.normalize();

  VectorXd uk(n);

  // For D-orthonormalization
  VectorXd firstVecD(n);
  firstVecD = firstVec.cwiseProduct(degrees);
  double mult1_denom = firstVec.dot(firstVecD);

  VectorXd residual(n);

  int num_iterations1 = 0;

  auto startTimerPart = std::chrono::high_resolution_clock::now();
  while (1) {

    uk = uk_hat;

    // D-orthonormalize
    double mult1_num = uk.dot(firstVecD);
    uk = uk - (mult1_num/mult1_denom)*firstVec;

    // Do matrix-vector product
    uk_hat = M*uk;
    uk_hat.normalize();

    num_iterations1++;

    // double residual_norm1 =
    //  residual.lpNorm<Infinity>()/(uk_hat.maxCoeff()-uk_hat.minCoeff());

#if 0
    double residual_dot = uk.dot(uk_hat);
    if (residual_dot >= (1-eps)) {
      break;
    }
#endif

    residual = uk-uk_hat;
    double residual_norm = residual.norm();
    // std::cout << residual_norm << std::endl;

    if (residual_norm < eps) {
      break;
    }
  }

  std::cout << "Num iterations for second eigenvector: " <<
    num_iterations1 << std::endl;

  // Save this eigenvector
  secondVec = uk_hat;
  auto endTimerPart = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elt = endTimerPart - startTimerPart;
  std::cout << "Second eigenvector computation time: " << elt.count() << " s." << std::endl;

  eps = 2.0*eps;
  std::cout << "Using eps " << eps << " for third eigenvector" << std::endl;

  // For D-orthonormalization
  VectorXd secondVecD(n);
  secondVecD = secondVec.cwiseProduct(degrees);
  double mult2_denom = secondVec.dot(secondVecD);

  // Initialized vectors are passed to function
  uk_hat = thirdVec;
  // uk_hat.setRandom();
  // uk_hat.normalize();

  startTimerPart = std::chrono::high_resolution_clock::now();
  int num_iterations2 = 0;
  while (1) {

    uk = uk_hat;

    // D-orthonormalize
    double mult1_num = uk.dot(firstVecD);
    uk = uk - (mult1_num/mult1_denom)*firstVec;
    double mult2_num = uk.dot(secondVecD);
    uk = uk - (mult2_num/mult2_denom)*secondVec;

    // Do matrix-vector product
    uk_hat = M*uk;
    uk_hat.normalize();

    num_iterations2++;

#if 0
    double residual_dot = uk.dot(uk_hat);
    if (residual_dot >= (1-eps)) {
      break;
    }
#endif

    residual = uk-uk_hat;
    double residual_norm = residual.norm();
    // std::cout << " " << residual_norm << std::endl;

    if (residual_norm < eps) {
      break;
    }

  }
  std::cout << "Num iterations for third eigenvector: " <<
    num_iterations2 << std::endl;

  // Save this eigenvector as well
  thirdVec = uk_hat;
  endTimerPart = std::chrono::high_resolution_clock::now();
  elt = endTimerPart - startTimerPart;
  std::cout << "Third eigenvector computation time: " << elt.count() << " s." << std::endl;

  std::cout << "Dot products of eigenvectors: "
  << firstVec.dot(secondVec) << " "
  << firstVec.dot(thirdVec) << " " << secondVec.dot(thirdVec) <<
  std::endl;

  return 0;

}

static int
RefineTutte(SparseMatrix<double,RowMajor>& M,
    VectorXd& secondVec, VectorXd& thirdVec,
    int numSmoothing) {

  std::cout << "Number of smoothing rounds: " << numSmoothing << std::endl;
  auto startTimerPart = std::chrono::high_resolution_clock::now();
  VectorXd unitVec(M.cols());;
  unitVec.setOnes();

  SparseMatrix<double,RowMajor> M2 = 2*M;
  // M2.diagonal() -= unitVec;
  M2.diagonal().setZero();

  for (int i=0; i<numSmoothing; i++) {
    secondVec = M2*secondVec;
    thirdVec = M2*thirdVec;
  }
  auto endTimerPart = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elt = endTimerPart - startTimerPart;
  std::cout << "RefineTutte Time: " << elt.count() << " s." << std::endl;

  return 0;
}

static int
writeCoords(SparseMatrix<double,RowMajor>& M,
    VectorXd& firstVec, VectorXd& secondVec, VectorXd& thirdVec,
    int coarseningType, int doHDE, int refineType,
    double eps, char *inputFilename) {

  // Write coordinates to file
  std::ofstream fout;
  std::string coordFilename(inputFilename);
  coordFilename += "_c" + std::to_string(coarseningType)
      + "_h" + std::to_string(doHDE)
      + "_r" + std::to_string(refineType)
      + "_eps" + to_string_with_precision(eps) + ".nxyz";
  std::cout << "Writing coordinates to file " << coordFilename << std::endl;
  fout.open(coordFilename);
  int n = M.cols();
  for (int i=0; i<n; i++) {
    fout << secondVec(i) << "," << thirdVec(i) << std::endl;
  }
  fout.close();

  /* print eigenvalues */
  double firstEigenVal = (M*firstVec).cwiseQuotient(firstVec).mean();
  double firstEigenValMin =
    (M*firstVec).cwiseQuotient(firstVec).minCoeff();
  double firstEigenValMax =
    (M*firstVec).cwiseQuotient(firstVec).maxCoeff();
  double secondEigenVal = (M*secondVec).cwiseQuotient(secondVec).mean();
  double secondEigenValMin =
    (M*secondVec).cwiseQuotient(secondVec).minCoeff();
  double secondEigenValMax =
    (M*secondVec).cwiseQuotient(secondVec).maxCoeff();
  double thirdEigenVal = (M*thirdVec).cwiseQuotient(thirdVec).mean();
  double thirdEigenValMin =
    (M*thirdVec).cwiseQuotient(thirdVec).minCoeff();
  double thirdEigenValMax =
    (M*thirdVec).cwiseQuotient(thirdVec).maxCoeff();

  std::cout << "First Eigenvalue  (mean, min, max): " << firstEigenVal
    << " " << firstEigenValMin << " " << firstEigenValMax << std::endl;
  std::cout << "Second Eigenvalue (mean, min, max): " << secondEigenVal
    << " " << secondEigenValMin << " " << secondEigenValMax << std::endl;
  std::cout << "Third Eigenvalue  (mean, min, max): " << thirdEigenVal
    << " " << thirdEigenValMin << " " << thirdEigenValMax << std::endl;

  return 0;
}

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

  if (argc != 5) {
    std::cout << "Usage: "<< argv[0] << " <csr filename> "
    "<0/1/2 (none,coarsen and continue,coarsen+stop)> <0/1 (HDE)> "
    "<0/1/2/3 (none,Koren,Tutte,Koren+Tutte)> "
    << std::endl;
  return 1;
  }

  char *inputFilename = argv[1];

  int coarseningType = atoi(argv[2]);
  if (coarseningType == 1) {
    std::cout << "Coarsening graph and continuing" << std::endl;
  } else if (coarseningType == 2) {
    std::cout << "Coarsening and stopping" << std::endl;
  } else {
    coarseningType = 0;
  }

  int doHDE = atoi(argv[3]);
  if (doHDE) {
    std::cout << "Running High-dimensional embedding" << std::endl;
    coarseningType = 0;
  } else {
    doHDE = 0;
  }

  int refineType = atoi(argv[4]);
  if (refineType == 0) {
    std::cout << "No eigenvector computation or refinement" << std::endl;
  } else if (refineType == 1) {
    std::cout << "Computing eigenvectors using Koren's algorithm" << std::endl;
  } else if (refineType == 2) {
    std::cout << "Refining coordinates using Tutte's algorithm" << std::endl;
  } else if (refineType == 3) {
    std::cout << "Eigenvectors followed by Tutte refinement" << std::endl;
  }

  // Read CSR file
  auto startTimer = std::chrono::high_resolution_clock::now();
  auto startTimerPart = std::chrono::high_resolution_clock::now();
  FILE *infp = fopen(inputFilename, "rb");
  if (infp == NULL) {
    std::cout << "Error: Could not open input file. Exiting ..." <<
    std::endl;
    return 1;
  }
  long n, m;
  long rest[4];
  unsigned int *rowOffsets, *adj;
  fread(&n, 1, sizeof(long), infp);
  fread(&m, 1, sizeof(long), infp);
  fread(rest, 4, sizeof(long), infp);
  rowOffsets = (unsigned int *) malloc (sizeof(unsigned int) * (n+1));
  assert(rowOffsets != NULL);
  adj = (unsigned int *) malloc (sizeof(unsigned int) * m);
  assert(adj != NULL);
  fread(rowOffsets, n+1, sizeof(unsigned int), infp);
  fread(adj, m, sizeof(unsigned int), infp);
  fclose(infp);
  auto endTimerPart = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> elt = endTimerPart - startTimerPart;
  std::cout << "CSR read time: " << elt.count() << " s." << std::endl;
  std::cout << "Num edges: " << m/2 << ", vertices: " << n << std::endl;

  graph_t g;
  g.n = n; g.m = m;
  g.rowOffsets = rowOffsets;
  g.adj = adj;

  simpleCoarsening(&g, coarseningType);

  VectorXd secondVecc;
  VectorXd thirdVecc;

  if (coarseningType > 0) {
    long n_coarse = g.n_coarse;
    SparseMatrix<double,RowMajor> Mc(n_coarse,n_coarse);
    VectorXd degreesc(n_coarse);
    loadToMatrix(Mc, degreesc, &g, coarseningType);
    VectorXd firstVecc(n_coarse);
    firstVecc.setOnes();
    firstVecc.normalize();
    secondVecc.resize(n_coarse);
    thirdVecc.resize(n_coarse);
    secondVecc.setRandom();
    if (secondVecc(0) < 0) {
      secondVecc = -secondVecc;
    }
    secondVecc.normalize();
    thirdVecc.setRandom();
    if (thirdVecc(0) < 0) {
      thirdVecc = -thirdVecc;
    }
    thirdVecc.normalize();
    double epsc = 1e-9;
    powerIterationKoren(Mc, degreesc, epsc,
      firstVecc, secondVecc, thirdVecc, coarseningType,
        inputFilename);
    if (coarseningType == 2) {
      writeCoords(Mc, firstVecc, secondVecc, thirdVecc,
        coarseningType, doHDE, refineType, epsc, inputFilename);
    }
  }

  // Load to matrix
  startTimerPart = std::chrono::high_resolution_clock::now();
  SparseMatrix<double,RowMajor> M(n,n);
  VectorXd degrees(n);
  // load the full graph/matrix now
  loadToMatrix(M, degrees, &g, 0);
  endTimerPart = std::chrono::high_resolution_clock::now();
  elt = endTimerPart - startTimerPart;
  std::cout << "Matrix load time: " << elt.count() << " s." << std::endl;

  // Compute second and third eigenvectors using
  // Koren's power iteration algorithm
  VectorXd firstVec(n);
  firstVec.setOnes();
  firstVec.normalize();

  VectorXd secondVec(n);
  VectorXd thirdVec(n);

  // Initialize with previously-found coarse vectors
  if (coarseningType == 1) {
    for (long i=0; i<g.n; i++) {
      secondVec(i) = secondVecc(g.coarseID[i]);
      thirdVec(i)  = thirdVecc(g.coarseID[i]);
    }
    secondVec.normalize();
    thirdVec.normalize();

  // initialize with HDE vectors
  } else if (doHDE == 1) {
    HDE(M, &g, degrees, secondVec, thirdVec);

  // Random vectors
  } else if (doHDE == 0) {
     secondVec.setRandom();
     if (secondVec(0) < 0) {
       secondVec = -secondVec;
     }
     secondVec.normalize();
     thirdVec.setRandom();
     if (thirdVec(0) < 0) {
       thirdVec = -thirdVec;
     }
     thirdVec.normalize();
  }

  if (coarseningType != 2) {
    int numTutteSmoothing = 500;
    double eps = 1e-5;
    if (refineType == 0) {
      secondVec.normalize();
      thirdVec.normalize();
    } else if (refineType == 1) {
      powerIterationKoren(M, degrees, eps, firstVec, secondVec, thirdVec,
      0, inputFilename);
    } else if (refineType == 2) {
      RefineTutte(M, secondVec, thirdVec, numTutteSmoothing);
    } else if (refineType == 3) {
      RefineTutte(M, secondVec, thirdVec, numTutteSmoothing);
      powerIterationKoren(M, degrees, eps, firstVec, secondVec, thirdVec,
      0, inputFilename);
    }
    writeCoords(M, firstVec, secondVec, thirdVec,
          coarseningType, doHDE, refineType, 0, inputFilename);
  }

  free(g.rowOffsets);
  free(g.adj);

  if (coarseningType > 0) {
    free(g.rowOffsetsCoarse);
    free(g.adjCoarse);
    free(g.coarseID);
    free(g.eweights);
  }
  endTimerPart = std::chrono::high_resolution_clock::now();
  elt = endTimerPart - startTimer;
  std::cout << "Overall time: " << elt.count() << " s." << std::endl;

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_ELLIPTIC_FUNCTIONS_GENERIC_ELLIPKE_HPP_INCLUDED
#define NT2_ELLIPTIC_FUNCTIONS_GENERIC_ELLIPKE_HPP_INCLUDED

#include <nt2/elliptic/functions/ellipke.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  (ellipke_, tag::cpu_,
                             (A0),
                             (generic_<floating_<A0> >)

                            )
  {
    typedef std::pair<A0, A0> result_type;

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename nt2::meta::scalar_of<A0>::type sA0;
      A0 first, second;
      nt2::ellipke(a0, nt2::Eps<sA0>(), first, second);
      return result_type(first, second);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (ellipke_, tag::cpu_,
                             (A0)(A1),
                             (generic_<floating_<A0> >)
                             (scalar_<floating_<A1> >)

                            )
  {
    typedef std::pair<A0, A0> result_type;

    NT2_FUNCTOR_CALL(2)
    {
      A0 first, second;
      nt2::ellipke(a0, a1, first, second);
      return result_type(first, second);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (  ellipke_, tag::cpu_,
                               (A0)(A1),
                               (generic_<floating_<A0> >)
                               (scalar_<floating_<A1> >)
                               (generic_<floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A1 const & a1,A0 & a3) const
    {
      A0 a2;
      nt2::ellipke(a0,a1,a2,a3);
      return a2;
    }
  };

} }
#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SIMD_COMMON_GAMMA_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SIMD_COMMON_GAMMA_HPP_INCLUDED

#include <nt2/euler/functions/gamma.hpp>
#include <nt2/euler/functions/details/gamma_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/any.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/floor.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/inbtrue.hpp>
#include <nt2/include/functions/simd/is_even.hpp>
#include <nt2/include/functions/simd/is_flint.hpp>
#include <nt2/include/functions/simd/is_greater_equal.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/negif.hpp>
#include <nt2/include/functions/simd/seladd.hpp>
#include <nt2/include/functions/simd/selsub.hpp>
#include <nt2/include/functions/simd/sinpi.hpp>
#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/stirling.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/functions/simd/is_nan.hpp>
#include <nt2/include/functions/simd/logical_or.hpp>
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  (gamma_, tag::cpu_,
                             (A0)(X),
                             ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    typedef typename meta::as_logical<A0>::type bA0;
    NT2_FUNCTOR_CALL(1)
    {
      bA0 nan_result = logical_and(is_ltz(a0), is_flint(a0));
      #ifndef BOOST_SIMD_NO_INVALIDS
      nan_result = logical_or(nt2::is_nan(a0), nan_result);
      #endif
      A0 q = nt2::abs(a0);
      bA0 test = lt(a0, nt2::splat<A0>(-33.0));
      std::size_t nb = nt2::inbtrue(test);
      A0 r =  Nan<A0>();
      if(nb > 0)
      {
        //treat negative large with reflection
        r = large_negative(q);
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_nan_else(nan_result, r);
      }
      A0 r1 = other(a0, test);
      A0 r2 = if_else(test, r, r1);
      return nt2::if_nan_else(nan_result, r2);
    }

  private :
    static inline A0 large_negative(const A0& q)
    {
      A0 st =  nt2::stirling(q);
      A0 p = nt2::floor(q);
      A0 sgngam = nt2::negif(nt2::is_even(p), One<A0>());
      A0 z = q - p;
      bA0 test2 = lt(z, nt2::Half<A0>() );
      z = nt2::selsub(test2, z, nt2::One<A0>());
      z = q*nt2::sinpi(z);
      z =  nt2::abs(z);
      return sgngam*nt2::Pi<A0>()/(z*st);
    }

    static inline A0 other(const A0& q, const bA0& test)
    {
      A0 x =  nt2::if_else(test, Two<A0>(), q);
      #ifndef BOOST_SIMD_NO_INFINITIES
      bA0 inf_result = eq(q, Inf<A0>());
      x = if_else(inf_result, Two<A0>(), x);
      #endif
      A0 z = nt2::One<A0>();
      bA0 test1 = ge(x,Three<A0>());
      while( nt2::any(test1) )
      {
        x = nt2::seladd(test1, x, nt2::Mone<A0>());
        z = nt2::if_else(   test1, z*x, z);
        test1 = ge(x,Three<A0>());
      }
      //all x are less than 3
      test1 = nt2::is_ltz(x);
      while( nt2::any(test1) )
      {
        z = nt2::if_else(test1, z/x, z);
        x = nt2::seladd(test1, x, nt2::One<A0>());
        test1 = nt2::is_ltz(x);
      }
      //all x are greater than 0 and less than 3
      bA0 test2 = lt(x,nt2::Two<A0>());
      while( nt2::any(test2))
      {
        z = nt2::if_else(test2, z/x, z);
        x = nt2::seladd(test2, x, nt2::One<A0>());
        test2 = lt(x,nt2::Two<A0>());
      }
      //all x are greater equal 2 and less than 3
      x = z*details::gamma_kernel<A0>::gamma1(x-nt2::Two<A0>());
      #ifndef BOOST_SIMD_NO_INFINITIES
      return if_else(inf_result, q, x);
      #else
      return x;
      #endif
    }
  };
} }

#endif


#include <CGAL/internal/Surface_mesh_segmentation/Disk_samplers.h>

#include <boost/tuple/tuple.hpp>
#include <vector>

typedef boost::tuple<double, double, double> boost_tuple;

void print(const std::vector<boost_tuple>& samples)
{
  const std::size_t map_size = 31;
  const std::size_t map_size_2 = 45;
  std::vector<std::vector<bool> > sample_map(map_size, std::vector<bool>(map_size_2, false));

  for(std::vector<boost_tuple>::const_iterator sample_it = samples.begin();
    sample_it != samples.end(); ++sample_it)
  {
    double x = (sample_it->get<0>() +1)/2;
    double y = (sample_it->get<1>() +1)/2;
    x *= (map_size-1);
    y *= (map_size_2-1);
    std::size_t x_c  = static_cast<std::size_t>(x + 0.49);
    std::size_t y_c  = static_cast<std::size_t>(y + 0.49);
    sample_map[x_c][y_c] = true;
  }
  for(std::size_t i = 0; i < map_size; ++i)
  {
    for(std::size_t j = 0; j < map_size_2; ++j)
    {
      if(sample_map[i][j]){ std::cout << "*"; }
      else                { std::cout << " "; }
    }
    std::cout << std::endl;
  }
  std::cout << std::endl;
}
/**
 * Uses disk sampling functors to sample points from unit-disk.
 * It also prints sampled points for visual debugging.
 *
 * Note that it always return EXIT_SUCCESS
 */
int main(void)
{
  CGAL::internal::Vogel_disk_sampling<boost_tuple> sampling_1;
  CGAL::internal::Vogel_disk_sampling<boost_tuple, true> sampling_2;
  CGAL::internal::Polar_disk_sampling<boost_tuple> sampling_3;
  CGAL::internal::Concentric_disk_sampling<boost_tuple> sampling_4;

  std::vector<boost_tuple> samples_1;
  std::vector<boost_tuple> samples_2;
  std::vector<boost_tuple> samples_3;
  std::vector<boost_tuple> samples_4;

  sampling_1(64, std::back_inserter(samples_1));
  sampling_2(64, std::back_inserter(samples_2));
  sampling_3(64, std::back_inserter(samples_3));
  sampling_4(64, std::back_inserter(samples_4));

  print(samples_1);
  print(samples_2);
  print(samples_3);
  print(samples_4);
}


/*
* Copyright (c) 2019, Intel Corporation
* 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 the Intel Corporation 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 <COPYRIGHT HOLDER> 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.
*/

#pragma once

#include "Teisko/Algorithm/Bit.hpp"
#include "Teisko/Algorithm/Functors.hpp"
#include "Teisko/Algorithm/NelderMead.hpp"
#include "Teisko/Color.hpp"
#include <Eigen/Dense>
#include <array>
#include <exception>
#include <numeric>
#include <vector>

namespace Teisko
{
    template <int N>
    int operator-(const cyclical_index<N>& a, const cyclical_index<N>& b)
    {
        return a.idx < b.idx ? N - (b.idx - a.idx) : b.idx - a.idx;
    }

    struct color_correction_matrix : Eigen::Matrix3d
    {
        color_correction_matrix()
        {
            setIdentity();
        }

        color_correction_matrix(const Eigen::Matrix3d& v) : Eigen::Matrix3d(v) {}

        // Apply color correction matrix
        template <rgb_cs CS = rgb_cs::sRGB, wp WP = wp::D65>
        rgb<double, CS, WP> apply(const rgb<double, rgb_cs::Sensor, wp::None>& pt) const
        {
            return rgb<double, CS, WP> (
                (*this)(0, 0) * pt.r + (*this)(0, 1) * pt.g + (*this)(0, 2) * pt.b,
                (*this)(1, 0) * pt.r + (*this)(1, 1) * pt.g + (*this)(1, 2) * pt.b,
                (*this)(2, 0) * pt.r + (*this)(2, 1) * pt.g + (*this)(2, 2) * pt.b);
        }
    };

    // Calculate color correction matrix from input and output matrices
    // Either calculate ccm by
    // 1. matrix inverse if matrix is invertible
    // 2. least square method using svd decomposition (high accuracy but slow)
    inline color_correction_matrix calculate_ccm(const Eigen::Matrix3d& in, const Eigen::Matrix3d& out)
    {
        using namespace Eigen;

        const double THRESHOLD = 1e-8;
        if (std::abs(in.determinant()) > THRESHOLD)
        {
            // CCM = M(out) / M(in) = M(out)*inverse(M(in))
            //
            // M(in)  = [RGB(1, in) ^ T, RGB(2, in) ^ T, RGB(grey) ^ T]
            // M(out) = [RGB(1, out) ^ T, RGB(2, out) ^ T, RGB(grey) ^ T]
            return color_correction_matrix(out * in.inverse());
        }
        JacobiSVD<Matrix3d> svd(in.transpose(), ComputeFullU | ComputeFullV);
        return color_correction_matrix(svd.solve(out.transpose()).transpose());
    }

    template <rgb_cs CS, wp WP = wp::D65>
    struct patch
    {
        rgb<double, rgb_cs::Sensor, wp::None> input;
        rgb<double, CS, WP> target;
        double weight;
        bool achromatic;
    };

    template <rgb_cs CS, wp WP = wp::D65>
    struct ccm_input_params
    {
        std::array<patch<CS, WP>, 24> color_checker_classic;
        //std::vector<patch<CS, WP>> munsell; // TODO: Implementation for Munsell

        void white_balance()
        {
            average_f<double> r_per_g;
            average_f<double> b_per_g;

            // Loop over achromatic patches (bottom row), dimmest and brightest omitted
            for (size_t i = 19; i < 23; ++i)
            {
                r_per_g += color_checker_classic[i].input.r / color_checker_classic[i].input.g;
                b_per_g += color_checker_classic[i].input.b / color_checker_classic[i].input.g;
            }

            auto max_gain = std::max({ (double)r_per_g, (double)b_per_g, 1.0 });
            auto gain_r = max_gain / r_per_g;
            auto gain_g = max_gain;
            auto gain_b = max_gain / b_per_g;

            for (auto& patch : color_checker_classic)
            {
                patch.input.r *= gain_r;
                patch.input.g *= gain_g;
                patch.input.b *= gain_b;
            }
        }

        void normalize()
        {
            average_f<double> ratio;
            for (size_t i = 19; i < 23; ++i)
            {
                ratio += color_checker_classic[i].target.mean() / color_checker_classic[i].input.mean();
            }

            for (auto& patch : color_checker_classic)
            {
                patch.input *= ratio;
            }
        }
    };

    template <rgb_cs CS, wp WP = wp::D65>
    struct ccm_optimization_params
    {
        ccm_input_params<CS, WP> input_params;

        ccm_optimization_params() = default;
        ccm_optimization_params(const ccm_input_params<CS, WP>& input) : input_params(input) { }

        void prepare()
        {
            input_params.white_balance();
            input_params.normalize();
        }
    };

    template <rgb_cs CS, wp WP = wp::D65>
    struct ccm_func_optimizer
    {
        ccm_optimization_params<CS, WP> params;

        ccm_func_optimizer(ccm_optimization_params<CS, WP>& p) : params(p) { }

        double operator()(double *p)
        {
            auto sum = 0.0;

            // Create CCM from optimization set. Middle column is constructed from
            // border columns (each row sum up to 1)
            auto ccm = color_correction_matrix();
            ccm <<
                p[0], 1 - p[0] - p[1], p[1],
                p[2], 1 - p[2] - p[3], p[3],
                p[4], 1 - p[4] - p[5], p[5];

            for (const auto& patch : params.input_params.color_checker_classic)
            {
                if (patch.weight > 0)
                {
                    lab<double, WP> pt1(ccm.apply<CS, WP>(patch.input));
                    lab<double, WP> pt2(patch.target); // TODO: Pre calculate

                    sum += color_diff<color_diff_type::DeltaE2000>::calculate(pt1, pt2) * patch.weight;
                }
            }
            return sum;
        }
    };

    template <rgb_cs CS, wp WP = wp::D65>
    class ccm_optimization
    {
    public:
        ccm_optimization(ccm_input_params<CS, WP>& input) : options(input) { }

        color_correction_matrix characterize()
        {
            // Do white balance and adjust input intensity
            options.prepare();

            // Get initial point for nelder mead optimization by using least square CCM
            auto initial_ccm = ls_ccm().data();

            // RinR, BinR, RinG, BinG, RinB, BinB indices
            std::vector<double> initial_pt =
            {
                initial_ccm[0], initial_ccm[2], initial_ccm[3], initial_ccm[5], initial_ccm[6], initial_ccm[8]
            };

            // 1. coarse optimization
            auto solver = ccm_func_optimizer<CS, WP>(options);
            auto result = nelder_mead_simplex(initial_pt, solver, {200, 1e-4, 1e-4});

            // 2. fine optimization
            result = nelder_mead_simplex(result.second, solver, {4000, 1e-12, 1e-4});

            // Create CCM from optimization set. Middle column is constructed from
            // border columns (each row should sum up to 1)
            color_correction_matrix ccm;
            ccm <<
                result.second[0], 1 - result.second[0] - result.second[1], result.second[1],
                result.second[2], 1 - result.second[2] - result.second[3], result.second[3],
                result.second[4], 1 - result.second[4] - result.second[5], result.second[5];
            return ccm;
        }

    private:
        ccm_optimization_params<CS, WP> options;

        // Solve a linear system Ax=b for initial CCM optimization
        // Where A is CCC input matrix and B is CCC target matrix
        // This method does not preserve greys nor does it minimize error in de2000.
        color_correction_matrix ls_ccm()
        {
            Eigen::Matrix<double, 24, 3> A;
            Eigen::Matrix<double, 24, 3> b;
            Eigen::Matrix3d x;

            // Initialize matrices
            size_t n = options.input_params.color_checker_classic.size();
            for (size_t i = 0; i < n; ++i)
            {
                const patch<CS, WP>& patch = options.input_params.color_checker_classic[i];
                A.row(i) << patch.input.r, patch.input.g, patch.input.b;
                b.row(i) << patch.target.r, patch.target.g, patch.target.b;
            }

            // Use HouseholderQR decomposition to solve the linear equation.
            // Prefer speed over accuracy for the decomposition method.
            return color_correction_matrix(A.householderQr().solve(b));
        }
    };
}


/* Author: Guido Kanschat, University of Heidelberg, 2003  */
/*         Baerbel Janssen, University of Heidelberg, 2010 */
/*         Wolfgang Bangerth, Texas A&M University, 2010   */

/*    $Id: step-16.cc 28506 2013-02-21 02:47:24Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2003-2004, 2006-2013 by the deal.II authors                   */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// As discussed in the introduction, most of this program is copied almost
// verbatim from step-6, which itself is only a slight modification of
// step-5. Consequently, a significant part of this program is not new if
// you've read all the material up to step-6, and we won't comment on that
// part of the functionality that is unchanged. Rather, we will focus on those
// aspects of the program that have to do with the multigrid functionality
// which forms the new aspect of this tutorial program.

// @sect3{Include files}

// Again, the first few include files are already known, so we won't comment
// on them:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria_boundary_lib.h>

#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>

// These, now, are the include necessary for the multi-level methods. The
// first two declare classes that allow us to enumerate degrees of freedom not
// only on the finest mesh level, but also on intermediate levels (that's what
// the MGDoFHandler class does) as well as allow to access this information
// (iterators and accessors over these cells).
//
// The rest of the include files deals with the mechanics of multigrid as a
// linear operator (solver or preconditioner).
#include <deal.II/multigrid/mg_dof_handler.h>
#include <deal.II/multigrid/mg_constrained_dofs.h>
#include <deal.II/multigrid/multigrid.h>
#include <deal.II/multigrid/mg_transfer.h>
#include <deal.II/multigrid/mg_tools.h>
#include <deal.II/multigrid/mg_coarse.h>
#include <deal.II/multigrid/mg_smoother.h>
#include <deal.II/multigrid/mg_matrix.h>

// This is C++:
#include <fstream>
#include <sstream>

// The last step is as in all previous programs:
namespace Step16
{
  using namespace dealii;

  // @sect3{The LaplaceProblem class template}

  // This main class is basically the same class as in step-6. As far as
  // member functions is concerned, the only addition is the
  // assemble_multigrid function that assembles the matrices that
  // correspond to the discrete operators on intermediate levels:
  template <int dim>
  class LaplaceProblem
  {
  public:
    LaplaceProblem (const unsigned int deg);
    void run ();

  private:
    void setup_system ();
    void assemble_system ();
    void assemble_multigrid ();
    void solve ();
    void refine_grid ();
    void output_results (const unsigned int cycle) const;

    Triangulation<dim>   triangulation;
    FE_Q<dim>            fe;
    MGDoFHandler<dim>    mg_dof_handler;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    // We need an additional object for the hanging nodes constraints. They
    // are handed to the transfer object in the multigrid. Since we call a
    // compress inside the multigrid these constraints are not allowed to be
    // inhomogeneous so we store them in different ConstraintMatrix objects.
    ConstraintMatrix     hanging_node_constraints;
    ConstraintMatrix     constraints;

    Vector<double>       solution;
    Vector<double>       system_rhs;

    const unsigned int degree;

    // The following four objects are the only additional member variables,
    // compared to step-6. They first three represent the operators that act
    // on individual levels of the multilevel hierarchy, rather than on the
    // finest mesh as do the objects above while the last object stores
    // information about the boundary indices on each level and information
    // about indices lying on a refinement edge between two different
    // refinement levels.
    //
    // To facilitate having objects on each level of a multilevel hierarchy,
    // deal.II has the MGLevelObject class template that provides storage for
    // objects on each level. What we need here are matrices on each level,
    // which implies that we also need sparsity patterns on each level. As
    // outlined in the @ref mg_paper, the operators (matrices) that we need
    // are actually twofold: one on the interior of each level, and one at the
    // interface between each level and that part of the domain where the mesh
    // is coarser. In fact, we will need the latter in two versions: for the
    // direction from coarse to fine mesh and from fine to
    // coarse. Fortunately, however, we here have a self-adjoint problem for
    // which one of these is the transpose of the other, and so we only have
    // to build one; we choose the one from coarse to fine.
    MGLevelObject<SparsityPattern>       mg_sparsity_patterns;
    MGLevelObject<SparseMatrix<double> > mg_matrices;
    MGLevelObject<SparseMatrix<double> > mg_interface_matrices;
    MGConstrainedDoFs                    mg_constrained_dofs;
  };

  // @sect3{Nonconstant coefficients}

  // The implementation of nonconstant coefficients is copied verbatim from
  // step-5 and step-6:

  template <int dim>
  class Coefficient : public Function<dim>
  {
  public:
    Coefficient () : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;

    virtual void value_list (const std::vector<Point<dim> > &points,
                             std::vector<double>            &values,
                             const unsigned int              component = 0) const;
  };

  template <int dim>
  double Coefficient<dim>::value (const Point<dim> &p,
                                  const unsigned int) const
  {
    if (p.square() < 0.5*0.5)
      return 20;
    else
      return 1;
  }

  template <int dim>
  void Coefficient<dim>::value_list (const std::vector<Point<dim> > &points,
                                     std::vector<double>            &values,
                                     const unsigned int              component) const
  {
    const unsigned int n_points = points.size();

    Assert (values.size() == n_points,
            ExcDimensionMismatch (values.size(), n_points));

    Assert (component == 0,
            ExcIndexRange (component, 0, 1));

    for (unsigned int i=0; i<n_points; ++i)
      values[i] = Coefficient<dim>::value (points[i]);
  }

  // @sect3{The LaplaceProblem class implementation}

  // @sect4{LaplaceProblem::LaplaceProblem}

  // The constructor is left mostly unchanged. We take the polynomial degree
  // of the finite elements to be used as a constructor argument and store it
  // in a member variable.
  //
  // By convention, all adaptively refined triangulations in deal.II never
  // change by more than one level across a face between cells. For our
  // multigrid algorithms, however, we need a slightly stricter guarantee,
  // namely that the mesh also does not change by more than refinement level
  // across vertices that might connect two cells. In other words, we must
  // prevent the following situation:
  //
  // @image html limit_level_difference_at_vertices.png ""
  //
  // This is achieved by passing the
  // Triangulation::limit_level_difference_at_vertices flag to the constructor
  // of the triangulation class.
  template <int dim>
  LaplaceProblem<dim>::LaplaceProblem (const unsigned int degree)
    :
    triangulation (Triangulation<dim>::
                   limit_level_difference_at_vertices),
    fe (degree),
    mg_dof_handler (triangulation),
    degree(degree)
  {}

  // @sect4{LaplaceProblem::setup_system}

  // The following function extends what the corresponding one in step-6
  // did. The top part, apart from the additional output, does the same:
  template <int dim>
  void LaplaceProblem<dim>::setup_system ()
  {
    mg_dof_handler.distribute_dofs (fe);

    // Here we output not only the degrees of freedom on the finest level, but
    // also in the multilevel structure
    deallog << "Number of degrees of freedom: "
            << mg_dof_handler.n_dofs();

    for (unsigned int l=0; l<triangulation.n_levels(); ++l)
      deallog << "   " << 'L' << l << ": "
              << mg_dof_handler.n_dofs(l);
    deallog  << std::endl;

    sparsity_pattern.reinit (mg_dof_handler.n_dofs(),
                             mg_dof_handler.n_dofs(),
                             mg_dof_handler.max_couplings_between_dofs());
    DoFTools::make_sparsity_pattern (mg_dof_handler, sparsity_pattern);

    solution.reinit (mg_dof_handler.n_dofs());
    system_rhs.reinit (mg_dof_handler.n_dofs());

    // But it starts to be a wee bit different here, although this still
    // doesn't have anything to do with multigrid methods. step-6 took care of
    // boundary values and hanging nodes in a separate step after assembling
    // the global matrix from local contributions. This works, but the same
    // can be done in a slightly simpler way if we already take care of these
    // constraints at the time of copying local contributions into the global
    // matrix. To this end, we here do not just compute the constraints do to
    // hanging nodes, but also due to zero boundary conditions. We will use
    // this set of constraints later on to help us copy local contributions
    // correctly into the global linear system right away, without the need
    // for a later clean-up stage:
    constraints.clear ();
    hanging_node_constraints.clear ();
    DoFTools::make_hanging_node_constraints (mg_dof_handler, hanging_node_constraints);
    DoFTools::make_hanging_node_constraints (mg_dof_handler, constraints);

    typename FunctionMap<dim>::type      dirichlet_boundary;
    ZeroFunction<dim>                    homogeneous_dirichlet_bc (1);
    dirichlet_boundary[0] = &homogeneous_dirichlet_bc;
    VectorTools::interpolate_boundary_values (static_cast<const DoFHandler<dim>&>(mg_dof_handler),
                                              dirichlet_boundary,
                                              constraints);
    constraints.close ();
    hanging_node_constraints.close ();
    constraints.condense (sparsity_pattern);
    sparsity_pattern.compress();
    system_matrix.reinit (sparsity_pattern);

    // The multigrid constraints have to be initialized. They need to know
    // about the boundary values as well, so we pass the
    // dirichlet_boundary here as well.
    mg_constrained_dofs.clear();
    mg_constrained_dofs.initialize(mg_dof_handler, dirichlet_boundary);

    // Now for the things that concern the multigrid data structures. First,
    // we resize the multi-level objects to hold matrices and sparsity
    // patterns for every level. The coarse level is zero (this is mandatory
    // right now but may change in a future revision). Note that these
    // functions take a complete, inclusive range here (not a starting index
    // and size), so the finest level is n_levels-1.  We first
    // have to resize the container holding the SparseMatrix classes, since
    // they have to release their SparsityPattern before the can be destroyed
    // upon resizing.
    const unsigned int n_levels = triangulation.n_levels();

    mg_interface_matrices.resize(0, n_levels-1);
    mg_interface_matrices.clear ();
    mg_matrices.resize(0, n_levels-1);
    mg_matrices.clear ();
    mg_sparsity_patterns.resize(0, n_levels-1);

    // Now, we have to provide a matrix on each level. To this end, we first
    // use the MGTools::make_sparsity_pattern function to first generate a
    // preliminary compressed sparsity pattern on each level (see the @ref
    // Sparsity module for more information on this topic) and then copy it
    // over to the one we really want. The next step is to initialize both
    // kinds of level matrices with these sparsity patterns.
    //
    // It may be worth pointing out that the interface matrices only have
    // entries for degrees of freedom that sit at or next to the interface
    // between coarser and finer levels of the mesh. They are therefore even
    // sparser than the matrices on the individual levels of our multigrid
    // hierarchy. If we were more concerned about memory usage (and possibly
    // the speed with which we can multiply with these matrices), we should
    // use separate and different sparsity patterns for these two kinds of
    // matrices.
    for (unsigned int level=0; level<n_levels; ++level)
      {
        CompressedSparsityPattern csp;
        csp.reinit(mg_dof_handler.n_dofs(level),
                   mg_dof_handler.n_dofs(level));
        MGTools::make_sparsity_pattern(mg_dof_handler, csp, level);

        mg_sparsity_patterns[level].copy_from (csp);

        mg_matrices[level].reinit(mg_sparsity_patterns[level]);
        mg_interface_matrices[level].reinit(mg_sparsity_patterns[level]);
      }
  }

  // @sect4{LaplaceProblem::assemble_system}

  // The following function assembles the linear system on the finesh level of
  // the mesh. It is almost exactly the same as in step-6, with the exception
  // that we don't eliminate hanging nodes and boundary values after
  // assembling, but while copying local contributions into the global
  // matrix. This is not only simpler but also more efficient for large
  // problems.
  //
  // This latter trick is something that only found its way into deal.II over
  // time and wasn't used in the initial version of this tutorial
  // program. There is, however, a discussion of this function in the
  // introduction of step-27.
  template <int dim>
  void LaplaceProblem<dim>::assemble_system ()
  {
    const QGauss<dim>  quadrature_formula(degree+1);

    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values    |  update_gradients |
                             update_quadrature_points  |  update_JxW_values);

    const unsigned int   dofs_per_cell = fe.dofs_per_cell;
    const unsigned int   n_q_points    = quadrature_formula.size();

    FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
    Vector<double>       cell_rhs (dofs_per_cell);

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    const Coefficient<dim> coefficient;
    std::vector<double>    coefficient_values (n_q_points);

    typename MGDoFHandler<dim>::active_cell_iterator
    cell = mg_dof_handler.begin_active(),
    endc = mg_dof_handler.end();
    for (; cell!=endc; ++cell)
      {
        cell_matrix = 0;
        cell_rhs = 0;

        fe_values.reinit (cell);

        coefficient.value_list (fe_values.get_quadrature_points(),
                                coefficient_values);

        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            {
              for (unsigned int j=0; j<dofs_per_cell; ++j)
                cell_matrix(i,j) += (coefficient_values[q_point] *
                                     fe_values.shape_grad(i,q_point) *
                                     fe_values.shape_grad(j,q_point) *
                                     fe_values.JxW(q_point));

              cell_rhs(i) += (fe_values.shape_value(i,q_point) *
                              1.0 *
                              fe_values.JxW(q_point));
            }

        cell->get_dof_indices (local_dof_indices);
        constraints.distribute_local_to_global (cell_matrix, cell_rhs,
                                                local_dof_indices,
                                                system_matrix, system_rhs);
      }
  }

  // @sect4{LaplaceProblem::assemble_multigrid}

  // The next function is the one that builds the linear operators (matrices)
  // that define the multigrid method on each level of the mesh. The
  // integration core is the same as above, but the loop below will go over
  // all existing cells instead of just the active ones, and the results must
  // be entered into the correct matrix. Note also that since we only do
  // multi-level preconditioning, no right-hand side needs to be assembled
  // here.
  //
  // Before we go there, however, we have to take care of a significant amount
  // of book keeping:
  template <int dim>
  void LaplaceProblem<dim>::assemble_multigrid ()
  {
    QGauss<dim>  quadrature_formula(1+degree);

    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values   | update_gradients |
                             update_quadrature_points | update_JxW_values);

    const unsigned int   dofs_per_cell   = fe.dofs_per_cell;
    const unsigned int   n_q_points      = quadrature_formula.size();

    FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    const Coefficient<dim> coefficient;
    std::vector<double>    coefficient_values (n_q_points);

    // Next a few things that are specific to building the multigrid data
    // structures (since we only need them in the current function, rather
    // than also elsewhere, we build them here instead of the
    // setup_system function). Some of the following may be a bit
    // obscure if you're not familiar with the algorithm actually implemented
    // in deal.II to support multilevel algorithms on adaptive meshes; if some
    // of the things below seem strange, take a look at the @ref mg_paper.
    //
    // Our first job is to identify those degrees of freedom on each level
    // that are located on interfaces between adaptively refined levels, and
    // those that lie on the interface but also on the exterior boundary of
    // the domain. As in many other parts of the library, we do this by using
    // boolean masks, i.e. vectors of booleans each element of which indicates
    // whether the corresponding degree of freedom index is an interface DoF
    // or not. The MGConstraints already computed the information
    // for us when we called initialize in setup_system().
    std::vector<std::vector<bool> > interface_dofs
      = mg_constrained_dofs.get_refinement_edge_indices ();
    std::vector<std::vector<bool> > boundary_interface_dofs
      = mg_constrained_dofs.get_refinement_edge_boundary_indices ();

    // The indices just identified will later be used to decide where the
    // assembled value has to be added into on each level.  On the other hand,
    // we also have to impose zero boundary conditions on the external
    // boundary of each level. But this the MGConstraints knows
    // it. So we simply ask for them by calling get_boundary_indices
    // ().  The third step is to construct constraints on all those
    // degrees of freedom: their value should be zero after each application
    // of the level operators. To this end, we construct ConstraintMatrix
    // objects for each level, and add to each of these constraints for each
    // degree of freedom. Due to the way the ConstraintMatrix stores its data,
    // the function to add a constraint on a single degree of freedom and
    // force it to be zero is called Constraintmatrix::add_line(); doing so
    // for several degrees of freedom at once can be done using
    // Constraintmatrix::add_lines():
    std::vector<ConstraintMatrix> boundary_constraints (triangulation.n_levels());
    std::vector<ConstraintMatrix> boundary_interface_constraints (triangulation.n_levels());
    for (unsigned int level=0; level<triangulation.n_levels(); ++level)
      {
        boundary_constraints[level].add_lines (interface_dofs[level]);
        boundary_constraints[level].add_lines (mg_constrained_dofs.get_boundary_indices()[level]);
        boundary_constraints[level].close ();

        boundary_interface_constraints[level]
        .add_lines (boundary_interface_dofs[level]);
        boundary_interface_constraints[level].close ();
      }

    // Now that we're done with most of our preliminaries, let's start the
    // integration loop. It looks mostly like the loop in
    // assemble_system, with two exceptions: (i) we don't need a
    // right hand side, and more significantly (ii) we don't just loop over
    // all active cells, but in fact all cells, active or not. Consequently,
    // the correct iterator to use is MGDoFHandler::cell_iterator rather than
    // MGDoFHandler::active_cell_iterator. Let's go about it:
    typename MGDoFHandler<dim>::cell_iterator cell = mg_dof_handler.begin(),
                                              endc = mg_dof_handler.end();

    for (; cell!=endc; ++cell)
      {
        cell_matrix = 0;
        fe_values.reinit (cell);

        coefficient.value_list (fe_values.get_quadrature_points(),
                                coefficient_values);

        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            for (unsigned int j=0; j<dofs_per_cell; ++j)
              cell_matrix(i,j) += (coefficient_values[q_point] *
                                   fe_values.shape_grad(i,q_point) *
                                   fe_values.shape_grad(j,q_point) *
                                   fe_values.JxW(q_point));

        // The rest of the assembly is again slightly different. This starts
        // with a gotcha that is easily forgotten: The indices of global
        // degrees of freedom we want here are the ones for current level, not
        // for the global matrix. We therefore need the function
        // MGDoFAccessorLLget_mg_dof_indices, not
        // MGDoFAccessor::get_dof_indices as used in the assembly of the
        // global system:
        cell->get_mg_dof_indices (local_dof_indices);

        // Next, we need to copy local contributions into the level
        // objects. We can do this in the same way as in the global assembly,
        // using a constraint object that takes care of constrained degrees
        // (which here are only boundary nodes, as the individual levels have
        // no hanging node constraints). Note that the
        // boundary_constraints object makes sure that the level
        // matrices contains no contributions from degrees of freedom at the
        // interface between cells of different refinement level.
        boundary_constraints[cell->level()]
        .distribute_local_to_global (cell_matrix,
                                     local_dof_indices,
                                     mg_matrices[cell->level()]);

        // The next step is again slightly more obscure (but explained in the
        // @ref mg_paper): We need the remainder of the operator that we just
        // copied into the mg_matrices object, namely the part on
        // the interface between cells at the current level and cells one
        // level coarser. This matrix exists in two directions: for interior
        // DoFs (index $i$) of the current level to those sitting on the
        // interface (index $j$), and the other way around. Of course, since
        // we have a symmetric operator, one of these matrices is the
        // transpose of the other.
        //
        // The way we assemble these matrices is as follows: since the are
        // formed from parts of the local contributions, we first delete all
        // those parts of the local contributions that we are not interested
        // in, namely all those elements of the local matrix for which not $i$
        // is an interface DoF and $j$ is not. The result is one of the two
        // matrices that we are interested in, and we then copy it into the
        // mg_interface_matrices object. The
        // boundary_interface_constraints object at the same time
        // makes sure that we delete contributions from all degrees of freedom
        // that are not only on the interface but also on the external
        // boundary of the domain.
        //
        // The last part to remember is how to get the other matrix. Since it
        // is only the transpose, we will later (in the solve()
        // function) be able to just pass the transpose matrix where
        // necessary.
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          for (unsigned int j=0; j<dofs_per_cell; ++j)
            if ( !(interface_dofs[cell->level()][local_dof_indices[i]]==true &&
                   interface_dofs[cell->level()][local_dof_indices[j]]==false))
              cell_matrix(i,j) = 0;

        boundary_interface_constraints[cell->level()]
        .distribute_local_to_global (cell_matrix,
                                     local_dof_indices,
                                     mg_interface_matrices[cell->level()]);
      }
  }

  // @sect4{LaplaceProblem::solve}

  // This is the other function that is significantly different in support of
  // the multigrid solver (or, in fact, the preconditioner for which we use
  // the multigrid method).
  //
  // Let us start out by setting up two of the components of multilevel
  // methods: transfer operators between levels, and a solver on the coarsest
  // level. In finite element methods, the transfer operators are derived from
  // the finite element function spaces involved and can often be computed in
  // a generic way independent of the problem under consideration. In that
  // case, we can use the MGTransferPrebuilt class that, given the constraints
  // on the global level and an MGDoFHandler object computes the matrices
  // corresponding to these transfer operators.
  //
  // The second part of the following lines deals with the coarse grid
  // solver. Since our coarse grid is very coarse indeed, we decide for a
  // direct solver (a Householder decomposition of the coarsest level matrix),
  // even if its implementation is not particularly sophisticated. If our
  // coarse mesh had many more cells than the five we have here, something
  // better suited would obviously be necessary here.
  template <int dim>
  void LaplaceProblem<dim>::solve ()
  {

    // Create the object that deals with the transfer between different
    // refinement levels. We need to pass it the hanging node constraints.
    MGTransferPrebuilt<Vector<double> > mg_transfer(hanging_node_constraints, mg_constrained_dofs);
    // Now the prolongation matrix has to be built.  This matrix needs to take
    // the boundary values on each level into account and needs to know about
    // the indices at the refinement egdes. The MGConstraints
    // knows about that so pass it as an argument.
    mg_transfer.build_matrices(mg_dof_handler);

    FullMatrix<double> coarse_matrix;
    coarse_matrix.copy_from (mg_matrices[0]);
    MGCoarseGridHouseholder<> coarse_grid_solver;
    coarse_grid_solver.initialize (coarse_matrix);

    // The next component of a multilevel solver or preconditioner is that we
    // need a smoother on each level. A common choice for this is to use the
    // application of a relaxation method (such as the SOR, Jacobi or
    // Richardson method) or a small number of iterations of a solver method
    // (such as CG or GMRES). The mg::SmootherRelaxation and
    // MGSmootherPrecondition classes provide support for these two kinds of
    // smoothers. Here, we opt for the application of a single SOR
    // iteration. To this end, we define an appropriate typedef
    // and then setup a smoother object.
    //
    // Since this smoother needs temporary vectors to store intermediate
    // results, we need to provide a VectorMemory object. Since these vectors
    // will be reused over and over, the GrowingVectorMemory is more time
    // efficient than the PrimitiveVectorMemory class in the current case.
    //
    // The last step is to initialize the smoother object with our level
    // matrices and to set some smoothing parameters.  The
    // initialize() function can optionally take additional
    // arguments that will be passed to the smoother object on each level. In
    // the current case for the SOR smoother, this could, for example, include
    // a relaxation parameter. However, we here leave these at their default
    // values. The call to set_steps() indicates that we will use
    // two pre- and two post-smoothing steps on each level; to use a variable
    // number of smoother steps on different levels, more options can be set
    // in the constructor call to the mg_smoother object.
    //
    // The last step results from the fact that we use the SOR method as a
    // smoother - which is not symmetric - but we use the conjugate gradient
    // iteration (which requires a symmetric preconditioner) below, we need to
    // let the multilevel preconditioner make sure that we get a symmetric
    // operator even for nonsymmetric smoothers:
    typedef PreconditionSOR<SparseMatrix<double> > Smoother;
    mg::SmootherRelaxation<Smoother, Vector<double> > mg_smoother;
    mg_smoother.initialize(mg_matrices);
    mg_smoother.set_steps(2);
    mg_smoother.set_symmetric(true);

    // The next preparatory step is that we must wrap our level and interface
    // matrices in an object having the required multiplication functions. We
    // will create two objects for the interface objects going from coarse to
    // fine and the other way around; the multigrid algorithm will later use
    // the transpose operator for the latter operation, allowing us to
    // initialize both up and down versions of the operator with the matrices
    // we already built:
    MGMatrix<> mg_matrix(&mg_matrices);
    MGMatrix<> mg_interface_up(&mg_interface_matrices);
    MGMatrix<> mg_interface_down(&mg_interface_matrices);

    // Now, we are ready to set up the V-cycle operator and the multilevel
    // preconditioner.
    Multigrid<Vector<double> > mg(mg_dof_handler,
                                  mg_matrix,
                                  coarse_grid_solver,
                                  mg_transfer,
                                  mg_smoother,
                                  mg_smoother);
    mg.set_edge_matrices(mg_interface_down, mg_interface_up);

    PreconditionMG<dim, Vector<double>, MGTransferPrebuilt<Vector<double> > >
    preconditioner(mg_dof_handler, mg, mg_transfer);

    // With all this together, we can finally get about solving the linear
    // system in the usual way:
    SolverControl solver_control (1000, 1e-12);
    SolverCG<>    cg (solver_control);

    solution = 0;

    cg.solve (system_matrix, solution, system_rhs,
              preconditioner);
    constraints.distribute (solution);

    std::cout << "   " << solver_control.last_step()
              << " CG iterations needed to obtain convergence."
              << std::endl;
  }

  // @sect4{Postprocessing}

  // The following two functions postprocess a solution once it is
  // computed. In particular, the first one refines the mesh at the beginning
  // of each cycle while the second one outputs results at the end of each
  // such cycle. The functions are almost unchanged from those in step-6, with
  // the exception of two minor differences: The KellyErrorEstimator::estimate
  // function wants an argument of type DoFHandler, not MGDoFHandler, and so
  // we have to cast from derived to base class; and we generate output in VTK
  // format, to use the more modern visualization programs available today
  // compared to those that were available when step-6 was written.
  template <int dim>
  void LaplaceProblem<dim>::refine_grid ()
  {
    Vector<float> estimated_error_per_cell (triangulation.n_active_cells());

    KellyErrorEstimator<dim>::estimate (static_cast<DoFHandler<dim>&>(mg_dof_handler),
                                        QGauss<dim-1>(3),
                                        typename FunctionMap<dim>::type(),
                                        solution,
                                        estimated_error_per_cell);
    GridRefinement::refine_and_coarsen_fixed_number (triangulation,
                                                     estimated_error_per_cell,
                                                     0.3, 0.03);
    triangulation.execute_coarsening_and_refinement ();
  }

  template <int dim>
  void LaplaceProblem<dim>::output_results (const unsigned int cycle) const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (mg_dof_handler);
    data_out.add_data_vector (solution, "solution");
    data_out.build_patches ();

    std::ostringstream filename;
    filename << "solution-"
             << cycle
             << ".vtk";

    std::ofstream output (filename.str().c_str());
    data_out.write_vtk (output);
  }

  // @sect4{LaplaceProblem::run}

  // Like several of the functions above, this is almost exactly a copy of of
  // the corresponding function in step-6. The only difference is the call to
  // assemble_multigrid that takes care of forming the matrices
  // on every level that we need in the multigrid method.
  template <int dim>
  void LaplaceProblem<dim>::run ()
  {
    for (unsigned int cycle=0; cycle<8; ++cycle)
      {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_ball (triangulation);

            static const HyperBallBoundary<dim> boundary;
            triangulation.set_boundary (0, boundary);

            triangulation.refine_global (1);
          }
        else
          refine_grid ();

        std::cout << "   Number of active cells:       "
                  << triangulation.n_active_cells()
                  << std::endl;

        setup_system ();

        std::cout << "   Number of degrees of freedom: "
                  << mg_dof_handler.n_dofs()
                  << " (by level: ";
        for (unsigned int level=0; level<triangulation.n_levels(); ++level)
          std::cout << mg_dof_handler.n_dofs(level)
                    << (level == triangulation.n_levels()-1
                        ? ")" : ", ");
        std::cout << std::endl;

        assemble_system ();
        assemble_multigrid ();

        solve ();
        output_results (cycle);
      }
  }
}

// @sect3{The main() function}
//
// This is again the same function as in step-6:
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step16;

      deallog.depth_console (0);

      LaplaceProblem<2> laplace_problem(1);
      laplace_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}