/* Copyright 2016 Ramakrishnan Kannan */

#ifndef DISTNMF_DISTIO_HPP_
#define DISTNMF_DISTIO_HPP_

#include <unistd.h>

#include <armadillo>
#include <cinttypes>
#include <string>

#include <algorithm>

#include "common/distutils.hpp"
#include "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;
  static const int k_row_seed = 160669;
  static const int k_col_seed = 162287;
  static const int k_seed_off = 617237;
#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
  IVEC rcounts;  // vector to hold row counts for uneven splits
  IVEC ccounts;  // vector to hold column counts for uneven splits

  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,
                  double sparsity, const int symm,
                  const bool adj_rand, MATTYPE* X) {
    int row_rank = m_mpicomm.row_rank();
    int col_rank = m_mpicomm.col_rank();

    int n_rows = (*X).n_rows;
    int n_cols = (*X).n_cols;

    // Set seed
    if (symm) {  // Special case: symmetric matrix
       int symseed = 0;
       if (row_rank < col_rank) {
        symseed = (k_row_seed * row_rank) + (k_col_seed * col_rank)
                  + k_seed_off;
       } else {
        symseed = (k_row_seed * col_rank) + (k_col_seed * row_rank)
                  + k_seed_off;
       }
       arma::arma_rng::set_seed(symseed);

      // Diagonal case
       if (row_rank == col_rank) {
         sparsity = sparsity * 0.5;
       }
    } else 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") {
      if (symm) {
        if (row_rank == col_rank) {
          // Diagonal blocks
          (*X) = arma::sprandu(n_rows, n_cols, sparsity);
          (*X) = 0.5 * ((*X)  + (*X).t());
        } else if (row_rank < col_rank) {
        // Lower triangular blocks
          (*X) = arma::sprandu(n_cols, n_rows, sparsity).t();
        } else {
          (*X) = arma::sprandu(n_rows, n_cols, sparsity);
        }
      } else {
        (*X).sprandu((*X).n_rows, (*X).n_cols, sparsity);
      }
    } else if (type == "normal") {
      if (symm) {
        if (row_rank == col_rank) {
          // Diagonal blocks
          (*X) = arma::sprandn(n_rows, n_cols, sparsity);
          (*X) = 0.5 * ((*X)  + (*X).t());
        } else if (row_rank < col_rank) {
        // Lower triangular blocks
          (*X) = arma::sprandn(n_cols, n_rows, sparsity).t();
        } else {
          (*X) = arma::sprandn(n_rows, n_cols, sparsity);
        }
      } else {
        (*X).sprandn((*X).n_rows, (*X).n_cols, sparsity);
      }
    }
    SP_MAT::iterator start_it = (*X).begin();
    SP_MAT::iterator end_it = (*X).end();
    if (adj_rand) {
      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") {
      if (symm) {
        if (row_rank == col_rank) {
          // Diagonal blocks
          (*X) = arma::randu(n_rows, n_cols);
          (*X) = 0.5 * ((*X)  + (*X).t());
        } else if (row_rank < col_rank) {
        // Lower triangular blocks
          (*X) = arma::randu(n_cols, n_rows).t();
        } else {
          (*X) = arma::randu(n_rows, n_cols);
        }
      } else {
        (*X).randu();
      }
    } else if (type == "normal") {
      if (symm) {
        if (row_rank == col_rank) {
          // Diagonal blocks
          (*X) = arma::randn(n_rows, n_cols);
          (*X) = 0.5 * ((*X)  + (*X).t());
        } else if (row_rank < col_rank) {
        // Lower triangular blocks
          (*X) = arma::randn(n_cols, n_rows).t();
        } else {
          (*X) = arma::randn(n_rows, n_cols);
        }
      } else {
        (*X).randn();
      }
    }
    if (adj_rand) {
      (*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,
                     const int symm, const bool adj_rand, MATTYPE* X) {
    uint start_row = 0, start_col = 0;
    uint end_row = 0, end_col = 0;

    int pr = m_mpicomm.pr();
    int pc = m_mpicomm.pc();

    int row_rank = m_mpicomm.row_rank();
    int col_rank = m_mpicomm.col_rank();

    switch (m_distio) {
      case ONED_ROW:
        start_row = startidx(m, pr, row_rank);
        end_row = start_row + (*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 = startidx(n, pc, col_rank);
        end_col = start_col + (*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 = startidx(m, pr, row_rank);
          end_row = start_row + (*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 = startidx(n, pc, col_rank);
          end_col = start_col + (*X).n_cols - 1;
        }
        break;
      case TWOD:
        start_row = startidx(m, pr, row_rank);
        end_row = start_row + (*X).n_rows - 1;
        start_col = startidx(n, pc, col_rank);
        end_col = start_col + (*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);

    if (symm) {
      Hrnd = Wrnd.t();
    } else {
      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);
      if (adj_rand) {
        (*it) = ceil(kalpha * tempVal + kbeta);
      } else {
        (*it) = tempVal;
      }
    }
#else
    // MAT templr;
    if ((*X).n_cols == n) {  // ONED_ROW
      MAT myWrnd = Wrnd.rows(start_row, end_row);
      (*X) = myWrnd * Hrnd;
    } else if ((*X).n_rows == m) {  // ONED_COL
      MAT myHcols = Hrnd.cols(start_col, end_col);
      (*X) = Wrnd * myHcols;
    } else {  // TWOD
      MAT myWrnd = Wrnd.rows(start_row, end_row);
      MAT myHcols = Hrnd.cols(start_col, end_col);
      (*X) = myWrnd * myHcols;
    }
    if (adj_rand) {
      (*X).for_each(
          [](MAT::elem_type& val) { val = ceil(kalpha * val + 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;
    unsigned int max_rows = 0, max_cols = 0;
    unsigned int my_rows = A.n_rows;
    unsigned 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,
                  MATTYPE& A)
      : m_mpicomm(mpic), m_distio(iod), m_A(A) {}

  /**
   * 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] symm - Flag to create symmetric matrix
   * @param[in] adj_rand - Flag to run elementwise scaling on each entry.
   * @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,
                 int symm = 0, bool adj_rand = false,
                 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_");
    // Check file sizes
    if (symm) {
      assert(m == n);
    }

    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");

      // Initialise the jagged splits
      rcounts.zeros(pr);
      ccounts.zeros(pc);

      for (int r = 0; r < pr; r++) rcounts[r] = itersplit(m, pr, r);

      for (int r = 0; r < pc; r++) ccounts[r] = itersplit(n, pc, r);

      int row_rank = m_mpicomm.row_rank();
      int col_rank = m_mpicomm.col_rank();

      switch (m_distio) {
        // TODO{seswar3} Have to check the ONED initialisations
        case ONED_ROW:
          m_Arows.zeros(rcounts[row_rank], n);
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, symm, adj_rand,
                     &m_Arows);
          if (type == "lowrank") {
            randomLowRank(m, n, k, symm, adj_rand, &m_Arows);
          }
          break;
        case ONED_COL:
          m_Acols.zeros(m, ccounts[col_rank]);
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, symm, adj_rand,
                     &m_Acols);
          if (type == "lowrank") {
            randomLowRank(m, n, k, symm, adj_rand, &m_Acols);
          }
          break;
        case ONED_DOUBLE: {
          int p = MPI_SIZE;
          m_Arows.set_size(rcounts[row_rank], n);
          m_Acols.set_size(m, ccounts[col_rank]);
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, symm, adj_rand,
                     &m_Arows);
          if (type == "lowrank") {
            randomLowRank(m, n, k, symm, adj_rand, &m_Arows);
          }
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, symm, adj_rand,
                     &m_Acols);
          if (type == "lowrank") {
            randomLowRank(m, n, k, symm, adj_rand, &m_Acols);
          }
          break;
        }
        case TWOD:
          m_A.zeros(rcounts[row_rank], ccounts[col_rank]);
          randMatrix(type, MPI_RANK + kPrimeOffset, sparsity, symm,
                     adj_rand, &m_A);
          if (type == "lowrank") {
            randomLowRank(m, n, k, symm, adj_rand, &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;
        // The global rank to pr, pc is not deterministic and can be different.
        // Hence naming the file for 2D distributed with pr, pc.
        // sr << file_name << MPI_ROW_RANK << "_" << MPI_COL_RANK;
        int pr = m_mpicomm.pr();
        int pc = m_mpicomm.pc();

        int srow = itersplit(m, pr, MPI_ROW_RANK);
        int scol = itersplit(n, pc, MPI_COL_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);
        m_A.resize(srow, scol);
#else
        readInputMatrix(m, n, file_name);
#endif
      }
    }
#ifndef BUILD_SPARSE
    if (i_normalization != NONE) {
      normalize(i_normalization);
    }
#endif
  }

  void readInputMatrix(int global_m, int global_n,
                       const std::string& input_file_name) {
    int pr = m_mpicomm.pr();
    int pc = m_mpicomm.pc();
    int row_rank = m_mpicomm.row_rank();
    int col_rank = m_mpicomm.col_rank();

    int local_m = itersplit(global_m, pr, row_rank);
    int local_n = itersplit(global_n, pc, col_rank);
    m_A.zeros(local_m, local_n);

    int gsizes[] = {global_m, global_n};
    int lsizes[] = {local_m, local_n};
    int starts[] = {startidx(global_m, pr, row_rank),
                    startidx(global_n, pc, col_rank)};
    MPI_Barrier(MPI_COMM_WORLD);

    MPI_Datatype view;
    MPI_Type_create_subarray(2, gsizes, lsizes, starts, MPI_ORDER_FORTRAN,
                             MPI_DOUBLE, &view);
    MPI_Type_commit(&view);

    MPI_File fh;
    int ret = MPI_File_open(MPI_COMM_WORLD, input_file_name.c_str(),
                            MPI_MODE_RDONLY, MPI_INFO_NULL, &fh);
    if (ISROOT && ret != MPI_SUCCESS) {
      DISTPRINTINFO("Error: Could not open file " << input_file_name
                                                  << std::endl);
      MPI_Abort(MPI_COMM_WORLD, 1);
    }

    MPI_Offset disp = 0;
    MPI_File_set_view(fh, disp, MPI_DOUBLE, view, "native", MPI_INFO_NULL);

    int count = lsizes[0] * lsizes[1];
    MPI_Status status;
    ret = MPI_File_read_all(fh, m_A.memptr(), count, MPI_DOUBLE, &status);
    if (ISROOT && ret != MPI_SUCCESS) {
      DISTPRINTINFO("Error could not read file " << input_file_name
                                                 << std::endl);
      MPI_Abort(MPI_COMM_WORLD, 1);
    }
    MPI_File_close(&fh);
    MPI_Type_free(&view);
  }
  /**
   * Writes the factor matrix as output_file_name_W/H
   * @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;
    if (m_distio == TWOD) {
      sw << output_file_name << "_W";
      sh << output_file_name << "_H";
    } else {
      sw << output_file_name << "_W_" << MPI_SIZE;
      sh << output_file_name << "_H_" << MPI_SIZE;
    }
    int size = m_mpicomm.size();

    int pr = m_mpicomm.pr();
    int pc = m_mpicomm.pc();

    int rrank = m_mpicomm.row_rank();
    int crank = m_mpicomm.col_rank();

    int Wm = W.n_rows;
    int Hm = H.n_rows;

    // AllReduce global sizes of W,H in case W,H are inconsistent
    int global_Wm, global_Hm;
    MPI_Allreduce(&Wm, &global_Wm, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);
    MPI_Allreduce(&Hm, &global_Hm, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

    int Widx = startidx(global_Wm, pr, rrank) +
               startidx(itersplit(global_Wm, pr, rrank), pc, crank);
    int Hidx = startidx(global_Hm, pc, crank) +
               startidx(itersplit(global_Hm, pc, crank), pr, rrank);

    writeOutputMatrix(W, global_Wm, Widx, sw.str());
    writeOutputMatrix(H, global_Hm, Hidx, sh.str());
  }

  /**
   * Uses MPIIO to write an output matrix X to a binary file with output_file_name.
   * X is assumed to be distributed in a 1D row-wise process grid.
   * @param[in] local output matrix
   * @param[in] global row count
   * @param[in] starting index in global matrix
   * @param[in] output file name
   */
  void writeOutputMatrix(const MAT& X, int global_m, int idx,
                         const std::string& output_file_name) {
    int rank = m_mpicomm.rank();
    int size = m_mpicomm.size();

    int local_m = X.n_rows;
    int global_n = X.n_cols;

    int gsizes[] = {global_m, global_n};
    int lsizes[] = {local_m, global_n};
    int starts[] = {idx, 0};

    MPI_Datatype view;
    if (local_m > 0) {  // process owns some part of X
      MPI_Type_create_subarray(2, gsizes, lsizes, starts, MPI_ORDER_FORTRAN,
                               MPI_DOUBLE, &view);
    } else {  // process owns no data and so previous function will fail
      MPI_Type_contiguous(0, MPI_DOUBLE, &view);
    }
    MPI_Type_commit(&view);

    MPI_File fh;
    int ret =
        MPI_File_open(m_mpicomm.gridComm(), output_file_name.c_str(),
                      MPI_MODE_WRONLY | MPI_MODE_CREATE, MPI_INFO_NULL, &fh);
    if (ISROOT & ret != MPI_SUCCESS) {
      DISTPRINTINFO("Error: Could not open file " << output_file_name
                                                  << std::endl);
    }

    MPI_Offset disp = 0;
    MPI_File_set_view(fh, disp, MPI_DOUBLE, view, "native", MPI_INFO_NULL);

    int count = lsizes[0] * lsizes[1];
    MPI_Status status;
    ret = MPI_File_write_all(fh, X.memptr(), count, MPI_DOUBLE, &status);
    if (ISROOT && ret != MPI_SUCCESS) {
      DISTPRINTINFO("Error could not write file " << output_file_name
                                                  << std::endl);
    }
    MPI_File_close(&fh);
    MPI_Type_free(&view);
  }

  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; }
  const IVEC& row_counts() const { return rcounts; }
  const IVEC& col_counts() const { return ccounts; }
};

}  // namespace planc

// run with mpi run 3.
void testDistIO(char argc, char* argv[]) {
  planc::MPICommunicator mpicomm(argc, argv);
#ifdef BUILD_SPARSE
  SP_MAT A;
  planc::DistIO<SP_MAT> dio(mpicomm, ONED_DOUBLE, A);
#else
  MAT A;
  planc::DistIO<MAT> dio(mpicomm, ONED_DOUBLE, A);
#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 <stdio.h>
#include <pthread.h>
#include <semaphore.h>
#include <time.h>
#include <opencv2/opencv.hpp>
#include <dlib/svm_threaded.h>

using namespace cv;

extern void init_embedding();
extern void embedding(const unsigned char *img, float *features);

static void *classifier(void *arg);

float diff_time_ms(struct timespec start, struct timespec end) {
  return (1e9 * (end.tv_sec - start.tv_sec) + end.tv_nsec - start.tv_nsec)/1e6;
}

typedef dlib::matrix<float,128,1> sample_type;
typedef dlib::linear_kernel<sample_type> lin_kernel;

static pthread_t thread;
static sem_t *semStart, *semDone;

static Mat image;
static dlib::multiclass_linear_decision_function<lin_kernel,std::string> df;
static std::pair<std::string,float> guess;

void init_classify(sem_t *start, sem_t *done) {
  semStart = start;
  semDone = done;

  init_embedding();

  std::ifstream inf("faces.svm");
  dlib::deserialize(df, inf);

  pthread_create(&thread, NULL, classifier, NULL);
}

void classify_image(Mat& next) {
  image = next.clone();
}

void classify_label(char *buf) {
  sprintf(buf, "%s (%f)", guess.first.c_str(), guess.second);
}

static void *classifier(void *arg) {
  float features[128];
  sample_type mat;
  struct timespec t1,t2,t3;

  while (sem_wait(semStart) == 0) {
    clock_gettime(CLOCK_MONOTONIC, &t1);
    embedding(image.ptr(0), features);
    clock_gettime(CLOCK_MONOTONIC, &t2);
    for (int i=0; i<128; i++) mat(i) = features[i];
    guess = df.predict(mat);
    clock_gettime(CLOCK_MONOTONIC, &t3);
    //printf("\n%s %f (%0.1f %0.1f)\n", guess.first.c_str(), guess.second, diff_time_ms(t1, t2), diff_time_ms(t2, t3));
    sem_post(semDone);
  }
  return NULL;
}


#include <Tests.hpp>
#include <armadillo>
#include <OrbitConversions.hpp>

int main(){

	Tests::run_tests(50000);

	return 0;

}


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

#ifndef FILECOIN_FR32_HPP
#define FILECOIN_FR32_HPP

#include <cassert>

#include <boost/endian/arithmetic.hpp>

namespace nil {
    namespace filecoin {
        typedef std::vector<bool> BitVecLEu8;

        /*!
         * @brief BitByte represents a size expressed in bytes extended
         * with bit precision, that is, not rounded.
         * Invariant: it is an error for bits to be > 7.
         */
        struct BitByte {
            static BitByte from_bits(std::size_t bits) {
                return {bits % 8, bits / 8};
            }

            static BitByte from_bytes(std::size_t bytes) {
                return from_bits(bytes * 8);
            }

            // How many bits in the BitByte (inverse of from_bits).
            std::size_t total_bits() {
                return bytes * 8 + bits;
            }

            // True if the BitByte has no bits component.
            bool is_byte_aligned() {
                return bits == 0;
            }

            // How many distinct bytes are needed to represent data of this size?
            std::size_t bytes_needed() {
                bytes + (bits == 1);
            }

            std::size_t bits;
            std::size_t bytes;
        };

        /*!
         * @brief PaddingMap represents a mapping between data and its padded equivalent.
         *
         * The padding process takes a *byte-aligned stream* of unpadded *raw* data
         * as input and returns another byte stream where padding is applied every
         * `data_bits` to align them to the byte boundary (`element_bits`). The
         * (inverse) *unpadding* process maps that output back to the raw input
         * that generated it.
         * # Padded layout
         * At the *byte-level*, the padded layout is:
         * ```text
         *        (full element)              (full)                 (incomplete)
            ||  data_bits  pad_bits  ||  data_bits  pad_bits  ||  some_data  (no_padding)
                                     ^^                               ^^
                              element boundary                (some_data < data_bits)
                               (byte-aligned)
           ```

         * Each *element* is a byte-aligned stream comprised of a *full unit* of `data_bits`
         * with `pad_bits` at the end to byte-align it (where `pad_bits` is less than a byte,
         * this is a *sub-byte padding* scheme). After the last element boundary there may be
         * an incomplete unit of data (`some_data`) with a length smaller than `data_bits`
         * that hasn't been padded. The padding rules are:
         * 1. Padding is always applied to a full unit of `data_bits`.
         * 2. A full data unit cannot exist without its corresponding padding.
         * 3. A unit of padding is complete by definition: padding can only be applied fully to each element.
         * 4. If there is padding present then there has to be an already formed
         * element there (an element is full if and only if its data unit is full).
         *
         * # Last byte
         *
         * When returning the byte-aligned output generated from the padded *bitstream*
         * (since the padding is done at the bit-level) the conversion results in the
         * last byte having (potentially) more bits than desired. At the *bit-level*
         * the layout of the last byte can either be a complete element (bits of raw
         * data followed by the corresponding padding bits) or an incomplete unit of
         * data: some number of *valid* data (D) bits followed by any number of *extra*
         * bits (X) necessary to complete the byte-aligned stream:
         *
         *   ```text
         *    |   D   D   D   D   X   X   X   X   |
         *            (data)         (extra)      ^ byte boundary (end of output)
         *   ```
         *
         *   (This diagram is just for illustrative purposes, we actually return the output
         *   in little-endian order, see `BitVecLEu8`).
         *
         *   It's important to distinguish these extra bits (generated as a side
         *   effect of the conversion to a byte-aligned stream) from the padding bits
         *   themselves introduced in the padding process: even though both will be
         *   left with a zero value, these extra bits are a place-holder for the actual
         *   raw data bits needed to complete the current unit of data (and hence also
         *   the element, with the corresponding padding bits added after it). Since
         *   extra bits are only a product of an incomplete unit of data there can't
         *   be extra bits after padding bits.
         *
         *   There's no metadata signaling the number of extra bits present in the
         *   last byte in any given padded layout, this is deduced from the fact
         *   that there's only a single number of valid data bits in the last byte,
         *   and hence a number of data bits in total, that maps to a byte-aligned
         *   (multiple of 8) raw data stream that could have been used as input.
         *
         *   # Example: `FR32_PADDING_MAP`
         *
         *   In this case the `PaddingMap` is defined with a data unit of 254 bits that
         *   are byte aligned to a 256-bit (32-byte) element. If the user writes as input,
         *   say, 40 bytes (320 bits) of raw input data to the padding process the resulting
         *   layout would be, at the element (byte) level:

            ```text
                  (full element: 32 bytes)         (incomplete: 9 bytes)
            ||  data_bits: 254  pad_bits: 2  ||   some_data: 66 bits (+ extra bits)
                                             ^^
                                      element boundary
            ```

         *
         * That is, of the original 320 bits (40 bytes) of raw input data, 254 are
         * padded in the first element and the remaining 66 bits form the incomplete
         * data unit after it, which is aligned to 9 bytes. At the bit level, that
         * last incomplete byte will have 2 valid bits and 6 extra bits.
         *
         * # Alignment of raw data bytes in the padded output
         *
         * This section is not necessary to use this structure but it does help to
         * reason about it. By the previous definition, the raw data bits *embedded*
         * in the padded layout are not necessarily grouped in the same byte units
         * as in the original raw data input (due to the inclusion of the padding
         * bits interleaved in that bit stream, which keep shifting the data bits
         * after them).
         *
         * This can also be stated as: the offsets of the bits (relative to the byte
         * they belong to, i.e., *bit-offset*) in the raw data input won't necessarily
         * match the bit-offsets of the raw data bits embedded in the padded layout.
         * The consequence is that each raw byte written to the padded layout won't
         * result in a byte-aligned bit stream output, i.e., it may cause the appearance
         * of extra bits (to convert the output to a byte-aligned stream).
         *
         * There are portions of the padded layout, however, where this alignment does
         * happen. Particularly, when the padded layout accumulates enough padding bits
         * that they altogether add up to a byte, the following raw data byte written
         * will result in a byte-aligned output, and the same is true for all the other
         * raw data byte that follow it up until the element end, where new padding bits
         * shift away this alignment. (The other obvious case is the first element, which,
         * with no padded bits in front of it, has by definition all its embedded raw data
         * bytes aligned, independently of the `data_bits`/`pad_bits` configuration used.)
         *
         * In the previous example, that happens after the fourth element, where 4 units
         * of `pad_bits` add up to one byte and all of the raw data bytes in the fifth
         * element will keep its original alignment from the byte input stream (and the
         * same will happen with every other element multiple of 4). When that fourth
         * element is completed we have then 127 bytes of raw data and 1 byte of padding
         * (totalling 32 * 4 = 128 bytes of padded output), so the interval of raw data
         * bytes `[127..159]` (indexed like this in the input raw data stream) will keep
         * its original alignment when embedded in the padded layout, i.e., every raw
         * data byte written will keep the output bit stream byte-aligned (without extra
         * bits). (Technically, the last byte actually won't be a full byte since its last
         * bits will be replaced by padding).
         *
         * # Key terms
         *
         * Collection of terms introduced in this documentation (with the format
         * `*<new-term>*`). This section doesn't provide a self-contained definition
         * of them (to avoid unnecessary repetition), it just provides (when appropriate)
         * an additional summary of what was already discussed.
         *
         * 1. Raw data: unpadded user-supplied data (we don't use the *unpadded* term
         * to avoid excessive *padding* suffixes in the code). Padding (data) bits.
         * 2. Element: byte-aligned stream consisting of a full unit of data plus the
         * padding bits.
         * 3. Full unit of raw `data_bits` (always followed by padding). Incomplete unit,
         * not followed by padding, doesn't form an element.
         * 4. Byte-aligned stream: always input and output of the (un)padding process,
         * either as raw data or padded (using the term "byte-aligned" and not "byte
         * stream" to stress the boundaries of the elements). Bit streams: used internally
         * when padding data (never returned as bits).
         * 5. Valid data bits, only in the context of the last byte of a byte-aligned stream
         * generated from the padding process. Extra bits: what's left unused of the last
         * byte (in a way the extra bits are the padding at the byte-level, but we don't
         * use that term here to avoid confusions).
         * 6. Sub-byte padding.
         * 7. Bit-offset: offset of a bit within the byte it belongs to, ranging in `[0..8]`.
         * 8. Embedded raw data: view of the input raw data when it has been decomposed in
         * bit streams and padded in the resulting output.
         */
        struct padding_map {
            std::size_t data_bits;
            std::size_t element_bits;

            padding_map(std::size_t data_bits, std::size_t element_bits) {
                // Check that we add less than 1 byte of padding (sub-byte padding).
                assert(("Padding (num bits: " element_bits - data_bits ") must be less than 1 byte.",
                        element_bits - data_bits <= 7, ));
                // Check that the element is byte aligned.
                assert(("Element (num bits: " element_bits ") must be byte aligned.", !element_bits % CHAR_BIT));
            }

            inline void pad(BitVecLEu8 &bits_out) {
                bits_out.insert(std::back_inserter(bits.out), pad_bits(), false);
                // TODO: Optimization: Drop this explicit `push` padding, the padding
                // should happen implicitly when byte-aligning the data unit.
            }

            inline std::size_t pad_bits() const {
                return element_bits - data_bits;
            }

            // Transform an offset (either a position or a size) *expressed in
            // bits* in a raw byte-aligned data stream to its equivalent in a
            // generated padded bit stream, that is, not byte aligned (so we
            // don't count the extra bits here). If `padding` is `false` calculate
            // the inverse transformation.
            std::size_t transform_bit_offset(std::size_t pos, bool padding) {
                // Set the sizes we're converting to and from.
                std::size_t from_size, to_size;
                if (padding) {
                    from_size = data_bits;
                    to_size = element_bits;
                } else {
                    from_size = element_bits;
                    to_size = data_bits;
                }

                // For both the padding and unpadding cases the operation is the same.
                // The quotient is the number of full, either elements, in the padded layout,
                // or groups of `data_bits`, in the raw data input (that will be converted
                // to full elements).
                // The remainder (in both cases) is the last *incomplete* part of either of
                // the two. Even in the padded layout, if there is an incomplete element it
                // has to consist *only* of data (see `PaddingMap#padded-layout`). That amount
                // of spare raw data doesn't need conversion, it can just be added to the new
                // position.
                let(full_elements, incomplete_data) = div_rem(pos, from_size);
                (full_elements * to_size) + incomplete_data
            }

            // Similar to `transform_bit_pos` this function transforms an offset
            // expressed in bytes, that is, we are taking into account the extra
            // bits here.
            // TODO: Evaluate the relationship between this function and `transform_bit_offset`,
            // it seems the two could be merged, or at least restructured to better expose
            // their differences.
            std::size_t transform_byte_offset(std::size_t pos, bool padding) {
                std::size_t transformed_bit_pos = transform_bit_offset(pos * 8, padding);

                std::size_t transformed_byte_pos = transformed_bit_pos / 8.;
                // TODO: Optimization: It might end up being cheaper to avoid this
                // float conversion and use / and %.

                // When padding, the final bits in the bit stream will grow into the
                // last (potentially incomplete) byte of the byte stream, so round the
                // number up (`ceil`). When unpadding, there's no way to know a priori
                // how many valid bits are in the last byte, we have to choose the number
                // that fits in a byte-aligned raw data stream, so round the number down
                // to that (`floor`).
                return padding ? std::ceil(transformed_byte_pos) : std::floor(transformed_byte_pos);
            }

            // From the `position` specified, it returns:
            // - the absolute position of the start of the next element,
            //   in bytes (since elements -with padding- are byte aligned).
            // - the number of bits left to read (write) from (to) the current
            //   data unit (assuming it's full).
            std::tuple<std::size_t, std::size_t> next_boundary(const BitByte &position) {
                std::size_t position_bits = position.total_bits();

                let(_, bits_after_last_boundary) = div_rem(position_bits, self.element_bits);

                let remaining_data_unit_bits = data_bits - bits_after_last_boundary;

                let next_element_position_bits = position_bits + remaining_data_unit_bits + pad_bits();

                (next_element_position_bits / 8, remaining_data_unit_bits)
            }

            // For a `Seek`able `target` of a byte-aligned padded layout, return:
            // - the size in bytes
            // - the size in bytes of raw data which corresponds to the `target` size
            // - a BitByte representing the number of padded bits contained in the
            //   byte-aligned padded layout
            template<typename SeekableType>
            std::tuple<std::uint64_t, std::uint64_t, BitByte> target_offsets(SeekableType &target) {
                // The current position in `target` is the number of padded bytes already written
                // to the byte-aligned stream.
                let padded_bytes = target.seek(SeekFrom::End(0));

                // Deduce the number of input raw bytes that generated that padded byte size.
                let raw_data_bytes = self.transform_byte_offset(padded_bytes, false);

                // With the number of raw data bytes elucidated it can now be specified the
                // number of padding bits in the generated bit stream (before it was converted
                // to a byte-aligned stream), that is, `raw_data_bytes * 8` is not necessarily
                // `padded_bits`).
                let padded_bits = self.transform_bit_offset(raw_data_bytes * 8, true);

                return std::make_tuple(padded_bytes, raw_data_bytes, BitByte::from_bits(padded_bits));
                // TODO: Why do we use `usize` internally and `u64` externally?
            }
        };

        // TODO: Optimization: Evaluate saving the state of a (un)padding operation
        // inside (e.g., as a cursor like in `BitVec`), maybe not in this structure but
        // in a new `Padder` structure which would remember the positions (remaining
        // data bits in the element, etc.) to avoid recalculating them each time across
        // different (un)pad calls.

        // This is the padding map corresponding to Fr32.
        // Most of the code in this module is general-purpose and could move elsewhere.
        // The application-specific wrappers which implicitly use Fr32 embed the FR32_PADDING_MAP.
        const static padding_map FR32_PADDING_MAP = {254, 256};

        ////////////////////////////////////////////////////////////////////////////////////////////////////
        // Convenience interface for API functions – all bundling FR32_PADDING_MAP
        // parameter/return types are tuned for current caller convenience.
        template<typename SeekableType>
        std::uint64_t target_unpadded_bytes(SeekableType &target) {
            let(_, unpadded, _) = FR32_PADDING_MAP.target_offsets(target) ? ;

            return unpadded;
        }

        // Leave the actual truncation to caller, since we can't do it generically.
        // Return the length to which target should be truncated.
        // We might should also handle zero-padding what will become the final byte of target.
        // Technically, this should be okay though because that byte will always be overwritten later.
        // If we decide this is unnecessary, then we don't need to pass target at all.
        template<typename SeekableType>
        std::size_t almost_truncate_to_unpadded_bytes(SeekableType &_target, std::uint64_t length) {
            let padded = BitByte::from_bits(FR32_PADDING_MAP.transform_bit_offset((length * 8) as usize, true));
            let real_length = padded.bytes_needed();
            let _final_bit_count = padded.bits;
            Ok(real_length)
        }

        std::uint64_t to_unpadded_bytes(std::uint64_t padded_bytes) {
            return FR32_PADDING_MAP.transform_byte_offset(padded_bytes, false);
        }

        std::size_t to_padded_bytes(std::size_t unpadded_bytes) {
            return FR32_PADDING_MAP.transform_byte_offset(unpadded_bytes, true);
        }
    }    // namespace filecoin
}    // namespace nil
#endif


#include <Eigen/Dense>
#include <fmt/core.h>
#include <fmt/ranges.h>

#include <algorithm>
#include <array>
#include <fstream>
#include <iostream>
#include <numeric>
#include <optional>
#include <sstream>
#include <stack>
#include <string>
#include <vector>

auto parseFile(char const *file_name) {
  auto file = std::ifstream(file_name);
  std::string line;
  std::vector<std::string> out;
  while (std::getline(file, line)) {
    out.push_back(line);
  }

  return out;
}

std::optional<char> corrupted(std::string_view v) {
  std::stack<char> s;
  for (auto c : v) {
    switch (c) {
    case '{':
      s.push('}');
      break;
    case '<':
      s.push('>');
      break;
    case '[':
      s.push(']');
      break;
    case '(':
      s.push(')');
      break;
    case '}':
    case '>':
    case ']':
    case ')':
      if (s.top() != c) {
        return c;
      }
      s.pop();
      break;
    default:
      fmt::print("char {} not allowed.\n", c);
      std::abort();
    }
  }

  return {};
}

auto completeAndScore(std::string_view v) {
  std::stack<char> s;
  for (auto c : v) {
    switch (c) {
    case '{':
      s.push('}');
      break;
    case '<':
      s.push('>');
      break;
    case '[':
      s.push(']');
      break;
    case '(':
      s.push(')');
      break;
    case '}':
    case '>':
    case ']':
    case ')':
      if (s.top() != c) {
        fmt::print("Corrupted!\n");
        std::abort();
      }
      s.pop();
      break;
    default:
      fmt::print("char {} not allowed.\n", c);
      std::abort();
    }
  }

  auto score = 0UL;
  while(!s.empty()){
    score *= 5;
    switch(s.top()){
      case ']':
        score += 2;
        break;
      case ')':
        score += 1;
        break;
      case '}':
        score += 3;
        break;
      case '>':
        score += 4;
        break;
    }
    s.pop();
  }

  return score;
}

int main(int _, char **argv) {
  auto lines = parseFile(argv[1]);

  std::vector<char> mismatches;
  std::vector<std::size_t> scores;
  for (auto const &l : lines) {
    if (auto bad = corrupted(l)) {
      mismatches.push_back(*bad);
    } else {
      scores.push_back(completeAndScore(l));
    }
  }

  auto sum = 0UL;
  for (auto m : mismatches) {
    switch (m) {
    case '}':
      sum += 1197;
      break;
    case ')':
      sum += 3;
      break;
    case ']':
      sum += 57;
      break;
    case '>':
      sum += 25137;
      break;
    }
  }

  fmt::print("Part 1 score: {}\n", sum);

  std::sort(scores.begin(), scores.end());
  for(auto i = 0; i < scores.size(); ++i){
    fmt::print("{}: {}\n", i, scores[i]);
  }
  fmt::print("Part 2 score: {}\n", scores[scores.size()/2]);
}


///////////////////////////////////////////////////////////////////////////////
/// \file shift.hpp
/// Defined the mathematical operator s on time series
//
//  Copyright 2006 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_TIME_SERIES_NUMERIC_SHIFT_EAN_06_05_2006
#define BOOST_TIME_SERIES_NUMERIC_SHIFT_EAN_06_05_2006

#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/time_series/concepts.hpp>
#include <boost/time_series/shifted_series.hpp>

namespace boost { namespace time_series
{

    /// \brief Shifts a \c TimeSeries by the specified offset.
    ///
    /// Shifts a \c TimeSeries by the specified offset. The offset can be positive,
    ///     which shifts the series right, or negative which shifts it left.
    ///
    /// \param series The series to shift.
    /// \param offset The offset by which the series is to be shifted.
    /// \pre \c Series is a model of the \c TimeSeries concept.
    /// \pre \c Offset is convertible to the \c offset_type of the input series.
    /// \return A <tt>shifted_series\<\></tt> view that models \c TimeSeries.
    /// \attention The complexity is O(1).
    template<typename Series, typename Offset>
    BOOST_CONCEPT_REQUIRES(
        ((concepts::TimeSeries<Series>))
        ((Convertible<Offset, typename concepts::TimeSeries<Series>::offset_type>)),
    (shifted_series<Series> const))
    shift(Series &series, Offset offset)
    {
        return shifted_series<Series>(series, offset);
    }

    /// \overload
    ///
    template<typename Series, typename Offset>
    BOOST_CONCEPT_REQUIRES(
        ((concepts::TimeSeries<Series const>))
        ((Convertible<Offset, typename concepts::TimeSeries<Series const>::offset_type>)),
    (shifted_series<Series const> const))
    shift(Series const &series, Offset offset)
    {
        return shifted_series<Series const>(series, offset);
    }

}}

#endif // BOOST_TIME_SERIES_NUMERIC_SHIFT_EAN_06_05_2006


/**
 * Author: Patrick Emami
 * Date: 9/30/15
 */
#include "tgen_manager.h"
#include <gtest/gtest.h>
#include <ros/console.h>
#include <sub8_alarm/alarm_helpers.h>
#include <sub8_msgs/PathPoint.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <boost/math/constants/constants.hpp>
#include "ompl/base/StateSampler.h"
#include "sub8_msgs/Path.h"
#include "sub8_state_space.h"
#include "tgen_common.h"

using sub8::trajectory_generator::TGenManager;
using sub8::trajectory_generator::TGenManagerPtr;
using sub8::trajectory_generator::Sub8StateSpace;
using sub8::AlarmBroadcasterPtr;
using sub8::AlarmBroadcaster;
using ompl::base::StateSamplerPtr;

class TGenManagerTest : public ::testing::Test
{
public:
  TGenManagerTest() : _node_handle(new ros::NodeHandle()), _test_space(new Sub8StateSpace()){};

  boost::shared_ptr<ros::NodeHandle> getNodeHandle()
  {
    return _node_handle;
  }

  TGenManagerPtr tgenManagerFactory()
  {
    AlarmBroadcasterPtr ab(new AlarmBroadcaster(getNodeHandle()));
    return TGenManagerPtr(new TGenManager(std::move(ab)));
  }

  // return a reference to the state space
  const ompl::base::StateSpacePtr& ss() const
  {
    return _test_space;
  }

  // Sample a random start and goal state
  std::vector<State*> randomProblemDef()
  {
    std::vector<State*> pd;

    // Set up start and goal states
    State* start_state = ss()->allocState();
    State* goal_state = ss()->allocState();

    StateSamplerPtr state_sampler = ss()->allocStateSampler();
    state_sampler->sampleUniform(start_state);
    state_sampler->sampleUniform(goal_state);

    pd.push_back(start_state);
    pd.push_back(goal_state);

    return pd;
  }

private:
  boost::shared_ptr<ros::NodeHandle> _node_handle;
  ompl::base::StateSpacePtr _test_space;
};

TEST_F(TGenManagerTest, testSetProblemDefinition)
{
  TGenManagerPtr test_tgen_manager = tgenManagerFactory();

  std::vector<State*> pd = randomProblemDef();
  test_tgen_manager->setProblemDefinition(pd[0], pd[1]);

  ss()->freeState(pd[0]);
  ss()->freeState(pd[1]);
}

TEST_F(TGenManagerTest, testBadProblemDefinition)
{
  TGenManagerPtr test_tgen_manager = tgenManagerFactory();

  // Set up start and goal states
  State* start_state = ss()->allocState();
  State* goal_state = ss()->allocState();

  std::vector<State*> pd = randomProblemDef();
  pd[0]->as<Sub8StateSpace::StateType>()->setX(10000);

  ASSERT_FALSE(test_tgen_manager->setProblemDefinition(pd[0], pd[1]));
}

TEST_F(TGenManagerTest, testGeneratePath)
{
  TGenManagerPtr test_tgen_manager = tgenManagerFactory();

  std::vector<State*> pd = randomProblemDef();
  test_tgen_manager->setProblemDefinition(pd[0], pd[1]);

  ROS_WARN("Testing TGEN planning with a random start and goal...");

  ASSERT_TRUE(test_tgen_manager->solve());

  ss()->freeState(pd[0]);
  ss()->freeState(pd[1]);
}

TEST_F(TGenManagerTest, testGetPath)
{
  TGenManagerPtr test_tgen_manager = tgenManagerFactory();

  std::vector<State*> pd = randomProblemDef();
  test_tgen_manager->setProblemDefinition(pd[0], pd[1]);

  ASSERT_TRUE(test_tgen_manager->solve());

  std::vector<State*> path = test_tgen_manager->getPath();

  ASSERT_GT(path.size(), 2);
}

TEST_F(TGenManagerTest, testGeneratePathMessage)
{
  TGenManagerPtr test_tgen_manager = tgenManagerFactory();

  std::vector<State*> pd = randomProblemDef();
  test_tgen_manager->setProblemDefinition(pd[0], pd[1]);

  ASSERT_TRUE(test_tgen_manager->solve());

  sub8_msgs::Path path_msg = test_tgen_manager->generatePathMessage();

  std::vector<sub8_msgs::PathPoint> pp = path_msg.path;

  ASSERT_NEAR(pd[0]->as<Sub8StateSpace::StateType>()->getX(), pp[0].position.x, 0.01);
  ASSERT_NEAR(pd[0]->as<Sub8StateSpace::StateType>()->getY(), pp[0].position.y, 0.01);
  ASSERT_NEAR(pd[0]->as<Sub8StateSpace::StateType>()->getZ(), pp[0].position.z, 0.01);
  ASSERT_NEAR(pd[0]->as<Sub8StateSpace::StateType>()->getYaw(), pp[0].yaw, 0.01);
}

// TODO - needs Octomap
TEST_F(TGenManagerTest, testPathValidation)
{
  ADD_FAILURE() << "Unimplemented test";
}

TEST_F(TGenManagerTest, testStateToPathPoint)
{
  TGenManagerPtr test_tgen_manager = tgenManagerFactory();

  State* state = ss()->allocState();
  state->as<Sub8StateSpace::StateType>()->setXYZ(1, 1, 1);

  state->as<Sub8StateSpace::StateType>()->setYaw(boost::math::constants::pi<double>());

  sub8_msgs::PathPoint path_point = test_tgen_manager->stateToPathPoint(state);

  ASSERT_NEAR(state->as<Sub8StateSpace::StateType>()->getX(), path_point.position.x, 0.01);
  ASSERT_NEAR(state->as<Sub8StateSpace::StateType>()->getY(), path_point.position.y, 0.01);
  ASSERT_NEAR(state->as<Sub8StateSpace::StateType>()->getZ(), path_point.position.z, 0.01);
  ASSERT_NEAR(state->as<Sub8StateSpace::StateType>()->getYaw(), path_point.yaw, 0.01);

  ss()->freeState(state);
}

TEST_F(TGenManagerTest, testPoseToState)
{
  TGenManagerPtr test_tgen_manager = tgenManagerFactory();

  State* state = ss()->allocState();

  geometry_msgs::Pose pose;
  pose.position.x = 1;
  pose.position.y = 1;
  pose.position.z = 2;
  pose.orientation.x = 1;
  pose.orientation.y = 0;
  pose.orientation.z = 0;
  pose.orientation.w = 0;
  state = test_tgen_manager->poseToState(pose);

  ASSERT_NEAR(pose.position.x, state->as<Sub8StateSpace::StateType>()->getX(), 0.01);
  ASSERT_NEAR(pose.position.y, state->as<Sub8StateSpace::StateType>()->getY(), 0.01);
  ASSERT_NEAR(pose.position.z, state->as<Sub8StateSpace::StateType>()->getZ(), 0.01);
  ASSERT_NEAR(0, state->as<Sub8StateSpace::StateType>()->getYaw(), 0.01);

  ss()->freeState(state);
}


#include <boost/numeric/ublas/operation/num_columns.hpp>


#include <stdio.h>
#include <cstdlib>
#include <ctime>
#include <fstream>
#include <dirent.h>
#include <iostream>
#include <sstream>
#include <string>

#include "stereo_sdf/SGMStereo.h"
#include "stereo_sdf/utils.h"

#include <Eigen/Dense>

// Headers from package stereo_utils.
#include <stereo_utils/stereo_utils.hpp>
#include <stereo_utils/Common.hpp>

using namespace std;

using JSON = nlohmann::json;
namespace su = stereo_utils;

/* -------------------------- SET THESE PARAMETERS --------------------------- */
/* stereo options */
#define STEREO_FULL_PIPELINE -1
#define STEREO_LEFT_ONLY      0

/* cost volume update options */
#define VOLUME_UPDATE_NAIVE   3
#define VOLUME_UPDATE_DIFFB   2
#define VOLUME_UPDATE_NEIGH   1
#define VOLUME_UPDATE_NONE   -1

/* image save flag */
#define SAVE_IMAGES           1

/* ground truth sampling option */
double SAMPLING_FRACTION   = 0.50;

/* disparity scaling factor (256 for KITTI) */
double SCALING_FACTOR      = 256.0;

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

/* ------------ Type definition for interfacing the JSON file. --------------- */

struct CaseDescription {
	std::string name    = "";   // Name of the test case.
	std::string fn0     = "";   // Filename of image Ref.
	std::string fn1     = "";   // Filename of image Tst.
	std::string fnD     = "";   // Filename of the true disparity map.
	std::string outDir  = "";   // Output directory.
	std::string fnQ     = "";   // Filename of the Q matrix.
	float qFactor       = 1.0f; // Scale factor of Q.
	float dOffs         = 0.0f; // Disparity offset. Non-zero for Middlebury dataset. Zero for other datasets.
    float distLimit     = 20.f; // The distance limit for point cloud generation.
    double gtSampleFrac = 0.5;  // Ground truth sampling option.

    SGMParams sgmParams = SGMParams();
};

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

void SemiGlobalMatching(const cv::Mat &leftImage,
                        const cv::Mat &rightImage,
                        cv::Mat &dispImage,
                        int STEREO_PIPELINE_MODE,
                        const std::string cameraParamFile,
                        cv::Mat depthImage,
                        cv::Mat weightImg,
                        int FUSE_FLAG,
                        const SGMParams &sgmParams)
{
    png::image<png::rgb_pixel> leftImageSGM, rightImageSGM;
    Utils utilities;
    utilities.convertCVMatToPNG(leftImage, leftImageSGM);
    utilities.convertCVMatToPNG(rightImage, rightImageSGM);
    size_t width = leftImageSGM.get_width();
    size_t height = leftImageSGM.get_height();
    if (width != rightImageSGM.get_width() ||
        height != rightImageSGM.get_height())
    {
        dispImage = cv::Mat1w();
        return;
    }
    float* dispImageFloat = (float*)malloc(width*height*sizeof(float));
    SGMStereo sgm;

    // Configure the SGM method.
    std::cout << "P1 = " << sgmParams.P1 << ", "
              << "P2 = " << sgmParams.P2 << ". \n";
    sgm.setSmoothnessCostParameters( sgmParams.P1, sgmParams.P2 );
    sgm.setDisparityTotal( sgmParams.total );

    cv::Mat leftImageGray;
    cv::cvtColor(leftImage, leftImageGray, CV_RGB2GRAY);
    sgm.compute(leftImageSGM,
                rightImageSGM,
                dispImageFloat,
                STEREO_PIPELINE_MODE,
                cameraParamFile,
                depthImage,
                FUSE_FLAG,
                leftImageGray,
                weightImg);
    dispImage = utilities.convertFloatToCVMat(width, height, dispImageFloat);
    free(dispImageFloat);
}

void displayMinMax(cv::Mat array)
{
    double min, max;
    cv::minMaxLoc(array, &min, &max);
    std::cout << "Minimum: " << min << " | Maximum: " << max << std::endl;
}

static void save_ply(
    const std::string &fn,
    const Eigen::MatrixXf &Q,
    const cv::Mat &dispFloat,
    const cv::Mat &color,
    float distLimit=20.f,
    float dOffs=0.f ) {

    cv::Mat dispFloat32;
    dispFloat.convertTo(dispFloat32, CV_32FC1);

    const bool flagFlip       = true; // Flip the point cloud so that the y-axis is upwards.
    const bool flagBinary     = true; // Write binary PLY files.
    const float distanceLimit = 20.f; // All points with depth greater than this value will be ignored.
    cv::Mat predDispOffs      = dispFloat32 + dOffs; // This is required by Middlebury dataset. dOffs will be zero for other dataset.
    su::write_ply_with_color(fn,
        predDispOffs, color,
        Q, flagFlip, distanceLimit, flagBinary);
}

static void process( const CaseDescription &cd ) {
    /* input and output directories */
    // std::string repo_dir = argv[1];
    // std::string left_image_uri = repo_dir + "imgs/stereo_left.png";
    // std::string right_image_uri = repo_dir + "imgs/stereo_right.png";
    // std::string left_depth_uri = repo_dir + "imgs/gt_disparity.png";
    // std::string save_dir = repo_dir + "results/";

    std::string left_image_uri  = cd.fn0;
    std::string right_image_uri = cd.fn1;
    std::string left_depth_uri  = cd.fnD;
    std::string save_dir        = cd.outDir + "/";

    su::test_directory(save_dir);

    Eigen::MatrixXf Q;
    if ( cd.fnQ != "" ) {
        su::read_matrix(cd.fnQ, 4, 4, " ", Q);

		// Update the Q matrix according to the scale factor.
		Q(0, 3) *= cd.qFactor;
		Q(1, 3) *= cd.qFactor;
		Q(2, 3) *= cd.qFactor;
    }

    Utils utilities;

    std::cout << "DATA DETAILS: " << std::endl;
    std::cout << "--- Left Image: " << left_image_uri << std::endl;
    std::cout << "--- Right Image: " << right_image_uri << std::endl;
    std::cout << "--- Disparity: (GT) " << left_depth_uri << std::endl;

    std::vector<int> compression_params;
    compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
    compression_params.push_back(0);

    cv::Mat left_image_clr = cv::imread(left_image_uri);
    cv::Mat right_image_clr = cv::imread(right_image_uri);

    cv::Mat_<double> disp_image = cv::imread(left_depth_uri,
                                             cv::IMREAD_ANYDEPTH);
    disp_image = disp_image / SCALING_FACTOR;
    std::cout << "{read status:} successfully read input images.." << std::endl;

    /* EVALUATION A: SEMI GLOBAL MATCHING */
    std::cout << "\n{EVALUATION A:} -- SEMI GLOBAL MATCHING -- " << std::endl;
    cv::Mat disparity_image;
    SemiGlobalMatching(left_image_clr,
                       right_image_clr,
                       disparity_image,
                       STEREO_LEFT_ONLY,
                       "no_params_needed",
                       cv::Mat(),
                       cv::Mat(),
                       VOLUME_UPDATE_NONE,
                       cd.sgmParams);
    if (SAVE_IMAGES) {
        std::string save_file_name = + "sgm_default.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{SGM} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, disparity_image, compression_params);
    }

    cv::Mat_<double> disp_SGM = disparity_image / SCALING_FACTOR;
    /* evaluate SGM */
    cv::Mat_<double> error_image_sgm;
    double average_error_sgm;
    cv::Mat sample_mask_sgm = cv::Mat::zeros(disp_SGM.rows,
                                             disp_SGM.cols,
                                             CV_32FC1);
    utilities.calculateAccuracy(disp_SGM,
                                disp_image,
                                average_error_sgm,
                                error_image_sgm,
                                sample_mask_sgm);
    std::cout << "{SGM} avg error: " << average_error_sgm << std::endl;

    cv::Mat sample_mask;
    // utilities.generateRandomSamplingMask(disp_image,
    //                                      sample_mask,
    //                                      SAMPLING_FRACTION);
    utilities.generateRandomSamplingMask(disp_image,
                                         sample_mask,
                                         cd.gtSampleFrac);

    if (SAVE_IMAGES) {
        std::string save_file_name = "sparse_mask.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{MASK} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, sample_mask, compression_params);
    }
    cv::Mat masked_depth;
    disp_image.copyTo(masked_depth, sample_mask);

    // Save PLY file.
    if ( cd.fnQ != "" ) {
        std::string plyFn = save_dir + "Cloud_SGM.ply";
        save_ply(plyFn, Q,
            disp_SGM, left_image_clr,
            cd.distLimit, cd.dOffs );
        std::cout << "Point cloud saved to " << plyFn << "\n";
    }

    /* EVALUATION B: USE SPARSE LIDAR POINTS FOR NAIVE FUSION */
    std::cout << "\n{EVALUATION B:} -- NAIVE LIDAR FUSION -- " << std::endl;
    cv::Mat disparity_image_sl_naive;
    SemiGlobalMatching(left_image_clr,
                       right_image_clr,
                       disparity_image_sl_naive,
                       STEREO_LEFT_ONLY,
                       "no_params_needed",
                       masked_depth,
                       cv::Mat(),
                       VOLUME_UPDATE_NAIVE,
                       cd.sgmParams);
    if (SAVE_IMAGES) {
        std::string save_file_name = "fuse_naive.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{Naive Fusion} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, disparity_image_sl_naive, compression_params);
    }
    cv::Mat_<double> disp_NF = disparity_image_sl_naive / SCALING_FACTOR;
    /* evaluate naive fusion */
    cv::Mat_<double> error_image_nf;
    double average_error_nf;
    utilities.calculateAccuracy(disp_NF,
                                disp_image,
                                average_error_nf,
                                error_image_nf, sample_mask);
    std::cout << "{NAIVE FUSION} avg error: " << average_error_nf << std::endl;

    // Save PLY file.
    if ( cd.fnQ != "" ) {
        std::string plyFn = save_dir + "Cloud_Naive.ply";
        save_ply(plyFn, Q,
            disp_NF, left_image_clr,
            cd.distLimit, cd.dOffs );
        std::cout << "Point cloud saved to " << plyFn << "\n";
    }

    /*EVALUATION C: USE DIFFUSION BASED METHOD */
    std::cout << "\n{EVALUATION C:} -- DIFFUSION BASED -- " << std::endl;
    cv::Mat disparity_image_db;
    SemiGlobalMatching(left_image_clr,
                       right_image_clr,
                       disparity_image_db,
                       STEREO_LEFT_ONLY,
                       "no_params_needed",
                       masked_depth,
                       cv::Mat(),
                       VOLUME_UPDATE_DIFFB,
                       cd.sgmParams);
    if (SAVE_IMAGES) {
        std::string save_file_name = "fuse_diffusionbased.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{DB} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, disparity_image_db, compression_params);
    }
    cv::Mat_<double> disp_DB = disparity_image_db / SCALING_FACTOR;
    /* evaluate diffusion based confidence propagation method */
    cv::Mat_<double> error_image_db;
    double average_error_db;
    utilities.calculateAccuracy(disp_DB,
                                disp_image,
                                average_error_db,
                                error_image_db,
                                sample_mask);
    std::cout << "{DB} avg error: " << average_error_db << std::endl;

    // Save PLY file.
    if ( cd.fnQ != "" ) {
        std::string plyFn = save_dir + "Cloud_Diffusion.ply";
        save_ply(plyFn, Q,
            disp_DB, left_image_clr,
            cd.distLimit, cd.dOffs );
        std::cout << "Point cloud saved to " << plyFn << "\n";
    }

    /*EVALUATION D: USE BASIC BILATERAL COST UPDATE */
    std::cout << "\n{EVALUATION D:} -- NEIGHBORHOOD SUPPORT -- " << std::endl;
    cv::Mat disparity_image_ns;
    SemiGlobalMatching(left_image_clr,
                       right_image_clr,
                       disparity_image_ns,
                       STEREO_LEFT_ONLY,
                       "no_params_needed",
                       masked_depth,
                       cv::Mat(),
                       VOLUME_UPDATE_NEIGH,
                       cd.sgmParams);
    if (SAVE_IMAGES) {
        std::string save_file_name = "fuse_neighborhoodsupport.png";
        std::string save_url = save_dir + save_file_name;
        std::cout << "{NS} saving image to: " << save_url << std::endl;
        cv::imwrite(save_url, disparity_image_ns, compression_params);
    }
    cv::Mat_<double> disp_NS = disparity_image_ns / SCALING_FACTOR;
    /* evaluate neighborhood support method */
    cv::Mat_<double> error_image_ns;
    double average_error_ns;
    utilities.calculateAccuracy(disp_NS,
                                disp_image,
                                average_error_ns,
                                error_image_ns,
                                sample_mask);
    std::cout << "{NS} avg error: " << average_error_ns << std::endl;

    // Save PLY file.
    if ( cd.fnQ != "" ) {
        std::string plyFn = save_dir + "Cloud_Neighbor.ply";
        save_ply(plyFn, Q,
            disp_NS, left_image_clr,
            cd.distLimit, cd.dOffs );
        std::cout << "Point cloud saved to " << plyFn << "\n";
    }
}

int main(int argc, char** argv)
{
    if ( argc < 2 ) {
		std::cerr << "Must specify the input JSON file. \n";
		throw std::runtime_error("Must specify the input JSON file. ");
	}

	// Read the JSON file.
	std::shared_ptr<JSON> pJSON = su::read_json(argv[1]);
	auto& cases = (*pJSON)["cases"];
	const int N = cases.size();

	// Process all the cases.
	for ( int i = 0; i < N; ++i ) {
        if ( true != cases[i]["enable"] )
            continue;

		auto cd = CaseDescription();
		cd.fn0  = cases[i]["fn0"]; // Filename of image 0.
		cd.fn1  = cases[i]["fn1"]; // Filename of image 1.

		// True disparity if exists.
		cd.fnD = cases[i]["fnD"];

		cd.name         = cases[i]["name"];         // Case name.
        cd.gtSampleFrac = cases[i]["gtSampleFrac"]; // The true data sample fraction.

        std::stringstream ss;
        std::string tempDir = cases[i]["outDir"]; // This strips the double quotes.
        ss << tempDir << "/" << cd.name << "_" << cd.gtSampleFrac;
        cd.outDir = ss.str(); // Output directory.

		// Q matrix if exists.
		if ( cases[i].find("fnQ") != cases[i].end() ) {
			cd.fnQ       = cases[i]["fnQ"];
			cd.qFactor   = cases[i]["QF"];
			cd.dOffs     = cases[i]["dOffs"];
            cd.distLimit = cases[i]["distLimit"];
		} else {
			cd.fnQ       = "";
			cd.qFactor   = 1.f;
			cd.dOffs     = 0.f;
            cd.distLimit = 20.f;
		}

        // SGM parameters.
        cd.sgmParams.P1    = cases[i]["sgm"]["P1"];
        cd.sgmParams.P2    = cases[i]["sgm"]["P2"];
        cd.sgmParams.total = cases[i]["sgm"]["total"];

		std::cout << "\n========== Procesing " << cd.name << "_" << cd.gtSampleFrac << ". ==========\n\n";
		process( cd );
	}

	std::cout << "SPS-Stereo done. \n";

    return 0;
}


//  boost integer_mask.hpp test program  -------------------------------------//

//  (C) Copyright Daryle Walker 2001.  Permission to copy, use, modify, sell
//  and distribute this software is granted provided this copyright
//  notice appears in all copies.  This software is provided "as is" without
//  express or implied warranty, and with no claim as to its suitability for
//  any purpose.

//  See http://www.boost.org for most recent version including documentation.

//  Revision History
//  23 Sep 2001  Initial version (Daryle Walker)

#define  BOOST_INCLUDE_MAIN
#include <boost/test/test_tools.hpp>  // for main

#include <boost/cstdlib.hpp>               // for boost::exit_success
#include <boost/integer/integer_mask.hpp>  // for boost::high_bit_mask_t, etc.

#include <iostream>  // for std::cout (std::endl indirectly)

#define PRIVATE_HIGH_BIT_SLOW_TEST(v)  BOOST_TEST( ::boost::high_bit_mask_t< \
 (v) >::high_bit == (1ul << (v)) );
#define PRIVATE_HIGH_BIT_FAST_TEST(v)  BOOST_TEST( ::boost::high_bit_mask_t< \
 (v) >::high_bit_fast == (1ul << (v)) );
#define PRIVATE_HIGH_BIT_TEST(v)  do { PRIVATE_HIGH_BIT_SLOW_TEST(v); \
 PRIVATE_HIGH_BIT_FAST_TEST(v); } while (false)

#define PRIVATE_LOW_BITS_SLOW_TEST(v)  BOOST_TEST( ::boost::low_bits_mask_t< \
 (v) >::sig_bits == ((1ul << (v)) - 1) );
#define PRIVATE_LOW_BITS_FAST_TEST(v)  BOOST_TEST( ::boost::low_bits_mask_t< \
 (v) >::sig_bits_fast == ((1ul << (v)) - 1) );
#define PRIVATE_LOW_BITS_TEST(v)  do { PRIVATE_LOW_BITS_SLOW_TEST(v); \
 PRIVATE_LOW_BITS_FAST_TEST(v); } while (false)

int test_main( int, char*[] )
{
    using std::cout;
    using std::endl;

    cout << "Doing high_bit_mask_t tests." << endl;
    PRIVATE_HIGH_BIT_TEST( 31 );
    PRIVATE_HIGH_BIT_TEST( 30 );
    PRIVATE_HIGH_BIT_TEST( 29 );
    PRIVATE_HIGH_BIT_TEST( 28 );
    PRIVATE_HIGH_BIT_TEST( 27 );
    PRIVATE_HIGH_BIT_TEST( 26 );
    PRIVATE_HIGH_BIT_TEST( 25 );
    PRIVATE_HIGH_BIT_TEST( 24 );
    PRIVATE_HIGH_BIT_TEST( 23 );
    PRIVATE_HIGH_BIT_TEST( 22 );
    PRIVATE_HIGH_BIT_TEST( 21 );
    PRIVATE_HIGH_BIT_TEST( 20 );
    PRIVATE_HIGH_BIT_TEST( 19 );
    PRIVATE_HIGH_BIT_TEST( 18 );
    PRIVATE_HIGH_BIT_TEST( 17 );
    PRIVATE_HIGH_BIT_TEST( 16 );
    PRIVATE_HIGH_BIT_TEST( 15 );
    PRIVATE_HIGH_BIT_TEST( 14 );
    PRIVATE_HIGH_BIT_TEST( 13 );
    PRIVATE_HIGH_BIT_TEST( 12 );
    PRIVATE_HIGH_BIT_TEST( 11 );
    PRIVATE_HIGH_BIT_TEST( 10 );
    PRIVATE_HIGH_BIT_TEST(  9 );
    PRIVATE_HIGH_BIT_TEST(  8 );
    PRIVATE_HIGH_BIT_TEST(  7 );
    PRIVATE_HIGH_BIT_TEST(  6 );
    PRIVATE_HIGH_BIT_TEST(  5 );
    PRIVATE_HIGH_BIT_TEST(  4 );
    PRIVATE_HIGH_BIT_TEST(  3 );
    PRIVATE_HIGH_BIT_TEST(  2 );
    PRIVATE_HIGH_BIT_TEST(  1 );
    PRIVATE_HIGH_BIT_TEST(  0 );

    cout << "Doing low_bits_mask_t tests." << endl;
    PRIVATE_LOW_BITS_TEST( 32 );  // Undefined behavior?  Whoops!
    PRIVATE_LOW_BITS_TEST( 31 );
    PRIVATE_LOW_BITS_TEST( 30 );
    PRIVATE_LOW_BITS_TEST( 29 );
    PRIVATE_LOW_BITS_TEST( 28 );
    PRIVATE_LOW_BITS_TEST( 27 );
    PRIVATE_LOW_BITS_TEST( 26 );
    PRIVATE_LOW_BITS_TEST( 25 );
    PRIVATE_LOW_BITS_TEST( 24 );
    PRIVATE_LOW_BITS_TEST( 23 );
    PRIVATE_LOW_BITS_TEST( 22 );
    PRIVATE_LOW_BITS_TEST( 21 );
    PRIVATE_LOW_BITS_TEST( 20 );
    PRIVATE_LOW_BITS_TEST( 19 );
    PRIVATE_LOW_BITS_TEST( 18 );
    PRIVATE_LOW_BITS_TEST( 17 );
    PRIVATE_LOW_BITS_TEST( 16 );
    PRIVATE_LOW_BITS_TEST( 15 );
    PRIVATE_LOW_BITS_TEST( 14 );
    PRIVATE_LOW_BITS_TEST( 13 );
    PRIVATE_LOW_BITS_TEST( 12 );
    PRIVATE_LOW_BITS_TEST( 11 );
    PRIVATE_LOW_BITS_TEST( 10 );
    PRIVATE_LOW_BITS_TEST(  9 );
    PRIVATE_LOW_BITS_TEST(  8 );
    PRIVATE_LOW_BITS_TEST(  7 );
    PRIVATE_LOW_BITS_TEST(  6 );
    PRIVATE_LOW_BITS_TEST(  5 );
    PRIVATE_LOW_BITS_TEST(  4 );
    PRIVATE_LOW_BITS_TEST(  3 );
    PRIVATE_LOW_BITS_TEST(  2 );
    PRIVATE_LOW_BITS_TEST(  1 );

    return boost::exit_success;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ALGORITHM_INPLACE_MERGE_HPP
#define BOOST_COMPUTE_ALGORITHM_INPLACE_MERGE_HPP

#include <iterator>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/merge.hpp>
#include <boost/compute/container/vector.hpp>

namespace boost {
namespace compute {

/// Merges the sorted values in the range [\p first, \p middle) with
/// the sorted values in the range [\p middle, \p last) in-place.
///
/// Space complexity: \Omega(n)
template<class Iterator>
inline void inplace_merge(Iterator first,
                          Iterator middle,
                          Iterator last,
                          command_queue &queue = system::default_queue())
{
    BOOST_ASSERT(first < middle && middle < last);

    typedef typename std::iterator_traits<Iterator>::value_type T;

    const context &context = queue.get_context();

    ptrdiff_t left_size = std::distance(first, middle);
    ptrdiff_t right_size = std::distance(middle, last);

    vector<T> left(left_size, context);
    vector<T> right(right_size, context);

    copy(first, middle, left.begin(), queue);
    copy(middle, last, right.begin(), queue);

    ::boost::compute::merge(
        left.begin(),
        left.end(),
        right.begin(),
        right.end(),
        first,
        queue
    );
}

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ALGORITHM_INPLACE_MERGE_HPP


// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <exception>
#include <fstream>
#include <iostream>

#include <boost/format.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/command_line.hpp"
#include "heom/common.hpp"
#include "heom/dipole_matrix.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/population_dynamics_solver.hpp"
#include "heom/two_dimensional_spectra_config.hpp"
#include "heom/sites_to_states.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using heom::int_t;
using heom::real_t;
using heom::complex_t;
using heom::real_format;
using heom::default_delimiter;
using heom::complex_matrix_t;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// output compile time configuration
	std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
	heom::write_compile_config(std::cout);
	std::cout << "------------------------------------------------------------" << std::endl;

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::command_line command_line { argc, argv };

		std::cout << "Parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "Parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::two_dimensional_spectra_config heom_config(command_line.heom_config_filename());

		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		const int_t steps_per_iteration = heom_config.program_observe_steps();
		const int_t t_1_iterations = heom_config.spectra_steps_t_1() / heom_config.program_observe_steps(); // not including zero
		const int_t t_3_iterations = heom_config.spectra_steps_t_3() / heom_config.program_observe_steps() + 1; // including 0 and max_steps 0 : steps_per_iteration : spectra_t_3_steps

		// prepare output file (so we run into possible file errors before starting the computation)
		auto& observation_filename = heom_config.observations().get().front().get().second; // filename of first specified observation, guaranteed to exist by config checks
		std::ofstream observation_file(observation_filename);
		if (observation_file.fail())
			throw std::runtime_error("Error: could not open output file: " + observation_filename);

		// output data structure for traces
		const int_t num_t_1_observations = t_1_iterations + 1;
		const int_t num_t_3_observations = t_3_iterations;

		// pre-allocate t_3 x t_1 sized 2d vector for accumulating the observations
		//std::vector<std::vector<heom::matrix_trace_observation_view>> observations(num_t_3_observations);
		std::vector<std::vector<complex_t>> observations(num_t_3_observations);
		for (auto& vec : observations) {
			vec.resize(num_t_1_observations, complex_t(0.0, 0.0));
		}

//		std::cout << "Coupling identity: " << heom::state_baths_coupling(heom_config.baths_coupling(), heom::sites_to_states_mode_t::identity) << std::endl;
//		std::cout << "Coupling with ground state: " << heom::state_baths_coupling(heom_config.baths_coupling(), heom::sites_to_states_mode_t::with_ground_state) << std::endl;
//		std::cout << "Coupling excited state absorption: " << heom::state_baths_coupling(heom_config.baths_coupling(), heom::sites_to_states_mode_t::excited_state_absorption) << std::endl;
//
//		std::cout << "Hamiltonian identity: " << heom::state_hamiltonian(heom_config.system_hamiltonian(), heom::sites_to_states_mode_t::identity) << std::endl;
//		std::cout << "Hamiltonian with ground state: " << heom::state_hamiltonian(heom_config.system_hamiltonian(), heom::sites_to_states_mode_t::with_ground_state) << std::endl;
//		std::cout << "Hamiltonian excited state absorption: " << heom::state_hamiltonian(heom_config.system_hamiltonian(), heom::sites_to_states_mode_t::excited_state_absorption) << std::endl;

		const auto& pathways = heom_config.spectra_pathways().get();

		// for progress estimate
		const size_t solver_runs = pathways.size() * heom_config.dipole_tensor_prefactors().size() * t_3_iterations;
		size_t solver_run = 0;

		// iterate over pathways
		for (size_t p = 0; p < pathways.size(); ++p)
		{
			DEBUG_ONLY( std::cout << "Pathway " << p << " is " << pathways.at(p) << std::endl; )
			const auto& pathway_spec = heom::spectra_pathway_to_specification.at(pathways[p]);
			const auto sts_mode = pathway_spec.sites_to_states_mode();

			heom::instance heom_instance(heom_config, sts_mode, complete_graph);
			heom_instance.set_hierarchy_top(); // NOTE: hierarchy top is initialised to zero with first element being complex_t(1.0,0.0) here

			// OpenCL range
			ocl::nd_range range{
				{}, // offset
				{1, static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
				{1, 1} // local size
			};

			// create a solver from configuration and instance
			solver_t solver(ocl_config, range, heom_config, heom_instance);
			std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
			solver.write_ocl_runtime_config(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;

			for (size_t tensor_index = 0; tensor_index < heom_config.dipole_tensor_prefactors().size(); ++tensor_index) {
				for (int_t t_3_index = 0; t_3_index < t_3_iterations; ++t_3_index) {
					const int_t t_3 = t_3_index * steps_per_iteration;

					// re-init instance
					heom_instance.null_hierarchy();
					heom_instance.set_hierarchy_top();
					// re-init solver
					solver.reset();
					++solver_run; // for progress estimate

					// TODO: optimise: multiply top only on host-side (synchronise host/device!) => think through where possible
					auto get_dipole_matrix = [&](size_t spec_index) {
						return heom::get_dipole_matrix_for_pathway(pathway_spec, spec_index, heom_config.system_sites(), heom_config, tensor_index);
					};

					// lambda for generic dipole multiplication with plus/minus, from left/right
					auto dipole_mult = [&](size_t spec_index) {
						const auto dipole_matrix = get_dipole_matrix(spec_index);

						// multiple from left or right on hierarchy according to pathway specification
						if (pathway_spec.pathway()[spec_index].second ==
						    heom::pathway_dipole_side_t::left)
							solver.hierarchy_mmult_left(dipole_matrix.data());
						else // must be right
							solver.hierarchy_mmult_right(dipole_matrix.data());
					};

					// first dipole multiplication from specification
					dipole_mult(0);

					// propagate t_3 steps
					solver.step_forward(t_3);

					// second dipole multiplication from specification
					dipole_mult(1);

					// propagate delay steps
					solver.step_forward(heom_config.spectra_steps_t_delay());

					// third dipole multiplication from specification
					dipole_mult(2);

					// propagate additional t_1 steps and observe

					// setup observer with pre-scaled dipole matrix
					auto dipole_matrix = get_dipole_matrix(3);
					dipole_matrix.scale(heom::get_dipole_tensor_prefactor(pathway_spec, heom_config, tensor_index));
					heom::matrix_trace_observer<solver_t> trace_observer(complete_graph, solver, dipole_matrix.data()); // use last (fourth) polarization for trace, i.e. index 3

					// generate first observation with initial value
					observations[t_3_index][0] += trace_observer.observe_trace(0.0).trace();

					for (int_t t_1_index = 0; t_1_index < t_1_iterations; ++t_1_index) {
						// propagate
						solver.step_forward(steps_per_iteration);

						// after propagation, we are at:
						const auto current_step = (t_1_index + 1) * steps_per_iteration;
						const real_t current_time = current_step * heom_config.solver_step_size();

						// observe
						observations[t_3_index][t_1_index + 1] += trace_observer.observe_trace(current_time).trace();

						// update status
						if(!command_line.no_progress())
							heom::write_progress(current_step - 1, heom_config.spectra_steps_t_1(), solver_run, solver_runs, "Calculating two dimensional spectra: ", std::cout);
					} // t_1
				} // t_3
			} // tensor_index

			// write_complex_matrix(result_buffer_top, heom_instance.states(), std::cout);
			std::cout << "-------------------- Solver Runtime Summary ----------------" << std::endl;
			solver.write_runtime_summary(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;
		} // pathways

		// TODO: re-enable
		// average traces
//		for (auto& vec : observations)
//			for (auto& elem : vec)
//				elem = { elem.real() / num_dipole_matrices, elem.imag() / num_dipole_matrices };
//				//elem.avg(num_dipole_matrices);

		// write traces
		auto format = real_format;
		observation_file << "t_3" << default_delimiter << "t_1" << default_delimiter << "trace_real" << default_delimiter << "trace_imag" << std::endl;
		for (int_t t_3_index = 0; t_3_index < num_t_3_observations; ++t_3_index) {
			for (int_t t_1_index = 0; t_1_index < num_t_1_observations; ++t_1_index) {
				const auto& current_obs = observations[t_3_index][t_1_index];
				observation_file << format % (t_3_index * steps_per_iteration * heom_config.solver_step_size());
				observation_file << default_delimiter;
				observation_file << format % (t_1_index * steps_per_iteration * heom_config.solver_step_size());
				observation_file << default_delimiter;
				observation_file << format % current_obs.real();
				observation_file << default_delimiter;
				observation_file << format % current_obs.imag();
				observation_file << '\n';
			}
		}
		observation_file << std::flush; // tell the OS to write to disk

		std::cout << "-------------------- Memory Summary ------------------------" << std::endl;
//		std::cout << "local hierarchy buffer size:            "
//		          <<  boost::format("%11.2f MiB\n") % (heom_instance_thermal_state.size_hierarchy_byte() / 1024.0 / 1024.0);
//		std::cout << "local instance size:                    "
//		          <<  boost::format("%11.2f MiB\n") % (heom_instance_thermal_state.allocated_byte() / 1024.0 / 1024.0);
		std::cout << "max. heap via aligned::allocate(..):    "
		          << boost::format("%11.2f MiB") % (heom::memory::aligned::instance().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
		          << boost::format("count: %6i") % heom::memory::aligned::instance().allocations_max()
		          << ')' << std::endl;
//		std::cout << "max. OpenCL buffer allocations:         "
//		          <<  boost::format("%11.2f MiB") % (bicgstab.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
//		          << " ("
//		          << boost::format("count: %6i") % bicgstab.ocl_helper().allocations_max()
//		          << ')' << std::endl;
//		std::cout << "max. OpenCL buffer allocations:         "
//		          <<  boost::format("%11.2f MiB") % (solver.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
//		          << " ("
//		          << boost::format("count: %6i") % solver.ocl_helper().allocations_max()
//		          << ')' << std::endl;
		std::cout << "------------------------------------------------------------" << std::endl;

	} catch (...) {
		eptr = std::current_exception();
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	std::cout << "time in main():\t" << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}


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

#ifndef MTL_MATRIX_OPERATORS_INCLUDE
#define MTL_MATRIX_OPERATORS_INCLUDE

#include <boost/utility/enable_if.hpp>
#include <boost/mpl/and.hpp>

#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/utility/is_multi_vector_expr.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>
#include <boost/numeric/mtl/matrix/all_mat_expr.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>

namespace mtl { namespace matrix {

template <typename E1, typename E2>
inline mat_mat_plus_expr<E1, E2>
operator+ (const mat_expr<E1>& e1, const mat_expr<E2>& e2)
{
    // do not add matrices with inconsistent value types
    MTL_STATIC_ASSERT((boost::is_same<typename ashape::ashape<E1>::type,
				      typename ashape::ashape<E2>::type>::value), "Matrices have not consistent algebraic shape (i.e. nested types).");
    return mat_mat_plus_expr<E1, E2>(static_cast<const E1&>(e1), static_cast<const E2&>(e2));
}

// if enabled it has priority over previous functions because that performs upcast
template <typename E1, typename E2>
typename boost::enable_if_c<mtl::traits::is_multi_vector_expr<E1>::value && mtl::traits::is_multi_vector_expr<E2>::value, mv_mv_plus_expr<E1, E2> >::type
inline operator+(const E1& e1, const E2& e2)
{
    return mv_mv_plus_expr<E1, E2>(e1, e2);
}

// Specialization for multi_vector
// template <typename V1, typename V2>
// inline mv_mv_plus_expr<multi_vector<V1>, multi_vector<V2> >
// operator+(const multi_vector<V1>& m1, const multi_vector<V2>& m2)
// {
//     return mv_mv_plus_expr<multi_vector<V1>, multi_vector<V2> >(m1, m2);
// }

#if 0
// Planned for future optimizations on sums of dense matrix expressions
template <typename E1, typename E2>
inline dmat_dmat_plus_expr<E1, E2>
operator+ (const dmat_expr<E1>& e1, const dmat_expr<E2>& e2)
{
    // do not add matrices with inconsistent value types
    MTL_STATIC_ASSERT((boost::is_same<typename ashape::ashape<E1>::type,
				      typename ashape::ashape<E2>::type>::value), "Matrices have not consistent algebraic shape (i.e. nested types).");
    return dmat_dmat_plus_expr<E1, E2>(static_cast<const E1&>(e1), static_cast<const E2&>(e2));
}
#endif

template <typename E1, typename E2>
inline mat_mat_minus_expr<E1, E2>
operator- (const mat_expr<E1>& e1, const mat_expr<E2>& e2)
{
    // do not add matrices with inconsistent value types
    MTL_STATIC_ASSERT((boost::is_same<typename ashape::ashape<E1>::type,
				      typename ashape::ashape<E2>::type>::value), "Matrices have not consistent algebraic shape (i.e. nested types).");
    return mat_mat_minus_expr<E1, E2>(static_cast<const E1&>(e1), static_cast<const E2&>(e2));
}

// if enabled it has priority over previous functions because that performs upcast
template <typename E1, typename E2>
typename boost::enable_if_c<mtl::traits::is_multi_vector_expr<E1>::value && mtl::traits::is_multi_vector_expr<E2>::value, mv_mv_minus_expr<E1, E2> >::type
inline operator-(const E1& e1, const E2& e2)
{
    return mv_mv_minus_expr<E1, E2>(e1, e2);
}

template <typename E1, typename E2>
inline mat_mat_ele_times_expr<E1, E2>
ele_prod(const mat_expr<E1>& e1, const mat_expr<E2>& e2)
{
    // do not multiply matrices element-wise with inconsistent value types
    MTL_STATIC_ASSERT((boost::is_same<typename ashape::ashape<E1>::type,
				      typename ashape::ashape<E2>::type>::value), "Matrices do not have consistent algebraic shape (i.e. nested types).");
    return mat_mat_ele_times_expr<E1, E2>(static_cast<const E1&>(e1), static_cast<const E2&>(e2));
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_OPERATORS_INCLUDE