/** \file ex_trisolve.cpp  \brief main routines. -*- c++ -*- */
/***************************************************************************
 -   begin                : 2006-11-03
 -   copyright            : (C) 2006 by Gunter Winkler
 -   email                : guwi17@gmx.de
 ***************************************************************************/

// 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)

// misc. boost includes
#include <boost/timer.hpp>

#include "trisolve.h"

// boost ublas includes
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>

// system includes
#include <iostream>
#include <limits>

#ifndef NDEBUG
const size_t default_size = 10;
#else
const size_t default_size = 100;
#endif


template<class M>
void setup_L(M& A, bool unit=false)
{
  using namespace boost::numeric::ublas;
  if (unit)
    A = triangular_adaptor<const scalar_matrix<double>,unit_lower>( scalar_matrix<double>(A.size1(), A.size2(), -0.2) );
  else
    A = triangular_adaptor<const scalar_matrix<double>,strict_lower>( scalar_matrix<double>(A.size1(), A.size2(), -0.2) );
}

template<class M>
void setup_U(M& A, bool unit=false)
{
  using namespace boost::numeric::ublas;
  if (unit)
    A = triangular_adaptor<const scalar_matrix<double>,unit_upper>( scalar_matrix<double>(A.size1(), A.size2(), 0.2) );
  else
    A = triangular_adaptor<const scalar_matrix<double>,strict_upper>( scalar_matrix<double>(A.size1(), A.size2(), 0.2) );
}

template<class V>
void setup_x(boost::numeric::ublas::vector_expression<V>& x)
{
  for (size_t i=0; i<x().size(); ++i) {
    x()(i) = 1.0 / sqrt(x().size());
  }
}

template<class V>
void setup_x(boost::numeric::ublas::matrix_expression<V>& x)
{
  double t = 1.0 / sqrt(x().size1());
  for (size_t i=0; i<x().size1(); ++i) {
    row(x(),i) = boost::numeric::ublas::scalar_vector<double>(x().size2(), t );
  }
}


// this is no real norm -> its only useful for this test
template<class MATRIX>
double
norm_2(const boost::numeric::ublas::matrix_expression<MATRIX>& A)
{
  double result=0.0;
  for (size_t k=0; k<A().size2(); ++k)
    result += norm_2(column( A(), k ));
  return result;
}

template<class MATRIX>
double
norm_inf_2(const boost::numeric::ublas::matrix_expression<MATRIX>& A)
{
  double result=0.0;
  for (size_t k=0; k<A().size2(); ++k)
    result += norm_inf(column( A(), k ));
  return result;
}

template<class VECTOR>
double
norm_inf_2(const boost::numeric::ublas::vector_expression<VECTOR>& A)
{
  return norm_inf(A());
}

template <class TRI, class MATRIX, class VECTOR>
bool do_test(const boost::numeric::ublas::matrix_expression<MATRIX>& A, 
             VECTOR& x, const VECTOR& rhs)
{
  using namespace boost::numeric::ublas;

  double eps = std::numeric_limits<double>::epsilon();

  x=rhs;
  boost::timer t;
  t.restart();
  inplace_solve(A(), x, typename TRI::triangular_type());
  double time = t.elapsed();

  VECTOR resid = (rhs - prod(A(),x));
  if ( TRI::one(1,1) ) resid -= x;
  double error = norm_2(resid)/norm_2(rhs);
  
#ifndef NDEBUG
  std::cerr << "||b-Ax|| = " << norm_2(resid) << ", " << norm_inf(resid) << std::endl;
  std::cerr << "||b|| = " << norm_2(rhs) << ", " << norm_inf(rhs) << std::endl;
  std::cerr << "||x|| = " << norm_2(x) << ", " << norm_inf(x) << std::endl;
#endif

  double limit = (A().size1()*eps);
  if (error <= limit) {
    std::cout << " (" << time << " sec)"
              << " relative error " << error 
              << std::endl;
    return true;
  } else {
    std::cout << " (" << time << " sec)"
              << " relative error " << error 
              << " greater than threshold " << limit
              << std::endl;
#ifndef NDEBUG
    std::cerr << " rhs = " << rhs << "\n";
    std::cerr << " x   = " << x << "\n";
    std::cerr << " res = " << resid << "\n";
#endif
    return false;
  }
}


template < class MATRIX >
void run_solver_tests(size_t size, size_t nrhs=5)
{
    using namespace boost::numeric::ublas;

    typedef MATRIX LMAT;
    typedef MATRIX UMAT;

    LMAT L(size,size);
    UMAT U(size,size);

    L.clear(); U.clear(); 
  
    vector<double> b(size);
    vector<double> x(size);
    vector<double> x2(size);

    matrix<double> X(size,nrhs);
    matrix<double> B(size,nrhs);

    setup_L(L,false);
    setup_U(U,false);


    setup_x(x);
    std::cout << "vec unit lower: ";
    b = x + prod(L, x);
    std::cerr << do_test<unit_lower>(L,x,b);

    setup_x(x);
    std::cout << "vec unit upper: ";
    b = x + prod(U, x);
    std::cerr << do_test<unit_upper>(U,x,b);

    setup_x(X);
    std::cout << "mat unit lower: ";
    B = X + prod(L, X);
    std::cerr << do_test<unit_lower>(L,X,B);

    setup_x(X);
    std::cout << "mat unit upper: ";
    B = X + prod(U, X);
    std::cerr << do_test<unit_upper>(U,X,B);


    L.clear(); U.clear(); 
    setup_L(L,true);
    setup_U(U,true);

    setup_x(x);
    std::cout << "vec lower: ";
    b = prod(L,x); 
    std::cerr << do_test<lower>(L,x,b);

    setup_x(x);
    std::cout << "vec upper: ";
    b = prod(U,x); 
    std::cerr << do_test<upper>(U,x,b);

    setup_x(X);
    std::cout << "mat lower: ";
    B = prod(L, X);
    std::cerr << do_test<lower>(L,X,B);

    setup_x(X);
    std::cout << "mat upper: ";
    B = prod(U, X);
    std::cerr << do_test<upper>(U,X,B);

}


int main(int argc, char *argv[])
{

  typedef size_t  size_type;
  size_type size = default_size;
  if (argc > 1)
	size = ::atoi (argv [1]);
  std::cout << "size " << size << std::endl;
  
  using namespace boost::numeric::ublas;

  std::cout << "dense column major\n";
  run_solver_tests< matrix<double, column_major> >(size);
  std::cout << "dense row major\n";
  run_solver_tests< matrix<double, row_major> >(size);

  std::cout << "compressed column_major major\n";
  run_solver_tests< compressed_matrix<double, column_major> >(size);
  std::cout << "compressed row major\n";
  run_solver_tests< compressed_matrix<double, row_major> >(size);

  std::cout << "mapped column_major major\n";
  run_solver_tests< mapped_matrix<double, column_major> >(size);
  std::cout << "mapped row major\n";
  run_solver_tests< mapped_matrix<double, row_major> >(size);

  return EXIT_SUCCESS;
}