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

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

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

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

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

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

//
// *** ViennaCL
//
//#define VIENNACL_DEBUG_ALL
#define VIENNACL_WITH_UBLAS 1
#include "viennacl/scalar.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/direct_solve.hpp"
#include "viennacl/linalg/lu.hpp"
#include "examples/tutorial/Random.hpp"

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

template<typename ScalarType, typename VCLVectorType>
ScalarType diff(ublas::vector<ScalarType> const & v1, VCLVectorType const & v2)
{
   ublas::vector<ScalarType> v2_cpu(v2.size());
   viennacl::backend::finish();  //workaround for a bug in APP SDK 2.7 on Trinity APUs (with Catalyst 12.8)
   viennacl::copy(v2.begin(), v2.end(), v2_cpu.begin());

   for (unsigned int i=0;i<v1.size(); ++i)
   {
      if (v2_cpu[i] != v1[i])
        return 1;
   }

   return 0;
}

template<typename ScalarType, typename VCLMatrixType>
ScalarType diff(ublas::matrix<ScalarType> const & mat1, VCLMatrixType const & mat2)
{
   ublas::matrix<ScalarType> mat2_cpu(mat2.size1(), mat2.size2());
   viennacl::backend::finish();  //workaround for a bug in APP SDK 2.7 on Trinity APUs (with Catalyst 12.8)
   viennacl::copy(mat2, mat2_cpu);

    for (unsigned int i = 0; i < mat2_cpu.size1(); ++i)
    {
      for (unsigned int j = 0; j < mat2_cpu.size2(); ++j)
      {
         if (mat2_cpu(i,j) != mat1(i,j))
           return 1;
      }
    }
   //std::cout << ret << std::endl;
   return 0;
}
//
// -------------------------------------------------------------
//

template<typename NumericT,
          typename UblasMatrixType, typename UblasVectorType,
          typename VCLMatrixType, typename VCLVectorType1, typename VCLVectorType2>
int test_prod_rank1(UblasMatrixType & ublas_m1, UblasVectorType & ublas_v1, UblasVectorType & ublas_v2,
                    VCLMatrixType & vcl_m1, VCLVectorType1 & vcl_v1, VCLVectorType2 & vcl_v2)
{
   int retval = EXIT_SUCCESS;

   // sync data:
   ublas_v1 = ublas::scalar_vector<NumericT>(ublas_v1.size(), NumericT(2));
   ublas_v2 = ublas::scalar_vector<NumericT>(ublas_v2.size(), NumericT(3));
   viennacl::copy(ublas_v1.begin(), ublas_v1.end(), vcl_v1.begin());
   viennacl::copy(ublas_v2.begin(), ublas_v2.end(), vcl_v2.begin());
   viennacl::copy(ublas_m1, vcl_m1);

   // --------------------------------------------------------------------------
   std::cout << "Rank 1 update" << std::endl;

   ublas_m1 += ublas::outer_prod(ublas_v1, ublas_v2);
   vcl_m1 += viennacl::linalg::outer_prod(vcl_v1, vcl_v2);
   if ( diff(ublas_m1, vcl_m1) != 0 )
   {
      std::cout << "# Error at operation: rank 1 update" << std::endl;
      std::cout << "  diff: " << diff(ublas_m1, vcl_m1) << std::endl;
      return EXIT_FAILURE;
   }

   // --------------------------------------------------------------------------
   std::cout << "Scaled rank 1 update - CPU Scalar" << std::endl;
   ublas_m1 += NumericT(4) * ublas::outer_prod(ublas_v1, ublas_v2);
   vcl_m1 += NumericT(2) * viennacl::linalg::outer_prod(vcl_v1, vcl_v2);
   vcl_m1 += viennacl::linalg::outer_prod(vcl_v1, vcl_v2) * NumericT(2);  //check proper compilation
   if ( diff(ublas_m1, vcl_m1) != 0 )
   {
      std::cout << "# Error at operation: scaled rank 1 update - CPU Scalar" << std::endl;
      std::cout << "  diff: " << diff(ublas_m1, vcl_m1) << std::endl;
      return EXIT_FAILURE;
   }

      // --------------------------------------------------------------------------
   std::cout << "Scaled rank 1 update - GPU Scalar" << std::endl;
   ublas_m1 += NumericT(4) * ublas::outer_prod(ublas_v1, ublas_v2);
   vcl_m1 += viennacl::scalar<NumericT>(2) * viennacl::linalg::outer_prod(vcl_v1, vcl_v2);
   vcl_m1 += viennacl::linalg::outer_prod(vcl_v1, vcl_v2) * viennacl::scalar<NumericT>(2);  //check proper compilation
   if ( diff(ublas_m1, vcl_m1) != 0 )
   {
      std::cout << "# Error at operation: scaled rank 1 update - GPU Scalar" << std::endl;
      std::cout << "  diff: " << diff(ublas_m1, vcl_m1) << std::endl;
      return EXIT_FAILURE;
   }

   //reset vcl_matrix:
   viennacl::copy(ublas_m1, vcl_m1);

   // --------------------------------------------------------------------------
   std::cout << "Matrix-Vector product" << std::endl;
   ublas_v1 = viennacl::linalg::prod(ublas_m1, ublas_v2);
   vcl_v1   = viennacl::linalg::prod(vcl_m1, vcl_v2);

   if ( diff(ublas_v1, vcl_v1) != 0 )
   {
      std::cout << "# Error at operation: matrix-vector product" << std::endl;
      std::cout << "  diff: " << diff(ublas_v1, vcl_v1) << std::endl;
      retval = EXIT_FAILURE;
   }
   // --------------------------------------------------------------------------
   std::cout << "Matrix-Vector product with scaled add" << std::endl;
   NumericT alpha = static_cast<NumericT>(2);
   NumericT beta = static_cast<NumericT>(3);
   viennacl::copy(ublas_v1.begin(), ublas_v1.end(), vcl_v1.begin());
   viennacl::copy(ublas_v2.begin(), ublas_v2.end(), vcl_v2.begin());

   ublas_v1 = alpha * viennacl::linalg::prod(ublas_m1, ublas_v2) + beta * ublas_v1;
   vcl_v1   = alpha * viennacl::linalg::prod(vcl_m1, vcl_v2) + beta * vcl_v1;

   if ( diff(ublas_v1, vcl_v1) != 0 )
   {
      std::cout << "# Error at operation: matrix-vector product with scaled additions" << std::endl;
      std::cout << "  diff: " << diff(ublas_v1, vcl_v1) << std::endl;
      retval = EXIT_FAILURE;
   }
   // --------------------------------------------------------------------------

   viennacl::copy(ublas_v1.begin(), ublas_v1.end(), vcl_v1.begin());
   viennacl::copy(ublas_v2.begin(), ublas_v2.end(), vcl_v2.begin());

   std::cout << "Transposed Matrix-Vector product" << std::endl;
   ublas_v2 = alpha * viennacl::linalg::prod(trans(ublas_m1), ublas_v1);
   vcl_v2   = alpha * viennacl::linalg::prod(trans(vcl_m1), vcl_v1);

   if ( diff(ublas_v2, vcl_v2) != 0 )
   {
      std::cout << "# Error at operation: transposed matrix-vector product" << std::endl;
      std::cout << "  diff: " << diff(ublas_v2, vcl_v2) << std::endl;
      retval = EXIT_FAILURE;
   }

   std::cout << "Transposed Matrix-Vector product with scaled add" << std::endl;
   ublas_v2 = alpha * viennacl::linalg::prod(trans(ublas_m1), ublas_v1) + beta * ublas_v2;
   vcl_v2   = alpha * viennacl::linalg::prod(trans(vcl_m1), vcl_v1) + beta * vcl_v2;

   if ( diff(ublas_v2, vcl_v2) != 0 )
   {
      std::cout << "# Error at operation: transposed matrix-vector product with scaled additions" << std::endl;
      std::cout << "  diff: " << diff(ublas_v2, vcl_v2) << std::endl;
      retval = EXIT_FAILURE;
   }
   // --------------------------------------------------------------------------

   return retval;
}

//
// -------------------------------------------------------------
//
template< typename NumericT, typename F>
int test()
{
   int retval = EXIT_SUCCESS;

   std::size_t num_rows = 141;
   std::size_t num_cols = 103;

   // --------------------------------------------------------------------------
   ublas::vector<NumericT> ublas_v1(num_rows);
   for (std::size_t i = 0; i < ublas_v1.size(); ++i)
     ublas_v1(i) = NumericT(i);
   ublas::vector<NumericT> ublas_v2 = ublas::scalar_vector<NumericT>(num_cols, NumericT(3));

   ublas::matrix<NumericT> ublas_m1(ublas_v1.size(), ublas_v2.size());
   ublas::matrix<NumericT> ublas_m2(ublas_v1.size(), ublas_v1.size());

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   for (std::size_t i = 0; i < ublas_m2.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m2.size2(); ++j)
       ublas_m2(i,j) = NumericT(j - i*j + i);

   viennacl::vector<NumericT> vcl_v1_native(ublas_v1.size());
   viennacl::vector<NumericT> vcl_v1_large(4 * ublas_v1.size());
   viennacl::vector_range< viennacl::vector<NumericT> > vcl_v1_range(vcl_v1_large, viennacl::range(3, ublas_v1.size() + 3));
   viennacl::vector_slice< viennacl::vector<NumericT> > vcl_v1_slice(vcl_v1_large, viennacl::slice(2, 3, ublas_v1.size()));

   viennacl::vector<NumericT> vcl_v2_native(ublas_v2.size());
   viennacl::vector<NumericT> vcl_v2_large(4 * ublas_v2.size());
   viennacl::vector_range< viennacl::vector<NumericT> > vcl_v2_range(vcl_v2_large, viennacl::range(8, ublas_v2.size() + 8));
   viennacl::vector_slice< viennacl::vector<NumericT> > vcl_v2_slice(vcl_v2_large, viennacl::slice(6, 2, ublas_v2.size()));

   viennacl::matrix<NumericT, F> vcl_m1_native(ublas_m1.size1(), ublas_m1.size2());
   viennacl::matrix<NumericT, F> vcl_m1_large(4 * ublas_m1.size1(), 4 * ublas_m1.size2());
   viennacl::matrix_range< viennacl::matrix<NumericT, F> > vcl_m1_range(vcl_m1_large,
                                                                        viennacl::range(8, ublas_m1.size1() + 8),
                                                                        viennacl::range(ublas_m1.size2(), 2 * ublas_m1.size2()) );
   viennacl::matrix_slice< viennacl::matrix<NumericT, F> > vcl_m1_slice(vcl_m1_large,
                                                                        viennacl::slice(6, 2, ublas_m1.size1()),
                                                                        viennacl::slice(ublas_m1.size2(), 2, ublas_m1.size2()) );

   viennacl::matrix<NumericT, F> vcl_m2_native(ublas_m2.size1(), ublas_m2.size2());
   viennacl::matrix<NumericT, F> vcl_m2_large(4 * ublas_m2.size1(), 4 * ublas_m2.size2());
   viennacl::matrix_range< viennacl::matrix<NumericT, F> > vcl_m2_range(vcl_m2_large,
                                                                        viennacl::range(8, ublas_m2.size1() + 8),
                                                                        viennacl::range(ublas_m2.size2(), 2 * ublas_m2.size2()) );
   viennacl::matrix_slice< viennacl::matrix<NumericT, F> > vcl_m2_slice(vcl_m2_large,
                                                                        viennacl::slice(6, 2, ublas_m2.size1()),
                                                                        viennacl::slice(ublas_m2.size2(), 2, ublas_m2.size2()) );

   //
   // Run a bunch of tests for rank-1-updates, matrix-vector products
   //
   std::cout << "------------ Testing rank-1-updates and matrix-vector products ------------------" << std::endl;

   std::cout << "* m = full, v1 = full, v2 = full" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_native, vcl_v1_native, vcl_v2_native);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = full, v1 = full, v2 = range" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_native, vcl_v1_native, vcl_v2_range);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = full, v1 = full, v2 = slice" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_native, vcl_v1_native, vcl_v2_slice);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   // v1 = range

   std::cout << "* m = full, v1 = range, v2 = full" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_native, vcl_v1_range, vcl_v2_native);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = full, v1 = range, v2 = range" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_native, vcl_v1_range, vcl_v2_range);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = full, v1 = range, v2 = slice" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_native, vcl_v1_range, vcl_v2_slice);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   // v1 = slice

   std::cout << "* m = full, v1 = slice, v2 = full" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_native, vcl_v1_slice, vcl_v2_native);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = full, v1 = slice, v2 = range" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_native, vcl_v1_slice, vcl_v2_range);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = full, v1 = slice, v2 = slice" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_native, vcl_v1_slice, vcl_v2_slice);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   ///////////////////////////// matrix_range

   std::cout << "* m = range, v1 = full, v2 = full" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_range, vcl_v1_native, vcl_v2_native);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = range, v1 = full, v2 = range" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_range, vcl_v1_native, vcl_v2_range);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = range, v1 = full, v2 = slice" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_range, vcl_v1_native, vcl_v2_slice);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   // v1 = range

   std::cout << "* m = range, v1 = range, v2 = full" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_range, vcl_v1_range, vcl_v2_native);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = range, v1 = range, v2 = range" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_range, vcl_v1_range, vcl_v2_range);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = range, v1 = range, v2 = slice" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_range, vcl_v1_range, vcl_v2_slice);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   // v1 = slice

   std::cout << "* m = range, v1 = slice, v2 = full" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_range, vcl_v1_slice, vcl_v2_native);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = range, v1 = slice, v2 = range" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_range, vcl_v1_slice, vcl_v2_range);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = range, v1 = slice, v2 = slice" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_range, vcl_v1_slice, vcl_v2_slice);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   ///////////////////////////// matrix_slice

   std::cout << "* m = slice, v1 = full, v2 = full" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_slice, vcl_v1_native, vcl_v2_native);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = slice, v1 = full, v2 = range" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_slice, vcl_v1_native, vcl_v2_range);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = slice, v1 = full, v2 = slice" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_slice, vcl_v1_native, vcl_v2_slice);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   // v1 = range

   std::cout << "* m = slice, v1 = range, v2 = full" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_slice, vcl_v1_range, vcl_v2_native);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = slice, v1 = range, v2 = range" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_slice, vcl_v1_range, vcl_v2_range);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = slice, v1 = range, v2 = slice" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_slice, vcl_v1_range, vcl_v2_slice);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   // v1 = slice

   std::cout << "* m = slice, v1 = slice, v2 = full" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_slice, vcl_v1_slice, vcl_v2_native);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = slice, v1 = slice, v2 = range" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_slice, vcl_v1_slice, vcl_v2_range);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   for (std::size_t i = 0; i < ublas_m1.size1(); ++i)
    for (std::size_t j = 0; j < ublas_m1.size2(); ++j)
      ublas_m1(i,j) = NumericT(i+j);

   std::cout << "* m = slice, v1 = slice, v2 = slice" << std::endl;
   retval = test_prod_rank1<NumericT>(ublas_m1, ublas_v1, ublas_v2,
                                      vcl_m1_slice, vcl_v1_slice, vcl_v2_slice);
   if (retval == EXIT_FAILURE)
   {
     std::cout << " --- FAILED! ---" << std::endl;
     return retval;
   }
   else
     std::cout << " --- PASSED ---" << std::endl;

   return retval;
}
//
// -------------------------------------------------------------
//
int main()
{
   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "## Test :: Matrix" << 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 int NumericT;
      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  numeric: int" << std::endl;
      std::cout << "  layout: row-major" << std::endl;
      retval = test<NumericT, viennacl::row_major>();
      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;
   {
      typedef int NumericT;
      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  numeric: int" << std::endl;
      std::cout << "  layout: column-major" << std::endl;
      retval = test<NumericT, viennacl::column_major>();
      if ( retval == EXIT_SUCCESS )
         std::cout << "# Test passed" << std::endl;
      else
         return retval;
   }
   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;

#ifdef VIENNACL_WITH_OPENCL
   if ( viennacl::ocl::current_device().double_support() )
#endif
   {
      {
         typedef long NumericT;
         std::cout << "# Testing setup:" << std::endl;
         std::cout << "  numeric: double" << std::endl;
         std::cout << "  layout: row-major" << std::endl;
         retval = test<NumericT, viennacl::row_major>();
            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;
      {
         typedef long NumericT;
         std::cout << "# Testing setup:" << std::endl;
         std::cout << "  numeric: double" << std::endl;
         std::cout << "  layout: column-major" << std::endl;
         retval = test<NumericT, viennacl::column_major>();
            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;
   }

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

   return retval;
}


// Copyright John Maddock 2014.

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

// Caution: this file contains Quickbook markup as well as code
// and comments, don't change any of the special comment markups!

#ifdef _MSC_VER
#  pragma warning (disable : 4996) // disable -D_SCL_SECURE_NO_WARNINGS C++ 'Checked Iterators'
#endif

#include <boost/math/distributions/hyperexponential.hpp>
#include <iostream>

#ifndef BOOST_NO_CXX11_HDR_ARRAY
#include <array>
#endif

int main()
{
   {
//[hyperexponential_snip1
//=#include <boost/math/distributions/hyperexponential.hpp>
//=#include <iostream>
//=int main()
//={
   const double rates[] = { 1.0 / 10.0, 1.0 / 12.0 };

   boost::math::hyperexponential he(rates);

   std::cout << "Average lifetime: "
      << boost::math::mean(he)
      << " years" << std::endl;
   std::cout << "Probability that the appliance will work for more than 15 years: "
      << boost::math::cdf(boost::math::complement(he, 15.0))
      << std::endl;
//=}
//]
   }
   using namespace boost::math;
#ifndef BOOST_NO_CXX11_HDR_ARRAY
   {
   //[hyperexponential_snip2
   std::array<double, 2> phase_prob = { 0.5, 0.5 };
   std::array<double, 2> rates = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he(phase_prob.begin(), phase_prob.end(), rates.begin(), rates.end());
   //]
   }

   {
   //[hyperexponential_snip3
   // We could be using any standard library container here... vector, deque, array, list etc:
   std::array<double, 2> phase_prob = { 0.5, 0.5 };
   std::array<double, 2> rates      = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he1(phase_prob, rates);    // Construct from standard library container.

   double phase_probs2[] = { 0.5, 0.5 };
   double rates2[]       = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he2(phase_probs2, rates2);  // Construct from native C++ array.
   //]
   }
   {
   //[hyperexponential_snip4
   // We could be using any standard library container here... vector, deque, array, list etc:
   std::array<double, 2> rates = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he(rates.begin(), rates.end());

   assert(he.probabilities()[0] == 0.5); // Phase probabilities will be equal and normalised to unity.
   //]
   }
   {
   //[hyperexponential_snip5
   std::array<double, 2> rates = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he(rates);

   assert(he.probabilities()[0] == 0.5); // Phase probabilities will be equal and normalised to unity.
   //]
   }
#endif
#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST) && !(defined(BOOST_GCC_VERSION) && (BOOST_GCC_VERSION < 40500))
   {
   //[hyperexponential_snip6
   hyperexponential he = { { 0.5, 0.5 }, { 1.0 / 10, 1.0 / 12 } };
   //]
   }
   {
   //[hyperexponential_snip7
   hyperexponential he = { 1.0 / 10, 1.0 / 12 };

   assert(he.probabilities()[0] == 0.5);
   //]
   }
#endif
   return 0;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
#define CROCODDYL_CORE_UTILS_MATH_HPP_

#include <boost/type_traits.hpp>

#include <Eigen/Dense>
#include <algorithm>
#include <limits>

// fwd

template <typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
struct pseudoInverseAlgo {
  typedef typename MatrixLike::Scalar Scalar;
  typedef typename MatrixLike::RealScalar RealScalar;

  static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a, const RealScalar& epsilon) {
    using std::max;
    Eigen::JacobiSVD<MatrixLike> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
    RealScalar tolerance =
        epsilon * static_cast<Scalar>(max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
    return svd.matrixV() *
           (svd.singularValues().array().abs() > tolerance)
               .select(svd.singularValues().array().inverse(), 0)
               .matrix()
               .asDiagonal() *
           svd.matrixU().adjoint();
  }
};

template <typename MatrixLike>
struct pseudoInverseAlgo<MatrixLike, false> {
  typedef typename MatrixLike::Scalar Scalar;
  typedef typename MatrixLike::RealScalar RealScalar;

  static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a, const RealScalar&) {
    return Eigen::MatrixBase<MatrixLike>::Zero(a.rows(), a.cols());
  }
};

template <typename MatrixLike>
MatrixLike pseudoInverse(const Eigen::MatrixBase<MatrixLike>& a,
                         const typename MatrixLike::RealScalar& epsilon =
                             Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) {
  return pseudoInverseAlgo<MatrixLike>::run(a, epsilon);
}

#endif  // CROCODDYL_CORE_UTILS_MATH_HPP_


#include <math.h>
#include <stdlib.h>
#include <string>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
//#include <opencv2/legacy/compat.hpp>

#include "dlib/opencv.h"
#include "dlib/image_processing/frontal_face_detector.h"
#include "dlib/image_processing/render_face_detections.h"
#include "dlib/gui_widgets.h"
#include <dlib/image_processing.h>

#include "util.h"
#include "constants.h"
#include "faceDetection.h"

using namespace std;
using namespace dlib;

void FaceFeatures::assign(cv::Point c_face_centre, cv::Point c_left_eye, cv::Point c_right_eye, cv::Point c_nose_tip, cv::Point c_mouth)
{
	face_centre = c_face_centre;
	left_eye = c_left_eye;
	right_eye = c_right_eye;
	nose_tip = c_nose_tip;
	mouth = c_mouth;

	mid_eye.x = (left_eye.x + right_eye.x) / 2.0;
	mid_eye.y = (left_eye.y + right_eye.y) / 2.0;

	//Find the nose base along the symmetry axis
	nose_base.x = mouth.x + (mid_eye.x - mouth.x) * Rm;
	nose_base.y = mouth.y - (mouth.y - mid_eye.y) * Rm;
}

void FaceData::assign(FaceFeatures *f)
{
	left_eye_nose_distance = get_distance(f->left_eye, f->nose_base);
	right_eye_nose_distance = get_distance(f->right_eye, f->nose_base);
	left_eye_right_eye_distance = get_distance(f->left_eye, f->right_eye);
	nose_mouth_distance = get_distance(f->nose_base, f->mouth);

	mid_eye_mouth_distance = get_distance(f->mid_eye, f->mouth);
	nose_base_nose_tip_distance = get_distance(f->nose_tip, f->nose_base);
}

void FacePose::assign(FaceFeatures *f, FaceData *d)
{
	roll = get_angle_between(f->left_eye, f->right_eye);
	if (roll > 180.0)
	{
		roll = roll - 360.0;
	}
	symm_x = get_angle_between(f->nose_base, f->mid_eye);
	//symm angle - angle between the symmetry axis and the 'x' axis
	tau = get_angle_between(f->nose_base, f->nose_tip);
	//tilt angle - angle between normal in image and 'x' axis
	theta = (abs(tau - symm_x)) * (PI / 180.0);
	//theta angle - angle between the symmetry axis and the image normal
	sigma = find_sigma(d->nose_base_nose_tip_distance, d->mid_eye_mouth_distance, Rn, theta);
	//std::cout<<"symm : "<<symm_x<<" tau : "<<tau<<" theta : "<<theta<<" sigma : "<<sigma<<" ";

	normal[0] = (sin(sigma)) * (cos((360 - tau) * (PI / 180.0)));
	normal[1] = (sin(sigma)) * (sin((360 - tau) * (PI / 180.0)));
	normal[2] = -cos(sigma);

	kalman_pitch_pre = pitch;
	pitch = acos(sqrt((normal[0] * normal[0] + normal[2] * normal[2]) / (normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2])));
	if ((f->nose_tip.y - f->nose_base.y) < 0)
	{
		pitch = -pitch;
	}

	kalman_yaw_pre = yaw;
	yaw = acos((abs(normal[2])) / (sqrt(normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2])));
	if ((f->nose_tip.x - f->nose_base.x) < 0)
	{
		yaw = -yaw;
	}

	pointer_2d.x = ((f->nose_base.x + cvRound(500 * (tan((double)yaw)))));
	pointer_2d.y = ((f->nose_base.y + cvRound(500 * (tan((double)pitch)))));
}

void draw_facial_normal(cv::Mat &img, dlib::full_object_detection shape, std::vector<double> normal, double mag)
{

	double del_x = mag * normal[0];
	double del_y = mag * normal[1];

	cv::line(img, cv::Point(shape.part(30).x(), shape.part(30).y()),
			 cv::Point(shape.part(30).x() + del_x, shape.part(30).y() + del_y), cv::Scalar(0), 3);

	//std::cout<<"magnitude : "<<vectorMagnitude(f->normal, 3)<<" ";
	//std::cout<<f->normal[0]<<", "<<f->normal[1]<<", "<<f->normal[2];
}

void draw_crosshair(cv::Mat img, CvPoint centre, int circle_radius, int line_radius)
{
	cv::Point pt1, pt2, pt3, pt4;
	cv::Scalar colour(255);

	pt1.x = centre.x;
	pt2.x = centre.x;
	pt1.y = centre.y - line_radius;
	pt2.y = centre.y + line_radius;
	pt3.x = centre.x - line_radius;
	pt4.x = centre.x + line_radius;
	pt3.y = centre.y;
	pt4.y = centre.y;

	cv::circle(img, centre, circle_radius, colour, 2, 4, 0);

	cv::line(img, pt1, pt2, colour, 1, 4, 0);
	cv::line(img, pt3, pt4, colour, 1, 4, 0);
}

void project_facial_pose(cv::Mat img, double normal[3], double sigma, double theta)
{

	cv::Point origin = cv::Point(50, 50);
	cv::Scalar colour = cv::Scalar(255);

	cv::Point projection_2d;
	projection_2d.x = origin.x + cvRound(60 * (normal[0]));
	projection_2d.y = origin.y + cvRound(60 * (normal[1]));

	if (normal[0] > 0 && normal[1] < 0)
	{
		cv::ellipse(img, origin, cv::Size(25, std::abs(cvRound(25 - sigma * (180 / (2 * PI))))), std::abs(180 - (theta * (180 / PI))), 0, 360, colour, 2, 4, 0);
	}
	else
	{
		cv::ellipse(img, origin, cv::Size(25, std::abs(cvRound(25 - sigma * (180 / (2 * PI))))), std::abs(theta * (180 / PI)), 0, 360, colour, 2, 4, 0);
	}

	cv::line(img, origin, projection_2d, colour, 2, 4, 0);
}

double find_sigma(int ln, int lf, double Rn, double theta)
{
	double dz = 0;
	double sigma;
	double m1 = ((double)ln * ln) / ((double)lf * lf);
	double m2 = (cos(theta)) * (cos(theta));

	if (m2 == 1)
	{
		dz = sqrt((Rn * Rn) / (m1 + (Rn * Rn)));
	}
	if (m2 >= 0 && m2 < 1)
	{
		dz = sqrt(((Rn * Rn) - m1 - 2 * m2 * (Rn * Rn) + sqrt(((m1 - (Rn * Rn)) * (m1 - (Rn * Rn))) + 4 * m1 * m2 * (Rn * Rn))) / (2 * (1 - m2) * (Rn * Rn)));
	}
	sigma = acos(dz);
	return sigma;
}


// [[Rcpp::depends(RcppArmadillo)]]
#define ARMA_DONT_PRINT_ERRORS
#include <iostream>
#include <fstream>
#include <cmath>
#include <armadillo>
#include <errno.h>
#include <RcppArmadillo.h>

//' Compute hazard of detection (Hayes-Buckland isotropic hazard)
//'
//' @param x x distance
//' @param y y distance
//' @param dt time step
//' @param observer_speed observer speed
//' @param parameter detection parameters
//' @param w truncation width
//' @param type transect type 0 = point, 1 = line
//' @return hazard of detection
double CalcHazard2(double x, double y, double dt, double observer_speed,
                   arma::vec parameter, double w = 0, int type = 0) {
  double hazard = 0;
  if (type == 1) {
    double r = x * x + y * y;
    hazard = dt * parameter(0) / pow(r, 0.5 * parameter(1));
  }
  if (type == 0) {
    if (y < 0) return 0;
    double abeta = 0.5 * (parameter(1) - 1.0);
    double r0 = x * x  + y * y;
    double y1 = y - observer_speed * dt;
    if (y1 < 0) y1 = 0;
    double r1 = x * x  + y1 * y1;
    if (r1 < 1e-10) return arma::datum::inf;
    if (fabs(x) < 1e-10) {
      if (fabs(parameter(1) - 1) < 1e-10) {
        hazard = log(sqrt(r1)) - log(sqrt(r0));
        hazard *= parameter(0);
      } else {
        hazard = 1.0 / pow(r1, abeta) - 1.0 / pow(r0, abeta);
        hazard *= parameter(0) / (parameter(1) - 1.0);
      }
    } else {
      hazard = R::pbeta(x * x / r1, abeta, 0.5, 1, 0) - R::pbeta(x * x / r0,
                        abeta, 0.5, 1, 0);
      hazard *= R::beta(abeta, 0.5) * parameter(0) / (2.0 * pow(fabs(x),
                                                parameter(1) - 1.0));
    }
  }
  return hazard;
}

//' Get Recorded forward distance once detection occurs
//'
//' @param x recorded x location
//' @param y recorded y location
//' @param accu_hazard hazard accumulated up to that time
//' @param u log random deviate
//' @param parameter detection parameters
//'
//' @return recorded forward distance
double GetRecordedYPosition(double x, double y, double accu_hazard,
                            double u, arma::vec parameter) {
  double r = x * x + y * y;
  double y1;
  double abeta =  0.5 * (parameter(1) - 1.0);
  if (fabs(x) < 1e-10) {
    if (fabs(parameter(1) - 1) < 1e-10) {
      y1 = sqrt(exp(2.0 * (u - accu_hazard) / parameter(0) + log(r)) - x * x);
    } else {
      y1 = sqrt(pow((u - accu_hazard) * (parameter(1) - 1.0) / parameter(0) +
        1 / pow(r, abeta), -1.0 / abeta) - x * x);
    }
  } else {
    y1 = R::pbeta(x * x / r, abeta, 0.5, 1, 0) + 2 * pow(fabs(x),
                  parameter(1) - 1.0) * (u - accu_hazard) / (parameter(0) * R::beta(abeta, 0.5));
    y1 = R::qbeta(y1, abeta, 0.5, 1, 0);
    y1 = fabs(x) * sqrt(1.0 / y1 - 1.0);
  }
  return y1;
}

//' Get Recorded time once detection occurs
//'
//' @param x recorded x location
//' @param y recorded y location
//' @param accu_hazard hazard accumulated up to that time
//' @param u log random deviate
//' @param parameter detection parameters
//'
//' @return recorded forward distance
double GetRecordedT(double x, double y, double accu_hazard,
                    double u, arma::vec parameter) {
  double r = x * x + y * y;
  double t_add = (u - accu_hazard) * pow(r, 0.5 * parameter(1)) / parameter(0);
  return t_add;
}

//' Simulate distance sampling survey
//'
//' @param true_parameter (detection shape, scale, diffusion sd)
//' @param N number of animals
//' @param auxiliary_info (region x-extent ,region y-extent, survey time, dt,
//' transect type (0 = point, 1 = line), observer_speed, number of transects, half width of transects)
//' @param dt time step
//' @param move = 0 no mvoement, 1 Brownian motion
//' @return Outputs csv data file
// [[Rcpp::export]]
int SimulateDsData(arma::vec true_parameter,
                   int N,
                   arma::vec auxiliary_info,
                   double dt,
                   int move = 0) {
  arma::vec area = auxiliary_info.rows(0, 1);
  double half_width = auxiliary_info(2) * 0.5;
  double survey_time = auxiliary_info(4);
  double transect_type = auxiliary_info(7);
  double observer_speed = auxiliary_info(5);
  int num_transects = auxiliary_info(6);
  arma::vec parameter(true_parameter);
  if (transect_type == 0) {
    parameter(0) = 2.0 * pow(parameter(0), parameter(1)) / R::beta(0.5 * parameter(1), 0.5);
  } else {
    parameter(0) = pow(parameter(0), parameter(1));
  }
  if (transect_type == 0) ++parameter(1);
  arma::vec observer_position(2);
  observer_position(0) = area(0) * 0.5;
  observer_position(1) = area(1) * 0.5;
  arma::vec detection_parameter(parameter.rows(0,1));
  double sd = parameter(2);
  // open data file
  std::ofstream data("./simulated_dsdata.csv");
  arma::vec x(N);
  arma::vec y(N);
  arma::vec u(N);
  arma::vec detected(N);
  arma::vec accu_hazard(N);
  double recorded_y;
  double recorded_t;
  double recorded_r;
  double hazard;
  bool include;
  for (int transect = 0; transect < num_transects; ++transect) {
    if (transect_type == 0) observer_position(1) = 0.0;
    x = arma::randu(N) * area(0);
    y = arma::randu(N) * area(1);
    u = -log(arma::randu(N));
    detected.zeros();
    accu_hazard.zeros();
    for (int t = 0; t < floor(survey_time / dt); ++t) {
      for (int i = 0; i < N; ++i) {
        hazard = CalcHazard2(x(i) - observer_position(0), y(i) -
          observer_position(1), dt, observer_speed, parameter, half_width, transect_type);
        if ((accu_hazard(i) + hazard >= u(i)) & (accu_hazard(i) < u(i))) {
          include = false;
          if (transect_type == 0) {
            recorded_y = GetRecordedYPosition(x(i) - observer_position(0),
                                              y(i) - observer_position(1), accu_hazard(i), u(i),
                                              parameter);
            recorded_t = t * dt + (y(i) - observer_position(1) - recorded_y) /
              observer_speed;
            if ((recorded_t < survey_time) & (fabs(x(i) - observer_position(0)) < half_width)) include = true;
          } else {
            recorded_y = y(i) - observer_position(1);
            recorded_t = t * dt + GetRecordedT(x(i) - observer_position(0),
                                               y(i) - observer_position(1),
                                               accu_hazard(i),
                                               u(i),
                                               parameter);
            recorded_r = pow(x(i) - observer_position(0), 2) + recorded_y * recorded_y;
            if (recorded_r <= half_width * half_width) include = true;
          }
          if (include) {
            data << transect + 1 << "," << x(i) - observer_position(0) << "," << recorded_y << "," <<
              recorded_t << "\n";
          }
        }
        accu_hazard(i) += hazard;
        // assume uniform initial behaviour distribution
        if (move == 1) {
          x(i) += sd * sqrt(dt) * R::rnorm(0, 1);
          y(i) += sd * sqrt(dt) * R::rnorm(0, 1);
        }
      }
      observer_position(1) += observer_speed * dt;
      arma::uvec outside = find(x < 0 || x > area(0) || y < 0 || y > area(1));
      arma::vec u2(arma::randu(N));
      u(outside) = u2(outside);
      accu_hazard(outside).fill(0.0);
      x(find(x < 0)) += area(0);
      x(find(x > area(0))) -= area(0);
      y(find(y < 0)) += area(1);
      y(find(y > area(1))) -= area(1);
    }
  }
  data.close();
  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FREXP_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FREXP_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing frexp capabilities

    Computes a mantissa and an exponent pair for the input

    @code
    m = frexp(x, e);
    @endcode

    is similar to:

    @code
    as_integer_t<T> e = exponent(x)+1;
    T m = mantissa(x)/2;
    @endcode

    The call

    @code
    std:pair<T,as_integer_t<T>> p = frexp(x);
    @endcode

    can also be used.

    @par Note:

    @c frexp splits a floating point value @c v f in a signed mantissa @c m and
    an exponent @c e so that:  @f$v = m\times 2^e@f$, with absolute value of @c m
    between 0.5 (included) and 1 (excluded)

    Take care that these results differ from the returns of the functions @ref mantissa
    and @ref exponent

    @see exponent,  mantissa

  **/
  const boost::dispatch::functor<tag::frexp_> frexp = {};
} }
#endif

#include <boost/simd/function/scalar/frexp.hpp>
#include <boost/simd/function/scalar/frexp.hpp>
#include <boost/simd/function/simd/frexp.hpp>

#endif


/**
 * @file 	MyDijkstra.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 10, 2017
 */

#include <boost/heap/fibonacci_heap.hpp>
#include "MyDijkstra.h"

using std::pair;

using Pair = pair<int, double>;
typedef graph_traits<Graph>::vertex_descriptor Vertex;

typename graph_traits<Graph>::out_edge_iterator out_i, out_end;

/**
 * Data that is stored in one node of a heap. Contains an integer and a double.
 * Comparisons are made by the double, smaller has higher priority
 */
struct heap_data
{
    heap::fibonacci_heap<heap_data>::handle_type handle;
    Pair pair;

    heap_data(Pair p):
        pair(p)
    {}

    bool operator<(heap_data const & rhs) const {
        return pair.second > rhs.pair.second;
    }
};

using Heap = heap::fibonacci_heap<heap_data>;

void MyDijkstra::computeShortestPaths(Graph& g, int numVertices, int source,
		double distances[], int predecessors[]) {

	predecessors[source] = source;

	Heap heap;

	Heap::handle_type *handles = new Heap::handle_type[numVertices];

	//initialization of the heap
	for (int i = 0; i < numVertices; ++i) {
		if (i == source) {
			handles[i] = heap.push(std::make_pair(i,0.));
			distances[i] = 0.;
		} else {
			handles[i] = heap.push(std::make_pair(i, std::numeric_limits<double>::infinity()));
			distances[i] = std::numeric_limits<double>::infinity();
		}
	}

	property_map<Graph, edge_weight_t>::type weights = get(edge_weight, g);
	property_map<Graph, vertex_index_t>::type index = get(vertex_index, g);

	//the actual algorithm
	while (!heap.empty()) {
		Pair min = heap.top().pair;
		heap.pop();
		for (tie(out_i, out_end) = out_edges(*(vertices(g).first+min.first), g);
				out_i != out_end; ++out_i) {
			double tmp = min.second + weights[*out_i];
			int targetIndex = index[target(*out_i, g)];
			if (tmp < distances[targetIndex]) {
				distances[targetIndex] = tmp;
				predecessors[targetIndex] = min.first;
				(*handles[targetIndex]).pair.second = tmp;
				heap.increase(handles[targetIndex]);
			}
		}
	}

	delete[] handles;
}


/*
 * types.hpp
 *
 *  Created on: Feb 23, 2018
 *      Author: graffy
 */

#ifndef TYPES_HPP_
#define TYPES_HPP_

#include <Eigen/Dense>
namespace scefig
{
	/*
	class Vector3f
	{

	};
	*/

	typedef Eigen::Vector3f Vector3;
	typedef Eigen::Vector4f Vector4;

	class AxisAlignedBoundingBox
	{
	public:
		AxisAlignedBoundingBox(const Vector3 & min, const Vector3 & max)
		: m_min(min)
		, m_max(max)
		{

		}
		const Vector3 & getMin(void) const {return m_min;}
		const Vector3 & getMax(void) const {return m_max;}
	protected:
		Vector3 m_min;
		Vector3 m_max;
	};
}

#endif /* TYPES_HPP_ */


/* +---------------------------------------------------------------------------+
|                     Mobile Robot Programming Toolkit (MRPT)               |
|                          http://www.mrpt.org/                             |
|                                                                           |
| Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
| See: http://www.mrpt.org/Authors - All rights reserved.                   |
| Released under BSD License. See details in http://www.mrpt.org/License    |
+---------------------------------------------------------------------------+ */

#include "vision-precomp.h"   // Precompiled headers

#include <iostream>
#include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
#include <Eigen/Dense>
#include <Eigen/SVD>

#include "ppnp.h"

mrpt::vision::pnp::ppnp::ppnp(const Eigen::MatrixXd& obj_pts, const Eigen::MatrixXd& img_pts, const Eigen::MatrixXd& cam_intrinsic)
{
	P = img_pts;
	S = obj_pts;
	C = cam_intrinsic;
}

bool mrpt::vision::pnp::ppnp::compute_pose(Eigen::Matrix3d& R, Eigen::Vector3d& t, int n)
{
	double tol=0.00001;

	Eigen::MatrixXd I=Eigen::MatrixXd::Identity(n, n), A(n,n), Y(n,3), E(n,3), E_old(n,3), U,V, I3 =Eigen::MatrixXd::Identity(3, 3), PR, Z=Eigen::MatrixXd::Zero(n, n);
	Eigen::VectorXd e(n), II(n), c(3), Zmindiag(n);

	e.fill(1);
	II.fill(1.0/((double)n));

	A=I-e*e.transpose()/n;

	double err=std::numeric_limits<double>::infinity();

	E_old.fill(1000);

    int cnt =0;

	while(err>tol)
	{

		Eigen::JacobiSVD<Eigen::MatrixXd> svd(P.transpose()*Z*A*S, Eigen::ComputeThinU | Eigen::ComputeThinV);
		U=svd.matrixU();
		V=svd.matrixV();

		I3(2,2) = (U*V.transpose()).determinant();
		R=U*I3*V.transpose();
		PR = P*R;

		c=(S-Z*PR).transpose()*II;

		Y=S-e*c.transpose();

		Zmindiag=((PR*Y.transpose()).diagonal()).array() / ((P.array()*P.array()).rowwise().sum()).array();

		for (int i = 0; i < n; i++)
			if (Zmindiag(i) < 0)
				Zmindiag(i) = 0;

		Z=Zmindiag.asDiagonal();

		E=Y-Z*PR;

		err=(E-E_old).norm();

		E_old=E;

        cnt ++;

	}

	t=-R*c;

	return 1;

}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic sine: \f$(e^{x}-e^{-x})/2\f$.

    @par Header <boost/simd/function/sinh.hpp>

    @see tanh, cosh, sech, csch, sinhcosh

    @par Example:

      @snippet sinh.cpp sinh

    @par Possible output:

      @snippet sinh.txt sinh
  **/
  IEEEValue sinh(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sinh.hpp>
#include <boost/simd/function/simd/sinh.hpp>

#endif


#ifndef DTREE_H
#define DTREE_H

#include <iostream>
#include <fstream>  //读取文件内容
#include <map>
#include <vector>
#include <string>
#include <Eigen/Dense>
#include <cmath>

using std::cout; using std::cin; using std::cerr; using std::endl;
using std::string; using std::ifstream; using std::istringstream;
using std::vector; using std::map; using std::ios;

using Eigen::MatrixXf;   //Eigen Matrix float 存储数据  训练数据
using Eigen::MatrixXi;  //Eigen Matrix int 存储数据  训练数据和标签
using Eigen::VectorXi;
using Eigen::VectorXf;

class DTree
{
public:
	struct TreeNode
	{   //节点信息
		string Attribute;   //此节点对应的属性
		bool LeafNode; //如果是叶子节点，此值反映分类结果。 //其他情况都是0;
		vector<TreeNode*> children; //孩子节点的地址。
		map<string, TreeNode*> AttributeLinkChildren;  //属性指向孩子节点，也就是对应的树形结构中的树枝

	};
	struct Attr    //每一列的属性
	{
		int colIndex;
		string Attribute;
		int typeNum;   //属性取值的个数
		vector<string> AttributeValue;
		map<string, unsigned char>  typeMap; //属性取值对应的整数值;
	};

private:
	struct MatInfo
	{
		int cols;
		int rows;

	};
	struct entropyInfo
	{
		vector<int> labelValue;
		vector<int> labelValueNum;
	};
public:
	//MatrixXi trainDataMat;
    MatrixXi trainDataMat;
	vector<vector<string>> predictedDataMat;
	MatInfo trainMatrixInfo;
	TreeNode *root;  //根节点
	vector<Attr> vectorAttr; //存储所有的矩阵信息，但不存储矩阵。
	DTree();
    DTree(string csvfilename);
	MatrixXi ReadTrainDataFile(string fileAddress);  //数据预处理
	//TreeNode* BuildTree(MatrixXi &data, vector<Attr> &dataAttr, string AlgorithmName);  // 指定是哪种算法
    int BuildTree(MatrixXi &data, vector<Attr> &dataAttr, string AlgorithmName);
	vector<vector<string>> ReadPredictedDataFile(string fileAddreess);
	//vector<string> Predicted(TreeNode* root, vector<vector<string>> &pData);  //返回值为int类型表示数据的分类。
    vector<string> Predicted(vector<vector<string>> &pData);
    ~DTree();

private:
    int stringDataToInt(const vector<vector<string>> &src, MatrixXi &dataMat, vector<Attr> &headAttrInfo);
	bool StringExistInVector(string aa, vector<string> A);
	TreeNode* AlgorithmID3(MatrixXi &data, vector<Attr> &a);
	TreeNode* AlgorithmC4_5(MatrixXi &data, vector<Attr> &a);
	TreeNode* AlgorithmCART(MatrixXi &data, vector<Attr> &a);
	//vector<float> CalculateEntropy(MatrixXi a);
	vector<float> CalculateInfGain(MatrixXi &a);
	int FindMaxInformationGain(vector<float> s);
	bool TheSameLabel(MatrixXi &a);
	MatrixXi GetNewMat(MatrixXi &a, vector<Attr> &properties, int maxIndex, string oneAttributeValue);
	int IntExistInVector(int a, vector<int> b);
	float GetDataEntropy(MatrixXi &data);
	float InformationGain(vector<int> value, map<int, entropyInfo> b, float dataEntropy, int rows);
	float Entropy(vector<float> ratio); // 计算熵
	//判断矩阵中是否有该属性
	bool DataExistAttribute(MatrixXi &data, vector<Attr> &properties, int maxIndex, string oneAttributeValue);
	int IndexOFAttribute(string nodeString, vector<Attr> &vectorAttr);
	string PredictedRecursion(TreeNode* nodeAddress, vector<string> &rowData, vector<Attr> &vecAttr);
	string FindAttrString(int a, Attr b);
	string MostInMatLabel(MatrixXi &data, vector<Attr> &properties);
    int destroyTree(TreeNode *root);
};

#endif


//  (C) Copyright Nick Thompson 2017.
//  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)

#ifndef BOOST_MATH_SPECIAL_CHEBYSHEV_HPP
#define BOOST_MATH_SPECIAL_CHEBYSHEV_HPP
#include <cmath>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/promotion.hpp>

#if (__cplusplus > 201103) || (defined(_CPPLIB_VER) && (_CPPLIB_VER >= 610))
#  define BOOST_MATH_CHEB_USE_STD_ACOSH
#endif

#ifndef BOOST_MATH_CHEB_USE_STD_ACOSH
#  include <boost/math/special_functions/acosh.hpp>
#endif

namespace boost { namespace math {

template<class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type chebyshev_next(T1 const & x, T2 const & Tn, T3 const & Tn_1)
{
    return 2*x*Tn - Tn_1;
}

namespace detail {

template<class Real, bool second, class Policy>
inline Real chebyshev_imp(unsigned n, Real const & x, const Policy&)
{
#ifdef BOOST_MATH_CHEB_USE_STD_ACOSH
    using std::acosh;
#define BOOST_MATH_ACOSH_POLICY
#else
   using boost::math::acosh;
#define BOOST_MATH_ACOSH_POLICY , Policy()
#endif
    using std::cosh;
    using std::pow;
    using std::sqrt;
    Real T0 = 1;
    Real T1;
    if (second)
    {
        if (x > 1 || x < -1)
        {
            Real t = sqrt(x*x -1);
            return static_cast<Real>((pow(x+t, (int)(n+1)) - pow(x-t, (int)(n+1)))/(2*t));
        }
        T1 = 2*x;
    }
    else
    {
        if (x > 1)
        {
            return cosh(n*acosh(x BOOST_MATH_ACOSH_POLICY));
        }
        if (x < -1)
        {
            if (n & 1)
            {
                return -cosh(n*acosh(-x BOOST_MATH_ACOSH_POLICY));
            }
            else
            {
                return cosh(n*acosh(-x BOOST_MATH_ACOSH_POLICY));
            }
        }
        T1 = x;
    }

    if (n == 0)
    {
        return T0;
    }

    unsigned l = 1;
    while(l < n)
    {
       std::swap(T0, T1);
       T1 = boost::math::chebyshev_next(x, T0, T1);
       ++l;
    }
    return T1;
}
} // namespace detail

template <class Real, class Policy>
inline typename tools::promote_args<Real>::type
chebyshev_t(unsigned n, Real const & x, const Policy&)
{
   typedef typename tools::promote_args<Real>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::chebyshev_imp<value_type, false>(n, static_cast<value_type>(x), forwarding_policy()), "boost::math::chebyshev_t<%1%>(unsigned, %1%)");
}

template<class Real>
inline typename tools::promote_args<Real>::type chebyshev_t(unsigned n, Real const & x)
{
    return chebyshev_t(n, x, policies::policy<>());
}

template <class Real, class Policy>
inline typename tools::promote_args<Real>::type
chebyshev_u(unsigned n, Real const & x, const Policy&)
{
   typedef typename tools::promote_args<Real>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::chebyshev_imp<value_type, true>(n, static_cast<value_type>(x), forwarding_policy()), "boost::math::chebyshev_u<%1%>(unsigned, %1%)");
}

template<class Real>
inline typename tools::promote_args<Real>::type chebyshev_u(unsigned n, Real const & x)
{
    return chebyshev_u(n, x, policies::policy<>());
}

template <class Real, class Policy>
inline typename tools::promote_args<Real>::type
chebyshev_t_prime(unsigned n, Real const & x, const Policy&)
{
   typedef typename tools::promote_args<Real>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   if (n == 0)
   {
      return result_type(0);
   }
   return policies::checked_narrowing_cast<result_type, Policy>(n * detail::chebyshev_imp<value_type, true>(n - 1, static_cast<value_type>(x), forwarding_policy()), "boost::math::chebyshev_t_prime<%1%>(unsigned, %1%)");
}

template<class Real>
inline typename tools::promote_args<Real>::type chebyshev_t_prime(unsigned n, Real const & x)
{
   return chebyshev_t_prime(n, x, policies::policy<>());
}

/*
 * This is Algorithm 3.1 of
 * Gil, Amparo, Javier Segura, and Nico M. Temme.
 * Numerical methods for special functions.
 * Society for Industrial and Applied Mathematics, 2007.
 * https://www.siam.org/books/ot99/OT99SampleChapter.pdf
 * However, our definition of c0 differs by a factor of 1/2, as stated in the docs. . .
 */
template<class Real, class T2>
inline Real chebyshev_clenshaw_recurrence(const Real* const c, size_t length, const T2& x)
{
    using boost::math::constants::half;
    if (length < 2)
    {
        if (length == 0)
        {
            return 0;
        }
        return c[0]/2;
    }
    Real b2 = 0;
    Real b1 = c[length -1];
    for(size_t j = length - 2; j >= 1; --j)
    {
        Real tmp = 2*x*b1 - b2 + c[j];
        b2 = b1;
        b1 = tmp;
    }
    return x*b1 - b2 + half<Real>()*c[0];
}

namespace detail {
template<class Real>
inline Real unchecked_chebyshev_clenshaw_recurrence(const Real* const c, size_t length, const Real & a, const Real & b, const Real& x)
{
    Real t;
    Real u;
    // This cutoff is not super well defined, but it's a good estimate.
    // See "An Error Analysis of the Modified Clenshaw Method for Evaluating Chebyshev and Fourier Series"
    // J. OLIVER, IMA Journal of Applied Mathematics, Volume 20, Issue 3, November 1977, Pages 379–391
    // https://doi.org/10.1093/imamat/20.3.379
    const Real cutoff = 0.6;
    if (x - a < b - x)
    {
        u = 2*(x-a)/(b-a);
        t = u - 1;
        if (t > -cutoff)
        {
            Real b2 = 0;
            Real b1 = c[length -1];
            for(size_t j = length - 2; j >= 1; --j)
            {
                Real tmp = 2*t*b1 - b2 + c[j];
                b2 = b1;
                b1 = tmp;
            }
            return t*b1 - b2 + c[0]/2;
        }
        else
        {
            Real b = c[length -1];
            Real d = b;
            Real b2 = 0;
            for (size_t r = length - 2; r >= 1; --r)
            {
                d = 2*u*b - d + c[r];
                b2 = b;
                b = d - b;
            }
            return t*b - b2 + c[0]/2;
        }
    }
    else
    {
        u = -2*(b-x)/(b-a);
        t = u + 1;
        if (t < cutoff)
        {
            Real b2 = 0;
            Real b1 = c[length -1];
            for(size_t j = length - 2; j >= 1; --j)
            {
                Real tmp = 2*t*b1 - b2 + c[j];
                b2 = b1;
                b1 = tmp;
            }
            return t*b1 - b2 + c[0]/2;
        }
        else
        {
            Real b = c[length -1];
            Real d = b;
            Real b2 = 0;
            for (size_t r = length - 2; r >= 1; --r)
            {
                d = 2*u*b + d + c[r];
                b2 = b;
                b = d + b;
            }
            return t*b - b2 + c[0]/2;
        }
    }
}

} // namespace detail

template<class Real>
inline Real chebyshev_clenshaw_recurrence(const Real* const c, size_t length, const Real & a, const Real & b, const Real& x)
{
    if (x < a || x > b)
    {
        throw std::domain_error("x in [a, b] is required.");
    }
    if (length < 2)
    {
        if (length == 0)
        {
            return 0;
        }
        return c[0]/2;
    }
    return detail::unchecked_chebyshev_clenshaw_recurrence(c, length, a, b, x);
}

}}
#endif


/*
 * Copyright 2020 Adobe. All rights reserved.
 * This file is licensed to you under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License. You may obtain a copy
 * of the License at http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software distributed under
 * the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS
 * OF ANY KIND, either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */
#include <cmath>
#include <iostream>

#include <lagrange/testing/common.h>

#include <Eigen/Geometry>

#include <lagrange/common.h>
#include <lagrange/compute_mesh_centroid.h>
#include <lagrange/create_mesh.h>
#include <lagrange/utils/safe_cast.h>

TEST_CASE("ComputeMeshCentroid", "[mesh][centroid]")
{
    using namespace lagrange;

    Vertices3D ref_vertices(6 + 3, 3);
    Triangles facets(4 + 1, 3);

    const double a = 0.5;
    const double b = 2.0;
    const double large_number = 1000;

    ref_vertices.row(0) << -a / 2., -b / 2., 0;
    ref_vertices.row(1) << +a / 2., -b / 2., 0;
    ref_vertices.row(2) << +a / 2., 0, 0;
    ref_vertices.row(3) << +a / 2., b / 4., 0;
    ref_vertices.row(4) << +a / 2., b / 2., 0;
    ref_vertices.row(5) << -a / 2., b / 2., 0;
    // Don't include in computation
    ref_vertices.row(6) << large_number, large_number, large_number;
    ref_vertices.row(7) << -large_number, -large_number, -large_number;
    ref_vertices.row(8) << 2 * large_number, 2 * large_number, 2 * large_number;

    facets.row(0) << 0, 1, 2;
    // Don't include in computation
    facets.row(1) << 6, 7, 8;
    //
    facets.row(2) << 0, 2, 3;
    facets.row(3) << 0, 3, 4;
    facets.row(4) << 0, 4, 5;

    // Reference values without transformations
    const double ref_area = a * b;
    Eigen::Vector3d ref_center(0, 0, 0);

    // Translate
    Eigen::Vector3d tr(-1, 3, 4);
    Eigen::Matrix3d rot =
        Eigen::AngleAxisd(1.2365, Eigen::Vector3d(-1, 2, 5.1).normalized()).toRotationMatrix();

    // Create the vertices
    Vertices3D vertices = (ref_vertices * rot.transpose()).rowwise() + tr.transpose();

    auto mesh_unique = lagrange::create_mesh(vertices, facets);
    auto out = compute_mesh_centroid(*mesh_unique, {0, 2, 3, 4});

    CHECK(out.area == Approx(ref_area));
    CHECK((out.centroid.transpose() - ref_center - tr).norm() == Approx(0.).margin(1e-10));

} // end of TEST


// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.

#include "../precomp.hpp"
#include "../usac.hpp"
#if defined(HAVE_EIGEN)
#include <Eigen/Eigen>
#elif defined(HAVE_LAPACK)
#include "opencv_lapack.h"
#endif

namespace cv { namespace usac {
// Essential matrix solver:
/*
* H. Stewenius, C. Engels, and D. Nister. Recent developments on direct relative orientation.
* ISPRS J. of Photogrammetry and Remote Sensing, 60:284,294, 2006
* http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.61.9329&rep=rep1&type=pdf
*/
class EssentialMinimalSolverStewenius5ptsImpl : public EssentialMinimalSolverStewenius5pts {
private:
    // Points must be calibrated K^-1 x
    const Mat * points_mat;
#if defined(HAVE_EIGEN) || defined(HAVE_LAPACK)
    const float * const pts;
#endif
public:
    explicit EssentialMinimalSolverStewenius5ptsImpl (const Mat &points_) :
        points_mat(&points_)
#if defined(HAVE_EIGEN) || defined(HAVE_LAPACK)
        , pts((float*)points_.data)
#endif
        {}

#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
    int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const override {
        // (1) Extract 4 null vectors from linear equations of epipolar constraint
        std::vector<double> coefficients(45); // 5 pts=rows, 9 columns
        auto *coefficients_ = &coefficients[0];
        for (int i = 0; i < 5; i++) {
            const int smpl = 4 * sample[i];
            const auto x1 = pts[smpl], y1 = pts[smpl+1], x2 = pts[smpl+2], y2 = pts[smpl+3];
            (*coefficients_++) = x2 * x1;
            (*coefficients_++) = x2 * y1;
            (*coefficients_++) = x2;
            (*coefficients_++) = y2 * x1;
            (*coefficients_++) = y2 * y1;
            (*coefficients_++) = y2;
            (*coefficients_++) = x1;
            (*coefficients_++) = y1;
            (*coefficients_++) = 1;
        }

        const int num_cols = 9, num_e_mat = 4;
        double ee[36]; // 9*4
        // eliminate linear equations
        if (!Math::eliminateUpperTriangular(coefficients, 5, num_cols))
            return 0;
        for (int i = 0; i < num_e_mat; i++)
            for (int j = 5; j < num_cols; j++)
                ee[num_cols * i + j] = (i + 5 == j) ? 1 : 0;
        // use back-substitution
        for (int e = 0; e < num_e_mat; e++) {
            const int curr_e = num_cols * e;
            // start from the last row
            for (int i = 4; i >= 0; i--) {
                const int row_i = i * num_cols;
                double acc = 0;
                for (int j = i + 1; j < num_cols; j++)
                    acc -= coefficients[row_i + j] * ee[curr_e + j];
                ee[curr_e + i] = acc / coefficients[row_i + i];
                // due to numerical errors return 0 solutions
                if (std::isnan(ee[curr_e + i]))
                    return 0;
            }
        }

        const Matx<double, 4, 9> null_space(ee);
        const Matx<double, 4, 1> null_space_mat[3][3] = {
                {null_space.col(0), null_space.col(3), null_space.col(6)},
                {null_space.col(1), null_space.col(4), null_space.col(7)},
                {null_space.col(2), null_space.col(5), null_space.col(8)}};

        // (2) Use the rank constraint and the trace constraint to build ten third-order polynomial
        // equations in the three unknowns. The monomials are ordered in GrLex order and
        // represented in a 10×20 matrix, where each row corresponds to an equation and each column
        // corresponds to a monomial
        Matx<double, 1, 10> eet[3][3];
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                // compute EE Transpose
                // Shorthand for multiplying the Essential matrix with its transpose.
                eet[i][j] = 2 * (multPolysDegOne(null_space_mat[i][0].val, null_space_mat[j][0].val) +
                                 multPolysDegOne(null_space_mat[i][1].val, null_space_mat[j][1].val) +
                                 multPolysDegOne(null_space_mat[i][2].val, null_space_mat[j][2].val));

        const Matx<double, 1, 10> trace = eet[0][0] + eet[1][1] + eet[2][2];
        Mat_<double> constraint_mat(10, 20);
        // Trace constraint
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                Mat(multPolysDegOneAndTwo(eet[i][0].val, null_space_mat[0][j].val) +
                    multPolysDegOneAndTwo(eet[i][1].val, null_space_mat[1][j].val) +
                    multPolysDegOneAndTwo(eet[i][2].val, null_space_mat[2][j].val) -
                    0.5 * multPolysDegOneAndTwo(trace.val, null_space_mat[i][j].val))
                        .copyTo(constraint_mat.row(3 * i + j));

        // Rank = zero determinant constraint
        Mat(multPolysDegOneAndTwo(
                (multPolysDegOne(null_space_mat[0][1].val, null_space_mat[1][2].val) -
                 multPolysDegOne(null_space_mat[0][2].val, null_space_mat[1][1].val)).val,
                 null_space_mat[2][0].val) +
                multPolysDegOneAndTwo(
                    (multPolysDegOne(null_space_mat[0][2].val, null_space_mat[1][0].val) -
                     multPolysDegOne(null_space_mat[0][0].val, null_space_mat[1][2].val)).val,
                     null_space_mat[2][1].val) +
                multPolysDegOneAndTwo(
                    (multPolysDegOne(null_space_mat[0][0].val, null_space_mat[1][1].val) -
                     multPolysDegOne(null_space_mat[0][1].val, null_space_mat[1][0].val)).val,
                     null_space_mat[2][2].val)).copyTo(constraint_mat.row(9));

#ifdef HAVE_EIGEN
        const Eigen::Matrix<double, 10, 20, Eigen::RowMajor> constraint_mat_eig((double *) constraint_mat.data);
        // (3) Compute the Gröbner basis. This turns out to be as simple as performing a
        // Gauss-Jordan elimination on the 10×20 matrix
        const Eigen::Matrix<double, 10, 10> eliminated_mat_eig = constraint_mat_eig.block<10, 10>(0, 0)
                .fullPivLu().solve(constraint_mat_eig.block<10, 10>(0, 10));

        // (4) Compute the 10×10 action matrix for multiplication by one of the un-knowns.
        // This is a simple matter of extracting the correct elements fromthe eliminated
        // 10×20 matrix and organising them to form the action matrix.
        Eigen::Matrix<double, 10, 10> action_mat_eig = Eigen::Matrix<double, 10, 10>::Zero();
        action_mat_eig.block<3, 10>(0, 0) = eliminated_mat_eig.block<3, 10>(0, 0);
        action_mat_eig.block<2, 10>(3, 0) = eliminated_mat_eig.block<2, 10>(4, 0);
        action_mat_eig.row(5) = eliminated_mat_eig.row(7);
        action_mat_eig(6, 0) = -1.0;
        action_mat_eig(7, 1) = -1.0;
        action_mat_eig(8, 3) = -1.0;
        action_mat_eig(9, 6) = -1.0;

        // (5) Compute the left eigenvectors of the action matrix
        Eigen::EigenSolver<Eigen::Matrix<double, 10, 10>> eigensolver(action_mat_eig);
        const Eigen::VectorXcd &eigenvalues = eigensolver.eigenvalues();
        const auto * const eig_vecs_ = (double *) eigensolver.eigenvectors().real().data();
#else
        Matx<double, 10, 10> A = constraint_mat.colRange(0, 10),
                         B = constraint_mat.colRange(10, 20), eliminated_mat;
        if (!solve(A, B, eliminated_mat, DECOMP_LU)) return 0;

        Mat eliminated_mat_dyn = Mat(eliminated_mat);
        Mat action_mat = Mat_<double>::zeros(10, 10);
        eliminated_mat_dyn.rowRange(0,3).copyTo(action_mat.rowRange(0,3));
        eliminated_mat_dyn.rowRange(4,6).copyTo(action_mat.rowRange(3,5));
        eliminated_mat_dyn.row(7).copyTo(action_mat.row(5));
        auto * action_mat_data = (double *) action_mat.data;
        action_mat_data[60] = -1.0; // 6 row, 0 col
        action_mat_data[71] = -1.0; // 7 row, 1 col
        action_mat_data[83] = -1.0; // 8 row, 3 col
        action_mat_data[96] = -1.0; // 9 row, 6 col

        int mat_order = 10, info, lda = 10, ldvl = 10, ldvr = 1, lwork = 100;
        double wr[10], wi[10] = {0}, eig_vecs[100], work[100]; // 10 = mat_order, 100 = lwork
        char jobvl = 'V', jobvr = 'N'; // only left eigen vectors are computed
        dgeev_(&jobvl, &jobvr, &mat_order, action_mat_data, &lda, wr, wi, eig_vecs, &ldvl,
                nullptr, &ldvr, work, &lwork, &info);
        if (info != 0) return 0;
#endif

        models = std::vector<Mat>(); models.reserve(10);

        // Read off the values for the three unknowns at all the solution points and
        // back-substitute to obtain the solutions for the essential matrix.
        for (int i = 0; i < 10; i++)
            // process only real solutions
#ifdef HAVE_EIGEN
            if (eigenvalues(i).imag() == 0) {
                Mat_<double> model(3, 3);
                auto * model_data = (double *) model.data;
                const int eig_i = 20 * i + 12; // eigen stores imaginary values too
                for (int j = 0; j < 9; j++)
                    model_data[j] = ee[j   ] * eig_vecs_[eig_i  ] + ee[j+9 ] * eig_vecs_[eig_i+2] +
                                    ee[j+18] * eig_vecs_[eig_i+4] + ee[j+27] * eig_vecs_[eig_i+6];
#else
            if (wi[i] == 0) {
                Mat_<double> model (3,3);
                auto * model_data = (double *) model.data;
                const int eig_i = 10 * i + 6;
                for (int j = 0; j < 9; j++)
                    model_data[j] = ee[j   ]*eig_vecs[eig_i  ] + ee[j+9 ]*eig_vecs[eig_i+1] +
                                    ee[j+18]*eig_vecs[eig_i+2] + ee[j+27]*eig_vecs[eig_i+3];
#endif
                models.emplace_back(model);
            }
        return static_cast<int>(models.size());
#else
    int estimate (const std::vector<int> &/*sample*/, std::vector<Mat> &/*models*/) const override {
        CV_Error(cv::Error::StsNotImplemented, "To use essential matrix solver LAPACK or Eigen has to be installed!");
#endif
    }

    // number of possible solutions is 0,2,4,6,8,10
    int getMaxNumberOfSolutions () const override { return 10; }
    int getSampleSize() const override { return 5; }
    Ptr<MinimalSolver> clone () const override {
        return makePtr<EssentialMinimalSolverStewenius5ptsImpl>(*points_mat);
    }
private:
    /*
     * Multiply two polynomials of degree one with unknowns x y z
     * @p = (p1 x + p2 y + p3 z + p4) [p1 p2 p3 p4]
     * @q = (q1 x + q2 y + q3 z + q4) [q1 q2 q3 a4]
     * @result is a new polynomial in x^2 xy y^2 xz yz z^2 x y z 1 of size 10
     */
    static inline Matx<double,1,10> multPolysDegOne(const double * const p,
                                                    const double * const q) {
        return
            {p[0]*q[0], p[0]*q[1]+p[1]*q[0], p[1]*q[1], p[0]*q[2]+p[2]*q[0], p[1]*q[2]+p[2]*q[1],
             p[2]*q[2], p[0]*q[3]+p[3]*q[0], p[1]*q[3]+p[3]*q[1], p[2]*q[3]+p[3]*q[2], p[3]*q[3]};
    }

    /*
     * Multiply two polynomials with unknowns x y z
     * @p is of size 10 and @q is of size 4
     * @p = (p1 x^2 + p2 xy + p3 y^2 + p4 xz + p5 yz + p6 z^2 + p7 x + p8 y + p9 z + p10)
     * @q = (q1 x + q2 y + q3 z + a4) [q1 q2 q3 q4]
     * @result is a new polynomial of size 20
     * x^3 x^2y xy^2 y^3 x^2z xyz y^2z xz^2 yz^2 z^3 x^2 xy y^2 xz yz z^2 x y z 1
     */
    static inline Matx<double, 1, 20> multPolysDegOneAndTwo(const double * const p,
                                                            const double * const q) {
        return Matx<double, 1, 20>
           ({p[0]*q[0], p[0]*q[1]+p[1]*q[0], p[1]*q[1]+p[2]*q[0], p[2]*q[1], p[0]*q[2]+p[3]*q[0],
                  p[1]*q[2]+p[3]*q[1]+p[4]*q[0], p[2]*q[2]+p[4]*q[1], p[3]*q[2]+p[5]*q[0],
                  p[4]*q[2]+p[5]*q[1], p[5]*q[2], p[0]*q[3]+p[6]*q[0], p[1]*q[3]+p[6]*q[1]+p[7]*q[0],
                  p[2]*q[3]+p[7]*q[1], p[3]*q[3]+p[6]*q[2]+p[8]*q[0], p[4]*q[3]+p[7]*q[2]+p[8]*q[1],
                  p[5]*q[3]+p[8]*q[2], p[6]*q[3]+p[9]*q[0], p[7]*q[3]+p[9]*q[1], p[8]*q[3]+p[9]*q[2],
                  p[9]*q[3]});
    }
};
Ptr<EssentialMinimalSolverStewenius5pts> EssentialMinimalSolverStewenius5pts::create
        (const Mat &points_) {
    return makePtr<EssentialMinimalSolverStewenius5ptsImpl>(points_);
}

class EssentialNonMinimalSolverImpl : public EssentialNonMinimalSolver {
private:
    const Mat * points_mat;
    const float * const points;
public:
    /*
     * Input calibrated points K^-1 x.
     * Linear 8 points algorithm is used for estimation.
     */
    explicit EssentialNonMinimalSolverImpl (const Mat &points_) :
        points_mat(&points_), points ((float *) points_.data) {}

    int estimate (const std::vector<int> &sample, int sample_size, std::vector<Mat>
            &models, const std::vector<double> &weights) const override {
        if (sample_size < getMinimumRequiredSampleSize())
            return 0;

        // ------- 8 points algorithm with Eigen and covariance matrix --------------
        double a[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1};
        double AtA[81] = {0}; // 9x9

        if (weights.empty()) {
            for (int i = 0; i < sample_size; i++) {
                const int pidx = 4*sample[i];
                const double x1 = points[pidx  ], y1 = points[pidx+1],
                             x2 = points[pidx+2], y2 = points[pidx+3];
                a[0] = x2*x1;
                a[1] = x2*y1;
                a[2] = x2;
                a[3] = y2*x1;
                a[4] = y2*y1;
                a[5] = y2;
                a[6] = x1;
                a[7] = y1;

                // calculate covariance for eigen
                for (int row = 0; row < 9; row++)
                    for (int col = row; col < 9; col++)
                        AtA[row*9+col] += a[row]*a[col];
            }
        } else {
            for (int i = 0; i < sample_size; i++) {
                const int smpl = 4*sample[i];
                const double weight = weights[i];
                const double x1 = points[smpl  ], y1 = points[smpl+1],
                             x2 = points[smpl+2], y2 = points[smpl+3];
                const double weight_times_x2 = weight * x2,
                             weight_times_y2 = weight * y2;

                a[0] = weight_times_x2 * x1;
                a[1] = weight_times_x2 * y1;
                a[2] = weight_times_x2;
                a[3] = weight_times_y2 * x1;
                a[4] = weight_times_y2 * y1;
                a[5] = weight_times_y2;
                a[6] = weight * x1;
                a[7] = weight * y1;
                a[8] = weight;

                // calculate covariance for eigen
                for (int row = 0; row < 9; row++)
                    for (int col = row; col < 9; col++)
                        AtA[row*9+col] += a[row]*a[col];
            }
        }

        // copy symmetric part of covariance matrix
        for (int j = 1; j < 9; j++)
            for (int z = 0; z < j; z++)
                AtA[j*9+z] = AtA[z*9+j];

#ifdef HAVE_EIGEN
        models = std::vector<Mat>{ Mat_<double>(3,3) };
        const Eigen::JacobiSVD<Eigen::Matrix<double, 9, 9>> svd((Eigen::Matrix<double, 9, 9>(AtA)),
                Eigen::ComputeFullV);
        // extract the last nullspace
        Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)models[0].data) = svd.matrixV().col(8);
#else
        Matx<double, 9, 9> AtA_(AtA), U, Vt;
        Vec<double, 9> W;
        SVD::compute(AtA_, W, U, Vt, SVD::FULL_UV + SVD::MODIFY_A);
        models = std::vector<Mat> { Mat_<double>(3, 3, Vt.val + 72 /*=8*9*/) };
#endif
        FundamentalDegeneracy::recoverRank(models[0], false /*E*/);
        return 1;
    }
    int getMinimumRequiredSampleSize() const override { return 8; }
    int getMaxNumberOfSolutions () const override { return 1; }
    Ptr<NonMinimalSolver> clone () const override {
        return makePtr<EssentialNonMinimalSolverImpl>(*points_mat);
    }
};
Ptr<EssentialNonMinimalSolver> EssentialNonMinimalSolver::create (const Mat &points_) {
    return makePtr<EssentialNonMinimalSolverImpl>(points_);
}
}}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REM_PIO2_MEDIUM_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REM_PIO2_MEDIUM_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object computes the remainder modulo
    \f$\pi/2\f$ with medium algorithm, and the angle quadrant between 0 and 3.

    @par Header <boost/simd/function/rem_pio2_medium.hpp>

    @par Note:

    - This is a medium version accurate if the input is in:
     \f$[-2^6\pi,2^6\pi]\f$ for float,
     \f$[-2^{18}\pi,2^{18}\pi]\f$ for double.
    \par

    The reduction of the argument modulo \f$\pi/2\f$ is generally
    the most difficult part of trigonometric evaluations.
    The accurate algorithm @ref rem_pio2 is over costly and implies the knowledge
    of a few hundred \f$\pi\f$ decimals
    some simpler algorithms as this one
    can be used, but the precision is only insured on smaller intervals.

    @see rem_pio2, rem_pio2_straight,rem_2pi,  rem_pio2_cephes,

    @par Example:

      @snippet rem_pio2_medium.cpp rem_pio2_medium

    @par Possible output:

      @snippet rem_pio2_medium.txt rem_pio2_medium

  **/
  std::pair<IEEEValue, IEEEValue> rem_pio2_medium(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/rem_pio2_medium.hpp>
#include <boost/simd/function/simd/rem_pio2_medium.hpp>

#endif