#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Sparse>

using Triplet = Eigen::Triplet<double>;
using Triplets = std::vector<Triplet>;
using Vector = Eigen::VectorXd;
using index_t = std::size_t;

class EllpackMat{
public:
  EllpackMat(const Triplets &triplets, index_t m, index_t n){
    this->m = m;
    this->n = n;
    this->maxcols = 0;

    // Sort and get maxcols in one go.
    std::vector<index_t> maxc(m);
    std::sort(triplets.begin(), triplets.end(),
              [this, &maxc](const Triplet &a, const Triplet &b) -> bool{ // <= this lambda constructor is apparently the problem
                ++maxc[a.row()];
                ++maxc[b.row()];
                this->maxcols = std::max(this->maxcols, std::max(maxc[a.row()], maxc[b.row()]));
                return a.row() < b.row() || (a.row () == b.row() && a.col() < b.col());
                return true;
              });
    
    // Allocate
    this->val = std::vector<double>(this->maxcols*m);
    this->col = std::vector<index_t>(this->maxcols*m);
    
    // Insert
    for(auto &triplet: triplets){
      index_t i = triplet.row()*this->maxcols+triplet.col();
      this->val[i] += triplet.value();
      this->col[i] = triplet.col();
    }
  };

  double operator() (index_t i, index_t j) const{
    for(index_t l=i*this->maxcols; l<(i+1)*this->maxcols; ++l){
      if(this->col[l] == j) return this->val[l];
    }
    return 0.;
  };

  void mvmult(const Vector &x, Vector &y) const{
    y = x;
    for(index_t i=0; i<this->m; ++i){
      for(index_t j=0; j<this->maxcols; ++j){
        int index = i*this->maxcols+j;
        // If we reached the end of the row, skip.
        if(j>0 && this->col[index] == 0)
          break;
        y(i) += this->val[index]*x(this->col[index]);
      }
    }
  };

  void mtvmult(const Vector &x, Vector &y) const{
    y = x;
    for(index_t j=0; j<this->maxcols; ++j){
      for(index_t i=0; i<this->m; ++i){
        int index = j*this->maxcols+i;
        y(i) += this->val[index]*x(this->col[index]);
      }
    }
  };

private:
  std::vector<double> val;
  std::vector<index_t> col;
  index_t maxcols;
  index_t m, n;
};

int main(){
  return 0;
}