void normalizeRM(matrix_t & Q, StateMap const & staMap, float subs)
  {
    vector_t equiFreq = deriveEquiFreqForReversibleRM(Q);
    HammingDistance hammingDistance(staMap);
    number_t normConst = 0;
    for (unsigned i = 0; i < Q.size1(); ++ i)
      for (unsigned j = 0; j < Q.size2(); ++ j)
	normConst += equiFreq(i) * Q(i,j) * hammingDistance(i,j);

    Q = Q * (subs / normConst);
  }

void output_matrix(ostream& out, matrix_t& M, unsigned n) {
	out << "matrix_t M[" << n << "] = {\n";
	for (unsigned i=0; i<M.size1(); ++i) {
		out << "\t{ ";
		out << M(i,0);
		for (unsigned j=1; j<M.size2() ; ++j)
			out << ',' << M(i,j);
		out << " },\n";
	}
	out << "}\n";
}

  unsigned nonZeroDiagonalEntryOrDie(matrix_t const & Q)
  {
    assert( Q.size1() == Q.size1() ); // Q is quadratic

    for (unsigned i = 0; i < Q.size1(); i++)
      if (Q(i, i) != 0.0)
	return i;

    errorAbort("All zero diagonal entries. Bails out.");
    return 0; // will never reach this, only to quiet compiler
  }

 color_matrix_transform(matrix_t const & matrix)
   : matrix_(matrix)
 {
   if (matrix.shape()[0] == 3 && matrix.shape()[1] == 3)
   {
     // TODO make specialization for 3x3 matrix
     matrix_.resize(boost::extents[4][5]);
     matrix_[3][3] = 1;
   }
   else
     BOOST_ASSERT(matrix.shape()[0] == 4 && matrix.shape()[1] == 5);
 }

// LU factorization of a general matrix A.  
//    Computes an LU factorization of a general M-by-N matrix A using
//    partial pivoting with row interchanges. Factorization has the form
//    A = P*L*U.
//    a       (IN/OUT - matrix(M,N)) On entry, the coefficient matrix A to be factored. On exit, the factors L and U from the factorization A = P*L*U.
//    ipivot  (OUT - vector(min(M,N))) Integer vector. The row i of A was interchanged with row IPIV(i).
//    info    (OUT - int)
//   0   :  successful exit
//   < 0 :  If INFO = -i, then the i-th argument had an illegal value.
//   > 0 :  If INFO = i, then U(i,i) is exactly zero. The  factorization has been completed, but the factor U is exactly singular, and division by zero will occur if it is used to solve a system of equations.
int getrf (matrix_t& a, pivot_t& ipivot)
{
	matrix_t::value_type* _a = a.data().begin();
	int _m = int(a.size1());
	int _n = int(a.size2());
	int _lda = _m;	// minor size
	int _info;

	rawLAPACK::getrf (_m, _n,	_a, _lda, ipivot.data().begin(), _info);

	return _info;
}

vector_t
svd_hint_from_most_similar(
    index_t  const user, 
    matrix_t const & sim, 
    matrix_t const & P, 
    matrix_t const & Q)
{
    stack::fe_asserter dummy{};
    vector_ll_t neighbors{std::min<index_t>(10,sim.get_rows())};
    vector_t    weights{std::min<index_t>(10,sim.get_rows())};
    most_similar(user, neighbors, weights, sim);
    return svd_hint(neighbors, weights, P, Q);
}

vector_t /* user_factor */
est_factor(
    vector_ll_t const & neighbors,
    vector_t    const & weights, 
    matrix_t    const & P)
{
    stack::fe_asserter dummy1{};
    scoped_timer dummy(std::string(__func__));

    vector_t user_factor{P.get_cols()};

    for(int i = 0; i < neighbors.get_len(); i++)
        user_factor += weights[i] * P.get_row_clone(neighbors[i]);
    return user_factor.normalize_2();
}

void matrix_t::filtra(matrix_t& M, matrix_item_t it, double precision){
    matrix_inx_t m = get_m();
    matrix_inx_t n = get_n();
    M.redimensiona(m, n);
    for (int i = 1; i < m; i++) {
        for (int j = 1; j < n ; j++) {
            if (M.igual(get_matrix_item(i, j),it,precision) == true) {
                M.set_matrix_item(i, j, get_matrix_item(i, j));
            } else {
                matrix_item_t x = 0.00000;
                M.set_matrix_item(i, j, x);
            }
        }
    }
}

static void applyGaussian(matrix_t& m) {
	matrix_t::array_type& arr = m.data();
	double sig = *std::max_element(arr.begin(), arr.end());

	BOOST_FOREACH(double& v, arr) {
		v = std::exp(-v * v / (2 * sig * sig));
	}

// mult X * D
matrix_t diag_t::left_mult(matrix_t const &X) const
{
    stack::fe_asserter fe{};
    stack_assert(diagonal.get_len() == X.get_cols());

    matrix_t ret = X.clone();

    if(ret.get_rows() < ret.get_cols())
        for(size_t r = 0; r < ret.get_rows(); r++)
            ret[r] *= diagonal;
    else
        for(size_t c = 0; c < ret.get_cols(); c++)
            ret.get_col(c) *= diagonal[c];

    return ret;
}

void multi2(const matrix_t &A, const matrix_t &B, matrix_t &C)
{
    size_t n = A.size(); 
    float **__restrict__ const da = A.data;
    float **__restrict__ const db = B.data;
    float **__restrict__ dc = C.data;

    const size_t chunk_size = 8;
    const size_t chunks = n / chunk_size;

#pragma omp parallel for num_threads(8)
    for(size_t i = 0; i < n; ++i)
    {
        __m256 a_line, b_line, c_line, r_line;
        for(size_t k = 0; k < n; ++k)
        {
            float c = da[i][k];
            a_line = _mm256_set_ps(c, c, c, c, c, c, c, c);
            for(size_t j = 0; j < chunks; ++j)
            {
                float mc[32] __attribute__((aligned(32))); 
                b_line = _mm256_load_ps(&db[k][j * chunk_size]);
                c_line = _mm256_load_ps(&dc[i][j * chunk_size]);
                r_line = _mm256_mul_ps(a_line, b_line);  
                r_line = _mm256_add_ps(r_line, c_line);
                _mm256_store_ps(&dc[i][j * chunk_size], r_line);
            } 

            for(size_t j = chunk_size * chunks; j < n; ++j)
            {
                dc[i][j] += c * db[k][j];
            }
        }
    }
}

bool search(matrix_t &matrix, int x, int y, string needle, int start_index, bool scan) {
  int x_max = matrix.size() - 1;
  int y_max = matrix[0].size() - 1;

  if (x < 0 || x > x_max) return false;
  if (y < 0 || y > y_max) return false;

  if (needle.size() == start_index) {
    return true;
  }

  if (matrix[x][y] == needle[start_index]) {
    int original_char = matrix[x][y];
    matrix[x][y] = '.';
    for (int i = 0; i < 8; ++i) {
      bool found = search(matrix, x + search_x[i], y + search_y[i], needle, start_index + 1, false);
      if (found) return true;
    }
    matrix[x][y] = original_char;
  }

  if (scan) {
    if (y < y_max) {
      return search(matrix, x, y + 1, needle, start_index, true);
    } else if (x < x_max) {
      return search(matrix, x + 1, 0, needle, start_index, true);
    }
  }

  return false;
}

// QR Factorization of a MxN General Matrix A.
//    a       (IN/OUT - matrix(M,N)) On entry, the coefficient matrix A. On exit , the upper triangle and diagonal is the min(M,N) by N upper triangular matrix R.  The lower triangle, together with the tau vector, is the orthogonal matrix Q as a product of min(M,N) elementary reflectors.
//    tau     (OUT - vector (min(M,N))) Vector of the same numerical type as A. The scalar factors of the elementary reflectors.
//    info    (OUT - int)
//   0   : function completed normally
//   < 0 : The ith argument, where i = abs(return value) had an illegal value.
int geqrf (matrix_t& a, vector_t& tau)
{
	int              _m = int(a.size1());
	int              _n = int(a.size2());
	int              _lda = int(a.size1());
	int              _info;

	// make_sure tau's size is greater than or equal to min(m,n)
	if (int(tau.size()) < (_n<_m ? _n : _m) )
		return -104;

	int ldwork = _n*_n;
	vector_t dwork(ldwork);
	rawLAPACK::geqrf (_m, _n, a.data().begin(), _lda, tau.data().begin(), dwork.data().begin(), ldwork, _info);

	return _info;
}

matrix_t matrixMul(const matrix_t & matrixA, const matrix_t & matrixB) {
    auto dimension = matrixA.size();

	assert(matrixA.size() == dimension);
	assert(matrixA[0].size() == dimension);
	assert(matrixB.size() == dimension);
	assert(matrixB[0].size() == dimension);

	matrix_t matrixC(dimension, typename matrix_t::value_type(dimension, 0));//0ed matrix

	for(int x{0}; x < dimension; ++x)
	for(int i{0}; i < dimension; ++i)
	for(int y{0}; y < dimension; ++y)
		matrixC[x][y] += matrixA[x][i] * matrixB[i][y];

	return matrixC;//move semantics ftw
}

  void NormalMeanPostFactor::mkFactor(matrix_t &m) const{
    //Remember to convert to index space
    boost::math::normal dist( (postmean_-minv_)*bins_/(maxv_-minv_), 
			      std::sqrt(postvar_)*bins_/(maxv_-minv_) );
    for(int i = 0; i < m.size2(); ++i){
      m(0,i) = cdf(dist, i+1)-cdf(dist,i);
    }
  }

matrix_t matrixMulTiled(const matrix_t & matrixA, const matrix_t & matrixB) {
	auto dimension = matrixA.size();

	assert(matrixA.size() == dimension);
	assert(matrixA[0].size() == dimension);
	assert(matrixB.size() == dimension);
	assert(matrixB[0].size() == dimension);

	matrix_t matrixC(dimension, typename matrix_t::value_type(dimension, 0));//0ed matrix

	const int m{8};//256bit
	const size_t n{dimension - dimension % m};
	int start{0};

	if(n >= m) {
		for (int i{0}; i < n; i+=m)
		for (int j{0}; j < n; j+=m)
		for (int k{0}; k < n; k+=m)
		for (int s{0}; s < m; s++)
		for (int t{0}; t < m; t++)
		for (int u{0}; u < m; u++)
			matrixC[i + s][j + t] += matrixA[i + s][k + u] * matrixB[k + u][j + t];
		start = n;
	}

	//finalize calculations within tiles
	for(int x{0}; x < n; ++x)
	for(int i{start}; i < dimension; ++i)
	for(int y{0}; y < n; ++y)
		matrixC[x][y] += matrixA[x][i] * matrixB[i][y];

	//calculate remaining rows
	for(int x{start}; x < dimension; ++x)
	for(int i{0}; i < dimension; ++i)
	for(int y{0}; y < dimension; ++y)
		matrixC[x][y] += matrixA[x][i] * matrixB[i][y];

	//calculate remaining elements (remaining columns without already calculated rows)
	for(int x{0}; x < n; ++x)
	for(int i{0}; i < dimension; ++i)
	for(int y{start}; y < dimension; ++y)
		matrixC[x][y] += matrixA[x][i] * matrixB[i][y];

	return matrixC;//move semantics ftw
}

void Print(const matrix_t& m)
{
    for (unsigned i = 0; i < m.size(); i++) {
        for (unsigned j = 0; j < m[i].size(); j++) {
            cout << m[i][j] << " ";
        }
        cout << endl;
    }
}

 AbstractFullyParameterizedFactor::AbstractFullyParameterizedFactor(string const & type, string const & id, matrix_t const & m, matrix_t const & pseudoCounts, matrix_t const & funBMat) 
   : AbstractBaseFactor(type, id, m.size1(), m.size2()), m_(m), pseudoCounts_(pseudoCounts), funBMat_(funBMat)
 {
   if (pseudoCounts.size1() != 0) {
     assert(pseudoCounts.size1() == size1_); 
     assert(pseudoCounts.size2() == size2_); 
   }
   if (funBMat.size1() != 0) {
     assert(funBMat.size1() == size1_); 
     assert(funBMat.size2() == size2_); 
   }
 }

  void deriveEquiFreqs(matrix_t const & Q, vector_t & equiFreq)
  {
    unsigned size = Q.size1();

    for (unsigned int j = 0; j < size; j++)
      if (equiFreq(j) == 0 && Q(j, j) != 0.0 )
	for (unsigned int i = 0; i < size; i++) 
	  if (equiFreq(i) != 0.0 && Q(j, i) != 0.0 )
	    equiFreq(j) = equiFreq(i) * Q(i, j) / Q(j, i);
  }

matrix_t gaussSeidelIteration(const matrix_t & grid) {
	auto dimension = grid.size();

	assert(grid[0].size() == dimension);

	auto gridCopy = grid;

	for(int x{1}; x < dimension - 1; ++x)
	for(int y{1}; y < dimension - 1; ++y)
		gridCopy[x][y] += 0.25 * (grid[x-1][y] + grid[x][y-1] + grid[x][y+1] + grid[x+1][y]);

	return gridCopy;
}