#include <iostream>
#include <functional>
#include <type_traits>

#include <boost/range/iterator_range.hpp>

#include <amgcl/amg.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/mpi/direct_solver/runtime.hpp>
#include <amgcl/mpi/subdomain_deflation.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/adapter/crs_tuple.hpp>

#include "amgcl_mpi.h"

//---------------------------------------------------------------------------
typedef amgcl::backend::builtin<double>                   Backend;
typedef boost::property_tree::ptree                       Params;

typedef
    amgcl::mpi::subdomain_deflation<
        amgcl::amg<Backend, amgcl::runtime::coarsening::wrapper, amgcl::runtime::relaxation::wrapper>,
        amgcl::runtime::solver::wrapper,
        amgcl::runtime::mpi::direct::solver<double>
    > Solver;

//---------------------------------------------------------------------------
struct deflation_vectors {
    int n;
    amgclDefVecFunction user_func;
    void *user_data;

    deflation_vectors(int n, amgclDefVecFunction user_func, void *user_data)
        : n(n), user_func(user_func), user_data(user_data)
    {}

    int dim() const { return n; }

    double operator()(int i, ptrdiff_t j) const {
        return user_func(i, j, user_data);
    }
};

//---------------------------------------------------------------------------
amgclHandle STDCALL amgcl_mpi_create(
        MPI_Comm             comm,
        ptrdiff_t            n,
        const ptrdiff_t     *ptr,
        const ptrdiff_t     *col,
        const double        *val,
        int                  n_def_vec,
        amgclDefVecFunction  def_vec_func,
        void                *def_vec_data,
        amgclHandle          params
        )
{
    std::function<double(ptrdiff_t, unsigned)> dv = deflation_vectors(n_def_vec, def_vec_func, def_vec_data);
    boost::property_tree::ptree prm = *static_cast<Params*>(params);
    prm.put("num_def_vec", n_def_vec);
    prm.put("def_vec",     &dv);

    return static_cast<amgclHandle>(
            new Solver(
                comm,
                std::make_tuple(
                    n,
                    boost::make_iterator_range(ptr, ptr + n + 1),
                    boost::make_iterator_range(col, col + ptr[n]),
                    boost::make_iterator_range(val, val + ptr[n])
                    ),
                prm
                )
            );
}

//---------------------------------------------------------------------------
conv_info STDCALL amgcl_mpi_solve(
        amgclHandle   handle,
        double const *rhs,
        double       *x
        )
{
    Solver *solver = static_cast<Solver*>(handle);

    size_t n = solver->size();

    boost::iterator_range<double*> x_range =
        boost::make_iterator_range(x, x + n);

    conv_info cnv;

    std::tie(cnv.iterations, cnv.residual) = (*solver)(
            boost::make_iterator_range(rhs, rhs + n), x_range
            );

    return cnv;
}

//---------------------------------------------------------------------------
void STDCALL amgcl_mpi_destroy(amgclHandle handle) {
    delete static_cast<Solver*>(handle);
}


//----------------------------------------------------------------------------------------------
// This file is part of the HAMLS software.
//
// MIT License
//
// Copyright (c) 2019 Peter Gerds
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//----------------------------------------------------------------------------------------------

// Project     : HAMLS
// File        : eigensolver.cc
// Description : example for H-AMLS methopd applied to an elliptic PDE eigenvalue problem
// Author      : Peter Gerds
// Copyright   : Peter Gerds, 2019. All Rights Reserved.

#include <iostream>
#include <hlib.hh>

#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include "hamls/TEigenArnoldi.hh"
#include "hamls/TEigenArpack.hh"
#include "hamls/THAMLS.hh"

using namespace std;
using namespace HLIB;
using namespace boost::filesystem;
using namespace boost::program_options;
using HLIB::uint;
using boost::format;

using std::cout;
using std::endl;
using std::unique_ptr;
using std::string;

using  real_t    = HLIB::real;
using  complex_t = HLIB::complex;

using namespace HAMLS;

namespace
{

template < typename T >
string
mem_per_dof ( T && A )
{
    const size_t  mem  = A->byte_size();
    const size_t  pdof = size_t( double(mem) / double(A->rows()) );

    return Mem::to_string( mem ) + " (" + Mem::to_string( pdof ) + "/dof)";
}

//
// Print some general informations of the given sparse matrix
//
TSparseMatrix *
import_matrix ( const std::string fmatrix )
{
    cout<<endl;
    cout<<"  importing sparse matrix from file \'"<<fmatrix<<"\'"<<endl;

    THLibMatrixIO   matrix_io;

    unique_ptr< TMatrix >   K( matrix_io.read( fmatrix ) );

    if ( K.get() == nullptr )
    {
        cout<<endl<<"  warning: no matrix found" << endl;
        exit( 1 );
    }// if

    if ( ! IS_TYPE( K.get(), TSparseMatrix ) )
    {
        // try to convert to sparse
        cout<<endl<<"  warning: matrix is of type "<<K->typestr()<<" --> converting to sparse format";

        auto  T = to_sparse( K.get() );

        if ( T.get() != nullptr )
            K = std::move( T );
        else
            exit( 1 );
    }// if

    TSparseMatrix * S = ptrcast( K.release(), TSparseMatrix );

    if ( S->rows() != S->cols() )
    {
        cout<<endl<<"  warning: matrix not quadratic" << endl;
        exit( 1 );
    }// if

    if ( S->test_symmetry() )
        S->set_form( symmetric );

    cout << "  matrix has dimension " << S->rows() << " x " << S->cols() << endl;
    cout << "    no of non-zeroes    = " << S->n_non_zero() << " ("
            << format( "%.2f" ) % ( 1000.0 * double(S->n_non_zero()) / Math::square(double(S->rows())) ) << "‰"
            << ", avg=" << S->avg_entries_per_row()
            << ", max=" << S->max_entries_per_row()
            << ")" << endl;
    cout << "    matrix is             " << ( S->is_complex() ? "complex" : "real" ) << " valued" << endl;
    cout << "    format              = ";
    if      ( S->is_unsymmetric() ) cout << "unsymmetric" << endl;
    else if ( S->is_symmetric()   ) cout << "symmetric" << endl;
    else if ( S->is_hermitian()   ) cout << "hermitian" << endl;
    cout << "  size of sparse matrix = " << mem_per_dof( S ) << endl;
    cout << "  |S|_F                 = " << format( "%.6e" ) % norm_F( S ) << endl;

    return S;
}

}// namespace anonymous

//
//
// main function
//
//
int
main ( int argc, char ** argv )
{
    //=====================================================================
    // program options
    //=====================================================================

    //---------------------------------------------------------
    //       general options
    //---------------------------------------------------------
    int     input_hlib_verbosity = 1;
    real_t  input_eta            = real_t(50);
    uint    input_nmin           = 0;
    size_t  input_n_ev          = 5;
    int     input_nthreads      = 0;
    string  input_data_path     = "/home/user/";
    bool    input_print_solver_info = false;
    //---------------------------------------------------------
    //       basic HAMLS options
    //---------------------------------------------------------
    real_t  input_rel_accuracy_transform = real_t(1e-8);
    uint    input_max_dof_subdomain      = 1000;
    //---------------------------------------------------------
    //       options for mode selection options applied in HAMLS
    //---------------------------------------------------------
    real_t   input_c_subdomain   = real_t(1.5);
    real_t   input_c_interface   = real_t(1.0);
    real_t   input_c_condense    = real_t(6.0);
    real_t   input_e_subdomain   = real_t(1)/real_t(3.0);
    real_t   input_e_interface   = real_t(1)/real_t(2.0);
    //---------------------------------------------------------
    //       options for classical iterativ eigensolver
    //---------------------------------------------------------
    real_t   input_shift              = real_t(0);

    //=====================================================================
    // define command line options
    //=====================================================================

    // define command line options
    options_description             all_opts;
    options_description             vis_opts( "usage: eigensolver [options] data_path\n  where options include" );
    options_description             general_opts( "General options", 0 );
    options_description             HAMLS_opts( "Basic options of H-AMLS", 0 );
    options_description             modalTrunc_opts( "Options for the modal trunction applied in H-AMLS", 0 );
    options_description             arnoldi_opts( "Options for the classical iterativ eigensolver", 0 );
    options_description             hid_opts( "Hidden options", 0 );
    positional_options_description  pos_opts;
    variables_map                   vm;

    // general options
    general_opts.add_options()
        ( "help,h",                             ": print this help text" )
        ( "hlib_verbosity",    value<int>(),    ": HLIBpro verbosity level" )
        ( "eta",               value<real_t>(), ": set eta for admissible condition" )
        ( "nmin",              value<uint>(),   ": set minimal cluster size" )
        ( "n_ev",              value<size_t>(), ": set the number of sought eigenpairs" )
        ( "threads",           value<int>(),    ": number of threads used for parallel computation (if set to 0 all available threads are used)" )
        ( "print_solver_info",                  ": print basic information regarding eigensolver execution to logfile" );
    // basic options of HAMLS
    HAMLS_opts.add_options()
        ( "rel_accuracy_transform", value<real_t>(), ": relative accurcy of H-matrix arithmetic used for the problem transformation in H-AMLS" )
        ( "max_dof_subdomain",  value<uint>(),       ": maximal degrees of freedom used for the subdomain problems applied in the matrix partitioning" );
    // options for mode selection applied in HAMLS
    modalTrunc_opts.add_options()
        ( "c_subdomain",  value<real_t>(),      ": select k_i smallest eigenpairs of subdomain problem with size N_i where k_i := c_subdomain (N_i)^(e_subdomain)" )
        ( "e_subdomain",value<real_t>(),        ": select k_i smallest eigenpairs of subdomain problem with size N_i where k_i := c_subdomain (N_i)^(e_subdomain)" )
        ( "c_interface",  value<real_t>(),      ": select k_i smallest eigenpairs of interface problem with size N_i where k_i := c_interface (N_i)^(e_interface)" )
        ( "e_interface",value<real_t>(),        ": select k_i smallest eigenpairs of interface problem with size N_i where k_i := c_interface (N_i)^(e_interface)" )
        ( "c_condense",   value<real_t>(),      ": select k_i smallest eigenpairs of condensed subdomain problem with size N_i where k_i := c_condense (N_i)^(e_subdomain)" );
    // parameter for classical iterativ eigensolver
    arnoldi_opts.add_options()
        ( "shift",             value<real_t>(), ": set shift displacement in get_shift" );

    hid_opts.add_options()
        ( "data_path",      value<string>(), ": path to the folder that must contain the following input files of this program\n"
                                             "      file 'K' contains the data for the sparse matrix K\n"
                                             "      file 'M' contains the data for the sparse matrix M\n"
                                             "      file 'coord' contains the data for the associated coordinate information of the problem\n");

    // options for command line parsing
    vis_opts.add(general_opts).add(HAMLS_opts).add(modalTrunc_opts).add(arnoldi_opts);
    all_opts.add( vis_opts ).add( hid_opts );

    // all "non-option" arguments should be "--data_path" arguments
    pos_opts.add( "data_path", -1 );

    //=====================================================================
    // parse command line options
    //=====================================================================
    try
    {
        store( command_line_parser( argc, argv ).options( all_opts ).positional( pos_opts ).run(), vm );
        notify( vm );
    }// try
    catch ( unknown_option &  e )
    {
        cout << e.what() << ", try \"-h\"" << endl;
        exit( 1 );
    }// catch

    //=====================================================================
    // eval command line options
    //=====================================================================
    if ( vm.count( "help") )
    {
        cout << vis_opts << endl;
        cout << "usage: eigensolver [options] data_path "<< endl;
        cout << "  where \'data_path\' is the path to the folder that must contain the following input files for this program (use corresponding HLIBpro format for each file):\n"
                "      file 'K' contains the data for the sparse matrix K\n"
                "      file 'M' contains the data for the sparse matrix M\n"
                "      file 'coord' contains the data for the associated coordinate information of the problem\n";

        exit( 1 );
    }// if

    if ( vm.count( "data_path" ) )
        input_data_path = vm["data_path"].as<string>();
    else
    {
        cout << "usage: eigensolver [options] data_path "<< endl;
        exit( 1 );
    }// if

    //---------------------------------------------------------
    //       general parameters
    //---------------------------------------------------------
    if ( vm.count( "hlib_verbosity" ) ) input_hlib_verbosity = vm["hlib_verbosity"].as<int>();
    if ( vm.count( "eta"            ) ) input_eta      = vm["eta"].as<real_t>();
    if ( vm.count( "nmin"           ) ) input_nmin     = vm["nmin"].as<uint>();
    if ( vm.count( "n_ev"           ) ) input_n_ev     = vm["n_ev"].as<size_t>();
    if ( vm.count( "threads"        ) ) input_nthreads = vm["threads"].as<int>();
    if ( vm.count( "print_solver_info" ) ) input_print_solver_info = true;
    //---------------------------------------------------------
    //       basic HAMLS parameters
    //---------------------------------------------------------
    if ( vm.count( "rel_accuracy_transform" ) ) input_rel_accuracy_transform = vm["rel_accuracy_transform"].as<real_t>();
    if ( vm.count( "max_dof_subdomain"      ) ) input_max_dof_subdomain      = vm["max_dof_subdomain"].as<uint>();
    //---------------------------------------------------------
    //       mode selection parameters of HAMLS
    //---------------------------------------------------------
    if ( vm.count( "c_subdomain"  ) ) input_c_subdomain   = vm["c_subdomain"].as<real_t>();
    if ( vm.count( "c_interface"  ) ) input_c_interface   = vm["c_interface"].as<real_t>();
    if ( vm.count( "c_condense"   ) ) input_c_condense    = vm["c_condense"].as<real_t>();
    if ( vm.count( "e_subdomain") ) input_e_subdomain = vm["e_subdomain"].as<real_t>();
    if ( vm.count( "e_interface") ) input_e_interface = vm["e_interface"].as<real_t>();
    //---------------------------------------------------------
    //       parameter for classical iterativ eigensolver
    //---------------------------------------------------------
    if ( vm.count( "shift"      ) ) input_shift = vm["shift"].as<real_t>();

    try
    {
        //----------------------------------------------------------------------------
        // init HLIBpro and set general HLIBpro options
        //----------------------------------------------------------------------------

        INIT();

        if ( input_nthreads != 0 )
            CFG::set_nthreads( input_nthreads );

        cout<<endl<<"# "<<CFG::nthreads()<<" threads are used for parallel computation"<<endl;

        if ( CFG::nthreads() > 1 )
            CFG::Arith::use_dag = true;
        else
            CFG::Arith::use_dag = false;

        CFG::set_verbosity( input_hlib_verbosity );

        //////////////////////////////////////////////////////////////////////////////
        //
        // Load sparse matrices K and M, and the associated coordinate information
        //
        //////////////////////////////////////////////////////////////////////////////
        cout<<endl<<"# Load Sparse Matrix K";
        unique_ptr< TSparseMatrix >  K( import_matrix( input_data_path + "input_K" ) );

        cout<<endl<<"# Load Sparse Matrix M";
        unique_ptr< TSparseMatrix >  M( import_matrix( input_data_path + "input_M" ) );

        cout<<endl<<"# Load Coordinate Data From File \'"<<input_data_path + "input_coord"<<"\'"<<endl;
        TAutoCoordIO    coord_io;
        unique_ptr< TCoordinate >  coord( coord_io.read ( input_data_path + "input_coord") );

        //////////////////////////////////////////////////////////////////////////////
        //
        // Construct H-matrix representations K_h and M_h of the sparse matrices K and M
        //
        //////////////////////////////////////////////////////////////////////////////

        //----------------------------------------------------------------------------
        // 1) Apply nested dissection for the creation of the cluster tree
        //----------------------------------------------------------------------------
        uint  nmin;

        if ( input_nmin == 0 )
            nmin = max( size_t(40), min( M->avg_entries_per_row(), size_t(100) ) );
        else
            nmin = uint(input_nmin);

        // partitioning of clusters is based on an adaptive bisection
        TGeomBSPPartStrat  part_strat( adaptive_split_axis );

        //NOTE: Choose this special sparse matrix for the construction of cluster tree
        //that has everywhere there non-zero entries where K or M has a non-zero entry,
        //i.e., choose for example |K|+|M|
        TBSPNDCTBuilder  ct_builder( M.get(), & part_strat, nmin );

        unique_ptr< TClusterTree > ct( ct_builder.build( coord.get() ) );

        //----------------------------------------------------------------------------
        // 2) Create corresponding block cluster tree
        //----------------------------------------------------------------------------
        TStdGeomAdmCond  adm_cond( input_eta );

        TBCBuilder                       bct_builder;
        unique_ptr< TBlockClusterTree >  bct( bct_builder.build( ct.get(), ct.get(), & adm_cond ) );

        //----------------------------------------------------------------------------
        // 3) Construct corresponding H-matrix representations
        //----------------------------------------------------------------------------
        TSparseMBuilder  h_builder_K( K.get(), ct->perm_i2e(), ct->perm_e2i() );
        TSparseMBuilder  h_builder_M( M.get(), ct->perm_i2e(), ct->perm_e2i() );

        TTruncAcc  acc_exact( real_t(0) );

        unique_ptr< TMatrix >  K_h( h_builder_K.build( bct.get(), K->form(), acc_exact ) );
        unique_ptr< TMatrix >  M_h( h_builder_M.build( bct.get(), M->form(), acc_exact ) );

        ////////////////////////////////////////////////////////////////////////////////////////
        //
        // Compute the eigendecomposition (D,Z) of the eigenvalue problem (K,M), i.e.,
        // solve the problem K*Z = M*Z*D where D is a diagonal matrix containing the
        // eigenvalues and Z is the matrix containing the associated eigenvectors.
        //
        ////////////////////////////////////////////////////////////////////////////////////////

        //--------------------------------------------------------------------------------------
        // Solve eigenvalue problem using the ARPACK eigensolver
        //
        // - this solver is fast and very robust
        // - this solver computes numerically exact eigenpairs
        // - this solver is based on the well-proven and well-tested ARPACK library
        // - this solver is recommended only when a small portion of the eigenpairs is sought
        // - this solver supports shifts
        // - this solver can be performed with and without a symmentric problem transformation
        // - this solver has been parallelized as well, however, only one problem can be solved
        //   by this solver concurrently, since the ARPACK library is not threadsafe
        //--------------------------------------------------------------------------------------
        cout<<endl<<"# Solve Eigenvalue Problem (K,M) via ARPACK and H-Matrix Arithmetic"<<endl;
        // initialise the output data
        unique_ptr< TDenseMatrix >  D_arpack ( new TDenseMatrix() );
        unique_ptr< TDenseMatrix >  Z_arpack ( new TDenseMatrix() );

        // adjust the ARPACK eigensolver
        TEigenArpack arpack_solver;
        arpack_solver.set_shift         ( input_shift );
        arpack_solver.set_print_info    ( input_print_solver_info);
        arpack_solver.set_n_ev_searched ( input_n_ev  );

        // solve the problem
        arpack_solver.comp_decomp( K_h.get(), M_h.get(), D_arpack.get(), Z_arpack.get() );

        // analyse the computed eigensolution
        TEigenAnalysis  eigen_analysis;
        eigen_analysis.set_verbosity( 4 );
        eigen_analysis.analyse_vector_residual ( K.get(), M.get(), D_arpack.get(), Z_arpack.get(), ct.get() );

        //--------------------------------------------------------------------------------------
        // Solve eigenvalue problem using the Arnoldi eigensolver
        //
        // - this solver is fast
        // - this solver computes numerically exact eigenpairs
        // - this solver is a basic implementation of the Arnoldi solver and uses a symmetric
        //   problem transformation, i.e., the generalized eigenvalue problem (K,M) is transformed
        //   to a standard eigenvaluue
        // - this solver is recommended only when a small portion of the eigenpairs is sought
        // - this solver supports shifts
        // - this solver has been parallelized several problem can be solved by this solver concurrently
        //--------------------------------------------------------------------------------------
        cout<<endl<<"# Solve Eigenvalue Problem (K,M) via Arnoli Method and H-Matrix Arithmetic"<<endl;
        // initialise the output data
        unique_ptr< TDenseMatrix >  D_arnoldi ( new TDenseMatrix() );
        unique_ptr< TDenseMatrix >  Z_arnoldi ( new TDenseMatrix() );

        // adjust the Arnoldi eigensolver
        TEigenArnoldi arnoldi_solver;
        arnoldi_solver.set_shift              ( input_shift );
        arnoldi_solver.set_print_info         ( input_print_solver_info);
        arnoldi_solver.set_n_ev_searched      ( input_n_ev  );

        // solve the problem
        arnoldi_solver.comp_decomp( K_h.get(), M_h.get(), D_arnoldi.get(), Z_arnoldi.get() );

        // analyse the computed eigensolution
        eigen_analysis.analyse_vector_residual ( K.get(), M.get(), D_arnoldi.get(), Z_arnoldi.get(), ct.get() );

        //--------------------------------------------------------------------------------------
        // Solve eigenvalue problem using the H-AMLS method
        //
        // - this solver is very fast especially when many eigenpairs are sought
        // - this solver computes only approximative eigenpairs, however, the approximation
        //   error can be controlled such that the error is of the order of the discretisation error
        // - this solver is highly recommended when a large portion of the eigenpairs is sought
        //   and when numerical exact eigenpairs are not needed, e.g., when one is actually
        //   interested in the eigensolutions of an underlying continuous problem
        //--------------------------------------------------------------------------------------
        cout<<endl<<"# Solve Eigenvalue Problem (K,M) via H-AMLS Method"<<endl;
        // initialise the output data
        unique_ptr< TDenseMatrix >  D_hamls   ( new TDenseMatrix() );
        unique_ptr< TDenseMatrix >  Z_hamls   ( new TDenseMatrix() );

        // adjust basic options of the H-AMLS eigensolver
        THAMLS hamls_solver;
        hamls_solver.set_n_ev_searched    ( input_n_ev );
        hamls_solver.set_max_dof_subdomain( input_max_dof_subdomain );
        hamls_solver.set_do_improving    ( true   );
        hamls_solver.set_do_condensing  ( true   );
        hamls_solver.set_print_info       ( input_print_solver_info );

        // set the accurcy of the H-matrix arithmetic used in H-AMLS
        TTruncAcc acc_transform( input_rel_accuracy_transform, real_t(0) );
        hamls_solver.set_acc_transform_K( acc_transform );
        hamls_solver.set_acc_transform_M( acc_transform );

        // set options for mode selection applied in HAMLS
        hamls_solver.set_factor_subdomain  ( input_c_subdomain  );
        hamls_solver.set_factor_interface  ( input_c_interface  );
        hamls_solver.set_factor_condense   ( input_c_condense  );
        hamls_solver.set_exponent_interface( input_e_interface );
        hamls_solver.set_exponent_subdomain( input_e_subdomain );

        // solve the problem
        hamls_solver.comp_decomp ( ct.get(), K_h.get(), M_h.get(), D_hamls.get(), Z_hamls.get(), K.get(), M.get() );

        // analyse the computed eigensolution
        eigen_analysis.analyse_vector_residual ( K.get(), M.get(), D_hamls.get(), Z_hamls.get(), ct.get() );

        DONE();
    }// try
    catch ( Error & e )
    {
        cout << e.to_string() << endl;
    }// catch
    catch ( std::exception & e )
    {
        std::cout << e.what() << std::endl;
    }// catch

    return 0;
}


/*=========================================================================

  Library   : Image Registration Toolkit (IRTK)
  Module    : $Id$
  Copyright : Imperial College, Department of Computing
              Visual Information Processing (VIP), 2008 onwards
  Date      : $Date$
  Version   : $Revision$
  Changes   : $Author$

Copyright (c) 1999-2014 and onwards, Imperial College London
All rights reserved.
See LICENSE for details

=========================================================================*/

// irtk includes
#include <irtkImage.h>
#include <irtkNoise.h>

// boost includes
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

template <class VoxelType> irtkRicianNoise<VoxelType>::irtkRicianNoise(double Amplitude) : irtkNoise<VoxelType>(Amplitude)
{
}

template <class VoxelType> const char *irtkRicianNoise<VoxelType>::NameOfClass()
{
  return "irtkRicianNoise";
}

template <class VoxelType> double irtkRicianNoise<VoxelType>::Run(int x, int y, int z, int t)
{
  boost::mt19937 rng;
  rng.seed(this->_Init);

  boost::normal_distribution<> nd(0.0, 1.0);
  boost::variate_generator<boost::mt19937&,
			   boost::normal_distribution<> > var_nor(rng, nd);

#ifdef OLD_RICIAN
  // For backward consistency:
  if (this->_input->Get(x, y, z) <= 3*this->_Amplitude) {
    // Rayleigh distributed noise in background and air
    double s1 = var_nor();
    double s2 = var_nor();
    double M  = sqrt(s1*s1+s2*s2);
    return double(this->_input->Get(x, y, z, y)) + this->_Amplitude * M;
  } else {
    // Gaussian distributed noise in foreground
    return double(this->_input->Get(x, y, z, y)) + this->_Amplitude * var_nor();
  }

#else
  // Add Rician noise by treating the magnitude image as the real part (r) of a new complex variable (r,i),
  // adding Gaussian noise to the separate parts of this complex variable, before taking the magnitude again.
  // Thanks to Ged Ridgway <gerard.ridgway@ucl.ac.uk> for this contribution.
  double r = double(this->_input->Get(x, y, z, t)) + this->_Amplitude * var_nor();
  double i = this->_Amplitude * var_nor();
  return sqrt(r*r + i*i);

#endif
}

template class irtkRicianNoise<irtkBytePixel>;
template class irtkRicianNoise<irtkGreyPixel>;
template class irtkRicianNoise<irtkRealPixel>;


#include "activation.hpp"
#include "layer.hpp"

#include <Eigen/Dense>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;

void init_activation(py::module&);

void init_layer(py::module&);

void init_nn(py::module& main)
{
    auto m  = main.def_submodule("nn");
    m.doc() = R"pbdoc(
        Neural Networks
        -----------------------
    )pbdoc";

    init_activation(m);
    init_layer(m);
}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"

#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h"
#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h"
#include "Tudat/External/SpiceInterface/spiceEphemeris.h"
#include "Tudat/External/SpiceInterface/spiceRotationalEphemeris.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/body.h"
#include "Tudat/SimulationSetup/PropagationSetup/createAccelerationModels.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
namespace tudat
{
namespace unit_tests
{

using namespace unit_conversions;
using namespace reference_frames;

BOOST_AUTO_TEST_SUITE( test_aerodynamic_angle_calculator )

//! Function to test the aerodynamic angle calculator.
/*!
 * Function to test the aerodynamic angle calculator from a current body-fixed Cartesian state, the
 *  current orientation angles, and the expected spherical 6-dimensional state.
 * \param testState Body-fixed cartesian state.
 * \param testHeadingAngle Heading angle corresponding to testState
 * \param testFlightPathAngle Flight path angle corresponding to testState
 * \param testLatitude Latitude angle corresponding to testState
 * \param testLongitude Longitude angle corresponding to testState
 * \param angleOfAttack Angle of attack of vehicle
 * \param angleOfSideslip Angle of sideslip of vehicle
 * \param bankAngle Bank angle of vehicle
 */
void testAerodynamicAngleCalculation(
        const Eigen::Vector6d& testState,
        double testHeadingAngle,
        double testFlightPathAngle,
        double testLatitude,
        double testLongitude,
        double angleOfAttack,
        double angleOfSideslip,
        double bankAngle )
{
    // Create angle calculator
    AerodynamicAngleCalculator aerodynamicAngleCalculator(
                [ & ]( ){ return testState; },
                [ & ]( ){ return Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ); }, "", 1,
                [ & ]( ){ return angleOfAttack; },
                [ & ]( ){ return angleOfSideslip; },
                [ & ]( ){ return bankAngle; } );

    // Update angle calculator.
    aerodynamicAngleCalculator.update( 0.0, true );

    // Compare expected against computed angles.
    BOOST_CHECK_SMALL(
                std::fabs( ( aerodynamicAngleCalculator.getAerodynamicAngle( heading_angle ) -
                             testHeadingAngle ) ),2.0E-15 );
    BOOST_CHECK_SMALL(
                std::fabs( ( aerodynamicAngleCalculator.getAerodynamicAngle( flight_path_angle ) -
                             testFlightPathAngle ) ),2.0E-15 );
    BOOST_CHECK_SMALL(
                std::fabs( ( aerodynamicAngleCalculator.getAerodynamicAngle( latitude_angle ) -
                             testLatitude ) ),2.0E-15 );
    BOOST_CHECK_SMALL(
                std::fabs( ( aerodynamicAngleCalculator.getAerodynamicAngle( longitude_angle ) -
                             testLongitude ) ),2.0E-15 );

    // Compute rotation matrices manually and from AerodynamicAngleCalculator
    Eigen::Matrix3d aerodynamicToBodyFrameMatrix =
            aerodynamicAngleCalculator.getRotationQuaternionBetweenFrames(
                aerodynamic_frame, body_frame ).toRotationMatrix( );
    Eigen::Matrix3d testAerodynamicToBodyFrameMatrix =
            getAirspeedBasedAerodynamicToBodyFrameTransformationMatrix(
                angleOfAttack, angleOfSideslip );

    Eigen::Matrix3d trajectoryToAerodynamicFrameMatrix =
            aerodynamicAngleCalculator.getRotationQuaternionBetweenFrames(
                trajectory_frame, aerodynamic_frame ).toRotationMatrix( );
    Eigen::Matrix3d testTrajectoryToAerodynamicFrameMatrix =
            getTrajectoryToAerodynamicFrameTransformationMatrix(
                bankAngle );

    Eigen::Matrix3d verticalToTrajectoryFrameMatrix =
            aerodynamicAngleCalculator.getRotationQuaternionBetweenFrames(
                vertical_frame, trajectory_frame ).toRotationMatrix( );
    Eigen::Matrix3d testVerticalToTrajectoryFrameMatrix =
            getLocalVerticalFrameToTrajectoryTransformationMatrix(
                testFlightPathAngle, testHeadingAngle );

    Eigen::Matrix3d corotatingToVerticalFrameMatrix =
            aerodynamicAngleCalculator.getRotationQuaternionBetweenFrames(
                corotating_frame, vertical_frame ).toRotationMatrix( );
    Eigen::Matrix3d testCorotatingToVerticalFrameMatrix =
            getRotatingPlanetocentricToLocalVerticalFrameTransformationMatrix(
                testLongitude, testLatitude );

    // Compare rotation matrices.
    for( unsigned int l = 0; l < 3; l++ )
    {
        for( unsigned int m = 0; m < 3; m++ )
        {
            BOOST_CHECK_SMALL( std::fabs( aerodynamicToBodyFrameMatrix( l, m ) -
                                          testAerodynamicToBodyFrameMatrix( l, m ) ), 2.0E-15 );
            BOOST_CHECK_SMALL( std::fabs( trajectoryToAerodynamicFrameMatrix( l, m ) -
                                          testTrajectoryToAerodynamicFrameMatrix( l, m ) ), 2.0E-15 );
            BOOST_CHECK_SMALL( std::fabs( verticalToTrajectoryFrameMatrix( l, m ) -
                                          testVerticalToTrajectoryFrameMatrix( l, m ) ), 2.0E-15 );
            BOOST_CHECK_SMALL( std::fabs( corotatingToVerticalFrameMatrix( l, m ) -
                                          testCorotatingToVerticalFrameMatrix( l, m ) ), 2.0E-15 );

        }

    }

    // Test rotation matrix obtained directly and from combination of two rotation matrices from
    // AerodynamicAngleCalculator
    for( unsigned int i = 0; i < 5; i++ )
    {
        for( unsigned int j = 0; j < 5; j++ )
        {
            // Calculate direct rotation matrix
            Eigen::Matrix3d directRotationMatrix =
                    aerodynamicAngleCalculator.getRotationQuaternionBetweenFrames(
                        static_cast< AerodynamicsReferenceFrames >( i ),
                        static_cast< AerodynamicsReferenceFrames >( j ) ).toRotationMatrix( );

            // Calculate each possible intermediate rotation matrix.
            for( unsigned int k = 0; k < 5; k++ )
            {
                Eigen::Matrix3d indirectRotationMatrix =
                        aerodynamicAngleCalculator.getRotationQuaternionBetweenFrames(
                            static_cast< AerodynamicsReferenceFrames >( k ),
                            static_cast< AerodynamicsReferenceFrames >( j ) ).toRotationMatrix( ) *
                        aerodynamicAngleCalculator.getRotationQuaternionBetweenFrames(
                            static_cast< AerodynamicsReferenceFrames >( i ),
                            static_cast< AerodynamicsReferenceFrames >( k ) ).toRotationMatrix( );

                // Compare matrices
                for( unsigned int l = 0; l < 3; l++ )
                {
                    for( unsigned int m = 0; m < 3; m++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( directRotationMatrix( l, m )
                                                      - indirectRotationMatrix( l, m ) ), 2.0E-15 );
                    }

                }
            }
        }
    }

}

//! Test inertial to rotating planetocentric frame transformations, using Matlab script of Erwin
//! Mooij to generate reference data.
BOOST_AUTO_TEST_CASE( testAerodynamicAngleCalculator )
{
    // Test case 1: arbitrary rotation
    {
        Eigen::Vector6d testState;
        testState << -1656517.23153109, -5790058.28764025, -2440584.88186829,
                6526.30784888051, -2661.34558272018, 2377.09572383163;

        double testHeadingAngle = 1.229357188236127;
        double testFlightPathAngle = -0.024894033070522;
        double testLatitude = -0.385027359562548;
        double testLongitude = -1.849449608688977;

        double angleOfAttack = 1.232;
        double angleOfSideslip = -0.00322;
        double bankAngle = 2.323432;\

        testAerodynamicAngleCalculation( testState, testHeadingAngle, testFlightPathAngle,
                                         testLatitude, testLongitude,
                                         angleOfAttack, angleOfSideslip, bankAngle );
    }

    // Test case 2: rotation with zero and half pi angles.
    {
        Eigen::Vector6d testState;
        testState << 0.0, 6498098.09700000, 0.0, 0.0, 0.0, 7.438147520000000e+03;

        double testHeadingAngle = 0.0;
        double testFlightPathAngle = 0.0;
        double testLatitude = 0.0;
        double testLongitude = mathematical_constants::PI / 2.0;

        double angleOfAttack = 1.232;
        double angleOfSideslip = -0.00322;
        double bankAngle = 2.323432;

        testAerodynamicAngleCalculation( testState, testHeadingAngle, testFlightPathAngle,
                                         testLatitude, testLongitude,
                                         angleOfAttack, angleOfSideslip, bankAngle );
    }

    // Test case 3: rotation with zero and half pi angles.
    {
        Eigen::Vector6d testState;
        testState << 0.0, 0.0, 6.498098097000000e3, -7.438147520000000e3, 0.0, 0.0;

        double testHeadingAngle = 0.0;
        double testFlightPathAngle = 0.0;
        double testLatitude = mathematical_constants::PI / 2.0;
        double testLongitude = 0.0;

        double angleOfAttack = 1.232;
        double angleOfSideslip = -0.00322;
        double bankAngle = 2.323432;

        testAerodynamicAngleCalculation( testState, testHeadingAngle, testFlightPathAngle,
                                         testLatitude, testLongitude,
                                         angleOfAttack, angleOfSideslip, bankAngle );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Copyright 2014 Nicolas Mellado
//
// Licensed 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 CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// -------------------------------------------------------------------------- //
//
// Authors: Nicolas Mellado
//
// This test runs super4PCS on multiple models and check the computed transformation
// matrix is the same than the one computed during a previous run.
// Dataset used: Armadillo scans, Stanford University Computer Graphics Laboratory
//               http://graphics.stanford.edu/data/3Dscanrep
//
//
// This test is part of the implementation of the Super 4-points Congruent Sets
// (Super 4PCS) algorithm presented in:
//
// Super 4PCS: Fast Global Pointcloud Registration via Smart Indexing
// Nicolas Mellado, Dror Aiger, Niloy J. Mitra
// Symposium on Geometry Processing 2014.
//
// Data acquisition in large-scale scenes regularly involves accumulating
// information across multiple scans. A common approach is to locally align scan
// pairs using Iterative Closest Point (ICP) algorithm (or its variants), but
// requires static scenes and small motion between scan pairs. This prevents
// accumulating data across multiple scan sessions and/or different acquisition
// modalities (e.g., stereo, depth scans). Alternatively, one can use a global
// registration algorithm allowing scans to be in arbitrary initial poses. The
// state-of-the-art global registration algorithm, 4PCS, however has a quadratic
// time complexity in the number of data points. This vastly limits its
// applicability to acquisition of large environments. We present Super 4PCS for
// global pointcloud registration that is optimal, i.e., runs in linear time (in
// the number of data points) and is also output sensitive in the complexity of
// the alignment problem based on the (unknown) overlap across scan pairs.
// Technically, we map the algorithm as an ‘instance problem’ and solve it
// efficiently using a smart indexing data organization. The algorithm is
// simple, memory-efficient, and fast. We demonstrate that Super 4PCS results in
// significant speedup over alternative approaches and allows unstructured
// efficient acquisition of scenes at scales previously not possible. Complete
// source code and datasets are available for research use at
// http://geometry.cs.ucl.ac.uk/projects/2014/super4PCS/.

#include "super4pcs/algorithms/4pcs.h"
#include "super4pcs/algorithms/super4pcs.h"
#include "super4pcs/io/io.h"
#include "super4pcs/utils/geometry.h"

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <array>
#include <fstream>
#include <iostream>
#include <string>

#include <boost/filesystem.hpp>

#include "testing.h"

#define WRITE_OUTPUT_FILES

#define sqr(x) ((x) * (x))

using namespace std;
using namespace GlobalRegistration;

using Scalar = Point3D::Scalar;
enum {Dim = 3};
typedef Eigen::Transform<Scalar, Dim, Eigen::Affine> Transform;

const int nbSet = 2;

struct TrVisitorType {
    inline void operator() (
            float fraction,
            float best_LCP,
            Eigen::Ref<Match4PCSBase::MatrixType> /*transformation*/) {
        std::cout << "New LCP: "
                  << static_cast<int>(fraction * 100)
                  << '%'
                  << best_LCP
                  <<std::endl;
    }
    constexpr bool needsGlobalTransformation() const { return false; }
};

constexpr Utils::LogLevel loglvl = Utils::Verbose;
Utils::Logger logger(loglvl);

/*!
  Datasets and associated parameters
  In the current state we use a conservative configuration, better timing
  could be achieved by tuning a bit more the parameters.
  */

std::array<std::string, nbSet> confFiles = {
    "./datasets/bunny/data/bun.conf",
    "./datasets/armadillo/ArmadilloSide.conf"
};

std::array<Scalar, nbSet> deltas  = {
    0.005,
    0.005,
};

std::array<Scalar, nbSet> overlaps = {
    0.8,
    0.8,
};

std::array<Scalar, nbSet> n_points = {
    200,
    200,
};

// Maximum allowed computation time.
int max_time_seconds = 1e9;

bool use_super4pcs = true;

/*!
  Read a configuration file from Standford 3D shape repository and
  output a set of filename and eigen transformations
  */
inline void
extractFilesAndTrFromStandfordConfFile(
        const std::string &confFilePath,
        std::vector<Transform>& transforms,
        std::vector<string>& files
        ){
    using namespace boost;
    using namespace std;

    VERIFY (filesystem::exists(confFilePath) && filesystem::is_regular_file(confFilePath));

    // extract the working directory for the configuration path
    const string workingDir = filesystem::path(confFilePath).parent_path().native();
    VERIFY (filesystem::exists(workingDir));

    // read the configuration file and call the matching process
    string line;
    ifstream confFile;
    confFile.open(confFilePath);
    VERIFY (confFile.is_open());

    while ( getline (confFile,line) )
    {
        istringstream iss (line);
        vector<string> tokens{istream_iterator<string>{iss},
                              istream_iterator<string>{}};

        // here we know that the tokens are:
        // [0]: keyword, must be bmesh
        // [1]: 3D object filename
        // [2-4]: target translation with previous object
        // [5-8]: target quaternion with previous object

        if (tokens.size() == 9){
            if (tokens[0].compare("bmesh") == 0){

                string inputfile = filesystem::path(confFilePath).parent_path().string()+string("/")+tokens[1];
                VERIFY(filesystem::exists(inputfile) && filesystem::is_regular_file(inputfile));

                // build the Eigen rotation matrix from the rotation and translation stored in the files
                Eigen::Matrix<Scalar, Dim, 1> tr (
                            std::atof(tokens[2].c_str()),
                            std::atof(tokens[3].c_str()),
                            std::atof(tokens[4].c_str()));

                Eigen::Quaternion<Scalar> quat(
                            std::atof(tokens[8].c_str()), // eigen starts by w
                            std::atof(tokens[5].c_str()),
                            std::atof(tokens[6].c_str()),
                            std::atof(tokens[7].c_str()));

                quat.normalize();

                Transform transform (Transform::Identity());
                transform.rotate(quat);
                transform.translate(-tr);

                transforms.push_back(transform);
                files.push_back(inputfile);

            }
        }
    }
    confFile.close();
}

void test_model(const vector<Transform> &transforms,
                const vector<string> &files,
                vector<Point3D> &mergedset,
                int i,
                int param_i){
    using namespace GlobalRegistration;

    const string input1 = files.at(i-1);
    const string input2 = files.at(i);

    cout << "Matching " << input2.c_str() << endl;

    vector<Point3D> set1, set2;
    vector<Eigen::Matrix2f> tex_coords1, tex_coords2;
    vector<typename Point3D::VectorType> normals1, normals2;
    vector<tripple> tris1, tris2;
    vector<std::string> mtls1, mtls2;

    IOManager iomanager;
    VERIFY(iomanager.ReadObject((char *)input1.c_str(), set1, tex_coords1, normals1, tris1, mtls1));
    VERIFY(iomanager.ReadObject((char *)input2.c_str(), set2, tex_coords2, normals2, tris2, mtls2));

    // clean only when we have pset to avoid wrong face to point indexation
    if (tris1.size() == 0)
        Utils::CleanInvalidNormals(set1, normals1);
    if (tris2.size() == 0)
        Utils::CleanInvalidNormals(set2, normals2);

    // first transform the first mesh to its gt coordinates:
    // we compare only pairwise matching, so we don't want to
    // accumulate error during the matching process
    // Transforms Q by the new transformation.
    {
        Match4PCSBase::MatrixType transformation = transforms[i-1].inverse().matrix();
        for (int j = 0; j < set1.size(); ++j) {
            set1[j].pos() = (transformation * set1[j].pos().homogeneous()).head<3>();

//            cv::Mat first(4, 1, CV_64F), transformed;
//            first.at<double>(0, 0) = set1[j].x();
//            first.at<double>(1, 0) = set1[j].y();
//            first.at<double>(2, 0) = set1[j].z();
//            first.at<double>(3, 0) = 1;
//            transformed = transformation * first;
//            set1[j].x() = transformed.at<double>(0, 0);
//            set1[j].y() = transformed.at<double>(1, 0);
//            set1[j].z() = transformed.at<double>(2, 0);
        }
    }

    mergedset.insert(mergedset.end(), set1.begin(), set1.end());

    // Our matcher.
    Match4PCSOptions options;

    // Set parameters.
    Match4PCSBase::MatrixType mat (Match4PCSBase::MatrixType::Identity());
    VERIFY(options.configureOverlap(overlaps[param_i]));
    options.sample_size = n_points[param_i];
    options.max_time_seconds = max_time_seconds;
    options.delta = deltas[param_i];

    Scalar score = 0.;

    if(use_super4pcs){
        MatchSuper4PCS matcher(options, logger);
        cout << "./Super4PCS -i "
             << input1.c_str() << " "
             << input2.c_str()
             << " -o " << options.getOverlapEstimation()
             << " -d " << options.delta
             << " -n " << options.sample_size
             << " -a " << options.max_normal_difference
             << " -c " << options.max_color_distance
             << " -t " << options.max_time_seconds
             << endl;
        score = matcher.ComputeTransformation(mergedset, &set2, mat);
    }else{
        Match4PCS matcher(options, logger);
        cout << "./Super4PCS -i "
             << input1.c_str() << " "
             << input2.c_str()
             << " -o " << options.getOverlapEstimation()
             << " -d " << options.delta
             << " -n " << options.sample_size
             << " -a " << options.max_normal_difference
             << " -c " << options.max_color_distance
             << " -t " << options.max_time_seconds
             << " -x "
             << endl;
        score = matcher.ComputeTransformation(mergedset, &set2, mat);
    }

#ifdef WRITE_OUTPUT_FILES
    stringstream iss;
    iss << input2;
    iss << "_aligned.ply";
    iomanager.WriteObject(iss.str().c_str(),
                           set2,
                           tex_coords2,
                           normals2,
                           tris2,
                           mtls2);
#endif

    Transform transformEst (mat);

    cout << "Reference: " << endl << transforms[i].matrix() << endl;
    cout << "Estimation: " << endl << transformEst.matrix() << endl;

    Eigen::Quaternion<Scalar>
            q    (transformEst.rotation()),
            qref (transforms[i].rotation());
    cout << " " << q.x()
         << " " << q.y()
         << " " << q.z()
         << " " << q.w()  << endl;
    cout << " " << qref.x()
         << " " << qref.y()
         << " " << qref.z()
         << " " << qref.w()  << endl;

    Scalar rotDiff = std::abs( ( q.vec().array().abs() - ( qref.vec().array().abs() ) ).abs().sum()) + std::abs(std::abs(q.w()) - std::abs(qref.w()));
    Scalar trDiff  = std::abs(transformEst.translation().dot(transforms[i].translation()));

    cout << "rotDiff = " << rotDiff << endl;
    cout << "trDiff = " << trDiff << endl;

    // use different tests, just to get more verbose output
    VERIFY(rotDiff <= 0.2);
    VERIFY(trDiff <= 0.1);
    VERIFY(rotDiff + trDiff <= 0.2);

#ifdef WRITE_OUTPUT_FILES
    stringstream iss2;
    iss2 << input1;
    iss2 << "_merged.ply";
    iomanager.WriteObject(iss2.str().c_str(),
                           mergedset,
                           vector<Eigen::Matrix2f>(),
                           vector<typename Point3D::VectorType>(),
                           vector<tripple>(),
                           vector<std::string>());
#endif
}

int main(int argc, const char **argv) {
    using std::string;

    const char* custom_argv [1] = {"matching"};

    if(!Testing::init_testing(1, custom_argv))
    {
        return EXIT_FAILURE;
    }

    if(argc < 2) // we accept only 1 argument: the test id
    {
        return EXIT_FAILURE;
    }

    int i = std::atoi(argv[1]);
    if (i > confFiles.size()) return EXIT_FAILURE;

    cout << "Starting to process test " << i << endl;

    vector<Transform> transforms;
    vector<string> files;
    extractFilesAndTrFromStandfordConfFile(confFiles.at(i), transforms, files);

    VERIFY(transforms.size() == files.size());
    const int nbTests = transforms.size()-1;

    // In this test we match each frame to the union of the previously matched
    // frames.
    // In practice we can't use the output of Super4PCS directly, it would require
    // a local ICP to avoid error accumulation. So we sum-up the GT transformations,
    // and check our Super4PCS is working well by comparing the estimated transformation
    // matrix and the GT.
    vector<Point3D> mergedset;

    for (int j = 1; j <= nbTests; ++j){
        CALL_SUBTEST(( test_model(transforms, files, mergedset, j, i) ));
    }

    return EXIT_SUCCESS;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_STANDARD_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED
#define NT2_TOOLBOX_STANDARD_FUNCTIONS_SCALAR_FREXP_HPP_INCLUDED
#include <nt2/toolbox/standard/functions/frexp.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/at.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <cmath>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is floating_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace meta
{
/////////////////////////////////////////////////////////////////////////////
// reference Implementation
/////////////////////////////////////////////////////////////////////////////
NT2_FUNCTOR_IMPLEMENTATION(nt2::standard::tag::frexp_, tag::cpu_,
			   (A0)(A1),
			   (scalar_<floating_<A0> >)
			   (scalar_<int32_<A1> >)
			   )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A1 & a1) const
    {
      return std::frexp(a0, &a1);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::standard::tag::frexp_, tag::cpu_,
                       (A0),
                       (scalar_<floating_<A0> >)
                      )
  {
    typedef boost::fusion::vector<A0,nt2::int32_t>             result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      boost::fusion::at_c<0>(res) =nt2::standard::frexp(a0, boost::fusion::at_c<1>(res));
      return res;
    }
  };
} }
#endif


#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
#include <math.h>
#include <random>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <quadrotor_msgs/PositionCommand.h>
#include <quadrotor_msgs/PolynomialTrajectory.h>

#include "pointcloudTraj/mosek.h"
#include "pointcloudTraj/backward.hpp"
#include "pointcloudTraj/data_type.h"
#include "pointcloudTraj/bezier_base.h"
#include "pointcloudTraj/corridor_finder.h"
#include "pointcloudTraj/traj_optimizer.h"

namespace backward {
backward::SignalHandling sh;
}

using namespace std;
using namespace Eigen;
using namespace pcl;

ros::Publisher _vis_corridor_pub;
ros::Publisher _vis_rrt_star_pub;
ros::Publisher _vis_ctrl_pts_pub;
ros::Publisher _vis_commit_target_pub;
ros::Publisher _traj_pub;
ros::Publisher _vis_target_points;
ros::Publisher _vis_traj_points;
ros::Publisher _vis_commit_traj_points;
ros::Publisher _vis_stop_traj_points;
ros::Subscriber _odom_sub;
ros::Subscriber _dest_pts_sub;
ros::Subscriber _map_sub;
ros::Timer _planning_timer;

/*  parameters read from lauch file  */
double _vel_max, _acc_max, _vel_mean, _acc_mean, _eps;
double _x_l, _x_h, _y_l, _y_h, _z_l, _z_h;  // For random map simulation : map boundary
double _refine_portion, _path_find_limit, _sample_portion, _goal_portion;
double _safety_margin, _search_margin, _max_radius, _sensing_range, _planning_rate, _stop_time, _time_commit;
int    _minimize_order, _poly_order_min, _poly_order_max, _max_samples;
bool   _use_preset_goal, _is_limit_vel, _is_limit_acc, _is_print;

/*  useful global variables  */
Vector3d _start_pos, _start_vel, _start_acc, _commit_target, _end_pos;
double _time_limit_1, _time_limit_2;
nav_msgs::Odometry _odom;

int _traj_id = 0;
int _segment_num;
vector<int> _poly_orderList;
ros::Time _odom_time;
ros::Time _start_time     = ros::TIME_MAX;
ros::Time _rcv_odom_time  = ros::TIME_MAX;
ros::Time _plan_traj_time = ros::TIME_MAX;

bool _is_traj_exist     = false;
bool _is_target_arrive  = false;
bool _is_target_receive = false;
bool _is_has_map        = false;

MatrixXd _Path;
MatrixXd _PolyCoeff;
VectorXd _Time;
VectorXd _Radius;

vector<MatrixXd> _FMList;
vector<VectorXd> _CList, _CvList, _CaList;
sensor_msgs::PointCloud2 traj_pts, target_pts, traj_commit_pts, traj_stop_pts;
PointCloud<PointXYZ> traj_pts_pcd, target_pts_pcd, traj_commit_pts_pcd, traj_stop_pts_pcd;
visualization_msgs::MarkerArray path_vis;
visualization_msgs::MarkerArray ctrlPt_vis;
safeRegionRrtStar _rrtPathPlaner;
TrajectoryOptimizerSOCP _trajectoryGeneratorSocp;

bool checkSafeTrajectory(double check_time);
int trajGeneration(MatrixXd path, VectorXd radius, double time_odom_delay);
Vector3d getCommitedTarget();
VectorXd timeAllocation(MatrixXd sphere_centers, VectorXd sphere_radius, Vector3d init_vel );
quadrotor_msgs::PolynomialTrajectory getBezierTraj();

void visCommitTraj(MatrixXd polyCoeff);
void visBezierTrajectory(MatrixXd polyCoeff);
void visCtrlPoint(MatrixXd polyCoeff);
void visFlightCorridor(MatrixXd path, VectorXd radius);
void visRrt(vector<NodePtr> nodes);

void getPosFromBezier(const MatrixXd & polyCoeff,  const double & t_now, const int & seg_now, Vector3d & ret);
void getStateFromBezier(const MatrixXd & polyCoeff,  const double & t_now, const int & seg_now, VectorXd & ret);

void rcvWaypointsCallBack(const nav_msgs::Path & wp)
{
    if(wp.poses[0].pose.position.z < 0.0)
      return;

    _end_pos(0) = wp.poses[0].pose.position.x;
    _end_pos(1) = wp.poses[0].pose.position.y;
    _end_pos(2) = wp.poses[0].pose.position.z;

    _is_target_receive  = true;
    _is_target_arrive   = false;
    _is_traj_exist      = false;
}

void rcvOdometryCallBack(const nav_msgs::Odometry odom)
{
    _odom = odom;
    _odom_time = odom.header.stamp;

    _start_pos(0) = _odom.pose.pose.position.x;
    _start_pos(1) = _odom.pose.pose.position.y;
    _start_pos(2) = _odom.pose.pose.position.z;
    _start_vel(0) = _odom.twist.twist.linear.x;
    _start_vel(1) = _odom.twist.twist.linear.y;
    _start_vel(2) = _odom.twist.twist.linear.z;
    _start_acc(0) = _odom.twist.twist.angular.x;
    _start_acc(1) = _odom.twist.twist.angular.y;
    _start_acc(2) = _odom.twist.twist.angular.z;

    _rcv_odom_time = ros::Time::now();
    _rrtPathPlaner.setStartPt(_start_pos, _end_pos);

    const static auto getDis = []( const Vector3d u, const Vector3d v ){
        const static auto sq = [] (double && val){
            return val * val;
        };

        return sqrt( sq(v[0] - u[0]) + sq(v[1] - u[1]) + sq(v[2] - u[2])  );
    };

    if( _is_traj_exist && getDis(_start_pos, _commit_target) < _eps )
        _is_target_arrive = true;
}

void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map )
{
    PointCloud<PointXYZ> cloud_input;
    pcl::fromROSMsg(pointcloud_map, cloud_input);

    if(cloud_input.points.size() == 0) return;

    _is_has_map = true;
    _rrtPathPlaner.setInput(cloud_input);
    if(checkSafeTrajectory(_stop_time))
    {
        ROS_WARN("[Demo] Collision Occur, Stop");
        quadrotor_msgs::PolynomialTrajectory traj;
        traj.action = quadrotor_msgs::PolynomialTrajectory::ACTION_WARN_IMPOSSIBLE;
        _traj_pub.publish(traj);
        _is_traj_exist = false;
    }
}

void corridorAugment(MatrixXd & path, VectorXd & radius, VectorXd & time)
{
    int seg_num = time.size();

    MatrixXd path_(seg_num + 1, 3);
    VectorXd radi_(seg_num + 1 );
    VectorXd time_(seg_num + 1 );

    path_.row(0) = path.row(0);
    path_.block(1, 0, seg_num, 3) = path;

    radi_(0) = radius(0);
    radi_.segment(1, seg_num) = radius;

    time_(0) = 0.5;
    time_.segment(1, seg_num) = time;
    time_(1) -= 0.5;

    path   = path_;
    radius = radi_;
    time   = time_;
}

int trajGeneration(MatrixXd path, VectorXd radius, double time_odom_delay)
{
    MatrixXd pos(2,3), vel(2,3), acc(2,3);

    if(_is_traj_exist){
        double time_est_opt = 0.03;
        double t_s =  (_odom_time - _start_time).toSec() + time_est_opt + time_odom_delay;
        int idx;
        for (idx = 0; idx < _segment_num; ++idx){
            if (t_s > _Time(idx) && idx + 1 < _segment_num)
                t_s -= _Time(idx);
            else break;
        }

        t_s /= _Time(idx);

        VectorXd state;
        getStateFromBezier(_PolyCoeff, t_s, idx, state);

        for(int i = 0; i < 3; i++ ){
            pos(0, i) = state(i) * _Time(idx);
            vel(0, i) = state(i + 3);
            acc(0, i) = state(i + 6) / _Time(idx);
        }
    }
    else{
        pos.row(0) = _start_pos;
        vel.row(0) = _start_vel;
        acc.row(0) = _start_acc;
    }

    pos.row(1) = _end_pos;
    vel.row(1) = VectorXd::Zero(3);
    acc.row(1) = VectorXd::Zero(3);

    _Time = timeAllocation( path, radius, vel.row(0) );
    corridorAugment(path, radius, _Time);
    _segment_num = radius.size();

    // Now we assign a order to each piece od the trajectory according to its size
    auto min_r = radius.minCoeff();
    auto max_r = radius.maxCoeff();

    _poly_orderList.clear();
    double d_order = (_poly_order_max - _poly_order_min ) / (max_r - min_r);

    for(int i =0; i < _segment_num; i ++ )
    {
        if(i == 0 || i == _segment_num - 1) _poly_orderList.push_back( _poly_order_max );
        else _poly_orderList.push_back( int( _poly_order_min + (radius(i) - min_r) * d_order ) );
    }

    ros::Time time_1 = ros::Time::now();

    _PolyCoeff = _trajectoryGeneratorSocp.BezierConicOptimizer(
                path, radius, _Time, _poly_orderList, _FMList, pos, vel, acc, _vel_max, _acc_max, _minimize_order, _is_limit_vel, _is_limit_acc, _is_print);

    _start_time = _odom_time + ros::Duration(ros::Time::now() - _rcv_odom_time);
    ros::Time time_2 = ros::Time::now();
    ROS_WARN("[Demo] Time in traj generation is %f", (time_2 - time_1).toSec());

    quadrotor_msgs::PolynomialTrajectory traj;
    if(_PolyCoeff.rows() == 3 && _PolyCoeff.cols() == 3){
        ROS_WARN("[Demo] Cannot find a feasible and optimal solution, somthing wrong with the mosek solver ... ");
        traj.action = quadrotor_msgs::PolynomialTrajectory::ACTION_WARN_IMPOSSIBLE;
        _traj_pub.publish(traj);
        _is_traj_exist     = false;
        _is_target_receive = false;
        return -1;
    }
    else{
        ROS_WARN("[Demo] Trajectory generated successed");
        traj = getBezierTraj();
        _traj_pub.publish(traj);
        _is_traj_exist = true;
    }

    _traj_id ++;
    return 1;
}

bool checkEndOfCommitedTraj()
{
    if(_is_target_arrive){
      _is_target_arrive = false;
      return true;
    }
    else return false;
}

Eigen::Vector3d getCommitedTarget()
{
    double t_s = _time_commit;

    int idx;
    for (idx = 0; idx < _segment_num; ++idx){
        if (t_s > _Time(idx) && idx + 1 < _segment_num)
            t_s -= _Time(idx);
        else break;
    }
    t_s /= _Time(idx);

    double t_start = t_s;
    Eigen::Vector3d coord_t;
    getPosFromBezier(_PolyCoeff, t_start, idx, coord_t);

    coord_t *= _Time(idx);
    target_pts_pcd.clear();
    target_pts_pcd.points.push_back(PointXYZ(coord_t(0), coord_t(1), coord_t(2)));
    target_pts_pcd.width = target_pts_pcd.points.size();
    target_pts_pcd.height = 1;
    target_pts_pcd.is_dense = true;

    pcl::toROSMsg(target_pts_pcd, target_pts);
    target_pts.header.frame_id = "map";
    _vis_target_points.publish(target_pts);

    return coord_t;
}

void planInitialTraj()
{
    _rrtPathPlaner.reset();
    _rrtPathPlaner.setPt( _start_pos, _end_pos, _x_l, _x_h, _y_l, _y_h, _z_l, _z_h, _sensing_range, _max_samples, _sample_portion, _goal_portion );

    ros::Time timeBef = ros::Time::now();
    _rrtPathPlaner.SafeRegionExpansion(_path_find_limit);
    ros::Time timeAft = ros::Time::now();
    double _path_time = (timeAft-timeBef).toSec();

    tie(_Path, _Radius) = _rrtPathPlaner.getPath();
    if( _rrtPathPlaner.getPathExistStatus() == false ){
        ROS_WARN("[Demo] Can't find a path, mission stall, please reset the target");
        quadrotor_msgs::PolynomialTrajectory traj;
        traj.action = quadrotor_msgs::PolynomialTrajectory::ACTION_WARN_IMPOSSIBLE;
        _traj_pub.publish(traj);
        _is_traj_exist = false;
        _is_target_receive = false;
    }
    else{ // Path finding succeed .
        if( trajGeneration( _Path, _Radius, _path_time ) == 1 ){
            _commit_target = getCommitedTarget();
            _rrtPathPlaner.resetRoot(_commit_target);
            visBezierTrajectory(_PolyCoeff);
            visCommitTraj(_PolyCoeff);
            visCtrlPoint( _PolyCoeff );
            visFlightCorridor(_Path, _Radius);
        }
    }

    visRrt(_rrtPathPlaner.getTree());
}

void planIncrementalTraj()
{
    if(_rrtPathPlaner.getGlobalNaviStatus() == true){
      visRrt(_rrtPathPlaner.getTree());
      return;
    }

    if( checkEndOfCommitedTraj() ) {   // arrive at the end of the commited trajectory.
        if( !_rrtPathPlaner.getPathExistStatus() ){ // no feasible path exists
            ROS_WARN("[Demo] reach commited target but no path exists, waiting for a path");
            _is_traj_exist = false;  // continue to fly
            return;
        }
        else{
            visFlightCorridor( _Path, _Radius);
            _plan_traj_time = ros::Time::now();
            if( trajGeneration( _Path, _Radius, (_plan_traj_time - _rcv_odom_time).toSec() ) == 1) { // Generate a new trajectory sucessfully.
                _commit_target = getCommitedTarget();
                _rrtPathPlaner.resetRoot(_commit_target);

                ros::Time time_1 = ros::Time::now();
                visBezierTrajectory(_PolyCoeff);
                visCommitTraj(_PolyCoeff);
                visCtrlPoint (_PolyCoeff);
            }
        }
    }
    else{   // continue to refine the uncommited trajectory
        _rrtPathPlaner.SafeRegionRefine  ( _time_limit_1 ); // add samples to the tree
        _rrtPathPlaner.SafeRegionEvaluate( _time_limit_2 ); // ensure that the path is collision-free

        if(_rrtPathPlaner.getPathExistStatus() == true){
            tie(_Path, _Radius) = _rrtPathPlaner.getPath();
            visFlightCorridor(_Path, _Radius);
        }

        visRrt(_rrtPathPlaner.getTree());
    }
}

void planningCallBack(const ros::TimerEvent& event)
{
    if( !_is_target_receive || !_is_has_map )
        return;

    if( !_is_traj_exist ) planInitialTraj();
    else planIncrementalTraj();
}

int main (int argc, char** argv)
{
    ros::init (argc, argv, "point_cloud_planer_node");
    ros::NodeHandle node_handle( "~" );

    node_handle.param("planParam/plan_rate",       _planning_rate,   10.0);
    node_handle.param("planParam/safety_margin",   _safety_margin,   0.65);
    node_handle.param("planParam/search_margin",   _search_margin,   0.35);
    node_handle.param("planParam/max_radius",      _max_radius,      10.0);
    node_handle.param("planParam/sensing_range",   _sensing_range,   10.0);
    node_handle.param("planParam/refine_portion",  _refine_portion,  0.80);
    node_handle.param("planParam/sample_portion",  _sample_portion,  0.25);     // the ratio to generate samples inside the map range
    node_handle.param("planParam/goal_portion",    _goal_portion,    0.05);     // the ratio to generate samples on the goal
    node_handle.param("planParam/path_find_limit", _path_find_limit, 0.05);
    node_handle.param("planParam/max_samples",     _max_samples,     3000);
    node_handle.param("planParam/stop_horizon",    _stop_time,       0.50);
    node_handle.param("planParam/commitTime",      _time_commit,      1.0);

    node_handle.param("demoParam/target_x",        _end_pos(0),       0.0);
    node_handle.param("demoParam/target_y",        _end_pos(1),       0.0);
    node_handle.param("demoParam/target_z",        _end_pos(2),       0.0);
    node_handle.param("demoParam/goal_input",      _use_preset_goal, true);
    node_handle.param("demoParam/is_limit_vel",    _is_limit_vel,    true);
    node_handle.param("demoParam/is_limit_acc",    _is_limit_acc,    true);
    node_handle.param("demoParam/is_print",        _is_print,        true);

    node_handle.param("dynamic/vec",       _vel_mean, 2.0);
    node_handle.param("dynamic/acc",       _acc_mean, 1.0);
    node_handle.param("dynamic/max_vec",   _vel_max,  3.0);
    node_handle.param("dynamic/max_acc",   _acc_max,  1.5);

    node_handle.param("mapBoundary/lower_x", _x_l,  -50.0);
    node_handle.param("mapBoundary/upper_x", _x_h,   50.0);
    node_handle.param("mapBoundary/lower_y", _y_l,  -50.0);
    node_handle.param("mapBoundary/upper_y", _y_h,   50.0);
    node_handle.param("mapBoundary/lower_z", _z_l,    0.0);
    node_handle.param("mapBoundary/upper_z", _z_h,    3.0);

    node_handle.param("optimization/poly_order_min", _poly_order_min,  5);
    node_handle.param("optimization/poly_order_max", _poly_order_max, 10);
    node_handle.param("optimization/minimize_order", _minimize_order,  3);

    Bernstein _bernstein;
    if(_bernstein.setParam(_poly_order_min, _poly_order_max, _minimize_order) == -1){
        ROS_ERROR(" The trajectory order is set beyond the library's scope, please re-set ");
        exit(EXIT_FAILURE);
    }

    _FMList  = _bernstein.getFM();
    _CList   = _bernstein.getC();
    _CvList  = _bernstein.getC_v();
    _CaList  = _bernstein.getC_a();

    _eps = 0.25;
    _rrtPathPlaner.setParam(_safety_margin, _search_margin, _max_radius, _sensing_range);

    _time_limit_1 =      _refine_portion  * 1.0 / _planning_rate;
    _time_limit_2 = (1 - _refine_portion) * 1.0 / _planning_rate;

    // subcribed msgs
    _dest_pts_sub = node_handle.subscribe( "waypoints",  1, rcvWaypointsCallBack );
    _map_sub      = node_handle.subscribe( "PointCloud", 1, rcvPointCloudCallBack);
    _odom_sub     = node_handle.subscribe( "odometry",   1, rcvOdometryCallBack );

    /*  publish traj msgs to traj_server  */
    _traj_pub           = node_handle.advertise<quadrotor_msgs::PolynomialTrajectory>("trajectory", 10);

    /*  publish visualization msgs  */
    _vis_ctrl_pts_pub       = node_handle.advertise<visualization_msgs::MarkerArray>("trajectory_ctrl_pts", 1);
    _vis_corridor_pub       = node_handle.advertise<visualization_msgs::MarkerArray>("flight_corridor",     1);
    _vis_rrt_star_pub       = node_handle.advertise<visualization_msgs::Marker>     ("rrt_tree",            1);
    _vis_commit_target_pub  = node_handle.advertise<visualization_msgs::Marker>     ("commited_target",     1);

    _vis_target_points      = node_handle.advertise<sensor_msgs::PointCloud2>("commit_target",              1);
    _vis_traj_points        = node_handle.advertise<sensor_msgs::PointCloud2>("trajectory_points",          1);
    _vis_commit_traj_points = node_handle.advertise<sensor_msgs::PointCloud2>("trajectory_commit_points",   1);
    _vis_stop_traj_points   = node_handle.advertise<sensor_msgs::PointCloud2>("trajectory_stop_points",     1);

    _planning_timer         = node_handle.createTimer(ros::Duration(1.0 / _planning_rate), planningCallBack);

    ros::Rate rate(100);

    if(_use_preset_goal)
    {
        ROS_WARN("[Demo] Demo using preset target");
        ROS_WARN("[Demo] Warming up ... ");

        sleep(2.0);
        _is_target_receive  = true;
        _is_target_arrive   = false;
        _is_traj_exist      = false;

    }

    bool status = ros::ok();
    while (status) {
        ros::spinOnce();
        status = ros::ok();
        rate.sleep();
    }
}

quadrotor_msgs::PolynomialTrajectory getBezierTraj()
{
    quadrotor_msgs::PolynomialTrajectory traj;
    traj.action = quadrotor_msgs::PolynomialTrajectory::ACTION_ADD;
    traj.num_segment = _Time.size();

    int polyTotalNum = 0;
    for(auto order:_poly_orderList)
        polyTotalNum += (order + 1);

    traj.coef_x.resize(polyTotalNum);
    traj.coef_y.resize(polyTotalNum);
    traj.coef_z.resize(polyTotalNum);

    int idx = 0;
    for(int i = 0; i < _segment_num; i++ )
    {
        int order = _poly_orderList[i];
        int poly_num1d = order + 1;

        for(int j =0; j < poly_num1d; j++)
        {
            traj.coef_x[idx] = _PolyCoeff(i,                  j);
            traj.coef_y[idx] = _PolyCoeff(i,     poly_num1d + j);
            traj.coef_z[idx] = _PolyCoeff(i, 2 * poly_num1d + j);
            idx++;
        }
    }

    traj.header.frame_id = "/bernstein";
    traj.header.stamp = _start_time;

    traj.time.resize(_Time.size());
    traj.order.resize(_Time.size());

    traj.mag_coeff = 1.0;

    for (int idx = 0; idx < _Time.size(); ++idx){
        traj.time[idx] = _Time(idx);
        traj.order[idx] = _poly_orderList[idx];
    }

    traj.start_yaw = 0.0;
    traj.final_yaw = 0.0;

    traj.trajectory_id = _traj_id;
    traj.action = quadrotor_msgs::PolynomialTrajectory::ACTION_ADD;

    return traj;
}

VectorXd timeAllocation( MatrixXd sphere_centers, VectorXd sphere_radius, Vector3d init_vel )
{
    int ball_num = sphere_centers.rows();
    MatrixXd check_pt( ball_num - 1 , 3 );

    const static auto getDis = []( const Vector3d u, const Vector3d v ){
          const static auto sq = [] (double && val){
                return val * val;
            };

          return sqrt( sq(v[0] - u[0]) + sq(v[1] - u[1]) + sq(v[2] - u[2])  );
    };

    Vector3d center, last_center;
    double radius, last_radius;

    int index = 0 ;
    last_center <<  sphere_centers(index, 0), sphere_centers(index, 1), sphere_centers(index, 2);
    last_radius  =  sphere_radius[index];

    for(index = 1; index < ball_num; index ++ ){
        center <<  sphere_centers(index, 0), sphere_centers(index, 1), sphere_centers(index, 2);
        radius  =  sphere_radius[index];

        double dist = getDis(last_center, center);

        Vector3d delta_Vec = center - last_center;
        Eigen::Vector3d joint_pt;
        joint_pt = delta_Vec * ( dist + last_radius - radius) / ( 2.0 * dist ) + last_center;

        check_pt.block( index - 1, 0, 1, 3 ) = joint_pt.transpose();

        last_center = center;
        last_radius = radius;
    }

    MatrixXd all_points( ball_num + 1 , 3 );
    all_points.row( 0 )                       = _start_pos;
    all_points.block( 1, 0, ball_num - 1, 3 ) =  check_pt;
    all_points.row( ball_num )                = _end_pos;

    VectorXd time_allocate(all_points.rows() - 1);
    Vector3d initv = init_vel;

    for (int k = 0; k < all_points.rows() - 1; k++){
        double dtxyz;
        Vector3d p0   = all_points.row(k);           // The start point of this segment
        Vector3d p1   = all_points.row(k + 1);       // The end point of this segment
        Vector3d d    = p1 - p0;                     // The position difference
        Vector3d v0(0.0, 0.0, 0.0);                  // The init velocity

        if( k == 0) v0 = initv;

        double D    = d.norm();                      // The norm of position difference
        double V0   = v0.dot(d / D);                 // Map velocity to the vector (p1-p0)
        double aV0  = fabs(V0);                      // The absolote mapped velocity

        double acct = ( _vel_mean - V0) / _acc_mean * ( (_vel_mean > V0 ) ? 1 : -1 );            // The time to speed up to to the max veloctiy
        double accd = V0 * acct + (_acc_mean * acct * acct / 2) * ((_vel_mean > V0) ? 1 : -1 );  // The distance to speed up
        double dcct = _vel_mean / _acc_mean;                                                     // The time to slow down.
        double dccd = _acc_mean * dcct * dcct / 2;                                               // The distance to slow down.

        if (D < aV0 * aV0 / (2 * _acc_mean)){           // if not enough distance to slow down
            double t1 = (V0 < 0)?2.0 * aV0 / _acc_mean:0.0;
            double t2 = aV0 / _acc_mean;
            dtxyz     = t1 + t2;
        }
        else if (D < accd + dccd){                      // if not enough distance to speed up and slow dwon
            double t1 = (V0 < 0)?2.0 * aV0 / _acc_mean:0.0;
            double t2 = (-aV0 + sqrt(aV0 * aV0 + _acc_mean * D - aV0 * aV0 / 2)) / _acc_mean;
            double t3 = (aV0 + _acc_mean * t2) / _acc_mean;
            dtxyz     = t1 + t2 + t3;
        }
        else{                                          // can reach max veloctiy and keep for a while.
            double t1 = acct;
            double t2 = (D - accd - dccd) / _vel_mean;
            double t3 = dcct;
            dtxyz     = t1 + t2 + t3;
        }
        time_allocate(k) = dtxyz;
    }

    return time_allocate;
}

inline void getStateFromBezier(const MatrixXd & polyCoeff,  const double & t_now, const int & seg_now, VectorXd & ret)
{
    ret = VectorXd::Zero(9);
    VectorXd ctrl_now = polyCoeff.row(seg_now);

    int order = _poly_orderList[seg_now];
    int ctrl_num1D = order + 1;

    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < ctrl_num1D; j++){
            ret(i) += _CList[order](j) * ctrl_now(i * ctrl_num1D + j) * pow(t_now, j) * pow((1 - t_now), (order - j) );

            if(j < ctrl_num1D - 1 )
              ret(i+3) += _CvList[order](j) * order
                        * ( ctrl_now(i * ctrl_num1D + j + 1) - ctrl_now(i * ctrl_num1D + j))
                        * pow(t_now, j) * pow((1 - t_now), (order - j - 1) );

            if(j < ctrl_num1D - 2 )
              ret(i+6) += _CaList[order](j) * order * (order - 1)
                        * ( ctrl_now(i * ctrl_num1D + j + 2) - 2 * ctrl_now(i * ctrl_num1D + j + 1) + ctrl_now(i * ctrl_num1D + j))
                        * pow(t_now, j) * pow((1 - t_now), (order - j - 2) );
        }

    }
}

inline void getPosFromBezier(const MatrixXd & polyCoeff,  const double & t_now, const int & seg_now, Vector3d & ret)
{
    ret = VectorXd::Zero(3);

    int order = _poly_orderList[seg_now];
    int ctrl_num1D = order + 1;
    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < ctrl_num1D; j++){
            ret(i) += _CList[order](j) * polyCoeff(seg_now, i * ctrl_num1D + j) * pow(t_now, j) * pow((1 - t_now), (order - j) );
        }
    }
}

bool checkSafeTrajectory(double check_time)
{
    if(!_is_traj_exist) return false;

    traj_stop_pts_pcd.points.clear();

    double t_s = max(0.0, (_odom.header.stamp - _start_time).toSec());
    int idx;
    for (idx = 0; idx < _segment_num; ++idx){
      if (t_s > _Time(idx) && idx + 1 < _segment_num)
          t_s -= _Time(idx);
      else break;
    }

    double t_ss;
    double t_accu = 0.0;
    for(int i = idx; i < _segment_num; i++ ){
      t_ss = (i == idx) ? t_s : 0.0;
      for (double t = t_ss; t < _Time(i); t += 0.02){
        t_accu += 0.02;
        if(t_accu > _stop_time) break;

        Vector3d traj_pt;
        getPosFromBezier(_PolyCoeff, t/_Time(i), i, traj_pt);
        traj_pt *= _Time(i);

        PointXYZ pt;
        pt.x = traj_pt(0);
        pt.y = traj_pt(1);
        pt.z = traj_pt(2);
        traj_stop_pts_pcd.points.push_back(pt);

        if(_rrtPathPlaner.checkTrajPtCol(traj_pt)){
          traj_stop_pts_pcd.width = traj_stop_pts_pcd.points.size();
          traj_stop_pts_pcd.height = 1;
          traj_stop_pts_pcd.is_dense = true;
          pcl::toROSMsg(traj_stop_pts_pcd, traj_stop_pts);
          traj_stop_pts.header.frame_id = "map";
          _vis_stop_traj_points.publish(traj_stop_pts);
          return true;
        }
      }
    }

    traj_stop_pts_pcd.width = traj_stop_pts_pcd.points.size();
    traj_stop_pts_pcd.height = 1;
    traj_stop_pts_pcd.is_dense = true;
    pcl::toROSMsg(traj_stop_pts_pcd, traj_stop_pts);
    traj_stop_pts.header.frame_id = "map";
    _vis_stop_traj_points.publish(traj_stop_pts);

    return false;
}

void visRrt(vector<NodePtr> nodes)
{
    //ROS_WARN("Prepare to vis rrt result");
    visualization_msgs::Marker line_list, root;

    root.header.frame_id    = line_list.header.frame_id    = "/map";
    root.header.stamp       = line_list.header.stamp       = ros::Time::now();
    root.ns                 = "/root";
    line_list.ns            = "/edges";
    root.action             = line_list.action             = visualization_msgs::Marker::ADD;
    root.pose.orientation.w = line_list.pose.orientation.w = 1.0;
    root.pose.orientation.x = line_list.pose.orientation.x = 0.0;
    root.pose.orientation.y = line_list.pose.orientation.y = 0.0;
    root.pose.orientation.z = line_list.pose.orientation.z = 0.0;
    root.id                 = line_list.id = 0;
    root.type               = visualization_msgs::Marker::SPHERE_LIST;
    line_list.type = visualization_msgs::Marker::LINE_LIST;

    root.scale.x      = root.scale.y      = root.scale.z = 0.25;
    line_list.scale.x = line_list.scale.y = line_list.scale.z = 0.025;

    root.color.a = 1.0;
    root.color.r = 0.0;
    root.color.g = 1.0;
    root.color.b = 1.0;

    line_list.color.a = 0.7;
    line_list.color.r = 0.0;
    line_list.color.g = 1.0;
    line_list.color.b = 0.0;
    line_list.points.clear();

    NodePtr nodeptr;
    for(int i = 0; i < int(nodes.size()); i++){
        nodeptr = nodes[i];
        if (nodeptr->preNode_ptr == NULL){
            geometry_msgs::Point r;
            r.x = nodeptr->coord(0);
            r.y = nodeptr->coord(1);
            r.z = nodeptr->coord(2);
            root.points.push_back(r);
            continue;
        }

        geometry_msgs::Point p_line;
        p_line.x = nodeptr->coord(0);
        p_line.y = nodeptr->coord(1);
        p_line.z = nodeptr->coord(2);
        line_list.points.push_back(p_line);

        p_line.x = nodeptr->preNode_ptr->coord(0);
        p_line.y = nodeptr->preNode_ptr->coord(1);
        p_line.z = nodeptr->preNode_ptr->coord(2);
        line_list.points.push_back(p_line);
    }

    _vis_rrt_star_pub.publish(root);
    _vis_rrt_star_pub.publish(line_list);
}

void visCommitTraj(MatrixXd polyCoeff)
{
    traj_commit_pts_pcd.points.clear();
    double t_s = max(0.0, (_odom.header.stamp - _start_time).toSec());
    int idx;
    for (idx = 0; idx < _segment_num; ++idx){
      if (t_s > _Time(idx) && idx + 1 < _segment_num)
          t_s -= _Time(idx);
      else break;
    }

    double duration = 0.0;
    double t_ss;
    for(int i = idx; i < _segment_num; i++ )
    {
        t_ss = (i == idx) ? t_s : 0.0;
        for(double t = t_ss; t < _Time(i); t += 0.02){
            double t_d = duration + t - t_ss;
            if( t_d > _time_commit ) break;
            Vector3d traj_pt;
            getPosFromBezier( polyCoeff, t/_Time(i), i, traj_pt );

            PointXYZ pt_point;
            pt_point.x = _Time(i) * traj_pt(0);
            pt_point.y = _Time(i) * traj_pt(1);
            pt_point.z = _Time(i) * traj_pt(2);

            traj_commit_pts_pcd.points.push_back(pt_point);
      }

      duration += _Time(i) - t_ss;
    }

    traj_commit_pts_pcd.width = traj_commit_pts_pcd.points.size();
    traj_commit_pts_pcd.height = 1;
    traj_commit_pts_pcd.is_dense = true;
    pcl::toROSMsg(traj_commit_pts_pcd, traj_commit_pts);
    traj_commit_pts.header.frame_id = "map";
    _vis_commit_traj_points.publish(traj_commit_pts);
}

void visBezierTrajectory(MatrixXd polyCoeff)
{
    double traj_len = 0.0;
    int count = 0;

    Vector3d cur, pre;
    cur.setZero();
    pre.setZero();

    traj_pts_pcd.points.clear();
    for(int i = 0; i < _segment_num; i++ )
    {
        for (double t = 0.0; t <= _Time(i); t += 0.02, count += 1)
        {
            Vector3d traj_pt;
            getPosFromBezier( polyCoeff, t / _Time(i), i, traj_pt );

            PointXYZ point;
            cur(0) = point.x = _Time(i) * traj_pt(0);
            cur(1) = point.y = _Time(i) * traj_pt(1);
            cur(2) = point.z = _Time(i) * traj_pt(2);

            if (count) traj_len += (pre - cur).norm();
            pre = cur;

            traj_pts_pcd.points.push_back(point);
        }
    }

    traj_pts_pcd.width = traj_pts_pcd.points.size();
    traj_pts_pcd.height = 1;
    traj_pts_pcd.is_dense = true;

    pcl::toROSMsg(traj_pts_pcd, traj_pts);
    traj_pts.header.frame_id = "map";

    _vis_traj_points.publish(traj_pts);

    ROS_INFO("[Demo] The length of the trajectory; %.3lfm.", traj_len);
}

void visFlightCorridor(MatrixXd path, VectorXd radius)
{
    for (auto & mk: path_vis.markers)
      mk.action = visualization_msgs::Marker::DELETE;

    _vis_corridor_pub.publish(path_vis);
    path_vis.markers.clear();

    visualization_msgs::Marker mk;
    mk.header.frame_id = "map";
    mk.header.stamp = ros::Time::now();
    mk.ns = "pcd_RRT/flight_corridor";
    mk.type = visualization_msgs::Marker::SPHERE;
    mk.action = visualization_msgs::Marker::ADD;
    mk.pose.orientation.x = 0.0;
    mk.pose.orientation.y = 0.0;
    mk.pose.orientation.z = 0.0;
    mk.pose.orientation.w = 1.0;
    mk.color.a = 0.4;
    mk.color.r = 1.0;
    mk.color.g = 1.0;
    mk.color.b = 1.0;

    for(int i = 0; i < int(path.rows()); i++){
        mk.id = i;
        mk.pose.position.x = path(i, 0);
        mk.pose.position.y = path(i, 1);
        mk.pose.position.z = path(i, 2);
        mk.scale.x = 2 * radius(i);
        mk.scale.y = 2 * radius(i);
        mk.scale.z = 2 * radius(i);

        path_vis.markers.push_back(mk);
    }

    _vis_corridor_pub.publish(path_vis);
}

void visCtrlPoint(MatrixXd polyCoeff)
{
    for (auto & mk: ctrlPt_vis.markers)
        mk.action = visualization_msgs::Marker::DELETE;

    _vis_ctrl_pts_pub.publish(ctrlPt_vis);

    ctrlPt_vis.markers.clear();
    visualization_msgs::Marker mk;
    mk.header.frame_id = "map";
    mk.header.stamp = ros::Time::now();
    mk.ns = "pcd_RRT/control_points";
    mk.type = visualization_msgs::Marker::SPHERE;
    mk.action = visualization_msgs::Marker::ADD;
    mk.pose.orientation.x = 0.0;
    mk.pose.orientation.y = 0.0;
    mk.pose.orientation.z = 0.0;
    mk.pose.orientation.w = 1.0;
    mk.color.a = 1.0;
    mk.color.r = 1.0;
    mk.color.g = 0.0;
    mk.color.b = 0.0;

    int idx = 0;
    for(int i = 0; i < _segment_num; i++)
    {
        int order = _poly_orderList[i];
        int ctrl_num = order + 1;

        for(int j = 0; j < ctrl_num; j++)
        {
            mk.id = idx;
            mk.pose.position.x = _Time(i) * polyCoeff(i, j);
            mk.pose.position.y = _Time(i) * polyCoeff(i, ctrl_num + j);
            mk.pose.position.z = _Time(i) * polyCoeff(i, 2 * ctrl_num + j);
            mk.scale.x = 0.25;
            mk.scale.y = 0.25;
            mk.scale.z = 0.25;
            ctrlPt_vis.markers.push_back(mk);
            idx ++;
        }
  }

  _vis_ctrl_pts_pub.publish(ctrlPt_vis);
}


//
// Copyright 2018-2020 Mateusz Loskot <mateusz at loskot dot net>
//
// 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
//
#include <boost/gil/point.hpp>

#include <boost/core/lightweight_test.hpp>

#include <type_traits>

namespace gil = boost::gil;

void test_default_constructor()
{
    gil::point<int> p;
    BOOST_TEST(p.x == 0);
    BOOST_TEST(p.y == 0);
}

void test_user_constructor()
{
    gil::point<int> p{1, 2};
    BOOST_TEST(p.x == 1);
    BOOST_TEST(p.y == 2);
}

void test_copy_constructor()
{
    gil::point<int> p1{1, 2};
    gil::point<int> p2{p1};
    BOOST_TEST(p1.x == p2.x);
    BOOST_TEST(p1.y == p2.y);
}

void test_copy_assignment_operator()
{
    gil::point<int> p1{1, 2};
    gil::point<int> p2;
    p2 = p1;
    BOOST_TEST(p1.x == p2.x);
    BOOST_TEST(p1.y == p2.y);
}

void test_index_operator()
{
    gil::point<int> p{1, 2};
    BOOST_TEST(p[0] == 1);
    BOOST_TEST(p[1] == 2);
}

void test_addition_operator()
{
    gil::point<int> p1{1, 1};
    gil::point<int> const p2{2, 4};
    auto p3 = p1 + p2;
    BOOST_TEST(p3.x == 3);
    BOOST_TEST(p3.y == 5);
}

void test_addition_assignment_operator()
{
    gil::point<int> p1{1, 1};
    gil::point<int> const p2{2, 4};
    p1 += p2;
    BOOST_TEST(p1.x == 3);
    BOOST_TEST(p1.y == 5);
}

void test_subtraction_assignment_operator()
{
    gil::point<int> p1{2, 4};
    gil::point<int> const p2{1, 1};
    p1 -= p2;
    BOOST_TEST(p1.x == 1);
    BOOST_TEST(p1.y == 3);
}

void test_subtraction_operator()
{
    gil::point<int> p1{2, 4};
    gil::point<int> const p2{1, 1};
    p1 = p1 - p2;
    BOOST_TEST(p1.x == 1);
    BOOST_TEST(p1.y == 3);
}

void test_unary_minus_operator()
{
    gil::point<int> p1{2, 4};
    auto p2 = -p1;
    BOOST_TEST(p2.x == -2);
    BOOST_TEST(p2.y == -4);
    p2 = -p2;
    BOOST_TEST(p2.x == p1.x);
    BOOST_TEST(p2.y == p1.y);
}

void test_division_assignment_operator()
{
    {
        gil::point<int> p1{2, 4};
        p1 /= 2;
        static_assert(std::is_same<decltype((p1 / short{}).x), int>::value, "!int");
        static_assert(std::is_same<decltype((p1 / int{}).x), int>::value, "!int");
        static_assert(std::is_same<decltype((p1 / float{}).x), float>::value, "!float");
        static_assert(std::is_same<decltype((p1 / double{}).x), double>::value, "!double");
        BOOST_TEST(p1.x == 1);
        BOOST_TEST(p1.y == 2);
    }
    // point / d
    {
        gil::point<int> p1{2, 4};
        auto p2 = p1 / float{2};
        static_assert(std::is_same<decltype((p2 / int{}).x), float>::value, "!float");
        static_assert(std::is_same<decltype((p2 / float{}).x), float>::value, "!float");
        static_assert(std::is_same<decltype((p2 / double{}).x), double>::value, "!double");
        BOOST_TEST(p2.x >= 1.0); // means == but >= avoids compiler warning
        BOOST_TEST(p2.y >= 2.0);
    }
}

void test_multiplication_operator()
{
    gil::point<int> p1{2, 4};
    p1 *= 2;
    BOOST_TEST(p1.x == 4);
    BOOST_TEST(p1.y == 8);

    // point * m
    {
        auto p2 = p1 * int{2};
        static_assert(std::is_same<decltype(p2.x), int>::value, "!int");
        static_assert(std::is_same<decltype(p2.y), int>::value, "!int");
        BOOST_TEST(p2.x == 8);
        BOOST_TEST(p2.y == 16);
    }
    // m * point
    {
        auto p2 = double{2} *p1;
        static_assert(std::is_same<decltype(p2.x), double>::value, "!double");
        static_assert(std::is_same<decltype(p2.y), double>::value, "!double");
        BOOST_TEST(p2.x >= 8); // means == but >= avoids compiler warning
        BOOST_TEST(p2.y >= 16);
    }
}

void test_multiplication_assignment_operator()
{
    gil::point<int> p1{2, 4};
    // point * m
    {
        auto p2 = p1 * int{2};
        static_assert(std::is_same<decltype(p2.x), int>::value, "!int");
        static_assert(std::is_same<decltype(p2.y), int>::value, "!int");
        BOOST_TEST(p2.x == 4);
        BOOST_TEST(p2.y == 8);
    }
    // m * point
    {
        auto p2 = double{2} * p1;
        static_assert(std::is_same<decltype(p2.x), double>::value, "!double");
        static_assert(std::is_same<decltype(p2.y), double>::value, "!double");
        BOOST_TEST(p2.x >= 4); // means == but >= avoids compiler warning
        BOOST_TEST(p2.y >= 8);
    }
}

void test_bitwise_left_shift_operator()
{
    gil::point<unsigned int> p{2, 4};
    p = p << 1;
    BOOST_TEST(p.x == 4u);
    BOOST_TEST(p.y == 8u);
}

void test_bitwise_right_shift_operator()
{
    gil::point<unsigned int> p{2, 4};
    p = p >> 1;
    BOOST_TEST(p.x == 2u / 2);
    BOOST_TEST(p.y == 4u / 2);
}

void test_equal_to_operator()
{
    gil::point<int> p1{2, 4};
    gil::point<int> p2{2, 4};
    BOOST_TEST(p1 == p2);
}

void test_not_equal_to_operator()
{
    gil::point<int> p1{1, 1};
    gil::point<int> p2{2, 4};
    BOOST_TEST(p1 != p2);
}

void test_axis_value()
{
    gil::point<int> p1{1, 2};
    gil::point<int> const p2{1, 2};
    BOOST_TEST(gil::axis_value<0>(p1) == p1.x);
    BOOST_TEST(gil::axis_value<1>(p1) == p1.y);
    BOOST_TEST(gil::axis_value<0>(p2) == p2.x);
    BOOST_TEST(gil::axis_value<1>(p2) == p2.y);
}

int main()
{
    test_default_constructor();
    test_user_constructor();
    test_copy_constructor();
    test_copy_assignment_operator();
    test_index_operator();
    test_addition_operator();
    test_addition_assignment_operator();
    test_subtraction_assignment_operator();
    test_subtraction_operator();
    test_unary_minus_operator();
    test_division_assignment_operator();
    test_multiplication_operator();
    test_multiplication_assignment_operator();
    test_bitwise_left_shift_operator();
    test_bitwise_right_shift_operator();
    test_equal_to_operator();
    test_not_equal_to_operator();
    test_axis_value();

    return boost::report_errors();
}