#undef  NDEBUG
#define BOOST_UBLAS_TYPE_CHECK 1
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>

namespace boost { namespace numeric {namespace ublas {

    template<class PM, class MV>
    BOOST_UBLAS_INLINE
    void unswap_rows (const PM &pm, MV &mv, vector_tag) {
        typedef typename PM::size_type size_type;
        typedef typename MV::value_type value_type;

        size_type size = pm.size ();
        for (size_type i = size; i > 0;) {
	    --i;
            if (i != pm (i))
                std::swap (mv (i), mv (pm (i)));
        }
    }
    template<class PM, class MV>
    BOOST_UBLAS_INLINE
    void unswap_rows (const PM &pm, MV &mv, matrix_tag) {
        typedef typename PM::size_type size_type;
        typedef typename MV::value_type value_type;

        size_type size = pm.size ();
        for (size_type i = size; i > 0;) {
	    --i;
            if (i != pm (i))
                row (mv, i).swap (row (mv, pm (i)));
        }
    }
    // Dispatcher
    template<class PM, class MV>
    BOOST_UBLAS_INLINE
    void unswap_rows (const PM &pm, MV &mv) {
        unswap_rows (pm, mv, typename MV::type_category ());
    }

    template<class MV, class M, class PMT, class PMA>
    void alt_lu_substitute (MV &mv, const M &m, const permutation_matrix<PMT, PMA> &pm) {
        lu_substitute (mv, m);
        unswap_rows (pm, mv);
    }
}}}

int main (int ac, char **av)
{
  size_t dim = 3;

  namespace ublas = boost::numeric::ublas;
  ublas::matrix<double> A(dim,dim);
  A(0,0)=2; A(0,1)=7; A(0,2)=8;
  A(1,0)=5; A(1,1)=1; A(1,2)=3;
  A(2,0)=4; A(2,1)=2; A(2,2)=1;

  ublas::matrix<double> At(ublas::trans(A));

  ublas::vector<double> c(dim);
  c(0)=1; c(1)=8; c(2)=6;

  std::cout << "Input A: " << A << "\n";
  std::cout << "Input c: " << c << "\n";

  ublas::permutation_matrix<> pm(dim);
  int result = ublas::lu_factorize(A, pm);
  if (result) {
    std::cerr << "lu_factorize of A failed\n";
    exit(1);
  }

  ublas::permutation_matrix<> pmt(dim);
  result = ublas::lu_factorize(At, pmt);
  if (result) {
    std::cerr << "lu_factorize of At failed\n";
    exit(1);
  }

  std::cout << "The following two solution vectors should be equal\n";
  
  ublas::vector<double> y(c);
  ublas::lu_substitute(y, A, pm);
  std::cout << "solving y' A = c' gave y: " << y << std::endl;

  ublas::vector<double> x(c);
  ublas::lu_substitute(At, pmt, x);
  std::cout << "solving A' x = c  gave x: " << x << std::endl;
  
  ublas::vector<double> y2(c);
  ublas::alt_lu_substitute(y2, A, pm);
  std::cout << "using alt_lu_substitute gave y: " << y2 << std::endl;

  return 0;
}