/* =========================================================================
   Copyright (c) 2010-2012, 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
============================================================================= */

#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>

//
// *** ViennaCL
//
//#define VIENNACL_DEBUG_ALL
#define VIENNACL_HAVE_UBLAS 1
#include "viennacl/scalar.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/ell_matrix.hpp"
#include "viennacl/hyb_matrix.hpp"
#include "viennacl/vector.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   << ": " << v1[i]   << " vs. " << v2_cpu[i]   << std::endl;
        //std::cout << "Neighbor: "      << i+1 << ": " << v1[i+1] << " vs. " << v2_cpu[i+1] << std::endl;
        exit(0);
      }
   }

   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;

  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_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) = 10.0; ublas_matrix(0, 1) = 0.1; ublas_matrix(0, 2) = 0.2; ublas_matrix(0, 3) = 0.3; ublas_matrix(0, 4) = 0.4;
   ublas_matrix(1,0) = 1.0; ublas_matrix(1, 1) = 1.1; ublas_matrix(1, 2) = 1.2; ublas_matrix(1, 3) = 1.3; ublas_matrix(1, 4) = 1.4;
   ublas_matrix(2,0) = 2.0; ublas_matrix(2, 1) = 2.1; ublas_matrix(2, 2) = 2.2; ublas_matrix(2, 3) = 2.3; ublas_matrix(2, 4) = 2.4;
   ublas_matrix(3,0) = 3.0; ublas_matrix(3, 1) = 3.1; ublas_matrix(3, 2) = 3.2; ublas_matrix(3, 3) = 3.3; ublas_matrix(3, 4) = 3.4;
   ublas_matrix(4,0) = 4.0; ublas_matrix(4, 1) = 4.1; ublas_matrix(4, 2) = 4.2; ublas_matrix(4, 3) = 4.3; ublas_matrix(4, 4) = 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) = 10.0; ublas_matrix(0, 1) = 0.1; ublas_matrix(0, 2) = 0.2; ublas_matrix(0, 3) = 0.3; ublas_matrix(0, 4) = 0.4;
   ublas_matrix(1,0) = 1.0; ublas_matrix(1, 1) = 1.1; ublas_matrix(1, 2) = 1.2; ublas_matrix(1, 3) = 1.3; ublas_matrix(1, 4) = 1.4;
   ublas_matrix(2,0) = 2.0; ublas_matrix(2, 1) = 2.1; ublas_matrix(2, 2) = 2.2; ublas_matrix(2, 3) = 2.3; ublas_matrix(2, 4) = 2.4;
   ublas_matrix(3,0) = 3.0; ublas_matrix(3, 1) = 3.1; ublas_matrix(3, 2) = 3.2; ublas_matrix(3, 3) = 3.3; ublas_matrix(3, 4) = 3.4;
   ublas_matrix(4,0) = 4.0; ublas_matrix(4, 1) = 4.1; ublas_matrix(4, 2) = 4.2; ublas_matrix(4, 3) = 4.3; ublas_matrix(4, 4) = 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) = 5.5; ublas_matrix(5, 6) = 5.6; ublas_matrix(5, 7) = 5.7; ublas_matrix(5, 8) = 5.8; ublas_matrix(5, 9) = 5.9;
   ublas_matrix(6,5) = 6.5; ublas_matrix(6, 6) = 6.6; ublas_matrix(6, 7) = 6.7; ublas_matrix(6, 8) = 6.8; ublas_matrix(6, 9) = 6.9;
   ublas_matrix(7,5) = 7.5; ublas_matrix(7, 6) = 7.6; ublas_matrix(7, 7) = 7.7; ublas_matrix(7, 8) = 7.8; ublas_matrix(7, 9) = 7.9;
   ublas_matrix(8,5) = 8.5; ublas_matrix(8, 6) = 8.6; ublas_matrix(8, 7) = 8.7; ublas_matrix(8, 8) = 8.8; ublas_matrix(8, 9) = 8.9;
   ublas_matrix(9,5) = 9.5; ublas_matrix(9, 6) = 9.6; ublas_matrix(9, 7) = 9.7; ublas_matrix(9, 8) = 9.8; ublas_matrix(9, 9) = 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) = 10.0; ublas_matrix(0, 1) = 0.1; ublas_matrix(0, 2) = 0.2; ublas_matrix(0, 3) = 0.3; ublas_matrix(0, 4) = 0.4;
   ublas_matrix(1,0) = 1.0; ublas_matrix(1, 1) = 1.1; ublas_matrix(1, 2) = 1.2; ublas_matrix(1, 3) = 1.3; ublas_matrix(1, 4) = 1.4;
   ublas_matrix(2,0) = 2.0; ublas_matrix(2, 1) = 2.1; ublas_matrix(2, 2) = 2.2; ublas_matrix(2, 3) = 2.3; ublas_matrix(2, 4) = 2.4;
   ublas_matrix(3,0) = 3.0; ublas_matrix(3, 1) = 3.1; ublas_matrix(3, 2) = 3.2; ublas_matrix(3, 3) = 3.3; ublas_matrix(3, 4) = 3.4;
   ublas_matrix(4,0) = 4.0; ublas_matrix(4, 1) = 4.1; ublas_matrix(4, 2) = 4.2; ublas_matrix(4, 3) = 4.3; ublas_matrix(4, 4) = 4.4;
   ublas_matrix(5,5) = 5.5; ublas_matrix(5, 6) = 5.6; ublas_matrix(5, 7) = 5.7; ublas_matrix(5, 8) = 5.8; ublas_matrix(5, 9) = 5.9;
   ublas_matrix(6,5) = 6.5; ublas_matrix(6, 6) = 6.6; ublas_matrix(6, 7) = 6.7; ublas_matrix(6, 8) = 6.8; ublas_matrix(6, 9) = 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(), 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"))
  {
    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>();
  }

  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::coordinate_matrix<NumericT> vcl_coordinate_matrix(rhs.size(), rhs.size());
  viennacl::ell_matrix<NumericT> vcl_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_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;
  }

  //
  // Triangular solvers for A \ b:
  //
  ublas::compressed_matrix<NumericT> ublas_matrix_trans = 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.

  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: 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 << "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;
  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 << "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;
  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;
  }

  // --------------------------------------------------------------------------
  // --------------------------------------------------------------------------
  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;
  }

  // --------------------------------------------------------------------------
  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_HAVE_OPENCL
  if( viennacl::ocl::current_device().double_support() )
#endif
  {
    {
      typedef double NumericT;
      NumericT epsilon = 1.0E-13;
      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_HAVE_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;
}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

// We need this to create a temporary uBLAS vector from a C-style array without copying data
#define BOOST_UBLAS_SHALLOW_ARRAY_ADAPTOR
#include "ompl/base/StateSpace.h"
#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/util/Exception.h"
#include "ompl/util/RandomNumbers.h"
#include "ompl/tools/config/MagicConstants.h"
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <functional>
#include <cmath>
#include <cstring>
#include <limits>

ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
{
    namespace nu = boost::numeric::ublas;

    RNG rng;
    Matrix projection(to, from);

    for (unsigned int j = 0 ; j < from ; ++j)
    {
        if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
            nu::column(projection, j) = nu::zero_vector<double>(to);
        else
            for (unsigned int i = 0 ; i < to ; ++i)
                projection(i, j) = rng.gaussian01();
    }

    for (unsigned int i = 0 ; i < to ; ++i)
    {
        nu::matrix_row<Matrix> row(projection, i);
        for (unsigned int j = 0 ; j < i ; ++j)
        {
            nu::matrix_row<Matrix> prevRow(projection, j);
            // subtract projection
            row -= inner_prod(row, prevRow) * prevRow;
        }
        // normalize
        row /= norm_2(row);
    }

    assert(scale.size() == from || scale.size() == 0);
    if (scale.size() == from)
    {
        unsigned int z = 0;
        for (unsigned int i = 0 ; i < from ; ++i)
        {
            if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
                z++;
            else
                nu::column(projection, i) /= scale[i];
        }
        if (z == from)
            OMPL_WARN("Computed projection matrix is all 0s");
    }
    return projection;
}

ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to)
{
    return ComputeRandom(from, to, std::vector<double>());
}

void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
{
    mat = ComputeRandom(from, to, scale);
}

void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
{
    mat = ComputeRandom(from, to);
}

void ompl::base::ProjectionMatrix::project(const double *from, EuclideanProjection& to) const
{
    namespace nu = boost::numeric::ublas;
    // create a temporary uBLAS vector from a C-style array without copying data
    nu::shallow_array_adaptor<const double> tmp1(mat.size2(), from);
    nu::vector<double, nu::shallow_array_adaptor<const double> > tmp2(mat.size2(), tmp1);
    to = prod(mat, tmp2);
}

void ompl::base::ProjectionMatrix::print(std::ostream &out) const
{
    out << mat << std::endl;
}

ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpace *space) :
    space_(space),
    bounds_(0), estimatedBounds_(0),
    defaultCellSizes_(true), cellSizesWereInferred_(false)
{
    params_.declareParam<double>("cellsize_factor", std::bind(&ProjectionEvaluator::mulCellSizes, this, std::placeholders::_1));
}

ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpacePtr &space) :
    space_(space.get()),
    bounds_(0), estimatedBounds_(0),
    defaultCellSizes_(true), cellSizesWereInferred_(false)
{
    params_.declareParam<double>("cellsize_factor", std::bind(&ProjectionEvaluator::mulCellSizes, this, std::placeholders::_1));
}

ompl::base::ProjectionEvaluator::~ProjectionEvaluator()
{
}

bool ompl::base::ProjectionEvaluator::userConfigured() const
{
    return !defaultCellSizes_ && !cellSizesWereInferred_;
}

void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
{
    defaultCellSizes_ = false;
    cellSizesWereInferred_ = false;
    cellSizes_ = cellSizes;
    checkCellSizes();
}

void ompl::base::ProjectionEvaluator::setBounds(const RealVectorBounds &bounds)
{
    bounds_ = bounds;
    checkBounds();
}

void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
{
    if (cellSizes_.size() >= dim)
        OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
    else
    {
        std::vector<double> c = cellSizes_;
        c[dim] = cellSize;
        setCellSizes(c);
    }
}

double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
{
    if (cellSizes_.size() > dim)
        return cellSizes_[dim];
    OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
    return 0.0;
}

void ompl::base::ProjectionEvaluator::mulCellSizes(double factor)
{
    if (cellSizes_.size() == getDimension())
    {
        std::vector<double> c(cellSizes_.size());
        for (std::size_t i = 0 ; i < cellSizes_.size() ; ++i)
            c[i] = cellSizes_[i] * factor;
        setCellSizes(c);
    }
}

void ompl::base::ProjectionEvaluator::checkCellSizes() const
{
    if (getDimension() <= 0)
        throw Exception("Dimension of projection needs to be larger than 0");
    if (cellSizes_.size() != getDimension())
        throw Exception("Number of dimensions in projection space does not match number of cell sizes");
}

void ompl::base::ProjectionEvaluator::checkBounds() const
{
    bounds_.check();
    if (hasBounds() && bounds_.low.size() != getDimension())
        throw Exception("Number of dimensions in projection space does not match dimension of bounds");
}

void ompl::base::ProjectionEvaluator::defaultCellSizes()
{
}

/// @cond IGNORE
namespace ompl
{
    namespace base
    {

        static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes, const EuclideanProjection &projection, ProjectionCoordinates &coord)
        {
            const std::size_t dim = cellSizes.size();
            coord.resize(dim);
            for (unsigned int i = 0 ; i < dim ; ++i)
                coord[i] = (int)floor(projection(i)/cellSizes[i]);
        }
    }
}
/// @endcond

void ompl::base::ProjectionEvaluator::inferBounds()
{
    if (estimatedBounds_.low.empty())
        estimateBounds();
    bounds_ = estimatedBounds_;
}

void ompl::base::ProjectionEvaluator::estimateBounds()
{
    unsigned int dim = getDimension();
    estimatedBounds_.resize(dim);
    if (dim > 0)
    {
        StateSamplerPtr sampler = space_->allocStateSampler();
        State *s = space_->allocState();
        EuclideanProjection proj(dim);

        estimatedBounds_.setLow(std::numeric_limits<double>::infinity());
        estimatedBounds_.setHigh(-std::numeric_limits<double>::infinity());

        for (unsigned int i = 0 ; i < magic::PROJECTION_EXTENTS_SAMPLES ; ++i)
        {
            sampler->sampleUniform(s);
            project(s, proj);
            for (unsigned int j = 0 ; j < dim ; ++j)
            {
                if (estimatedBounds_.low[j] > proj[j])
                    estimatedBounds_.low[j] = proj[j];
                if (estimatedBounds_.high[j] < proj[j])
                    estimatedBounds_.high[j] = proj[j];
            }
        }
        // make bounding box 10% larger (5% padding on each side)
        std::vector<double> diff(estimatedBounds_.getDifference());
        for (unsigned int j = 0; j < dim; ++j)
        {
            estimatedBounds_.low[j] -= magic::PROJECTION_EXPAND_FACTOR * diff[j];
            estimatedBounds_.high[j] += magic::PROJECTION_EXPAND_FACTOR * diff[j];
        }

        space_->freeState(s);
    }
}

void ompl::base::ProjectionEvaluator::inferCellSizes()
{
    cellSizesWereInferred_ = true;
    if (!hasBounds())
        inferBounds();
    unsigned int dim = getDimension();
    cellSizes_.resize(dim);
    for (unsigned int j = 0 ; j < dim ; ++j)
    {
        cellSizes_[j] = (bounds_.high[j] - bounds_.low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
        if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
        {
            cellSizes_[j] = 1.0;
            OMPL_WARN("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary value of 1 instead.",
                      j, space_->getName().c_str());
        }
    }
}

void ompl::base::ProjectionEvaluator::setup()
{
    typedef void(ProjectionEvaluator::*setCellSizesFunctionType)(unsigned int, double);
    typedef double(ProjectionEvaluator::*getCellSizesFunctionType)(unsigned int) const;

    if (defaultCellSizes_)
        defaultCellSizes();

    if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
        inferCellSizes();

    checkCellSizes();
    checkBounds();

    unsigned int dim = getDimension();
    for (unsigned int i = 0 ; i < dim ; ++i)
        params_.declareParam<double>("cellsize." + std::to_string(i),
                                     std::bind((setCellSizesFunctionType)&ProjectionEvaluator::setCellSizes, this, i, std::placeholders::_1),
                                     std::bind((getCellSizesFunctionType)&ProjectionEvaluator::getCellSizes, this, i));
}

void ompl::base::ProjectionEvaluator::computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
{
    computeCoordinatesHelper(cellSizes_, projection, coord);
}

void ompl::base::ProjectionEvaluator::printSettings(std::ostream &out) const
{
    out << "Projection of dimension " << getDimension() << std::endl;
    out << "Cell sizes";
    if (cellSizesWereInferred_)
        out << " (inferred by sampling)";
    else
    {
        if (defaultCellSizes_)
            out << " (computed defaults)";
        else
            out << " (set by user)";
    }
    out << ": [";
    for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
    {
        out << cellSizes_[i];
        if (i + 1 < cellSizes_.size())
            out << ' ';
    }
    out << ']' << std::endl;
}

void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
{
    out << projection << std::endl;
}

ompl::base::SubspaceProjectionEvaluator::SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse) :
    ProjectionEvaluator(space), index_(index), specifiedProj_(projToUse)
{
    if (!space_->isCompound())
        throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
    if (space_->as<CompoundStateSpace>()->getSubspaceCount() <= index_)
        throw Exception("State space " + space_->getName() + " does not have a subspace at index " + std::to_string(index_));
}

void ompl::base::SubspaceProjectionEvaluator::setup()
{
    if (specifiedProj_)
        proj_ = specifiedProj_;
    else
        proj_ = space_->as<CompoundStateSpace>()->getSubspace(index_)->getDefaultProjection();
    if (!proj_)
        throw Exception("No projection specified for subspace at index " + std::to_string(index_));

    cellSizes_ = proj_->getCellSizes();
    ProjectionEvaluator::setup();
}

unsigned int ompl::base::SubspaceProjectionEvaluator::getDimension() const
{
    return proj_->getDimension();
}

void ompl::base::SubspaceProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
{
    proj_->project(state->as<CompoundState>()->components[index_], projection);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_SPHERICAL_UNIFORM_INVERSE_TRANSFORM_SAMPLING_HPP
#define BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_SPHERICAL_UNIFORM_INVERSE_TRANSFORM_SAMPLING_HPP

#include <random>
#include <random>

#include <boost/geometry/algorithms/equals.hpp>

#include <boost/geometry/extensions/random/strategies/uniform_point_distribution.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace uniform_point_distribution
{

template
<
    typename Point,
    typename DomainGeometry,
    int Dim
>
struct uniform_inverse_transform_sampling
{};

template
<
    typename Point,
    typename DomainGeometry
>
struct uniform_inverse_transform_sampling<Point, DomainGeometry, 2>
{
    uniform_inverse_transform_sampling(DomainGeometry const& g) {}
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                uniform_inverse_transform_sampling const& r_strategy) const
    {
        return boost::geometry::equals(l_domain.domain(), r_domain.domain());
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        Point out;
        typedef typename coordinate_type<Point>::type coordinate_type;
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type computation_type;
        std::uniform_real_distribution<computation_type> lon_dist(
            get_as_radian<0, 0>(d),
            get_as_radian<1, 0>(d));
        set_from_radian<0>(out, lon_dist(g));

        coordinate_type lat1 = get_as_radian<0, 1>(d);
        coordinate_type lat2 = get_as_radian<1, 1>(d);
        coordinate_type x1   = (1.0 - std::cos(lat1)) / 2,
                        x2   = (1.0 - std::cos(lat2)) / 2;
        std::uniform_real_distribution<computation_type> x_dist(
            std::min(x1, x2),
            std::max(x1, x2));
        coordinate_type x = x_dist(g);
        set_from_radian<1>(out, std::acos(1.0 - 2.0 * x));
        return out;
    }
    void reset(DomainGeometry const&) {};
};

template
<
    typename Point,
    typename DomainGeometry
>
struct uniform_inverse_transform_sampling<Point, DomainGeometry, 3>
{
    uniform_inverse_transform_sampling(DomainGeometry const& g) {}
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                uniform_inverse_transform_sampling const& r_strategy) const
    {
        return boost::geometry::equals(l_domain.domain(), r_domain.domain());
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        uniform_inverse_transform_sampling<Point, DomainGeometry, 2> helper(d);
        Point out = helper.apply(g, d);
        typedef typename coordinate_type<Point>::type coordinate_type;
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type computation_type;
        coordinate_type r1 = get<0, 2>(d);
        coordinate_type r2 = get<1, 2>(d);
        std::uniform_real_distribution<computation_type>
            r_dist( r1 * r1 * r1 , r2 * r2 * r2 );
        set<2>(out, std::cbrt(r_dist(g)));
        return out;
    }
    void reset(DomainGeometry const&) {};
};

namespace services {

template
<
    typename Point,
    typename DomainGeometry
>
struct default_strategy
<
    Point,
    DomainGeometry,
    box_tag,
    single_tag, //There are no MultiBoxes right now
    2,
    spherical_tag
> : public uniform_inverse_transform_sampling<Point, DomainGeometry, 2> {
    typedef uniform_inverse_transform_sampling<Point, DomainGeometry, 2> base;
    using base::base;
};

template
<
    typename Point,
    typename DomainGeometry
>
struct default_strategy
<
    Point,
    DomainGeometry,
    box_tag,
    single_tag, //There are no MultiBoxes right now
    3,
    spherical_tag
> : public uniform_inverse_transform_sampling<Point, DomainGeometry, 3> {
    typedef uniform_inverse_transform_sampling<Point, DomainGeometry, 3> base;
    using base::base;
};

} // namespace services

}} // namespace strategy::uniform_point_distribution

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_SPHERICAL_UNIFORM_INVERSE_TRANSFORM_SAMPLING_HPP


/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */

#include <string.h>
#include "compute_smd_tlsph_stress.h"
#include "atom.h"
#include "update.h"
#include "modify.h"
#include "comm.h"
#include "force.h"
#include "memory.h"
#include "error.h"
#include "pair.h"
#include <Eigen/Eigen>
using namespace Eigen;
using namespace LAMMPS_NS;

/*
 * deviator of a tensor
 */
static Matrix3d Deviator(Matrix3d M) {
	Matrix3d eye;
	eye.setIdentity();
	eye *= M.trace() / 3.0;
	return M - eye;
}

/* ---------------------------------------------------------------------- */

ComputeSMDTLSPHStress::ComputeSMDTLSPHStress(LAMMPS *lmp, int narg, char **arg) :
		Compute(lmp, narg, arg) {
	if (narg != 3)
		error->all(FLERR, "Illegal compute smd/tlsph_stress command");

	peratom_flag = 1;
	size_peratom_cols = 7;

	nmax = 0;
	stress_array = NULL;
}

/* ---------------------------------------------------------------------- */

ComputeSMDTLSPHStress::~ComputeSMDTLSPHStress() {
	memory->sfree(stress_array);
}

/* ---------------------------------------------------------------------- */

void ComputeSMDTLSPHStress::init() {

	int count = 0;
	for (int i = 0; i < modify->ncompute; i++)
		if (strcmp(modify->compute[i]->style, "smd/tlsph_stress") == 0)
			count++;
	if (count > 1 && comm->me == 0)
		error->warning(FLERR, "More than one compute smd/tlsph_stress");
}

/* ---------------------------------------------------------------------- */

void ComputeSMDTLSPHStress::compute_peratom() {
	invoked_peratom = update->ntimestep;
	Matrix3d stress_deviator;
	double von_mises_stress;

	// grow vector array if necessary

	if (atom->nmax > nmax) {
		memory->destroy(stress_array);
		nmax = atom->nmax;
		memory->create(stress_array, nmax, size_peratom_cols, "stresstensorVector");
		array_atom = stress_array;
	}

	int itmp = 0;
	Matrix3d *T = (Matrix3d *) force->pair->extract("smd/tlsph/stressTensor_ptr", itmp);
	if (T == NULL) {
		error->all(FLERR, "compute smd/tlsph_stress could not access stress tensors. Are the matching pair styles present?");
	}
	int nlocal = atom->nlocal;
	int *mask = atom->mask;

	for (int i = 0; i < nlocal; i++) {
		if (mask[i] & groupbit) {
			stress_deviator = Deviator(T[i]);
			von_mises_stress = sqrt(3. / 2.) * stress_deviator.norm();
			stress_array[i][0] = T[i](0, 0); // xx
			stress_array[i][1] = T[i](1, 1); // yy
			stress_array[i][2] = T[i](2, 2); // zz
			stress_array[i][3] = T[i](0, 1); // xy
			stress_array[i][4] = T[i](0, 2); // xz
			stress_array[i][5] = T[i](1, 2); // yz
			stress_array[i][6] = von_mises_stress;
		} else {
			for (int j = 0; j < size_peratom_cols; j++) {
				stress_array[i][j] = 0.0;
			}
		}
	}
}

/* ----------------------------------------------------------------------
 memory usage of local atom-based array
 ------------------------------------------------------------------------- */

double ComputeSMDTLSPHStress::memory_usage() {
	double bytes = size_peratom_cols * nmax * sizeof(double);
	return bytes;
}


#include "mex.h"
#include <Eigen/Dense>
#include "../radialpose.h"
#include "../misc/ransac_estimator.h"
#include <RansacLib/ransac.h>

using namespace radialpose;

void print_usage() {
	mexPrintf("[R,t,f,params] = ransac_radialpose_mex(x,X,solver,tol,[min_iter],[max_iter]);\n");
	mexPrintf(" Solvers:\n");
	mexPrintf("  1 - D(1,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  2 - D(2,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  3 - D(3,0) - 5p -- Larsson et al.  ICCV 2019  (Minimal)\n");
	mexPrintf("  4 - D(3,3) - 8p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  5 - U(1,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  6 - U(0,1) - 4p -- Larsson et al.  ICCV 2017  (Minimal, Non-planar)\n");
	mexPrintf("  7 - U(0,1) - 4p -- Bujnak et al.   ACCV 2010  (Minimal, Non-planar)\n");
	mexPrintf("  8 - U(0,1) - 5p -- Kukelova et al. ICCV 2013\n");
	mexPrintf("  9 - U(0,2) - 5p -- Kukelova et al. ICCV 2013\n");
	mexPrintf(" 10 - U(0,3) - 5p -- Kukelova et al. ICCV 2013  (Minimal)\n");
	mexPrintf(" 11 - U(0,1) - 4p -- Oskarsson       arxiv 2018 (Minimal, Planar)\n");
	mexPrintf(" 12 - N/A    - 5p -- Kukelova et al. ICCV 2013  (Minimal, 1D Radial)\n\n");
}

void save_pose(int nlhs, mxArray *plhs[], Camera pose) {
	int n_sols = 1;
	int n_params = 0;
	if (n_sols > 0)
		n_params = pose.dist_params.size();

	if (nlhs >= 1) {
		plhs[0] = mxCreateDoubleMatrix(3, 3, mxREAL);
		double *p = mxGetPr(plhs[0]);
		for (int j = 0; j < 9; ++j)
			p[j] = pose.R(j);
	}
	if (nlhs >= 2) {
		plhs[1] = mxCreateDoubleMatrix(3, 1, mxREAL);
		double *p = mxGetPr(plhs[1]);
		for (int j = 0; j < 3; ++j)
			p[j] = pose.t(j);
	}
	if (nlhs >= 3) {
		plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
		double *p = mxGetPr(plhs[2]);
		*p = pose.focal;

	}
	if (nlhs >= 4) {
		plhs[3] = mxCreateDoubleMatrix(n_params, 1, mxREAL);
		double *p = mxGetPr(plhs[3]);
		for (int j = 0; j < n_params; ++j)
			p[j] = pose.dist_params[j];
	}
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	if (nrhs < 4 || nrhs > 7) {
		print_usage();
		mexErrMsgTxt("Incorrect number of input arguments.");
	}
	if (nlhs > 4) {
		print_usage();
		mexErrMsgTxt("Wrong number of output arguments.");
	}

	if (mxGetM(prhs[0]) != 2) {
		print_usage();
		mexErrMsgTxt("First input must be 2 x N matrix.");
	}
	if (mxGetM(prhs[1]) != 3) {
		print_usage();
		mexErrMsgTxt("Second input must be 3 x N matrix.");
	}
	if (mxGetN(prhs[0]) != mxGetN(prhs[1])) {
		print_usage();
		mexErrMsgTxt("Not the same number of 2D points and 3D points.");
	}

	double tol = 5.0;
	if (nrhs >= 4) {
		tol = mxGetScalar(prhs[3]);
	}

	ransac_lib::LORansacOptions options;
	options.squared_inlier_threshold_ = tol * tol;

	options.final_least_squares_ = true;

	if (nrhs >= 5)
		options.min_num_iterations_ = static_cast<int>(mxGetScalar(prhs[4]));
	if (nrhs >= 6)
		options.max_num_iterations_ = static_cast<int>(mxGetScalar(prhs[5]));

	double damp_factor = 0.0;
	if (nrhs >= 7) {
		damp_factor = mxGetScalar(prhs[6]);
	}

	Eigen::Matrix<double, 2, Eigen::Dynamic> x = Eigen::Map<Eigen::Matrix<double, 2, Eigen::Dynamic>>(mxGetPr(prhs[0]), 2, mxGetN(prhs[0]));
	Eigen::Matrix<double, 3, Eigen::Dynamic> X = Eigen::Map<Eigen::Matrix<double, 3, Eigen::Dynamic>>(mxGetPr(prhs[1]), 3, mxGetN(prhs[1]));
	int solver_idx = static_cast<int>(mxGetScalar(prhs[2]));

	ransac_lib::RansacStatistics ransac_stats;
	int inliers = 0;
	Camera best_model;
	best_model.R = Eigen::Matrix3d::Identity();
	best_model.t = Eigen::Vector3d::Zero();

	if (solver_idx == 1) { // D(1,0)
		radialpose::larsson_iccv19::Solver<1, 0, true> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<1, 0, true>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<1, 0, true>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 2) { // D(2,0)
		radialpose::larsson_iccv19::Solver<2, 0, true> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<2, 0, true>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<2, 0, true>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 3) { // D(3,0)
		radialpose::larsson_iccv19::Solver<3, 0, true> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<3, 0, true>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<3, 0, true>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 4) { // D(3,3)
		radialpose::larsson_iccv19::Solver<3, 3, true> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<3, 3, true>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<3, 3, true>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 5) { // U(1,0)
		radialpose::larsson_iccv19::Solver<1, 0, false> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<1, 0, false>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<1, 0, false>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 6) { // U(0,1)
		radialpose::larsson_iccv17::NonPlanarSolver estimator;
		radialpose::RansacEstimator<larsson_iccv17::NonPlanarSolver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv17::NonPlanarSolver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 7) {  // U(0,1)
		radialpose::bujnak_accv10::NonPlanarSolver estimator;
		radialpose::RansacEstimator<bujnak_accv10::NonPlanarSolver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<bujnak_accv10::NonPlanarSolver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 8) {  // U(0,1)
		radialpose::kukelova_iccv13::Solver estimator(1);
		radialpose::RansacEstimator<kukelova_iccv13::Solver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<kukelova_iccv13::Solver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 9) {  // U(0,2)
		radialpose::kukelova_iccv13::Solver estimator(2);
		radialpose::RansacEstimator<kukelova_iccv13::Solver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<kukelova_iccv13::Solver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 10) { // U(0,3)
		radialpose::kukelova_iccv13::Solver estimator(3);
		radialpose::RansacEstimator<kukelova_iccv13::Solver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<kukelova_iccv13::Solver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 11) {  // U(0,1)
		radialpose::oskarsson_arxiv18::PlanarSolver estimator;
		radialpose::RansacEstimator<oskarsson_arxiv18::PlanarSolver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<oskarsson_arxiv18::PlanarSolver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 12) {  // N/A
		radialpose::kukelova_iccv13::Radial1DSolver estimator;
		radialpose::RansacEstimator<kukelova_iccv13::Radial1DSolver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<kukelova_iccv13::Radial1DSolver>> lomsac;
		solver.use_local_opt = false;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else {
		print_usage();
		mexErrMsgTxt("Solver NYI.\n");
	}

	save_pose(nlhs, plhs, best_model);

}


//  Copyright John Maddock 2007.
//  Use, modification and distribution are subject to 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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_ref_snip8

#include <boost/math/distributions/negative_binomial.hpp>

using namespace boost::math;
using namespace boost::math::policies;

typedef negative_binomial_distribution<
      double,
      policy<discrete_quantile<integer_round_nearest> >
   > dist_type;

// Lower quantile rounded up:
double x = quantile(dist_type(20, 0.3), 0.05);
// Upper quantile rounded down:
double y = quantile(complement(dist_type(20, 0.3), 0.05));

//]

#include <iostream>

int main()
{
   std::cout << x << " " << y << std::endl;
}


#include "rand_state.hpp"
#include "pauli_product.cpp"
#include <armadillo>
#include <cmath>
#include <stdlib.h>

using namespace std;
using namespace arma;

void apply_rand_gate(unsigned int q1, unsigned int q2, cx_dvec &psi)
{
  int x1, x2, z1, z2;
  x1 = rand()%2;
  x2 = rand()%2;
  z1 = rand()%2;
  z2 = rand()%2;

  int x, z;
  x = (x1<<q1) + (x2<<q2);
  z = (z1<<q1) + (z2<<q2);

  double theta = (double)rand() / (double)RAND_MAX * 3.14159265358979;

  apply_ppr(x, z, theta, psi);
}

cx_dvec rand_haar(unsigned int n_q)
{
  cx_vec psi;
  psi.randn(1<<n_q);
  psi = normalise(psi, 2);
  return psi;
}

cx_dvec scrambled_1d(unsigned int n_q, unsigned int depth)
{
  cx_vec psi;
  psi.zeros(1<<n_q);
  psi(0) = 1;
  for (int d=0; d<depth; d++)
    {
      if (d%2==0)
	{
	  for (int i=0; i<n_q; i++)
	    {
	      if (i%2==0)
		{
		  if (i<n_q-1)
		      apply_rand_gate(i, i+1, psi);
		  //		  else
		  //		      apply_rand_gate(i,0, psi);
		}
	    }
	}
      else
	{
	  for (int i=0; i<n_q; i++)
	    {
	      if (i%2==1)
		{
		  if (i<n_q-1)
		      apply_rand_gate(i, i+1, psi);
		  //		  else
		  //		      apply_rand_gate(i,0, psi);
		}
	    }
	}
    }
  return psi;
}


/*
 * Copyright (c) 2021. Kun Huang.
 * This Source Code Form is subject to the terms of the Apache License, v. 2.0.
 */

//
// Created by huangkun on 2021/7/9.
//

#ifndef OPENGV2_UTILITY_HPP
#define OPENGV2_UTILITY_HPP

#include <functional>
#include <numeric>
#include <unordered_map>

#include <Eigen/Eigen>
#include <Eigen/StdVector>
#include <nanoflann.hpp>
//#include <cereal/cereal.hpp>

namespace opengv2 {
    template<typename EigenMatrixType>
    using vectorofEigenMatrix = std::vector<EigenMatrixType, Eigen::aligned_allocator<EigenMatrixType>>;

    template<typename T>
    struct EigenMatrixCompare {
        inline bool operator()(const T &lhs, const T &rhs) const {
            return lhs.norm() < rhs.norm();
        }
    };

    /**
     * @brief Hash function for Eigen matrix and vector.
     * The code is from `hash_combine` function of the Boost library. See
     * http://www.boost.org/doc/libs/1_55_0/doc/html/hash/reference.html#boost.hash_combine .
     */
    template<typename T>
    struct EigenMatrixHash : std::unary_function<T, size_t> {
        std::size_t operator()(T const &matrix) const {
            // Note that it is oblivious to the storage order of Eigen matrix (column- or
            // row-major). It will give you the same hash value for two different matrices if they
            // are the transpose of each other in different storage order.
            size_t seed = 0;
            for (size_t i = 0; i < matrix.size(); ++i) {
                auto elem = *(matrix.data() + i);
                seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
            }
            return seed;
        }
    };

    /**
     * @brief Generic functor base for use with the Eigen-nonlinear optimization
     * toolbox. Please refer to the Eigen-documentation for further information.
     */
    template<typename Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
    struct EigenOptimizationFunctor {
        enum {
            InputsAtCompileTime = NX,
            ValuesAtCompileTime = NY
        };
        typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
        typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
        typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

        const int m_inputs, m_values;

        EigenOptimizationFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}

        EigenOptimizationFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

        int inputs() const { return m_inputs; }

        int values() const { return m_values; }

        // you should define that in the subclass :
        // void operator() (const InputType& x, ValueType* v, JacobianType* _j=0) const;
    };

    struct pair_hash {
        template<class T1, class T2>
        std::size_t operator()(const std::pair<T1, T2> &pair) const {
            return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
        }
    };

    /**
     * @brief Fit normal distribution.
     * @tparam T
     * @param v
     * @param mean
     * @param stdev
     */
    template<class T>
    inline void fitNormal(const std::vector<T> &v, T &mean, T &stdev) {
        double sum = std::accumulate(v.begin(), v.end(), 0.0);
        mean = sum / v.size();

        std::vector<T> diff(v.size());
        std::transform(v.begin(), v.end(), diff.begin(), [mean](T x) { return x - mean; });
        double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
        stdev = std::sqrt(sq_sum / (v.size() - 1));
    }
}

namespace nanoflann {
    template<typename T>
    struct SO3DataSetAdaptor {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        explicit SO3DataSetAdaptor(const opengv2::vectorofEigenMatrix<Eigen::Quaternion<T>> &Q_set) : QSet(Q_set) {}

        const opengv2::vectorofEigenMatrix<Eigen::Quaternion<T>> &QSet;

        // Must return the number of data points
        inline size_t kdtree_get_point_count() const { return QSet.size(); }

        // Returns the dim'th component of the idx'th point in the class:
        // Since this is inlined and the "dim" argument is typically an immediate value, the
        //  "if/else's" are actually solved at compile time.
        inline T kdtree_get_pt(const size_t idx, const size_t dim) const {
            return QSet[idx].coeffs()[dim];
        }

        // Optional bounding-box computation: return false to default to a standard bbox computation loop.
        //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
        //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
        template<class BBOX>
        bool kdtree_get_bbox(BBOX & /* bb */) const { return false; }
    };

    template<typename num_t>
    using SO3_KDTree = KDTreeSingleIndexAdaptor<SO3_Adaptor<num_t, SO3DataSetAdaptor<num_t>>, SO3DataSetAdaptor<num_t>, 4>;
}

/*namespace cereal {
    template<class Archive, class Derived>
    inline
    typename std::enable_if<
            traits::is_output_serializable < BinaryData < typename Derived::Scalar>, Archive>::value, void>

    ::type
    save(Archive &ar, Eigen::PlainObjectBase<Derived> const &m) {
        typedef Eigen::PlainObjectBase<Derived> ArrT;
        if (ArrT::RowsAtCompileTime == Eigen::Dynamic) ar(m.rows());
        if (ArrT::ColsAtCompileTime == Eigen::Dynamic) ar(m.cols());
        ar(binary_data(m.data(), m.size() * sizeof(typename Derived::Scalar)));
    }

    template<class Archive, class Derived>
    inline
    typename std::enable_if<
            traits::is_input_serializable < BinaryData < typename Derived::Scalar>, Archive>::value, void>

    ::type
    load(Archive &ar, Eigen::PlainObjectBase<Derived> &m) {
        typedef Eigen::PlainObjectBase<Derived> ArrT;
        Eigen::Index rows = ArrT::RowsAtCompileTime, cols = ArrT::ColsAtCompileTime;
        if (rows == Eigen::Dynamic) ar(rows);
        if (cols == Eigen::Dynamic) ar(cols);
        m.resize(rows, cols);
        ar(binary_data(m.data(), static_cast<std::size_t>(rows * cols * sizeof(typename Derived::Scalar))));
    }

    template<class Archive, class Scalar, int Options>
    inline void serialize(Archive &ar, ::Eigen::Quaternion<Scalar, Options> &quat) {
        ar(make_nvp("w", quat.w()), make_nvp("x", quat.x()), make_nvp("y", quat.y()), make_nvp("z", quat.z()));
    }
}*/

#endif //OPENGV2_UTILITY_HPP


#ifndef SS_MATRIX_HPP
#define SS_MATRIX_HPP

#include <Eigen/Dense>
#include <ss/data/data.hpp>

/*
 * This file defines convenient functions for manipulating an Eigen matrix.
 */

namespace SS
{
    typedef Eigen::MatrixXd Matrix;
    typedef Eigen::VectorXd Vector;

    namespace MatrixUtils
    {
        // Apply a functor to every element in the matrix
        template <typename F> void each(const Matrix &m, F f)
        {
            for (auto i = 0; i < m.rows(); i++)
            {
                for (auto j = 0; j < m.cols(); j++)
                {
                    f(i, j, m(i, j));
                }
            }
        }

        template <typename Iter> static Vector vector(const Iter &x)
        {
            Vector X(x.size());

            auto i = 0;

            for (const auto &t : x)
            {
                X(i++) = t;
            }

            return X;
        }

        template <typename T> static Matrix matrix(const T *t, Counts n)
        {
            Matrix X(n, 1);

            for (auto i = 0; i < n; i++, t++)
            {
                X(i, 0) = *t;
            }

            return X;
        }

        template <typename Iter> static Matrix matrix(const Iter &x)
        {
            Matrix X(x.size(), 1);

            auto i = 0;

            for (const auto &t : x)
            {
                X(i++, 0) = t;
            }

            return X;
        }

        struct MatrixCount
        {
            // Row sums
            Eigen::VectorXd rsums;

            // Column sums
            Eigen::VectorXd csums;

            // Overall sums
            Real sums = 0;
        };

        inline MatrixCount count(const Matrix &m)
        {
            MatrixCount c;

            c.rsums = Eigen::VectorXd(m.rows());
            c.csums = Eigen::VectorXd(m.cols());

            c.rsums.setConstant(0);
            c.csums.setConstant(0);

            MatrixUtils::each(m, [&](Index i, Index j, Real x)
            {
                c.sums += x;
                c.rsums(i) += x;
                c.csums(j) += x;
            });

            return c;
        }
    }
}

#endif


/* boost random/additive_combine.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: additive_combine.hpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_ADDITIVE_COMBINE_HPP
#define BOOST_RANDOM_ADDITIVE_COMBINE_HPP

#include <istream>
#include <iosfwd>
#include <algorithm> // for std::min and std::max
#include <boost/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/detail/seed.hpp>
#include <boost/random/linear_congruential.hpp>

namespace boost {
namespace random {

/**
 * An instantiation of class template @c additive_combine_engine models a
 * \pseudo_random_number_generator. It combines two multiplicative
 * \linear_congruential_engine number generators, i.e. those with @c c = 0.
 * It is described in
 *
 *  @blockquote
 *  "Efficient and Portable Combined Random Number Generators", Pierre L'Ecuyer,
 *  Communications of the ACM, Vol. 31, No. 6, June 1988, pp. 742-749, 774
 *  @endblockquote
 *
 * The template parameters MLCG1 and MLCG2 shall denote two different
 * \linear_congruential_engine number generators, each with c = 0. Each
 * invocation returns a random number
 * X(n) := (MLCG1(n) - MLCG2(n)) mod (m1 - 1),
 * where m1 denotes the modulus of MLCG1.
 */
template<class MLCG1, class MLCG2>
class additive_combine_engine
{
public:
    typedef MLCG1 first_base;
    typedef MLCG2 second_base;
    typedef typename MLCG1::result_type result_type;

    // Required by old Boost.Random concept
    BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
    /**
     * Returns the smallest value that the generator can produce
     */
    static result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return 1; }
    /**
     * Returns the largest value that the generator can produce
     */
    static result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
    { return MLCG1::modulus-1; }

    /**
     * Constructs an @c additive_combine_engine using the
     * default constructors of the two base generators.
     */
    additive_combine_engine() : _mlcg1(), _mlcg2() { }
    /**
     * Constructs an @c additive_combine_engine, using seed as
     * the constructor argument for both base generators.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(additive_combine_engine,
        result_type, seed_arg)
    {
        _mlcg1.seed(seed_arg);
        _mlcg2.seed(seed_arg);
    }
    /**
     * Constructs an @c additive_combine_engine, using seq as
     * the constructor argument for both base generators.
     *
     * @xmlwarning
     * The semantics of this function are liable to change.
     * A @c seed_seq is designed to generate all the seeds
     * in one shot, but this seeds the two base engines
     * independantly and probably ends up giving the same
     * sequence to both.
     * @endxmlwarning
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(additive_combine_engine,
        SeedSeq, seq)
    {
        _mlcg1.seed(seq);
        _mlcg2.seed(seq);
    }
    /**
     * Constructs an @c additive_combine_engine, using
     * @c seed1 and @c seed2 as the constructor argument to
     * the first and second base generators, respectively.
     */
    additive_combine_engine(typename MLCG1::result_type seed1,
                            typename MLCG2::result_type seed2)
      : _mlcg1(seed1), _mlcg2(seed2) { }
    /**
     * Contructs an @c additive_combine_engine with
     * values from the range defined by the input iterators first
     * and last.  first will be modified to point to the element
     * after the last one used.
     *
     * Throws: @c std::invalid_argument if the input range is too small.
     *
     * Exception Safety: Basic
     */
    template<class It> additive_combine_engine(It& first, It last)
      : _mlcg1(first, last), _mlcg2(first, last) { }

    /**
     * Seeds an @c additive_combine_engine using the default
     * seeds of the two base generators.
     */
    void seed()
    {
        _mlcg1.seed();
        _mlcg2.seed();
    }

    /**
     * Seeds an @c additive_combine_engine, using @c seed as the
     * seed for both base generators.
     */
    BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(additive_combine_engine,
        result_type, seed_arg)
    {
        _mlcg1.seed(seed_arg);
        _mlcg2.seed(seed_arg);
    }

    /**
     * Seeds an @c additive_combine_engine, using @c seq to
     * seed both base generators.
     *
     * See the warning on the corresponding constructor.
     */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(additive_combine_engine,
        SeedSeq, seq)
    {
        _mlcg1.seed(seq);
        _mlcg2.seed(seq);
    }

    /**
     * Seeds an @c additive_combine generator, using @c seed1 and @c seed2 as
     * the seeds to the first and second base generators, respectively.
     */
    void seed(typename MLCG1::result_type seed1,
              typename MLCG2::result_type seed2)
    {
        _mlcg1.seed(seed1);
        _mlcg2.seed(seed2);
    }

    /**
     * Seeds an @c additive_combine_engine with
     * values from the range defined by the input iterators first
     * and last.  first will be modified to point to the element
     * after the last one used.
     *
     * Throws: @c std::invalid_argument if the input range is too small.
     *
     * Exception Safety: Basic
     */
    template<class It> void seed(It& first, It last)
    {
        _mlcg1.seed(first, last);
        _mlcg2.seed(first, last);
    }

    /** Returns the next value of the generator. */
    result_type operator()() {
        result_type val1 = _mlcg1();
        result_type val2 = _mlcg2();
        if(val2 < val1) return val1 - val2;
        else return val1 - val2 + MLCG1::modulus - 1;
    }

    /** Fills a range with random values */
    template<class Iter>
    void generate(Iter first, Iter last)
    { detail::generate_from_int(*this, first, last); }

    /** Advances the state of the generator by @c z. */
    void discard(boost::uintmax_t z)
    {
        _mlcg1.discard(z);
        _mlcg2.discard(z);
    }

    /**
     * Writes the state of an @c additive_combine_engine to a @c
     * std::ostream.  The textual representation of an @c
     * additive_combine_engine is the textual representation of
     * the first base generator followed by the textual representation
     * of the second base generator.
     */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, additive_combine_engine, r)
    { os << r._mlcg1 << ' ' << r._mlcg2; return os; }

    /**
     * Reads the state of an @c additive_combine_engine from a
     * @c std::istream.
     */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, additive_combine_engine, r)
    { is >> r._mlcg1 >> std::ws >> r._mlcg2; return is; }

    /**
     * Returns: true iff the two @c additive_combine_engines will
     * produce the same sequence of values.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(additive_combine_engine, x, y)
    { return x._mlcg1 == y._mlcg1 && x._mlcg2 == y._mlcg2; }
    /**
     * Returns: true iff the two @c additive_combine_engines will
     * produce different sequences of values.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(additive_combine_engine)

private:
    MLCG1 _mlcg1;
    MLCG2 _mlcg2;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
template<class MLCG1, class MLCG2>
const bool additive_combine_engine<MLCG1, MLCG2>::has_fixed_range;
#endif

/// \cond show_deprecated

/** Provided for backwards compatibility. */
template<class MLCG1, class MLCG2, typename MLCG1::result_type val = 0>
class additive_combine : public additive_combine_engine<MLCG1, MLCG2>
{
    typedef additive_combine_engine<MLCG1, MLCG2> base_t;
public:
    typedef typename base_t::result_type result_type;
    additive_combine() {}
    template<class T>
    additive_combine(T& arg) : base_t(arg) {}
    template<class T>
    additive_combine(const T& arg) : base_t(arg) {}
    template<class It>
    additive_combine(It& first, It last) : base_t(first, last) {}
};

/// \endcond

/**
 * The specialization \ecuyer1988 was suggested in
 *
 *  @blockquote
 *  "Efficient and Portable Combined Random Number Generators", Pierre L'Ecuyer,
 *  Communications of the ACM, Vol. 31, No. 6, June 1988, pp. 742-749, 774
 *  @endblockquote
 */
typedef additive_combine_engine<
    linear_congruential_engine<uint32_t, 40014, 0, 2147483563>,
    linear_congruential_engine<uint32_t, 40692, 0, 2147483399>
> ecuyer1988;

} // namespace random

using random::ecuyer1988;

} // namespace boost

#endif // BOOST_RANDOM_ADDITIVE_COMBINE_HPP


/*
 * Common.hpp
 *
 *  Created on: Feb 9, 2014
 *      Author: Bloeschm
 */

#ifndef LWF_COMMON_HPP_
#define LWF_COMMON_HPP_

#include <map>
#include <type_traits>
#include <tuple>
#include <Eigen/Dense>
#include <iostream>
#include "kindr/Core"
#include "lightweight_filtering/PropertyHandler.hpp"

typedef kindr::RotationQuaternionPD QPD;
typedef kindr::RotationMatrixPD MPD;
typedef Eigen::Vector3d V3D;
typedef Eigen::Matrix3d M3D;
typedef Eigen::VectorXd VXD;
typedef Eigen::MatrixXd MXD;
inline M3D gSM(const V3D& vec){
  return kindr::getSkewMatrixFromVector(vec);
}

static void enforceSymmetry(MXD& mat){
  mat = 0.5*(mat+mat.transpose()).eval();
}

inline M3D Lmat (const V3D& a) {
  return kindr::getJacobianOfExponentialMap(a);
}

namespace LWF{
  enum FilteringMode{
    ModeEKF,
    ModeUKF,
    ModeIEKF
  };
}

#endif /* LWF_COMMON_HPP_ */


// Header
#include "Translate.hpp"

// Boost
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

namespace QuadProgMm {
  /* From QuadProg++:

  The problem is in the expression:

  min 0.5 * x G x + g0 x
  s.t.
      CE^T x + ce0 = 0
      CI^T x + ci0 >= 0

   The matrix and vectors dimensions are as follows:
       G: n * n
      g0: n

      CE: n * p
     ce0: p

      CI: n * m
     ci0: m

       x: n
  */
  Translation::Translation(const std::set<Variable>& variables_, int p, int m) :
    Translation(
      std::vector<Variable>(variables_.begin(), variables_.end()),
      variables_.size(),
      p,
      m
    )
  {}

  Translation::Translation(const std::vector<Variable>& variables_, int n, int p, int m) :
    variables(variables_),
    G(n, n), g0(n), g00(0),
    CE(n, p), ce0(p),
    CI(n, m), ci0(m)
  {}

  void translate(
    const std::vector<Variable>& variables,
    const QuadraticExpression& q,
    quadprogpp::Matrix<double>& G,
    quadprogpp::Vector<double>& g0,
    double& g00
  ) {
    int index1 = 0;
    foreach(Variable v1, variables) {
      int index2 = 0;
      foreach(Variable v2, variables) {
        const double coeff = q.getQuadraticCoefficient(v1, v2);
        if (index1 == index2) {
          G[index1][index1] = 2 * coeff;
        } else {
          G[index1][index2] = coeff;
          G[index2][index1] = coeff;
        }
        ++index2;
      }
      ++index1;
    }

    int index = 0;
    foreach(Variable v, variables) {
      g0[index] = q.getLinearCoefficient(v);
      ++index;
    }

    g00 = q.getConstantCoefficient();
  }

  void translate(
    const std::vector<Variable>& variables,
    const LinearExpression& l,
    quadprogpp::Matrix<double>& CIE,
    quadprogpp::Vector<double>& cie0,
    int indexIE
  ) {
    int index = 0;
    foreach(Variable v, variables) {
      CIE[index][indexIE] = l.getLinearCoefficient(v);
      ++index;
    }

    cie0[indexIE] = l.getConstantCoefficient();
  }

  Translation translate(const QuadraticExpression& q, const std::vector<Constraint>& constraints) {
    int p = 0;
    int m = 0;
    std::set<Variable> variables = q.getVariables();
    foreach(Constraint c, constraints) {
      switch(c.getType()) {
        case Constraint::ZERO:
          ++p;
          break;
        case Constraint::POSITIVE:
          ++m;
          break;
      }
      const std::set<Variable> constraintVariables = c.getLinearExpression().getVariables();
      variables.insert(constraintVariables.begin(), constraintVariables.end());
    }

    Translation t(variables, p, m);

    translate(t.variables, q, t.G, t.g0, t.g00);

    int indexE = 0;
    int indexI = 0;
    foreach(Constraint c, constraints) {
      const LinearExpression& l = c.getLinearExpression();

      switch(c.getType()) {
        case Constraint::ZERO:
          translate(t.variables, l, t.CE, t.ce0, indexE);
          ++indexE;
          break;
        case Constraint::POSITIVE:
          translate(t.variables, l, t.CI, t.ci0, indexI);
          ++indexI;
          break;
      }
    }

    return t;
  }
}


/**
 *  This test ensures that the sketch application (for CombBLAS matrices) is
 *  done correctly (on-the-fly matrix multiplication in the code is compared
 *  to true matrix multiplication).
 *  This test builds on the following assumptions:
 *
 *      - CombBLAS PSpGEMM returns the correct result, and
 *      - the random numbers in row_idx and row_value (see
 *        hash_transform_data_t) are drawn from the promised distributions.
 */

#include <vector>

#include <boost/mpi.hpp>
#include <boost/test/minimal.hpp>

//FIXME: ugly, fix header problem!
#define SKYLARK_SKETCH_HPP 1

#define SKYLARK_NO_ANY
#include "El.hpp"
#include "../../base/sparse_matrix.hpp"

#include "../../base/context.hpp"
#include "../../utility/distributions.hpp"
#include "../../sketch/hash_transform.hpp"

typedef FullyDistVec<size_t, double> mpi_vector_t;
typedef SpDCCols<size_t, double> col_t;
typedef SpParMat<size_t, double, col_t> DistMatrixType;
typedef PlusTimesSRing<double, double> PTDD;
typedef skylark::base::sparse_matrix_t<double> LocalMatrixType;

template < typename InputMatrixType,
           typename OutputMatrixType = InputMatrixType >
struct Dummy_t : public skylark::sketch::hash_transform_t<
    InputMatrixType, OutputMatrixType,
    boost::random::uniform_int_distribution,
    skylark::utility::rademacher_distribution_t > {

    typedef skylark::sketch::hash_transform_t<
        InputMatrixType, OutputMatrixType,
        boost::random::uniform_int_distribution,
        skylark::utility::rademacher_distribution_t >
            hash_t;

    Dummy_t(int N, int S, skylark::base::context_t& context)
        : skylark::sketch::hash_transform_t<InputMatrixType, OutputMatrixType,
          boost::random::uniform_int_distribution,
          skylark::utility::rademacher_distribution_t>(N, S, context)
    {}

    std::vector<size_t> getRowIdx() { return hash_t::row_idx; }
    std::vector<double> getRowValues() { return hash_t::row_value; }
};

template<typename sketch_t>
void compute_sketch_matrix(sketch_t sketch, const DistMatrixType &A,
                           DistMatrixType &result) {

    std::vector<size_t> row_idx = sketch.getRowIdx();
    std::vector<double> row_val = sketch.getRowValues();

    // PI generated by random number gen
    size_t sketch_size = row_val.size();
    mpi_vector_t cols(sketch_size);
    mpi_vector_t rows(sketch_size);
    mpi_vector_t vals(sketch_size);

    for(size_t i = 0; i < sketch_size; ++i) {
        cols.SetElement(i, i);
        rows.SetElement(i, row_idx[i]);
        vals.SetElement(i, row_val[i]);
    }

    result = DistMatrixType(result.getnrow(), result.getncol(),
                            rows, cols, vals);
}

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

    //////////////////////////////////////////////////////////////////////////
    //[> Parameters <]

    //FIXME: use random sizes?
    const size_t n   = 200;
    const size_t m   = 100;
    const size_t n_s = 120;
    const size_t m_s = 60;

    //////////////////////////////////////////////////////////////////////////
    //[> Setup test <]
    namespace mpi = boost::mpi;
    mpi::environment env(argc, argv);
    mpi::communicator world;
    const size_t rank = world.rank();

    skylark::base::context_t context (0);

    double count = 1.0;

    const size_t matrix_full = n * m;
    mpi_vector_t colsf(matrix_full);
    mpi_vector_t rowsf(matrix_full);
    mpi_vector_t valsf(matrix_full);

    for(size_t i = 0; i < matrix_full; ++i) {
        colsf.SetElement(i, i % m);
        rowsf.SetElement(i, i / m);
        valsf.SetElement(i, count);
        count++;
    }

    DistMatrixType A(n, m, rowsf, colsf, valsf);

    //////////////////////////////////////////////////////////////////////////
    //[> Column wise application DistSparseMatrix -> DistSparseMatrix <]

    //[> 1. Create the sketching matrix <]
    Dummy_t<DistMatrixType, DistMatrixType> Sparse(n, n_s, context);

    //[> 2. Create space for the sketched matrix <]
    mpi_vector_t zero;
    DistMatrixType sketch_A(n_s, m, zero, zero, zero);

    //[> 3. Apply the transform <]
    Sparse.apply(A, sketch_A, skylark::sketch::columnwise_tag());

    //[> 4. Build structure to compare <]
    DistMatrixType pi_sketch(n_s, n, zero, zero, zero);
    compute_sketch_matrix(Sparse, A, pi_sketch);
    DistMatrixType expected_A = Mult_AnXBn_Synch<PTDD, double, col_t>(pi_sketch, A, false, false);
    if (!static_cast<bool>(expected_A == sketch_A))
        BOOST_FAIL("Result of colwise (dist -> dist) application not as expected");

    //////////////////////////////////////////////////////////////////////////
    //[> Column wise application DistSparseMatrix -> LocalSparseMatrix <]

    Dummy_t<DistMatrixType, LocalMatrixType> LocalSparse(n, n_s, context);
    LocalMatrixType local_sketch_A;
    LocalSparse.apply(A, local_sketch_A, skylark::sketch::columnwise_tag());

    if(rank == 0) {
        std::vector<size_t> row_idx = LocalSparse.getRowIdx();
        std::vector<double> row_val = LocalSparse.getRowValues();

        // PI generated by random number gen
        int sketch_size = row_val.size();
        typename LocalMatrixType::coords_t coords;
        for(int i = 0; i < sketch_size; ++i) {
            typename LocalMatrixType::coord_tuple_t new_entry(row_idx[i], i, row_val[i]);
            coords.push_back(new_entry);
        }

        LocalMatrixType pi_sketch_l;
        pi_sketch_l.set(coords);

        typename LocalMatrixType::coords_t coords_new;
        const int* indptr = pi_sketch_l.indptr();
        const int* indices = pi_sketch_l.indices();
        const double* values = pi_sketch_l.locked_values();

        // multiply with vector where an entry has the value:
        //   col_idx + row_idx * m + 1.
        // See creation of A.
        for(int col = 0; col < pi_sketch_l.width(); col++) {
            for(int idx = indptr[col]; idx < indptr[col + 1]; idx++) {
                for(int ccol = 0; ccol < m; ++ccol) {
                    typename LocalMatrixType::coord_tuple_t new_entry(indices[idx],
                            ccol, values[idx] * (ccol + col * m + 1));
                    coords_new.push_back(new_entry);
                }
            }
        }

        LocalMatrixType expected_A_l;
        expected_A_l.set(coords_new, n_s, m);

        if (!static_cast<bool>(expected_A_l == local_sketch_A))
            BOOST_FAIL("Result of local colwise application not as expected");
    }

    //////////////////////////////////////////////////////////////////////////
    //[> Row wise application DistSparseMatrix -> DistSparseMatrix <]

    //[> 1. Create the sketching matrix <]
    Dummy_t<DistMatrixType, DistMatrixType> Sparse_r(m, m_s, context);

    //[> 2. Create space for the sketched matrix <]
    DistMatrixType sketch_A_r(n, m_s, zero, zero, zero);

    //[> 3. Apply the transform <]
    Sparse_r.apply(A, sketch_A_r, skylark::sketch::rowwise_tag());

    //[> 4. Build structure to compare <]
    DistMatrixType pi_sketch_r(m_s, m, zero, zero, zero);
    compute_sketch_matrix(Sparse_r, A, pi_sketch_r);
    pi_sketch_r.Transpose();
    DistMatrixType expected_AR = PSpGEMM<PTDD>(A, pi_sketch_r);

    if (!static_cast<bool>(expected_AR == sketch_A_r))
        BOOST_FAIL("Result of rowwise (dist -> dist) application not as expected");

    return 0;
}


// Copyright 2020 Gareth Cross
#pragma once
#include <gtest/gtest.h>
#include <Eigen/Dense>

// Numerical tolerances for tests.
namespace tol {
static constexpr double kDeci = 1.0e-1;
static constexpr double kCenti = 1.0e-2;
static constexpr double kMilli = 1.0e-3;
static constexpr double kMicro = 1.0e-6;
static constexpr double kNano = 1.0e-9;
static constexpr double kPico = 1.0e-12;
}  // namespace tol

// Print variable w/ name.
#define PRINT(x) printImpl(#x, x)

template <typename Xpr>
void printImpl(const std::string& name, Xpr xpr) {
  std::cout << name << "=" << xpr << std::endl;
}

// Define a test on a class.
#define TEST_FIXTURE(object, function) \
  TEST_F(object, function) { function(); }

// Macro to compare eigen matrices and print a nice error.
#define EXPECT_EIGEN_NEAR(a, b, tol) EXPECT_PRED_FORMAT3(math::expectEigenNear, a, b, tol)
#define ASSERT_EIGEN_NEAR(a, b, tol) ASSERT_PRED_FORMAT3(math::expectEigenNear, a, b, tol)

namespace math {

// 300 randomly generated rotation vectors (range 0 to 2pi), used for testing.
extern const std::vector<Eigen::Vector3d> kRandomRotationVectorsZero2Pi;

// 300 more in 0 to pi.
extern const std::vector<Eigen::Vector3d> kRandomRotationVectorsZeroPi;

// Compare two eigen matrices. Use EXPECT_EIGEN_NEAR()
template <typename Ta, typename Tb>
testing::AssertionResult expectEigenNear(const std::string& name_a, const std::string& name_b,
                                         const std::string& name_tol,
                                         const Eigen::MatrixBase<Ta>& a,
                                         const Eigen::MatrixBase<Tb>& b, double tolerance) {
  if (a.rows() != b.rows() || a.cols() != b.cols()) {
    return testing::AssertionFailure()
           << "Dimensions of " << name_a << " and " << name_b << " do not match.";
  }
  for (int i = 0; i < a.rows(); ++i) {
    for (int j = 0; j < a.cols(); ++j) {
      const double delta = a(i, j) - b(i, j);
      if (std::abs(delta) > tolerance || std::isnan(delta)) {
        const std::string index_str = "(" + std::to_string(i) + ", " + std::to_string(j) + ")";
        return testing::AssertionFailure()
               << "Matrix equality " << name_a << " == " << name_b << " failed because:\n"
               << name_a << index_str << " - " << name_b << index_str << " = " << delta << " > "
               << name_tol << "\nWhere " << name_a << " evaluates to:\n"
               << a << "\n and " << name_b << " evaluates to:\n"
               << b << "\n and " << name_tol << " evaluates to: " << tolerance << "\n";
      }
    }
  }
  return testing::AssertionSuccess();
}

/// Create a vector in the specified range.
/// Begins at `start` and increments by `step` until >= `end`.
std::vector<double> Range(double start, double end, double step);
/**
 * @brief Exponential map via power series. Computes the value of exp(A), where A is a square
 *  matrix.
 * @note Refer to:
 *  "Chapter 4: Basics of Classical Lie Groups: The Exponential Map,
 *   Lie Groups, and Lie Algebras" - Jean Gallier
 * @param num_terms Number of terms in power series. Note that large values
 *  will result in floating-point underflow, so be careful.
 */
template <typename Scalar, int Rows, int Cols>
Eigen::Matrix<Scalar, Rows, Cols> ExpMatrixSeries(const Eigen::Matrix<Scalar, Rows, Cols>& A,
                                                  const int num_terms = 15) {
  Eigen::Matrix<double, Rows, Cols> A_power, solution;
  A_power.setIdentity();
  double fac_accum = 1;
  solution.setIdentity();  //  term 0
  for (int p = 1; p < num_terms; ++p) {
    A_power *= A.template cast<double>();
    fac_accum *= p;
    solution.noalias() += A_power / fac_accum;
  }
  return solution.template cast<Scalar>();
}

}  // namespace math