//============================================================================
//
// This file is part of the Shapes Generative Model Library (SGML) project.
//
// Copyright (c) 2010-2014 - Evangelos Kalogerakis, Siddhartha Chaudhuri (authors of the code) / Stanford University, UMass-Amherst
//
// SGML is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.

// SGML is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.

// You should have received a copy of the GNU General Public License
// along with SGML.  If not, see <http://www.gnu.org/licenses/>.
//
//============================================================================

#ifndef __S12_MATRIXOPS_HPP
#define __S12_MATRIXOPS_HPP

#include "S12_Common.hpp"
#include "S12_Gamma.hpp"
#include <cmath>
#include <cstdlib>
#include <stdexcept>

namespace SNL {
namespace S12 {

// helper functions

template<class T>
struct IndexCmp
{
  T V;
  IndexCmp(T _V) : V(_V) {}
  bool operator()(const size_t a, const size_t b) const
  {
    return V[a] < V[b];
  }
};

template <class T>
size_t size(const SNL_VECTOR( T ) & v)
{
  return v.size();
}


template <class T>
T unifRand(T a, T b)
{
  double ad = double(a);
  double bd = double(b);
  double unifrand = double( rand() ) / double(RAND_MAX);
  return T( (bd-ad)*unifrand + ad );
}


template<class T>
string toString(const T& value)
{
  std::ostringstream oss;
  oss << value;
  return oss.str();
}


template <class T>
SNL_MATRIX(T) invert(const SNL_MATRIX(T) & cov, bool* problem)
{
  SNL_MATRIX(T) invcov(cov.numRows(), cov.numColumns());
  //double sumabscov = sumabs(cov);

  //// std::cerr << "inverting: " << cov << std::endl;

  //if ( sumabscov > SUPERMAXEPSILON )
  //{
  //  invcov.fill(0);
  //  return invcov;
  //}
  //if ( sumabs(cov) == 0.0 )
  //{
  //  invcov.fill(SUPERMAXEPSILON);
  //  return invcov;
  //}

  char UPLO = 'L';
  integer N = (integer)cov.numRows();
  double * COV = new double[ cov.numRows() * (cov.numRows() + 1) / 2 ];
  double * WORK = new double[ cov.numRows() ];
  integer * IPIV = new integer[ cov.numRows() ];
  integer INFO = 0;

  for (size_t i = 0; i < cov.numRows(); i++)
  {
    for (size_t j = 0; j <= i; j++)
    {
      if (i != j)
        COV[i + j * (2 * cov.numRows() - j - 1) / 2] = double(cov[i][j]);
      else
        COV[i + j * (2 * cov.numRows() - j - 1) / 2] = double(cov[i][j]);
    }
  }

  dsptrf_(&UPLO, &N, COV, IPIV, &INFO);

  if (INFO != 0)
  {
    std::cerr << "invert: Singular matrix:" << std::endl;
    std::cerr << cov;

    std::cout << "invert failed in dsptrf_ with exit code " << INFO << std::endl;

    invcov.fill(0);
    for (size_t i = 0; i < cov.numRows(); i++)
    {
      invcov[i][i] = 1.0 / (cov[i][i]+SUPEREPSILON);
    }

    *problem = true;

#if 0
    // cause seg fault to see what's wrong
    char * ch = NULL;
    *ch = 'A';
#endif

    return invcov;
  }

  dsptri_(&UPLO, &N, COV, IPIV, WORK, &INFO);

  if (INFO != 0)
  {
    std::cerr << "invert: Singular matrix:" << std::endl;
    std::cerr << cov;

    std::cout << "invert failed in dsptri_ with exit code " << INFO << std::endl;

    invcov.fill(0);
    for (size_t i = 0; i < cov.numRows(); i++)
    {
      invcov[i][i] = 1.0 / (cov[i][i]+SUPEREPSILON);
    }
    *problem = true;

//    invcov[1000][1000] = 343248.0; // cause seg fault to see what's wrong

    return invcov;
  }

  for (size_t i = 0; i < cov.numRows(); i++)
  {
    for (size_t j = 0; j <= i; j++)
    {
      invcov[i][j] = T(COV[i + j * (2 * cov.numRows() - j - 1) / 2]);
      invcov[j][i] = invcov[i][j];
    }
  }

  delete[] COV;
  delete[] WORK;
  delete[] IPIV;
  return invcov;
}


template <class T>
double determinant(const SNL_MATRIX(T) & cov, size_t & rank)
{
  double determinant = 1.0;
  char JOBZ = 'N';
  char UPLO = 'L';

  assert(cov.isSquare());

  integer N = cov.numRows();
  double * EIG = new double[ N ];
  double * WORK = new double[(N + 2) * N ];
  integer LWORK = (N + 2) * N;
  double * COV = new double[ N * N ];
  integer INFO = 0;

  for (size_t i = 0; i < cov.numRows(); i++)
    for (size_t j = 0; j < cov.numColumns(); j++)
      COV[i + cov.numRows()*j] = double(cov[i][j]);

  dsyev_(&JOBZ, &UPLO, &N, COV, &N, EIG, WORK, &LWORK, &INFO);

  rank = 0;

  for (size_t i = 0; i < cov.numRows(); i++)
  {
    if (EIG[i] > SUPEREPSILON)
    {
      rank++;
      determinant *= EIG[i];
    }
    //determinant *= T(max(EIG[i], T(0.0)));
  }

  delete[] EIG;
  delete[] WORK;
  delete[] COV;
  return determinant;
}


template <class T>
double determinant(const SNL_MATRIX(T) & cov)
{
  size_t rank = 0;
  return determinant<T>(cov, rank);
}


template <class T>
SNL_VECTOR(T) increasingNumberVector(T a, T b)
{
  SNL_VECTOR(T) v(b - a);
  for (T i = a; i < b; i++)
  {
    v[i - a] = i;
  }
  return v;
}

template <class T>
void fill(SNL_VECTOR( T )& v, T value)
{
  for (size_t i = 0; i < v.size(); i++)
  {
    v[i] = value;
  }
}


template <class T>
SNL_VECTOR(T) replaceElementsOfYAccordingToX(const SNL_VECTOR( T ) & x, const SNL_MATRIX( T ) & y)
{
  SNL_VECTOR(T) yx(x.size());

  for (size_t d = 0; d < x.size(); d++)
  {
    yx[d] = y[d][ x[d] ];
  }

  return yx;
}


template <class T>
SNL_VECTOR(T) replaceElementsOfYAccordingToX(const SNL_VECTOR( T ) & x, const SNL_VECTOR( SNL_VECTOR(T) ) & y)
{
  SNL_VECTOR(T) yx(x.size());

  for (size_t d = 0; d < x.size(); d++)
  {
    yx[d] = y[d][ x[d] ];
  }

  return yx;
}


template <class T>
SNL_VECTOR(T) subvector(const SNL_VECTOR( T ) & x, size_t d1)
{
  SNL_VECTOR(T) xx(x.size() - d1);
  size_t s = 0;

  for (size_t d = d1; d < x.size(); d++, s++)
  {
    xx[s] = x[d];
  }

  return xx;
}


template <class T>
SNL_VECTOR(T) subvector(const SNL_VECTOR( T ) & x, size_t d1, size_t d2)
{
  SNL_VECTOR(T) xx(d2 - d1 + 1);
  size_t s = 0;

  for (size_t d = d1; d <= d2; d++, s++)
  {
    xx[s] = x[d];
  }

  return xx;
}


template <class T>
SNL_MATRIX(T) subsqmatrix(const SNL_MATRIX( T ) & x, size_t d1, size_t d2)
{
  SNL_MATRIX(T) xx(d2 - d1 + 1, d2 - d1 + 1);
  size_t s1 = 0;

  for (size_t da = d1; da <= d2; da++, s1++)
  {
    size_t s2 = 0;

    for (size_t db = d1; db <= d2; db++, s2++)
    {
      xx[s1][s2] = x[da][db];
    }
  }

  return xx;
}


template <class T>
inline SNL_MATRIX(T) mult(const SNL_MATRIX(T) & x, const SNL_MATRIX(T) & y)
{
  size_t M = x.numRows(), N = x.numColumns();
  if (M == 0)
  {
    return SNL_MATRIX(T)(0, 0);
  }

  size_t L = y.numRows(), K = y.numColumns();
  if (N == 0 || L == 0)
  {
    return SNL_MATRIX(T)(0, 0);
  }

  if (N != L)
  {
    std::cout << "mult: number of columns in x is not equal to number of rows in y" << std::endl;
    std::cout << "x is " << M << "x" <<  N << std::endl;
    std::cout << "y is " << L << "x" <<  K << std::endl;
    exit(-1);
  }

  SNL_MATRIX(T) z(M, K);

  for (size_t i = 0; i < M; i++)
  {
    for (size_t j = 0; j < K; j++)
    {
      z[i][j] = (T)0;

      for (size_t r = 0; r < N; r++)
      {
        z[i][j] += x[i][r] * y[r][j];
      }
    }
  }

  return z;
}


template <class T1, class T2>
inline SNL_MATRIX(T1) mult1(const SNL_MATRIX(T1) & x, const SNL_MATRIX(T2) & y)
{
  size_t M = x.numRows(), N = x.numColumns();
  if (M == 0)
  {
    return SNL_MATRIX(T1)(0, 0);
  }

  size_t L = y.numRows(), K = y.numColumns();
  if (N == 0 || L == 0)
  {
    return SNL_MATRIX(T1)(0, 0);
  }

  if (N != L)
  {
    std::cout << "mult1: number of columns in x is not equal to number of rows in y" << std::endl;
    std::cout << "x is " << M << "x" <<  N << std::endl;
    std::cout << "y is " << L << "x" <<  K << std::endl;
    exit(-1);
  }

  SNL_MATRIX(T1) z(M, K);

  for (size_t i = 0; i < M; i++)
  {
    for (size_t j = 0; j < K; j++)
    {
      z[i][j] = (T1)0;

      for (size_t r = 0; r < N; r++)
      {
        z[i][j] += x[i][r] * (T1)y[r][j];
      }
    }
  }

  return z;
}


template <class T1, class T2>
inline SNL_MATRIX(T2) mult2(const SNL_MATRIX(T1)& x, const SNL_MATRIX(T2)& y)
{
  size_t M = x.numRows(), N = x.numColumns();
  if (M == 0)
  {
    return SNL_MATRIX(T2)(0, 0);
  }

  size_t L = y.numRows(), K = y.numColumns();
  if (N == 0 || L == 0)
  {
    return SNL_MATRIX(T2)(0, 0);
  }

  if (N != L)
  {
    std::cout << "mult2: number of columns in x is not equal to number of rows in y" << std::endl;
    std::cout << "x is " << M << "x" <<  N << std::endl;
    std::cout << "y is " << L << "x" <<  K << std::endl;
    exit(-1);
  }

  SNL_MATRIX(T2) z(M, K);

  for (size_t i = 0; i < M; i++)
  {
    for (size_t j = 0; j < K; j++)
    {
      z[i][j] = (T2)0;

      for (size_t r = 0; r < N; r++)
      {
        z[i][j] += (T2)x[i][r] * y[r][j];
      }
    }
  }

  return z;
}


template <class T>
inline SNL_VECTOR(T) mult(const SNL_MATRIX(T) & x, const SNL_VECTOR( T ) & y)
{
  // std::cerr << "mult matrix-vector:" << std::endl;
  // std::cerr << "  M = " << x;
  // std::cerr << "  V = " << y;

  size_t M = x.numRows();
  SNL_VECTOR( T ) z(M, 0);
  if (M == 0)
  {
    return z;
  }
  size_t N = x.numColumns();
  size_t L = y.size();
  if (N == 0 || L == 0)
  {
    return z;
  }

  if (N != L)
  {
    std::cout << "mult matrix-vector: number of columns in x is not equal to number of rows in y" << std::endl;
    std::cout << "x is " << M << "x" <<  N << std::endl;
    std::cout << "y is " << L << "x1" << std::endl;
    exit(-1);
  }

  for (size_t i = 0; i < M; i++)
  {
    for (size_t r = 0; r < N; r++)
    {
      z[i] += x[i][r] * y[r];
    }
  }

  return z;
}


template <class T>
inline SNL_VECTOR(T) mult(const SNL_VECTOR( T ) & y, const SNL_MATRIX(T) & x)
{
  size_t M = x.numRows();
  SNL_VECTOR( T ) z(M, 0);
  if (M == 0)
  {
    return z;
  }
  size_t N = x.numColumns();
  size_t L = y.size();
  if (N == 0 || L == 0)
  {
    return z;
  }

  if (N != L)
  {
    std::cout << "mult matrix-vector: number of columns in x is not equal to number of rows in y" << std::endl;
    std::cout << "x is " << M << "x" <<  N << std::endl;
    std::cout << "y is " << L << "x1" << std::endl;
    exit(-1);
  }

  for (size_t i = 0; i < M; i++)
  {
    for (size_t r = 0; r < N; r++)
    {
      z[i] += x[i][r] * y[r];
    }
  }

  return z;
}


template <class T>
inline SNL_VECTOR(T) mult(const SNL_VECTOR(T) & x, T y)
{
  SNL_VECTOR(T) z = x;
  for (size_t d = 0; d < x.size(); d++)
    z[d] *=  y;
  return z;
}


template <class T>
inline SNL_VECTOR(T) mult(T y, const SNL_VECTOR(T) & x)
{
  // std::cerr << "mult scalar-vector:" << std::endl;
  // std::cerr << "  S = " << y << std::endl;
  // std::cerr << "  V = " << x;

  SNL_VECTOR(T) z = x;
  for (size_t d = 0; d < x.size(); d++)
    z[d] *=  y;
  return z;
}


template <class T>
inline SNL_MATRIX(T) mult(const SNL_MATRIX(T) & x, T y)
{
  SNL_MATRIX(T) z = x;
  for (size_t i = 0; i < x.numRows(); i++)
    for (size_t j = 0; j < x.numColumns(); j++)
      z[i][j] *= y;

  return z;
}


template <class T>
inline SNL_MATRIX(T) mult(T m, const SNL_MATRIX(T) & x)
{
  SNL_MATRIX(T) z = x;
  for (size_t i = 0; i < x.numRows(); i++)
    for (size_t j = 0; j < x.numColumns(); j++)
      z[i][j] *= m;

  return z;
}


template <class T>
inline SNL_VECTOR(T) mult(const SNL_VECTOR(T) & x, const SNL_VECTOR(T) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "mult vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  SNL_VECTOR(T) z(x.size(), T(0));

  for (size_t d = 0; d < x.size(); d++)
    z[d] = x[d] * y[d];

  return z;
}


template <class T1, class T2>
inline SNL_VECTOR(T1) mult1(const SNL_VECTOR(T1) & x, const SNL_VECTOR(T2) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "mult1 vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  SNL_VECTOR(T1) z(x.size(), T1(0));

  for (size_t d = 0; d < x.size(); d++)
    z[d] = x[d] * (T1)y[d];

  return z;
}


template <class T1, class T2>
inline SNL_VECTOR(T2) mult2(const SNL_VECTOR(T1) & x, const SNL_VECTOR(T2) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "mult2 vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  SNL_VECTOR(T2) z(x.size(), T2(0));

  for (size_t d = 0; d < x.size(); d++)
    z[d] = (T2)x[d] * y[d];

  return z;
}


template <class T>
inline T dot(const SNL_VECTOR(T)& x, const SNL_VECTOR(T)& y)
{
  if (x.size() != y.size())
  {
    std::cout << "dot vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  T z = T(0);
  for (size_t d = 0; d < x.size(); d++)
    z += x[d]*y[d];
  return z;
}


template <class T>
inline SNL_VECTOR(T) add(const SNL_VECTOR(T) & x, const SNL_VECTOR(T) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "add vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  SNL_VECTOR(T) z(x.size(), T(0));

  for (size_t d = 0; d < x.size(); d++)
    z[d] = x[d] + y[d];

  return z;
}


template <class T>
inline SNL_VECTOR(T) add(const SNL_VECTOR(T) & x, T y)
{
  SNL_VECTOR(T) z(x.size(), T(0));

  for (size_t d = 0; d < x.size(); d++)
    z[d] = x[d] + y;

  return z;
}


template <class T>
inline SNL_MATRIX(T) add(const SNL_MATRIX(T) & x, const SNL_MATRIX(T) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "add vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }
  SNL_MATRIX(T) z(x.numRows(), x.numColumns(), T(0));

  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      z[d][d2] = x[d][d2] + y[d][d2];

  return z;
}


template <class T>
inline SNL_MATRIX(T) regularize(const SNL_MATRIX(T) & x, T reg)
{
  SNL_MATRIX(T) z(x.numRows(), x.numColumns(), T(0));

  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      z[d][d2] = x[d][d2] * (1 - reg);

  for (size_t d = 0; d < x.numRows(); d++)
    z[d][d] = x[d][d] + reg;

  return z;
}


template <class T>
inline int numUniqueNonZeroEntries(const SNL_MATRIX(T) & x)
{
  int num_dependent_entries = 0;
  int num_entries = 0;

  for (size_t d = 0; d < x.numRows(); d++)
  {
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
    {
      num_entries++;
      if ( x[d][d2] == T(0) )
        num_dependent_entries++;
      else if ( abs( x[d][d2] - x[d2][d] ) <= T(EPSILON) )
        num_dependent_entries++;
    }
  }

  return num_entries - num_dependent_entries;
}


template <class T>
inline int numNonZeroEntries(const SNL_MATRIX(T) & x)
{
  int num_dependent_entries = 0;
  int num_entries = 0;

  for (size_t d = 0; d < x.numRows(); d++)
  {
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
    {
      num_entries++;
      if ( x[d][d2] == T(0) )
        num_dependent_entries++;
    }
  }

  return num_entries - num_dependent_entries;
}


template <class T>
inline bool equals(const SNL_VECTOR( T ) & x, const SNL_VECTOR( T ) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "equals vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  for (size_t d = 0; d < x.size(); d++)
    if (x[d] != y[d])
      return false;

  return true;
}


template <class T1, class T2>
inline bool equals(const SNL_VECTOR( T1 ) & x, const SNL_VECTOR( T2 ) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "equals vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  for (size_t d = 0; d < x.size(); d++)
    if (x[d] != (T1)y[d])
      return false;

  return true;
}


template <class T>
inline bool equals(const SNL_MATRIX(T) & x, const SNL_MATRIX(T) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "equals vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      if (x[d][d2] != y[d][d2])
        return false;

  return true;
}


template <class T>
inline SNL_VECTOR(T) subtract(const SNL_VECTOR(T) & x, const SNL_VECTOR(T) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "subtract vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  if (x.empty() || y.empty())
    return x;
  SNL_VECTOR(T) z(x.size(), T(0));

  for (size_t d = 0; d < x.size(); d++)
    z[d] = x[d] - y[d];

  return z;
}


template <class T1, class T2>
inline SNL_VECTOR(T1) subtract1(const SNL_VECTOR(T1) & x, const SNL_VECTOR(T2) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "subtract1 vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  if (x.empty() || y.empty())
    return x;
  SNL_VECTOR(T1) z(x.size(), T1(0));

  for (size_t d = 0; d < x.size(); d++)
    z[d] = x[d] - (T1)y[d];

  return z;
}


template <class T1, class T2>
inline SNL_VECTOR(T2) subtract2(const SNL_VECTOR(T1) & x, const SNL_VECTOR(T2) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "subtract2 vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  if (x.empty() || y.empty())
    return y;
  SNL_VECTOR(T2) z(x.size(), T2(0));

  for (size_t d = 0; d < x.size(); d++)
    z[d] = (T2)x[d] - y[d];

  return z;
}


template <class T>
inline SNL_MATRIX(T) subtract(const SNL_MATRIX(T) & x, const SNL_MATRIX(T) & y)
{
	if ( (x.numRows() != y.numRows()) || (x.numColumns() != y.numColumns()) )
  {
    std::cout << "subtract vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.numRows() << "rows, " << x.numColumns() << "columns, "  << std::endl;
    std::cout << "y has " << y.numRows() << "rows, " << y.numColumns() << "columns, "  << std::endl;
    exit(-1);
  }

  if (x.isEmpty() || y.isEmpty())
    return x;
  SNL_MATRIX(T) z(x.numRows(), x.numColumns());

  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      z[d][d2] = x[d][d2] - y[d][d2];

  return z;
}


template <class T1, class T2>
inline SNL_MATRIX(T1) subtract1(const SNL_MATRIX(T1) & x, const SNL_MATRIX(T2) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "subtract1 vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  if (x.isEmpty() || y.isEmpty())
    return x;
  SNL_MATRIX(T1) z(x.numRows(), x.numColumns());

  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      z[d][d2] = x[d][d2] - (T1)y[d][d2];

  return z;
}


template <class T1, class T2>
inline SNL_MATRIX(T2) subtract2(const SNL_MATRIX(T1) & x, const SNL_MATRIX(T2) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "subtract2 vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  if (x.isEmpty() || y.isEmpty())
    return y;
  SNL_MATRIX(T2) z(x.numRows(), x.numColumns());

  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      z[d][d2] = (T2)x[d][d2] - y[d][d2];

  return z;
}


template <class T>
inline SNL_MATRIX(T) transpose(const SNL_MATRIX(T) & x)
{
  SNL_MATRIX(T) z(x.numColumns(), x.numRows());

  for (size_t d = 0; d < x.numColumns(); d++)
    for (size_t d2 = 0; d2 < x.numRows(); d2++)
      z[d][d2] = x[d2][d];

  return z;
}


template <class T>
inline T sum(const SNL_VECTOR(T)& x)
{
  T sum = 0.0;

  for (size_t d = 0; d < x.size(); d++)
    sum += x[d];

  return sum;
}


template <class T>
inline T sum(const SNL_MATRIX(T) & x)
{
  T sum = 0.0;

  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      sum += x[d][d2];

  return sum;
}


template <class T>
inline T sumabs(const SNL_MATRIX(T) & x)
{
  if (x.isEmpty())
    return std::numeric_limits<T>::max();

  T sum = 0.0;

  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      sum += abs(x[d][d2]);

  return sum;
}


template <class T, class T2>
inline SNL_VECTOR(T2) convert(const SNL_VECTOR(T) & x)
{
  SNL_VECTOR(T2) xx(x.size());

  for (size_t d = 0; d < x.size(); d++)
  {
    xx[d] = T2(x[d]);
  }

  return xx;
}


template <class T>
inline T dist2(const SNL_VECTOR(T) & x, const SNL_VECTOR(T) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "dist2 vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  T dist2 = 0.0;

  for (size_t d = 0; d < x.size(); d++)
    dist2 += (y[d] - x[d]) * (y[d] - x[d]);

  return dist2;
}


template <class T>
inline T dist(const SNL_VECTOR(T) & x, const SNL_VECTOR(T) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "dist vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  T dist2 = 0.0;

  for (size_t d = 0; d < x.size(); d++)
    dist2 += (y[d] - x[d]) * (y[d] - x[d]);

  return sqrt(dist2);
}


template <class T1, class T2>
inline T1 dist(const SNL_VECTOR(T1) & x, const SNL_VECTOR(T2) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "dist vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  T1 dist2 = 0.0;

  for (size_t d = 0; d < x.size(); d++)
    dist2 += ((T1)y[d] - x[d]) * ((T1)y[d] - x[d]);

  return sqrt(dist2);
}


template <class T1, class T2>
inline T1 dist2(const SNL_VECTOR(T1) & x, const SNL_VECTOR(T2) & y)
{
  if (x.size() != y.size())
  {
    std::cout << "dist2 vector-vector: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  T1 dist2 = 0.0;

  for (size_t d = 0; d < x.size(); d++)
    dist2 += ((T1)y[d] - x[d]) * ((T1)y[d] - x[d]);

  return dist2;
}


template <class T>
inline SNL_MATRIX(double) covfromvector(const SNL_VECTOR(T) & x, T regul)
{
  SNL_MATRIX(double) c(x.size(), x.size());

  for (size_t d = 0; d < x.size(); d++)
    for (size_t d2 = 0; d2 < x.size(); d2++)
      c[d][d2] = x[d] * x[d2] * (1 - regul);

  for (size_t d = 0; d < x.size(); d++)
    c[d][d] += regul;                   // regularization

  return c;
}


template <class T>
inline SNL_MATRIX(double) covfromvector(const SNL_VECTOR(T) & x, const SNL_VECTOR(T) & y)
{
  SNL_MATRIX(double) c(x.size(), y.size());

  for (size_t d = 0; d < x.size(); d++)
    for (size_t d2 = 0; d2 < y.size(); d2++)
      c[d][d2] = x[d] * y[d2];

  return c;
}


template <class T>
inline void concatenateUniqueVectors(const SNL_VECTOR(T) & v1, const SNL_VECTOR(T) & v2)
{
  for (size_t j = 0; j < v2.size(); j++)
  {
    bool found = false;

    for (size_t i = 0; i < v1.size(); i++)
    {
      if (v2[j] == v1[i])
        found = true;
    }

    if (!found)
      v1.push_back(v2[j]);
  }
}


template <class T>
inline SNL_VECTOR(T) concatenateVectors(const SNL_VECTOR(T)& v1, const SNL_VECTOR(T)& v2)
{
  SNL_VECTOR(T) newv = v1;
  newv.insert(newv.end(), v2.begin(), v2.end());
  return newv;
}


template <class T, class T2>
inline SNL_VECTOR(T) concatenateVectors(const SNL_VECTOR(T)& v1, const SNL_VECTOR(T2)& v2)
{
  SNL_VECTOR(T) newv = v1;
  for (size_t d = 0; d < v2.size(); d++)
  {
    newv.push_back((T)v2[d]);
  }
  return newv;
}

template <class T, class T2>
inline SNL_VECTOR(T) concatenateVectors(const SNL_VECTOR(T)& v1, const SNL_MATRIX(T2)& v2)
{
  SNL_VECTOR(T) newv;

  size_t total_size = v1.size() + v2.numRows() * v2.numColumns();
  newv.reserve(total_size);

  for (size_t d = 0; d < v1.size(); d++)
    newv.push_back(v1[d]);

  for (size_t d = 0; d < v2.numRows(); d++)
  {
    for (size_t d2 = 0; d2 < v2.numColumns(); d2++)
    {
      newv.push_back((T)v2[d][d2]);
    }
  }
  return newv;
}


template <class T>
inline SNL_MATRIX(T) concatenateMatrixHorizontally(const SNL_MATRIX(T)& v1, const SNL_MATRIX(T)& v2)
{
  if ( v1.numRows() != v2.numRows() )
  {
    std::cout << "concatenateMatrixHorizontally: input v1 and v2 matrices should have the same number of rows (v1.size() == v2.size())" << std::endl;
    exit(-1);
  }

  SNL_MATRIX(T) v( v1.numRows(), v1.numColumns() + v2.numColumns() );
  for (size_t d = 0; d < v1.numRows(); d++)
  {
    for (size_t d2 = 0; d2 < v1.numColumns(); d2++)
    {
      v[d][d2] = v1[d][d2];
    }
    for (size_t d2 = 0; d2 < v2.numColumns(); d2++)
    {
      v[d][d2 + v1.numColumns()] = v2[d][d2];
    }
  }

  return v;
}


template <class T>
inline SNL_MATRIX(T) concatenateMatrixVertically(const SNL_MATRIX(T)& v1, const SNL_MATRIX(T)& v2)
{
  if ( v1.numColumns() != v2.numColumns() )
  {
    std::cout << "concatenateMatrixVertically: input v1 and v2 matrices should have the same number of columns (v1[0].size() == v2[0].size())" << std::endl;
    exit(-1);
  }

  SNL_MATRIX(T) v( v1.numRows() + v2.numRows(), v1.numColumns() );
  for (size_t d = 0; d < v1.numRows(); d++)
  {
    for (size_t d2 = 0; d2 < v1.numColumns(); d2++)
      v[d][d2] = v1[d][d2];
  }
  for (size_t d = 0; d < v2.numRows(); d++)
  {
    for (size_t d2 = 0; d2 < v2.numColumns(); d2++)
      v[d + v1.numRows()][d2] = v2[d][d2];
  }

  return v;
}


template <class T>
inline SNL_VECTOR(size_t) sortVectorIndices(const SNL_VECTOR(T) & V)
{
  SNL_VECTOR(size_t) VI(V.size());

  for (size_t i = 0; i < V.size(); i++)
  {
    VI[i] = i;
  }

  sort(VI.begin(), VI.end(), IndexCmp<SNL_VECTOR(T)&>(V));
  return VI;
}


template <class T>
inline T maximum(T * v, size_t size)
{
  T result = boost::numeric::bounds<T>::lowest();

  for (size_t i = 0; i < size; i++)
  {
    if (v[i] > result)
    {
      result = v[i];
    }
  }

  return result;
}


template <class T>
inline T maximum(const SNL_VECTOR(T) & v)
{
  T result = boost::numeric::bounds<T>::lowest();

  for (int i = 0; i < v.size(); i++)
  {
    if (v[i] > result)
    {
      result = v[i];
    }
  }

  return result;
}


template <class T>
inline T maximumabs(const SNL_VECTOR(T) & v)
{
  T result = boost::numeric::bounds<T>::lowest();

  for (int i = 0; i < v.size(); i++)
  {
    if (abs(v[i]) > result)
    {
      result = abs(v[i]);
    }
  }

  return result;
}


template <class T>
inline size_t maximumId(const SNL_VECTOR(T) & v)
{
  size_t id = 0;
  T result = boost::numeric::bounds<T>::lowest();

  for (size_t i = 0; i < v.size(); i++)
  {
    if (v[i] > result)
    {
      result = v[i];
      id = i;
    }
  }

  return id;
}


template <class T>
inline SNL_VECTOR(size_t) maximumId_perrow(const SNL_MATRIX(T) & v)
{
  SNL_VECTOR(size_t) maxid(v.numRows());

  for (size_t d = 0; d < v.numRows(); d++)
  {
    maxid[d] = 0;
    T result = boost::numeric::bounds<T>::lowest();

    for (size_t d2 = 0; d2 < v.numColumns(); d2++)
    {
      if (v[d][d2] > result)
      {
        result = v[d][d2];
        maxid[d] = d2;
      }
    }
  }

  return maxid;
}


template <class T>
inline size_t maximumId_percolumn(const SNL_MATRIX(T) & v)
{
  SNL_VECTOR(size_t) maxid(v.numColumns());

  for (size_t d = 0; d < v.numColumns(); d++)
  {
    maxid[d] = 0;
    T result = boost::numeric::bounds<T>::lowest();

    for (size_t d2 = 0; d2 < v.numRows(); d2++)
    {
      if (v[d2][d] > result)
      {
        result = v[d2][d];
        maxid[d] = d2;
      }
    }
  }

  return maxid;
}


template <class T>
inline T minimum(const SNL_VECTOR(T) & v)
{
  T result = std::numeric_limits<T>::max();

  for (int i = 0; i < v.size(); i++)
  {
    if (v[i] < result)
    {
      result = v[i];
    }
  }

  return result;
}


template <class T>
inline size_t minimumId(const SNL_VECTOR(T) & v)
{
  size_t id = 0;
  T result = std::numeric_limits<T>::max();

  for (size_t i = 0; i < v.size(); i++)
  {
    if (v[i] < result)
    {
      result = v[i];
      id = i;
    }
  }

  return id;
}


template <class T>
inline T mean(const SNL_VECTOR(T) & v)
{
  T result = 0;

  for (int i = 0; i < v.size(); i++)
  {
    result += v[i];
  }

  result /= (T)v.size();
  return result;
}


template <class T>
inline T weightedmean(const SNL_VECTOR(T) & v, const SNL_VECTOR(T) & w)
{
  if (v.size() != w.size())
  {
    std::cout << "weightedmean : number of elements in v is not equal to number of elements in w" << std::endl;
    std::cout << "v has " << v.size() << "elements" << std::endl;
    std::cout << "w has " << w.size() << "elements" << std::endl;
    exit(-1);
  }

  T result = 0;
  T sumw = 0;

  for (int i = 0; i < v.size(); i++)
  {
    result += v[i] * w[i];
    sumw += w[i];
  }

  result /= sumw;
  return result;
}


template <class T>
inline T median(const SNL_VECTOR(T)& w)
{
  SNL_VECTOR(T) v = w;
  std::sort(v.begin(), v.end());

  if (v.size() % 2 == 0)
  {
    return (v[ v.size() / 2 ] +  v[(v.size() / 2) - 1 ]) / 2.0f;
  }
  else
  {
    return v[ v.size() / 2 ];
  }
}


template <class T>
inline T weightedmedian(const SNL_VECTOR(T) & v, const SNL_VECTOR(T) & w)
{
  if (v.size() != w.size())
  {
    std::cout << "weightedmedian : number of elements in v is not equal to number of elements in w" << std::endl;
    std::cout << "v has " << v.size() << "elements" << std::endl;
    std::cout << "w has " << w.size() << "elements" << std::endl;
    exit(-1);
  }

  SNL_VECTOR(size_t) VI = sortVectorIndices(v);
  double sumw = 0.0f, csumw = 0.0f;
  int i;

  for (i = 0; i < w.size(); i++)
  {
    sumw += (double)w[i];
  }

  for (i = 0; i < w.size(); i++)
  {
    csumw += (double)w[ VI[i] ] / (double)sumw;

    if (csumw >= .5f)
      break;
  }

  return v[ VI[i] ];
}


template <class T>
inline T mean2(const SNL_VECTOR(T) & v)
{
  T result = 0;

  for (int i = 0; i < v.size(); i++)
  {
    result += v[i] * v[i];
  }

  result /= (T)v.size();
  return result;
}


template <class T>
inline T mean4(const SNL_VECTOR(T) & v)
{
  T result = 0;

  for (int i = 0; i < v.size(); i++)
  {
    result += v[i] * v[i] * v[i] * v[i];
  }

  result /= (T)v.size();
  return result;
}


template <class T>
inline T meansqrt(const SNL_VECTOR(T) & v)
{
  T result = 0;

  for (int i = 0; i < v.size(); i++)
  {
    result += sqrt(v[i]);
  }

  result /= (T)v.size();
  return result;
}


template <class T>
inline T meansqrtsqrt(const SNL_VECTOR(T) & v)
{
  T result = 0;

  for (int i = 0; i < v.size(); i++)
  {
    result += sqrt(sqrt(v[i]));
  }

  result /= (T)v.size();
  return result;
}


template <class T>
inline T variance(const SNL_VECTOR(T) & v, T meanValue)
{
  T result = 0;

  for (int i = 0; i < v.size(); i++)
  {
    result += (v[i] - meanValue) * (v[i] - meanValue);
  }

  result /= (T)v.size();
  return result;
}


template <class T>
inline T percentile(const SNL_VECTOR(T) v, float k)
{
  std::sort(v.begin(), v.end());

  if (k == 0.0f)
    return v[0];
  else if (k == 1.0f)
    return v[ v.size() - 1 ];
  else
    return v[(int)floor(k * (float)v.size()) ];
}


template <class T>
inline T robust_variance(const SNL_VECTOR(T) & v, T meanValue)
{
  T result = 0;
  T v1 = percentile(v, .1f);
  T v2 = percentile(v, .9f);

  for (size_t i = 0; i < v.size(); i++)
  {
    if (v[i] > v1 && v[i] < v2 && fabs(v[i]) < FLT_MAX && v[i] == v[i])
      result += (v[i] - meanValue) * (v[i] - meanValue);
  }

  result /= (T)v.size();
  return result;
}


template <class T>
inline T skewness(const SNL_VECTOR(T) & v, T meanValue, T varianceValue)
{
  T result = 0;

  for (int i = 0; i < v.size(); i++)
  {
    result += (v[i] - meanValue) * (v[i] - meanValue) * (v[i] - meanValue);
  }

  result /= (T)v.size();
  result /= sqrt(varianceValue * varianceValue * varianceValue + EPSILON);
  return result;
}


template <class T>
inline T kurtosis(const SNL_VECTOR(T) & v, T meanValue, T varianceValue)
{
  T result = 0;

  for (int i = 0; i < v.size(); i++)
  {
    result += (v[i] - meanValue) * (v[i] - meanValue) * (v[i] - meanValue) * (v[i] - meanValue);
  }

  result /= (T)v.size();
  result /= (varianceValue * varianceValue  + EPSILON);
  return result;
}


template <class T>
inline SNL_VECTOR(T) unique(const SNL_VECTOR(T) & v)
{
  SNL_VECTOR(T) ret;
  if (v.empty())
    return ret;

  ret.reserve(v.size());
  ret.push_back(v[0]);

  for (size_t i = 1; i < v.size(); i++)
  {
    bool isunique = true;

    for (size_t j = 0; j < ret.size(); j++)
    {
      if (v[i] == ret[j])
      {
        isunique = false;
        break;
      }
    }

    if (isunique == true)
    {
      ret.push_back(v[i]);
    }
  }

  return ret;
}


template <class T>
inline SNL_VECTOR(T) removePositiveOutliers(const SNL_VECTOR(T) & v, float percentile)
{
  SNL_VECTOR(T) result(v.size());

  for (int i = 0; i < v.size(); i++)
  {
    result[i] = fabs(v[i]);
  }

  std::sort(result.begin(), result.end());
  float outlierValue = result[(size_t)floor(percentile * (float)v.size()) ];

  for (int i = 0; i < v.size(); i++)
  {
    if (fabs(v[i]) < outlierValue)
    {
      result[i] = v[i];
    }
    else
    {
      result[i] = sgn(v[i]) * outlierValue;
    }
  }

  return result;
}


template <class T>
inline SNL_VECTOR(T) removeOutliers(const SNL_VECTOR(T) & v, float percentile)
{
  SNL_VECTOR(T) result(v.size());

  for (int i = 0; i < v.size(); i++)
  {
    result[i] = v[i];
  }

  std::sort(result.begin(), result.end());
  float outlierValueMax = result[(size_t)floor(percentile * (float)v.size()) ];
  float outlierValueMin = result[(size_t)floor((1.0f - percentile) * (float)v.size()) ];

  for (int i = 0; i < v.size(); i++)
  {
    if (v[i] < outlierValueMin)
    {
      result[i] = outlierValueMin;
    }
    else if (v[i] > outlierValueMax)
    {
      result[i] = outlierValueMax;
    }
    else
    {
      result[i] = v[i];
    }
  }

  return result;
}


template <class T>
inline SNL_VECTOR(T) removeOutliers(const SNL_VECTOR(T) & v, float percentilePositive, float percentileNegative)
{
  SNL_VECTOR(T) result(v.size());

  for (int i = 0; i < v.size(); i++)
  {
    result[i] = v[i];
  }

  std::sort(result.begin(), result.end());
  float outlierValueMax = result[(size_t)floor(percentilePositive * (float)v.size()) ];
  float outlierValueMin = result[(size_t)floor(percentileNegative * (float)v.size()) ];

  for (int i = 0; i < v.size(); i++)
  {
    if (v[i] < outlierValueMin)
    {
      result[i] = outlierValueMin;
    }
    else if (v[i] > outlierValueMax)
    {
      result[i] = outlierValueMax;
    }
    else
    {
      result[i] = v[i];
    }
  }

  return result;
}


template <class T>
inline SNL_VECTOR(float) hist(const SNL_VECTOR(T) & v, int numbins, float minimum_v = T(0), float maximum_v = T(1))
{
  SNL_VECTOR(float) values(numbins, 0.0f);
  float lim = (float)minimum_v;
  float spacing = ((float)maximum_v - (float)minimum_v) / float(numbins);
  SNL_VECTOR(float) limits(numbins);

  for (size_t b = 0; (int)b < numbins; b++)
  {
    limits[b] = lim;
    lim += spacing;
  }

  for (size_t i = 0; i < v.size(); i++)
  {
    size_t b = 0;

    for (b = 0; (int)b < numbins - 1; b++)
      if ((float)v[i] >= limits[b] && (float)v[i] < limits[b + 1])
        break;

    values[b] += 1.0f;
  }

  T norm = sum(values);

  for (size_t i = 0; i < values.size(); i++)
    values[i] /= norm;

  return values;
}


inline double logcdk(double d, double k)
{
  double logcdk = 0.0;
  logcdk += (d * k / 2.0) * log(2.0);
  logcdk += (d * (d - 1.0) / 4.0) * log(M_PI);

  for (size_t i = 0; i < (size_t)d; i++)
  {
    //logcdk += LogGamma( (k + 1.0 - (i+1.0) ) / 2.0 );
    logcdk += LogGamma((k - i) / 2.0);
  }

  return -logcdk;
}


// CD must have at least 9 elements
inline void covar(double const * D, size_t N, double * CD)
{
  CD[0] = 0.0f;

  for (size_t r = 0; r < N; r++)
    CD[0]  += D[r * 3] * D[r * 3];

  for (size_t j = 0; j <= 1; j++)
  {
    CD[1 + j * 3] = 0.0f;

    for (size_t r = 0; r < N; r++)
    {
      CD[1 + j * 3] +=  D[1 + r * 3] * D[j + r * 3];
    }
  }

  for (size_t j = 0; j <= 2; j++)
  {
    CD[2 + j * 3] = 0.0f;

    for (size_t r = 0; r < N; r++)
    {
      CD[2 + j * 3] +=  D[2 + r * 3] * D[j + r * 3];
    }
  }

  CD[3] = CD[1];
  CD[6] = CD[2];
  CD[7] = CD[5];
}


inline SNL_VECTOR(size_t) fromOffsetToVector(size_t offset, const SNL_VECTOR(size_t) & parent_domain_sizes)
{
  SNL_VECTOR(size_t) x(parent_domain_sizes.size());

  for (int k = 0; k < x.size(); k++)
  {
    int prod = 1;

    for (int l = k + 1; l < x.size(); l++)
      prod *= parent_domain_sizes[l];

    x[k] = offset / prod;
    offset = offset % prod;
  }

  //  return offset;
  return x;
}


template <class T>
inline SNL_VECTOR(T) intersect(const SNL_VECTOR(T) & v1, const SNL_VECTOR(T) & v2)
{
  SNL_VECTOR(T) c;
  c.reserve(min(v1.size(), v2.size()));

  for (size_t i = 0; i < v1.size(); i++)
  {
    bool exists = false;
    for (size_t j = 0; j < v2.size(); j++)
    {
      if ( v1[i] == v2[j] )
      {
        exists = true;
        break;
      }
    }
    if (exists)
      c.push_back( v1[i] );
  }
  c = unique(c);
  //  return offset;
  return c;
}


// leave only elements that exist in v1 but not in v2
template <class T>
inline SNL_VECTOR(T) setdiff(SNL_VECTOR(T) v1, SNL_VECTOR(T) v2)
{
  SNL_VECTOR(T) c;
  c.reserve(v1.size());

  for (size_t i = 0; i < v1.size(); i++)
  {
    bool exists = false;
    for (size_t j = 0; j < v2.size(); j++)
    {
      if ( v1[i] == v2[j] )
      {
        exists = true;
        break;
      }
    }
    if (!exists)
      c.push_back( v1[i] );
  }
  c = unique(c);
  //  return offset;
  return c;
}


template <class T>
inline bool ismember(T v, SNL_VECTOR(T) v1)
{
  for (size_t i = 0; i < v1.size(); i++)
  {
    if ( v1[i] == v )
      return true;
  }
  return false;
}

//////////////////////////////
// SELF FUNCTIONS //
/////////////////////////////


template <class T>
inline void selfadd(SNL_VECTOR(T) & x, const SNL_VECTOR(T)& y)
{
  if (x.size() != y.size())
  {
    std::cout << "selfadd : number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }

  for (size_t d = 0; d < x.size(); d++)
    x[d] += y[d];
}


template <class T>
inline void selfsubtract(SNL_VECTOR(T) & x, const SNL_VECTOR(T)& y)
{
  if (x.size() != y.size())
  {
    std::cout << "selfsubtract : number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.size() << "elements" << std::endl;
    std::cout << "y has " << y.size() << "elements" << std::endl;
    exit(-1);
  }
  for (size_t d = 0; d < x.size(); d++)
    x[d] -= y[d];
}


template <class T>
inline void selfsubtract(SNL_VECTOR(T) & x, T y)
{
  for (size_t d = 0; d < x.size(); d++)
    x[d] -= y;
}


template <class T>
inline void selfsubtract_butkeepitmax(SNL_VECTOR(T) & x, T y, T m)
{
  for (size_t d = 0; d < x.size(); d++)
  {
    x[d] -= y;
    x[d] = max(x[d], m);
  }
}


template <class T>
inline void selfdiv(SNL_VECTOR(T) & x, T y)
{
  if (y == T(0))
    y = T(SUPEREPSILON);
  for (size_t d = 0; d < x.size(); d++)
    x[d] /= y;
}


template <class T>
inline void selfadd(SNL_MATRIX(T) & x, const SNL_MATRIX(T)& y)
{
	if ( (x.numRows() != y.numRows()) || (x.numColumns() != y.numColumns()) )
  {
    std::cout << "selfadd matrix-matrix: number of elements in x is not equal to number of elements in y" << std::endl;
    std::cout << "x has " << x.numRows() << "rows, " << x.numColumns() << "columns, "  << std::endl;
    std::cout << "y has " << y.numRows() << "rows, " << y.numColumns() << "columns, "  << std::endl;
    exit(-1);
  }
  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      x[d][d2] += y[d][d2];
}


template <class T>
inline void selfdiv(SNL_MATRIX(T) & x, T y)
{
  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      x[d][d2] /=  y;
}


template <class T>
inline void selfinit(SNL_MATRIX(T) & x, T y)
{
  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      x[d][d2] =  y;
}


template <class T>
inline void selfinit(SNL_VECTOR( T ) & x, T y)
{
  for (size_t d = 0; d < x.size(); d++)
    x[d] =  y;
}


template <class T>
inline void selfDiagonalize(SNL_MATRIX(T) & x, size_t start_from_dim = 0)
{
  for (size_t d = 0; d < x.numRows(); d++)
  {
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
    {
      if (d >= start_from_dim || d2 >= start_from_dim)
      {
        if ( d != d2 )
          x[d][d2] = T(0);
      }
    }
  }
}


template <class T>
inline void selfPruneValues(SNL_MATRIX(T) & x, T val)
{
  for (size_t d = 0; d < x.numRows(); d++)
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
      if ( x[d][d2] < val )
	x[d][d2] = val;
}


template <class T>
inline void selfNullify(SNL_MATRIX(T) & x, size_t start_from_dim = 0, T value = T(0) )
{
  for (size_t d = 0; d < x.numRows(); d++)
  {
    for (size_t d2 = 0; d2 < x.numColumns(); d2++)
    {
      if (d >= start_from_dim || d2 >= start_from_dim)
      {
          x[d][d2] = value;
      }
    }
  }
}


template <class T>
inline void selfConcatenateVectors(SNL_VECTOR(T)& v1, const SNL_VECTOR(T)& v2)
{
  v1.insert( v1.end(), v2.begin(), v2.end() );
  return v1;
}


template <class T, class T2>
inline void selfConcatenateVectors(SNL_VECTOR(T)& v1, const SNL_VECTOR(T2)& v2)
{
  for (size_t d = 0; d < v2.size(); d++)
  {
    v1.push_back( (T)v2[d] );
  }
  return v1;
}


template <class T1, class T2>
inline void selfAverageWithExistingVector(SNL_VECTOR(T1) & v, const SNL_VECTOR(T2) & w)
{
  if (v.empty())
  {
    v.resize(w.size());

    for (size_t i = 0; i < v.size(); i++)
      v[i] = (T1)w[i];
  }
  else
  {
    for (size_t i = 0; i < v.size(); i++)
      v[i] = (v[i] + (T1)w[i]) / T1(2);
  }
}


//////////////////////////////
// iostream operators
///////////////////////////////////


template <class T>
static inline std::ostream &operator << (std::ostream &os, const SNL_VECTOR(T) &v)

{
  if (os == std::cout || os == std::cerr)
  {
    os << "(size: " << v.size() << ")" << std::endl;

    for (size_t i = 0; i < v.size(); i++)
      os << v[i] << " ";

    return os << std::endl;
  }

  //previous version
  os << v.size() << " ";
  for (size_t i = 0; i < v.size(); i++)
    os << v[i] << " ";
  return os << std::endl;
}


template <class T>
static inline std::ostream &operator << (std::ostream &os, const SNL_MATRIX(T) &v)

{
  if (os == std::cout || os == std::cerr)
  {
    if (v.isEmpty())
    {
      os << "matrix empty" << std::endl;
      return os;
    }
    os << "(size: " << v.numRows() << "x" << v.numColumns() << ")" << std::endl;

    for (size_t i = 0; i < v.numRows(); i++)
    {
      os << "(#" << v.numColumns() << ") ";
      for (size_t j = 0; j < v.numColumns(); j++)
        os << v[i][j] << " ";
      os << std::endl;
    }
    return os;
  }
  os << v.numRows() << " ";
  for (size_t i = 0; i < v.numRows(); i++)
  {
    os << v.numColumns() << " ";
    for (size_t j = 0; j < v.numColumns(); j++)
      os << v[i][j] << " ";
    os << std::endl;
  }
  return os;
}


template <class T>
static inline std::istream & operator >> (std::istream & is, SNL_VECTOR(T) & v)
{
  size_t s;
  is >> s;
  v.resize(s);

  for (size_t i = 0; i < s; i++)
    is >> v[i];

  return is;
}


template <class T>
static inline std::istream & operator >> (std::istream & is, SNL_MATRIX(T) & v)
{
  size_t nr, nc;
  is >> nr;
  if (nr <= 0)
  {
    v.resize(0, 0);
  }
  else
  {
    for (size_t i = 0; i < nr; i++)
    {
      is >> nc;
      if (i == 0)
      {
        v.resize(nr, nc);
      }
      else
      {
        if (nc != v.numColumns())
          throw std::runtime_error("Matrix row sizes are not consistent");
      }

      for (size_t j = 0; j < nc; j++)
        is >> v[i][j];
    }
  }

  return is;
}

template <class T>
static inline std::istream & operator >> (std::istream & is, SNL_VECTOR( SNL_VECTOR(T) ) & v)
{
  size_t s;
  is >> s;
  v.resize(s);

  for (size_t i = 0; i < v.size(); i++)
  {
    is >> s;
    v[i].resize(s);

    for (size_t j = 0; j < v[i].size(); j++)
      is >> v[i][j];
  }

  return is;
}













//////////////////////////////
// TEST functions
/////////////////////////////



inline bool histTest()
{
  std::cout << "Testing hist functionality...";

  //SNL_VECTOR( size_t ) X;
  //X.push_back(3);
  //X.push_back(4);
  //X.push_back(2);
  //X.push_back(1);
  //for (size_t d = 0; d < 3*4*2*1; d++)
  //  std::cout << fromOffsetToVector(d, X ) << std::endl;
  //system("pause");

  SNL_VECTOR( float ) votes;
  votes.push_back(0.4f);    // 3
  votes.push_back(0.33f);   // 3
  votes.push_back(0.2f);    // 2
  votes.push_back(0.5f);    // 4
  votes.push_back(0.7f);    // 5
  votes.push_back(0.8f);    // 6
  votes.push_back(1.0f);    // 7
  votes.push_back(1.0f);    // 7
  votes.push_back(0.2f);    // 2
  votes.push_back(0.2f);    // 2
  votes.push_back(0.25f);   // 2
  SNL_VECTOR(float) histvalues = hist(votes, 7);

  if (histvalues[ histvalues.size() - 1 ] == 2.0f / 11.0f && sum(histvalues) == 1.0f)
  {
    std::cout << "Pass!" << std::endl;
    return true;
  }
  else
  {
    std::cout << "Fail." << std::endl;
    return false;
  }
}

} // namespace S12
} // namespace SNL

#endif
