/* Copyright 2020 Gopinath Chennupati, Raviteja Vangara, Namita Kharat, Erik Skau and Boian Alexandrov,

Triad National Security, LLC. All rights reserved
This program was produced under U.S. Government contract 89233218CNA000001 for Los Alamos National Laboratory (LANL), which is operated by Triad National Security, LLC for the U.S. Department of Energy/National Nuclear Security Administration. All rights in the program are reserved by Triad National Security, LLC, and the U.S. Department of Energy/National Nuclear Security Administration. The Government is granted for itself and others acting on its behalf a nonexclusive, paid-up, irrevocable worldwide license in this material to reproduce, prepare derivative works, distribute copies to the public, perform publicly and display publicly, and to permit others to do so.
This header files computes the reordering of columns of W based on the maximums.

This file is modified from distio.hpp file of Ramakrishnan Kannan's Planc library to include additional i/o operations required for distnmfk library
*/

#ifndef DISTNMF_DISTIO_HPP_
#define DISTNMF_DISTIO_HPP_

#include <unistd.h>
#include <armadillo>
#include <string>
#include "../planc-master/common/distutils.hpp"
#include "../planc-master/distnmf/mpicomm.hpp"

/**
 * File name formats
 * A is the filename
 * 1D distribution Arows_totalpartitions_rank or Acols_totalpartitions_rank
 * Double 1D distribution (both row and col distributed)
 * Arows_totalpartitions_rank and Acols_totalpartitions_rank
 * TWOD distribution A_totalpartition_rank
 * Just send the first parameter Arows and the second parameter Acols to be
 * zero.
 */

namespace planc {

template <class MATTYPE>
class DistIO {
 private:
  const MPICommunicator& m_mpicomm;
  MATTYPE m_Arows;  /// ONED_ROW and ONED_DOUBLE
  MATTYPE m_Acols;  /// ONED_COL and ONED_DOUBLE
  MATTYPE m_A;      /// TWOD
  // don't start getting prime number from 2;
  static const int kPrimeOffset = 10;
  // Hope no one hits on this number.
  static const int kW_seed_idx = 1210873;
#ifdef BUILD_SPARSE
  static const int kalpha = 5;
  static const int kbeta = 10;
#else
  static const int kalpha = 1;
  static const int kbeta = 0;
#endif

  const iodistributions m_distio;
  /**
   * A random matrix is always needed for sparse case
   * to get the pattern. That is., the indices where
   * numbers are being filled. We will use this pattern
   * and change the number later.
   */
  void randMatrix(const std::string type, const int primeseedidx,
                  const double sparsity, MATTYPE* X) {
    if (primeseedidx == -1) {
      arma::arma_rng::set_seed_random();
    } else {
      arma::arma_rng::set_seed(random_sieve(primeseedidx));
    }
#ifdef DEBUG_VERBOSE
    DISTPRINTINFO("randMatrix::" << primeseedidx << "::sp=" << sparsity);
#endif
#ifdef BUILD_SPARSE
    if (type == "uniform" || type == "lowrank") {
      (*X).sprandu((*X).n_rows, (*X).n_cols, sparsity);
    } else if (type == "normal") {
      (*X).sprandn((*X).n_rows, (*X).n_cols, sparsity);
    }
    SP_MAT::iterator start_it = (*X).begin();
    SP_MAT::iterator end_it = (*X).end();
    for (SP_MAT::iterator it = start_it; it != end_it; ++it) {
      double currentValue = (*it);
      (*it) = ceil(kalpha * currentValue + kbeta);
      if ((*it) < 0) (*it) = kbeta;
    }
    // for (uint i=0; i<(*X).n_nonzero;i++){
    //     double currentValue = (*X).values[i];
    //     currentValue *= kalpha;
    //     currentValue += kbeta;
    //     currentValue = ceil(currentValue);
    //     if (currentValue <= 0) currentValue=kbeta;
    //     (*X).values[i]=currentValue;
    // }
#else
    if (type == "uniform") {
      (*X).randu();
    } else if (type == "normal") {
      (*X).randn();
    }
    (*X) = kalpha * (*X) + kbeta;
    (*X) = ceil(*X);
    (*X).elem(find((*X) < 0)).zeros();
#endif
  }

#ifndef BUILD_SPARSE
  /* normalization */
  void normalize(normtype i_normtype) {
    ROWVEC globalnormA = arma::zeros<ROWVEC>(m_A.n_cols);
    ROWVEC normc = arma::zeros<ROWVEC>(m_A.n_cols);
    MATTYPE normmat = arma::zeros<MATTYPE>(m_A.n_rows, m_A.n_cols);
    switch (m_distio) {
      case ONED_ROW:
        if (i_normtype == L2NORM) {
          normc = arma::sum(arma::square(m_A));
          MPI_Allreduce(normc.memptr(), globalnormA.memptr(), m_A.n_cols,
                        MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);

        } else if (i_normtype == MAXNORM) {
          normc = arma::max(m_A);
          MPI_Allreduce(normc.memptr(), globalnormA.memptr(), m_A.n_cols,
                        MPI_DOUBLE, MPI_MAX, MPI_COMM_WORLD);
        }

        break;
      case ONED_COL:
      case ONED_DOUBLE:
        if (i_normtype == L2NORM) {
          globalnormA = arma::sum(arma::square(m_Arows));
        } else if (i_normtype == MAXNORM) {
          globalnormA = arma::max(arma::square(m_Arows));
        }
        break;
      case TWOD:
        if (i_normtype == L2NORM) {
          normc = arma::sum(arma::square(m_A));
          MPI_Allreduce(normc.memptr(), globalnormA.memptr(), m_A.n_cols,
                        MPI_DOUBLE, MPI_SUM, this->m_mpicomm.commSubs()[1]);
        } else if (i_normtype == MAXNORM) {
          normc = arma::max(m_A);
          MPI_Allreduce(normc.memptr(), globalnormA.memptr(), m_A.n_cols,
                        MPI_DOUBLE, MPI_SUM, this->m_mpicomm.commSubs()[1]);
        }
        break;
      default:
        INFO << "cannot normalize" << std::endl;
    }
    normmat = arma::repmat(globalnormA, m_A.n_rows, 1);
    m_A /= normmat;
  }
#endif

  /**
   * Uses the pattern from the input matrix X but
   * the value is computed as low rank.
   */
  void randomLowRank(const UWORD m, const UWORD n, const UWORD k, MATTYPE* X) {
    uint start_row = 0, end_row = 0, start_col = 0, end_col = 0;
    switch (m_distio) {
      case ONED_ROW:
        start_row = MPI_RANK * (*X).n_rows;
        end_row = ((MPI_RANK + 1) * (*X).n_rows) - 1;
        start_col = 0;
        end_col = (*X).n_cols - 1;
        break;
      case ONED_COL:
        start_row = 0;
        end_row = (*X).n_rows - 1;
        start_col = MPI_RANK * (*X).n_cols;
        end_col = ((MPI_RANK + 1) * (*X).n_cols) - 1;
        break;
      // in the case of ONED_DOUBLE we are stitching
      // m/p x n/p matrices and in TWOD we are
      // stitching m/pr * n/pc matrices.
      case ONED_DOUBLE:
        if ((*X).n_cols == n) {  //  m_Arows
          start_row = MPI_RANK * (*X).n_rows;
          end_row = ((MPI_RANK + 1) * (*X).n_rows) - 1;
          start_col = 0;
          end_col = n - 1;
        }
        if ((*X).n_rows == m) {  // m_Acols
          start_row = 0;
          end_row = m - 1;
          start_col = MPI_RANK * (*X).n_cols;
          end_col = ((MPI_RANK + 1) * (*X).n_cols) - 1;
        }
        break;
      case TWOD:
        start_row = MPI_ROW_RANK * (*X).n_rows;
        end_row = ((MPI_ROW_RANK + 1) * (*X).n_rows) - 1;
        start_col = MPI_COL_RANK * (*X).n_cols;
        end_col = ((MPI_COL_RANK + 1) * (*X).n_cols) - 1;
        break;
    }
    // all machines will generate same Wrnd and Hrnd
    // at all times.
    arma::arma_rng::set_seed(kW_seed_idx);
    MAT Wrnd(m, k);
    Wrnd.randu();
    MAT Hrnd(k, n);
    Hrnd.randu();
#ifdef BUILD_SPARSE
    SP_MAT::iterator start_it = (*X).begin();
    SP_MAT::iterator end_it = (*X).end();
    double tempVal = 0.0;
    for (SP_MAT::iterator it = start_it; it != end_it; ++it) {
      VEC Wrndi = vectorise(Wrnd.row(start_row + it.row()));
      VEC Hrndj = Hrnd.col(start_col + it.col());
      tempVal = dot(Wrndi, Hrndj);
      (*it) = ceil(kalpha * tempVal + kbeta);
    }
#else
    MAT templr;
    if ((*X).n_cols == n) {  // ONED_ROW
      MAT myWrnd = Wrnd.rows(start_row, end_row);
      templr = myWrnd * Hrnd;
    } else if ((*X).n_rows == m) {  // ONED_COL
      MAT myHcols = Hrnd.cols(start_col, end_col);
      templr = Wrnd * myHcols;
    } else if ((*X).n_rows == (m / MPI_SIZE) && (*X).n_cols == (n / MPI_SIZE) ||
               ((*X).n_rows == (m / this->m_mpicomm.pr()) &&
                (*X).n_cols == (n / this->m_mpicomm.pc()))) {
      MAT myWrnd = Wrnd.rows(start_row, end_row);
      MAT myHcols = Hrnd.cols(start_col, end_col);
      templr = myWrnd * myHcols;
    }
    (*X) = ceil(kalpha * templr + kbeta);
#endif
  }

#ifdef BUILD_SPARSE
  void uniform_dist_matrix(MATTYPE& A) {
    // make the matrix of ONED distribution
    // sometimes in the pacoss it gives nearly equal
    // distribution. we have to make sure everyone of
    // same size;
    int max_rows = 0, max_cols = 0;
    int my_rows = A.n_rows;
    int my_cols = A.n_cols;
    bool last_exist = false;
    double my_min_value = 0.0;
    double overall_min;
    UWORD my_correct_nnz = 0;
    if (A.n_nonzero > 0) {
      my_min_value = A.values[0];
    }
    MPI_Allreduce(&my_rows, &max_rows, 1, MPI_INT, MPI_MAX, MPI_COMM_WORLD);
    MPI_Allreduce(&my_cols, &max_cols, 1, MPI_INT, MPI_MAX, MPI_COMM_WORLD);
    // choose max rows and max cols nicely
    max_rows -= (max_rows % m_mpicomm.pr());
    max_cols -= (max_cols % m_mpicomm.pc());
    // count the number of nnz's that satisfies this
    // criteria
    try {
      if (A.n_nonzero > 0) {
        SP_MAT::iterator start_it = A.begin();
        SP_MAT::iterator end_it = A.end();
        for (SP_MAT::iterator it = start_it; it != end_it; ++it) {
          if (it.row() < max_rows && it.col() < max_cols) {
            my_correct_nnz++;
            if (*it != 0 && my_min_value < *it) {
              my_min_value = *it;
            }
          }
          if (it.row() == max_rows - 1 && it.col() == max_cols - 1) {
            last_exist = true;
          }
        }
      }
      if (!last_exist) {
        my_correct_nnz++;
      }

      if (A.n_nonzero == 0) {
        my_correct_nnz++;
      }
      MPI_Allreduce(&my_min_value, &overall_min, 1, MPI_INT, MPI_MIN,
                    MPI_COMM_WORLD);
      arma::umat locs;
      VEC vals;
      DISTPRINTINFO("max_rows::" << max_rows << "::max_cols::" << max_cols
                                 << "::my_rows::" << my_rows << "::my_cols::"
                                 << my_cols << "::last_exist::" << last_exist
                                 << "::my_nnz::" << A.n_nonzero
                                 << "::my_correct_nnz::" << my_correct_nnz);
      locs = arma::zeros<arma::umat>(2, my_correct_nnz);
      vals = arma::zeros<VEC>(my_correct_nnz);
      if (A.n_nonzero > 0) {
        SP_MAT::iterator start_it = A.begin();
        SP_MAT::iterator end_it = A.end();
        double idx = 0;
        for (SP_MAT::iterator it = start_it; it != end_it; ++it) {
          if (it.row() < max_rows && it.col() < max_cols) {
            locs(0, idx) = it.row();
            locs(1, idx) = it.col();
            vals(idx++) = *it;
          }
        }
      } else {
        locs(0, 0) = 0;
        locs(1, 0) = 0;
        vals(0) = overall_min;
      }
      if (A.n_nonzero > 0 && !last_exist) {
        locs(0, my_correct_nnz - 1) = max_rows - 1;
        locs(1, my_correct_nnz - 1) = max_cols - 1;
        vals(my_correct_nnz - 1) = overall_min;
      }
      SP_MAT A_new(locs, vals);
      A.clear();
      A = A_new;
    } catch (const std::exception& e) {
      DISTPRINTINFO(e.what()
                    << "max_rows::" << max_rows << "::max_cols::" << max_cols
                    << "::my_rows::" << my_rows << "::my_cols::" << my_cols
                    << "::last_exist::" << last_exist << "::my_nnz::"
                    << A.n_nonzero << "::my_correct_nnz::" << my_correct_nnz);
    }
  }
#endif

 public:
  DistIO<MATTYPE>(const MPICommunicator& mpic, const iodistributions& iod)
      : m_mpicomm(mpic), m_distio(iod) {}
  /**
   * We need m,n,pr,pc only for rand matrices. If otherwise we are
   * expecting the file will hold all the details.
   * If we are loading by file name we dont need distio flag.
   * If the filename is rand_lowrank/rand_uniform, appropriate
   * random functions will be called. Otherwise, it will be loaded from file.
   * @param[in] file_name. For random matrices rand_lowrank/rand_uniform
   * @param[in] m - globalm. Needed only for random matrices.
   *                otherwise, we will know from file.
   * @param[in] n - globaln. Needed only for random matrices
   * @param[in] k - low rank. Used for generating synthetic lowrank matrices.
   * @param[in] sparsity - sparsity factor between 0-1 for sparse matrices.
   * @param[in] pr - Number of row processors in the 2D processor grid
   * @param[in] pc - Number of columnn processors in the 2D processor grid
   * @param[in] normalization - L2 column normalization of input matrix.
   */
  void readInput(const std::string file_name, UWORD m = 0, UWORD n = 0,
                 UWORD k = 0, double sparsity = 0, UWORD pr = 0, UWORD pc = 0,
                 normtype i_normalization = NONE) {
    // INFO << "readInput::" << file_name << "::" << distio << "::"
    //     << m << "::" << n << "::" << pr << "::" << pc
    //     << "::" << this->MPI_RANK << "::" << this->m_mpicomm.size() <<
    //     std::endl;
    std::string rand_prefix("rand_");
    if (!file_name.compare(0, rand_prefix.size(), rand_prefix)) {
      std::string type = file_name.substr(rand_prefix.size());
      assert(type == "normal" || type == "lowrank" || type == "uniform");
      switch (m_distio) {
        case ONED_ROW:
          m_Arows.zeros(m / MPI_SIZE, n);
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, &m_Arows);
          if (type == "lowrank") {
            randomLowRank(m, n, k, &m_Arows);
          }
          break;
        case ONED_COL:
          m_Acols.zeros(m, n / MPI_SIZE);
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, &m_Acols);
          if (type == "lowrank") {
            randomLowRank(m, n, k, &m_Acols);
          }
          break;
        case ONED_DOUBLE: {
          int p = MPI_SIZE;
          m_Arows.set_size(m / p, n);
          m_Acols.set_size(m, n / p);
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, &m_Arows);
          if (type == "lowrank") {
            randomLowRank(m, n, k, &m_Arows);
          }
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, &m_Acols);
          if (type == "lowrank") {
            randomLowRank(m, n, k, &m_Acols);
          }
          break;
        }
        case TWOD:
          m_A.zeros(m / pr, n / pc);
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, &m_A);
          if (type == "lowrank") {
            randomLowRank(m, n, k, &m_A);
          }
          break;
      }
    } else {
      std::stringstream sr, sc;
      if (m_distio == ONED_ROW || m_distio == ONED_DOUBLE) {
        sr << file_name << "rows_" << MPI_SIZE << "_" << MPI_RANK;
#ifdef BUILD_SPARSE
        m_Arows.load(sr.str(), arma::coord_ascii);
        uniform_dist_matrix(m_Arows);
#else
        m_Arows.load(sr.str());
#endif
      }
      if (m_distio == ONED_COL || m_distio == ONED_DOUBLE) {
        sc << file_name << "cols_" << MPI_SIZE << "_" << MPI_RANK;
#ifdef BUILD_SPARSE
        m_Acols.load(sc.str(), arma::coord_ascii);
        uniform_dist_matrix(m_Acols);
#else
        m_Acols.load(sc.str());
#endif
        m_Acols = m_Acols.t();
      }
      if (m_distio == TWOD) {
        // sr << file_name << "_" << MPI_SIZE << "_" << MPI_RANK;
        sr << file_name << MPI_RANK;
#ifdef BUILD_SPARSE
        MAT temp_ijv;
        temp_ijv.load(sr.str(), arma::raw_ascii);
        if (temp_ijv.n_rows > 0 && temp_ijv.n_cols > 0) {
          MAT vals(2, temp_ijv.n_rows);
          MAT idxs_only = temp_ijv.cols(0, 1);
          arma::umat idxs = arma::conv_to<arma::umat>::from(idxs_only);
          arma::umat idxst = idxs.t();
          vals = temp_ijv.col(2);
          SP_MAT temp_spmat(idxst, vals);
          m_A = temp_spmat;
        } else {
          arma::umat idxs = arma::zeros<arma::umat>(2, 1);
          VEC vals = arma::zeros<VEC>(1);
          SP_MAT temp_spmat(idxs, vals);
          m_A = temp_spmat;
        }
        // m_A.load(sr.str(), arma::coord_ascii);
        uniform_dist_matrix(m_A);
#else
        m_A.load(sr.str());
#endif
      }
    }
#ifndef BUILD_SPARSE
    if (i_normalization != NONE) {
      normalize(i_normalization);
    }
#endif
  }

  /**
   * Produce a distributed uniform random matrix \beta from the interval (-0.1, 0.1)
   * Add \beta + 1. Element-wise multiplication of \beta and A, that is A % beta
   */
  void randBetaMatrix(const std::string type, const int primeseedidx,
                  MATTYPE* b) {
    if (primeseedidx == -1) {
      arma::arma_rng::set_seed_random();
    } else {
      arma::arma_rng::set_seed(random_sieve(primeseedidx));
    }
#ifdef DEBUG_VERBOSE
    DISTPRINTINFO("randAtimesBeta::" << primeseedidx);
#endif
#ifdef BUILD_SPARSE
    if (type == "uniform" || type == "lowrank") {
      (*X).sprandu((*X).n_rows, (*X).n_cols, sparsity);
    } else if (type == "normal") {
      (*X).sprandn((*X).n_rows, (*X).n_cols, sparsity);
    }
    SP_MAT::iterator start_it = (*X).begin();
    SP_MAT::iterator end_it = (*X).end();
    for (SP_MAT::iterator it = start_it; it != end_it; ++it) {
      double currentValue = (*it);
      (*it) = ceil(kalpha * currentValue + kbeta);
      if ((*it) < 0) (*it) = kbeta;
    }
    // for (uint i=0; i<(*X).n_nonzero;i++){
    //     double currentValue = (*X).values[i];
    //     currentValue *= kalpha;
    //     currentValue += kbeta;
    //     currentValue = ceil(currentValue);
    //     if (currentValue <= 0) currentValue=kbeta;
    //     (*X).values[i]=currentValue;
    // }
#else
    if (type == "uniform") {
      (*b).randu();
    } else if (type == "normal") {
      (*b).randn();
    }
    float lb = -0.001, ub = 0.001;
    (*b) =  (ub-lb) * (*b) + lb;
    (*b) = (*b) + 1;
    //(*X) = (*X) % (*b);
#endif
  }

  /**
   * Writes the factor matrix as output_file_name_W_MPISIZE_MPIRANK
   * @param[in] Local W factor matrix
   * @param[in] Local H factor matrix
   * @param[in] output file name
   */
  void writeOutput(const MAT& W, const MAT& H,
                   const std::string& output_file_name) {
    std::stringstream sw, sh;
    sw << output_file_name << "_W_" << MPI_SIZE << "_" << MPI_RANK;
    sh << output_file_name << "_H_" << MPI_SIZE << "_" << MPI_RANK;
    W.save(sw.str(), arma::raw_ascii);
    H.save(sh.str(), arma::raw_ascii);
  }

  void writeOutput(const MAT& W, const MAT& H, int &iter, int &k,
                   const std::string& output_file_name) {
    std::stringstream sw, sh;
    sw << output_file_name << "_W_K" << k << "_Run" << iter << "_Procs" << MPI_SIZE << "_proc" << MPI_RANK;
    sh << output_file_name << "_H_K" << k << "_Run" << iter << "_Procs" << MPI_SIZE << "_proc" << MPI_RANK;
    W.save(sw.str(), arma::raw_ascii);
    H.save(sh.str(), arma::raw_ascii);
  }

  void writeSilhouettes(const MAT& Si, int &k, const std::string& output_silhouettes) {
    if(MPI_RANK == 0)   {
        std::stringstream si;
        si << output_silhouettes << "Si_at_k" << k;
        Si.save(si.str(), arma::raw_ascii);
      }
  }

  void writeRandInput() {
    std::string file_name("Arnd");
    std::stringstream sr, sc;
    if (m_distio == TWOD) {
      sr << file_name << "_" << MPI_SIZE << "_" << MPI_RANK;
      DISTPRINTINFO("Writing rand input file " << sr.str()
                                               << PRINTMATINFO(m_A));

#ifdef BUILD_SPARSE
      this->m_A.save(sr.str(), arma::coord_ascii);
#else
      this->m_A.save(sr.str(), arma::raw_ascii);
#endif
    }
    if (m_distio == ONED_ROW || m_distio == ONED_DOUBLE) {
      sr << file_name << "rows_" << MPI_SIZE << "_" << MPI_RANK;
      DISTPRINTINFO("Writing rand input file " << sr.str()
                                               << PRINTMATINFO(m_Arows));
#ifdef BUILD_SPARSE
      this->m_Arows.save(sr.str(), arma::coord_ascii);
#else
      this->m_Arows.save(sr.str(), arma::raw_ascii);
#endif
    }
    if (m_distio == ONED_COL || m_distio == ONED_DOUBLE) {
      sc << file_name << "cols_" << MPI_SIZE << "_" << MPI_RANK;
      DISTPRINTINFO("Writing rand input file " << sc.str()
                                               << PRINTMATINFO(m_Acols));
#ifdef BUILD_SPARSE
      this->m_Acols.save(sc.str(), arma::coord_ascii);
#else
      this->m_Acols.save(sc.str(), arma::raw_ascii);
#endif
    }
  }
  const MATTYPE& Arows() const { return m_Arows; }
  const MATTYPE& Acols() const { return m_Acols; }
  const MATTYPE& A() const { return m_A; }
  const MPICommunicator& mpicomm() const { return m_mpicomm; }
};

}  // namespace planc

// run with mpi run 3.
void testDistIO(char argc, char* argv[]) {
  planc::MPICommunicator mpicomm(argc, argv);
#ifdef BUILD_SPARSE
  planc::DistIO<SP_MAT> dio(mpicomm, ONED_DOUBLE);
#else
  planc::DistIO<MAT> dio(mpicomm, ONED_DOUBLE);
#endif
  dio.readInput("rand", 12, 9, 0.5);
  INFO << "Arows:" << mpicomm.rank() << std::endl
       << arma::conv_to<MAT>::from(dio.Arows()) << std::endl;
  INFO << "Acols:" << mpicomm.rank() << std::endl
       << arma::conv_to<MAT>::from(dio.Acols()) << std::endl;
}

#endif  // DISTNMF_DISTIO_HPP_


#include <Eigen/Dense>
#include <gtest/gtest.h>

namespace Eigen {
    // C++ and/or gtest require that these two methods, which are used by calling
    // ASSERT_PRED_FORMAT2, be in the namespace of its argument.

    // utility function to print an eigen object to an ostream; gtest will use this when
    // it outputs a matrix used in a failed assertion. Without this function, gtest seems
    // to dump some kind of byte representation of an eigen matrix, which is not very
    // helpful.
    template <class Derived>
    void PrintTo(const Eigen::EigenBase<Derived>& mat, ::std::ostream* os)
    {
        (*os) << mat.derived() << "\n";
    }

    // utility function for gtest to use to check if two eigen objects are identical.
    // returns assertion success when they are identical; returns assertion failure along
    // with a nicely formatted message with the matrix contents when they are not
    // identical.
    //
    // I put this function in this matrix test cpp file for a few reasons: 1) there is
    // not already a header file to put common test code for eigen3-hdf5, and 2) because
    // I needed it to help me debug test failures as I implemented the no copy read and
    // write functions. I really think that providing a header for common test code
    // should be addressed at some point, and then this function (and its companion
    // PrintTo) should be moved there.
    //
    // Usage:
    //
    // ASSERT_PRED_FORMAT2(assert_same, mat, mat2);
    template <class DerivedExp, class DerivedAct>
    ::testing::AssertionResult assert_same(const char* exp_expr,
        const char* act_expr,
        const Eigen::EigenBase<DerivedExp>& exp,
        const Eigen::EigenBase<DerivedAct>& act)
    {
        if (exp.rows() == act.rows() &&
            exp.cols() == act.cols() &&
            exp.derived() == act.derived())
        {
            return ::testing::AssertionSuccess();
        }

        // if eigen did not define the == operator, you could use
        // exp.derived().cwiseEqual(act.derived()).all();

        ::testing::AssertionResult result = ::testing::AssertionFailure()
            << "Eigen objects are not the same: ("
            << exp_expr << ", " << act_expr << ")\n"
            << exp_expr << ":\n"
            << ::testing::PrintToString(exp)
            << "\n---and\n" << act_expr << ":\n"
            << ::testing::PrintToString(act)
            << "\n---are not equal!\n";

        return result;
    }
} // namespace Eigen


#pragma once

#include "Matrix3x3.hpp"
#include "Point2d.hpp"
#include "Point3d.hpp"
#include "Pixel.hpp"
#include "StaticVector.hpp"
#include "structures.hpp"
#include <boost/property_tree/ptree.hpp>
#include <string>
#include <vector>
#include <map>

namespace HeroMesh
{

	namespace bpt = boost::property_tree;

	namespace sfmData {
		class SfMData;
	} // namespace sfmData

	namespace mvsUtils {

		enum class EFileType {
			P = 0,
			K = 1,
			iK = 2,
			R = 3,
			iR = 4,
			C = 5,
			iP = 6,
			har = 7,
			prematched = 8,
			growed = 10,
			op = 11,
			occMap = 12,
			wshed = 13,
			nearMap = 14,
			img = 17,
			graphCutMap = 20,
			graphCutPts = 21,
			growedMap = 22,
			agreedMap = 23,
			agreedPts = 24,
			refinedMap = 25,
			seeds_sfm = 26,
			radial_disortion = 27,
			graphCutMesh = 28,
			agreedMesh = 29,
			nearestAgreedMap = 30,
			segPlanes = 31,
			agreedVisMap = 32,
			diskSizeMap = 33,
			imgT = 34,
			depthMap = 35,
			simMap = 36,
			mapPtsTmp = 37,
			camMap = 39,
			mapPtsSimsTmp = 40,
			nmodMap = 41,
			D = 42,
			normalMap = 43,
		};

		class MultiViewParams
		{
		public:
			/// prepareDenseScene data
			std::string _imagesFolder;
			/// camera projection matrix P
			std::vector<Matrix3x4> camArr;
			/// camera intrinsics matrix K: [focalx skew u; 0 focaly v; 0 0 1]
			std::vector<Matrix3x3> KArr;
			/// inverse camera intrinsics matrix K
			std::vector<Matrix3x3> iKArr;
			/// camera rotation matrix R
			std::vector<Matrix3x3> RArr;
			/// inverse camera rotation matrix R
			std::vector<Matrix3x3> iRArr;
			/// camera position C in world coordinate system
			std::vector<Point3d> CArr;
			/// K * R inverse matrix
			std::vector<Matrix3x3> iCamArr;
			///
			std::vector<Point3d> FocK1K2Arr;
			/// number of cameras
			int ncams;
			///
			float simThr;
			int g_border = 2;
			bool verbose;

			boost::property_tree::ptree userParams;

			MultiViewParams(const sfmData::SfMData& sfmData,
				const std::string& imagesFolder = "",
				const std::string& depthMapsFolder = "",
				const std::string& depthMapsFilterFolder = "",
				bool readFromDepthMaps = false,
				int downscale = 1,
				StaticVector<CameraMatrices>* cameras = nullptr);

			~MultiViewParams();

			inline Point3d backproject(const int camIndex, const Point2d& pix, double depth) const
			{
				const Point3d p = CArr[camIndex] + (iCamArr[camIndex] * pix).normalize() * depth;
				return p;
			}
			inline const std::string& getImagePath(int index) const
			{
				return _imagesParams.at(index).path;
			}

			inline int getViewId(int index) const
			{
				return _imagesParams.at(index).viewId;
			}

			inline int getOriginalWidth(int index) const
			{
				return _imagesParams.at(index).width;
			}

			inline int getOriginalHeight(int index) const
			{
				return _imagesParams.at(index).height;
			}

			inline int getOriginalSize(int index) const
			{
				return _imagesParams.at(index).size;
			}

			inline int getWidth(int index) const
			{
				return _imagesParams.at(index).width / getDownscaleFactor(index);
			}

			inline int getHeight(int index) const
			{
				return _imagesParams.at(index).height / getDownscaleFactor(index);
			}

			inline int getSize(int index) const
			{
				return _imagesParams.at(index).size / getDownscaleFactor(index);
			}

			inline const std::vector<ImageParams>& getImagesParams() const
			{
				return _imagesParams;
			}

			inline const ImageParams& getImageParams(int i) const
			{
				return _imagesParams.at(i);
			}

			inline int getDownscaleFactor(int index) const
			{
				return _imagesScale.at(index) * _processDownscale;
			}

			inline int getProcessDownscale() const
			{
				return _processDownscale;
			}

			inline int getMaxImageWidth() const
			{
				return _maxImageWidth;
			}

			inline int getMaxImageHeight() const
			{
				return _maxImageHeight;
			}

			inline int getNbCameras() const
			{
				return _imagesParams.size();
			}

			inline int getIndexFromViewId(IndexT viewId) const
			{
				return _imageIdsPerViewId.at(viewId);
			}

			inline float getMinViewAngle() const
			{
				return _minViewAngle;
			}

			inline float getMaxViewAngle() const
			{
				return _maxViewAngle;
			}

			inline std::vector<double> getOriginalP(int index) const
			{
				std::vector<double> p44; // projection matrix (4x4) scale 1
				const Matrix3x4& p34 = camArr.at(index); // projection matrix (3x4) scale = getDownscaleFactor()
				const int downscale = getDownscaleFactor(index);
				p44.assign(p34.m, p34.m + 12);
				std::transform(p44.begin(), p44.begin() + 8, p44.begin(), std::bind1st(std::multiplies<double>(), downscale));
				p44.push_back(0);
				p44.push_back(0);
				p44.push_back(0);
				p44.push_back(1);
				return p44;
			}

			inline const std::string& getDepthMapsFolder() const
			{
				return _depthMapsFolder;
			}

			inline const std::string& getDepthMapsFilterFolder() const
			{
				return _depthMapsFilterFolder;
			}

			inline const sfmData::SfMData& getInputSfMData() const
			{
				return _sfmData;
			}

			const std::map<std::string, std::string>& getMetadata(int index) const;

			bool is3DPointInFrontOfCam(const Point3d* X, int rc) const;

			void getMinMaxMidNbDepth(int index, float& min, float& max, float& mid, std::size_t& nbDepths, float percentile = 0.999f) const;
			void getPixelFor3DPoint(Point2d* out, const Point3d& X, const Matrix3x4& P) const;
			void getPixelFor3DPoint(Point2d* out, const Point3d& X, int rc) const;
			void getPixelFor3DPoint(Pixel* out, const Point3d& X, int rc) const;
			double getCamPixelSize(const Point3d& x0, int cam) const;
			double getCamPixelSize(const Point3d& x0, int cam, float d) const;
			double getCamPixelSizeRcTc(const Point3d& p, int rc, int tc, float d) const;
			double getCamPixelSizePlaneSweepAlpha(const Point3d& p, int rc, int tc, int scale, int step) const;
			double getCamPixelSizePlaneSweepAlpha(const Point3d& p, int rc, StaticVector<int>* tcams, int scale, int step) const;

			double getCamsMinPixelSize(const Point3d& x0, std::vector<unsigned short>* tcams) const;
			double getCamsMinPixelSize(const Point3d& x0, StaticVector<int>& tcams) const;

			bool isPixelInSourceImage(const Pixel& pixRC, int camId, int margin) const;
			bool isPixelInImage(const Pixel& pix, int camId, int margin) const;
			bool isPixelInImage(const Pixel& pix, int camId) const;
			bool isPixelInImage(const Point2d& pix, int camId) const;
			bool isPixelInImage(const Point2d& pix, int camId, int margin) const;
			void decomposeProjectionMatrix(Point3d& Co, Matrix3x3& Ro, Matrix3x3& iRo, Matrix3x3& Ko, Matrix3x3& iKo, Matrix3x3& iPo, const Matrix3x4& P) const;

			/**
			 * @brief findCamsWhichIntersectsHexahedron
			 * @param hexah 0-3 frontal face, 4-7 back face
			 * @param minMaxDepthsFileName
			 * @return
			 */
			StaticVector<int> findCamsWhichIntersectsHexahedron(const Point3d hexah[8], const std::string& minMaxDepthsFileName) const;

			/**
			 * @brief findCamsWhichIntersectsHexahedron
			 * @param hexah 0-3 frontal face, 4-7 back face
			 * @return
			 */
			StaticVector<int> findCamsWhichIntersectsHexahedron(const Point3d hexah[8]) const;

			/**
			 * @brief findNearestCamsFromLandmarks
			 * @param rc
			 * @param nbNearestCams
			 * @return
			 */
			StaticVector<int> findNearestCamsFromLandmarks(int rc, int nbNearestCams) const;

			inline void setMinViewAngle(float minViewAngle)
			{
				_minViewAngle = minViewAngle;
			}

			inline void setMaxViewAngle(float maxViewAngle)
			{
				_maxViewAngle = maxViewAngle;
			}

		private:
			/// image params list (width, height, size)
			std::vector<ImageParams> _imagesParams;
			/// image id per view id
			std::map<IndexT, int> _imageIdsPerViewId;
			/// image scale list
			std::vector<int> _imagesScale;
			/// downscale apply to input images during process
			int _processDownscale = 1;
			/// maximum width
			int _maxImageWidth = 0;
			/// maximum height
			int _maxImageHeight = 0;
			/// depthMapEstimate data folder
			std::string _depthMapsFolder;
			/// depthMapFilter data folder
			std::string _depthMapsFilterFolder;
			/// use silhouettes
			bool _useSil = false;
			/// minimum view angle
			float _minViewAngle = 2.0f;
			/// maximum view angle
			float _maxViewAngle = 70.0f;  // WARNING: may be too low, especially when using seeds from SfM
			/// input sfmData
			const sfmData::SfMData& _sfmData;

			void loadMatricesFromTxtFile(int index, const std::string& fileNameP, const std::string& fileNameD);
			void loadMatricesFromRawProjectionMatrix(int index, const double* rawProjMatix);
			void loadMatricesFromSfM(int index);

			inline void resizeCams(int _ncams)
			{
				ncams = _ncams;
				camArr.resize(ncams);
				KArr.resize(ncams);
				iKArr.resize(ncams);
				RArr.resize(ncams);
				iRArr.resize(ncams);
				CArr.resize(ncams);
				iCamArr.resize(ncams);
				FocK1K2Arr.resize(ncams);
				_imagesScale.resize(ncams);
			}
		};

	} // namespace mvsUtils
} // namespace HeroMesh


/**\file ampdispersion.c
 * \author Piyush Agram.
 *  */

#include "despeck.hpp"
#include "gdal.h"
#include "gdal_priv.h"
#include "cpl_conv.h"
#include "fringe/fringe_common.hpp"
#include <armadillo>

int despeck_process(despeckOptions *opts)
{

    //Print user options to screen
    opts->print();

    int cols, rows, nbands;
    int blockysize, boxesperblock;

    //First thing to do is to read dimenions.
    GDALDataset* inDataset = NULL;
    GDALDataset* wtsDataset = NULL;
    GDALDataset* outDataset = NULL;

    //Clock variables
    double t_start, t_end;

    //Register GDAL drivers
    GDALAllRegister();

    //Output data type
    GDALDataType outType;

    //Determine the number of uint32 bytes needed to store weights
    int nulong = ceil( ((2*opts->Ny+1)*(2*opts->Nx+1))/ 32.0);
    std::cout << "Number of uint32 bytes for mask: " << nulong << "\n";

    //Open the stack dataset
    inDataset = reinterpret_cast<GDALDataset *>( GDALOpenShared( opts->inputDS.c_str(), GA_ReadOnly));

    if (inDataset == NULL)
    {
        std::cout << "Cannot open stack file { " <<  opts->inputDS << " } with GDAL for reading. \n";

        std::cout << "GDALOpen failed - " << opts->inputDS << "\n";
        std::cout << "Exiting with error code .... (102) \n";
        GDALDestroyDriverManager();
        return 102;
    }

    cols = inDataset->GetRasterXSize();
    rows = inDataset->GetRasterYSize();
    nbands = inDataset->GetRasterCount();

    std::cout << "Number of rows  = " << rows << "\n";
    std::cout << "Number of cols  = " << cols << "\n";
    std::cout << "Number of bands = " << nbands << "\n";

    if ((opts->ibands[0] <= 0) || (opts->ibands[0] > nbands))
    {
        std::cout << "Master band " << opts->ibands[0] << " outside the range of permissible bands\n";
        std::cout << "Exiting with error code ... (102) \n";
        GDALClose(inDataset);
        GDALDestroyDriverManager();
        return 102;
    }

    if ((opts->ibands[1] > 0) && (opts->ibands[1] > nbands))
    {
        std::cout << "Slave band " << opts->ibands[1] << "outside the range of permissible bands\n";
        std::cout << "Exiting with error code ... (102) \n";
        GDALClose(inDataset);
        GDALDestroyDriverManager();
        return 102;
    }

    //Open weights file
    wtsDataset = reinterpret_cast<GDALDataset *>( GDALOpenShared( opts->wtsDS.c_str(), GA_ReadOnly));

    if (wtsDataset == NULL)
    {
        std::cout << "Could not open weights dataset {" << opts->wtsDS << "}\n";
        std::cout << "Exiting with non-zero error code ... 105 \n";
        GDALClose(inDataset);
        GDALDestroyDriverManager();
        return 105;
    }

    //Check for consistency between input and weight file
    {
        int code = 0;
        int cc = wtsDataset->GetRasterXSize();
        if (cc != cols)
        {
            std::cout << "Width mismatch between input dataset and weight dataset\n";
            code = 106;
        }

        int rr = wtsDataset->GetRasterYSize();
        if (rr != rows)
        {
            std::cout << "Length mismatch between input dataset and weight dataset \n";
            code = 107;
        }

        int bb = wtsDataset->GetRasterCount();
        if (bb != nulong)
        {
            std::cout << "Number of bands mismatch for weights and window size \n";
            code = 108;
        }

        {
            int inNx = ::atoi(wtsDataset->GetMetadataItem("HALFWINDOWX", "ENVI"));
            int inNy = ::atoi(wtsDataset->GetMetadataItem("HALFWINDOWY", "ENVI"));

            if (inNx != opts->Nx)
            {
                std::cout << "Half window size x of wts is different from input. \n";
                code = 109;
            }
            if (inNx == 0)
            {
                std::cout << "No non-zero metadata item called HALFWINDOWX \n";
                code = 109;
            }

            if (inNy != opts->Ny)
            {
                std::cout << "Half window size y of wts is different from input. \n";
                code = 110;
            }
            if (inNy == 0)
            {
                std::cout << "No non-zero metadata item called HALFWINDOWY \n";
                code = 110;
            }
        }

        if (code != 0)
        {
            std::cout <<"Exiting with error code ....("<<code<<")\n";
            GDALClose(inDataset);
            GDALClose(wtsDataset);
            GDALDestroyDriverManager();

            return code;
        }
    }

    //Determine blocksizes
    boxesperblock = int((opts->memsize * 1.0e6)/cols) / (opts->blocksize * 4 * (6+nulong)) ;
    blockysize = boxesperblock * opts->blocksize;
    if (blockysize < opts->blocksize)
    {
        blockysize = opts->blocksize;
        boxesperblock = 1;
    }

    std::cout << "Block size = " << blockysize << " lines \n";

    int totalblocks = ceil( rows / (1.0 * blockysize));
    std::cout << "Total number of blocks to process: " << totalblocks << "\n";

    //Start the clock
    t_start = getWallTime();

    //Temporary array for reading in complex data
    arma::cx_fvec cpxdata1(cols*blockysize);
    arma::cx_fvec cpxdata2(cols*blockysize);
    arma::Mat<unsigned int> wts(nulong, cols*blockysize);
    arma::cx_fvec filt(cols*blockysize);

    //Start block-by-block processing
    int blockcount = 0;
    int status;
    int nthreads;

    //Creation of output dataset
    {
        GDALDriver *poDriver = (GDALDriver*) GDALGetDriverByName("ENVI");
        char **mOptions = NULL;
        mOptions = CSLSetNameValue( mOptions, "INTERLEAVE", "BIP");
        mOptions = CSLSetNameValue( mOptions, "SUFFIX", "ADD");

        outType = GDT_Float32;
        if (opts->ibands[1] > 0)
        {
            outType = GDT_CFloat32;
        }

        outDataset = (GDALDataset*) poDriver->Create(opts->outputDS.c_str(), cols, rows, 1, outType, mOptions);

        if (outDataset == NULL)
        {
            std::cout << "Could not create despecked dataset {" << opts->outputDS << "} \n";
            std::cout << "Exiting with non-zero error code ... 104 \n";

            GDALClose(inDataset);
            GDALClose(wtsDataset);
            GDALDestroyDriverManager();
            return 104;
        }
        CSLDestroy(mOptions);
    }

    nthreads = numberOfThreads();

    //Useful variables
    int Ny = opts->Ny;
    int Nx = opts->Nx;
    int Wy = 2*Ny+1;
    int Wx = 2*Nx+1;
    int band1 = opts->ibands[0];
    int band2 = opts->ibands[1];

    const Ulongmask bitmask = {Ny,Nx};

    //Block-by-block processing
    int yoff = 0;

    while( yoff < rows)
    {
        //Increment block counter
        blockcount++;

        int inysize = blockysize;
        //Number of lines to read for the block
        if ((yoff+inysize) > rows)
            inysize = rows - yoff;

//        std::cout << "Block " << blockcount << ": " << inysize << " lines \n";

        //Block logic
        int firstlinetowrite;
        int linestowrite;
        int rollback;

        //For first block include the top half window.
        if (blockcount == 1)
        {
            firstlinetowrite = 0;
            linestowrite = inysize - Ny;
            rollback = Ny;

            if( (yoff+blockysize) >= rows) //There is only one block, write the whole thing
            {
                linestowrite = inysize;
                rollback = 0;
            }

        }
        else if( (yoff+blockysize) >= rows) //Last block. Write bottom half window.
        {
            firstlinetowrite = Ny;
            linestowrite = inysize-Ny;
            rollback = 0;
        }
        else
        {
            firstlinetowrite = Ny;
            linestowrite = inysize-2*Ny;
            rollback = 0;
        }

        //Init to zeros
        cpxdata1.zeros();
        cpxdata2.zeros();
        wts.zeros();

        //Read in the data
        status = inDataset->GetRasterBand(band1)->RasterIO( GF_Read,
                    0, yoff,
                    cols, inysize,
                    (void*) (cpxdata1.memptr()),
                    cols, inysize, GDT_CFloat32,
                    sizeof(std::complex<float>),
                    sizeof(std::complex<float>)*cols, NULL);

        if (status != 0)
        {
            std::cout << "Error reading data from band " << band1 << " at line "
                    << yoff << "\n";
            std::cout << "Exiting with error code .... (108) \n";
            GDALClose(inDataset);
            GDALClose(wtsDataset);
            GDALClose(outDataset);
            GDALDestroyDriverManager();
            return 108;
        }

        if (band2 > 0)
        {
            status = inDataset->GetRasterBand(band2)->RasterIO( GF_Read,
                        0, yoff,
                        cols, inysize,
                        (void*) (cpxdata2.memptr()),
                        cols, inysize, GDT_CFloat32,
                        sizeof(std::complex<float>),
                        sizeof(std::complex<float>)*cols, NULL);

            if (status != 0)
            {
                std::cout << "Error reading data from band " << band2 << " at line "
                    << yoff << "\n";
                std::cout << "Exiting with error code .... (108) \n";
                GDALClose(inDataset);
                GDALClose(wtsDataset);
                GDALClose(outDataset);
                GDALDestroyDriverManager();
                return 108;
            }

            //If coherence is desired instead of amplitude
            if (opts->computeCoherence)
            {

    #pragma omp parallel for\
        default(shared)
                for(int jj=0; jj< (inysize*cols); jj++)
                {
                    float amp1 = std::abs(cpxdata1(jj));
                    float amp2 = std::abs(cpxdata2(jj));
                    cpxdata1(jj) *= std::conj( cpxdata2(jj));
                    cpxdata2(jj) = std::complex<float>(amp1*amp1, amp2*amp2);
                }
            }
            else
            {

    #pragma omp parallel for\
        default(shared)
                for(int jj=0; jj< (inysize*cols); jj++)
                {
                    cpxdata1(jj) *= std::conj(cpxdata2(jj));
                    cpxdata2(jj) = 1.0;
                }
            }
        }
        else
        {

        //If only a single band is provided, changing the data to abs values
    #pragma omp parallel for\
            default(shared)
            for(int jj=0; jj< (inysize*cols); jj++)
            {
                cpxdata1(jj) = std::abs(cpxdata1(jj));
            }
            cpxdata2.ones();
        }

        filt.zeros();

        //Read in the wts
        status = wtsDataset->RasterIO( GF_Read,
                    0, yoff,
                    cols, inysize,
                    (void*) (wts.memptr()),
                    cols, inysize, GDT_UInt32,
                    nulong, NULL,
                    4 * nulong, 4 * nulong * cols,
                    4, NULL);

        if (status != 0)
        {
            std::cout << "Error reading weights band at line "<< yoff << "\n";
            std::cout << "Exiting with error code .... (108) \n";
            GDALClose(inDataset);
            GDALClose(outDataset);
            GDALClose(wtsDataset);
            GDALDestroyDriverManager();
            return 108;
        }

    #pragma omp parallel for\
        default(shared)
        for(int pp=(firstlinetowrite*cols); pp< ((firstlinetowrite+linestowrite)*cols); pp++)
        {
            unsigned int *cenptr = wts.colptr(pp);
            if (bitmask.getbit(cenptr, 0, 0) == 0) continue;

            int cenii = pp/cols;
            int cenjj = pp%cols;

            int xmin = std::max(cenjj-Nx, 0);
            int xmax = std::min(cols-1, cenjj+Nx);

            int ymin = std::max(cenii-Ny, 0);
            int ymax = std::min(inysize-1, cenii+Ny);

            std::complex<float> val = 0.0;
            std::complex<float> sumw = 0.0;

            for(int ii=ymin; ii<=ymax; ii++)
            {
                for(int jj=xmin; jj<=xmax; jj++)
                {
                    if (bitmask.getbit(cenptr, ii-cenii, jj-cenjj)!=0)
                    {
                        val += cpxdata1(ii*cols + jj);
                        sumw += cpxdata2(ii*cols + jj);
                    }
                }
            }

            //Summing and normalization
            if (sumw.real() > 0)
            {
                if (opts->computeCoherence)
                {
                    if (sumw.imag() > 0)
                    {
                        filt(pp) = val / (std::sqrt(sumw.real()) * std::sqrt(sumw.imag()));
                    }
                }
                else
                {
                    filt(pp) = val / sumw.real();
                }
            }
        }

        //Print debugs
/*        std::cout << "Block number: " << blockcount << "\n";
        std::cout << "yoff: " << yoff << "\n";
        std::cout << "firstlinetowrite: " << firstlinetowrite << "\n";
        std::cout << "linestowrite: " << linestowrite << "\n";
        std::cout << "rollback: " << rollback << "\n";

        std::cout << "Gdal block \n";
        std::cout << "First line : " << yoff + firstlinetowrite << "\n";
        std::cout << "Number lines: " << linestowrite << "\n";
        std::cout << "Next read start: " << yoff + linestowrite - rollback << "\n"; */

        //Write stuff to output files

        //Write to count file
        status = outDataset->GetRasterBand(1)->RasterIO(GF_Write, 0, yoff+firstlinetowrite,
                    cols, linestowrite,
                    (void*) (&filt[firstlinetowrite*cols]),
                    cols, linestowrite, outType,
                    sizeof(std::complex<float>), sizeof(std::complex<float>)*cols, NULL);

        if (status != 0)
        {
            std::cout << "Error writing despeck data at line " << yoff << "\n";
            std::cout << "Exiting with error code .... (110) \n";
            GDALClose(inDataset);
            GDALClose(outDataset);
            GDALClose(wtsDataset);
            GDALDestroyDriverManager();
            return 110;
        }

//        std::cout << "Weights at (10,10): " << count(10,10) << " "
//                  << wts(3,10*cols+10)<< "\n";
//        bitmask.print( wts.colptr(10*cols + 10));

        if ((yoff+blockysize) < rows)
        {
            yoff += linestowrite - rollback;
        }
        else
        {
            yoff=rows;
        }

        status = GDALTermProgress( yoff/(1.0*rows), NULL, NULL);
    }

    t_end = getWallTime();

    std::cout << "despeckamp processing time: " << (t_end-t_start)/60.0 << " mins \n";

    //Close the datasets
    GDALClose(inDataset);
    GDALClose(outDataset);
    GDALClose(wtsDataset);

    return(0);

};

/* Main driver*/
int main(int  argc, const char *argv[] ) {

    //Options
    despeckOptions opts;
    int status;

    //Parse command line options
    status = opts.initFromCmdLine(argc, argv);

    if (status != 0)
    {
        std::cout << "Error parsing command line inputs \n";
        std::cout << "Exiting with non-zero return code .... \n";
        return (101);
    }

    //Execute
    status = despeck_process(&opts);

    if (status != 0)
    {
        std::cout << "Error processing calamp \n";
        std::cout << "Exiting with non-zero return code ....\n";
        return (status);
    }

    return(0);

};


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<float, 3, 4> test_float_3_4(const Eigen::Matrix<float, 3, 4> & M)
{
	return M;
}
void export_float_3_4()
{
	boost::python::def("test_float_3_4",test_float_3_4);
}


#define BOOST_TEST_MODULE Tests
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>

#include "../src/Board.hpp"
#include "../src/Hashing.hpp"
#include "../src/Solver.hpp"
#include "../src/TileType.hpp"

using namespace WayoutPlayer;

BOOST_AUTO_TEST_CASE(hashingTheEmptyStringTest) {
  BOOST_CHECK(0xcf83e1357eefb8bdUL == hashString(""));
}

BOOST_AUTO_TEST_CASE(hashingDigitsTest) {
  BOOST_CHECK(0xbb96c2fc40d2d546UL == hashString("0123456789"));
}

BOOST_AUTO_TEST_CASE(tileTypeConversionsToCharacters) {
  for (const auto tileType : TileTypes) {
    BOOST_CHECK(tileType == tileTypeFromCharacter(tileTypeToCharacter(tileType)));
  }
}

BOOST_AUTO_TEST_CASE(tileTypeConversionsToIntegers) {
  for (const auto tileType : TileTypes) {
    BOOST_CHECK(tileType == tileTypeFromInteger(tileTypeToInteger(tileType)));
  }
}

BOOST_AUTO_TEST_CASE(boardIsSolvedWorksOnSolvedBoards) {
  BOOST_CHECK(Board::fromString("D0").isSolved());
}

BOOST_AUTO_TEST_CASE(boardIsSolvedWorksOnUnsolvedBoards) {
  BOOST_CHECK(!Board::fromString("D1").isSolved());
  BOOST_CHECK(!Board::fromString("B0").isSolved());
  BOOST_CHECK(!Board::fromString("B1").isSolved());
}

BOOST_AUTO_TEST_CASE(boardSplittingShouldWorkWithDefaultTiles) {
  const auto connectedBoardString = "D0 D0\n"
                                    "   D0";
  BOOST_CHECK(Board::fromString(connectedBoardString).splitComponents().size() == 1);
  const auto disconnectedBoardString = "D0   \n"
                                       "   D0";
  BOOST_CHECK(Board::fromString(disconnectedBoardString).splitComponents().size() == 2);
}

BOOST_AUTO_TEST_CASE(boardSplittingShouldWorkWithTwins) {
  const auto boardString = "P0   \n"
                           "   P0";
  BOOST_CHECK(Board::fromString(boardString).splitComponents().size() == 1);
}

BOOST_AUTO_TEST_CASE(boardConversionsTest) {
  const auto boardString = "D0 V1 D0\n"
                           "D1 D1 D1\n"
                           "D0 D1 D0\n"
                           "T1 H1 T1";
  const auto board = Board::fromString(boardString);
  BOOST_CHECK(board.toString() == boardString);
}

BOOST_AUTO_TEST_CASE(boardConversionsTestWithGaps) {
  const auto boardString = "D0    D0\n"
                           "D1 D1 D1\n"
                           "   D1   \n"
                           "T1 H1 T1";
  const auto board = Board::fromString(boardString);
  BOOST_CHECK(board.toString() == boardString);
}

BOOST_AUTO_TEST_CASE(activatingAChainTest) {
  const auto boardString = "C1 D0";
  auto board = Board::fromString(boardString);
  board.activate(0, 0);
  BOOST_CHECK(board.toString() == "C0 D1");
}

BOOST_AUTO_TEST_CASE(boardSolutionTest) {
  const auto boardString = "D0 D1 D0\n"
                           "D1 D1 D1\n"
                           "D0 D1 D0";
  const auto solver = Solver();
  const auto boardSolution = solver.findSolution(Board::fromString(boardString));
  const auto expectedSolution = Solution({Position{1, 1}}, true);
  BOOST_CHECK(boardSolution == expectedSolution);
}

BOOST_AUTO_TEST_CASE(boardSolutionTestWithTaps) {
  const auto boardString = "D0 D1 D0\n"
                           "D1 T1 D1\n"
                           "D0 D1 D0";
  const auto solver = Solver();
  const auto boardSolution = solver.findSolution(Board::fromString(boardString));
  const auto expectedSolution = Solution({Position{1, 1}}, true);
  BOOST_CHECK(boardSolution == expectedSolution);
}

BOOST_AUTO_TEST_CASE(boardsShouldBeDifferentIfTheirTilesAreNotBlocked) {
  const auto boardAString = "B1";
  const auto boardBString = "D1";
  const auto boardA = Board::fromString(boardAString);
  const auto boardB = Board::fromString(boardBString);
  BOOST_CHECK(boardA != boardB);
}

BOOST_AUTO_TEST_CASE(largeDefaultBoardShouldBeSolved) {
  const auto boardString = "D1 D0 D1 D0 D1\n"
                           "D0 D1 D1 D0 D0\n"
                           "D1 D0 D1 D1 D1\n"
                           "D1 D0 D0 D0 D1\n"
                           "D0 D1 D0 D0 D0";
  const auto solver = Solver();
  const auto boardSolution = solver.findSolution(Board::fromString(boardString));
  BOOST_CHECK(boardSolution.getClicks().size() == 8);
}

BOOST_AUTO_TEST_CASE(boardWithTwinsShouldBeSolved) {
  const auto boardString = "D0 D1 D0 D0 D0\n"
                           "D0 P1 D1 D0 D0\n"
                           "D0 D0 D0 D1 D0\n"
                           "D1 D1 D0 D1 P0\n"
                           "P0 D0 D1 D0 D0";
  const auto solver = Solver();
  const auto boardSolution = solver.findSolution(Board::fromString(boardString));
  BOOST_CHECK(boardSolution.getClicks().size() == 6);
}

BOOST_AUTO_TEST_CASE(activatingDownNeighborTwinsShouldBehaveAsInTheGame) {
  const auto boardString = "D0 D0         \n"
                           "P1    D0 D1 D0\n"
                           "D0    P1 P0 P1\n"
                           "P1    D0 D1 D0\n"
                           "D1 D0         ";
  auto board = Board::fromString(boardString);
  board.activate(2, 3);
  const auto expectedFinalBoardString = "D0 D0         \n"
                                        "P1    D0 D0 D0\n"
                                        "D0    P1 P1 P1\n"
                                        "P1    D0 D0 D0\n"
                                        "D1 D0         ";
  BOOST_CHECK(board == Board::fromString(expectedFinalBoardString));
}

BOOST_AUTO_TEST_CASE(activatingUpNeighborTwinsShouldBehaveAsInTheGame) {
  const auto boardString = "D0 D0         \n"
                           "P1    D0 D1 D0\n"
                           "D0    P1 P0 P1\n"
                           "P1    D0 D1 D0\n"
                           "D1 D0         ";
  auto board = Board::fromString(boardString);
  board.activate(3, 0);
  const auto expectedFinalBoardString = "D0 D0         \n"
                                        "P0    D0 D1 D0\n"
                                        "D1    P0 P0 P0\n"
                                        "P0    D0 D1 D0\n"
                                        "D0 D0         ";
  BOOST_CHECK(board == Board::fromString(expectedFinalBoardString));
}

BOOST_AUTO_TEST_CASE(solvingABoardWithNeighboringTwinsShouldBehaveAsInTheGame) {
  const auto boardString = "D0 D0         \n"
                           "P1    D0 D1 D0\n"
                           "D0    P1 P0 P1\n"
                           "P1    D0 D1 D0\n"
                           "D1 D0         ";
  auto board = Board::fromString(boardString);
  board.activate(3, 0);
  board.activate(2, 3);
  board.activate(2, 0);
  const auto expectedFinalBoardString = "D0 D0         \n"
                                        "P0    D0 D0 D0\n"
                                        "D0    P0 P0 P0\n"
                                        "P0    D0 D0 D0\n"
                                        "D0 D0         ";
  BOOST_CHECK(board == Board::fromString(expectedFinalBoardString));
}

BOOST_AUTO_TEST_CASE(positionsShouldBecomeSensibleStrings) {
  BOOST_CHECK(Position(-1, -1).toString() == "(-1, -1)");
  BOOST_CHECK(Position(0, 0).toString() == "(0, 0)");
  BOOST_CHECK(Position(127, 127).toString() == "(127, 127)");
}

BOOST_AUTO_TEST_CASE(splittingComponentsShouldJudgeTheNeedForMultipleClicks) {
  const auto boardString = "D0    B1 D0\n"
                           "D1    D0 B1";
  const auto board = Board::fromString(boardString);
  BOOST_CHECK(board.splitComponents().size() == 2);
  auto requiringMultipleClicks = 0;
  auto notRequiringMultipleClicks = 0;
  for (const auto &component : board.splitComponents()) {
    if (component.mayNeedMultipleClicks()) {
      requiringMultipleClicks++;
    } else {
      notRequiringMultipleClicks++;
    }
  }
  BOOST_CHECK(requiringMultipleClicks == 1);
  BOOST_CHECK(notRequiringMultipleClicks == 1);
}

BOOST_AUTO_TEST_CASE(shouldUnderstandThatABlockedTilesMayNeedMultipleClicks) {
  const auto boardString = "B1 D0\n"
                           "D0 B1";
  const auto board = Board::fromString(boardString);
  BOOST_CHECK(board.mayNeedMultipleClicks());
}

BOOST_AUTO_TEST_CASE(boardWithBlockedTilesShouldBehaveAsInTheGame) {
  const auto boardString = "B1 D0\n"
                           "D0 B1";
  auto board = Board::fromString(boardString);
  board.activate(0, 1);
  BOOST_CHECK(board.toString() == "D1 D1\nD0 D1");
  board.activate(0, 1);
  BOOST_CHECK(board.toString() == "D0 D0\nD0 D0");
}

BOOST_AUTO_TEST_CASE(shouldSolveSmallBoardWithBlockedTiles) {
  const auto boardString = "B1 D0\n"
                           "D0 B1";
  const auto board = Board::fromString(boardString);
  BOOST_CHECK(Solver().findSolution(board).getClicks().size() == 2);
}

BOOST_AUTO_TEST_CASE(shouldFindComponentsInASmallBoardWithBlockedTiles) {
  const auto boardString = "B1 D0\n"
                           "D0 B1";
  const auto board = Board::fromString(boardString);
  const auto components = board.splitComponents();
  BOOST_CHECK(components.size() == 1);
}

BOOST_AUTO_TEST_CASE(splittingComponentsShouldWork) {
  const auto boardString = "D0   \n"
                           "   D1";
  const auto board = Board::fromString(boardString);
  const auto components = board.splitComponents();
  BOOST_CHECK(components.size() == 2);
  BOOST_CHECK(components[0] != board);
  BOOST_CHECK(components[1] != board);
  BOOST_CHECK(Board::mergeComponents(components) == board);
}

BOOST_AUTO_TEST_CASE(splittingComponentsShouldRespectTwins) {
  const auto boardString = "D0 D0         \n"
                           "P1    D0 D1 D0\n"
                           "D0    P1 P0 P1\n"
                           "P1    D0 D1 D0\n"
                           "D1 D0         ";
  const auto board = Board::fromString(boardString);
  const auto components = board.splitComponents();
  BOOST_CHECK(components.size() == 1);
  BOOST_CHECK(components.front() == board);
  BOOST_CHECK(Board::mergeComponents(components) == board);
}


#include "scene_loader.hpp"
#include "spatial/2d/convex_hull.hpp"
#include <boost/log/trivial.hpp>
#include <boost/filesystem.hpp>
#include "rendering/2d/disk_fluid.hpp"
#include <Eigen/Geometry>

/**
 * Loads a scene from a file <scenename>.json located in the scenes-folder.
 * Inserts the objects from the scene directly into the Box2D physics scene and the RenderScene.
 *
 * The scene coinsists of objects made of triangles and faces. Each object, as well as the scene,
 * can have names. These are currently only here for debug reasons.
 *
 * The scene object has a scale and a translation. The scaling is applied around (0, 0) before the
 * whole scene is translated by the translation vector.
 *
 * Currently all scene objects are just enclosed in an axis-aligned bounding box going from (min_x, min_y)
 * to (max_x, max_y) where these are the minimal, resp. maximal x and y coordinates of all vertices in the
 * object's list of vertices.
 * Later on, we shall implement convex hulls instead.
 *
 * The triangles of each object are currently colored grey (RGB = (0.5, 0.5, 0.5)).
 * Further color specifications may be implemented if deemed necessary.
 *
 * Authored: vramp, 19.11.18 (vramp@student.ethz.ch)
 */

namespace GooBalls {

using namespace d2;

bool showDebugOutput = true;

bool SceneLoader::loadScene(Physics::Scene& physScene, Render::Scene& renderScene, std::string pathStr) {

    std::ifstream ifs(pathStr);

    if (!ifs.is_open()) {
        BOOST_LOG_TRIVIAL(error) << "Error on opening scene file: " + pathStr;
        return false;
    }
    auto path = boost::filesystem::canonical(pathStr);
    Json::Reader reader;
    Json::Value scene;
    reader.parse(ifs, scene);
    BOOST_LOG_TRIVIAL(debug) << "Loading Scene: " << scene["name"].asString() << std::endl;
    if(scene.isMember("includes")){
        const auto& includes = scene["includes"];
        for(unsigned int i = 0; i < includes.size(); ++i){
            auto includePath = boost::filesystem::path(includes[i].asString());
            if(includePath.is_absolute()){
                loadScene(physScene, renderScene, includePath.string());
            } else {
                auto thisDir = path.parent_path();
                loadScene(physScene, renderScene, (thisDir / includePath).string());
            }
        }
    }
    scale = 1.0;
    transl_x = 0.0;
    transl_y = 0.0;
    if(scene.isMember("scale")){
        scale = scene["scale"].asDouble();
    }
    if(scene.isMember("translation")){
        transl_x = scene["translation"][0].asDouble();
        transl_y = scene["translation"][1].asDouble();
    }

    // iterate through the scene's objects
    //int objectCount = obj["objects"]["count"].asInt();
    for (unsigned int objIndex = 0; objIndex < scene["objects"].size(); objIndex++) {
        const auto& obj = scene["objects"][objIndex];
        readObject(physScene, renderScene, obj);
    }

    if(scene.isMember("fluid")){
        readFluid(physScene, renderScene, scene["fluid"]);
    } else {
        setEmptyFluid(physScene, renderScene);
    }
    if(scene.isMember("room")){
        readRoom(physScene, renderScene, scene["room"]);
    }
    if(scene.isMember("gravity")){
        physScene.gravity[0] = scene["gravity"][0].asDouble();
        physScene.gravity[1] = scene["gravity"][1].asDouble();
    }
    return true;
}

void SceneLoader::readRoom(Physics::Scene& physScene, Render::Scene&, const Json::Value& room) const {
    if(room.isMember("floor")){
        physScene.room.floor = room["floor"].asDouble()*scale + transl_y;
    }
    if(room.isMember("ceiling")){
        physScene.room.ceiling = room["ceiling"].asDouble()*scale + transl_y;
    }
    if(room.isMember("leftWall")){
        physScene.room.left_wall = room["leftWall"].asDouble()*scale + transl_x;
    }
    if(room.isMember("rightWall")){
        physScene.room.right_wall = room["rightWall"].asDouble()*scale + transl_x;
    }
}

Coordinates2d SceneLoader::createGrid(int px, int py, Float gridSpacing) const {
    Coordinates2d coords;
    coords.setZero(px*py, 2);
    for(int i = 0; i < px; ++i){
        for(int j = 0; j < py; ++j){
            coords(i*py + j, 0) = i;
            coords(i*py + j, 1) = j;
        }
    }
    coords = gridSpacing * coords;
    return coords;
}

Coordinates2d SceneLoader::readGrid(const Json::Value& grid) const {
    assert(grid["type"].asString() == "grid");
    int px = grid["gridWidth"].asInt();
    int py = grid["gridHeight"].asInt();
    Float distance = grid["gridSpacing"].asDouble();
    return createGrid(px, py, distance);
}

void SceneLoader::setEmptyFluid(Physics::Scene& physScene, Render::Scene& renderScene) const {
    auto particleCoordinates = std::make_shared<Coordinates2d>();
    auto boundaryCoords = std::make_shared<Coordinates2d>();
    auto fluidPhys = std::make_unique<Physics::Fluid>(particleCoordinates, boundaryCoords);
    auto fluidRender = std::make_unique<Render::DiskFluid>(particleCoordinates, boundaryCoords);

    physScene.fluid = std::move(fluidPhys);
    renderScene.fluids.resize(1);
    renderScene.fluids[0] = std::move(fluidRender);
}

void SceneLoader::readSubFluid(Physics::Scene& physScene, Render::Scene& renderScene, const Json::Value& fluid) const {
    auto& fluidPhys = *physScene.fluid;
    auto& fluidRender = *renderScene.fluids[0];
    auto& particles = fluidPhys.particles_position();
    const auto& parts = fluid["particles"];
    Coordinates2d newCoords;
    if(parts["type"].asString() == "grid"){
        newCoords = readGrid(parts);

    } else if(parts["type"].asString() == "coordinates") {
        newCoords = readCoordinates(parts["positions"]);
    }

    Float pos_x = 0;
    Float pos_y = 0;
    if(fluid.isMember("position")){
        pos_x = fluid["position"][0].asDouble();
        pos_y = fluid["position"][1].asDouble();
    }
    newCoords.array() *= scale;
    newCoords.col(0).array() += transl_x + pos_x*scale;
    newCoords.col(1).array() += transl_y + pos_y*scale;
    const int ps = particles.rows();
    const int pn = newCoords.rows();
    const int pa = ps+pn;
    particles.conservativeResize(pa, Eigen::NoChange);
    particles.block(ps, 0, pn, 2) = newCoords;
    // physics data
    fluidPhys.particles_velocity().conservativeResize(pa, Eigen::NoChange);
    auto vs = fluidPhys.particles_velocity().block(ps, 0, pn, 2);
    vs.setZero();
    if(fluid.isMember("initialVelocity")){
        const auto& vinit = fluid["initialVelocity"];
        if(vinit[0].isDouble()){
            vs.col(0).array() = scale * vinit[0].asDouble();
            vs.col(1).array() = scale * vinit[1].asDouble();
        } else {
            Coordinates2d v0 = readCoordinates(fluid["initialVelocity"]);
            vs.array() = scale*v0;
        }
    }
    // TODO: compute the mass from rho*V = m
    // TODO: check all known constraint equations (CLF)
    // TOOD: make sanity checks
    fluidPhys.particles_mass().conservativeResize(pa, Eigen::NoChange);
    auto ms = fluidPhys.particles_mass().block(ps, 0, pn, 1);
    ms.setOnes();
    if(fluid.isMember("mass"))
        ms.array() = fluid["mass"].asDouble();
    assert(ms.minCoeff() > 0.0);
    fluidPhys.particles_velocity_correction().conservativeResize(pa, Eigen::NoChange);
    auto cs = fluidPhys.particles_velocity_correction().block(ps, 0, pn, 1);
    if(fluid.isMember("velocityCorrectionCoefficient"))
        cs.array() = fluid["velocityCorrectionCoefficient"].asDouble();

    // Render properties
    fluidRender.particles_color().conservativeResize(pa, 3);
    auto col = fluidRender.particles_color().block(ps, 0, pn, 3);
    if(fluid.isMember("color")){
        col.col(0).array() = fluid["color"][0].asDouble();
        col.col(1).array() = fluid["color"][1].asDouble();
        col.col(2).array() = fluid["color"][2].asDouble();
    }
    fluidRender.particles_radius().conservativeResize(pa, 1);
    auto rad = fluidRender.particles_radius().block(ps, 0, pn, 1);
    rad.setOnes();
    if(fluid.isMember("radius")){
        rad.array() = scale * fluid["radius"].asDouble();
    }
}

void SceneLoader::readFluid(Physics::Scene& physScene, Render::Scene& renderScene, const Json::Value& fluid) const {
    if(physScene.fluid.get() == nullptr){
        setEmptyFluid(physScene, renderScene);
    }
    if(fluid.isMember("fluids")){
        for(auto& subFluid : fluid["fluids"]){
            readSubFluid(physScene, renderScene, subFluid);
        }
    } else {
        readSubFluid(physScene, renderScene, fluid);
    }
    auto& fluidPhys = *physScene.fluid;
    Float h = fluid["kernelRadius"].asDouble() * scale;
    fluidPhys.h(h);
    BOOST_LOG_TRIVIAL(debug) << "Loader: kernelRadius: " << h;

    // create physics data

    if(fluid.isMember("stiffnessConstant"))
        fluidPhys.stiffnessConstant(fluid["stiffnessConstant"].asDouble());
    if(fluid.isMember("restDensity"))
        fluidPhys.rest_density(fluid["restDensity"].asDouble());
    if(fluid.isMember("surfaceTension"))
        fluidPhys.surface_tension(fluid["surfaceTension"].asDouble());
    if(fluid.isMember("fluidViscosity"))
        fluidPhys.fluid_viscosity(fluid["fluidViscosity"].asDouble());
    if(fluid.isMember("boundaryViscosity"))
        fluidPhys.boundary_viscosity(fluid["boundaryViscosity"].asDouble());
    if(fluid.isMember("pressureGamma"))
        fluidPhys.pressure_gamma(fluid["pressureGamma"].asDouble());

    if(fluid.isMember("velocityCorrectionCoefficientMin"))
        fluidPhys.particles_lower_velocity_correction_limit() = fluid["velocityCorrectionCoefficientMin"].asDouble();
    if(fluid.isMember("weakeningSpeed"))
        fluidPhys.particles_weakening_speed() = fluid["weakeningSpeed"].asDouble();
    if(fluid.isMember("yieldCriterion"))
        fluidPhys.particles_yield_criterion() = fluid["yieldCriterion"].asDouble();
    if(fluid.isMember("mergeThreshold"))
        fluidPhys.merge_threshold() = fluid["mergeThreshold"].asDouble();
    if(fluid.isMember("splitThreshold"))
        fluidPhys.split_threshold() = fluid["splitThreshold"].asDouble();
    if(fluid.isMember("boundaryMergeThreshold"))
        fluidPhys.boundary_merge_threshold() = fluid["boundaryMergeThreshold"].asDouble();
    if(fluid.isMember("continuousMerge"))
        fluidPhys.continuous_merge(fluid["continuousMerge"].asBool());
    if(fluid.isMember("continuousSplit"))
        fluidPhys.continuous_split(fluid["continuousSplit"].asBool());
}

Coordinates2d SceneLoader::readCoordinates(const Json::Value& coords) const {
    Coordinates2d vertices(coords.size(), 2);
    for (unsigned int i = 0; i < coords.size(); i++) {
        vertices(i, 0) = coords[i][0].asDouble();
        vertices(i, 1) = coords[i][1].asDouble();
    }
    return vertices;
}

void SceneLoader::readObject(Physics::Scene& physScene, Render::Scene& renderScene, const Json::Value& obj) const {
    const auto& vertex = obj["vertices"];
    int faceCount = obj["faces"].size();

    BOOST_LOG_TRIVIAL(debug) << "OBJECT: " << obj["name"] << std::endl << "-----------";

    // iterate through the object's vertices
    Coordinates2d vertices = readCoordinates(vertex);
    if(obj.isMember("rotation")){
        Float degreeAngle = obj["rotation"].asDouble();
        Eigen::Rotation2D<Float> rot(degreeAngle/180*M_PI);
        RotationMatrix rotM = rot.toRotationMatrix();
        vertices = vertices * rotM.transpose();
    }
    vertices = vertices*scale;
    vertices.col(0) = vertices.col(0).array() + transl_x;
    vertices.col(1) = vertices.col(1).array() + transl_y;
    // iterate through the object's faces
    TriangleList faces(faceCount, 3);
    for (int i = 0; i < faceCount; i++) {
        faces(i, 0) = obj["faces"][i][0].asInt();
        faces(i, 1) = obj["faces"][i][1].asInt();
        faces(i, 2) = obj["faces"][i][2].asInt();
    }

    BOOST_LOG_TRIVIAL(debug) << "Vertices:" << std::endl << vertices;
    BOOST_LOG_TRIVIAL(debug) << "Faces:" << std::endl << faces;

    if(m_convertMeshToBoundingBox){
        convertMeshToBoundingBox(vertices, faces);
    }

    // create a PhysicsMesh object for Box2D
    auto vertex_ptr = std::make_shared<Coordinates2d>(vertices);
    auto face_ptr = std::make_shared<TriangleList>(faces);
    Physics::Mesh physMesh(vertex_ptr, face_ptr);

    // create a body for box2d
    b2BodyDef bodyDefinition;
    //bodyDefinition.position.Set(min_x + half_width, min_y + half_width); // the center of the body
    if(obj.isMember("position")){
        bodyDefinition.position.x = scale*obj["position"][0].asDouble();
        bodyDefinition.position.y = scale*obj["position"][1].asDouble();
        BOOST_LOG_TRIVIAL(debug) << "position: " << bodyDefinition.position.x << ", " << bodyDefinition.position.y;
    }
    if(obj.isMember("initialVelocity")){
        bodyDefinition.linearVelocity.x = obj["initialVelocity"][0].asDouble();
        bodyDefinition.linearVelocity.y = obj["initialVelocity"][1].asDouble();
        BOOST_LOG_TRIVIAL(debug) << "velocity: " << bodyDefinition.linearVelocity.x << ", " << bodyDefinition.linearVelocity.y;
    }
    if(obj.isMember("acceleration")){
        // only for non-dynamic objects
        physMesh.linearAcceleration.x = obj["acceleration"][0].asDouble();
        physMesh.linearAcceleration.y = obj["acceleration"][1].asDouble();
        BOOST_LOG_TRIVIAL(debug) << "acceleration: " << physMesh.linearAcceleration.x << ", " << physMesh.linearAcceleration.y;
    }
    if(obj.isMember("rotation")){
        //bodyDefinition.angle = obj["rotation"].asDouble()/180*M_PI;
    }
    if(obj.isMember("angularVelocity")){
        //bodyDefinition.angularVelocity = obj["angularVelocity"].asDouble();
    }
    if(obj.isMember("angularAcceleration")){
        //physMesh.angularAcceleration = obj["angularAcceleration"].asDouble();
    }
    if(obj.isMember("dynamic")){
        BOOST_LOG_TRIVIAL(warning) << "Found deprecated property 'dynamic', please use 'type' = 'dynamic'. Ignoring property.";
    }
    if(obj.isMember("type") && obj["type"].asString() == "dynamic"){
        bodyDefinition.type = b2_dynamicBody;
    } else if (obj.isMember("type") && obj["type"].asString() == "kinematic") {
        bodyDefinition.type = b2_kinematicBody;
    } else {
        bodyDefinition.type = b2_staticBody;
    }
    physMesh.body = physScene.world.CreateBody(&bodyDefinition); // attach the body to the mesh

    // create bounding volume
    b2PolygonShape boundingBox;
    auto b2Verts = computeConvexHull(vertices);
    boundingBox.Set(b2Verts.data(), b2Verts.size());
    Float objDensity = 1000.0f;
    if(obj.isMember("density")){
        objDensity = obj["density"].asDouble();
    }
    physMesh.body->CreateFixture(&boundingBox, objDensity); // attach the bounding box to the body

    if(obj.isMember("velocityCorrectionCoefficient")){
        physMesh.particles_velocity_correction_coefficient() = obj["velocityCorrectionCoefficient"].asDouble();
    }

    // insert the box2d mesh into the scene (including body and bounding box)
    physScene.meshes.push_back(std::move(physMesh));

    // insert the mesh into the scene's rendering
    auto mesh = std::make_unique<Render::Mesh>(vertex_ptr, face_ptr);
    mesh->vertices_color() = ColorsFloatRGB::Zero(vertex.size(), 3);
    mesh->vertices_color().array() += 0.5; // grey
    renderScene.meshes.push_back(std::move(mesh));
}

std::vector<b2Vec2> SceneLoader::computeConvexHull(const Coordinates2d& vertices) const{
    auto convexHull = Spatial::convex_hull(vertices);
    std::vector<b2Vec2> b2Verts(convexHull.rows());
    for(int i = 0; i < convexHull.rows(); ++i) {
        b2Verts[i].x = convexHull(i, 0);
        b2Verts[i].y = convexHull(i, 1);
    }
    return b2Verts;
}

std::vector<b2Vec2> SceneLoader::computeAABB(const Coordinates2d& vertices) const {
    double min_x, min_y, max_x, max_y;
    Coordinates2d maxCoords = vertices.colwise().maxCoeff();
    Coordinates2d minCoords = vertices.colwise().minCoeff();
    min_x = minCoords(0, 0);
    min_y = minCoords(0, 1);
    max_x = maxCoords(0, 0);
    max_y = maxCoords(0, 1);
    BOOST_LOG_TRIVIAL(debug) << "Bounding Box at (" << min_x << ", " << min_y << "), (" << max_x << ", " << max_y << ")";
    std::vector<b2Vec2> b2Verts(4);
    b2Verts[0].x = min_x;
    b2Verts[0].y = min_y;
    b2Verts[1].x = max_x;
    b2Verts[1].y = min_y;
    b2Verts[2].x = max_x;
    b2Verts[2].y = max_y;
    b2Verts[3].x = min_x;
    b2Verts[3].y = max_y;
    return b2Verts;
}

void SceneLoader::convertMeshToBoundingBox(Coordinates2d vertices, TriangleList& faces) const {
    double min_x, min_y, max_x, max_y;
    Coordinates2d maxCoords = vertices.colwise().maxCoeff();
    Coordinates2d minCoords = vertices.colwise().minCoeff();
    min_x = minCoords(0, 0);
    min_y = minCoords(0, 1);
    max_x = maxCoords(0, 0);
    max_y = maxCoords(0, 1);

    vertices.resize(4,2);
    vertices(0,0) = min_x;
    vertices(0,1) = min_y;
    vertices(1,0) = max_x;
    vertices(1,1) = min_y;
    vertices(2,0) = max_x;
    vertices(2,1) = max_y;
    vertices(3,0) = min_x;
    vertices(3,1) = max_y;
    faces.resize(2,3);
    faces(0,0) = 0;
    faces(0,1) = 1;
    faces(0,2) = 2;
    faces(1,0) = 2;
    faces(1,1) = 3;
    faces(1,2) = 0;
}

} // GooBalls


// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff/
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//
// Contributors: Tobias Leibner

#ifndef DUNE_STUFF_LA_CONTAINER_VECTOR_INTERFACE_HH
#define DUNE_STUFF_LA_CONTAINER_VECTOR_INTERFACE_HH

#include <cmath>
#include <limits>
#include <iostream>
#include <vector>
#include <complex>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/ftraits.hh>

#include <dune/stuff/common/crtp.hh>
#include <dune/stuff/common/exceptions.hh>
#include <dune/stuff/common/float_cmp.hh>
#include <dune/stuff/common/type_utils.hh>
#include <dune/stuff/common/vector.hh>
#include <dune/stuff/common/math.hh>

#include "container-interface.hh"
#include "vector-interface-internal.hh"

namespace Dune {
namespace Stuff {
namespace LA {

/**
 *  \brief  Contains tags mostly needed for python bindings.
 */
namespace Tags {

class VectorInterface
{
};

} // namespace Tags

template <class Traits, class ScalarImp = typename Traits::ScalarType>
class VectorInterface : public ContainerInterface<Traits, ScalarImp>, public Tags::VectorInterface
{
public:
  typedef typename Traits::derived_type derived_type;
  typedef typename Dune::FieldTraits<ScalarImp>::field_type ScalarType;
  typedef typename Dune::FieldTraits<ScalarImp>::real_type RealType;

  typedef internal::VectorInputIterator<Traits, ScalarType> const_iterator;
  typedef internal::VectorOutputIterator<Traits, ScalarType> iterator;

  static_assert(std::is_same<ScalarType, typename Traits::ScalarType>::value, "");

  virtual ~VectorInterface() {}

  /// \name Have to be implemented by a derived class in addition to the ones required by ContainerInterface!
  /// \{

  /**
   * \brief   The size of the vector.
   * \return  The size of the vector.
   */
  inline size_t size() const
  {
    CHECK_CRTP(this->as_imp().size());
    return this->as_imp().size();
  }

  /**
   * \brief Add a scalar to the iith entry.
   */
  inline void add_to_entry(const size_t ii, const ScalarType& value)
  {
    CHECK_AND_CALL_CRTP(this->as_imp().add_to_entry(ii, value));
  }

  /**
   * \brief Set the iith entry to given scalar.
   */
  inline void set_entry(const size_t ii, const ScalarType& value)
  {
    CHECK_AND_CALL_CRTP(this->as_imp().set_entry(ii, value));
  }

  /**
   * \brief Get the iith entry.
   * \todo  Default implement using get_entry_ref!
   */
  inline ScalarType get_entry(const size_t ii) const
  {
    CHECK_CRTP(this->as_imp().get_entry(ii));
    return this->as_imp().get_entry(ii);
  }

  inline ScalarType& get_entry_ref(const size_t ii)
  {
    CHECK_CRTP(this->as_imp().get_entry_ref(ii));
    return this->as_imp().get_entry_ref(ii);
  }

  inline const ScalarType& get_entry_ref(const size_t ii) const
  {
    CHECK_CRTP(this->as_imp().get_entry_ref(ii));
    return this->as_imp().get_entry_ref(ii);
  }

  /// \}
  /// \name Provided by the interface for convenience!
  /// \note Those marked as virtual may be implemented more efficiently in a derived class!
  /// \{

  virtual void set_all(const ScalarType& val)
  {
    for (auto& element : *this)
      element = val;
  }

  virtual bool valid() const
  {
    for (const auto& val : *this) {
      if (Common::isnan(val) || Common::isinf(val))
        return false;
    }
    return true;
  } // ... valid()

  /**
   * \brief Get writable reference to the iith entry.
   */
  inline ScalarType& operator[](const size_t ii) { return get_entry_ref(ii); }

  /**
   * \brief Get read-only reference to the iith entry.
   */
  inline const ScalarType& operator[](const size_t ii) const { return get_entry_ref(ii); }

  /**
   * \brief   The dimension of the vector.
   * \return  The dimension of the vector.
   * \see     size()
   */
  inline size_t dim() const { return size(); }

  virtual ScalarType mean() const
  {
    ScalarType ret = 0.0;
    for (const auto& element : *this)
      ret += element;
    ret /= size();
    return ret;
  } // ... mean()

  /**
   *  \brief  The maximum absolute value of the vector.
   *  \return A pair of the lowest index at which the maximum is attained and the absolute maximum value.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual std::pair<size_t, RealType> amax() const
  {
    auto result = std::make_pair(size_t(0), RealType(0));
    for (size_t ii = 0; ii < size(); ++ii) {
      const auto value = std::abs(get_entry_ref(ii));
      if (value > result.second) {
        result.first  = ii;
        result.second = value;
      }
    }
    return result;
  } // ... amax(...)

  /**
   *  \brief  Check vectors for equality.
   *          Equality of two vectors is defined as in Dune::FloatCmp componentwise.
   *  \param  other   A vector of same dimension to compare with.
   *  \param  epsilon See Dune::FloatCmp.
   *  \return Truth value of the comparison.
   *  \see    Dune::Stuff::Common::FloatCmp
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual bool
      almost_equal(const derived_type& other,
                   const ScalarType epsilon = Stuff::Common::FloatCmp::DefaultEpsilon<ScalarType>::value()) const
  {
    if (other.size() != size())
      DUNE_THROW(Exceptions::shapes_do_not_match,
                 "The size of other (" << other.size() << ") does not match the size of this (" << size() << ")!");
    return Stuff::Common::FloatCmp::eq(this->as_imp(), other, epsilon);
  } // ... almost_equal(...)

  /**
   *  \brief  Check vectors for equality (variant for arbitrary derived combinations).
   *          Equality of two vectors is defined as in Dune::FloatCmp componentwise.
   *  \param  other   A vector of same dimension to compare with.
   *  \param  epsilon See Dune::FloatCmp.
   *  \return Truth value of the comparison.
   *  \see    Dune::Stuff::Common::FloatCmp
   */
  template <class T>
  bool almost_equal(const VectorInterface<T>& other,
                    const ScalarType epsilon = Stuff::Common::FloatCmp::DefaultEpsilon<ScalarType>::value()) const
  {
    if (other.size() != size())
      DUNE_THROW(Exceptions::shapes_do_not_match,
                 "The size of other (" << other.size() << ") does not match the size of this (" << size() << ")!");
    return Stuff::Common::FloatCmp::eq(this->as_imp(), other.as_imp(), epsilon);
  } // ... almost_equal(...)

  /**
   *  \brief  Computes the scalar products between two vectors.
   *  \param  other The second factor.
   *  \return The scalar product.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual ScalarType dot(const derived_type& other) const
  {
    if (other.size() != size())
      DUNE_THROW(Exceptions::shapes_do_not_match,
                 "The size of other (" << other.size() << ") does not match the size of this (" << size() << ")!");
    ScalarType result = 0;
    for (size_t ii = 0; ii < size(); ++ii)
      result += std::conj(get_entry_ref(ii)) * other.get_entry_ref(ii);
    return result;
  } // ... dot(...)

  /**
   *  \brief  The l1-norm of the vector.
   *  \return The l1-norm of the vector.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual RealType l1_norm() const
  {
    RealType result = 0;
    for (size_t ii = 0; ii < size(); ++ii)
      result += std::abs(get_entry_ref(ii));
    return result;
  } // ... l1_norm(...)

  /**
   *  \brief  The l2-norm of the vector.
   *  \return The l2-norm of the vector.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual RealType l2_norm() const
  {
    return std::sqrt(std::abs(dot(this->as_imp(*this)))); // std::abs is only needed for the right return type:
    // v.dot(v) should always be a ScalarType with zero imaginary part
  }

  /**
   *  \brief  The l-infintiy-norm of the vector.
   *  \return The l-infintiy-norm of the vector.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual RealType sup_norm() const { return amax().second; }

  virtual ScalarType standard_deviation() const
  {
    const ScalarType mu = mean();
    ScalarType sigma = 0.0;
    for (const auto& x_i : *this)
      sigma += std::pow(x_i - mu, 2);
    sigma /= size();
    return std::sqrt(sigma);
  } // ... standard_deviation(...)

  /**
   *  \brief  Adds two vectors.
   *  \param  other   The right summand.
   *  \param  result  Vector to write the result of this + other to
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   *  \note   If you are looking for the old (now deprecated) add() method, \see add_to_entry().
   */
  virtual void add(const derived_type& other, derived_type& result) const
  {
    if (other.size() != size())
      DUNE_THROW(Exceptions::shapes_do_not_match,
                 "The size of other (" << other.size() << ") does not match the size of this (" << size() << ")!");
    if (result.size() != size())
      DUNE_THROW(Exceptions::shapes_do_not_match,
                 "The size of result (" << result.size() << ") does not match the size of this (" << size() << ")!");
    for (size_t ii = 0; ii < size(); ++ii)
      result.set_entry(ii, get_entry_ref(ii) + other.get_entry_ref(ii));
  } // ... add(...)

  /**
   *  \brief  Adds two vectors.
   *  \param  other The right summand.
   *  \return The sum of this and other.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   *  \note   If you are looking for the old (now deprecated) add() method, \see add_to_entry().
   */
  virtual derived_type add(const derived_type& other) const
  {
    derived_type result = this->copy();
    result.iadd(other);
    return result;
  } // ... add(...)

  /**
   *  \brief  Inplace variant of add().
   *  \param  other The right summand.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual void iadd(const derived_type& other)
  {
    if (other.size() != size())
      DUNE_THROW(Exceptions::shapes_do_not_match,
                 "The size of other (" << other.size() << ") does not match the size of this (" << size() << ")!");
    for (size_t ii = 0; ii < size(); ++ii)
      set_entry(ii, get_entry_ref(ii) + other.get_entry_ref(ii));
  } // ... iadd(...)

  /**
   *  \brief  Subtracts two vectors.
   *  \param  other   The subtrahend.
   *  \param  result  The vectror to write the difference between this and other to.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual void sub(const derived_type& other, derived_type& result) const
  {
    if (other.size() != size())
      DUNE_THROW(Exceptions::shapes_do_not_match,
                 "The size of other (" << other.size() << ") does not match the size of this (" << size() << ")!");
    if (result.size() != size())
      DUNE_THROW(Exceptions::shapes_do_not_match,
                 "The size of result (" << result.size() << ") does not match the size of this (" << size() << ")!");
    for (size_t ii = 0; ii < size(); ++ii)
      result.set_entry(ii, get_entry_ref(ii) - other.get_entry_ref(ii));
  } // ... sub(...)

  /**
   *  \brief  Subtracts two vectors.
   *  \param  other The subtrahend.
   *  \return The difference between this and other.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual derived_type sub(const derived_type& other) const
  {
    derived_type result = this->copy();
    result.isub(other);
    return result;
  } // ... sub(...)

  /**
   *  \brief  Inplace variant of sub().
   *  \param  other The subtrahend.
   *  \note   If you override this method please use exceptions instead of assertions (for the python bindings).
   */
  virtual void isub(const derived_type& other)
  {
    if (other.size() != size())
      DUNE_THROW(Exceptions::shapes_do_not_match,
                 "The size of other (" << other.size() << ") does not match the size of this (" << size() << ")!");
    for (size_t ii = 0; ii < size(); ++ii)
      set_entry(ii, get_entry_ref(ii) - other.get_entry_ref(ii));
  } // ... isub(...)

  /**
   *  \brief  Multiplies every component of this by a scalar.
   *  \param  alpha The scalar.
   *  \return The scaled copy of this.
   */
  virtual derived_type operator*(const ScalarType& alpha)
  {
    derived_type ret = this->copy();
    ret *= alpha;
    return ret;
  } // ... operator*() ...

  /**
   *  \brief  Computes the scalar products between this and another vector.
   *  \param  other The second factor.
   *  \return The scalar product.
   *  \see dot()
   */
  virtual ScalarType operator*(const derived_type& other) { return dot(other); }

  /**
   *  \brief  Adds another vector to this, in-place variant.
   *  \param  other The second summand.
   *  \return The sum of this and other.
   */
  virtual derived_type& operator+=(const derived_type& other)
  {
    iadd(other);
    return this->as_imp(*this);
  }

  /**
   *  \brief  Subtracts another vector from this, in-place variant.
   *  \param  other The subtrahend.
   *  \return The difference between this and other.
   */
  virtual derived_type& operator-=(const derived_type& other)
  {
    isub(other);
    return this->as_imp(*this);
  }

  /**
   *  \brief  Adds two vectors.
   *  \param  other The second summand.
   *  \return The sum of the two vectors.
   */
  virtual derived_type operator+(const derived_type& other) const { return add(other); }

  /**
   *  \brief  Substracts two vectors.
   *  \param  other The subtrahend
   *  \return The difference.
   */
  virtual derived_type operator-(const derived_type& other) const { return sub(other); }

  virtual derived_type& operator+=(const ScalarType& scalar)
  {
    for (auto& element : *this)
      element += scalar;
    return this->as_imp(*this);
  }

  virtual derived_type& operator-=(const ScalarType& scalar)
  {
    for (auto& element : *this)
      element -= scalar;
    return this->as_imp(*this);
  }

  virtual derived_type& operator/=(const ScalarType& scalar)
  {
    for (auto& element : *this)
      element /= scalar;
    return this->as_imp(*this);
  }

  /**
   *  \brief  Check vectors for equality (componentwise) using almost_equal()
   *  \param  other   A vector of same dimension to compare with.
   *  \return Truth value of the comparison.
   *  \see    almost_equal()
   */
  virtual bool operator==(const derived_type& other) const { return almost_equal(other); }

  /**
   *  \brief  Check vectors for inequality using !almost_equal()
   *  \param  other   A vector of same dimension to compare with.
   *  \return Truth value of the comparison.
   */
  virtual bool operator!=(const derived_type& other) const { return !(this->operator==(other)); }

  /// \}
  /// \name Necesarry for the python bindings.
  /// \{

  /**
   * \brief Variant of dim() needed for the python bindings.
   * \see   dim()
   */
  inline DUNE_STUFF_SSIZE_T pb_dim() const
  {
    try {
      return boost::numeric_cast<DUNE_STUFF_SSIZE_T>(dim());
    } catch (boost::bad_numeric_cast& ee) {
      DUNE_THROW(Exceptions::external_error,
                 "There was an error in boost converting '" << dim() << "' to '"
                                                            << Common::Typename<ScalarType>::value()
                                                            << "': "
                                                            << ee.what());
    }
  } // ... pb_dim(...)

  /**
   * \brief Variant of add_to_entry() needed for the python bindings.
   * \see   add_to_entry()
   */
  inline void pb_add_to_entry(const DUNE_STUFF_SSIZE_T ii, const ScalarType& value)
  {
    try {
      add_to_entry(boost::numeric_cast<size_t>(ii), value);
    } catch (boost::bad_numeric_cast& ee) {
      DUNE_THROW(Exceptions::external_error,
                 "There was an error in boost converting '" << ii << "' to '" << Common::Typename<size_t>::value()
                                                            << "': "
                                                            << ee.what());
    }
  } // ... pb_add_to_entry(...)

  /**
   * \brief Variant of set_entry() needed for the python bindings.
   * \see   set_entry()
   */
  inline void pb_set_entry(const DUNE_STUFF_SSIZE_T ii, const ScalarType& value)
  {
    try {
      set_entry(boost::numeric_cast<size_t>(ii), value);
    } catch (boost::bad_numeric_cast& ee) {
      DUNE_THROW(Exceptions::external_error,
                 "There was an error in boost converting '" << ii << "' to '" << Common::Typename<size_t>::value()
                                                            << "': "
                                                            << ee.what());
    }
  } // ... pb_set_entry(...)

  /**
   * \brief Variant of get_entry() needed for the python bindings.
   * \see   get_entry()
   */
  inline ScalarType pb_get_entry(const DUNE_STUFF_SSIZE_T ii)
  {
    try {
      return get_entry(boost::numeric_cast<size_t>(ii));
    } catch (boost::bad_numeric_cast& ee) {
      DUNE_THROW(Exceptions::external_error,
                 "There was an error in boost converting '" << ii << "' to '" << Common::Typename<size_t>::value()
                                                            << "': "
                                                            << ee.what());
    }
  } // ... pb_get_entry(...)

  /**
   * \brief Variant of amax() needed for the python bindings.
   * \see   amax()
   */
  std::vector<RealType> pb_amax() const
  {
    const auto max = amax();
    try {
      return {boost::numeric_cast<RealType>(max.first), max.second};
    } catch (boost::bad_numeric_cast& ee) {
      DUNE_THROW(Exceptions::external_error,
                 "There was an error in boost converting '" << max.first << "' to '"
                                                            << Common::Typename<RealType>::value()
                                                            << "': "
                                                            << ee.what());
    }
  } // ... pb_amax(...)

  std::vector<ScalarType> components(const std::vector<DUNE_STUFF_SSIZE_T>& component_indices) const
  {
    if (component_indices.size() > dim())
      DUNE_THROW(Exceptions::index_out_of_range,
                 "size of component_indices (" << component_indices.size() << ") is larger than the dim of this ("
                                               << dim()
                                               << ")!");
    std::vector<ScalarType> values(component_indices.size(), ScalarType(0));
    try {
      for (size_t ii = 0; ii < component_indices.size(); ++ii) {
        const size_t component = boost::numeric_cast<size_t>(component_indices[ii]);
        if (component >= dim())
          DUNE_THROW(Exceptions::index_out_of_range,
                     "component_indices[" << ii << "] is too large for this (" << dim() << ")!");
        values[ii] = get_entry(component);
      }
    } catch (boost::bad_numeric_cast& ee) {
      DUNE_THROW(Exceptions::external_error, "There was an error in boost during a numeric_cast: " << ee.what());
    }
    return values;
  } // components(...)

  /// \}

  iterator begin() { return iterator(*this); }

  const_iterator begin() const { return const_iterator(*this); }

  iterator end() { return iterator(*this, true); }

  const_iterator end() const { return const_iterator(*this, true); }

  operator std::vector<ScalarType>() const
  {
    std::vector<ScalarType> ret(dim());
    for (size_t ii = 0; ii < dim(); ++ii)
      ret[ii] = this->operator[](ii);
    return ret;
  }

private:
  template <class T, class S>
  friend std::ostream& operator<<(std::ostream& /*out*/, const VectorInterface<T, S>& /*vector*/);
}; // class VectorInterface

namespace internal {

template <class V>
struct is_vector_helper
{
  DSC_has_typedef_initialize_once(Traits) DSC_has_typedef_initialize_once(ScalarType)

      static const bool is_candidate = DSC_has_typedef(Traits)<V>::value && DSC_has_typedef(ScalarType)<V>::value;
}; // class is_vector_helper

} // namespace internal

template <class V, bool candidate = internal::is_vector_helper<V>::is_candidate>
struct is_vector : public std::is_base_of<VectorInterface<typename V::Traits, typename V::ScalarType>, V>
{
};

template <class V>
struct is_vector<V, false> : public std::false_type
{
};

namespace internal {

template <class VectorImp>
struct VectorAbstractionBase
{
  static const bool is_vector = LA::is_vector<VectorImp>::value;

  static const bool has_static_size = false;

  static const size_t static_size = std::numeric_limits<size_t>::max();

  typedef typename std::conditional<is_vector, VectorImp, void>::type VectorType;
  typedef typename std::conditional<is_vector, typename VectorImp::ScalarType, void>::type ScalarType;
  typedef typename std::conditional<is_vector, typename VectorImp::RealType, void>::type RealType;
  typedef ScalarType S;
  typedef RealType R;

  static inline typename std::enable_if<is_vector, VectorType>::type create(const size_t sz) { return VectorType(sz); }

  static inline typename std::enable_if<is_vector, VectorType>::type create(const size_t sz, const ScalarType& val)
  {
    return VectorType(sz, val);
  }
}; // struct VectorAbstractionBase

} // namespace internal

template <class T, class S>
std::ostream& operator<<(std::ostream& out, const VectorInterface<T, S>& vector)
{
  out << "[";
  const size_t sz = vector.size();
  if (sz > 0) {
    out << vector[0];
    for (size_t ii = 1; ii < sz; ++ii)
      out << "\n " << vector[ii];
  } else
    out << " ";
  out << "]";
  return out;
} // ... operator<<(...)

} // namespace LA
} // namespace Stuff
} // namespace Dune

#endif // DUNE_STUFF_LA_CONTAINER_VECTOR_INTERFACE_HH