inline void for_each(boost::numeric::ublas::matrix<T> & mat, Fun f){
    for(auto it1 = mat.begin1(); it1 < mat.end1(); ++it1){
        for(auto it = it1.begin(); it < it1.end(); ++it){
            f(*it);
        }
    }
}

void fill_boost_matrix_from_munkres_matrix (boost::numeric::ublas::matrix <T> & boost_matrix, const Matrix <T> & matrix)
{
    const int dimention = std::min (boost_matrix.size1 (), boost_matrix.size2 () );
    for (int i = 0; i < dimention; ++i) {
        for (int j = 0; j < dimention; ++j) {
            boost_matrix (i, j) = matrix (i, j);
        }
    }
};

    double det_chol(const ublas::matrix<double,F,A> &m)
	// Compute determinant of Cholesky matrix. 
    {
        assert(m.size1() == m.size2());
        double d = 1.;
        for (size_t i=0; i < m.size1(); ++i)
            d *= m(i,i);
        return d;
    }

double summOffDiagonal2(boost::numeric::ublas::matrix<double> &S) {
	double sum = 0;
	for (int i = 0; i < S.size1(); i++) {
		for (int j = i + 1; j < S.size2(); j++)
		{
			sum += abs(S(i, j));
		}
	}
	return sum;
}

double diameter(const ublas::matrix<double>& D)
{
  double total = 0;
  for(int i=0;i<D.size1();i++)
    for(int j=0;j<i;j++)
      total += D(i,j);
  
  int N = D.size1() * (D.size1() - 1) /2;

  return total/N;
}

void printMatrix(boost::numeric::ublas::matrix<int> matrix) {
    for (unsigned int i=0; i < matrix.size1(); i++) {
        for (unsigned int j=0; j < matrix.size2(); j++) {
            cout << matrix(i, j);
            if(j+1 != matrix.size2()) {
                cout << "\t";
            }    
        }
        cout << endl;
    }
}

Matrix <T> convert_boost_matrix_to_munkres_matrix (const boost::numeric::ublas::matrix <T> & boost_matrix)
{
    const int dimention = std::min (boost_matrix.size1 (), boost_matrix.size2 () );
    Matrix <T> matrix (dimention, dimention);
    for (int i = 0; i < dimention; ++i) {
        for (int j = 0; j < dimention; ++j) {
            matrix (i, j) = boost_matrix (i, j);
        }
    }

    return matrix;
};

double determinant( bnu::matrix<double>& m ) {
    bnu::permutation_matrix<std ::size_t> pm(m.size1());
    double det = 1.0;
    if( bnu::lu_factorize(m,pm) ) {
        det = 0.0;
    } else {
        for(int i = 0; i < m.size1(); i++)
            det *= m(i,i); // multiply by elements on diagonal
        det = det * determinant_sign( pm );
    }
    return det;
}

	bool is_symmetric(const ublas::matrix<double, F, A> &m)
	{
		if (m.size1() != m.size2())
			return false;

		for (size_t i = 0; i < m.size1(); ++i)
			for (size_t j = i+1; j < m.size2(); ++j)
				if (m(i,j) != m(j,i))
					return false;

		return true;
	}

matrix<double> BoostMatrixFacade::invert(const boost::numeric::ublas::matrix<double> matrix)
{
    boost::numeric::ublas::matrix<double> inverted(matrix.size1(), matrix.size2());
    cpu_invert(matrix, inverted);

    for( int i = 0; i < inverted.size1(); i++ )
        for( int j = 0; j < inverted.size2(); j++ )
            if( isnan(inverted(i, j)) )
                inverted(i, j) = 0;

    return inverted;
}

cv::Mat Bridge::createImgMap(const boost::numeric::ublas::matrix<STATE> &groundMap) {
    cv::Mat ground(groundMap.size1(), groundMap.size2(), CV_8UC3, cv::Scalar(0, 0, 0));
    for (unsigned int i = 0; i < groundMap.size1(); ++i) {
        for (unsigned int j = 0; j < groundMap.size2(); ++j) {
            if (groundMap(i, j) == 0) {
                ground.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 0, 0);
            } else {
                ground.at<cv::Vec3b>(i, j) = cv::Vec3b(255, 255, 255);
            }
        }
    }
    return ground;
}

const boost::numeric::ublas::matrix<double>
  FixedLagSmootherKalmanFilter::CreateTermA(
  const int lag,
  const int state_size)
{
  //
  assert(lag > 0 && "Term A is not needed for a lag of zero");
  const boost::numeric::ublas::matrix<double> v
    = Matrix::SimplifyVectorOfMatrix(CreateComplexTermA(lag,state_size));
  assert(lag * state_size == boost::numeric_cast<int>(v.size1()));
  assert(  1 * state_size == boost::numeric_cast<int>(v.size2()));
  return v;
}

      void pprint(const boost::numeric::ublas::matrix<double>& m) {
	cout << "[";
	for( uint r=0; r < m.size1(); r++) {
	  for( uint c=0; c < m.size2(); c++) {
	    cout << m(r,c);
	    if(c == m.size2()-1 && r != m.size1()-1) 
	      cout << "\n ";
	    else if(c != m.size2()-1)
	      cout << " , ";
	  }
	}
	cout << "]\n";
      }

inline void serialize(json_output_handler& os, const boost::numeric::ublas::matrix<double>& A)
{
    os.begin_array();
    for (size_t i = 0; i < A.size1(); ++i)
    {
        os.begin_array();
        for (size_t j = 0; j < A.size2(); ++j)
        {
            os.value(A(i, j));
        }
        os.end_array();
    }
    os.end_array();
}

void resize(ublas::matrix<int>& M1,int s1,int s2,int clear=0)
{
  ublas::matrix<int> M2(s1,s2);

  for(int i=0;i<M2.size1();i++)
    for(int j=0;j<M2.size2();j++)
      M2(i,j) = clear;

  for(int i=0;i<M1.size1() and i<M2.size1();i++)
    for(int j=0;j< M1.size2() and j<M2.size2();j++)
      M2(i,j) = M1(i,j);

  M1.swap(M2);
}

	FixedMatrix( const boost::numeric::ublas::matrix<TYPE> &boost_matrix ) throw ( std::logic_error & ) {
		if( boost_matrix.size1() == ROWS && boost_matrix.size2() == COLS ) {
			for( size_t m = 0; m < ROWS; m++ ) {
				for( size_t n = 0; n < COLS; n++ ) {
					this->elem( n, m ) = boost_matrix( m, n );
				}
			}
		} else {
			LOG( Runtime, error ) << "The size of the boost matrix ("
								  << boost_matrix.size1() << ", " << boost_matrix.size2()
								  << ") does not coincide with the size of the isis matrix (" << ROWS << ", " << COLS << ").";
			throw( std::logic_error( "Size mismatch" ) );
		}
	};

int jacobiSync(
	boost::numeric::ublas::matrix<double> &S,
	boost::numeric::ublas::vector<double> &e,
	boost::numeric::ublas::matrix<double>  &U,
	int &iter,
	bool isOptimized)
{
	iter = 0;
	int col, row;
	bool iterating = true;
	int n = S.size1();
	if (S.size2() != n)
	{
		return -1;
	}
	boost::numeric::ublas::matrix<double> M(n, n);
	e = boost::numeric::ublas::zero_vector<double>(n);
	U = boost::numeric::ublas::identity_matrix<double>(n, n);
	while (iterating)
	{
		M = S;
		abs(M);
		iter++;
		for (int k = 0; k < n; k++)
		{
			M(k, k) = 0;
		}
		findMax(M, row, col);
		if (row == col)
		{
			for (int i = 0; i < n; i++) e(i) = S(i, i);
			return 0;
		}
		double Smax = S(row, col);
		if (isOptimized) {
			rotateColRowJacobi(S, row, col);
		}
		else {
			rotateRowCol(S, U, row, col);
		}
		//cout<<row<<" row|col "<<col <<" sum: "<< sumOffDiagonal(S) << endl;
		//if (Smax < _EPS * norm_frobenius(S)) iterating = false;
		if (sumOffDiagonal(S) < _EPS) iterating = false;
	}

	for (int i = 0; i < n; i++) e(i) = S(i, i);

	return 0;
}

 bool invert(const boost::numeric::ublas::matrix<T>& input, boost::numeric::ublas::matrix<T>& inverse) {

   // create a working copy of the input
   boost::numeric::ublas::matrix<T> A(input);

   // create a permutation matrix for the LU-factorization
   boost::numeric::ublas::permutation_matrix<std::size_t> pm(A.size1());

   // perform LU-factorization
   typename boost::numeric::ublas::matrix<T>::size_type res = boost::numeric::ublas::lu_factorize(A, pm);
   if( res != 0 ){
     LOG_FREE(Info, "boost.ublas", "boost::numeric::ublas::lu_factorize returned res = " << res <<
                    ", A = " << A << ", pm = " << pm << " for input = " << input);
     return false;
   }

   // create identity matrix of "inverse"
   inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));

   // backsubstitute to get the inverse
   try {
     boost::numeric::ublas::lu_substitute(A, pm, inverse);
   }catch (std::exception& e){
     LOG_FREE(Info, "boost.ublas", "boost::numeric::ublas::lu_substitute threw exception '" << e.what() <<
                    "' for A = " << A << ", pm = " << pm);
     return false;
   }

   return true;
 }

int SAFForwardingTable::determineRowOfFace(int face_id, boost::numeric::ublas::matrix<double> tab, std::vector<int> faces)
{
  // check if table fits to faces
  if(tab.size1 () != faces.size ())
  {
    fprintf(stderr, "Error the number of faces dont correspond to the table!\n");
    return FACE_NOT_FOUND;
  }

  if(std::find(faces.begin (), faces.end (), face_id) == faces.end ())
  {
    fprintf(stderr, "Face Not Found!!!\n");
    return FACE_NOT_FOUND;
  }

  //determine row of face
  int faceRow = FACE_NOT_FOUND;
  std::sort(faces.begin(), faces.end());//order

  int rowCounter = 0;
  for(std::vector<int>::iterator i = faces.begin (); i != faces.end() ; ++i)
  {
    //fprintf(stderr, "*i=%d ; face_id=%d\n",*i,face_id);
    if(*i == face_id)
    {
      faceRow = rowCounter;
      break;
    }
    rowCounter++;
  }
  return faceRow;
}

ScalarType matrix_compare(ublas::matrix<ScalarType>& res,
                            ublas::matrix<ScalarType>& ref)
{
    ScalarType diff = 0.0;
    ScalarType mx = 0.0;

    for(std::size_t i = 0; i < res.size1(); i++)
    {
        for(std::size_t j = 0; j < res.size2(); j++)
        {
            diff = std::max(diff, std::abs(res(i, j) - ref(i, j)));
            mx = std::max(mx, res(i, j));
        }
    }

    return diff / mx;
}