/*
 *
 * Copyright (c) Kresimir Fresl 2003
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

/* for UMFPACK Copyright, License and Availability see umfpack_inc.hpp */

#ifndef BOOST_NUMERIC_BINDINGS_UMFPACK_OVERLOADS_HPP
#define BOOST_NUMERIC_BINDINGS_UMFPACK_OVERLOADS_HPP

#include <boost/numeric/bindings/umfpack/umfpack_inc.hpp>
#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>

namespace boost { namespace numeric { namespace bindings {

  namespace umfpack { namespace detail {

    ////////////////////////////
    // UMFPACK primary routines:
    ////////////////////////////

    // symbolic

    inline
    int symbolic (int n_row, int n_col,
                  int const* Ap, int const* Ai, double const* Ax,
                  void **Symbolic, double const* Control, double* Info)
    {
      return umfpack_di_symbolic (n_row, n_col, Ap, Ai, Ax,
                                  Symbolic, Control, Info);
    }

    inline
    int symbolic (int n_row, int n_col,
                  int const* Ap, int const* Ai, traits::complex_d const* Ax,
                  void **Symbolic, double const* Control, double* Info)
    {
      int nnz = Ap[n_col];
      traits::detail::array<double> Axr (nnz);
      if (!Axr.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Axi (nnz);
      if (!Axi.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::disentangle (Ax, Ax+nnz,
                                   Axr.storage(), Axi.storage());
      return umfpack_zi_symbolic (n_row, n_col, Ap, Ai,
                                  Axr.storage(), Axi.storage(),
                                  Symbolic, Control, Info);
    }

    // numeric

    inline
    int numeric (int, int,
                 int const* Ap, int const* Ai, double const* Ax,
                 void *Symbolic, void **Numeric,
                 double const* Control, double* Info)
    {
      return umfpack_di_numeric (Ap, Ai, Ax, Symbolic, Numeric, Control, Info);
    }

    inline
    int numeric (int, int n_col,
                 int const* Ap, int const* Ai, traits::complex_d const* Ax,
                 void *Symbolic, void **Numeric,
                 double const* Control, double* Info)
    {
      int nnz = Ap[n_col];
      traits::detail::array<double> Axr (nnz);
      if (!Axr.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Axi (nnz);
      if (!Axi.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::disentangle (Ax, Ax+nnz,
                                   Axr.storage(), Axi.storage());
      return umfpack_zi_numeric (Ap, Ai, Axr.storage(), Axi.storage(),
                                 Symbolic, Numeric, Control, Info);
    }

    // solve

    inline
    int solve (int sys, int,
               int const* Ap, int const* Ai, double const* Ax,
               double* X, double const* B, void *Numeric,
               double const* Control, double* Info)
    {
      return umfpack_di_solve (sys, Ap, Ai, Ax, X, B, Numeric, Control, Info);
    }

    inline
    int solve (int sys, int n,
               int const* Ap, int const* Ai, traits::complex_d const* Ax,
               traits::complex_d* X, traits::complex_d const* B,
               void *Numeric, double const* Control, double* Info)
    {
      int nnz = Ap[n];
      traits::detail::array<double> Axr (nnz);
      if (!Axr.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Axi (nnz);
      if (!Axi.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::disentangle (Ax, Ax+nnz,
                                   Axr.storage(), Axi.storage());
      traits::detail::array<double> Br (n);
      if (!Br.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Bi (n);
      if (!Bi.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::disentangle (B, B+n,
                                   Br.storage(), Bi.storage());
      traits::detail::array<double> Xr (n);
      if (!Xr.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Xi (n);
      if (!Xi.valid()) return UMFPACK_ERROR_out_of_memory;

      int status = umfpack_zi_solve (sys, Ap, Ai,
                                     Axr.storage(), Axi.storage(),
                                     Xr.storage(), Xi.storage(),
                                     Br.storage(), Bi.storage(),
                                     Numeric, Control, Info);
      if (status != UMFPACK_OK) return status;
      traits::detail::interlace (Xr.storage(), Xr.storage() + n,
                                 Xi.storage(), X);
      return status;
    }

    // free_symbolic

    inline
    void free_symbolic (double, int, void **Symbolic) {
      umfpack_di_free_symbolic (Symbolic);
    }
    inline
    void free_symbolic (traits::complex_d const&, int, void **Symbolic) {
      umfpack_zi_free_symbolic (Symbolic);
    }

    // free_numeric

    inline
    void free_numeric (double, int, void **Numeric) {
      umfpack_di_free_numeric (Numeric);
    }
    inline
    void free_numeric (traits::complex_d const&, int, void **Numeric) {
      umfpack_zi_free_numeric (Numeric);
    }

    ////////////////////////////////
    // UMFPACK alternative routines:
    ////////////////////////////////

    // default control

    inline
    void defaults (double, int, double* Control) {
      umfpack_di_defaults (Control);
    }
    inline
    void defaults (traits::complex_d const&, int, double* Control) {
      umfpack_zi_defaults (Control);
    }

    // symbolic with specified column preordering

    inline
    int qsymbolic (int n_row, int n_col,
                   int const* Ap, int const* Ai, double const* Ax,
                   int const* Qinit,
                   void **Symbolic, double const* Control, double* Info)
    {
      return umfpack_di_qsymbolic (n_row, n_col, Ap, Ai, Ax, Qinit,
                                  Symbolic, Control, Info);
    }

    inline
    int qsymbolic (int n_row, int n_col,
                   int const* Ap, int const* Ai, traits::complex_d const* Ax,
                   int const* Qinit,
                   void **Symbolic, double const* Control, double* Info)
    {
      int nnz = Ap[n_col];
      traits::detail::array<double> Axr (nnz);
      if (!Axr.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Axi (nnz);
      if (!Axi.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::disentangle (Ax, Ax+nnz,
                                   Axr.storage(), Axi.storage());
      return umfpack_zi_qsymbolic (n_row, n_col, Ap, Ai,
                                   Axr.storage(), Axi.storage(),
                                   Qinit, Symbolic, Control, Info);
    }

    ///////////////////////////////////////
    // UMFPACK matrix manipulation routines
    ///////////////////////////////////////

    // triplet (coordinate) to compressed column

    inline
    int triplet_to_col (int n_row, int n_col, int nz,
                        int const* Ti, int const* Tj, double const* Tx,
                        int* Ap, int* Ai, double* Ax, int* Map)
    {
      return umfpack_di_triplet_to_col (n_row, n_col, nz,
                                        Ti, Tj, Tx,
                                        Ap, Ai, Ax, Map);
    }

    inline
    int triplet_to_col (int n_row, int n_col, int nz,
                        int const* Ti, int const* Tj,
                        traits::complex_d const* Tx,
                        int* Ap, int* Ai, traits::complex_d* Ax,
                        int* Map)
    {
      assert (Tx == 0 && Ax == 0 || Tx != 0 && Ax != 0);
      double *Txr = 0, *Txi = 0;
      if (Tx != 0) {
        traits::detail::array<double> ATxr (nz);
        if (!ATxr.valid()) return UMFPACK_ERROR_out_of_memory;
        traits::detail::array<double> ATxi (nz);
        if (!ATxi.valid()) return UMFPACK_ERROR_out_of_memory;
        Txr = ATxr.storage();
        Txi = ATxi.storage();
        traits::detail::disentangle (Tx, Tx+nz, Txr, Txi);
      }
      double *Axr = 0, *Axi = 0;
      if (Ax != 0) {
        traits::detail::array<double> AAxr (nz);
        if (!AAxr.valid()) return UMFPACK_ERROR_out_of_memory;
        traits::detail::array<double> AAxi (nz);
        if (!AAxi.valid()) return UMFPACK_ERROR_out_of_memory;
        Axr = AAxr.storage();
        Axi = AAxi.storage();
      }
      int status;
      status = umfpack_zi_triplet_to_col (n_row, n_col, nz,
                                          Ti, Tj, Txr, Txi,
                                          Ap, Ai, Axr, Axi, Map);
      if (Ax != 0) {
        if (status != UMFPACK_OK) return status;
        traits::detail::interlace (Axr, Axr + nz, Axi, Ax);
      }
      return status;
    }

    // scale

    inline
    int scale (int, double* X, double const* B, void* Numeric) {
      return umfpack_di_scale (X, B, Numeric);
    }

    inline
    int scale (int n, traits::complex_d* X,
               traits::complex_d const* B, void* Numeric)
    {
      traits::detail::array<double> Br (n);
      if (!Br.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Bi (n);
      if (!Bi.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::disentangle (B, B+n,
                                   Br.storage(), Bi.storage());
      traits::detail::array<double> Xr (n);
      if (!Xr.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Xi (n);
      if (!Xi.valid()) return UMFPACK_ERROR_out_of_memory;

      int status = umfpack_zi_scale (Xr.storage(), Xi.storage(),
                                     Br.storage(), Bi.storage(),
                                     Numeric);
      if (status != UMFPACK_OK) return status;
      traits::detail::interlace (Xr.storage(), Xr.storage() + n,
                                 Xi.storage(), X);
      return status;
    }

    //////////////////////////////
    // UMFPACK reporting routines:
    //////////////////////////////

    // report status

    inline
    void report_status (double, int, double const* Control, int status) {
      umfpack_di_report_status (Control, status);
    }
    inline
    void report_status (traits::complex_d const&, int,
                        double const* Control, int status)
    {
      umfpack_zi_report_status (Control, status);
    }

    // report control

    inline
    void report_control (double, int, double const* Control) {
      umfpack_di_report_control (Control);
    }
    inline
    void
    report_control (traits::complex_d const&, int, double const* Control) {
      umfpack_zi_report_control (Control);
    }

    // report info

    inline
    void report_info (double, int, double const* Control, double const* Info) {
      umfpack_di_report_info (Control, Info);
    }
    inline
    void report_info (traits::complex_d const&, int,
                      double const* Control, double const* Info)
    {
      umfpack_zi_report_info (Control, Info);
    }

    // report matrix

    inline
    int report_matrix (int n_row, int n_col,
                       int const* Ap, int const* Ai, double const* Ax,
                       int col_form, double const* Control)
    {
      return umfpack_di_report_matrix (n_row, n_col, Ap, Ai, Ax,
                                       col_form, Control);
    }

    inline
    int report_matrix (int n_row, int n_col,
                       int const* Ap, int const* Ai,
                       traits::complex_d const* Ax,
                       int col_form, double const* Control)
    {
      int nnz = Ap[n_col];
      traits::detail::array<double> Axr (nnz);
      if (!Axr.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Axi (nnz);
      if (!Axi.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::disentangle (Ax, Ax+nnz,
                                   Axr.storage(), Axi.storage());
      return umfpack_zi_report_matrix (n_row, n_col, Ap, Ai,
                                       Axr.storage(), Axi.storage(),
                                       col_form, Control);
    }

    // report triplet (coordinate)

    int report_triplet (int n_row, int n_col, int nz,
                        int const* Ti, int const* Tj, double const* Tx,
                        double const* Control)
    {
      return umfpack_di_report_triplet (n_row, n_col, nz, Ti, Tj, Tx, Control);
    }

    int report_triplet (int n_row, int n_col, int nz,
                        int const* Ti, int const* Tj,
                        traits::complex_d const* Tx,
                        double const* Control)
    {
      traits::detail::array<double> Txr (nz);
      if (!Txr.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Txi (nz);
      if (!Txi.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::disentangle (Tx, Tx+nz,
                                   Txr.storage(), Txi.storage());
      return umfpack_zi_report_triplet (n_row, n_col, nz, Ti, Tj,
                                        Txr.storage(), Txi.storage(),
                                        Control);
    }

    // report vector

    int report_vector (int n, double const* X, double const* Control) {
      return umfpack_di_report_vector (n, X, Control);
    }

    int report_vector (int n, traits::complex_d const* X,
                       double const* Control)
    {
#if 0
      // see UMFPACK v 4.1 User Guide
      traits::detail::array<double> Xr (n);
      if (!Xr.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::array<double> Xi (n);
      if (!Xi.valid()) return UMFPACK_ERROR_out_of_memory;
      traits::detail::disentangle (X, X+n,
                                   Xr.storage(), Xi.storage());
      return umfpack_zi_report_vector (n, Xr.storage(), Xi.storage(), Control);
#endif
      return umfpack_zi_report_vector (n,
                                       reinterpret_cast<double const*> (X),
                                       reinterpret_cast<double const*> (0),
                                       Control);
    }

    // report numeric

    inline
    int report_numeric (double, int, void* Numeric, double const* Control) {
      return umfpack_di_report_numeric (Numeric, Control);
    }
    inline
    int report_numeric (traits::complex_d const&, int,
                        void* Numeric, double const* Control)
    {
      return umfpack_zi_report_numeric (Numeric, Control);
    }

    // report symbolic

    inline
    int report_symbolic (double, int, void* Symbolic, double const* Control) {
      return umfpack_di_report_symbolic (Symbolic, Control);
    }
    inline
    int report_symbolic (traits::complex_d const&, int,
                         void* Symbolic, double const* Control)
    {
      return umfpack_zi_report_symbolic (Symbolic, Control);
    }

    // report permutation vector

    inline
    int report_perm (double, int, int np,
                     int const* Perm, double const* Control) {
      return umfpack_di_report_perm (np, Perm, Control);
    }
    inline
    int report_perm (traits::complex_d const&, int, int np,
                     int const* Perm, double const* Control) {
      return umfpack_zi_report_perm (np, Perm, Control);
    }

  }}

}}}

#endif // BOOST_NUMERIC_BINDINGS_UMFPACK_OVERLOADS_HPP


/*=========================================================================
 *
 *  Copyright UMC Utrecht and contributors
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/
#include "itkSmartPointer.h"
#include "itkArray.h"
#include <vector>
#include <algorithm>
#include <iomanip>

// Report timings
#include <ctime>
#include "itkTimeProbe.h"
#include "itkTimeProbesCollectorBase.h"

// Multi-threading using ITK threads
#include "itkPlatformMultiThreader.h"

// Multi-threading using OpenMP
#ifdef ELASTIX_USE_OPENMP
#include <omp.h>
#endif

// Single-threaded vector arithmetic using Eigen
#ifdef ELASTIX_USE_EIGEN
#include <Eigen/Dense>
#include <Eigen/Core>
#endif

// select double or float internal type of array
#if 0
typedef float InternalScalarType;
#else
typedef double InternalScalarType;
#endif

#ifdef ELASTIX_USE_EIGEN
#if 0
typedef Eigen::VectorXf ParametersTypeEigen;
#else
typedef Eigen::VectorXd ParametersTypeEigen;
#endif
#endif

class OptimizerTEMP : public itk::Object
{
public:

  /** Standard class typedefs. */
  typedef OptimizerTEMP             Self;
  typedef itk::SmartPointer< Self > Pointer;
  itkNewMacro( Self );

  typedef itk::Array< InternalScalarType > ParametersType;

  unsigned long      m_NumberOfParameters;
  ParametersType     m_CurrentPosition;
  ParametersType     m_Gradient;
  InternalScalarType m_LearningRate;

  typedef itk::PlatformMultiThreader             ThreaderType;
  typedef ThreaderType::WorkUnitInfo ThreadInfoType;
  ThreaderType::Pointer m_Threader;
  bool                  m_UseOpenMP;
  bool                  m_UseEigen;
  bool                  m_UseMultiThreaded;

  struct MultiThreaderParameterType
  {
    ParametersType * t_NewPosition;
    Self *           t_Optimizer;
  };

  OptimizerTEMP()
  {
    this->m_NumberOfParameters = 0;
    this->m_LearningRate       = 0.0;
    this->m_Threader           = ThreaderType::New();
    this->m_Threader->SetNumberOfWorkUnits( 8 );
    this->m_UseOpenMP        = false;
    this->m_UseEigen         = false;
    this->m_UseMultiThreaded = false;
  }

  void AdvanceOneStep( void )
  {
    const unsigned int spaceDimension = m_NumberOfParameters;
    ParametersType &   newPosition    = this->m_CurrentPosition;

    if( !this->m_UseMultiThreaded )
    {
      /** Get a pointer to the current position. */
      const InternalScalarType * currentPosition = this->m_CurrentPosition.data_block();
      const double               learningRate    = this->m_LearningRate;
      const InternalScalarType * gradient        = this->m_Gradient.data_block();
      InternalScalarType *       newPos          = newPosition.data_block();

      /** Update the new position. */
      for( unsigned int j = 0; j < spaceDimension; j++ )
      {
        //newPosition[j] = currentPosition[j] - this->m_LearningRate * this->m_Gradient[j];
        newPos[ j ] = currentPosition[ j ] - learningRate * gradient[ j ];
      }
    }
#ifdef ELASTIX_USE_OPENMP
    else if( this->m_UseOpenMP && !this->m_UseEigen )
    {
      /** Get a pointer to the current position. */
      const InternalScalarType * currentPosition = this->m_CurrentPosition.data_block();
      const InternalScalarType   learningRate    = this->m_LearningRate;
      const InternalScalarType * gradient        = this->m_Gradient.data_block();
      InternalScalarType *       newPos          = newPosition.data_block();

      /** Update the new position. */
      const int nthreads = static_cast< int >( this->m_Threader->GetNumberOfWorkUnits() );
      omp_set_num_threads( nthreads );
      #pragma omp parallel for
      for( int j = 0; j < static_cast< int >( spaceDimension ); j++ )
      {
        newPos[ j ] = currentPosition[ j ] - learningRate * gradient[ j ];
      }
    }
#endif
#ifdef ELASTIX_USE_EIGEN
    else if( !this->m_UseOpenMP && this->m_UseEigen )
    {
      /** Get a reference to the current position. */
      const ParametersType &   currentPosition = this->m_CurrentPosition;
      const InternalScalarType learningRate    = this->m_LearningRate;

      /** Wrap itk::Arrays into Eigen jackets. */
      Eigen::Map< ParametersTypeEigen >       newPositionE( newPosition.data_block(), spaceDimension );
      Eigen::Map< const ParametersTypeEigen > currentPositionE( currentPosition.data_block(), spaceDimension );
      Eigen::Map< ParametersTypeEigen >       gradientE( this->m_Gradient.data_block(), spaceDimension );

      /** Update the new position. */
      //Eigen::setNbThreads( this->m_Threader->GetNumberOfWorkUnits() );
      newPositionE = currentPositionE - learningRate * gradientE;
    }
#endif
    else
    {
      /** Fill the threader parameter struct with information. */
      MultiThreaderParameterType * temp = new  MultiThreaderParameterType;
      temp->t_NewPosition = &newPosition;
      temp->t_Optimizer   = this;

      /** Call multi-threaded AdvanceOneStep(). */
      this->m_Threader->SetSingleMethod( AdvanceOneStepThreaderCallback, (void *)( temp ) );
      this->m_Threader->SingleMethodExecute();

      delete temp;
    }
  }  // end

  /** The callback function. */
  static itk::ITK_THREAD_RETURN_TYPE AdvanceOneStepThreaderCallback( void * arg )
  {
    /** Get the current thread id and user data. */
    ThreadInfoType *             infoStruct = static_cast< ThreadInfoType * >( arg );
    itk::ThreadIdType            threadID   = infoStruct->WorkUnitID;
    MultiThreaderParameterType * temp
      = static_cast< MultiThreaderParameterType * >( infoStruct->UserData );

    /** Call the real implementation. */
    temp->t_Optimizer->ThreadedAdvanceOneStep2( threadID, *( temp->t_NewPosition ) );

    return ITK_THREAD_RETURN_DEFAULT_VALUE;

  }  // end AdvanceOneStepThreaderCallback()

  /** The threaded implementation of AdvanceOneStep(). */
  inline void ThreadedAdvanceOneStep( itk::ThreadIdType threadId, ParametersType & newPosition )
  {
    /** Compute the range for this thread. */
    const unsigned int spaceDimension = m_NumberOfParameters;
    const unsigned int subSize        = static_cast< unsigned int >(
      std::ceil( static_cast< double >( spaceDimension )
      / static_cast< double >( this->m_Threader->GetNumberOfWorkUnits() ) ) );
    const unsigned int jmin = threadId * subSize;
    unsigned int       jmax = ( threadId + 1 ) * subSize;
    jmax = ( jmax > spaceDimension ) ? spaceDimension : jmax;

    /** Get a reference to the current position. */
    const ParametersType & currentPosition = this->m_CurrentPosition;
    const double           learningRate    = this->m_LearningRate;
    const ParametersType & gradient        = this->m_Gradient;

    /** Advance one step: mu_{k+1} = mu_k - a_k * gradient_k */
    for( unsigned int j = jmin; j < jmax; j++ )
    {
      newPosition[ j ] = currentPosition[ j ] - learningRate * gradient[ j ];
    }

  }  // end ThreadedAdvanceOneStep()

  /** The threaded implementation of AdvanceOneStep(). */
  inline void ThreadedAdvanceOneStep2( itk::ThreadIdType threadId, ParametersType & newPosition )
  {
    /** Compute the range for this thread. */
    const unsigned int spaceDimension = m_NumberOfParameters;
    const unsigned int subSize        = static_cast< unsigned int >(
      std::ceil( static_cast< double >( spaceDimension )
      / static_cast< double >( this->m_Threader->GetNumberOfWorkUnits() ) ) );
    const unsigned int jmin = threadId * subSize;
    unsigned int       jmax = ( threadId + 1 ) * subSize;
    jmax = ( jmax > spaceDimension ) ? spaceDimension : jmax;

    /** Get a pointer to the current position. */
    const InternalScalarType * currentPosition = this->m_CurrentPosition.data_block();
    const double               learningRate    = this->m_LearningRate;
    const InternalScalarType * gradient        = this->m_Gradient.data_block();
    InternalScalarType *       newPos          = newPosition.data_block();

    /** Advance one step: mu_{k+1} = mu_k - a_k * gradient_k */
    for( unsigned int j = jmin; j < jmax; j++ )
    {
      newPos[ j ] = currentPosition[ j ] - learningRate * gradient[ j ];
    }

  }  // end ThreadedAdvanceOneStep()

};

// end class Optimizer

//-------------------------------------------------------------------------------------

int
main( int argc, char * argv[] )
{
  // Declare and setup
  std::cout << std::fixed << std::showpoint << std::setprecision( 8 );
  std::cout << "RESULTS FOR InternalScalarType = " << typeid( InternalScalarType ).name()
            << "\n\n" << std::endl;

  /** Typedefs. */
  typedef OptimizerTEMP                  OptimizerClass;
  typedef OptimizerClass::ParametersType ParametersType;

  OptimizerClass::Pointer optimizer = OptimizerClass::New();

  // test parameters
  std::vector< unsigned int > arraySizes;
  arraySizes.push_back( 1e2 ); arraySizes.push_back( 1e3 ); arraySizes.push_back( 1e4 );
  arraySizes.push_back( 1e5 ); arraySizes.push_back( 1e6 ); arraySizes.push_back( 1e7 );
  std::vector< unsigned int > repetitions;
  repetitions.push_back( 2e7 ); repetitions.push_back( 2e6 ); repetitions.push_back( 2e5 );
  repetitions.push_back( 2e4 ); repetitions.push_back( 1e3 ); repetitions.push_back( 1e2 );

  /** For all sizes. */
  for( unsigned int s = 0; s < arraySizes.size(); ++s )
  {
    std::cout << "Array size = " << arraySizes[ s ] << std::endl;

    /** Setup. */
    itk::TimeProbesCollectorBase timeCollector;
    repetitions[ s ] = 1; // outcomment this line for full testing

    ParametersType newPos( arraySizes[ s ] );
    ParametersType curPos( arraySizes[ s ] );
    ParametersType gradient( arraySizes[ s ] );
    for( unsigned int i = 0; i < arraySizes[ s ]; ++i )
    {
      curPos[ i ]   = 2.1;
      gradient[ i ] = 2.1;
    }
    optimizer->m_NumberOfParameters = arraySizes[ s ];
    optimizer->m_LearningRate       = 3.67;
    optimizer->m_CurrentPosition    = curPos;
    optimizer->m_Gradient           = gradient;

    /** Time the ITK single-threaded implementation. */
    optimizer->m_UseOpenMP        = false;
    optimizer->m_UseEigen         = false;
    optimizer->m_UseMultiThreaded = false;
    for( unsigned int i = 0; i < repetitions[ s ]; ++i )
    {
      timeCollector.Start( "st" );
      optimizer->AdvanceOneStep();
      timeCollector.Stop( "st" );
    }

    /** Time the ITK multi-threaded implementation. */
    optimizer->m_UseOpenMP        = false;
    optimizer->m_UseEigen         = false;
    optimizer->m_UseMultiThreaded = true;
    unsigned int rep = repetitions[ s ] / 1000.0;
    if( rep < 10 ) { rep = 10; }
    for( unsigned int i = 0; i < rep; ++i )
    {
      timeCollector.Start( "ITK (mt)" );
      optimizer->AdvanceOneStep();
      timeCollector.Stop( "ITK (mt)" );
    }

    /** Time the OpenMP multi-threaded implementation. */
#ifdef ELASTIX_USE_OPENMP
    optimizer->m_UseOpenMP        = true;
    optimizer->m_UseEigen         = false;
    optimizer->m_UseMultiThreaded = true;
    if( arraySizes[ s ] < 10000 )
    {
      rep = repetitions[ s ] / 100.0;
      if( rep < 10 ) { rep = 10; }
    }
    else { rep = repetitions[ s ]; }
    for( unsigned int i = 0; i < rep; ++i )
    {
      timeCollector.Start( "OMP (mt)" );
      optimizer->AdvanceOneStep();
      timeCollector.Stop( "OMP (mt)" );
    }
#endif

    /** Time the Eigen single-threaded implementation. */
#ifdef ELASTIX_USE_EIGEN
    optimizer->m_UseOpenMP        = false;
    optimizer->m_UseEigen         = true;
    optimizer->m_UseMultiThreaded = true;
    for( unsigned int i = 0; i < repetitions[ s ]; ++i )
    {
      timeCollector.Start( "Eigen (st)" );
      optimizer->AdvanceOneStep();
      timeCollector.Stop( "Eigen (st)" );
    }
#endif

    // Report timings for this array size
    timeCollector.Report( std::cout, false, true );
    std::cout << std::endl;

  } // end loop over array sizes

  return EXIT_SUCCESS;

} // end main


#ifndef GREY_LEVEL_HISTOGRAM_HPP_45FTSEBNG7
#define GREY_LEVEL_HISTOGRAM_HPP_45FTSEBNG7

#include <log4cxx/logger.h>
#include <log4cxx/basicconfigurator.h>
#include <log4cxx/helpers/exception.h>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/unordered_map.hpp>
#include <boost/multi_array.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

//namespace std {using namespace __gnu_cxx;}
using namespace std;

namespace prhlt {
    typedef boost::unordered_map<int,float> sparse_histogram;

    class Grey_Level_Histogram{
        public:
            Grey_Level_Histogram();
            Grey_Level_Histogram(cv::Mat &ex_image);
            ~Grey_Level_Histogram();
            void set_image(cv::Mat &ex_image);
            float run();
            sparse_histogram histogram;
        private:
            cv::Mat image;
            log4cxx::LoggerPtr logger;
            float histogram_sum;
            float calculate_grey_level_histogram();
    };
}

#endif /* end of include guard: GREY_LEVEL_HISTOGRAM_HPP_45FTSEBNG7*/


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

// This file was created by Steffen Urban (urbste@googlemail.com) or
// company address (steffen.urban@zeiss.com)
// January 2019

#include "gtest/gtest.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include "theia/math/util.h"
#include "theia/sfm/pose/six_point_radial_distortion_homography.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/util/random.h"

namespace theia {
namespace {
using Eigen::AngleAxisd;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;

RandomNumberGenerator rng(53);

// Creates a test scenario from ground truth 3D points and ground truth rotation
// and translation. Projection noise is optional (set to 0 for no
// noise).
void GenerateDistortedImagePoints(
    const std::vector<Vector3d>& points_3d,
    const double projection_noise_std_dev,
    const Quaterniond& expected_rotation,
    const Vector3d& expected_translation,
    const double focal_length1,
    const double focal_length2,
    const double radial_distortion1,
    const double radial_distortion2,
    std::vector<Vector2d>* image_1_points_normalized,
    std::vector<Vector2d>* image_2_points_normalized,
    std::vector<Vector2d>* image_1_points,
    std::vector<Vector2d>* image_2_points) {
  image_1_points->reserve(points_3d.size());
  image_2_points->reserve(points_3d.size());

  for (int i = 0; i < points_3d.size(); i++) {
    Vector3d point3_cam1 = points_3d[i];
    Vector3d point3_cam2 =
        (expected_rotation * points_3d[i] + expected_translation);

    Vector2d distorted_point1, distorted_point2;
    DistortPoint(
        point3_cam1, focal_length1, radial_distortion1, distorted_point1);
    DistortPoint(
        point3_cam2, focal_length2, radial_distortion2, distorted_point2);

    image_1_points->push_back(distorted_point1);
    image_2_points->push_back(distorted_point2);

    if (projection_noise_std_dev) {
      for (int i = 0; i < points_3d.size(); i++) {
        AddNoiseToProjection(
            projection_noise_std_dev, &rng, &((*image_1_points)[i]));
        AddNoiseToProjection(
            projection_noise_std_dev, &rng, &((*image_2_points)[i]));
      }
    }

    Vector3d points2d_1, points2d_2;
    // normalize points with focal length (and principal point) estimate for
    // estimation
    UndistortPoint(distorted_point1, focal_length1, 0.0, points2d_1);
    UndistortPoint(distorted_point2, focal_length2, 0.0, points2d_2);

    image_1_points_normalized->push_back(points2d_1.hnormalized());
    image_2_points_normalized->push_back(points2d_2.hnormalized());
  }
}

// Run a test for the homography with at least 4 points.
void SixPointHomographyWithNoiseTest(const std::vector<Vector3d>& points_3d,
                                     const double projection_noise_std_dev,
                                     const Quaterniond& expected_rotation,
                                     const Vector3d& expected_translation,
                                     const double kMaxSymmetricError,
                                     const double focal_length1,
                                     const double focal_length2,
                                     const double radial_distortion1,
                                     const double radial_distortion2) {
  std::vector<Vector2d> image_1_points, image_points_1_normalized;
  std::vector<Vector2d> image_2_points, image_points_2_normalized;
  // generate distorted points for both cameras
  GenerateDistortedImagePoints(points_3d,
                               projection_noise_std_dev,
                               expected_rotation,
                               expected_translation,
                               focal_length1,
                               focal_length2,
                               radial_distortion1,
                               radial_distortion2,
                               &image_points_1_normalized,
                               &image_points_2_normalized,
                               &image_1_points,
                               &image_2_points);
  // Compute two-sided radial distortion homography matrix.
  std::vector<RadialHomographyResult> radial_homography_result;
  EXPECT_TRUE(SixPointRadialDistortionHomography(image_points_1_normalized,
                                                 image_points_2_normalized,
                                                 &radial_homography_result));
  bool one_correct_solution = false;
  for (int i = 0; i < radial_homography_result.size(); ++i) {
    // we need to scale the radial distortion values,
    // since we used normalized image points for estimation
    // radial_homography_result[i].l1 /= (focal_length1 * focal_length1);
    // radial_homography_result[i].l2 /= (focal_length2 * focal_length2);

    double sym_error = 0.0;
    for (int p = 0; p < image_1_points.size(); ++p) {
      sym_error += CheckRadialSymmetricError(radial_homography_result[i],
                                             image_1_points[p],
                                             image_2_points[p],
                                             focal_length1,
                                             focal_length2);
    }
    sym_error /= (double)image_1_points.size();
    if (sym_error < kMaxSymmetricError) {
      one_correct_solution = true;
    }
  }
  EXPECT_TRUE(one_correct_solution);
}

void BasicTest() {
  const std::vector<Vector3d> points_3d = {Vector3d(-1.0, 3.0, 1.0),
                                           Vector3d(1.0, -1.0, 1.0),
                                           Vector3d(-1.0, 1.0, 1.0),
                                           Vector3d(2.0, 1.0, 1.0),
                                           Vector3d(3.0, 1.0, 1.0),
                                           Vector3d(2.0, 2.0, 1.0)};

  const Quaterniond soln_rotation(
      AngleAxisd(DegToRad(10.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 0.0 / 512.0;
  const double kMaxSymmetricError = 1e-4;

  const double focal_length1 = 1500.0;
  const double focal_length2 = 1600.0;
  const double radial_distortion1 = -1e-7;
  const double radial_distortion2 = -2e-7;

  SixPointHomographyWithNoiseTest(points_3d,
                                  kNoise,
                                  soln_rotation,
                                  soln_translation,
                                  kMaxSymmetricError,
                                  focal_length1,
                                  focal_length2,
                                  radial_distortion1,
                                  radial_distortion2);
}

TEST(SixPointRadialHomography, BasicTest) { BasicTest(); }

TEST(SixPointRadialHomography, NoiseTest) {
  const std::vector<Vector3d> points_3d = {Vector3d(-1.0, 3.0, 1.0),
                                           Vector3d(1.0, -1.0, 1.0),
                                           Vector3d(-1.0, 1.0, 1.0),
                                           Vector3d(2.0, 1.0, 1.0),
                                           Vector3d(3.0, 1.0, 1.0),
                                           Vector3d(2.0, 2.0, 1.0)};

  const Quaterniond soln_rotation(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(0.0, 1.0, 1.0);
  const double kNoise = 0.5;
  const double kMaxSymmetricError = 2.0;

  const double focal_length1 = 1500.0;
  const double focal_length2 = 1600.0;
  const double radial_distortion1 = -1e-7;
  const double radial_distortion2 = -2e-7;

  SixPointHomographyWithNoiseTest(points_3d,
                                  kNoise,
                                  soln_rotation,
                                  soln_translation,
                                  kMaxSymmetricError,
                                  focal_length1,
                                  focal_length2,
                                  radial_distortion1,
                                  radial_distortion2);
}

}  // namespace
}  // namespace theia


#define BOOST_TEST_MODULE FmuExportCrossCompile_test1

#include <fmi4cpp/fmi2/fmi2.hpp>

#include <boost/test/unit_test.hpp>

#include <string>
#include <vector>

using namespace fmi4cpp::fmi2;

const std::string fmu_path = "../resources/fmus/2.0/cs/OpenModelica/v1.11.0/"
                        "FmuExportCrossCompile/modelDescription.xml";

BOOST_AUTO_TEST_CASE(FmuExportCrossCompile_test1)
{

    auto md = parse_model_description(fmu_path);

    BOOST_CHECK_EQUAL("structured", md->variable_naming_convention.value());

    std::vector<unknown> derivatives = md->model_structure->derivatives;
    BOOST_CHECK_EQUAL(2, derivatives.size());

    BOOST_CHECK_EQUAL(3, derivatives[0].index);
    BOOST_CHECK_EQUAL(4, derivatives[1].index);

    BOOST_CHECK_EQUAL(true, derivatives[0].dependencies.has_value());
    BOOST_CHECK_EQUAL(2, derivatives[0].dependencies.value()[0]);
    BOOST_CHECK_EQUAL("dependent", derivatives[0].dependencies_kind.value()[0]);
}


// (C) Copyright Jeremy Siek 2000-2004.
// 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 <functional>
#include <algorithm>
#include <iostream>
#include <boost/iterator/transform_iterator.hpp>

// What a bummer. We can't use std::binder1st with transform iterator
// because it does not have a default constructor. Here's a version
// that does.

namespace boost {

  template <class Operation>
  class binder1st {
  public:
    typedef typename Operation::result_type result_type;
    typedef typename Operation::second_argument_type argument_type;
  protected:
    Operation op;
    typename Operation::first_argument_type value;
  public:
    binder1st() { } // this had to be added!
    binder1st(const Operation& x,
              const typename Operation::first_argument_type& y)
        : op(x), value(y) {}
    typename Operation::result_type
    operator()(const typename Operation::second_argument_type& x) const {
      return op(value, x);
    }
  };

  template <class Operation, class T>
  inline binder1st<Operation> bind1st(const Operation& op, const T& x) {
    typedef typename Operation::first_argument_type arg1_type;
    return binder1st<Operation>(op, arg1_type(x));
  }

} // namespace boost

int
main(int, char*[])
{
  // This is a simple example of using the transform_iterators class to
  // generate iterators that multiply the value returned by dereferencing
  // the iterator. In this case we are multiplying by 2.
  // Would be cooler to use lambda library in this example.

  int x[] = { 1, 2, 3, 4, 5, 6, 7, 8 };
  const int N = sizeof(x)/sizeof(int);

  typedef boost::binder1st< std::multiplies<int> > Function;
  typedef boost::transform_iterator<Function, int*> doubling_iterator;

  doubling_iterator i(x, boost::bind1st(std::multiplies<int>(), 2)),
    i_end(x + N, boost::bind1st(std::multiplies<int>(), 2));

  std::cout << "multiplying the array by 2:" << std::endl;
  while (i != i_end)
    std::cout << *i++ << " ";
  std::cout << std::endl;

  std::cout << "adding 4 to each element in the array:" << std::endl;

  std::copy(boost::make_transform_iterator(x, boost::bind1st(std::plus<int>(), 4)),
            boost::make_transform_iterator(x + N, boost::bind1st(std::plus<int>(), 4)),
            std::ostream_iterator<int>(std::cout, " "));
  std::cout << std::endl;

  return 0;
}


// Copyright 2014 Jonathan Graehl-http://graehl.org/
//
// 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.
/**
   \file

   Traits for (n-)best paths of graphs/hypergraphs (useful for lazy_forest_kbest.hpp). Note: Your cost_type
   may be a<b means a better than b (mostly to simplify) or you can override better_cost(). better is used to
   order the heap and test convergence. updates is the same for viterbi but may always return true if you want
   to sum all paths? unsure.
 */

#ifndef GRAEHL_SHARED__PATH_TRAITS_HPP
#define GRAEHL_SHARED__PATH_TRAITS_HPP
#pragma once

#include <boost/graph/graph_traits.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <graehl/shared/epsilon.hpp>
#include <graehl/shared/nan.hpp>
#ifdef _MSC_VER
#undef min
#endif

namespace graehl {

// for additive costs (lower = better)
template <class Float = float>
struct cost_path_traits {
  typedef Float cost_type;
  static const bool viterbi
      = true;  // means updates() sometimes returns false. a<b with combine(a, a)=a would suffice
  static inline cost_type unreachable() { return std::numeric_limits<cost_type>::infinity(); }
  static inline cost_type start() { return 0; }
  static inline cost_type extend(cost_type a, cost_type b) { return a + b; }
  static inline cost_type extendBy(cost_type delta, cost_type& b) { return b += delta; }
  static inline cost_type retract(cost_type a, cost_type b) { return a - b; }
  static inline cost_type combine(cost_type a, cost_type b) { return std::min(a, b); }
  static inline bool better(cost_type a, cost_type const& b) { return a < b; }
  static inline bool update(cost_type candidate, cost_type& best) {
    if (candidate < best) {
      best = candidate;
      return true;
    }
    return false;
  }
  static inline bool updates(cost_type candidate, cost_type best) { return candidate < best; }
  static inline cost_type repeat(cost_type a, float n) { return a * n; }
  static inline bool includes(cost_type candidate,
                              cost_type best) {  // update(you can assert this after update)
    return !(candidate < best);
  }
  /**
     \return candidate isn't (much) better (lower) than best
  */
  static inline bool includes(cost_type candidate, cost_type best, float delta_relative) {
    assert(delta_relative >= 0);
    float delta = delta_relative;
    if (candidate < 0)  // relative delta; unweighted (relative to 1) if 0 candidate
      delta *= -candidate;
    if (candidate > 0) delta *= candidate;
    return !(candidate + delta < best);
  }
  /**
     \return a and b
  */
  static inline bool close_enough(cost_type a, cost_type b) {
    return few_ieee_apart_equate_inf_nan(a, b, 200);
    // 200 floats distance ~ 1 part in 50,000
  }
  // may be different from includes in the same way that better is different from update:
  static inline bool converged(cost_type improver, cost_type incumbent, cost_type epsilon) {
    return includes(improver, incumbent,
                    epsilon);  // may be different for other cost types (because float may not = cost_Type)
  }
};

template <class G>
struct path_traits : cost_path_traits<float> {};

/*
  template <class G>
  static inline bool converged(typename path_traits<G>::cost_type const& improver, typename
  path_traits<G>::cost_type const& incumbent
  ,typename path_traits<G>::cost_type const& epsilon)
  {
  typedef path_traits<G> PT;
  return PT::better(incumbent, PT::combine(improver, epsilon));
  }
*/

// for graphs which have edges with sources (plural) and not source - ordered multihypergraphs
template <class G>
struct edge_traits {
  typedef typename path_traits<G>::cost_type cost_type;
  typedef boost::graph_traits<G> GT;
  typedef unsigned tail_descriptor;
  typedef boost::counting_iterator<tail_descriptor> tail_iterator;
  typedef unsigned tails_size_type;  // must always be unsigned. for now
};

/*
  free fns (ADL):

  none. statics in path_traits, instead
*/

template <class G>
struct updates_cost {
  typedef path_traits<G> PT;
  typedef typename PT::cost_type cost_type;
  typedef bool result_type;
  bool operator()(cost_type const& a, cost_type const& b) const { return PT::updates(a, b); }
};

template <class G>
struct better_cost {
  typedef path_traits<G> PT;
  typedef typename PT::cost_type cost_type;
  typedef bool result_type;
  bool operator()(cost_type const& a, cost_type const& b) const { return PT::better(a, b); }
};

}

#endif


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward 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_SDK_META_FLOATING_HPP_INCLUDED
#define NT2_SDK_META_FLOATING_HPP_INCLUDED

#include <nt2/sdk/meta/make_real.hpp>
#include <nt2/extension/parameters.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>

namespace nt2  { namespace meta
{
  struct floating
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct  result<This(A0)>
    {
      typedef float base_real;
      BOOST_STATIC_CONSTANT ( std::size_t
                            , value = (sizeof(A0) > sizeof(base_real))
                                    ?  sizeof(A0) : sizeof(base_real)
                            );
      typedef typename meta::make_real<value>::type type;
    };

    #define M1(z,n,t)                                               \
    BOOST_STATIC_CONSTANT ( std::size_t                             \
                          , BOOST_PP_CAT(value,BOOST_PP_INC(n)) =   \
                            (   BOOST_PP_CAT(value,n)               \
                              > sizeof(BOOST_PP_CAT(A,n))           \
                            ) ? BOOST_PP_CAT(value,n)               \
                              : sizeof(BOOST_PP_CAT(A,n))           \
                          );                                        \
    /**/
    #define M0(z,n,t)                                                     \
    template<class This,BOOST_PP_ENUM_PARAMS(n,class A)>                  \
    struct  result<This(BOOST_PP_ENUM_PARAMS(n,A))>                       \
    {                                                                     \
      typedef float base_real;                                            \
      BOOST_STATIC_CONSTANT ( std::size_t, value0 =  sizeof(base_real) ); \
      BOOST_PP_REPEAT(n,M1,~)                                             \
      typedef typename                                                    \
      meta::make_real<BOOST_PP_CAT(value,n)>::type type;                  \
    };                                                                    \
     /**/

    BOOST_PP_REPEAT_FROM_TO(2,NT2_MAX_ARITY,M0,~)
    #undef M1
    #undef M0
  };
} }

#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_thread_loop"

#include <boost/test/unit_test.hpp>
#include "nanocv/timer.h"
#include "nanocv/scalar.h"
#include "nanocv/string.h"
#include "nanocv/tabulator.h"
#include "nanocv/math/abs.hpp"
#include "nanocv/math/stats.hpp"
#include "nanocv/thread/loopi.hpp"
#include <iostream>
#include <numeric>

namespace test
{
        using namespace ncv;

        // run loop for the given number of trials using no threads
        template
        <
                typename toperator
        >
        stats_t<scalar_t> test_cpu(int size, int trials, toperator op)
        {
                stats_t<scalar_t> timings;
                for (int t = 0; t < trials; t ++)
                {
                        const ncv::timer_t timer;

                        for (int i = 0; i < size; i ++)
                        {
                                op(i);
                        }

                        timings(timer.miliseconds());
                }

                return timings;
        }

#ifdef _OPENMP
        // run loop for the given number of trials using OpenMP
        template
        <
                typename toperator
        >
        stats_t<scalar_t> test_omp(int size, int trials, toperator op)
        {
                stats_t<scalar_t> timings;
                for (int t = 0; t < trials; t ++)
                {
                        const ncv::timer_t timer;

                        #pragma omp parallel for schedule(static)
                        for (int i = 0; i < size; i ++)
                        {
                                op(i);
                        }

                        timings(timer.miliseconds());
                }

                return timings;
        }
#endif

        // run loop for the given number of trials using thread_loop
        template
        <
                typename toperator
        >
        stats_t<scalar_t> test_ncv(int size, int trials, toperator op)
        {
                stats_t<scalar_t> timings;
                for (int t = 0; t < trials; t ++)
                {
                        const ncv::timer_t timer;

                        thread_loopi(size, op);

                        timings(timer.miliseconds());
                }

                return timings;
        }

        // run loop for the given number of trials using thread_loop
        template
        <
                typename toperator
        >
        stats_t<scalar_t> test_ncv_pool(int size, int trials, toperator op)
        {
                static thread_pool_t pool;

                stats_t<scalar_t> timings;
                for (int t = 0; t < trials; t ++)
                {
                        const ncv::timer_t timer;

                        thread_loopi(size, pool, op);

                        timings(timer.miliseconds());
                }

                return timings;
        }

        string_t to_string(const stats_t<scalar_t>& timings)
        {
                return text::to_string(timings.avg()) + " +/- " + text::to_string(timings.stdev());
        }
}

BOOST_AUTO_TEST_CASE(test_thread_loop)
{
        using namespace ncv;

        const size_t min_size = 37;
        const size_t max_size = 256 * 1024;

        const size_t trials = 16;

        tabulator_t table("method");
        table.header() << "cpu";
#ifdef _OPENMP
        table.header() << "openmp";
#endif
        table.header() << "nanocv";
        table.header() << "nanocv(pool)";

        // test for different problems size
        for (size_t size = min_size; size <= max_size; size *= 3)
        {
                scalars_t results(size);

                // operator to test
                auto op = [&](int i)
                {
                        results[i] = std::cos(i + 0.0) + std::sin(i + 0.0);

                        double temp = 0.0;
                        for (int j = 0; j < 16; j ++)
                        {
                                temp += j * std::tan(-i + j);
                        }

                        results[i] *= temp / 32.0;
                };

                // 1CPU
                std::fill(std::begin(results), std::end(results), 0.0);
                const stats_t<scalar_t> timings_cpu = test::test_cpu(size, trials, op);
                const scalar_t sum_cpu = std::accumulate(std::begin(results), std::end(results), 0.0);

#ifdef _OPENMP
                // OpenMP
                std::fill(std::begin(results), std::end(results), 0.0);
                const stats_t<scalar_t> timings_omp = test::test_omp(size, trials, op);
                const scalar_t sum_omp = std::accumulate(std::begin(results), std::end(results), 0.0);
#endif

                // NanoCV threads
                std::fill(std::begin(results), std::end(results), 0.0);
                const stats_t<scalar_t> timings_ncv = test::test_ncv(size, trials, op);
                const scalar_t sum_ncv = std::accumulate(std::begin(results), std::end(results), 0.0);

                // NanoCV threads with reusing the thread pool
                std::fill(std::begin(results), std::end(results), 0.0);
                const stats_t<scalar_t> timings_ncv_pool = test::test_ncv_pool(size, trials, op);
                const scalar_t sum_ncv_loop = std::accumulate(std::begin(results), std::end(results), 0.0);

                table.append("test [" + text::to_string(size) + "]")
                << test::to_string(timings_cpu)
#ifdef _OPENMP
                << test::to_string(timings_omp)
#endif
                << test::to_string(timings_ncv)
                << test::to_string(timings_ncv_pool);

                // Check accuracy
                const scalar_t eps = 1e-12;
                BOOST_CHECK_LE(math::abs(sum_cpu - sum_cpu), eps);
#ifdef _OPENMP
                BOOST_CHECK_LE(math::abs(sum_cpu - sum_omp), eps);
#endif
                BOOST_CHECK_LE(math::abs(sum_cpu - sum_ncv), eps);
                BOOST_CHECK_LE(math::abs(sum_cpu - sum_ncv_loop),  eps);
        }

        table.print(std::cout);
}


// Copyright Aleksey Gurtovoy 2001-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/transform_view.hpp>

#include <boost/mpl/max_element.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef list<int,long,char,char[50],double> types;
    typedef list<
        sizeof_<int>::type,
        sizeof_<long>::type,
        sizeof_<char>::type,
        sizeof_<char[50]>::type,
        sizeof_<double>::type
    > sizes;

    MPL_ASSERT(( equal< transform_view< types, sizeof_<_> >::type,sizes > ));

    typedef max_element<
          transform_view< types, sizeof_<_> >
        >::type iter;

    MPL_ASSERT_RELATION( deref<iter>::type::value, ==, 50 );
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-predicates
    Function object implementing is_not_nan capabilities

    Returns @ref False or @ref True according x is @ref Nan or not.

    @par Semantic:

    @code
    auto r = is_not_nan(x);
    @endcode

    is similar to:

    @code
    auto r = x == x;
    @endcode

  **/
  as_logical_t<Value> is_not_nan(Value const& x);
} }
#endif

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

#endif


#pragma once

#include <vector>
#include <fstream>
#include <array>
#include <Eigen/Dense>

#include "types.hpp"

namespace Discregrid
{

class DiscreteGrid
{
public:

	using CoefficientVector = Eigen::Matrix<real, 32, 1>;
	using ContinuousFunction = std::function<real(Vector3r const&)>;
	using MultiIndex = Eigen::Vector3i;
	using Predicate = std::function<bool(Vector3r const&, real)>;
	using SamplePredicate = std::function<bool(Vector3r const&)>;

	DiscreteGrid() = default;
	DiscreteGrid(AlignedBox3r const& domain, Eigen::Vector3i const& resolution)
		: m_domain(domain), m_resolution(resolution), m_n_fields(0u)
	{
		m_cell_size = domain.diagonal().cwiseQuotient(resolution.cast<real>());
		m_inv_cell_size = m_cell_size.cwiseInverse();
		m_n_cells = resolution.prod();
	}
	DiscreteGrid(AlignedBox3r const& minimum_domain, Vector3r const& cell_size)
	    : m_cell_size(cell_size), m_n_fields(0u)
	{
        m_inv_cell_size = m_cell_size.cwiseInverse();
	    Vector3r res_f = minimum_domain.diagonal().cwiseProduct(m_inv_cell_size);
	    Eigen::Vector3i res = res_f.array().ceil().cast<int>();
	    m_resolution[0] = res[0]; m_resolution[1] = res[1]; m_resolution[2] = res[2];
	    Vector3r domain_size = m_cell_size.cwiseProduct(res.cast<real>());
	    Vector3r domain_center = minimum_domain.center();
	    m_domain = AlignedBox3r(domain_center - 0.5*domain_size, domain_center + 0.5*domain_size);
	    m_n_cells = res.prod();
	}
	virtual ~DiscreteGrid() = default;

	virtual void save(std::string const& filename) const = 0;
	virtual void load(std::string const& filename) = 0;

	virtual int addFunction(ContinuousFunction const& func, bool verbose = false,
		SamplePredicate const& pred = nullptr) = 0;

	real interpolate(Vector3r const& xi, Vector3r* gradient = nullptr) const
	{
		return interpolate(0u, xi, gradient);
	}

	virtual real interpolate(int field_id, Vector3r const& xi,
		Vector3r* gradient = nullptr) const = 0;

	/**
	 * @brief Determines the shape functions for the discretization with ID field_id at point xi.
	 *
	 * @param field_id Discretization ID
	 * @param x Location where the shape functions should be determined
	 * @param cell cell of x
	 * @param c0 vector required for the interpolation
	 * @param N	shape functions for the cell of x
	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
	 * @return Success of the function.
	 */
	virtual bool determineShapeFunctions(int field_id, Vector3r const &x,
		std::array<int, 32> &cell, Vector3r &c0, Eigen::Matrix<real, 32, 1> &N,
		Eigen::Matrix<real, 32, 3> *dN = nullptr) const = 0;

	/**
	 * @brief Evaluates the given discretization with ID field_id at point xi.
	 *
	 * @param field_id Discretization ID
	 * @param xi Location where the discrete function is evaluated
	 * @param cell cell of xi
	 * @param c0 vector required for the interpolation
	 * @param N	shape functions for the cell of xi
	 * @param gradient (Optional) if a pointer to a vector is passed the gradient of the discrete function will be evaluated
	 * @param dN (Optional) derivatives of the shape functions, required to compute the gradient
	 * @return real Results of the evaluation of the discrete function at point xi
	 */
	virtual real interpolate(int field_id, Vector3r const& xi, const std::array<int, 32> &cell, const Vector3r &c0, const Eigen::Matrix<real, 32, 1> &N,
		Vector3r* gradient = nullptr, Eigen::Matrix<real, 32, 3> *dN = nullptr) const = 0;

	virtual void reduceField(int field_id, Predicate pred) {}

	MultiIndex singleToMultiIndex(int i) const;
	int multiToSingleIndex(MultiIndex const& ijk) const;

	AlignedBox3r subdomain(MultiIndex const& ijk) const;
	AlignedBox3r subdomain(int l) const;

	AlignedBox3r const& domain() const { return m_domain; }
	Eigen::Vector3i const& resolution() const { return m_resolution; };
	Vector3r const& cellSize() const { return m_cell_size;}
	Vector3r const& invCellSize() const { return m_inv_cell_size;}

protected:

	AlignedBox3r m_domain;
	Eigen::Vector3i m_resolution;
	Vector3r m_cell_size;
	Vector3r m_inv_cell_size;
	std::size_t m_n_cells;
	std::size_t m_n_fields;
};
}


#include <Eigen/Geometry>
#include <Eigen/Jacobi>
#include <Eigen/SVD>
#include <stdio.h>
#include <iostream>
#include <arc_utilities/pretty_print.hpp>
#include <functional>
#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <arc_utilities/eigen_helpers.hpp>

#ifndef EIGEN_HELPERS_CONVERSIONS_HPP
#define EIGEN_HELPERS_CONVERSIONS_HPP

namespace EigenHelpersConversions
{
    inline Eigen::Vector3d GeometryPointToEigenVector3d(const geometry_msgs::Point& point)
    {
        Eigen::Vector3d eigen_point(point.x, point.y, point.z);
        return eigen_point;
    }

    inline geometry_msgs::Point EigenVector3dToGeometryPoint(const Eigen::Vector3d& point)
    {
        geometry_msgs::Point geom_point;
        geom_point.x = point.x();
        geom_point.y = point.y();
        geom_point.z = point.z();
        return geom_point;
    }

    inline Eigen::Vector4d GeometryPointToEigenVector4d(const geometry_msgs::Point& point)
    {
        Eigen::Vector4d eigen_point(point.x, point.y, point.z, 1.0);
        return eigen_point;
    }

    inline geometry_msgs::Point EigenVector4dToGeometryPoint(const Eigen::Vector4d& point)
    {
        geometry_msgs::Point geom_point;
        geom_point.x = point(0);
        geom_point.y = point(1);
        geom_point.z = point(2);
        return geom_point;
    }

    inline geometry_msgs::PointStamped EigenVector3dToGeometryPointStamped(const Eigen::Vector3d& point, const std::string& frame_id)
    {
        geometry_msgs::PointStamped point_stamped;
        point_stamped.header.frame_id = frame_id;
        point_stamped.point = EigenVector3dToGeometryPoint(point);
        return point_stamped;
    }

    inline Eigen::Vector3d GeometryVector3ToEigenVector3d(const geometry_msgs::Vector3& vector)
    {
        Eigen::Vector3d eigen_vector(vector.x, vector.y, vector.z);
        return eigen_vector;
    }

    inline geometry_msgs::Vector3 EigenVector3dToGeometryVector3(const Eigen::Vector3d& vector)
    {
        geometry_msgs::Vector3 geom_vector;
        geom_vector.x = vector.x();
        geom_vector.y = vector.y();
        geom_vector.z = vector.z();
        return geom_vector;
    }

    inline Eigen::Vector4d GeometryVector3ToEigenVector4d(const geometry_msgs::Vector3& vector)
    {
        Eigen::Vector4d eigen_vector(vector.x, vector.y, vector.z, 0.0);
        return eigen_vector;
    }

    inline geometry_msgs::Vector3 EigenVector4dToGeometryVector3(const Eigen::Vector4d& vector)
    {
        geometry_msgs::Vector3 geom_vector;
        geom_vector.x = vector(0);
        geom_vector.y = vector(1);
        geom_vector.z = vector(2);
        return geom_vector;
    }

    inline Eigen::Quaterniond GeometryQuaternionToEigenQuaterniond(const geometry_msgs::Quaternion& quat)
    {
        Eigen::Quaterniond eigen_quaternion(quat.w, quat.x, quat.y, quat.z);
        return eigen_quaternion;
    }

    inline geometry_msgs::Quaternion EigenQuaterniondToGeometryQuaternion(const Eigen::Quaterniond& quat)
    {
        geometry_msgs::Quaternion geom_quaternion;
        geom_quaternion.w = quat.w();
        geom_quaternion.x = quat.x();
        geom_quaternion.y = quat.y();
        geom_quaternion.z = quat.z();
        return geom_quaternion;
    }

    inline Eigen::Isometry3d GeometryPoseToEigenIsometry3d(const geometry_msgs::Pose& pose)
    {
        const Eigen::Translation3d trans(pose.position.x, pose.position.y, pose.position.z);
        const Eigen::Quaterniond quat(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
        const Eigen::Isometry3d eigen_pose = trans * quat;
        return eigen_pose;
    }

    inline geometry_msgs::Pose EigenIsometry3dToGeometryPose(const Eigen::Isometry3d& transform)
    {
        const Eigen::Vector3d trans = transform.translation();
        const Eigen::Quaterniond quat(transform.rotation());
        geometry_msgs::Pose geom_pose;
        geom_pose.position.x = trans.x();
        geom_pose.position.y = trans.y();
        geom_pose.position.z = trans.z();
        geom_pose.orientation.w = quat.w();
        geom_pose.orientation.x = quat.x();
        geom_pose.orientation.y = quat.y();
        geom_pose.orientation.z = quat.z();
        return geom_pose;
    }

    inline geometry_msgs::PoseStamped EigenIsometry3dToGeometryPoseStamped(const Eigen::Isometry3d& transform, const std::string& frame_id)
    {
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.frame_id = frame_id;
        pose_stamped.pose = EigenIsometry3dToGeometryPose(transform);
        return pose_stamped;
    }

    inline Eigen::Isometry3d GeometryTransformToEigenIsometry3d(const geometry_msgs::Transform& transform)
    {
        const Eigen::Translation3d trans(transform.translation.x, transform.translation.y, transform.translation.z);
        const Eigen::Quaterniond quat(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
        const Eigen::Isometry3d eigen_transform = trans * quat;
        return eigen_transform;
    }

    inline geometry_msgs::Transform EigenIsometry3dToGeometryTransform(const Eigen::Isometry3d& transform)
    {
        const Eigen::Vector3d trans = transform.translation();
        const Eigen::Quaterniond quat(transform.rotation());
        geometry_msgs::Transform geom_transform;
        geom_transform.translation.x = trans.x();
        geom_transform.translation.y = trans.y();
        geom_transform.translation.z = trans.z();
        geom_transform.rotation.w = quat.w();
        geom_transform.rotation.x = quat.x();
        geom_transform.rotation.y = quat.y();
        geom_transform.rotation.z = quat.z();
        return geom_transform;
    }

    inline geometry_msgs::TransformStamped EigenIsometry3dToGeometryTransformStamped(const Eigen::Isometry3d& transform, const std::string& frame_id)
    {
        geometry_msgs::TransformStamped transform_stamped;
        transform_stamped.header.frame_id = frame_id;
        transform_stamped.transform = EigenIsometry3dToGeometryTransform(transform);
        return transform_stamped;
    }

    inline Eigen::Matrix3Xd VectorGeometryPointToEigenMatrix3Xd(const std::vector<geometry_msgs::Point>& vector_geom)
    {
        Eigen::Matrix3Xd eigen_matrix = Eigen::MatrixXd(3, vector_geom.size());
        for (size_t idx = 0; idx < vector_geom.size(); idx++)
        {
            eigen_matrix.block<3,1>(0, idx) = GeometryPointToEigenVector3d(vector_geom[idx]);
        }
        return eigen_matrix;
    }

    inline std::vector<geometry_msgs::Point> EigenMatrix3XdToVectorGeometryPoint(const Eigen::Matrix3Xd& eigen_matrix)
    {
        std::vector<geometry_msgs::Point> vector_geom(eigen_matrix.cols());
        for (size_t idx = 0; idx < vector_geom.size(); idx++)
        {
            vector_geom[idx] = EigenVector3dToGeometryPoint(eigen_matrix.block<3,1>(0,idx));
        }
        return vector_geom;
    }

    inline std::vector<geometry_msgs::Point> VectorEigenVector3dToVectorGeometryPoint(const EigenHelpers::VectorVector3d& vector_eigen)
    {
        std::vector<geometry_msgs::Point> vector_geom(vector_eigen.size());
        for (size_t idx = 0; idx < vector_eigen.size(); idx++)
        {
            vector_geom[idx] = EigenVector3dToGeometryPoint(vector_eigen[idx]);
        }
        return vector_geom;
    }

    inline EigenHelpers::VectorVector3d VectorGeometryPointToVectorEigenVector3d(const std::vector<geometry_msgs::Point>& vector_geom)
    {
        EigenHelpers::VectorVector3d vector_eigen(vector_geom.size());
        for (size_t idx = 0; idx < vector_geom.size(); idx++)
        {
            vector_eigen[idx] = GeometryPointToEigenVector3d(vector_geom[idx]);
        }
        return vector_eigen;
    }

    inline EigenHelpers::VectorVector3d VectorGeometryVector3ToEigenVector3d(const std::vector<geometry_msgs::Vector3>& vector_geom)
    {
        EigenHelpers::VectorVector3d vector_eigen(vector_geom.size());
        for (size_t idx = 0; idx < vector_geom.size(); idx++)
        {
            vector_eigen[idx] = GeometryVector3ToEigenVector3d(vector_geom[idx]);
        }
        return vector_eigen;
    }

    inline EigenHelpers::VectorIsometry3d VectorGeometryPoseToVectorIsometry3d(const std::vector<geometry_msgs::Pose>& vector_geom)
    {
        EigenHelpers::VectorIsometry3d vector_eigen(vector_geom.size());
        for (size_t idx = 0; idx < vector_geom.size(); idx++)
        {
            vector_eigen[idx] = GeometryPoseToEigenIsometry3d(vector_geom[idx]);
        }
        return vector_eigen;
    }

    inline EigenHelpers::VectorIsometry3d VectorGeometryPoseToVectorIsometry3d(const std::vector<geometry_msgs::Transform>& vector_geom)
    {
        EigenHelpers::VectorIsometry3d vector_eigen(vector_geom.size());
        for (size_t idx = 0; idx < vector_geom.size(); idx++)
        {
            vector_eigen[idx] = GeometryTransformToEigenIsometry3d(vector_geom[idx]);
        }
        return vector_eigen;
    }

    inline std::vector<geometry_msgs::Pose> VectorIsometry3dToVectorGeometryPose(const EigenHelpers::VectorIsometry3d& vector_eigen)
    {
        std::vector<geometry_msgs::Pose> vector_geom(vector_eigen.size());
        for (size_t idx = 0; idx < vector_eigen.size(); idx++)
        {
            vector_geom[idx] = EigenIsometry3dToGeometryPose(vector_eigen[idx]);
        }
        return vector_geom;
    }

    inline std::vector<geometry_msgs::Transform> VectorIsometry3dToVectorGeometryTransform(const EigenHelpers::VectorIsometry3d& vector_eigen)
    {
        std::vector<geometry_msgs::Transform> vector_geom(vector_eigen.size());
        for (size_t idx = 0; idx < vector_eigen.size(); idx++)
        {
            vector_geom[idx] = EigenIsometry3dToGeometryTransform(vector_eigen[idx]);
        }
        return vector_geom;
    }

    template<typename data_type, int LENGTH>
    inline Eigen::Matrix<data_type, Eigen::Dynamic, 1> VectorEigenVectorToEigenVectorX(const std::vector<Eigen::Matrix<data_type, LENGTH, 1>>& vector_eigen_input)
    {
        assert(vector_eigen_input.size() > 0);

        Eigen::Matrix<data_type, Eigen::Dynamic, 1> eigen_result;
        eigen_result.resize((ssize_t)vector_eigen_input.size() * vector_eigen_input[0].rows());

        for (size_t idx = 0; idx < vector_eigen_input.size(); idx++)
        {
            eigen_result.segment((ssize_t)idx * LENGTH, LENGTH) = vector_eigen_input[idx];
        }

        return eigen_result;
    }

    template<typename data_type, int LENGTH>
    inline std::vector<Eigen::Matrix<data_type, LENGTH, 1>, Eigen::aligned_allocator<Eigen::Matrix<data_type, LENGTH, 1>>> EigenVectorXToVectorEigenVector(const Eigen::Matrix<data_type, Eigen::Dynamic, 1>& eigen_input)
    {
        assert(eigen_input.rows() % LENGTH == 0);
        size_t num_vectors = eigen_input.rows() / LENGTH;

        std::vector<Eigen::Matrix<data_type, LENGTH, 1>, Eigen::aligned_allocator<Eigen::Matrix<data_type, LENGTH, 1>>> vector_eigen_output(num_vectors);

        for (size_t idx = 0; idx < num_vectors; idx++)
        {
            vector_eigen_output[idx] = eigen_input.segment<LENGTH>((ssize_t)idx * LENGTH);
        }

        return vector_eigen_output;
    }
}

#endif // EIGEN_HELPERS_CONVERSIONS_HPP


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

#include <lagrange/common.h>
#include <lagrange/create_mesh.h>
#include <lagrange/experimental/IndexedAttributeManager.h>

namespace {

template <typename T, int DIM>
using TestArray = Eigen::Matrix<T, Eigen::Dynamic, DIM, Eigen::RowMajor>;

template <typename TargetValueDerived, typename TargetIndexDerived>
void check_indexed_attribute(
    lagrange::experimental::IndexedAttributeManager& manager,
    const std::string& name,
    const Eigen::MatrixBase<TargetValueDerived>& target_values,
    const Eigen::MatrixBase<TargetIndexDerived>& target_indices,
    void* values_ptr = nullptr,
    void* indices_ptr = nullptr)
{
    REQUIRE(manager.has(name));

    using ValueDerived = Eigen::Matrix<
        typename TargetValueDerived::Scalar,
        Eigen::Dynamic,
        Eigen::Dynamic,
        TargetValueDerived::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor>;
    using IndexDerived = Eigen::Matrix<
        typename TargetIndexDerived::Scalar,
        Eigen::Dynamic,
        Eigen::Dynamic,
        TargetIndexDerived::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor>;
    auto values_view = manager.view_values<ValueDerived>(name);
    auto indices_view = manager.view_indices<IndexDerived>(name);

    REQUIRE(values_view == target_values);
    REQUIRE(indices_view == target_indices);

    if (values_ptr != nullptr) {
        // When data pointers differ, data must be copied into the array.
        REQUIRE(values_view.data() == values_ptr);
    }
    if (indices_ptr != nullptr) {
        // When data pointers differ, data must be copied into the array.
        REQUIRE(indices_view.data() == indices_ptr);
    }
}

} // namespace

TEST_CASE("experimental/IndexedAttributes", "[attribute][indexed]")
{
    using namespace lagrange;

    SECTION("Simple usage")
    {
        experimental::IndexedAttributeManager manager;

        TestArray<int, 3> int_array(3, 3);
        int_array.setZero();

        TestArray<float, 3> float_array(3, 3);
        float_array.setOnes();

        manager.add("int_attr");
        manager.set("int_attr", int_array, int_array);

        REQUIRE(manager.has("int_attr"));

        manager.add("float_attr", float_array, int_array);
        REQUIRE(manager.has("float_attr"));

        int_array.setOnes();
        manager.set("int_attr", int_array, int_array);

        REQUIRE(manager.view_values<TestArray<int, 3>>("int_attr").minCoeff() == 1);
        REQUIRE(manager.view_values<TestArray<int, 3>>("int_attr").maxCoeff() == 1);
        REQUIRE(manager.view_indices<TestArray<int, 3>>("int_attr").minCoeff() == 1);
        REQUIRE(manager.view_indices<TestArray<int, 3>>("int_attr").maxCoeff() == 1);

        float_array.setOnes();
        manager.set("float_attr", float_array, int_array);
        REQUIRE(manager.view_values<TestArray<float, 3>>("float_attr").minCoeff() == 1);
        REQUIRE(manager.view_values<TestArray<float, 3>>("float_attr").maxCoeff() == 1);
        REQUIRE(manager.view_indices<TestArray<int, 3>>("float_attr").minCoeff() == 1);
        REQUIRE(manager.view_indices<TestArray<int, 3>>("float_attr").maxCoeff() == 1);

        SECTION("Update")
        {
            auto arr_int = manager.view_values<TestArray<int, 3>>("int_attr");
            auto arr_float = manager.view_values<TestArray<float, 3>>("float_attr");

            arr_int(0, 0) = 10;
            arr_float(0, 0) = 11;

            REQUIRE(manager.view_values<TestArray<int, 3>>("int_attr")(0, 0) == arr_int(0, 0));
            REQUIRE(
                manager.view_values<TestArray<float, 3>>("float_attr")(0, 0) == arr_float(0, 0));
        }
    }

    SECTION("Check for copies")
    {
        experimental::IndexedAttributeManager manager;

        {
            TestArray<short, 3> values(3, 3);
            values.setIdentity();
            TestArray<uint64_t, 3> indices(3, 3);
            indices << 0, 0, 0, 1, 1, 1, 2, 2, 2;
            manager.add("test", values, indices);
        }

        REQUIRE(manager.has("test"));

        // The return type of view_values is an Eigen::Map, so no need
        // to capture it by reference.
        auto values_1 = manager.view_values<TestArray<short, 3>>("test");
        auto values_2 = manager.view_values<TestArray<short, 3>>("test");
        REQUIRE(values_1.data() == values_2.data());

        // To make a copy of the data, fully specify the result type.
        TestArray<short, 3> values_3 = manager.view_values<TestArray<short, 3>>("test");
        REQUIRE(values_1.data() != values_3.data());

        SECTION("What if I remove the attribute?")
        {
            manager.remove("test");
            REQUIRE(!manager.has("test"));
            // TODO:
            // REQUIRE(values_1(0, 0) == 1);
            // ^ This is a bad idea, because it is accessing invalid memory.
            //   Not sure how to prevent this at compile time or check it at run
            //   time.
        }
    }

    SECTION("Creation")
    {
        experimental::IndexedAttributeManager manager;

        SECTION("Create by copying Eigen matrices")
        {
            TestArray<float, 3> values(3, 3);
            values.setOnes();
            TestArray<int, 3> indices(3, 3);
            indices.setZero();

            manager.add("test", values, indices);
            check_indexed_attribute(manager, "test", values, indices);
            manager.remove("test");
        }

        SECTION("Create by moving Eigen matrices")
        {
            TestArray<float, 3> values(3, 3);
            values.setOnes();
            TestArray<int, 3> indices(3, 3);
            indices.setZero();

            void* values_ptr = values.data();
            void* indices_ptr = indices.data();
            auto values_copy = values;
            auto indices_copy = indices;

            manager.add("test", std::move(values), std::move(indices));
            check_indexed_attribute(
                manager,
                "test",
                values_copy,
                indices_copy,
                values_ptr,
                indices_ptr);
            manager.remove("test");
        }

        SECTION("Create by copying Eigen::Map, which will result in copying")
        {
            std::vector<float> values(9, 0);
            std::vector<int> indices(9, 1);

            Eigen::Map<TestArray<float, 3>> values_map(values.data(), 3, 3);
            Eigen::Map<TestArray<int, 3>> indices_map(indices.data(), 3, 3);

            manager.add("test", values_map, indices_map);
            check_indexed_attribute(manager, "test", values_map, indices_map);
            manager.remove("test");
        }

        SECTION("Create by moving Eigen::Map, which will also result in copy")
        {
            std::vector<float> values(9, 0);
            std::vector<int> indices(9, 1);

            Eigen::Map<TestArray<float, 3>> values_map(values.data(), 3, 3);
            Eigen::Map<TestArray<int, 3>> indices_map(indices.data(), 3, 3);

            TestArray<float, 3> values_copy = values_map;
            TestArray<int, 3> indices_copy = indices_map;

            manager.add("test", std::move(values_map), std::move(indices_map));
            check_indexed_attribute(manager, "test", values_copy, indices_copy);
            manager.remove("test");
        }

        SECTION("Create with Array, no copying")
        {
            TestArray<float, 3> values(3, 3);
            values.setOnes();
            TestArray<int, 3> indices(3, 3);
            indices.setZero();

            auto values_array = to_shared_ptr(experimental::create_array(values));
            auto indices_array = to_shared_ptr(experimental::create_array(indices));

            manager.add("test", values_array, indices_array);
            check_indexed_attribute(
                manager,
                "test",
                values,
                indices,
                values_array->data(),
                indices_array->data());
            manager.remove("test");
        }

        SECTION("Create with wrapped Array, no copying")
        {
            TestArray<float, 3> values(3, 3);
            values.setOnes();
            TestArray<int, 3> indices(3, 3);
            indices.setZero();

            auto values_array = to_shared_ptr(experimental::wrap_with_array(values));
            auto indices_array = to_shared_ptr(experimental::wrap_with_array(indices));

            manager.add("test", values_array, indices_array);
            check_indexed_attribute(
                manager,
                "test",
                values,
                indices,
                values.data(),
                indices.data());
            manager.remove("test");
        }

        SECTION("Mixed type creation")
        {
            TestArray<float, 3> values(3, 3);
            values.setOnes();
            TestArray<int, 3> indices(3, 3);
            indices.setZero();

            SECTION("Moved Eigen::Matrix + copied Eigen::Matrix")
            {
                auto values_copy = values;
                void* values_copy_ptr = values_copy.data();

                manager.add("test", std::move(values_copy), indices);
                check_indexed_attribute(manager, "test", values, indices, values_copy_ptr);
                manager.remove("test");
            }

            SECTION("Array + Eigen::Matrix")
            {
                auto values_array = to_shared_ptr(experimental::create_array(values));

                manager.add("test", values_array, indices);
                check_indexed_attribute(manager, "test", values, indices, values_array->data());
                manager.remove("test");
            }

            SECTION("Array + Eigen::Map")
            {
                auto values_array = to_shared_ptr(experimental::create_array(values));
                Eigen::Map<TestArray<int, 3>> indices_map(indices.data(), 3, 3);

                manager.add("test", values_array, indices_map);
                check_indexed_attribute(manager, "test", values, indices, values_array->data());
                manager.remove("test");
            }

            SECTION("Array + moved Eigen::Matrix")
            {
                auto values_array = to_shared_ptr(experimental::create_array(values));
                auto indices_copy = indices;
                void* indices_copy_ptr = indices_copy.data();

                manager.add("test", values_array, std::move(indices_copy));
                check_indexed_attribute(
                    manager,
                    "test",
                    values,
                    indices,
                    values_array->data(),
                    indices_copy_ptr);
                manager.remove("test");
            }
        }
    }

    SECTION("Import and export")
    {
        experimental::IndexedAttributeManager manager;

        TestArray<float, 3> values(3, 3);
        values.setOnes();
        TestArray<int, 3> indices(3, 3);
        indices.setZero();
        void* values_ptr = values.data();
        void* indices_ptr = indices.data();

        manager.add("test", std::move(values), std::move(indices));

        SECTION("Export")
        {
            TestArray<float, 3> tmp_values;
            TestArray<int, 3> tmp_indices;
            manager.export_data("test", tmp_values, tmp_indices);
            REQUIRE(tmp_values.data() == values_ptr);
            REQUIRE(tmp_indices.data() == indices_ptr);
        }

        SECTION("Export with a different Eigen type will result in copy")
        {
            TestArray<float, Eigen::Dynamic> tmp_values(3, 3);
            TestArray<int, Eigen::Dynamic> tmp_indices(3, 3);
            manager.export_data("test", tmp_values, tmp_indices);
            REQUIRE(tmp_values.data() != values_ptr);
            REQUIRE(tmp_indices.data() != indices_ptr);
        }

        SECTION("Exporting raw array will result in a copy")
        {
            std::vector<float> raw_values(9, 0);
            Eigen::Map<TestArray<float, 3>> tmp_values(raw_values.data(), 3, 3);
            TestArray<int, 3> tmp_indices(3, 3);
            tmp_indices.setIdentity();

            values_ptr = tmp_values.data();
            indices_ptr = tmp_indices.data();

            manager.add("test2", tmp_values, std::move(tmp_indices));

            TestArray<float, 3> out_values;
            TestArray<int, 3> out_indices;
            manager.export_data("test2", out_values, out_indices);

            REQUIRE(out_values.data() != values_ptr); // Values are copied.
            REQUIRE(out_indices.data() == indices_ptr); // Indices are not.
        }

        SECTION("Import")
        {
            TestArray<float, 3> tmp_values(3, 3);
            tmp_values.setIdentity();
            TestArray<int, 3> tmp_indices(4, 3);
            tmp_indices.setConstant(10);

            auto values_copy = tmp_values;
            auto indices_copy = tmp_indices;

            values_ptr = tmp_values.data();
            indices_ptr = tmp_indices.data();

            manager.import_data("test", std::move(tmp_values), std::move(tmp_indices));
            check_indexed_attribute(
                manager,
                "test",
                values_copy,
                indices_copy,
                values_ptr,
                indices_ptr);
        }

        SECTION("Import with a different type will fail")
        {
            TestArray<float, Eigen::Dynamic> tmp_values(3, 3);
            tmp_values.setIdentity();
            TestArray<int, Eigen::Dynamic> tmp_indices(4, 3);
            tmp_indices.setConstant(10);

            values_ptr = tmp_values.data();
            indices_ptr = tmp_indices.data();

            // It is not possible to import if the Eigen type does not exactly
            // match (for now)...  A warning should be issued.
            manager.import_data("test", std::move(tmp_values), std::move(tmp_indices));

            auto attr = manager.get("test");
            REQUIRE(attr != nullptr);
            void* out_values_ptr = attr->get_values()->data();
            void* out_indices_ptr = attr->get_indices()->data();

            REQUIRE(values_ptr != out_values_ptr);
            REQUIRE(indices_ptr != out_indices_ptr);
        }

        SECTION("Export -> update -> import")
        {
            manager.export_data("test", values, indices);
            REQUIRE(values.data() == values_ptr);
            REQUIRE(indices.data() == indices_ptr);

            values.setConstant(10);
            indices.setConstant((int)1e5);
            REQUIRE(values.data() == values_ptr);
            REQUIRE(indices.data() == indices_ptr);

            auto values_copy = values;
            auto indices_copy = indices;

            manager.import_data("test", values, indices);
            check_indexed_attribute(
                manager,
                "test",
                values_copy,
                indices_copy,
                values_ptr,
                indices_ptr);
        }
    }
}