/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \file tests/src/sparse.cpp  Tests sparse matrix operations.
*   \test  Tests sparse matrix operations.
**/

#ifndef NDEBUG
 #define NDEBUG
#endif

//
// *** System
//
#include <iostream>

//
// *** Boost
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

//
// *** ViennaCL
//
//#define VIENNACL_DEBUG_ALL
#define VIENNACL_WITH_UBLAS 1
#include "viennacl/scalar.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/compressed_compressed_matrix.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/ell_matrix.hpp"
#include "viennacl/sliced_ell_matrix.hpp"
#include "viennacl/hyb_matrix.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/vector_proxy.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/detail/ilu/common.hpp"
#include "viennacl/io/matrix_market.hpp"
#include "examples/tutorial/Random.hpp"
#include "examples/tutorial/vector-io.hpp"

//
// -------------------------------------------------------------
//
using namespace boost::numeric;
//
// -------------------------------------------------------------
//
template<typename ScalarType>
ScalarType diff(ScalarType & s1, viennacl::scalar<ScalarType> & s2)
{
   if (s1 != s2)
      return (s1 - s2) / std::max(fabs(s1), std::fabs(s2));
   return 0;
}

template<typename ScalarType>
ScalarType diff(ublas::vector<ScalarType> & v1, viennacl::vector<ScalarType> & v2)
{
   ublas::vector<ScalarType> v2_cpu(v2.size());
   viennacl::backend::finish();
   viennacl::copy(v2.begin(), v2.end(), v2_cpu.begin());

   for (unsigned int i=0;i<v1.size(); ++i)
   {
      if ( std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) ) > 0 )
      {
        //if (std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) ) < 1e-10 )  //absolute tolerance (avoid round-off issues)
        //  v2_cpu[i] = 0;
        //else
          v2_cpu[i] = std::fabs(v2_cpu[i] - v1[i]) / std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) );
      }
      else
         v2_cpu[i] = 0.0;

      if (v2_cpu[i] > 0.0001)
      {
        //std::cout << "Neighbor: "      << i-1 << ": " << v1[i-1] << " vs. " << v2_cpu[i-1] << std::endl;
        std::cout << "Error at entry " << i   << ": Should: " << v1[i]   << " vs. Is: " << v2[i]   << std::endl;
        //std::cout << "Neighbor: "      << i+1 << ": " << v1[i+1] << " vs. " << v2_cpu[i+1] << std::endl;
        exit(EXIT_FAILURE);
      }
   }

   return norm_inf(v2_cpu);
}

template<typename ScalarType, typename VCL_MATRIX>
ScalarType diff(ublas::compressed_matrix<ScalarType> & cpu_matrix, VCL_MATRIX & gpu_matrix)
{
  typedef ublas::compressed_matrix<ScalarType>  CPU_MATRIX;
  CPU_MATRIX from_gpu(gpu_matrix.size1(), gpu_matrix.size2());

  viennacl::backend::finish();
  viennacl::copy(gpu_matrix, from_gpu);

  ScalarType error = 0;

  //step 1: compare all entries from cpu_matrix with gpu_matrix:
  //std::cout << "Ublas matrix: " << std::endl;
  for (typename CPU_MATRIX::const_iterator1 row_it = cpu_matrix.begin1();
        row_it != cpu_matrix.end1();
        ++row_it)
  {
    //std::cout << "Row " << row_it.index1() << ": " << std::endl;
    for (typename CPU_MATRIX::const_iterator2 col_it = row_it.begin();
          col_it != row_it.end();
          ++col_it)
    {
      //std::cout << "(" << col_it.index2() << ", " << *col_it << std::endl;
      ScalarType current_error = 0;

      if ( std::max( std::fabs(cpu_matrix(col_it.index1(), col_it.index2())),
                      std::fabs(from_gpu(col_it.index1(), col_it.index2()))   ) > 0 )
        current_error = std::fabs(cpu_matrix(col_it.index1(), col_it.index2()) - from_gpu(col_it.index1(), col_it.index2()))
                          / std::max( std::fabs(cpu_matrix(col_it.index1(), col_it.index2())),
                                      std::fabs(from_gpu(col_it.index1(), col_it.index2()))   );
      if (current_error > error)
        error = current_error;
    }
  }

  //step 2: compare all entries from gpu_matrix with cpu_matrix (sparsity pattern might differ):
  //std::cout << "ViennaCL matrix: " << std::endl;
  for (typename CPU_MATRIX::const_iterator1 row_it = from_gpu.begin1();
        row_it != from_gpu.end1();
        ++row_it)
  {
    //std::cout << "Row " << row_it.index1() << ": " << std::endl;
    for (typename CPU_MATRIX::const_iterator2 col_it = row_it.begin();
          col_it != row_it.end();
          ++col_it)
    {
      //std::cout << "(" << col_it.index2() << ", " << *col_it << std::endl;
      ScalarType current_error = 0;

      if ( std::max( std::fabs(cpu_matrix(col_it.index1(), col_it.index2())),
                      std::fabs(from_gpu(col_it.index1(), col_it.index2()))   ) > 0 )
        current_error = std::fabs(cpu_matrix(col_it.index1(), col_it.index2()) - from_gpu(col_it.index1(), col_it.index2()))
                          / std::max( std::fabs(cpu_matrix(col_it.index1(), col_it.index2())),
                                      std::fabs(from_gpu(col_it.index1(), col_it.index2()))   );
      if (current_error > error)
        error = current_error;
    }
  }

  return error;
}

template<typename NumericT, typename VCL_MatrixT, typename Epsilon, typename UblasVectorT, typename VCLVectorT>
int strided_matrix_vector_product_test(Epsilon epsilon,
                                        UblasVectorT & result, UblasVectorT const & rhs,
                                        VCLVectorT & vcl_result, VCLVectorT & vcl_rhs)
{
    int retval = EXIT_SUCCESS;

    ublas::compressed_matrix<NumericT> ublas_matrix2(5, 4);
    ublas_matrix2(0, 0) = NumericT(2.0); ublas_matrix2(0, 2) = NumericT(-1.0);
    ublas_matrix2(1, 0) = NumericT(3.0); ublas_matrix2(1, 2) = NumericT(-5.0);
    ublas_matrix2(2, 1) = NumericT(5.0); ublas_matrix2(2, 2) = NumericT(-2.0);
    ublas_matrix2(3, 2) = NumericT(1.0); ublas_matrix2(3, 3) = NumericT(-6.0);
    ublas_matrix2(4, 1) = NumericT(7.0); ublas_matrix2(4, 2) = NumericT(-5.0);
    project(result, ublas::slice(1, 3, 5))     = ublas::prod(ublas_matrix2, project(rhs, ublas::slice(3, 2, 4)));

    VCL_MatrixT vcl_sparse_matrix2;
    viennacl::copy(ublas_matrix2, vcl_sparse_matrix2);
    viennacl::vector<NumericT> vec(4);
    vec(0) = rhs(3);
    vec(1) = rhs(5);
    vec(2) = rhs(7);
    vec(3) = rhs(9);
    viennacl::project(vcl_result, viennacl::slice(1, 3, 5)) = viennacl::linalg::prod(vcl_sparse_matrix2, viennacl::project(vcl_rhs, viennacl::slice(3, 2, 4)));

    if ( std::fabs(diff(result, vcl_result)) > epsilon )
    {
      std::cout << "# Error at operation: matrix-vector product with stided vectors, part 1" << std::endl;
      std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
      retval = EXIT_FAILURE;
    }
    vcl_result(1)  = NumericT(1.0);
    vcl_result(4)  = NumericT(1.0);
    vcl_result(7)  = NumericT(1.0);
    vcl_result(10) = NumericT(1.0);
    vcl_result(13) = NumericT(1.0);

    viennacl::project(vcl_result, viennacl::slice(1, 3, 5)) = viennacl::linalg::prod(vcl_sparse_matrix2, vec);

    if ( std::fabs(diff(result, vcl_result)) > epsilon )
    {
      std::cout << "# Error at operation: matrix-vector product with strided vectors, part 2" << std::endl;
      std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
      retval = EXIT_FAILURE;
    }

    return retval;
}

template< typename NumericT, typename VCL_MATRIX, typename Epsilon >
int resize_test(Epsilon const& epsilon)
{
   int retval = EXIT_SUCCESS;

   ublas::compressed_matrix<NumericT> ublas_matrix(5,5);
   VCL_MATRIX vcl_matrix;

   ublas_matrix(0,0) = NumericT(10.0); ublas_matrix(0, 1) = NumericT(0.1); ublas_matrix(0, 2) = NumericT(0.2); ublas_matrix(0, 3) = NumericT(0.3); ublas_matrix(0, 4) = NumericT(0.4);
   ublas_matrix(1,0) = NumericT(1.0);  ublas_matrix(1, 1) = NumericT(1.1); ublas_matrix(1, 2) = NumericT(1.2); ublas_matrix(1, 3) = NumericT(1.3); ublas_matrix(1, 4) = NumericT(1.4);
   ublas_matrix(2,0) = NumericT(2.0);  ublas_matrix(2, 1) = NumericT(2.1); ublas_matrix(2, 2) = NumericT(2.2); ublas_matrix(2, 3) = NumericT(2.3); ublas_matrix(2, 4) = NumericT(2.4);
   ublas_matrix(3,0) = NumericT(3.0);  ublas_matrix(3, 1) = NumericT(3.1); ublas_matrix(3, 2) = NumericT(3.2); ublas_matrix(3, 3) = NumericT(3.3); ublas_matrix(3, 4) = NumericT(3.4);
   ublas_matrix(4,0) = NumericT(4.0);  ublas_matrix(4, 1) = NumericT(4.1); ublas_matrix(4, 2) = NumericT(4.2); ublas_matrix(4, 3) = NumericT(4.3); ublas_matrix(4, 4) = NumericT(4.4);

   viennacl::copy(ublas_matrix, vcl_matrix);
   ublas::compressed_matrix<NumericT> other_matrix(ublas_matrix.size1(), ublas_matrix.size2());
   viennacl::copy(vcl_matrix, other_matrix);

   std::cout << "Checking for equality after copy..." << std::endl;
    if ( std::fabs(diff(ublas_matrix, vcl_matrix)) > epsilon )
    {
        std::cout << "# Error at operation: equality after copy with sparse matrix" << std::endl;
        std::cout << "  diff: " << std::fabs(diff(ublas_matrix, vcl_matrix)) << std::endl;
        return EXIT_FAILURE;
    }

   std::cout << "Testing resize to larger..." << std::endl;
   ublas_matrix.resize(10, 10, false); //ublas does not allow preserve = true here
   ublas_matrix(0,0) = NumericT(10.0); ublas_matrix(0, 1) = NumericT(0.1); ublas_matrix(0, 2) = NumericT(0.2); ublas_matrix(0, 3) = NumericT(0.3); ublas_matrix(0, 4) = NumericT(0.4);
   ublas_matrix(1,0) = NumericT( 1.0); ublas_matrix(1, 1) = NumericT(1.1); ublas_matrix(1, 2) = NumericT(1.2); ublas_matrix(1, 3) = NumericT(1.3); ublas_matrix(1, 4) = NumericT(1.4);
   ublas_matrix(2,0) = NumericT( 2.0); ublas_matrix(2, 1) = NumericT(2.1); ublas_matrix(2, 2) = NumericT(2.2); ublas_matrix(2, 3) = NumericT(2.3); ublas_matrix(2, 4) = NumericT(2.4);
   ublas_matrix(3,0) = NumericT( 3.0); ublas_matrix(3, 1) = NumericT(3.1); ublas_matrix(3, 2) = NumericT(3.2); ublas_matrix(3, 3) = NumericT(3.3); ublas_matrix(3, 4) = NumericT(3.4);
   ublas_matrix(4,0) = NumericT( 4.0); ublas_matrix(4, 1) = NumericT(4.1); ublas_matrix(4, 2) = NumericT(4.2); ublas_matrix(4, 3) = NumericT(4.3); ublas_matrix(4, 4) = NumericT(4.4);
   //std::cout << ublas_matrix << std::endl;

   vcl_matrix.resize(10, 10, true);

    if ( std::fabs(diff(ublas_matrix, vcl_matrix)) > epsilon )
    {
        std::cout << "# Error at operation: resize (to larger) with sparse matrix" << std::endl;
        std::cout << "  diff: " << std::fabs(diff(ublas_matrix, vcl_matrix)) << std::endl;
        return EXIT_FAILURE;
    }

   ublas_matrix(5,5) = NumericT(5.5); ublas_matrix(5, 6) = NumericT(5.6); ublas_matrix(5, 7) = NumericT(5.7); ublas_matrix(5, 8) = NumericT(5.8); ublas_matrix(5, 9) = NumericT(5.9);
   ublas_matrix(6,5) = NumericT(6.5); ublas_matrix(6, 6) = NumericT(6.6); ublas_matrix(6, 7) = NumericT(6.7); ublas_matrix(6, 8) = NumericT(6.8); ublas_matrix(6, 9) = NumericT(6.9);
   ublas_matrix(7,5) = NumericT(7.5); ublas_matrix(7, 6) = NumericT(7.6); ublas_matrix(7, 7) = NumericT(7.7); ublas_matrix(7, 8) = NumericT(7.8); ublas_matrix(7, 9) = NumericT(7.9);
   ublas_matrix(8,5) = NumericT(8.5); ublas_matrix(8, 6) = NumericT(8.6); ublas_matrix(8, 7) = NumericT(8.7); ublas_matrix(8, 8) = NumericT(8.8); ublas_matrix(8, 9) = NumericT(8.9);
   ublas_matrix(9,5) = NumericT(9.5); ublas_matrix(9, 6) = NumericT(9.6); ublas_matrix(9, 7) = NumericT(9.7); ublas_matrix(9, 8) = NumericT(9.8); ublas_matrix(9, 9) = NumericT(9.9);
   viennacl::copy(ublas_matrix, vcl_matrix);

   std::cout << "Testing resize to smaller..." << std::endl;
   ublas_matrix.resize(7, 7, false); //ublas does not allow preserve = true here
   ublas_matrix(0,0) = NumericT(10.0); ublas_matrix(0, 1) = NumericT(0.1); ublas_matrix(0, 2) = NumericT(0.2); ublas_matrix(0, 3) = NumericT(0.3); ublas_matrix(0, 4) = NumericT(0.4);
   ublas_matrix(1,0) = NumericT( 1.0); ublas_matrix(1, 1) = NumericT(1.1); ublas_matrix(1, 2) = NumericT(1.2); ublas_matrix(1, 3) = NumericT(1.3); ublas_matrix(1, 4) = NumericT(1.4);
   ublas_matrix(2,0) = NumericT( 2.0); ublas_matrix(2, 1) = NumericT(2.1); ublas_matrix(2, 2) = NumericT(2.2); ublas_matrix(2, 3) = NumericT(2.3); ublas_matrix(2, 4) = NumericT(2.4);
   ublas_matrix(3,0) = NumericT( 3.0); ublas_matrix(3, 1) = NumericT(3.1); ublas_matrix(3, 2) = NumericT(3.2); ublas_matrix(3, 3) = NumericT(3.3); ublas_matrix(3, 4) = NumericT(3.4);
   ublas_matrix(4,0) = NumericT( 4.0); ublas_matrix(4, 1) = NumericT(4.1); ublas_matrix(4, 2) = NumericT(4.2); ublas_matrix(4, 3) = NumericT(4.3); ublas_matrix(4, 4) = NumericT(4.4);
   ublas_matrix(5,5) = NumericT( 5.5); ublas_matrix(5, 6) = NumericT(5.6); ublas_matrix(5, 7) = NumericT(5.7); ublas_matrix(5, 8) = NumericT(5.8); ublas_matrix(5, 9) = NumericT(5.9);
   ublas_matrix(6,5) = NumericT( 6.5); ublas_matrix(6, 6) = NumericT(6.6); ublas_matrix(6, 7) = NumericT(6.7); ublas_matrix(6, 8) = NumericT(6.8); ublas_matrix(6, 9) = NumericT(6.9);

   vcl_matrix.resize(7, 7);

   //std::cout << ublas_matrix << std::endl;
    if ( std::fabs(diff(ublas_matrix, vcl_matrix)) > epsilon )
    {
        std::cout << "# Error at operation: resize (to smaller) with sparse matrix" << std::endl;
        std::cout << "  diff: " << std::fabs(diff(ublas_matrix, vcl_matrix)) << std::endl;
        retval = EXIT_FAILURE;
    }

   ublas::vector<NumericT> ublas_vec = ublas::scalar_vector<NumericT>(ublas_matrix.size1(), NumericT(3.1415));
   viennacl::vector<NumericT> vcl_vec(ublas_matrix.size1());

  std::cout << "Testing transposed unit lower triangular solve: compressed_matrix" << std::endl;
  viennacl::copy(ublas_vec, vcl_vec);
  std::cout << "matrix: " << ublas_matrix << std::endl;
  std::cout << "vector: " << ublas_vec << std::endl;
  std::cout << "ViennaCL matrix size: " << vcl_matrix.size1() << " x " << vcl_matrix.size2() << std::endl;

  std::cout << "ublas..." << std::endl;
  boost::numeric::ublas::inplace_solve((ublas_matrix), ublas_vec, boost::numeric::ublas::unit_lower_tag());
  std::cout << "ViennaCL..." << std::endl;
  viennacl::linalg::inplace_solve((vcl_matrix), vcl_vec, viennacl::linalg::unit_lower_tag());

  /*
  std::list< viennacl::backend::mem_handle > multifrontal_L_row_index_arrays_;
  std::list< viennacl::backend::mem_handle > multifrontal_L_row_buffers_;
  std::list< viennacl::backend::mem_handle > multifrontal_L_col_buffers_;
  std::list< viennacl::backend::mem_handle > multifrontal_L_element_buffers_;
  std::list< std::size_t > multifrontal_L_row_elimination_num_list_;

  viennacl::vector<NumericT> multifrontal_U_diagonal_;

  viennacl::linalg::detail::multifrontal_setup_L(vcl_matrix,
                                                  multifrontal_U_diagonal_, //dummy
                                                  multifrontal_L_row_index_arrays_,
                                                  multifrontal_L_row_buffers_,
                                                  multifrontal_L_col_buffers_,
                                                  multifrontal_L_element_buffers_,
                                                  multifrontal_L_row_elimination_num_list_);

  viennacl::linalg::detail::multifrontal_substitute(vcl_vec,
                                                    multifrontal_L_row_index_arrays_,
                                                    multifrontal_L_row_buffers_,
                                                    multifrontal_L_col_buffers_,
                                                    multifrontal_L_element_buffers_,
                                                    multifrontal_L_row_elimination_num_list_);

  std::cout << "ublas..." << std::endl;
  boost::numeric::ublas::inplace_solve((ublas_matrix), ublas_vec, boost::numeric::ublas::upper_tag());
  std::cout << "ViennaCL..." << std::endl;
  std::list< viennacl::backend::mem_handle > multifrontal_U_row_index_arrays_;
  std::list< viennacl::backend::mem_handle > multifrontal_U_row_buffers_;
  std::list< viennacl::backend::mem_handle > multifrontal_U_col_buffers_;
  std::list< viennacl::backend::mem_handle > multifrontal_U_element_buffers_;
  std::list< std::size_t > multifrontal_U_row_elimination_num_list_;

  multifrontal_U_diagonal_.resize(vcl_matrix.size1(), false);
  viennacl::linalg::single_threaded::detail::row_info(vcl_matrix, multifrontal_U_diagonal_, viennacl::linalg::detail::SPARSE_ROW_DIAGONAL);
  viennacl::linalg::detail::multifrontal_setup_U(vcl_matrix,
                                                 multifrontal_U_diagonal_,
                                                 multifrontal_U_row_index_arrays_,
                                                 multifrontal_U_row_buffers_,
                                                 multifrontal_U_col_buffers_,
                                                 multifrontal_U_element_buffers_,
                                                 multifrontal_U_row_elimination_num_list_);

  vcl_vec = viennacl::linalg::element_div(vcl_vec, multifrontal_U_diagonal_);
  viennacl::linalg::detail::multifrontal_substitute(vcl_vec,
                                                    multifrontal_U_row_index_arrays_,
                                                    multifrontal_U_row_buffers_,
                                                    multifrontal_U_col_buffers_,
                                                    multifrontal_U_element_buffers_,
                                                    multifrontal_U_row_elimination_num_list_);
  */
  for (std::size_t i=0; i<ublas_vec.size(); ++i)
  {
    std::cout << ublas_vec[i] << " vs. " << vcl_vec[i] << std::endl;
  }

  /*std::cout << "Testing transposed unit upper triangular solve: compressed_matrix" << std::endl;
  viennacl::copy(ublas_vec, vcl_vec);
  std::cout << "matrix: " << ublas_matrix << std::endl;
  std::cout << "vector: " << ublas_vec << std::endl;
  std::cout << "ViennaCL matrix size: " << vcl_matrix.size1() << " x " << vcl_matrix.size2() << std::endl;

  std::cout << "ublas..." << std::endl;
  boost::numeric::ublas::inplace_solve((ublas_matrix), ublas_vec, boost::numeric::ublas::lower_tag());
  std::cout << "ViennaCL..." << std::endl;
  viennacl::linalg::inplace_solve((vcl_matrix), vcl_vec, viennacl::linalg::lower_tag());

  for (std::size_t i=0; i<ublas_vec.size(); ++i)
  {
    std::cout << ublas_vec[i] << " vs. " << vcl_vec[i] << std::endl;
  }*/

  return retval;
}

//
// -------------------------------------------------------------
//
template< typename NumericT, typename Epsilon >
int test(Epsilon const& epsilon)
{
  std::cout << "Testing resizing of compressed_matrix..." << std::endl;
  int retval = resize_test<NumericT, viennacl::compressed_matrix<NumericT> >(epsilon);
  if (retval != EXIT_SUCCESS)
    return retval;
  std::cout << "Testing resizing of coordinate_matrix..." << std::endl;
  //if (retval != EXIT_FAILURE)
  //  retval = resize_test<NumericT, viennacl::coordinate_matrix<NumericT> >(epsilon);
  //else
  //  return retval;

  // --------------------------------------------------------------------------
  ublas::vector<NumericT> rhs;
  ublas::vector<NumericT> result;
  ublas::compressed_matrix<NumericT> ublas_matrix;

  if (viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx") == EXIT_FAILURE)
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return EXIT_FAILURE;
  }

  //unsigned int cg_mat_size = cg_mat.size();
  std::cout << "done reading matrix" << std::endl;

  rhs.resize(ublas_matrix.size2());
  for (std::size_t i=0; i<rhs.size(); ++i)
  {
    ublas_matrix(i,i) = NumericT(0.5);   // Get rid of round-off errors by making row-sums unequal to zero:
    rhs[i] = NumericT(1) + random<NumericT>();
  }

  // add some random numbers to the double-compressed matrix:
  ublas::compressed_matrix<NumericT> ublas_cc_matrix(ublas_matrix.size1(), ublas_matrix.size2());
  ublas_cc_matrix(42,199) = NumericT(3.1415);
  ublas_cc_matrix(31, 69) = NumericT(2.71);
  ublas_cc_matrix(23, 32) = NumericT(6);
  ublas_cc_matrix(177,57) = NumericT(4);
  ublas_cc_matrix(21, 97) = NumericT(-4);
  ublas_cc_matrix(92, 25) = NumericT(2);
  ublas_cc_matrix(89, 62) = NumericT(11);
  ublas_cc_matrix(1,   7) = NumericT(8);
  ublas_cc_matrix(85, 41) = NumericT(13);
  ublas_cc_matrix(66, 28) = NumericT(8);
  ublas_cc_matrix(21, 74) = NumericT(-2);

  result = rhs;

  viennacl::vector<NumericT> vcl_rhs(rhs.size());
  viennacl::vector<NumericT> vcl_result(result.size());
  viennacl::vector<NumericT> vcl_result2(result.size());
  viennacl::compressed_matrix<NumericT> vcl_compressed_matrix(rhs.size(), rhs.size());
  viennacl::compressed_compressed_matrix<NumericT> vcl_compressed_compressed_matrix(rhs.size(), rhs.size());
  viennacl::coordinate_matrix<NumericT> vcl_coordinate_matrix(rhs.size(), rhs.size());
  viennacl::ell_matrix<NumericT> vcl_ell_matrix;
  viennacl::sliced_ell_matrix<NumericT> vcl_sliced_ell_matrix;
  viennacl::hyb_matrix<NumericT> vcl_hyb_matrix;

  viennacl::copy(rhs.begin(), rhs.end(), vcl_rhs.begin());
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);
  viennacl::copy(ublas_cc_matrix, vcl_compressed_compressed_matrix);
  viennacl::copy(ublas_matrix, vcl_coordinate_matrix);

  // --------------------------------------------------------------------------
  std::cout << "Testing products: ublas" << std::endl;
  result     = viennacl::linalg::prod(ublas_matrix, rhs);

  std::cout << "Testing products: compressed_matrix" << std::endl;
  vcl_result = viennacl::linalg::prod(vcl_compressed_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products: compressed_matrix, strided vectors" << std::endl;
  retval = strided_matrix_vector_product_test<NumericT, viennacl::compressed_matrix<NumericT> >(epsilon, result, rhs, vcl_result, vcl_rhs);
  if (retval != EXIT_SUCCESS)
    return retval;

  //
  // Triangular solvers for A \ b:
  //
  ublas::compressed_matrix<NumericT> ublas_matrix_trans(ublas_matrix.size2(), ublas_matrix.size1(), ublas_matrix.nnz()); // = trans(ublas_matrix); //note: triangular solvers with uBLAS show atrocious performance, while transposed solvers are quite okay. To keep execution times short, we use a double-transpose-trick in the following.

  // fast transpose:
  for (typename ublas::compressed_matrix<NumericT>::iterator1 row_it  = ublas_matrix.begin1();
                                                              row_it != ublas_matrix.end1();
                                                            ++row_it)
  {
    for (typename ublas::compressed_matrix<NumericT>::iterator2 col_it  = row_it.begin();
                                                                col_it != row_it.end();
                                                              ++col_it)
    {
      ublas_matrix_trans(col_it.index1(), col_it.index2()) = *col_it;
    }
  }

  std::cout << "Testing unit upper triangular solve: compressed_matrix" << std::endl;
  result = rhs;
  viennacl::copy(result, vcl_result);
  boost::numeric::ublas::inplace_solve(trans(ublas_matrix_trans), result, boost::numeric::ublas::unit_upper_tag());
  viennacl::linalg::inplace_solve(vcl_compressed_matrix, vcl_result, viennacl::linalg::unit_upper_tag());

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: unit upper triangular solve with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing upper triangular solve: compressed_matrix" << std::endl;
  result = rhs;
  viennacl::copy(result, vcl_result);
  boost::numeric::ublas::inplace_solve(trans(ublas_matrix_trans), result, boost::numeric::ublas::upper_tag());
  viennacl::linalg::inplace_solve(vcl_compressed_matrix, vcl_result, viennacl::linalg::upper_tag());

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: upper triangular solve with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing unit lower triangular solve: compressed_matrix" << std::endl;
  result = rhs;
  viennacl::copy(result, vcl_result);
  boost::numeric::ublas::inplace_solve(trans(ublas_matrix_trans), result, boost::numeric::ublas::unit_lower_tag());
  viennacl::linalg::inplace_solve(vcl_compressed_matrix, vcl_result, viennacl::linalg::unit_lower_tag());

  /*std::list< viennacl::backend::mem_handle > multifrontal_L_row_index_arrays_;
  std::list< viennacl::backend::mem_handle > multifrontal_L_row_buffers_;
  std::list< viennacl::backend::mem_handle > multifrontal_L_col_buffers_;
  std::list< viennacl::backend::mem_handle > multifrontal_L_element_buffers_;
  std::list< std::size_t > multifrontal_L_row_elimination_num_list_;

  viennacl::vector<NumericT> multifrontal_U_diagonal_;

  viennacl::switch_memory_domain(multifrontal_U_diagonal_, viennacl::MAIN_MEMORY);
  multifrontal_U_diagonal_.resize(vcl_compressed_matrix.size1(), false);
  viennacl::linalg::single_threaded::detail::row_info(vcl_compressed_matrix, multifrontal_U_diagonal_, viennacl::linalg::detail::SPARSE_ROW_DIAGONAL);

  viennacl::linalg::detail::multifrontal_setup_L(vcl_compressed_matrix,
                                                  multifrontal_U_diagonal_, //dummy
                                                  multifrontal_L_row_index_arrays_,
                                                  multifrontal_L_row_buffers_,
                                                  multifrontal_L_col_buffers_,
                                                  multifrontal_L_element_buffers_,
                                                  multifrontal_L_row_elimination_num_list_);

  viennacl::linalg::detail::multifrontal_substitute(vcl_result,
                                                    multifrontal_L_row_index_arrays_,
                                                    multifrontal_L_row_buffers_,
                                                    multifrontal_L_col_buffers_,
                                                    multifrontal_L_element_buffers_,
                                                    multifrontal_L_row_elimination_num_list_);*/

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: unit lower triangular solve with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing lower triangular solve: compressed_matrix" << std::endl;
  result = rhs;
  viennacl::copy(result, vcl_result);
  boost::numeric::ublas::inplace_solve(trans(ublas_matrix_trans), result, boost::numeric::ublas::lower_tag());
  viennacl::linalg::inplace_solve(vcl_compressed_matrix, vcl_result, viennacl::linalg::lower_tag());

  /*std::list< viennacl::backend::mem_handle > multifrontal_U_row_index_arrays_;
  std::list< viennacl::backend::mem_handle > multifrontal_U_row_buffers_;
  std::list< viennacl::backend::mem_handle > multifrontal_U_col_buffers_;
  std::list< viennacl::backend::mem_handle > multifrontal_U_element_buffers_;
  std::list< std::size_t > multifrontal_U_row_elimination_num_list_;

  multifrontal_U_diagonal_.resize(vcl_compressed_matrix.size1(), false);
  viennacl::linalg::single_threaded::detail::row_info(vcl_compressed_matrix, multifrontal_U_diagonal_, viennacl::linalg::detail::SPARSE_ROW_DIAGONAL);
  viennacl::linalg::detail::multifrontal_setup_U(vcl_compressed_matrix,
                                                 multifrontal_U_diagonal_,
                                                 multifrontal_U_row_index_arrays_,
                                                 multifrontal_U_row_buffers_,
                                                 multifrontal_U_col_buffers_,
                                                 multifrontal_U_element_buffers_,
                                                 multifrontal_U_row_elimination_num_list_);

  vcl_result = viennacl::linalg::element_div(vcl_result, multifrontal_U_diagonal_);
  viennacl::linalg::detail::multifrontal_substitute(vcl_result,
                                                    multifrontal_U_row_index_arrays_,
                                                    multifrontal_U_row_buffers_,
                                                    multifrontal_U_col_buffers_,
                                                    multifrontal_U_element_buffers_,
                                                    multifrontal_U_row_elimination_num_list_);*/

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: lower triangular solve with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

/*
  std::cout << "Testing lower triangular solve: compressed_matrix" << std::endl;
  result = rhs;
  viennacl::copy(result, vcl_result);
  boost::numeric::ublas::inplace_solve(ublas_matrix, result, boost::numeric::ublas::lower_tag());
  viennacl::linalg::inplace_solve(vcl_compressed_matrix, vcl_result, viennacl::linalg::lower_tag());

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: lower triangular solve with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }*/

  //
  // Triangular solvers for A^T \ b
  //

  std::cout << "Testing transposed unit upper triangular solve: compressed_matrix" << std::endl;
  result = rhs;
  viennacl::copy(result, vcl_result);
  boost::numeric::ublas::inplace_solve(trans(ublas_matrix), result, boost::numeric::ublas::unit_upper_tag());
  viennacl::linalg::inplace_solve(trans(vcl_compressed_matrix), vcl_result, viennacl::linalg::unit_upper_tag());

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: unit upper triangular solve with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing transposed upper triangular solve: compressed_matrix" << std::endl;
  result = rhs;
  viennacl::copy(result, vcl_result);
  boost::numeric::ublas::inplace_solve(trans(ublas_matrix), result, boost::numeric::ublas::upper_tag());
  viennacl::linalg::inplace_solve(trans(vcl_compressed_matrix), vcl_result, viennacl::linalg::upper_tag());

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: upper triangular solve with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing transposed unit lower triangular solve: compressed_matrix" << std::endl;
  result = rhs;
  viennacl::copy(result, vcl_result);
  boost::numeric::ublas::inplace_solve(trans(ublas_matrix), result, boost::numeric::ublas::unit_lower_tag());
  viennacl::linalg::inplace_solve(trans(vcl_compressed_matrix), vcl_result, viennacl::linalg::unit_lower_tag());

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: unit lower triangular solve with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing transposed lower triangular solve: compressed_matrix" << std::endl;
  result = rhs;
  viennacl::copy(result, vcl_result);
  boost::numeric::ublas::inplace_solve(trans(ublas_matrix), result, boost::numeric::ublas::lower_tag());
  viennacl::linalg::inplace_solve(trans(vcl_compressed_matrix), vcl_result, viennacl::linalg::lower_tag());

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: lower triangular solve with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products: compressed_compressed_matrix" << std::endl;
  result     = viennacl::linalg::prod(ublas_cc_matrix, rhs);
  vcl_result = viennacl::linalg::prod(vcl_compressed_compressed_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with compressed_compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  {
    ublas::compressed_matrix<NumericT> temp(vcl_compressed_compressed_matrix.size1(), vcl_compressed_compressed_matrix.size2());
    viennacl::copy(vcl_compressed_compressed_matrix, temp);

    // check that entries are correct by computing the product again:
    result     = viennacl::linalg::prod(temp, rhs);

    if ( std::fabs(diff(result, vcl_result)) > epsilon )
    {
      std::cout << "# Error at operation: matrix-vector product with compressed_compressed_matrix (after copy back)" << std::endl;
      std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
      retval = EXIT_FAILURE;
    }

  }

  std::cout << "Testing products: coordinate_matrix" << std::endl;
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result = viennacl::linalg::prod(vcl_coordinate_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with coordinate_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products: coordinate_matrix, strided vectors" << std::endl;
  //std::cout << " --> SKIPPING <--" << std::endl;
  retval = strided_matrix_vector_product_test<NumericT, viennacl::coordinate_matrix<NumericT> >(epsilon, result, rhs, vcl_result, vcl_rhs);
  if (retval != EXIT_SUCCESS)
    return retval;

  //std::cout << "Copying ell_matrix" << std::endl;
  viennacl::copy(ublas_matrix, vcl_ell_matrix);
  ublas_matrix.clear();
  viennacl::copy(vcl_ell_matrix, ublas_matrix);// just to check that it's works

  std::cout << "Testing products: ell_matrix" << std::endl;
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result.clear();
  vcl_result = viennacl::linalg::prod(vcl_ell_matrix, vcl_rhs);
  //viennacl::linalg::prod_impl(vcl_ell_matrix, vcl_rhs, vcl_result);
  //std::cout << vcl_result << "\n";
  //std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
  //std::cout << "First entry of result vector: " << vcl_result[0] << std::endl;

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with ell_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products: ell_matrix, strided vectors" << std::endl;
  retval = strided_matrix_vector_product_test<NumericT, viennacl::ell_matrix<NumericT> >(epsilon, result, rhs, vcl_result, vcl_rhs);
  if (retval != EXIT_SUCCESS)
    return retval;

  //std::cout << "Copying sliced_ell_matrix" << std::endl;
  viennacl::copy(ublas_matrix, vcl_sliced_ell_matrix);
  //ublas_matrix.clear();
  //viennacl::copy(vcl_hyb_matrix, ublas_matrix);// just to check that it's works
  //viennacl::copy(ublas_matrix, vcl_hyb_matrix);

  std::cout << "Testing products: sliced_ell_matrix" << std::endl;
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result.clear();
  vcl_result = viennacl::linalg::prod(vcl_sliced_ell_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with sliced_ell_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products: sliced_ell_matrix, strided vectors" << std::endl;
  retval = strided_matrix_vector_product_test<NumericT, viennacl::sliced_ell_matrix<NumericT> >(epsilon, result, rhs, vcl_result, vcl_rhs);
  if (retval != EXIT_SUCCESS)
    return retval;

  //std::cout << "Copying hyb_matrix" << std::endl;
  viennacl::copy(ublas_matrix, vcl_hyb_matrix);
  ublas_matrix.clear();
  viennacl::copy(vcl_hyb_matrix, ublas_matrix);// just to check that it's works
  viennacl::copy(ublas_matrix, vcl_hyb_matrix);

  std::cout << "Testing products: hyb_matrix" << std::endl;
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result.clear();
  vcl_result = viennacl::linalg::prod(vcl_hyb_matrix, vcl_rhs);
  //viennacl::linalg::prod_impl(vcl_hyb_matrix, vcl_rhs, vcl_result);
  //std::cout << vcl_result << "\n";
  //std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
  //std::cout << "First entry of result vector: " << vcl_result[0] << std::endl;

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with hyb_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products: hyb_matrix, strided vectors" << std::endl;
  retval = strided_matrix_vector_product_test<NumericT, viennacl::hyb_matrix<NumericT> >(epsilon, result, rhs, vcl_result, vcl_rhs);
  if (retval != EXIT_SUCCESS)
    return retval;

  // --------------------------------------------------------------------------
  // --------------------------------------------------------------------------
  NumericT alpha = static_cast<NumericT>(2.786);
  NumericT beta = static_cast<NumericT>(1.432);
  copy(rhs.begin(), rhs.end(), vcl_rhs.begin());
  copy(result.begin(), result.end(), vcl_result.begin());
  copy(result.begin(), result.end(), vcl_result2.begin());

  std::cout << "Testing scaled additions of products and vectors" << std::endl;
  result     = alpha * viennacl::linalg::prod(ublas_matrix, rhs) + beta * result;
  vcl_result2 = alpha * viennacl::linalg::prod(vcl_compressed_matrix, vcl_rhs) + beta * vcl_result;

  if ( std::fabs(diff(result, vcl_result2)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product (compressed_matrix) with scaled additions" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result2)) << std::endl;
    retval = EXIT_FAILURE;
  }

  vcl_result2.clear();
  vcl_result2 = alpha * viennacl::linalg::prod(vcl_coordinate_matrix, vcl_rhs) + beta * vcl_result;

  if ( std::fabs(diff(result, vcl_result2)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product (coordinate_matrix) with scaled additions" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result2)) << std::endl;
    retval = EXIT_FAILURE;
  }

  vcl_result2.clear();
  vcl_result2 = alpha * viennacl::linalg::prod(vcl_ell_matrix, vcl_rhs) + beta * vcl_result;

  if ( std::fabs(diff(result, vcl_result2)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product (ell_matrix) with scaled additions" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result2)) << std::endl;
    retval = EXIT_FAILURE;
  }

  vcl_result2.clear();
  vcl_result2 = alpha * viennacl::linalg::prod(vcl_hyb_matrix, vcl_rhs) + beta * vcl_result;

  if ( std::fabs(diff(result, vcl_result2)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product (hyb_matrix) with scaled additions" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result2)) << std::endl;
    retval = EXIT_FAILURE;
  }

  ////////////// Test of .clear() ////////////////
  ublas_matrix.clear();

  std::cout << "Testing products after clear(): compressed_matrix" << std::endl;
  vcl_compressed_matrix.clear();
  result     = ublas::scalar_vector<NumericT>(result.size(), NumericT(1));
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result = viennacl::linalg::prod(vcl_compressed_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products after clear(): compressed_compressed_matrix" << std::endl;
  vcl_compressed_compressed_matrix.clear();
  result     = ublas::scalar_vector<NumericT>(result.size(), NumericT(1));
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result = viennacl::linalg::prod(vcl_compressed_compressed_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with compressed_compressed_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products after clear(): coordinate_matrix" << std::endl;
  vcl_coordinate_matrix.clear();
  result     = ublas::scalar_vector<NumericT>(result.size(), NumericT(1));
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result = viennacl::linalg::prod(vcl_coordinate_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with coordinate_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products after clear(): ell_matrix" << std::endl;
  vcl_ell_matrix.clear();
  result     = ublas::scalar_vector<NumericT>(result.size(), NumericT(1));
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result = viennacl::linalg::prod(vcl_ell_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with ell_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products after clear(): hyb_matrix" << std::endl;
  vcl_hyb_matrix.clear();
  result     = ublas::scalar_vector<NumericT>(result.size(), NumericT(1));
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result = viennacl::linalg::prod(vcl_hyb_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with hyb_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  std::cout << "Testing products after clear(): sliced_ell_matrix" << std::endl;
  vcl_sliced_ell_matrix.clear();
  result     = ublas::scalar_vector<NumericT>(result.size(), NumericT(1));
  result     = viennacl::linalg::prod(ublas_matrix, rhs);
  vcl_result = viennacl::linalg::prod(vcl_sliced_ell_matrix, vcl_rhs);

  if ( std::fabs(diff(result, vcl_result)) > epsilon )
  {
    std::cout << "# Error at operation: matrix-vector product with sliced_ell_matrix" << std::endl;
    std::cout << "  diff: " << std::fabs(diff(result, vcl_result)) << std::endl;
    retval = EXIT_FAILURE;
  }

  // --------------------------------------------------------------------------
  return retval;
}
//
// -------------------------------------------------------------
//
int main()
{
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "## Test :: Sparse Matrices" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << std::endl;

  int retval = EXIT_SUCCESS;

  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << std::endl;
  {
    typedef float NumericT;
    NumericT epsilon = static_cast<NumericT>(1E-4);
    std::cout << "# Testing setup:" << std::endl;
    std::cout << "  eps:     " << epsilon << std::endl;
    std::cout << "  numeric: float" << std::endl;
    retval = test<NumericT>(epsilon);
    if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
    else
        return retval;
  }
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << std::endl;

#ifdef VIENNACL_WITH_OPENCL
  if ( viennacl::ocl::current_device().double_support() )
#endif
  {
    {
      typedef double NumericT;
      NumericT epsilon = 1.0E-12;
      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  eps:     " << epsilon << std::endl;
      std::cout << "  numeric: double" << std::endl;
      retval = test<NumericT>(epsilon);
      if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
      else
        return retval;
    }
    std::cout << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << std::endl;
  }
#ifdef VIENNACL_WITH_OPENCL
  else
    std::cout << "No double precision support, skipping test..." << std::endl;
#endif

  std::cout << std::endl;
  std::cout << "------- Test completed --------" << std::endl;
  std::cout << std::endl;

  return retval;
}


#include <ros/ros.h>
#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Empty.h>
#include <boost/ref.hpp>

#include "teleop_tracking/mesh.h"
#include "visualizations.hpp"

#include <eigen3/Eigen/Dense>
#include <eigen_conversions/eigen_msg.h>

#include "teleop_robot/robot_interface.h"

struct TeleopOptions
{
  double max_theta; // deviation from world z (rad)
  double standoff; // distance (m)
};

/**
 *
 */
class TrackingMode {
public:
  struct Transaction {
    Eigen::Affine3d target;
    teleop_tracking::TrianglePose triangle_pose;
  };

  TrackingMode(const teleop_tracking::Mesh& mesh, const TeleopOptions& options)
    : mesh_(mesh)
    , options_(options)
  {}

  void start(const Eigen::Affine3d& init_pose)
  {
    // closest point start
    current_pose_ = mesh_.closestPose(init_pose.translation() - Eigen::Vector3d(0.5, 0, 0));
  }

  void startDir(const Eigen::Affine3d& init_pose)
  {
    teleop_tracking::TrianglePosition intersect;
    double dist;
    if (mesh_.intersectRay(init_pose.translation() - Eigen::Vector3d(0.5, 0, 0),
                           init_pose.matrix().col(2).head<3>(),
                           intersect, dist))
    {
      current_pose_ = mesh_.toPose(intersect);
    }
    else
    {
      ROS_WARN("Could not intersect point-of-view with part");
    }
  }

  Transaction calc(const geometry_msgs::Twist& twist)
  {
    // calculate new pose
    Transaction transaction;
    transaction.triangle_pose = current_pose_;

    Eigen::Affine3d& new_pose = transaction.triangle_pose.pose;
    unsigned& new_index = transaction.triangle_pose.index;

    if (twist.linear.z != 0.0)
    {
       options_.standoff += twist.linear.z;
    }

    if (twist.angular.z != 0.0)
    {
      Eigen::AngleAxisd z_rot (twist.angular.z, Eigen::Vector3d::UnitZ());
      new_pose = new_pose * z_rot;
    }

    if (std::abs(twist.linear.x) > 0.0001 || std::abs(twist.linear.y) > 0.0001)
    {
      Eigen::Vector2d t (twist.linear.x, twist.linear.y);
      new_pose =  mesh_.walkTriangle2(new_pose, new_index, new_index, t);
    }

    transaction.target = modelToRobotPose(Eigen::Translation3d(0.5, 0, 0)*new_pose,
                                          twist.angular);
    return transaction;
  }

  void update(const Transaction& transaction)
  {
    current_pose_ = transaction.triangle_pose;
  }

  const teleop_tracking::Mesh& mesh() const { return mesh_; }

private:
  Eigen::Affine3d modelToRobotPose(const Eigen::Affine3d& model,
                                   const geometry_msgs::Vector3& ang) const
  {
    const static Eigen::AngleAxisd flip_z (M_PI, Eigen::Vector3d::UnitX());

    Eigen::Affine3d target;
    target = model;

    // Calculate offset between world and pose

    const Eigen::Vector3d& model_z = model.matrix().col(2).head<3>();
    const Eigen::Vector3d& world_z = Eigen::Vector3d::UnitZ();
    double amt = std::acos(model_z.dot(world_z));

    // Apply the static limits to tool orientation from Z
    if (std::abs(amt) > options_.max_theta)
    {
      Eigen::Vector3d axis = model_z.cross(world_z);
      Eigen::Vector3d local_axis = model.inverse().linear() * axis;
      Eigen::AngleAxisd local_rot(options_.max_theta, local_axis.normalized());
      target = model * local_rot;
    }

    // If the user gave x or y angular input, perform further adjustments
    if (ang.x != 0.0)
    {
      target *= Eigen::AngleAxisd(ang.x, Eigen::Vector3d::UnitX());
    }

    if (ang.y != 0.0)
    {
      target *= Eigen::AngleAxisd(ang.y, Eigen::Vector3d::UnitY());
    }

    // Translate to a valid camera position
     target = target * Eigen::Translation3d(0, 0, options_.standoff) * flip_z;

    return target;
  }

  const teleop_tracking::Mesh& mesh_;
  teleop_tracking::TrianglePose current_pose_;
  TeleopOptions options_;
};

/**
 *
 */
class FreeMode {
public:
  struct Transaction {
    Eigen::Affine3d target;
  };

  FreeMode()
    : current_pose_(Eigen::Affine3d::Identity())
  {}

  void start(const Eigen::Affine3d& init_pose)
  {
    current_pose_ = init_pose;
  }

  Transaction calc(const geometry_msgs::Twist& twist) const
  {
    Transaction trans;
    trans.target = current_pose_;
    Eigen::Affine3d step;
    step = Eigen::Translation3d(twist.linear.x, -twist.linear.y, -twist.linear.z);

    step = step * Eigen::AngleAxisd(twist.angular.x/10.0, Eigen::Vector3d::UnitX())
                * Eigen::AngleAxisd(-twist.angular.y/10.0, Eigen::Vector3d::UnitY())
                * Eigen::AngleAxisd(twist.angular.z, Eigen::Vector3d::UnitZ());

    trans.target = trans.target * step;
    return trans;
  }

  void update(const Transaction& transaction)
  {
    current_pose_ = transaction.target;
  }

private:
  Eigen::Affine3d current_pose_;
};

class Teleop
{
public:
  enum class OperationMode
  {
    Tracking, Free
  };

  Teleop(teleop_tracking::Mesh& mesh,
         teleop_robot::RobotInterface& interface,
         const TeleopOptions& options,
         ros::NodeHandle& nh)
    : tracking_mode_(mesh, options)
    , free_mode_()
    , active_mode_(OperationMode::Tracking)
    , last_message_(ros::Time::now())
    , interface_(interface)
  {
    // Initialize state machine
    Eigen::Affine3d start_pose = Eigen::Affine3d::Identity();
    start_pose.translation() = Eigen::Vector3d(0.1, 0.1, 1.0); // arbitrary starting position
    tracking_mode_.start(start_pose);

    // Initialize publishers
    pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("track_point", 1);
    state_pub_ = nh.advertise<sensor_msgs::JointState>("target", 1);
    visual_pub_ = nh.advertise<visualization_msgs::Marker>("looking_at", 1);
    points_pub_ = nh.advertise<geometry_msgs::Point>("incident_point", 100);
  }

  void update(const geometry_msgs::TwistConstPtr& mv)
  {
    if (seed_.empty())
    {
      return;
    }

    if (ros::Time::now() - last_message_ < ros::Duration(1.0 / 30.0))
    {
      return;
    }
    last_message_ = ros::Time::now();

    if (active_mode_ == OperationMode::Tracking)
    {
      // Calc looking point, add to point cloud
      Eigen::Vector3d v;
      if (this->lookingAt(v))
      {
        geometry_msgs::Point pt;
        pt.x = v(0); pt.y = v(1); pt.z = v(2);
        points_pub_.publish(pt);
      }

      // tracking surface
      TrackingMode::Transaction t = tracking_mode_.calc(*mv);
      pose_pub_.publish(makeStampedPose(t.triangle_pose.pose));
      if (interface_.planAndMove(seed_, t.target, state_pub_))
      {
        tracking_mode_.update(t);
      }
      else
      {
        ROS_WARN("Failed to move to new track position.");
      }
    }
    else
    {
      // free move
      FreeMode::Transaction t = free_mode_.calc(*mv);
      if (interface_.planAndMove(seed_, t.target, state_pub_))
      {
        free_mode_.update(t);
        // cast ray
        teleop_tracking::TrianglePosition intersect;
        double dist;
        if (tracking_mode_.mesh().intersectRay(t.target.translation() - Eigen::Vector3d(0.5, 0, 0),
                               t.target.matrix().col(2).head<3>(),
                               intersect, dist))
        {
          visual_pub_.publish(makeMarker(intersect.position, 0));
        }

      }
      else
      {
        ROS_WARN("Failed to move to new free position");
      }
    }
  }

  void updateSeedState(const sensor_msgs::JointStateConstPtr& joint_ptr)
  {
    seed_ = joint_ptr->position;
  }

  void updateMode(OperationMode mode)
  {
    if (mode == active_mode_) return;

    Eigen::Affine3d eef_pose;
    if (!interface_.fk(teleop_robot::fromVector(seed_), eef_pose))
    {
      ROS_WARN("Failed to compute FK to seed track mode.");
      return;
    }
    active_mode_ = mode;

    if (mode == OperationMode::Tracking)
    {
      tracking_mode_.startDir(eef_pose);
    }
    else
    {
      free_mode_.start(eef_pose);
    }
  }

  bool lookingAt(Eigen::Vector3d& pt) const
  {
    Eigen::Affine3d eef_pose;
    if (!interface_.fk(teleop_robot::fromVector(seed_), eef_pose))
    {
      ROS_WARN("Failed to compute FK to seed track mode.");
      return false;
    }

    teleop_tracking::TrianglePosition intersect;
    double dist;
    if (tracking_mode_.mesh().intersectRay(eef_pose.translation() - Eigen::Vector3d(0.5, 0, 0),
                           eef_pose.matrix().col(2).head<3>(),
                           intersect, dist))
    {
      pt = intersect.position;
      return true;
    }
    ROS_INFO("no intersect");
    return false;
  }

  OperationMode mode() const { return active_mode_; }

private:
  // Motion modes
  TrackingMode tracking_mode_;
  FreeMode free_mode_;
  OperationMode active_mode_;

  // Current state
  ros::Time last_message_;

  // Member objects
  teleop_robot::RobotInterface& interface_;

  // ROS Publishers
  ros::Publisher pose_pub_; // tracking pose (debug)
  ros::Publisher state_pub_; // publishes joint targets
  ros::Publisher visual_pub_; // publishes marker that indicates look pt in free-mode (debug)
  ros::Publisher points_pub_; // publishes the incident points in tracking-mode - integrates w/
                              // trail publishing node

  // Current position of the robot, used for IK/FK
  std::vector<double> seed_;
};

void callback(const std_msgs::EmptyConstPtr&, Teleop& teleop)
{
  ROS_INFO("TOGGLING MODE");
  if (teleop.mode() == Teleop::OperationMode::Free) {
    teleop.updateMode(Teleop::OperationMode::Tracking);
  } else {
    teleop.updateMode(Teleop::OperationMode::Free);
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "teleop_robot_node");
  ros::NodeHandle nh, pnh("~");

  std::string mesh_name;
  pnh.param<std::string>("mesh_file", mesh_name, "");

  if (mesh_name.empty())
  {
    ROS_FATAL("Mesh name required");
    return 1;
  }

  teleop_tracking::Mesh mesh(mesh_name);

  TeleopOptions options;
  options.max_theta = M_PI_4;
  options.standoff = 0.25;

  teleop_robot::RobotInterface interface("manipulator_camera");

  Teleop teleop (mesh, interface, options, nh);

  ros::Subscriber sub =
      nh.subscribe<geometry_msgs::Twist>("commands", 1,
                                         boost::bind(&Teleop::update, &teleop, _1));

  ros::Subscriber state_sub =
      nh.subscribe<sensor_msgs::JointState>("/joint_states", 1,
                                            boost::bind(&Teleop::updateSeedState, &teleop, _1));

  ros::Subscriber mode_sub =
      nh.subscribe<std_msgs::Empty>("change_mode", 1,
                                    boost::bind(callback, _1, boost::ref(teleop)));

  ros::spin();

  return 0;
}


#ifndef STAN_MATH_TORSTEN_PKMODEL_PRED_PREDSS_TWOCPT_HPP
#define STAN_MATH_TORSTEN_PKMODEL_PRED_PREDSS_TWOCPT_HPP

#include <stan/math/torsten/PKModel/Pred/PolyExp.hpp>
#include <stan/math/torsten/PKModel/ModelParameters.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>
#include <iostream>
#include <vector>
#include <limits>

namespace torsten {

struct PredSS_twoCpt {
  PredSS_twoCpt() { }

  /**
   * One compartment model with first-order absorption.
   * Calculate amount in each compartment at the end of a steady-state dosing interval
   * or during a steady-state constant input (if ii=0)
   *
   *  If the initial time equals the time of the event, then the code does
   *	not run the ode integrator, and sets the predicted amount equal to the
   *	initial condition. This can happen when we are dealing with events that
   *	occur simultaneously. The change to the predicted amount caused by bolus
   *	dosing events is handled later in the main Pred function.
   *
   *	 @tparam T_time type of scalar for time
   *	 @tparam T_amt type of scalar for amount
   *	 @tparam T_rate type of scalar for rate
   *	 @tparam T_ii type of scalar for interdose interval
   *	 @tparam T_parameters type of scalar for model parameters
   *	 @tparam T_addParm type of scalar for additional model parameters
   *	 @param[in] parameter model parameters at current event
   *	 @param[in] rate
   *	 @param[in] ii interdose interval
   *	 @param[in] cmt compartment in which the event occurs
   *   @return an eigen vector that contains predicted amount in each compartment
   *           at the current event.
   */
  template<typename T_time, typename T_amt, typename T_rate, typename T_ii,
           typename T_parameters, typename T_biovar, typename T_tlag>
  Eigen::Matrix<typename boost::math::tools::promote_args<T_amt, T_rate,
    T_ii, T_parameters>::type, 1, Eigen::Dynamic>
  operator() (const ModelParameters<T_time, T_parameters, T_biovar,
                T_tlag>& parameter,
              const T_amt& amt,
              const T_rate& rate,
              const T_ii& ii,
              const int& cmt) const {
    using std::vector;
    using Eigen::Matrix;
    using Eigen::Dynamic;
    using boost::math::tools::promote_args;

    typedef typename boost::math::tools::promote_args<T_amt, T_rate,
      T_ii, T_parameters>::type scalar;

    double inf = std::numeric_limits<double>::max();  // "infinity"

    T_parameters CL = parameter.get_RealParameters(false)[0],
      Q = parameter.get_RealParameters(false)[1],
      V2 = parameter.get_RealParameters(false)[2],
      V3 = parameter.get_RealParameters(false)[3],
      ka = parameter.get_RealParameters(false)[4],
      k10 = CL / V2,
      k12 = Q / V2,
      k21 = Q / V3,
      ksum = k10 + k12 + k21;

    vector<T_parameters> alpha(3, 0);
    alpha[0] = (ksum + sqrt(ksum * ksum - 4 * k10 * k21)) / 2,
      alpha[1] = (ksum - sqrt(ksum * ksum - 4 * k10 * k21)) / 2,
      alpha[2] = ka;

    vector<scalar> a(3, 0);
    Matrix<scalar, 1, Dynamic> pred = Matrix<scalar, 1, Dynamic>::Zero(3);
    if (rate == 0) {  // bolus dose
      if (cmt == 1) {
        pred(0, 0) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 3);
        a[0] = ka * (k21 - alpha[0]) / ((ka - alpha[0])
                                          * (alpha[1] - alpha[0]));
        a[1] = ka * (k21 - alpha[1]) / ((ka - alpha[1])
                                          * (alpha[0] - alpha[1]));
        a[2] = -(a[0] + a[1]);
        pred(0, 1) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 3);
        a[0] = ka * k12 / ((ka - alpha[0]) * (alpha[1] - alpha[0]));
        a[1] = ka * k12 / ((ka - alpha[1]) * (alpha[0] - alpha[1]));
        a[2] = -(a[0] + a[1]);
        pred(0, 2) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 3);
      } else if (cmt == 2) {
        a[0] = (k21 - alpha[0]) / (alpha[1] - alpha[0]);
        a[1] = (k21 - alpha[1]) / (alpha[0] - alpha[1]);
        pred(0, 1) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 2);
        a[0] = ka * k12 / ((ka - alpha[0]) * (alpha[1] - alpha[0]));
        a[1] = ka * k12 / ((ka - alpha[1]) * (alpha[0] - alpha[1]));
        pred(0, 2) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 3);
      } else {  // cmt=3
        a[0] = k21 / (alpha[1] - alpha[0]);
        a[1] = -a[0];
        pred(0, 1) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 2);
        a[0] = (k10 + k12 - alpha[0]) / (alpha[1] - alpha[0]);
        a[1] = (k10 + k12 - alpha[1]) / (alpha[0] - alpha[1]);
        pred(0, 2) = PolyExp(ii, amt, 0, 0, ii, true, a, alpha, 2);
      }
    } else if (ii > 0) {  // multiple truncated infusions
      double delta = unpromote(amt / rate);
      static const char* function("Steady State Event");
      check_mti(amt, delta, ii, function);

      if (cmt == 1) {
        a[2] = 1;
        pred(0, 0) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 3);
        a[0] = ka * (k21 - alpha[0]) / ((ka - alpha[0])
                                          * (alpha[1] - alpha[0]));
        a[1] = ka * (k21 - alpha[1]) / ((ka - alpha[1])
                                          * (alpha[0] - alpha[1]));
        a[2] = - (a[0] + a[1]);
        pred(0, 1) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 3);
        a[0] = ka * k12 / ((ka - alpha[0]) * (alpha[1] - alpha[0]));
        a[1] = ka * k12 / ((ka - alpha[1]) * (alpha[0] - alpha[1]));
        a[2] = -(a[0] + a[1]);
        pred(0, 2) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 3);
      } else if (cmt == 2) {
        a[0] = (k21 - alpha[0]) / (alpha[1] - alpha[0]);
        a[1] = (k21 - alpha[1]) / (alpha[0] - alpha[1]);
        pred(0, 1) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 2);
        a[0] = k12 / (alpha[1] - alpha[0]);
        a[1] = -a[0];
        pred(0, 2) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 2);
      } else {  // cmt=3
        a[0] = k21 / (alpha[1] - alpha[0]);
        a[1] = -a[0];
        pred(0, 1) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 2);
        a[0] = (k10 + k12 - alpha[0]) / (alpha[1] - alpha[0]);
        a[1] = (k10 + k12 - alpha[1]) / (alpha[0] - alpha[1]);
        pred(0, 2) = PolyExp(ii, 0, rate, amt / rate, ii, true, a, alpha, 2);
      }
    } else {  // constant infusion
      if (cmt == 1) {
        a[2] = 1;
        pred(0, 0) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 3);
        a[0] = ka * (k21 - alpha[0]) / ((ka - alpha[0])
                                          * (alpha[1] - alpha[0]));
        a[1] = ka * (k21 - alpha[1]) / ((ka - alpha[1])
                                          * (alpha[0] - alpha[1]));
        a[2] = -(a[0] + a[1]);
        pred(0, 1) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 3);
        a[0] = ka * k12 / ((ka - alpha[0]) * (alpha[1] - alpha[0]));
        a[1] = ka * k12 / ((ka - alpha[1]) * (alpha[0] - alpha[1]));
        a[2] = -(a[0] + a[1]);
        pred(0, 2) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 3);
      } else if (cmt == 2) {
        a[0] = (k21 - alpha[0]) / (alpha[1] - alpha[0]);
        a[1] = (k21 - alpha[1]) / (alpha[0] - alpha[1]);
        pred(0, 1) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 2);
        a[0] = k12 / (alpha[1] - alpha[0]);
        a[1] = -a[0];
        pred(0, 2) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 2);
      } else {  // cmt=3
        a[0] = k21 / (alpha[1] - alpha[0]);
        a[1] = -a[0];
        pred(0, 1) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 2);
        a[0] = (k10 + k12 - alpha[0]) / (alpha[1] - alpha[0]);
        a[1] = (k10 + k12 - alpha[1]) / (alpha[0] - alpha[1]);
        pred(0, 2) = PolyExp(0, 0, rate, inf, 0, true, a, alpha, 2);
      }
    }
    return pred;
  }
};

}
#endif


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include "ArborX_EnableViewComparison.hpp"
#include <ArborX_DetailsHeap.hpp>
#include <ArborX_DetailsPriorityQueue.hpp> // Less, Greater

#include <Kokkos_Array.hpp>

#include <boost/test/unit_test.hpp>

using namespace ArborX::Details;
namespace tt = boost::test_tools;

BOOST_AUTO_TEST_SUITE(HeapOperations)

BOOST_AUTO_TEST_CASE(push_heap)
{
  // Here checking against the example of binary heap insertion from
  // https://en.wikipedia.org/wiki/Binary_heap#Insert
  Kokkos::Array<int, 6> a = {11, 5, 8, 3, 4, 15};
  BOOST_TEST(isHeap(a.data(), a.data() + 5, Less<int>()));
  BOOST_TEST(!isHeap(a.data(), a.data() + 6, Less<int>()));
  pushHeap(a.data(), a.data() + 6, Less<int>());
  BOOST_TEST(isHeap(a.data(), a.data() + 6, Less<int>()));
  Kokkos::Array<int, 6> ref = {15, 5, 11, 3, 4, 8};
  BOOST_TEST(a == ref, tt::per_element());
}

BOOST_AUTO_TEST_CASE(pop_heap)
{
  // See https://en.wikipedia.org/wiki/Binary_heap#Extract
  Kokkos::Array<int, 5> a = {11, 5, 8, 3, 4};
  BOOST_TEST(isHeap(a.data(), a.data() + 5, Less<int>()));
  popHeap(a.data(), a.data() + 5, Less<int>());
  BOOST_TEST(isHeap(a.data(), a.data() + 4, Less<int>()));
  BOOST_TEST(!isHeap(a.data(), a.data() + 5, Less<int>()));
  Kokkos::Array<int, 5> ref = {8, 5, 4, 3, 11};
  BOOST_TEST(a == ref, tt::per_element());
}

BOOST_AUTO_TEST_CASE(max_heap)
{
  // Here attempting to reproduce examples code from cppreference.com for
  // std::push_heap() and std::pop_heap().  It turns out that calling
  // std::make_heap() on { 3, 1, 4, 1, 5, 9 } is not equivalent to inserting
  // one element at a time (I also tried to insert them in reverse order out
  // of curiosity) and yields 9 5 4 1 1 3 :/
  // cpluplus.com does make a note that the order of the elements will depend
  // on implementation.
  // Nevertheless, I thought it helps understanding how the algorithm works so
  // I decided to keep it as a unit test.
  Kokkos::Array<int, 7> a = {3, 1, 4, 1, 5, 9, 6};
  Kokkos::Array<int, 7> ref;

  pushHeap(a.data(), a.data() + 0, Less<int>());
  ref = {3, 1, 4, 1, 5, 9, 6};
  BOOST_TEST(a == ref, tt::per_element());

  pushHeap(a.data(), a.data() + 1, Less<int>());
  ref = {3, 1, 4, 1, 5, 9, 6};
  BOOST_TEST(a == ref, tt::per_element());

  pushHeap(a.data(), a.data() + 2, Less<int>());
  ref = {3, 1, 4, 1, 5, 9, 6};
  BOOST_TEST(a == ref, tt::per_element());

  pushHeap(a.data(), a.data() + 3, Less<int>());
  ref = {4, 1, 3, 1, 5, 9, 6};
  BOOST_TEST(a == ref, tt::per_element());

  pushHeap(a.data(), a.data() + 4, Less<int>());
  ref = {4, 1, 3, 1, 5, 9, 6};
  BOOST_TEST(a == ref, tt::per_element());

  pushHeap(a.data(), a.data() + 5, Less<int>());
  ref = {5, 4, 3, 1, 1, 9, 6};
  BOOST_TEST(a == ref, tt::per_element());

  pushHeap(a.data(), a.data() + 6, Less<int>());
  ref = {9, 4, 5, 1, 1, 3, 6};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 6, Less<int>());
  ref = {5, 4, 3, 1, 1, 9, 6};
  BOOST_TEST(a == ref, tt::per_element());

  pushHeap(a.data(), a.data() + 6, Less<int>());
  ref = {9, 4, 5, 1, 1, 3, 6};
  BOOST_TEST(a == ref, tt::per_element());

  pushHeap(a.data(), a.data() + 7, Less<int>());
  ref = {9, 4, 6, 1, 1, 3, 5};
  BOOST_TEST(a == ref, tt::per_element());
}

BOOST_AUTO_TEST_CASE(min_heap)
{
  // Here reproducing the example of a binary min heap from Wikipedia and then
  // popping all elements one by one.
  // This helped resolving an issue with ProprityQueue::pop() that was not
  // exposed by other unit tests in this file, partly because they were too
  // trivial.  The bug was only showing with real kNN search problems when
  // comparing results from BoundingVolumeHierarchy with boost::rtree.
  Kokkos::Array<int, 9> a = {1, 2, 3, 17, 19, 36, 7, 25, 100};
  Kokkos::Array<int, 9> ref;
  for (int i = 0; i < 9; ++i)
  {
    pushHeap(a.data(), a.data() + i, Greater<int>());
    ref = {1, 2, 3, 17, 19, 36, 7, 25, 100};
    BOOST_TEST(a == ref, tt::per_element());
  }

  popHeap(a.data(), a.data() + 9, Greater<int>());
  ref = {2, 17, 3, 25, 19, 36, 7, 100, 1};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 8, Greater<int>());
  ref = {3, 17, 7, 25, 19, 36, 100, 2, 1};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 7, Greater<int>());
  ref = {7, 17, 36, 25, 19, 100, 3, 2, 1};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 6, Greater<int>());
  ref = {17, 19, 36, 25, 100, 7, 3, 2, 1};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 5, Greater<int>());
  ref = {19, 25, 36, 100, 17, 7, 3, 2, 1};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 4, Greater<int>());
  ref = {25, 100, 36, 19, 17, 7, 3, 2, 1};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 3, Greater<int>());
  ref = {36, 100, 25, 19, 17, 7, 3, 2, 1};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 2, Greater<int>());
  ref = {100, 36, 25, 19, 17, 7, 3, 2, 1};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 1, Greater<int>());
  ref = {100, 36, 25, 19, 17, 7, 3, 2, 1};
  BOOST_TEST(a == ref, tt::per_element());

  popHeap(a.data(), a.data() + 0, Greater<int>());
  ref = {100, 36, 25, 19, 17, 7, 3, 2, 1};
  BOOST_TEST(a == ref, tt::per_element());
}

BOOST_AUTO_TEST_CASE(make_heap)
{
  for (auto v : {
           std::vector<int>{}, std::vector<int>{6}, std::vector<int>{2, 1},
           std::vector<int>{1, 6, 2, 2, 9, 4, 16},
           std::vector<int>{8, 6, 7, 2, 0}, std::vector<int>{3, 3, 3, 3, 3, 1},
           std::vector<int>{36, 19, 25, 17, 3, 7, 1, 2, 9}, // <- already a heap
       })
  {
    makeHeap(v.data(), v.data() + v.size(), Less<int>());
    BOOST_TEST(std::is_heap(v.begin(), v.end()));
  }
}

BOOST_AUTO_TEST_CASE(sort_heap)
{
  for (auto heap : {std::vector<int>{}, std::vector<int>{3},
                    std::vector<int>{36, 19, 25, 17, 3, 7, 1, 2, 9},
                    std::vector<int>{36, 19, 25, 17, 3, 9, 1, 2, 7},
                    std::vector<int>{100, 19, 36, 17, 3, 25, 1, 2, 7},
                    std::vector<int>{15, 5, 11, 3, 4, 8}})
  {
    sortHeap(heap.data(), heap.data() + heap.size(), Less<int>());
    // std::sort_heap( heap.begin(), heap.end() );
    BOOST_TEST(std::is_sorted(heap.begin(), heap.end()));
  }
}

BOOST_AUTO_TEST_CASE(is_heap)
{
  for (auto heap : {std::vector<int>{36, 19, 25, 17, 3, 7, 1, 2, 9},
                    std::vector<int>{36, 19, 25, 17, 3, 9, 1, 2, 7},
                    std::vector<int>{100, 19, 36, 17, 3, 25, 1, 2, 7},
                    std::vector<int>{15, 5, 11, 3, 4, 8}})
  {
    BOOST_TEST(isHeap(heap.data(), heap.data() + heap.size(), Less<int>()));
  }
  for (auto not_heap : {std::vector<int>{0, 1, 2, 3, 4, 3, 2, 1, 0},
                        std::vector<int>{2, 1, 0, 1, 2}})
  {
    BOOST_TEST(!isHeap(not_heap.data(), not_heap.data() + not_heap.size(),
                       Less<int>()));
  }
}

BOOST_AUTO_TEST_SUITE_END()


//
// boost-geometry-render
//
// Copyright (c) 2011
// osyo-manga : http://d.hatena.ne.jp/osyo-manga/
//
// License:
// Boost Software License - Version 1.0
// <http://www.boost.org/LICENSE_1_0.txt>
//
#include <cstdlib>
#include <gl/glut.h>
#include <gl/graphics.hpp>
#include <gl/geometry_render.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <iostream>
#include <boost/thread.hpp>

namespace bg = boost::geometry;

typedef bg::model::d2::point_xy<float> point_type;

template<typename Geometry>
struct geometry_renderer{
	geometry_renderer(Geometry& geometry) : geometry_(geometry){};
	void operator ()(){
		glColor3d(1.0f, 0.0f, 0.0f);
		gl::render(geometry_);
	}
private:
	Geometry& geometry_;
};

template<typename Geometry>
geometry_renderer<Geometry>
make_geometry_renderer(Geometry& geometry){
	return geometry_renderer<Geometry>(geometry);
}

void
idle(){
	boost::this_thread::sleep(boost::posix_time::milliseconds(16));
	glutPostRedisplay();
}

int
main(int argc, char* argv[]){
	bg::model::box<point_type> box(point_type(-0.3f, -0.3f), point_type(0.3f, 0.6f));

	gl::graphics g(argc, argv, 500, 500);

	gl::displayfunc(g, make_geometry_renderer(box));

	gl::idlefunc(g, &idle);

	g.run();
	return 0;
}