//
// Copyright (c) 2019-2020 Kris Jusiak (kris at jusiak dot net)
//
// 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)
//
#include <boost/ut.hpp>
#include <fstream>
#include <numeric>
#include <streambuf>
#include <string>

template <class T>
class calculator {
 public:
  auto enter(const T& value) -> void { values_.push_back(value); }
  auto add() -> void {
    result_ = std::accumulate(std::cbegin(values_), std::cend(values_), T{});
  }
  auto sub() -> void {
    result_ = std::accumulate(std::cbegin(values_) + 1, std::cend(values_),
                              values_.front(), std::minus{});
  }
  auto get() const -> T { return result_; }

 private:
  std::vector<T> values_{};
  T result_{};
};

int main(int argc, const char** argv) {
  using namespace boost::ut;

  bdd::gherkin::steps steps = [](auto& steps) {
    steps.feature("Calculator") = [&] {
      steps.scenario("*") = [&] {
        steps.given("I have calculator") = [&] {
          calculator<int> calc{};
          steps.when("I enter {value}") = [&](int value) { calc.enter(value); };
          steps.when("I press add") = [&] { calc.add(); };
          steps.when("I press sub") = [&] { calc.sub(); };
          steps.then("I expect {value}") = [&](int result) {
            expect(that % calc.get() == result);
          };
        };
      };
    };
  };

  // clang-format off
  "Calculator"_test = steps |
    R"(
      Feature: Calculator

        Scenario: Addition
          Given I have calculator
           When I enter 40
           When I enter 2
           When I press add
           Then I expect 42

        Scenario: Subtraction
          Given I have calculator
           When I enter 4
           When I enter 2
           When I press sub
           Then I expect 2
    )";
  // clang-format on

  if (argc == 2) {
    const auto file = [](const auto path) {
      std::ifstream file{path};
      return std::string{(std::istreambuf_iterator<char>(file)),
                         std::istreambuf_iterator<char>()};
    };

    "Calculator"_test = steps | file(argv[1]);
  }
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2020 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include "BlockMatrixIterators.hpp"
#include <iostream>                                 // for operator<<, ostream
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include "SiconosAlgebra.hpp"
#include "SimpleMatrixFriends.hpp"
#include "BlockMatrix.hpp"
#include "SimpleMatrix.hpp"
#include "SiconosVector.hpp"
#include "SiconosMatrixSetBlock.hpp" // for setBlock
#include "SiconosAlgebraTools.hpp" // for isComparableTo
#include "Tools.hpp"
#include "SiconosException.hpp"

using  std::cout;
using std::endl;
using Siconos::Algebra::isComparableTo;

// =================================================
//                CONSTRUCTORS
// =================================================

BlockMatrix::BlockMatrix(const SiconosMatrix &m): SiconosMatrix(Siconos::BLOCK), _dimRow(0), _dimCol(0)
{
  _tabRow.reset(new Index());
  _tabCol.reset(new Index());
  if(m.isBlock())
  {
    const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
    unsigned int nbRows = m.numberOfBlocks(0);
    unsigned int nbCols = m.numberOfBlocks(1);
    _tabRow->reserve(nbRows);
    _tabCol->reserve(nbCols);

    // mat construction
    _mat.reset(new BlocksMat(nbRows, nbCols, nbRows * nbCols));

    unsigned int i, j;
    ConstBlocksIterator1 it1;
    ConstBlocksIterator2 it2;
    bool firstLoop = true;
    // We scan all the blocks of m ...
    for(it1 = mB._mat->begin1(); it1 != mB._mat->end1(); ++it1)
    {
      _dimRow += (*(it1.begin()))->size(0);
      _tabRow->push_back(_dimRow);
      for(it2 = it1.begin(); it2 != it1.end(); ++it2)
      {
        i = it2.index1();
        j = it2.index2();
        if((*it2)->isBlock())   // if the current matrix is a blockMatrix
          _mat->insert_element(i, j, std::shared_ptr<SiconosMatrix>(new BlockMatrix(**it2)));
        else
          _mat->insert_element(i, j, std::shared_ptr<SiconosMatrix>(new SimpleMatrix(**it2)));
        // _dimCol must be incremented only at first "column-loop"
        if(firstLoop)
        {
          _dimCol += (*it2)->size(1);
          _tabCol->push_back(_dimCol);
        }
      }
      firstLoop = false;
    }
  }
  else // if m is a SimpleMatrix
  {
    _tabRow->reserve(1);
    _tabCol->reserve(1);
    // _mat construction
    _mat.reset(new BlocksMat(1, 1, 1));
    _mat->insert_element(0, 0, std::shared_ptr<SiconosMatrix>(new SimpleMatrix(m)));

    _dimRow = m.size(0);
    _dimCol = m.size(1);
    _tabRow->push_back(_dimRow);
    _tabCol->push_back(_dimCol);
  }
}

BlockMatrix::BlockMatrix(const BlockMatrix &m): SiconosMatrix(Siconos::BLOCK), _dimRow(0), _dimCol(0)
{
  unsigned int nbRows = m.numberOfBlocks(0);
  unsigned int nbCols = m.numberOfBlocks(1);
  _tabRow.reset(new Index());
  _tabCol.reset(new Index());
  _tabRow->reserve(nbRows);
  _tabCol->reserve(nbCols);

  // _mat construction
  _mat.reset(new BlocksMat(nbRows, nbCols, nbRows * nbCols));

  unsigned int i, j;
  // We scan all the blocks of m ...
  ConstBlocksIterator1 it1;
  ConstBlocksIterator2 it2;
  bool firstLoop = true;
  // We scan all the blocks of m ...
  for(it1 = m._mat->begin1(); it1 != m._mat->end1(); ++it1)
  {
    _dimRow += (*(it1.begin()))->size(0);
    _tabRow->push_back(_dimRow);
    for(it2 = it1.begin(); it2 != it1.end(); ++it2)
    {
      i = it2.index1();
      j = it2.index2();
      if((*it2)->isBlock())   // if the current _matrix is a blockMatrix
        _mat->insert_element(i, j, std::shared_ptr<SiconosMatrix>(new BlockMatrix(**it2)));
      else
        _mat->insert_element(i, j, std::shared_ptr<SiconosMatrix>(new SimpleMatrix(**it2)));

      // _dimCol must be incremented only at first "column-loop"
      if(firstLoop)
      {
        _dimCol += (*it2)->size(1);
        _tabCol->push_back(_dimCol);
      }
    }
    firstLoop = false;
  }
}

BlockMatrix::BlockMatrix(const std::vector<SP::SiconosMatrix >& m, unsigned int row, unsigned int col):
  SiconosMatrix(Siconos::BLOCK), _dimRow(0), _dimCol(0)
{
  if(m.size() != (row * col))
    THROW_EXCEPTION("number of blocks inconsistent with provided dimensions.");

  _tabRow.reset(new Index());
  _tabCol.reset(new Index());
  _tabRow->reserve(row);
  _tabCol->reserve(col);

  // _mat construction
  _mat.reset(new BlocksMat(row, col, row * col));

  unsigned int k = 0;
  bool firstRowLoop = true;
  bool firstColLoop = true;

  for(unsigned int i = 0; i < row; ++i)
  {
    for(unsigned int j = 0; j < col; ++j)
    {
      (*_mat)(i, j) = m[k++];

      // _dimCol must be incremented only at first "column-loop"
      if(firstColLoop)
      {
        _dimCol += m[k - 1]->size(1);
        _tabCol->push_back(_dimCol);
      }
      if(firstRowLoop)
      {
        _dimRow += m[k - 1]->size(0);
        _tabRow->push_back(_dimRow);
        firstRowLoop = false;
      }
    }
    firstColLoop = false;
    firstRowLoop = true;
  }
}

BlockMatrix::BlockMatrix(SP::SiconosMatrix A, SP::SiconosMatrix B, SP::SiconosMatrix C, SP::SiconosMatrix D):
  SiconosMatrix(Siconos::BLOCK), _dimRow(0), _dimCol(0)
{
  if(A->size(0) != B->size(0) || C->size(0) != D->size(0) ||  A->size(1) != C->size(1) ||  B->size(1) != D->size(1))
    THROW_EXCEPTION("inconsistent sizes between A, B, C or D SiconosMatrices.");

  // _mat = [ A B ]
  //       [ C D ]

  // _mat construction
  _mat.reset(new BlocksMat(2, 2, 4));

  _tabRow.reset(new Index());
  _tabCol.reset(new Index());
  _tabRow->reserve(2);
  _tabCol->reserve(2);

  (*_mat)(0, 0) = A;
  (*_mat)(0, 1) = B;
  (*_mat)(1, 0) = C;
  (*_mat)(1, 1) = D;
  _dimRow = A->size(0);
  _tabRow->push_back(_dimRow);
  _dimRow += C->size(0);
  _tabRow->push_back(_dimRow);
  _dimCol = A->size(1);
  _tabCol->push_back(_dimCol);
  _dimCol += B->size(1);
  _tabCol->push_back(_dimCol);

}

BlockMatrix::~BlockMatrix()
{

  _mat->clear();

  _tabRow->clear();
  _tabCol->clear();
}

// =================================================
//    get number of blocks
// =================================================

unsigned int BlockMatrix::numberOfBlocks(unsigned int dim) const
{
  if(dim == 0)
    return _tabRow->size();
  else
    return _tabCol->size();
}

// =================================================
//        get Ublas component (dense ...)
// =================================================

// return the boost dense _matrix of the block (i, j)
const DenseMat  BlockMatrix::getDense(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp;
  tmp = (*_mat)(row, col);

  if(tmp->num() != Siconos::DENSE)
    THROW_EXCEPTION("the matrix at (row, col) is not a Dense matrix");

  return (tmp->getDense());
}

// return the boost triangular matrix of the block (i, j)
const TriangMat BlockMatrix::getTriang(unsigned int row, unsigned int col) const
{
  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::TRIANGULAR)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Triangular matrix");
  }
  return (tmp->getTriang());
}

// return the boost symmetric matrix of the block (i, j)
const SymMat BlockMatrix::getSym(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SYMMETRIC)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Symmmetric matrix");
  }
  return (tmp->getSym());
}

// return the boost sparse matrix of the block (i, j)
const SparseMat  BlockMatrix::getSparse(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SPARSE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Sparse matrix");
  }
  return (tmp->getSparse());
}

// return the boost sparse matrix of the block (i, j)
const SparseCoordinateMat  BlockMatrix::getSparseCoordinate(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SPARSE_COORDINATE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Sparse matrix");
  }
  return (tmp->getSparseCoordinate());
}
// return the boost banded matrix of the block (i, j)
const BandedMat  BlockMatrix::getBanded(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::BANDED)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Banded matrix");
  }
  return (tmp->getBanded());
}

// return the boost zero matrix of the block (i, j)
const ZeroMat  BlockMatrix::getZero(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::ZERO)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Zero matrix");
  }
  return (tmp->getZero());
}

// return the boost identity matrix of the block (i, j)
const IdentityMat  BlockMatrix::getIdentity(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::IDENTITY)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Identity matrix");
  }
  return (tmp->getIdentity());
}

// The following functions return the corresponding pointers
DenseMat*  BlockMatrix::dense(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::DENSE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Dense matrix");
  }

  return (tmp->dense());
}

TriangMat* BlockMatrix::triang(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::TRIANGULAR)
  {
    THROW_EXCEPTION("The matrix at (row, col) is not a Triangular matrix");
  }
  return (tmp->triang());
}
SymMat* BlockMatrix::sym(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SYMMETRIC)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Symmmetric matrix");
  }
  return (tmp->sym());
}

SparseMat*  BlockMatrix::sparse(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SPARSE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Sparse matrix");
  }
  return (tmp->sparse());
}
SparseCoordinateMat*  BlockMatrix::sparseCoordinate(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SPARSE_COORDINATE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Sparse coordinate matrix");
  }
  return (tmp->sparseCoordinate());
}

BandedMat*  BlockMatrix::banded(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::BANDED)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Banded matrix");
  }
  return (tmp->banded());
}

ZeroMat*  BlockMatrix::zero_mat(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::ZERO)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Zero matrix");
  }
  return (tmp->zero_mat(row, col));
}

IdentityMat*  BlockMatrix::identity(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::IDENTITY)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Identity matrix");
  }
  return (tmp->identity());
}

double* BlockMatrix::getArray(unsigned int i, unsigned int j) const
{
  SP::SiconosMatrix tmp = (*_mat)(i, j);
  return tmp->getArray();
}

// ===========================
//       fill matrix
// ===========================

void BlockMatrix::zero()
{
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      (*it2)->zero();
    }
  }
}

void BlockMatrix::randomize()
{
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      (*it2)->randomize();
    }
  }
}

void BlockMatrix::randomize_sym()
{
  THROW_EXCEPTION("not yet implemented for block matrices.");
}

void BlockMatrix::eye()
{
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;

  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      if(it2.index1() == it2.index2())
        (*it2)->eye();
      else
        (*it2)->zero();
    }
  }
}

unsigned int BlockMatrix::size(unsigned int index) const
{
  if(index == 0) return _dimRow;
  else return _dimCol;
};

//=======================
// set matrix dimensions
//=======================

void BlockMatrix::resize(unsigned int, unsigned int, unsigned int, unsigned int, bool)
{
  THROW_EXCEPTION("forbidden for block matrices.");
}

//=======================
//       get norm
//=======================

double BlockMatrix::normInf()const
{
  double sum = 0, norm = 0;
  for(unsigned int i = 0; i < size(0); i++)
  {
    for(unsigned int j = 0; j < size(1); j++)
    {
      sum += (*this)(i, j);
    }
    if(fabs(sum) > norm) norm = fabs(sum);
    sum = 0;
  }
  return norm;
}

//=====================
// screen display
//=====================

void BlockMatrix::display(void)const
{
  std::cout << "==========> BlockMatrix (" << numberOfBlocks(0) << " X " << numberOfBlocks(1) << " blocks): " << std::endl;
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      (*it2)->display();
    }
  }
  std::cout << "===========================================================================================" << std::endl;
}
void BlockMatrix::displayExpert(bool brief)const
{
  std::cout << "==========> BlockMatrix (" << numberOfBlocks(0) << " X " << numberOfBlocks(1) << " blocks): " << std::endl;
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      (*it2)->displayExpert(brief);
    }
  }
  std::cout << "===========================================================================================" << std::endl;
}

//=====================
// convert to a string
//=====================

std::string BlockMatrix::toString() const
{
  return ::toString(*this);
}

//=====================
// convert to an ostream
//=====================

std::ostream& operator<<(std::ostream& os, const BlockMatrix& bm)
{
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  os << "[" << bm.numberOfBlocks(0) << "," << bm.numberOfBlocks(1) << "](";
  for(it = bm._mat->begin1(); it != bm._mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      if(it2 != it.begin()) os << ",";
      if(*it2) os << **it2;
      else os << "(nil)";
    }
  }
  os << ")";
  return os;
}

//=============================
// Elements access (get or set)
//=============================

double& BlockMatrix::operator()(unsigned int row, unsigned int col)
{
  unsigned int nbRow = 0;
  unsigned int nbCol = 0;

  while(row >= (*_tabRow)[nbRow] && nbRow < _tabRow->size())
    nbRow ++;

  while(col >= (*_tabCol)[nbCol] && nbCol < _tabCol->size())
    nbCol ++;

  unsigned int posRow = row;
  unsigned int posCol = col;

  if(nbRow != 0)
    posRow -= (*_tabRow)[nbRow - 1];
  if(nbCol != 0)
    posCol -= (*_tabCol)[nbCol - 1];

  SP::SiconosMatrix tmp = (*_mat)(nbRow, nbCol);
  return (*tmp)(posRow, posCol);
}

double BlockMatrix::operator()(unsigned int row, unsigned int col) const
{

  unsigned int nbRow = 0;
  unsigned int nbCol = 0;

  while(row >= (*_tabRow)[nbRow] && nbRow < _tabRow->size())
    nbRow ++;

  while(col >= (*_tabCol)[nbCol] && nbCol < _tabCol->size())
    nbCol ++;

  unsigned int posRow = row;
  unsigned int posCol = col;

  if(nbRow != 0)
    posRow -= (*_tabRow)[nbRow - 1];
  if(nbCol != 0)
    posCol -= (*_tabCol)[nbCol - 1];

  SP::SiconosMatrix tmp = (*_mat)(nbRow, nbCol);
  return (*tmp)(posRow, posCol);
}

double BlockMatrix::getValue(unsigned int row, unsigned int col) const
{
  unsigned int nbRow = 0;
  unsigned int nbCol = 0;

  while(row >= (*_tabRow)[nbRow] && nbRow < _tabRow->size())
    nbRow ++;

  while(col >= (*_tabCol)[nbCol] && nbCol < _tabCol->size())
    nbCol ++;

  unsigned int posRow = row;
  unsigned int posCol = col;

  if(nbRow != 0)
    posRow -= (*_tabRow)[nbRow - 1];
  if(nbCol != 0)
    posCol -= (*_tabCol)[nbCol - 1];

  SP::SiconosMatrix tmp = (*_mat)(nbRow, nbCol);
  return (*tmp)(posRow, posCol);
}

void BlockMatrix::setValue(unsigned int row, unsigned int col, double value)
{
  unsigned int nbRow = 0;
  unsigned int nbCol = 0;

  while(row >= (*_tabRow)[nbRow] && nbRow < _tabRow->size())
    nbRow ++;

  while(col >= (*_tabCol)[nbCol] && nbCol < _tabCol->size())
    nbCol ++;

  unsigned int posRow = row;
  unsigned int posCol = col;

  if(nbRow != 0)
    posRow -= (*_tabRow)[nbRow - 1];
  if(nbCol != 0)
    posCol -= (*_tabCol)[nbCol - 1];

  SP::SiconosMatrix tmp = (*_mat)(nbRow, nbCol);
  (*tmp)(posRow, posCol) = value;
}

//============================================
// Access (get or set) to blocks of elements
//============================================

void BlockMatrix::getRow(unsigned int r, SiconosVector &v) const
{
  unsigned int numRow = 0, posRow = r, start = 0, stop = 0;

  if(r > _dimRow)
    THROW_EXCEPTION("row number is out of range");

  // Verification of the size of the result vector
  if(v.size() != _dimCol)
    THROW_EXCEPTION("inconsistent sizes");

  // Find the row-block number where "r" is
  while(r >= (*_tabRow)[numRow] && numRow < _tabRow->size())
    numRow ++;

  // Computation of the value of the index row into this block
  if(numRow != 0)
    posRow -= (*_tabRow)[numRow - 1];

  for(unsigned int j = 0; j < _tabCol->size(); j++)
  {
    start = stop;
    SP::SiconosMatrix tmp = (*_mat)(numRow, j);
    stop += tmp->size(1);
    ublas::subrange(*(v.dense()), start, stop) = ublas::row(*(tmp->dense()), posRow);
  }
}

void BlockMatrix::getCol(unsigned int c, SiconosVector &v) const
{
  unsigned int numCol = 0, posCol = c, start = 0, stop = 0;

  if(c > _dimCol)
    THROW_EXCEPTION("column number is out of range");

  // Verification of the size of the result vector
  if(v.size() != _dimRow)
    THROW_EXCEPTION("inconsistent sizes");

  // Find the column-block number where "c" is
  while(c >= (*_tabCol)[numCol] && numCol < _tabCol->size())
    numCol ++;

  // Computation of the value of the index column into this block
  if(numCol != 0)
    posCol -= (*_tabCol)[numCol - 1];

  for(unsigned int i = 0; i < _tabRow->size(); i++)
  {
    start = stop;
    SP::SiconosMatrix tmp = (*_mat)(i, numCol);
    stop += tmp->size(0);
    ublas::subrange(*(v.dense()), start, stop) = ublas::column(tmp->getDense(), posCol);
  }
}

void BlockMatrix::setRow(unsigned int r, const SiconosVector &v)
{

  unsigned int numRow = 0, posRow = r, start = 0, stop = 0;

  if(v.size() != _dimCol)
    THROW_EXCEPTION("inconsistent sizes");

  while(r >= (*_tabRow)[numRow] && numRow < _tabRow->size())
    numRow ++;

  if(numRow != 0)
    posRow -= (*_tabRow)[numRow - 1];

  for(unsigned int j = 0; j < _tabCol->size(); j++)
  {
    start = stop;
    SP::SiconosMatrix tmp = (*_mat)(numRow, j);
    stop += tmp->size(1);
    ublas::row(*(tmp->dense()), posRow) = ublas::subrange(*(v.dense()), start, stop);
  }
}

void BlockMatrix::setCol(unsigned int col, const SiconosVector &v)
{

  unsigned int numCol = 0, posCol = col, start = 0, stop = 0;

  if(v.size() != _dimRow)
    THROW_EXCEPTION("inconsistent sizes");

  while(col >= (*_tabCol)[numCol] && numCol < _tabCol->size())
    numCol ++;

  if(numCol != 0)
    posCol -= (*_tabCol)[numCol - 1];

  for(unsigned int i = 0; i < _tabRow->size(); i++)
  {
    start = stop;
    SP::SiconosMatrix tmp = (*_mat)(i, numCol);
    stop += tmp->size(0);
    ublas::column(*(tmp->dense()), posCol) = ublas::subrange(*(v.dense()), start, stop);
  }
}

void BlockMatrix::addSimple(unsigned int& indRow, unsigned int& indCol, const SiconosMatrix& m)
{
  // Add a part of m (starting from (indRow,indCol) to the current matrix.
  // m must be a SimpleMatrix.

  // At the end of the present function, indRow (resp. indCol) is equal to indRow + the corresponding dimension of the added sub-matrix.

  unsigned int row = m.size(0) - indRow; // number of rows of the block to be added.
  unsigned int col = m.size(1) - indCol; // number of columns of the block to be added.
  unsigned int initCol = indCol;

  if(row > _dimRow || col > _dimCol) THROW_EXCEPTION("invalid ranges");

  Siconos::UBLAS_TYPE numM = m.num();

  // iterators through this
  BlocksMat::iterator1 it1;
  BlocksMat::iterator2 it2;
  unsigned int currentRow = 0, currentCol = 0, currentNum;
  for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
  {
    for(it2 = it1.begin(); it2 != it1.end(); ++it2)
    {
      if((*it2)->isBlock())   // if the sub-block is also a BlockMatrix ...
        (std::static_pointer_cast<BlockMatrix>(*it2))->addSimple(indRow, indCol, m);

      else
      {
        currentCol = (*it2)->size(1);
        currentRow = (*it2)->size(0);
        currentNum = (*it2)->num();
        if(numM != currentNum) THROW_EXCEPTION("inconsistent types.");

        if(numM == Siconos::DENSE)
          noalias(*(*it2)->dense()) += ublas::subrange(*m.dense(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::TRIANGULAR)
          noalias(*(*it2)->triang()) += ublas::subrange(*m.triang(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::SYMMETRIC)
          noalias(*(*it2)->sym()) += ublas::subrange(*m.sym(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::SPARSE)
          noalias(*(*it2)->sparse()) += ublas::subrange(*m.sparse(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::BANDED)
          noalias(*(*it2)->banded()) += ublas::subrange(*m.banded(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::ZERO) {}
        else
          THROW_EXCEPTION("inconsistent types.");
      }
      indCol += currentCol;
    }
    indRow += currentRow;
    indCol = initCol;
  }
}

void BlockMatrix::subSimple(unsigned int& indRow, unsigned int& indCol, const SiconosMatrix& m)
{
  // subtract a part of m (starting from (indRow,indCol) to the current matrix.
  // m must be a SimpleMatrix.

  // At the end of the present function, indRow (resp. indCol) is equal to indRow + the corresponding dimension of the subtracted sub-matrix.

  unsigned int row = m.size(0) - indRow; // number of rows of the block to be added.
  unsigned int col = m.size(1) - indCol; // number of columns of the block to be added.
  unsigned int initCol = indCol;
  if(row > _dimRow || col > _dimCol) THROW_EXCEPTION("invalid ranges");

  Siconos::UBLAS_TYPE numM = m.num();

  // iterators through this
  BlocksMat::iterator1 it1;
  BlocksMat::iterator2 it2;
  unsigned int currentRow = 0, currentCol = 0;
  Siconos::UBLAS_TYPE currentNum;
  for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
  {
    for(it2 = it1.begin(); it2 != it1.end(); ++it2)
    {
      if((*it2)->isBlock())   // if the sub-block is also a BlockMatrix ...
        (std::static_pointer_cast<BlockMatrix>(*it2))->subSimple(indRow, indCol, m);

      else
      {
        currentCol = (*it2)->size(1);
        currentRow = (*it2)->size(0);
        currentNum = (*it2)->num();
        if(numM != currentNum) THROW_EXCEPTION("inconsistent types.");

        if(numM == Siconos::DENSE)
          noalias(*(*it2)->dense()) -= ublas::subrange(*m.dense(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::TRIANGULAR)
          noalias(*(*it2)->triang()) -= ublas::subrange(*m.triang(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::SYMMETRIC)
          noalias(*(*it2)->sym()) -= ublas::subrange(*m.sym(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::SPARSE)
          noalias(*(*it2)->sparse()) -= ublas::subrange(*m.sparse(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::BANDED)
          noalias(*(*it2)->banded()) -= ublas::subrange(*m.banded(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::ZERO) {}
        else
          THROW_EXCEPTION("inconsistent types.");
      }
      indCol += currentCol;
    }
    indRow += currentRow;
    indCol = initCol;
  }
}

//===============
//  Assignment
//===============

BlockMatrix& BlockMatrix::operator = (const SiconosMatrix &m)
{
  if(&m == this) return *this;  // auto-assignment.

  if(m.size(0) != _dimRow || m.size(1) != _dimCol)
    THROW_EXCEPTION("Left and Right values have inconsistent sizes.");

  // Warning: we do not reallocate the blocks, but only copy the values. This means that
  // all blocks are already allocated and that dim of m and mat are to be consistent.
  // Thus, _tabRow and _tabCol remains unchanged.
  // If m and mat are not "block-consistent", we use the () operator for a component-wise copy.

  if(m.isBlock())
  {
    if(isComparableTo(*this, m))
    {
      const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
      // iterators through this
      BlocksIterator1 it1;
      BlocksIterator2 it2;
      // iterators through m
      ConstBlocksIterator1 itM1 = mB._mat->begin1();
      ConstBlocksIterator2 itM2;

      for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
      {
        itM2 = itM1.begin();
        for(it2 = it1.begin(); it2 != it1.end(); ++it2)
        {
          (**it2) = (**itM2);
          itM2++; // increment column pos. in m.
        }
        itM1++; // increment row pos. in m.
      }
    }
    else
    {
      for(unsigned int i = 0; i < _dimRow; ++i)
        for(unsigned int j = 0; j < _dimCol; ++j)
          (*this)(i, j) = m(i, j);
    }
  }
  else // if m is a SimpleMatrix
  {
    BlocksIterator1 it;
    BlocksIterator2 it2;
    unsigned int posRow = 0;
    unsigned int posCol = 0;
    Index subDim(2);
    Index subPos(4);

    for(it = _mat->begin1(); it != _mat->end1(); ++it)
    {
      for(it2 = it.begin(); it2 != it.end(); ++it2)
      {
        // a sub-block of m is copied into this
        subDim[0] = (*it2)->size(0);
        subDim[1] = (*it2)->size(1);
        subPos[0] = posRow;
        subPos[1] = posCol;
        subPos[2] = 0;
        subPos[3] = 0;
        setBlock(createSPtrConstSiconosMatrix(m), *it2, subDim, subPos);
        posCol += subDim[1];
      }
      posRow += (*it)->size(0);
      posCol = 0;
    }
  }

  return *this;
}

BlockMatrix& BlockMatrix::operator = (const BlockMatrix &m)
{
  if(&m == this) return *this;  // auto-assignment.

  if(m.size(0) != _dimRow || m.size(1) != _dimCol)
    THROW_EXCEPTION("Left and Right values have inconsistent sizes.");

  // Warning: we do not reallocate the blocks, but only copy the values. This means that
  // all blocks are already allocated and that dim of m and mat are to be consistent.
  // Thus, _tabRow and _tabCol remains unchanged.
  // If m and mat are not "block-consistent", we use the () operator for a componet-wise copy.

  if(isComparableTo(*this, m))
  {
    const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
    // iterators through this
    BlocksIterator1 it1;
    BlocksIterator2 it2;
    // iterators through m
    ConstBlocksIterator1 itM1 = mB._mat->begin1();
    ConstBlocksIterator2 itM2;

    for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
    {
      itM2 = itM1.begin();
      for(it2 = it1.begin(); it2 != it1.end(); ++it2)
      {
        (**it2) = (**itM2);
        itM2++; // increment column pos. in m.
      }
      itM1++; // increment row pos. in m.
    }
  }
  else
  {
    for(unsigned int i = 0; i < _dimRow; ++i)
      for(unsigned int j = 0; j < _dimCol; ++j)
        (*this)(i, j) = m(i, j);
  }
  return *this;
}

BlockMatrix& BlockMatrix::operator = (const DenseMat &m)
{
  THROW_EXCEPTION("Not yet implemented.");
  return *this;
}

//=================================
// Op. and assignment (+=, -= ... )
//=================================

BlockMatrix& BlockMatrix::operator += (const SiconosMatrix &m)
{
  if(&m == this)
  {
    BlocksMat::iterator1 it1;
    BlocksMat::iterator2 it2;
    for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
    {
      for(it2 = it1.begin(); it2 != it1.end(); ++it2)
      {
        **it2 += **it2;
      }
    }
    return *this;
  }

  if(m.size(0) != _dimRow || m.size(1) != _dimCol)
    THROW_EXCEPTION("Left and Right values have inconsistent sizes.");

  if(m.isBlock())
  {
    if(isComparableTo(m, *this))
    {
      const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
      // iterators through this
      BlocksMat::iterator1 it1;
      BlocksMat::iterator2 it2;
      // iterators through m
      BlocksMat::const_iterator1 itM1 = mB._mat->begin1();
      BlocksMat::const_iterator2 itM2;

      for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
      {
        itM2 = itM1.begin();
        for(it2 = it1.begin(); it2 != it1.end(); ++it2)
        {
          (**it2) += (**itM2);
          itM2++; // increment column pos. in m.
        }
        itM1++; // increment row pos. in m.
      }
    }
    else
    {
      for(unsigned int i = 0; i < _dimRow; ++i)
        for(unsigned int j = 0; j < _dimCol; ++j)
          (*this)(i, j) += m(i, j);
    }
  }
  else // if m is a SimpleMatrix
  {
    unsigned int indRow = 0, indCol = 0;
    addSimple(indRow, indCol, m); // a sub-block of m is added to each block of this.
  }
  return *this;
}

BlockMatrix& BlockMatrix::operator -= (const SiconosMatrix &m)
{
  if(&m == this)
  {
    BlocksMat::iterator1 it1;
    BlocksMat::iterator2 it2;
    for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
    {
      for(it2 = it1.begin(); it2 != it1.end(); ++it2)
      {
        **it2 -= **it2;
      }
    }
    return *this;
  }

  if(m.size(0) != _dimRow || m.size(1) != _dimCol)
    THROW_EXCEPTION("Left and Right values have inconsistent sizes.");

  if(m.isBlock())
  {
    if(isComparableTo(m, *this))
    {
      const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
      // iterators through this
      BlocksMat::iterator1 it1;
      BlocksMat::iterator2 it2;
      // iterators through m
      BlocksMat::const_iterator1 itM1 = mB._mat->begin1();
      BlocksMat::const_iterator2 itM2;

      for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
      {
        itM2 = itM1.begin();
        for(it2 = it1.begin(); it2 != it1.end(); ++it2)
        {
          (**it2) -= (**itM2);
          itM2++; // increment column pos. in m.
        }
        itM1++; // increment row pos. in m.
      }
    }
    else
    {
      for(unsigned int i = 0; i < _dimRow; ++i)
        for(unsigned int j = 0; j < _dimCol; ++j)
          (*this)(i, j) -= m(i, j);
    }
  }
  else // if m is a SimpleMatrix
  {
    unsigned int indRow = 0, indCol = 0;
    subSimple(indRow, indCol, m); // a sub-block of m is subtracted to each block of this.
  }
  return *this;
}

void BlockMatrix::trans()
{
  THROW_EXCEPTION("not yet implemented.");
}

void BlockMatrix::trans(const SiconosMatrix &m)
{
  THROW_EXCEPTION("not yet implemented.");
}

void BlockMatrix::PLUFactorizationInPlace()
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}
void BlockMatrix::Factorize()
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}

void BlockMatrix::PLUInverseInPlace()
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}

void BlockMatrix::PLUForwardBackwardInPlace(SiconosMatrix &B)
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}
void BlockMatrix::Solve(SiconosMatrix &B)
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}

void BlockMatrix::PLUForwardBackwardInPlace(SiconosVector &B)
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}
void BlockMatrix::Solve(SiconosVector &B)
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}

SP::SiconosMatrix BlockMatrix::block(unsigned int row, unsigned int col)
{
  return (*_mat)(row, col);
}

SPC::SiconosMatrix BlockMatrix::block(unsigned int row, unsigned int col) const
{
  return std::shared_ptr<SiconosMatrix>((*_mat)(row, col));
}

size_t BlockMatrix::nnz(double tol)
{
  size_t nnz = 0;
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
      nnz += (**it2).nnz();
  }
  return nnz;
}


/***
 * @file arma_extend.hpp
 * @author Ryan Curtin
 *
 * Include Armadillo extensions which currently are not part of the main
 * Armadillo codebase.
 *
 */
#ifndef MLPACK_CORE_ARMA_EXTEND_ARMA_EXTEND_HPP
#define MLPACK_CORE_ARMA_EXTEND_ARMA_EXTEND_HPP

// Add batch constructor for sparse matrix (if version <= 3.810.0).
#define ARMA_EXTRA_SPMAT_PROTO mlpack/core/arma_extend/SpMat_extra_bones.hpp
#define ARMA_EXTRA_SPMAT_MEAT  mlpack/core/arma_extend/SpMat_extra_meat.hpp

// Add row_col_iterator and row_col_const_iterator for Mat.
#define ARMA_EXTRA_MAT_PROTO mlpack/core/arma_extend/Mat_extra_bones.hpp
#define ARMA_EXTRA_MAT_MEAT mlpack/core/arma_extend/Mat_extra_meat.hpp

// Add boost serialization for Cube.
#define ARMA_EXTRA_CUBE_PROTO mlpack/core/arma_extend/Cube_extra_bones.hpp
#define ARMA_EXTRA_CUBE_MEAT mlpack/core/arma_extend/Cube_extra_meat.hpp

// Manually set ARMA_{64,32}BIT_WORD for _WIN64 or win32
#if defined(_MSC_VER)
    #ifdef _WIN64
        #define ARMA_64BIT_WORD
        #ifdef ARMA_32BIT_WORD
            #undef ARMA_32BIT_WORD
        #endif
    #else
        #define ARMA_32BIT_WORD
        #ifdef ARMA_64BIT_WORD
            #undef ARMA_64BIT_WORD
        #endif
    #endif
#endif

// Force definition of old HDF5 API.  Thanks to Mike Roberts for helping find
// this workaround.
#if !defined(H5_USE_110_API)
  #define H5_USE_110_API
#endif

// Include everything we'll need for serialize().
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/array.hpp>

#include <armadillo>

#endif


//
//  OpenNIConnector.cpp
//  gl
//
//  Created by Fabian on 5/5/13.
//
//

#include <XnOpenNI.h>
#include <XnCodecIDs.h>
#include <XnCppWrapper.h>
#include <XnPropNames.h>

//#include <ni2/OpenNI.h>

#include "core/file_functions.hpp"
#include "gl/Texture.hpp"
#include "gl/Buffer.hpp"
#include <boost/timer/timer.hpp>
#include "OpenNIConnector.h"

using namespace std;

inline float raw_depth_to_meters(int raw_depth)
{
    if (raw_depth < 2047)
    {
        return 1.0 / (raw_depth * -0.0030711016 + 3.3309495161);
    }
    return 0;
}

namespace kinski{ namespace gl{

    struct OpenNIConnector::Obj
    {
        xn::Context m_context;
        xn::ScriptNode m_scriptNode;
        xn::DepthGenerator m_depthGenerator;
        xn::UserGenerator m_userGenerator;
        xn::Player m_player;
        std::vector<uint8_t> m_pixel_buffer;
        std::vector<float> m_histogram_buffer;

        std::vector<glm::vec4> m_point_buffer;

        Obj(){}

        ~Obj()
        {
            m_scriptNode.Release();
            m_depthGenerator.Release();
            m_userGenerator.Release();
            m_player.Release();
            m_context.Release();
        }

        static void XN_CALLBACK_TYPE new_user(xn::UserGenerator& /*generator*/, XnUserID nId,
                                              void* cookie);
        static void XN_CALLBACK_TYPE lost_user(xn::UserGenerator& /*generator*/, XnUserID nId,
                                               void* cookie);
        static void XN_CALLBACK_TYPE calibration_start(xn::SkeletonCapability& /*capability*/,
                                                       XnUserID nId, void* cookie);
        static void XN_CALLBACK_TYPE calibration_complete(xn::SkeletonCapability& /*capability*/,
                                                          XnUserID nId, XnCalibrationStatus eStatus,
                                                   void* cookie);
        static void XN_CALLBACK_TYPE pose_detected(xn::PoseDetectionCapability& /*capability*/,
                                                   const XnChar* strPose, XnUserID nId, void* cookie);
    };

    OpenNIConnector::OpenNIConnector():
    m_new_frame(false),
    m_depth_texture(640, 480, gl::Texture::Format()),
    m_running(false),
    m_live_input(Property_<bool>::create("Live input", false)),
    m_config_path(Property_<string>::create("Config path", "ni_config.xml")),
    m_oni_path(Property_<string>::create("Oni path", ""))
    {
        set_name("OpenNIConnector");
        register_property(m_live_input);
        register_property(m_config_path);
        register_property(m_oni_path);

        m_user_colors.resize(50);
        for (int i= 0; i < m_user_colors.size(); ++i)
        {m_user_colors[i] = gl::Color(glm::vec3(random(0.f, 1.f), .2f, random(0.f, 1.f)), 1.f);}
        m_user_colors.push_back(gl::Color(1));
    }

    OpenNIConnector::~OpenNIConnector()
    {
        stop();
    }

    void OpenNIConnector::init()
    {
        LOG_DEBUG<<"initializing OpenNI ...";
        m_obj.reset(new Obj);
        XnStatus ni_status = XN_STATUS_OK;

        // hook up a attached camera
        if(*m_live_input)
        {
            xn::EnumerationErrors errors;
            try
            {
                ni_status = m_obj->m_context.InitFromXmlFile(fs::search_file(*m_config_path).c_str(),
                                                             m_obj->m_scriptNode, &errors);
            } catch (fs::FileNotFoundException &e){LOG_ERROR<<e.what();}

            if(ni_status != XN_STATUS_OK){throw OpenNIException(xnGetStatusString(ni_status));}
        }
        // load oni file
        else
        {
            if(m_oni_path->value().empty()){throw OpenNIException("empty .oni path");};

            m_obj->m_context.Init();
            try
            {
                ni_status = m_obj->m_context.OpenFileRecording(fs::search_file(*m_oni_path).c_str(),
                                                               m_obj->m_player);
            }catch (fs::FileNotFoundException &e){LOG_ERROR<<e.what();}
            if(ni_status != XN_STATUS_OK){throw OpenNIException(xnGetStatusString(ni_status));}
        }

        // create a depth generator
        ni_status = m_obj->m_context.FindExistingNode(XN_NODE_TYPE_DEPTH, m_obj->m_depthGenerator);
        if(ni_status != XN_STATUS_OK){throw OpenNIException(xnGetStatusString(ni_status));}
        m_obj->m_depthGenerator.StartGenerating();
        LOG_DEBUG<<"created depth node";

        // create a user generator
        if(false)
        {
            ni_status = m_obj->m_context.FindExistingNode(XN_NODE_TYPE_USER, m_obj->m_userGenerator);
            if (ni_status != XN_STATUS_OK)
            {
                ni_status = m_obj->m_userGenerator.Create(m_obj->m_context);
                if(ni_status != XN_STATUS_OK){throw OpenNIException(xnGetStatusString(ni_status));}
            }
            LOG_DEBUG<<"created user node";

            if (!m_obj->m_userGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
            {
                LOG_WARNING<<"Supplied user generator doesn't support skeleton";
            }
            XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete;
            ni_status = m_obj->m_userGenerator.RegisterUserCallbacks(Obj::new_user, Obj::lost_user,
                                                                     m_obj.get(), hUserCallbacks);
            if(ni_status != XN_STATUS_OK){throw OpenNIException(xnGetStatusString(ni_status));}

            ni_status = m_obj->m_userGenerator.GetSkeletonCap().RegisterToCalibrationStart(Obj::calibration_start,
                                                                                m_obj.get(),
                                                                                hCalibrationStart);
            if(ni_status != XN_STATUS_OK){throw OpenNIException(xnGetStatusString(ni_status));}
            ni_status = m_obj->m_userGenerator.GetSkeletonCap().RegisterToCalibrationComplete(Obj::calibration_complete,
                                                                                m_obj.get(),
                                                                                hCalibrationComplete);
            if(ni_status != XN_STATUS_OK){throw OpenNIException(xnGetStatusString(ni_status));}

            ni_status = m_obj->m_userGenerator.StartGenerating();;
            if(ni_status != XN_STATUS_OK){throw OpenNIException(xnGetStatusString(ni_status));}
        }

        XnFieldOfView fov;
        m_obj->m_depthGenerator.GetFieldOfView(fov);
        LOG_DEBUG<<"Sensor fov: H: "<<glm::degrees(fov.fHFOV)<<" -- V: "<<glm::degrees(fov.fVFOV);
        LOG_DEBUG<<"init complete";
    }

    // Callback: New user was detected
    void XN_CALLBACK_TYPE OpenNIConnector::Obj::new_user(xn::UserGenerator& /*generator*/,
                                                         XnUserID nId, void* cookie)
    {
        Obj *myObj = static_cast<Obj*>(cookie);
        char str_pose[20];
        LOG_DEBUG<<"New User "<<nId;

        // New user found
        if (false)//g_bNeedPose)
        {
            myObj->m_userGenerator.GetPoseDetectionCap().StartPoseDetection(str_pose, nId);
        }
        else
        {
            myObj->m_userGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
        }
    }

    // Callback: An existing user was lost
    void XN_CALLBACK_TYPE OpenNIConnector::Obj::lost_user(xn::UserGenerator& /*generator*/,
                                                          XnUserID nId, void* cookie)
    {
        //Obj *myObj = static_cast<Obj*>(cookie);
        LOG_DEBUG<<"Lost user "<<nId;
    }

    void XN_CALLBACK_TYPE
    OpenNIConnector::Obj::pose_detected(xn::PoseDetectionCapability& /*capability*/,
                                        const XnChar* strPose, XnUserID nId, void* cookie)
    {
        Obj *myObj = static_cast<Obj*>(cookie);
        LOG_DEBUG<<"Pose "<<strPose<<" detected for user "<<nId;
        myObj->m_userGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
        myObj->m_userGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
    }
    // Callback: Started calibration
    void XN_CALLBACK_TYPE
    OpenNIConnector::Obj::calibration_start(xn::SkeletonCapability& /*capability*/, XnUserID nId,
                                            void* /*pCookie*/)
    {
        LOG_DEBUG<<"Calibration started for user "<<nId;
    }

    // Callback: Finished calibration
    void XN_CALLBACK_TYPE
    OpenNIConnector::Obj::calibration_complete(xn::SkeletonCapability& /*capability*/,
                                               XnUserID nId, XnCalibrationStatus eStatus,
                                               void* cookie)
    {
        Obj *myObj = static_cast<Obj*>(cookie);
        if (eStatus == XN_CALIBRATION_STATUS_OK)
        {
            // Calibration succeeded
            LOG_DEBUG<<"Calibration complete, start tracking user "<<nId;
            myObj->m_userGenerator.GetSkeletonCap().StartTracking(nId);
        }
        else
        {
            char str_pose[20];

            // Calibration failed
            LOG_DEBUG<<"Calibration failed for user "<<nId;
            if(eStatus==XN_CALIBRATION_STATUS_MANUAL_ABORT)
            {
                LOG_DEBUG<<"Manual abort occured, stop attempting to calibrate!";
                return;
            }
            if (false)//g_bNeedPose)
            {
                myObj->m_userGenerator.GetPoseDetectionCap().StartPoseDetection(str_pose, nId);
            }
            else
            {
                myObj->m_userGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
            }
        }
    }

    void OpenNIConnector::updateProperty(const Property::ConstPtr &theProperty)
    {
        if(theProperty == m_live_input)
        {
            stop();
            if(*m_live_input) start();
        }
        else if(theProperty == m_oni_path)
        {
            if(!*m_live_input)
            {
                stop();
                start();
            }
        }
    }

    void OpenNIConnector::start()
    {
        if(m_running) return;
        m_thread = std::thread(std::bind(&OpenNIConnector::run, this));
        m_running = true;
    }

    void OpenNIConnector::stop()
    {
        m_running = false;

        if(m_thread.joinable())
        {
            try{m_thread.join();}
            catch(std::exception &e){LOG_ERROR<<e.what();}
        }
    }

    void OpenNIConnector::run()
    {
        m_running = true;
        try{init();}
        catch(OpenNIException &e){LOG_ERROR<<e.what(); m_running = false;}

        // measure elapsed time with these
        xn::SceneMetaData sceneMD;
        xn::DepthMetaData depthMD;

        while(m_running)
        {
            // Read next available data
            m_obj->m_context.WaitOneUpdateAll(m_obj->m_depthGenerator);

            // Process the data
            m_obj->m_depthGenerator.GetMetaData(depthMD);
//            m_obj->m_userGenerator.GetUserPixels(0, sceneMD);
//
//            XnPoint3D user_center;
//            XnUInt16 num_users = m_obj->m_userGenerator.GetNumberOfUsers();
//            uint32_t user_ids[num_users];
//            m_obj->m_userGenerator.GetUsers(user_ids, num_users);

            //locked scope
            {
                std::unique_lock<std::mutex> lock(m_mutex);
                update_depth_texture(depthMD, sceneMD);
//
//                m_user_list.clear();
//
//                for (int i = 0; i < num_users; ++i)
//                {
//                    m_obj->m_userGenerator.GetCoM(user_ids[i], user_center);
//                    glm::vec3 p = glm::make_vec3((float*)&user_center);
//                    // zombie filter
//                    if(p != glm::vec3(0))
//                    {
//                        m_user_list.push_back(User(user_ids[i], p));
//                    }
//                }
                m_new_frame = true;
            }
        }
        m_user_list.clear();
        m_obj->m_pixel_buffer.clear();
        m_running = false;
    }

    OpenNIConnector::UserList OpenNIConnector::get_user_positions() const
    {
        std::unique_lock<std::mutex> lock(m_mutex);
        m_new_frame = false;
        return m_user_list;
    }

    gl::Texture OpenNIConnector::get_depth_texture()
    {
//        // needs OpenGL -> can only be called from main thread
//        // TODO: move or keep !?
//        std::unique_lock<std::mutex> lock(m_mutex);
//        m_new_frame = false;
//        if(m_obj && !m_obj->m_pixel_buffer.empty())
//            m_depth_texture.update(&m_obj->m_pixel_buffer[0], GL_UNSIGNED_BYTE, GL_RGB, 640, 480, true);
//        return m_depth_texture;

        m_depth_texture_raw.update((void*)m_depth_data, GL_UNSIGNED_SHORT, GL_RED, 640, 480, true);
        return m_depth_texture_raw;
    }

    void OpenNIConnector::update_depth_buffer(gl::Buffer the_vertex_buf) const
    {
        // needs OpenGL -> can only be called from main thread
        std::unique_lock<std::mutex> lock(m_mutex);
        m_new_frame = false;
        if(m_obj && !m_obj->m_point_buffer.empty())
        {
            the_vertex_buf.set_data(m_obj->m_point_buffer);
        }
    }

    void OpenNIConnector::update_depth_texture(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
    {
        unsigned int nZRes = dmd.ZRes();
        m_obj->m_histogram_buffer.resize(nZRes);
        std::fill(m_obj->m_histogram_buffer.begin(), m_obj->m_histogram_buffer.end(), 0);
        float* pDepthHist = &m_obj->m_histogram_buffer[0];

        unsigned int nValue = 0;
        unsigned int nHistValue = 0;
        unsigned int nIndex = 0;
        unsigned int nX = 0;
        unsigned int nY = 0;
        unsigned int nNumberOfPoints = 0;
        XnUInt16 x_res = dmd.XRes();
        XnUInt16 y_res = dmd.YRes();

        m_obj->m_pixel_buffer.resize(x_res * y_res * 3);
        unsigned char* pDestImage = &m_obj->m_pixel_buffer[0];

        const XnDepthPixel* pDepth = dmd.Data();
        const XnLabel* pLabels = smd.Data();

        // make raw depth image available for gpu processing
        if(true)
        {
            m_depth_data = static_cast<const uint16_t*>(dmd.Data());
            return;
        }

        // Calculate the accumulative histogram
        for (nY=0; nY<y_res; nY++)
        {
            for (nX=0; nX<x_res; nX++)
            {
                nValue = *pDepth;

                if (nValue != 0)
                {
                    pDepthHist[nValue]++;
                    nNumberOfPoints++;
                }
                pDepth++;
            }
        }

        for (nIndex=1; nIndex<nZRes; nIndex++)
        {
            pDepthHist[nIndex] += pDepthHist[nIndex-1];
        }
        if (nNumberOfPoints)
        {
            for (nIndex=1; nIndex<nZRes; nIndex++)
            {
                pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (pDepthHist[nIndex] / nNumberOfPoints)));
            }
        }

        pDepth = dmd.Data();

        if (false)//g_bDrawPixels)
        {
            XnUInt32 nIndex = 0;
            int num_colors = m_user_colors.size() - 1;

            // Prepare the texture map
            for (nY=0; nY<y_res; nY++)
            {
                for (nX = 0; nX < x_res; nX++, nIndex++)
                {
                    pDestImage[0] = 0;
                    pDestImage[1] = 0;
                    pDestImage[2] = 0;
                    if (/*g_bDrawBackground*/true || *pLabels != 0)
                    {
                        nValue = *pDepth;
                        XnLabel label = *pLabels;
                        XnUInt32 nColorID = label % num_colors;
                        if (label == 0)
                        {
                            nColorID = num_colors;
                        }

                        if (nValue != 0)
                        {
                            nHistValue = pDepthHist[nValue];
                            pDestImage[0] = nHistValue * m_user_colors[nColorID][0];
                            pDestImage[1] = nHistValue * m_user_colors[nColorID][1];
                            pDestImage[2] = nHistValue * m_user_colors[nColorID][2];
                        }
                    }
                    pDepth++;
                    pLabels++;
                    pDestImage+=3;
                }
            }
        }
        else
        {
            std::fill(m_obj->m_pixel_buffer.begin(), m_obj->m_pixel_buffer.end(), 0);
        }

        if(false)
        {
            // point buffer
            int w = 640, h = 480;

            m_obj->m_point_buffer.resize(w * h);

            // reset pointer
            pDepth = dmd.Data();

            XnPoint3D p2d, p3d;
            for (int i = 0; i < h; i++)
                for (int j = 0; j < w; j++)
                {
                    p2d.X = j;
                    p2d.Y = i;
                    p2d.Z = pDepth[i * w + j];
                    m_obj->m_depthGenerator.ConvertProjectiveToRealWorld(1, &p2d, &p3d);

                    m_obj->m_point_buffer[i * w + j] = glm::vec4(p3d.X, p3d.Y, -p3d.Z, 1.f);

                    //                m_obj->m_point_buffer[i * w + j] = glm::vec4(j, i, pDepth[i * w + j], 1.f);
                }
        }

    }

}}


#ifndef SEQUENTIAL_IMAGE_GENERATOR_H
#define SEQUENTIAL_IMAGE_GENERATOR_H

#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/video.hpp>
#include "utils.hpp"

#include <opencv2/core.hpp>
#include <Eigen/Dense>

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

class SequentialImageGenerator{
public:
  SequentialImageGenerator();
  SequentialImageGenerator(string _firstFrameFilename, string _groundTruthFile, string _detectionsFile);
  bool hasEnded();
  void moveNext();
  Mat getFrame(int frame_num);
  vector<Target> getGroundTruth(int frame_num);
  vector<Rect> getDetections();
  VectorXd getDetectionWeights();
  MatrixXd getDetectionFeatures();
  size_t getDatasetSize();
  void readDetections(int frame);

private:
  void readGroundTruth(string str);
  int frame_id;
  void getNextFilename(string& filename);
  vector<Mat> images;

  string detectionsFileName;
  ifstream dt_file;
  int pointerPos;

  vector< vector<Target> > ground_truth;
  vector<Rect> detections;
  VectorXd detection_weights;
  MatrixXd features;
};

#endif // IMAGE_GENERATOR_H


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

#include <boost/simd/include/functions/extract.hpp>
#include <boost/simd/sdk/functor/preprocessor/call.hpp>
#include <boost/simd/sdk/simd/pack/forward.hpp>
#include <boost/simd/sdk/simd/category.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , tag::cpu_
                                    , (A0)(A1)
                                    , ((ast_<A0, boost::simd::domain>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef typename A0::value_type   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 a1) const
    {
      return extract(evaluate(a0),a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( extract_
                                    , tag::cpu_
                                    , (A0)(A1)(Tag)
                                    , ((node_ < A0, Tag
                                              , boost::mpl::long_<0>
                                              , boost::simd::domain
                                              >
                                      ))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef typename A0::value_type   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 a1) const
    {
      return a0[a1];
    }
  };
} } }

#endif


#include <boost/numeric/odeint/external/vexcl/vexcl_norm_inf.hpp>


/**
*   Polygon.hpp
*
*   Copyright 2015 The Climate Corporation
*   Copyright 2015 Tony Stone
*
*   Licensed under the Apache License, Version 2.0 (the "License");
*   you may not use this file except in compliance with the License.
*   You may obtain a copy of the License at
*
*   http://www.apache.org/licenses/LICENSE-2.0
*
*   Unless required by applicable law or agreed to in writing, software
*   distributed under the License is distributed on an "AS IS" BASIS,
*   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*   See the License for the specific language governing permissions and
*   limitations under the License.
*
*   Created by Tony Stone on 6/9/15.
*
*   MODIFIED 2015 BY Tony Stone. Modifications licensed under Apache License, Version 2.0.
*
*/
#pragma once

#ifndef __Polygon_HPP_
#define __Polygon_HPP_

#include "Geometry.hpp"
#include "Ring.hpp"
#include "LineString.hpp"
#include "MultiLineString.hpp"
#include "MultiPoint.hpp"

#include <boost/concept/assert.hpp>

#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/geometries/concepts/point_concept.hpp>

namespace geofeatures {

    /**
     * @class       Polygon
     *
     * @brief       A Polygon with an outer Ring and inner Rings or wholes.
     *
     * @author      Tony Stone
     * @date        6/9/15
     */
    class Polygon : public Geometry {

    public:

        // Member types
        typedef Point PointType;
        typedef Ring RingType;
        typedef std::vector<RingType> InnerContainerType;

        inline Polygon () noexcept : Geometry(), outer_(), inners_() {}
        inline Polygon (Polygon const & other) noexcept :  Geometry(), outer_(other.outer_), inners_(other.inners_) {}
        inline Polygon (Polygon & other)       noexcept :  Geometry(), outer_(other.outer_), inners_(other.inners_) {}

        inline virtual ~Polygon() noexcept {}

    public:
        inline RingType const & outer () const noexcept { return outer_; }
        inline InnerContainerType const & inners () const noexcept { return inners_; }

        inline RingType & outer () noexcept { return outer_; }
        inline InnerContainerType & inners () noexcept { return inners_; }

        inline void setOuter(const RingType & outer) noexcept { outer_ = outer; }
        inline void setInners(const InnerContainerType & inners) noexcept { inners_ = inners; }

    private:
        RingType outer_;
        InnerContainerType inners_;
    };

}   // namespace geofeatures

namespace geofeatures_boost {
    namespace geometry {
        namespace traits {
            template<> struct tag<geofeatures::Polygon> { typedef polygon_tag type; };
            template<> struct ring_const_type<geofeatures::Polygon> { typedef geofeatures::Polygon::RingType const& type; };
            template<> struct ring_mutable_type<geofeatures::Polygon> { typedef geofeatures::Polygon::RingType & type; };

            template<> struct interior_const_type<geofeatures::Polygon> { typedef  geofeatures::Polygon::InnerContainerType const& type; };
            template<> struct interior_mutable_type<geofeatures::Polygon> { typedef geofeatures::Polygon::InnerContainerType & type; };

            template<> struct exterior_ring<geofeatures::Polygon>
            {
                static inline typename geofeatures::Polygon::RingType & get(geofeatures::Polygon& p)
                {
                    return p.outer();
                }

                static inline typename geofeatures::Polygon::RingType const& get(geofeatures::Polygon const& p)
                {
                    return p.outer();
                }
            };

            template<> struct interior_rings<geofeatures::Polygon>
            {
                static inline typename geofeatures::Polygon::InnerContainerType & get(geofeatures::Polygon& p)
                {
                    return p.inners();
                }

                static inline typename geofeatures::Polygon::InnerContainerType const& get(geofeatures::Polygon const& p)
                {
                    return p.inners();
                }
            };
        }
    }
} // namespace boost::geometry::traits

#endif //__Polygon_HPP_


// Copyright (c) 2014-2016 The Uscoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "chainparams.h"
#include "validation.h"
#include "net.h"

#include "test/test_uscoin.h"

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

static void TestBlockSubsidyHalvings(const Consensus::Params& consensusParams)
{
    int maxHalvings = 64;
    CAmount nInitialSubsidy = 50 * COIN;

    CAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);
    for (int nHalvings = 0; nHalvings < maxHalvings; nHalvings++) {
        int nHeight = nHalvings * consensusParams.nSubsidyHalvingInterval;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK(nSubsidy <= nInitialSubsidy);
        BOOST_CHECK_EQUAL(nSubsidy, nPreviousSubsidy / 2);
        nPreviousSubsidy = nSubsidy;
    }
    BOOST_CHECK_EQUAL(GetBlockSubsidy(maxHalvings * consensusParams.nSubsidyHalvingInterval, consensusParams), 0);
}

static void TestBlockSubsidyHalvings(int nSubsidyHalvingInterval)
{
    Consensus::Params consensusParams;
    consensusParams.nSubsidyHalvingInterval = nSubsidyHalvingInterval;
    TestBlockSubsidyHalvings(consensusParams);
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    TestBlockSubsidyHalvings(chainParams->GetConsensus()); // As in main
    TestBlockSubsidyHalvings(150); // As in regtest
    TestBlockSubsidyHalvings(1000); // Just another interval
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 14000000; nHeight += 1000) {
        CAmount nSubsidy = GetBlockSubsidy(nHeight, chainParams->GetConsensus());
        BOOST_CHECK(nSubsidy <= 50 * COIN);
        nSum += nSubsidy * 1000;
        BOOST_CHECK(MoneyRange(nSum));
    }
    BOOST_CHECK_EQUAL(nSum, 2099999997690000ULL);
}

bool ReturnFalse() { return false; }
bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), CombinerAll> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
}
BOOST_AUTO_TEST_SUITE_END()


#include "skadi/sample.h"

#include <cstddef>
#include <cmath>
#include <stdexcept>
#include <limits>
#include <list>
#include <fstream>
#include <boost/regex.hpp>
#include <sys/stat.h>

#include <boost/filesystem.hpp>
#include <boost/optional.hpp>

#include "midgard/logging.h"
#include "midgard/pointll.h"

namespace {
  //srtmgl1 holds 1x1 degree tiles but oversamples the egde of the tile
  //by .5 seconds on all sides. that means that the center of pixel 0 is
  //located at the tiles lat,lon (which is important for bilinear filtering)
  //it also means that there are 3601 pixels per row and per column
  constexpr size_t HGT_DIM = 3601;
  constexpr size_t HGT_PIXELS = HGT_DIM * HGT_DIM;
  constexpr int16_t NO_DATA_VALUE = -32768;
  constexpr int16_t NO_DATA_HIGH = 16384;
  constexpr int16_t NO_DATA_LOW = -16384;
  constexpr size_t TILE_COUNT = 180 * 360;

  //macro is faster than inline funciton for this..
  #define out_of_range(v) v > NO_DATA_HIGH || v < NO_DATA_LOW

  std::list<std::string> get_files(const std::string& root_dir) {
    std::list<std::string> files;
    try {
      for (boost::filesystem::recursive_directory_iterator i(root_dir), end; i != end; ++i)
        if (!is_directory(i->path()))
          files.push_back(i->path().string());
    }//couldn't get data
    catch(...) {
      LOG_WARN(root_dir + " has no elevation tiles");
      files.clear();
    }
    return files;
  }

  std::string hgt_name(int lon, int lat) {
    std::string name(lat < 0 ? "S" : "N");
    name.append(std::to_string(std::abs(lat)));
    name.insert(1, 3 - name.size(), '0');
    name.push_back(lon < 0 ? 'W' : 'E');
    name.append(std::to_string(std::abs(lon)));
    name.append(".hgt");
    name.insert(4, 11 - name.size(), '0');
    return name;
  }

  uint16_t is_hgt(const std::string& name) {
    boost::smatch m;
    boost::regex e(".*/([NS])([0-9]{2})([WE])([0-9]{3})\\.hgt$");
    if(boost::regex_search(name, m, e)) {
      auto lon = std::stoul(m[4]) * (m[3] == "E" ? 1 : -1) + 180;
      auto lat = std::stoul(m[2]) * (m[1] == "N" ? 1 : -1) + 90;
      if(lon >= 0 && lon < 360 && lat >=0 && lat < 180)
        return lat * 360 + lon;
    }
    return -1;
  }

  int16_t flip(int16_t value) {
    return ((value & 0xFF) << 8) | ((value >> 8) & 0xFF);
  }

  uint64_t file_size(const std::string& file_name) {
    struct stat s;
    int rc = stat(file_name.c_str(), &s);
    return rc == 0 ? s.st_size : -1;
  }

}

namespace valhalla {
namespace skadi {

  sample::sample(const std::string& data_source):cache(TILE_COUNT) {
    //check the directory for files that look like what we need
    auto files = get_files(data_source);
    for(const auto& f : files) {
      auto index = is_hgt(f);
      if(index < cache.size()) {
        if(file_size(f) == HGT_PIXELS * sizeof(int16_t))
          cache[index].map(f, HGT_PIXELS * sizeof(int16_t));
        else
          LOG_WARN("Corrupt elevation data: " + f);
      }
    }
  }

  template <class coord_t>
  double sample::get(const coord_t& coord) const {
    //check the cache and load
    auto lon = std::floor(coord.first);
    auto lat = std::floor(coord.second);
    auto index = static_cast<uint16_t>(lat + 90) * 360 + static_cast<uint16_t>(lon + 180);
    if(index > cache.size() || !cache[index])
      return NO_DATA_VALUE;

    //grab the data array and what row and column we need
    //NOTE: data is arranged from upper left to bottom right, so y is flipped
    const int16_t* t = cache[index].get();

    //fractional pixel
    double u = (coord.first - lon) * (HGT_DIM - 1);
    double v = (1.0 - (coord.second - lat)) * (HGT_DIM - 1);

    //integer pixel
    size_t x = std::floor(u);
    size_t y = std::floor(v);

    //coefficients
    double u_ratio = u - x;
    double v_ratio = v - y;
    double u_inv = 1 - u_ratio;
    double v_inv = 1 - v_ratio;
    double a_coef = u_inv * v_inv;
    double b_coef = u_ratio * v_inv;
    double c_coef = u_inv * v_ratio;
    double d_coef = u_ratio * v_ratio;

    //values
    double adjust = 0;
    auto a = flip(t[y * HGT_DIM + x]);
    auto b = flip(t[y * HGT_DIM + x + 1]);
    if(out_of_range(a)) a_coef = 0;
    if(out_of_range(b)) b_coef = 0;

    //first part of the bilinear interpolation
    auto value = a * a_coef + b * b_coef;
    adjust += a_coef + b_coef;
    //LOG_INFO('{' + std::to_string(y * HGT_DIM + x) + ',' + std::to_string(a) + '}');
    //LOG_INFO('{' + std::to_string(y * HGT_DIM + x + 1) + ',' + std::to_string(b) + '}');
    //only need the second part if you aren't right on the row
    //this also protects from a corner case where you sample past the end of the image
    if(y < HGT_DIM - 1) {
      auto c = flip(t[(y + 1) * HGT_DIM + x]);
      auto d = flip(t[(y + 1) * HGT_DIM + x + 1]);
      if(out_of_range(c)) c_coef = 0;
      if(out_of_range(d)) d_coef = 0;
      //LOG_INFO('{' + std::to_string((y + 1) * HGT_DIM + x) + ',' + std::to_string(c) + '}');
      //LOG_INFO('{' + std::to_string((y + 1) * HGT_DIM + x + 1) + ',' + std::to_string(d) + '}');
      value += c * c_coef + d * d_coef;
      adjust += c_coef + d_coef;
    }
    //if we are missing everything then give up
    if(adjust == 0)
      return NO_DATA_VALUE;
    //if we were missing some we need to adjust by that
    return value / adjust;
  }

  template <class coords_t>
  std::vector<double> sample::get_all(const coords_t& coords) const {
    std::vector<double> values;
    values.reserve(coords.size());
    for(const auto& coord : coords)
      values.emplace_back(get(coord));
    return values;
  }

  double sample::get_no_data_value() const {
    return NO_DATA_VALUE;
  }

  //explicit instantiations for templated get
  template double sample::get<std::pair<double, double> >(const std::pair<double, double>&) const;
  template double sample::get<std::pair<float, float> >(const std::pair<float, float>&) const;
  template double sample::get<midgard::PointLL>(const midgard::PointLL&) const;
  template double sample::get<midgard::Point2>(const midgard::Point2&) const;
  template std::vector<double> sample::get_all<std::list<std::pair<double, double> > >(const std::list<std::pair<double, double> >&) const;
  template std::vector<double> sample::get_all<std::vector<std::pair<double, double> > >(const std::vector<std::pair<double, double> >&) const;
  template std::vector<double> sample::get_all<std::list<std::pair<float, float> > >(const std::list<std::pair<float, float> >&) const;
  template std::vector<double> sample::get_all<std::vector<std::pair<float, float> > >(const std::vector<std::pair<float, float> >&) const;
  template std::vector<double> sample::get_all<std::list<midgard::PointLL> >(const std::list<midgard::PointLL>&) const;
  template std::vector<double> sample::get_all<std::vector<midgard::PointLL> >(const std::vector<midgard::PointLL>&) const;
  template std::vector<double> sample::get_all<std::list<midgard::Point2> >(const std::list<midgard::Point2>&) const;
  template std::vector<double> sample::get_all<std::vector<midgard::Point2> >(const std::vector<midgard::Point2>&) const;

}
}


// Copyright (c) 2011 The University of Sydney

/// @author Vsevolod Vlaskine

#include <algorithm>
#include <cmath>
#include <unordered_map>
#include <boost/array.hpp>
#include <boost/lexical_cast.hpp>
#include <qnamespace.h>
#include <comma/string/string.h>
#include "colored.h"

namespace snark { namespace graphics { namespace view {

static color_t color_from_name( const std::string& name )
{
         if( name == "black" )   { return color_t( Qt::black ); }
    else if( name == "white" )   { return color_t( Qt::white ); }
    else if( name == "red" )     { return color_t( Qt::red ); }
    else if( name == "green" )   { return color_t( Qt::green ); }
    else if( name == "blue" )    { return color_t( Qt::blue ); }
    else if( name == "yellow" )  { return color_t( Qt::yellow ); }
    else if( name == "magenta" ) { return color_t( Qt::magenta ); }
    else if( name == "cyan" )    { return color_t( Qt::cyan ); }
    else if( name == "grey" )    { return color_t( Qt::gray ); }
    else if( name == "pink" )    { return color_t( 255, 128, 128 ); }
    else if( name == "orange" )  { return color_t( 255, 128, 0 ); }
    else if( name == "salad" )   { return color_t( 128, 255, 128 ); }
    else if( name == "sky" )     { return color_t( 128, 128, 255 ); }
    else
    {
        auto split = []( const std::string& s, char delimiter )
        {
            const std::vector< std::string >& v = comma::split( s, delimiter );
            try
            {
                switch( v.size() )
                {
                    case 3: return color_t( boost::lexical_cast< int >( v[0] ), boost::lexical_cast< int >( v[1] ), boost::lexical_cast< int >( v[2] ) );
                    case 4: return color_t( boost::lexical_cast< int >( v[0] ), boost::lexical_cast< int >( v[1] ), boost::lexical_cast< int >( v[2] ), boost::lexical_cast< int >( v[3] ) );
                    default: COMMA_THROW( comma::exception, "expected color as rgb or rgba, got '" << s << "'" );
                }
            }
            catch( std::exception& ex ) { COMMA_THROW( comma::exception, "expected color, got '" << s << "'" << "(" << ex.what() << ")" ); }
            catch( ... ) { COMMA_THROW( comma::exception, "expected color, got '" << s << "' (unknown exception)" ); }
        };
        try { return split( name, '|' ); } catch ( ... ) { return split( name, ',' ); }
    }
}

static color_t multiply( const color_t& color, double scalar )
{
#if Qt3D_VERSION==1
    double red = scalar * color.redF();
    double green = scalar * color.greenF();
    double blue = scalar * color.blueF();
    double alpha = scalar * color.alphaF();
    return color_t::fromRgbF( red, green, blue, alpha );
#else
    return color * scalar;
#endif
}

static color_t add( const color_t& a, const color_t& b )
{
#if Qt3D_VERSION==1
    unsigned int red = std::min( a.red() + b.red(), 255 );
    unsigned int green = std::min( a.green() + b.green(), 255 );
    unsigned int blue = std::min( a.blue() + b.blue(), 255 );
    unsigned int alpha = std::min( a.alpha() + b.alpha(), 255 );
    return color_t( red, green, blue, alpha );
#else
    return a + b;
#endif
}

fixed::fixed( const std::string& name ) : color_( color_from_name( name ) ) {}

color_t fixed::color( const Eigen::Vector3d&, comma::uint32, double, const color_t& ) const { return color_; }

by_height::by_height( double from
                    , double to
                    , const color_t& from_color
                    , const color_t& to_color
                    , bool cyclic
                    , bool linear
                    , bool sharp )
    : from( from )
    , to( to )
    , sum( to + from )
    , diff( to - from )
    , middle( from / sum )
    , from_color( from_color )
    , to_color( to_color )
    , cyclic( cyclic )
    , linear( linear )
    , sharp( sharp )
{
    average_color = color_t( from_color.red() / 2   + to_color.red() / 2
                           , from_color.green() / 2 + to_color.green() / 2
                           , from_color.blue() / 2  + to_color.blue() / 2
                           , from_color.alpha() / 2 + to_color.alpha() / 2 );
}

color_t by_height::color( const Eigen::Vector3d& point, comma::uint32, double, const color_t& ) const
{
    if( cyclic )
    {
        double v = std::fmod( point.z() - std::floor( point.z() / sum ) * sum, sum ) / sum;
        if( sharp ) { return v < middle ? from_color : to_color; }
        if( linear )
        {
            if( v < middle * 0.5 )
            {
                return add( multiply( average_color, 1 - v ), multiply( from_color, v ) );
            }
            else if( v < ( 1 + middle ) * 0.5 )
            {
                v = v - middle * 0.5;
                return add( multiply( from_color, ( 1 - v ) ), multiply( to_color, v ) );
            }
            else
            {
                v = v - ( 1 + middle ) * 0.5;
                return add( multiply( to_color, 1 - v ), multiply( average_color, v ) );
            }
        }
        else
        {
            if( v < middle )
            {
                v = v / middle - 0.5;
                v = v * v * 4;
                return add( multiply( from_color, 1 - v ), multiply( average_color, v ) );
            }
            else
            {
                v = ( v - middle ) / ( 1 - middle ) - 0.5;
                v = v * v * 4;
                return add( multiply( average_color, v ), multiply( to_color, 1 - v ) );
            }
        }
    }
    else
    {
        double v = ( point.z() - from ) / diff;
        v = ( v < 0 ? 0 : v > 1 ? 1 : v );
        return add( multiply( from_color, 1 - v ), multiply( to_color, v ) );
    }
}

by_scalar::by_scalar( double from, double to, const color_t& from_color, const color_t& to_color )
    : from( from )
    , to( to )
    , diff( to - from )
    , from_color( from_color )
    , to_color( to_color )
{
}

by_scalar::by_scalar( double from, double to, const snark::render::colour_map::values& map )
    : from( from )
    , to( to )
    , diff( to - from )
    , map( map )
{
}

color_t by_scalar::color( const Eigen::Vector3d& point, comma::uint32, double scalar, const color_t& ) const
{
    (void)point;
    double v = ( scalar - from ) / diff;
    v = ( v < 0 ? 0 : v > 1 ? 1 : v );
    if( !map ) { return add( multiply( from_color, 1 - v ), multiply( to_color, v ) ); }
    unsigned int i = v * 255;
    return color_t( ( *map )[i][0], ( *map )[i][1], ( *map )[i][2], 255 );
}

namespace impl {

static boost::array< unsigned int, 256 > colorInit()
{
    boost::array< unsigned int, 256 > a;
    for( std::size_t i = 0; i < 256; ++i ) { a[i] = i; }
    std::random_shuffle( a.begin(), a.end() );
    return a;
}
static unsigned char colorIndex = 23; // whatever
static boost::array< unsigned int, 256 > colorIndices = colorInit();

} // namespace impl {

by_id::by_id( const color_t& backgroundcolor )
    : background_( backgroundcolor )
    , has_scalar_( false )
{
}

by_id::by_id( const color_t& backgroundcolor
            , double from
            , double to )
    : background_( backgroundcolor )
    , has_scalar_( true )
    , from_( from )
    , diff_( to - from )
{
}

color_t by_id::color( const Eigen::Vector3d&, comma::uint32 id, double scalar, const color_t& ) const // quick and dirty
{
    static const float b = 0.95f;
    unsigned char i = ( id & 0xff ) + ( ( id & 0xff00 ) >> 16 );
    const float h = b * float( impl::colorIndices[ i ] ) / 255;
    const float a = 0.3f * float( 255 - impl::colorIndices[ i ] ) / 255;
    color_t color;
    switch( ( i * 13 + impl::colorIndex ) % 6 )
    {
#if Qt3D_VERSION==1
        case 0 : color = color_t::fromRgbF( b, h, a ); break;
        case 1 : color = color_t::fromRgbF( a, b, h ); break;
        case 2 : color = color_t::fromRgbF( h, a, b ); break;
        case 3 : color = color_t::fromRgbF( b, a, h ); break;
        case 4 : color = color_t::fromRgbF( h, b, a ); break;
        default: color = color_t::fromRgbF( a, h, b ); break;
#else
        case 0 : color = color_t( b, h, a ); break;
        case 1 : color = color_t( a, b, h ); break;
        case 2 : color = color_t( h, a, b ); break;
        case 3 : color = color_t( b, a, h ); break;
        case 4 : color = color_t( h, b, a ); break;
        default: color = color_t( a, h, b ); break;
#endif
    }
    if( !has_scalar_ ) { return color; }
    double v = ( scalar - from_ ) / diff_;
    v = ( v < 0 ? 0 : v > 1 ? 1 : v );
    return add( multiply( color, v ), multiply( background_, 1 - v ) );
}

by_id_color_map::by_id_color_map( const std::unordered_map< comma::uint32, color_t >& colors, color_t not_found, bool cyclic ): colors_( colors ), not_found_( not_found ), cyclic_( cyclic ) {}

color_t by_id_color_map::color( const Eigen::Vector3d&, comma::uint32 id, double scalar, const color_t& ) const
{
    if( cyclic_ ) { return colors_.find( id % colors_.size() )->second; }
    auto it = colors_.find( id );
    return it == colors_.end() ? not_found_ : it->second;
}

color_t by_rgb::color( const Eigen::Vector3d& , comma::uint32, double, const color_t& c ) const { return c; }

colored* color_from_string( const std::string& t, const std::string& fields, const color_t& backgroundcolor )
{
    std::string s = t.empty() ? std::string( "0:1,cyan:magenta" ) : t;
    std::vector< std::string > f = comma::split( fields, ',' );
    bool has_id = false;
    for( unsigned int i = 0; !has_id && i < f.size(); ++i ) { has_id = f[i] == "id"; } // quick and dirty
    bool has_scalar = false;
    for( unsigned int i = 0; !has_scalar && i < f.size(); ++i ) { has_scalar = f[i] == "scalar"; } // quick and dirty
    bool r = false;
    bool g = false;
    bool b = false;
    bool a = false;
    for( unsigned int i = 0; !( r && g && b ) && !a && i < f.size(); ++i ) // quick and dirty
    {
        if( f[i] == "r" ) { r = true; }
        if( f[i] == "g" ) { g = true; }
        if( f[i] == "b" ) { b = true; }
        if( f[i] == "a" ) { a = true; }
    }
    if( has_id )
    {
        if( has_scalar )
        {
            const std::vector< std::string >& v = comma::split( comma::split( s, ',' )[0], ':' );
            if( v.size() != 2 ) { COMMA_THROW( comma::exception, "expected range (-5:20), got " << s ); }
            double from = boost::lexical_cast< double >( v[0] );
            double to = boost::lexical_cast< double >( v[1] );
            return new by_id( backgroundcolor, from, to );
        }
        if( t.empty() ) { return new by_id( backgroundcolor ); }
        const std::vector< std::string >& v = comma::split( t, ',' );
        std::unordered_map< comma::uint32, color_t > colors;
        bool cyclic = false;
        bool not_cyclic = false;
        color_t default_color = color_from_name( "grey" );
        for( unsigned int i = 0; i < v.size(); ++i )
        {
            const std::vector< std::string >& w = comma::split( v[i], ':' );
            switch( w.size() )
            {
                case 1:
                    cyclic = true;
                    colors[i] = color_from_name( w[0] );
                    break;
                case 2:
                    not_cyclic = true;
                    if( w[0] == "default" ) { default_color = color_from_name( w[1] ); }
                    else { colors[ boost::lexical_cast< comma::uint32 >( w[0] ) ] = color_from_name( w[1] ); }
                    break;
                default:
                    COMMA_THROW( comma::exception, "expected color definition; got --color='" << s << "'" );
            }
        }
        if( cyclic && not_cyclic ) { COMMA_THROW( comma::exception, "expected either color list or color map; got --color='" << s << "'" ); }
        return new by_id_color_map( colors, default_color, cyclic );
    }
    if( r || g || b || a ) { return new by_rgb; }
    if( has_scalar )
    {
        color_t from_color, to_color;
        double from = 0;
        double to = 0;
        boost::optional< snark::render::colour_map::values > map;
        std::vector< std::string > v = comma::split( s, ',' );
        switch( v.size() )
        {
            case 1:
            {
                std::vector< std::string > w = comma::split( v[0], ':' );
                switch( w.size() )
                {
                    case 1:
                        if( w[0] == "green" ) { map = snark::render::colour_map::constant( 0, 255, 0 ); }
                        else if( w[0] == "red" ) { map = snark::render::colour_map::constant( 255, 0, 0 ); }
                        else if( w[0] == "hot" ) { map = snark::render::colour_map::temperature( 96, 96 ); }
                        else if( w[0] == "jet" ) { map = snark::render::colour_map::jet(); }
                        else { COMMA_THROW( comma::exception, "expected colour map, got: " << s ); }
                        break;
                    case 2:
                        if( w[0][0] >= 'a' && w[0][0] <= 'z' )
                        {
                            from_color = color_from_name( w[0] );
                            to_color = color_from_name( w[1] );
                        }
                        else
                        {
                            from_color = color_from_name( "cyan" );
                            to_color = color_from_name( "magenta" );
                            from = boost::lexical_cast< double >( w[0] );
                            to = boost::lexical_cast< double >( w[1] );
                        }
                        break;
                    default:
                        COMMA_THROW( comma::exception, "expected range (e.g. -5:20,red:blue or 3:10), or colour map name, got " << s );
                }
                break;
            }
            case 2:
            {
                std::vector< std::string > w = comma::split( v[0], ':' );
                from = boost::lexical_cast< double >( w[0] );
                to = boost::lexical_cast< double >( w[1] );
                w = comma::split( v[1], ':' );
                switch( w.size() )
                {
                    case 1:
                        if( w[0] == "green" ) { map = snark::render::colour_map::constant( 0, 255, 0 ); }
                        else if( w[0] == "red" ) { map = snark::render::colour_map::constant( 255, 0, 0 ); }
                        else if( w[0] == "hot" ) { map = snark::render::colour_map::temperature( 96, 96 ); }
                        else if( w[0] == "jet" ) { map = snark::render::colour_map::jet(); }
                        else { COMMA_THROW( comma::exception, "expected colour map, got: " << s ); }
                        break;
                    case 2:
                        from_color = color_from_name( w[0] );
                        to_color = color_from_name( w[1] );
                        break;
                    default:
                        COMMA_THROW( comma::exception, "expected range (e.g. -5:20,red:blue or 3:10), or colour map name, got " << s );
                }
                break;
            }
            default:
                COMMA_THROW( comma::exception, "expected range (e.g. -5:20,red:blue or 3:10), or colour map name, got " << s );
        }
        return map ? new by_scalar( from, to, *map ) : new by_scalar( from, to, from_color, to_color );
    }
    colored* c;
    try
    {
        c = new fixed( s );
    }
    catch( ... )
    {
        std::vector< std::string > v = comma::split( s, ',' );
        double from = 0;
        double to = 1;
        color_t from_color = color_from_name( "red" );
        color_t to_color = color_from_name( "yellow" );
        bool sharp = false;
        bool cyclic = false;
        bool linear = true;
        for( unsigned int i = 0; i < v.size(); ++i )
        {
            if( v[i].empty() ) { continue; }
            else if( v[i] == "sharp" ) { sharp = true; }
            else if( v[i] == "smooth" ) { sharp = false; }
            else if( v[i] == "cyclic" ) { cyclic = true; from = 1; }
            else if( v[i] == "linear" ) { linear = true; }
            else if( v[i] == "quadratic" ) { linear = false; }
            else
            {
                std::vector< std::string > w = comma::split( v[i], ':' );
                if( w[0][0] >= 'a' && w[0][0] <= 'z' )
                {
                    from_color = color_from_name( w[0] );
                    to_color = color_from_name( w[1] );
                }
                else
                {
                    from = boost::lexical_cast< double >( w[0] );
                    to = boost::lexical_cast< double >( w[ w.size() == 1 ? 0 : 1 ] );
                }
            }
        }
        c = new by_height( from, to, from_color, to_color, cyclic, linear, sharp );
    }
    return c;
}

} } } // namespace snark { namespace graphics { namespace view {


#define BOOST_TEST_MODULE "test_read_system"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/System.hpp>
#include <mjolnir/input/read_system.hpp>

BOOST_AUTO_TEST_CASE(read_system_with_unlimited_boundary)
{
    mjolnir::LoggerManager::set_default_logger("test_read_system.log");

    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    constexpr real_type tol = 1e-8;
    {
        using namespace toml::literals;
        const auto v = u8R"(
            [files]
            # this table is required because [[systems]] table can be provided
            # as a different file
            [[systems]]
            boundary_shape = {}
            attributes = {test_attr = 3.14}
            particles  = [
                {m = 1.0, pos = [1.0, 2.0, 3.0], vel = [1.1, 2.1, 3.1], name = "testnm", group = "testgrp"},
                {m = 2.0, pos = [1.2, 2.2, 3.2], vel = [1.3, 2.3, 3.3], name = "testnm", group = "testgrp"},
                {m = 3.0, pos = [1.4, 2.4, 3.4], vel = [1.5, 2.5, 3.5]}
            ]
        )"_toml;

        const auto sys = mjolnir::read_system<traits_type>(v, 0);
        BOOST_TEST(sys.size() == 3u);

        BOOST_TEST(sys.mass(0) == 1.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.mass(1) == 2.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.mass(2) == 3.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.rmass(0) == 1.0 / 1.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.rmass(1) == 1.0 / 2.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.rmass(2) == 1.0 / 3.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.position(0).at(0) == 1.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(0).at(1) == 2.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(0).at(2) == 3.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(1).at(0) == 1.2, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(1).at(1) == 2.2, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(1).at(2) == 3.2, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(2).at(0) == 1.4, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(2).at(1) == 2.4, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(2).at(2) == 3.4, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.velocity(0).at(0) == 1.1, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(0).at(1) == 2.1, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(0).at(2) == 3.1, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(1).at(0) == 1.3, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(1).at(1) == 2.3, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(1).at(2) == 3.3, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(2).at(0) == 1.5, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(2).at(1) == 2.5, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(2).at(2) == 3.5, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.force(0).at(0) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(0).at(1) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(0).at(2) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(1).at(0) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(1).at(1) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(1).at(2) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(2).at(0) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(2).at(1) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(2).at(2) == 0.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.name(0) == "testnm");
        BOOST_TEST(sys.name(1) == "testnm");
        BOOST_TEST(sys.name(2) == "X");

        BOOST_TEST(sys.group(0) == "testgrp");
        BOOST_TEST(sys.group(1) == "testgrp");
        BOOST_TEST(sys.group(2) == "NONE");

        BOOST_TEST(sys.has_attribute("test_attr"));
        BOOST_TEST(sys.attribute("test_attr") == 3.14, boost::test_tools::tolerance(tol));
    }
}

BOOST_AUTO_TEST_CASE(read_system_with_cuboidal_periodic_boundary)
{
    mjolnir::LoggerManager::set_default_logger("test_read_system.log");

    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::CuboidalPeriodicBoundary>;
    constexpr real_type tol = 1e-8;
    {
        using namespace toml::literals;
        const auto v = u8R"(
            [files]
            # this table is required because [[systems]] table can be provided
            # as a different file
            [[systems]]
            boundary_shape.upper = [10.0, 11.0, 12.0]
            boundary_shape.lower = [ 0.0,  1.0,  2.0]
            attributes = {test_attr = 3.14}
            particles  = [
                {m = 1.0, pos = [1.0, 2.0, 3.0], vel = [1.1, 2.1, 3.1], name = "testnm", group = "testgrp"},
                {m = 2.0, pos = [1.2, 2.2, 3.2], vel = [1.3, 2.3, 3.3], name = "testnm", group = "testgrp"},
                {m = 3.0, pos = [1.4, 2.4, 3.4], vel = [1.5, 2.5, 3.5]}
            ]
        )"_toml;

        const auto sys = mjolnir::read_system<traits_type>(v, 0);

        BOOST_TEST(sys.boundary().lower_bound().at(0) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.boundary().lower_bound().at(1) == 1.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.boundary().lower_bound().at(2) == 2.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.boundary().upper_bound().at(0) == 10.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.boundary().upper_bound().at(1) == 11.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.boundary().upper_bound().at(2) == 12.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.size() == 3u);

        BOOST_TEST(sys.mass(0) == 1.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.mass(1) == 2.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.mass(2) == 3.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.rmass(0) == 1.0 / 1.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.rmass(1) == 1.0 / 2.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.rmass(2) == 1.0 / 3.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.position(0).at(0) == 1.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(0).at(1) == 2.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(0).at(2) == 3.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(1).at(0) == 1.2, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(1).at(1) == 2.2, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(1).at(2) == 3.2, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(2).at(0) == 1.4, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(2).at(1) == 2.4, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.position(2).at(2) == 3.4, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.velocity(0).at(0) == 1.1, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(0).at(1) == 2.1, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(0).at(2) == 3.1, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(1).at(0) == 1.3, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(1).at(1) == 2.3, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(1).at(2) == 3.3, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(2).at(0) == 1.5, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(2).at(1) == 2.5, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.velocity(2).at(2) == 3.5, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.force(0).at(0) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(0).at(1) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(0).at(2) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(1).at(0) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(1).at(1) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(1).at(2) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(2).at(0) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(2).at(1) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(2).at(2) == 0.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(sys.name(0) == "testnm");
        BOOST_TEST(sys.name(1) == "testnm");
        BOOST_TEST(sys.name(2) == "X");

        BOOST_TEST(sys.group(0) == "testgrp");
        BOOST_TEST(sys.group(1) == "testgrp");
        BOOST_TEST(sys.group(2) == "NONE");

        BOOST_TEST(sys.has_attribute("test_attr"));
        BOOST_TEST(sys.attribute("test_attr") == 3.14, boost::test_tools::tolerance(tol));
    }
}