/*
 * DIPlib 3.0
 * This file contains the definition of SeparateFilter().
 *
 * (c)2017, Cris Luengo.
 * Based on original DIPlib code: (c)1995-2014, Delft University of Technology.
 *
 * 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.
 */

#include "diplib.h"
#include "diplib/linear.h"
#include "diplib/pixel_table.h"

#if defined(__GNUG__) || defined(__clang__)
// For this file, turn off -Wsign-conversion, Eigen is really bad at this!
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic ignored "-Wconversion"
#if __GNUC__ >= 7
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#endif
#if __GNUC__ >= 9
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#endif

#include <Eigen/SVD>

namespace dip {

namespace {

template< typename T > // T is either dfloat or dcomplex
void dip__SeparateFilter(
      Image& filter,
      std::vector< dfloat >& out,
      dip::uint nPixels,
      dip::uint length
) {
   constexpr bool isComplex = std::is_same< T, dcomplex >::value;
   using Matrix = Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic >;
   using Vector = Eigen::Matrix< T, Eigen::Dynamic, 1 >;
   // Make a matrix out of `filter` that has `nPixel` rows and `length` columns
   Eigen::Map< Matrix > matrix( static_cast< T* >( filter.Origin() ),
                                static_cast< Eigen::Index >( nPixels ),
                                static_cast< Eigen::Index >( length ));
   // Compute SVD
   Eigen::JacobiSVD< Matrix > svd( matrix, Eigen::ComputeThinU | Eigen::ComputeThinV );
   // Expect all but first singular value to be close to 0. If not, it's not separable, and we return {}.
   auto S = svd.singularValues();
   dfloat s1 = S( 0 );
   dfloat s2 = S( 1 );
   dfloat tolerance = 1e-7 * static_cast< dfloat >( std::max( nPixels, length )) * std::abs( s1 );
   //std::cout << "s1 = " << s1 << ", s2 = " << s2 << ", tol = " << tolerance << std::endl;
   if( s2 > tolerance ) {
      // Not separable!
      return;
   }
   // 1D filter is first column of V -- write V into 1D filter buffer
   out.resize( length * ( isComplex ? 2 : 1 ));
   Eigen::Map< Vector > oneDFilter( reinterpret_cast< T* >( out.data() ), static_cast< Eigen::Index >( length ));
   oneDFilter = svd.matrixV().col( 0 );
   // The ndims-1--dimensional remainder is the first column of U * singular value --
   // write U * s1 back into input image, we'll use fewer pixels than before
   Eigen::Map< Vector > remainder( static_cast< T* >( filter.Origin() ),
                                   static_cast< Eigen::Index >( nPixels ));
   remainder = svd.matrixU().col( 0 ) * s1;
}

} // namespace

OneDimensionalFilterArray SeparateFilter( Image const& c_in ) {
   DIP_THROW_IF( !c_in.IsForged(), E::IMAGE_NOT_FORGED );
   DIP_THROW_IF( !c_in.IsScalar(), E::IMAGE_NOT_SCALAR );
   dip::uint ndims = c_in.Dimensionality();
   DIP_THROW_IF( ndims < 1, E::DIMENSIONALITY_NOT_SUPPORTED );
   OneDimensionalFilterArray out( ndims );
   // Complex data is handled a little differently from real data
   bool isComplex = c_in.DataType().IsComplex();
   // Copy the input image, we will need it as a scratch pad
   Image filter = Convert( c_in, isComplex ? DT_DCOMPLEX : DT_DFLOAT ); // Filter is DFLOAT or DCOMPLEX and has normal strides
   DIP_ASSERT( filter.HasNormalStrides() );
   UnsignedArray sizes = filter.Sizes();
   dip::uint nPixels = sizes.product();
   // Shave dimensions off the filter from the end
   while( ndims > 1 ) {
      dip::uint dim = ndims - 1; // Current dimension
      dip::uint length = sizes[ dim ]; // Number of pixels in 1D filter for this dimension
      if( length > 1 ) {
         nPixels /= length; // Number of pixels in remainder
         if( isComplex ) {
            dip__SeparateFilter< dcomplex >( filter, out[ dim ].filter, nPixels, length );
            out[ dim ].isComplex = true;
         } else {
            dip__SeparateFilter< dfloat >( filter, out[ dim ].filter, nPixels, length );
         }
         if( out[ dim ].filter.empty() ) {
            return {}; // The filter is not separable
         }
      }
      // else: the output filter will have size 0, and we continue as usual.
      --ndims;
   }
   out[ 0 ].filter.resize( nPixels * ( isComplex ? 2 : 1 ));
   std::copy( static_cast< dfloat* >( filter.Origin() ),
              static_cast< dfloat* >( filter.Origin() ) + nPixels * ( isComplex ? 2 : 1 ), // for complex data, copy two values per pixel
              out[ 0 ].filter.data() );
   out[ 0 ].isComplex = isComplex;
   return out;
}

} // namespace dip

#ifdef DIP__ENABLE_DOCTEST
#include "doctest.h"
#include "diplib/statistics.h"
//#include "diplib/timer.h"

DOCTEST_TEST_CASE("[DIPlib] testing the filter separation") {
   dip::Image delta3D( { 30, 30, 30 }, 1, dip::DT_SFLOAT );
   delta3D.Fill( 0 );
   delta3D.At( 15, 15, 15 ) = 1;

   auto rectPt = dip::PixelTable( "rectangular", { 10, 11, 5 } );
   dip::Image rect = dip::Convert( rectPt.AsImage(), dip::DT_UINT8 );
   //dip::Timer timer;
   dip::OneDimensionalFilterArray rect1 = dip::SeparateFilter( rect );
   //timer.Stop();
   //std::cout << "Separating filter: " << timer << std::endl;
   DOCTEST_REQUIRE( rect1.size() == 3 );
   DOCTEST_CHECK( rect1[ 0 ].filter.size() == 10 );
   DOCTEST_CHECK( rect1[ 1 ].filter.size() == 11 );
   DOCTEST_CHECK( rect1[ 2 ].filter.size() == 5 );
   //timer.Reset();
   //dip::Image tmp1 = dip::SeparableConvolution( delta3D, rect1 );
   //timer.Stop();
   //std::cout << "Apply separated filter: " << timer << std::endl;
   //timer.Reset();
   //dip::Image tmp2 = dip::GeneralConvolution( delta3D, rect );
   //timer.Stop();
   //std::cout << "Apply full filter: " << timer << std::endl;
   //DOCTEST_CHECK( dip::All( tmp1 == tmp2 ).As< bool >() );
   bool correct = true;
   for( dip::uint ii = 0; ii < 3; ++ii ) {
      for( auto v : rect1[ ii ].filter ) {
         if( doctest::Approx( v ) != rect1[ ii ].filter[ 0 ] ) {
            correct = false;
         }
      }
   }
   DOCTEST_CHECK( correct );
   dip::dfloat product = rect1[ 0 ].filter[ 0 ] * rect1[ 1 ].filter[ 0 ] * rect1[ 2 ].filter[ 0 ];
   DOCTEST_CHECK( product == doctest::Approx( 1.0 ));

   auto circPt = dip::PixelTable( "elliptic", { 10, 11, 5 } );
   dip::Image circ = dip::Convert( circPt.AsImage(), dip::DT_UINT8 );
   //timer.Reset();
   dip::OneDimensionalFilterArray circ1 = dip::SeparateFilter( circ );
   //timer.Stop();
   //std::cout << "Separating filter: " << timer << std::endl;
   DOCTEST_CHECK( circ1.size() == 0 );

   //timer.Reset();
   dip::Image gaussRes = dip::GaussFIR( delta3D, { 3, 2, 3 }, { 1, 0, 2 } );
   //timer.Stop();
   //std::cout << "Compute Gaussian: " << timer << std::endl;
   dip::Image gauss = gaussRes.Cropped( { 3 * 6 + 1, 2 * 6 + 1, 3 * 6 + 1 } );
   //timer.Reset();
   dip::OneDimensionalFilterArray gauss1 = dip::SeparateFilter( gauss );
   //timer.Stop();
   //std::cout << "Separating filter: " << timer << std::endl;
   DOCTEST_REQUIRE( gauss1.size() == 3 );
   //timer.Reset();
   dip::Image tmp3 = dip::SeparableConvolution( delta3D, gauss1 );
   //timer.Stop();
   //std::cout << "Apply separated filter: " << timer << std::endl;
   //timer.Reset();
   //tmp2 = dip::GeneralConvolution( delta3D, gauss );
   //timer.Stop();
   //std::cout << "Apply full filter: " << timer << std::endl;
   tmp3 -= gaussRes;
   auto m = dip::MaximumAndMinimum( tmp3 );
   DOCTEST_CHECK( m.Minimum() > -1e-5 );
   DOCTEST_CHECK( m.Maximum() < 1e-5 );
}

#endif // DIP__ENABLE_DOCTEST


// Run like this
//./generate --data competition/S1a/S1b_short_dataset1_training.csv
// This code will forecast 4 unknown parameters of the Matern covariance matrix
// Developed by Alexander Litvinenko (RWTH Aachen) and Ronald Kriemann (MIS MPG Leipzig)
// Based on the HLIBPro library (v. 2.9) www.hlibpro.com
// No warranties.

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

#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/bessel.hpp>

#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_gamma.h>

#include <gsl/gsl_multimin.h>

#include "hlib.hh"

using namespace std;
using boost::format;
using namespace HLIB;
using namespace boost::program_options;
using  real_t    = HLIB::real;

enum {
    IDX_SIGMA  = 0,
    IDX_LENGTH = 1,
    IDX_NU     = 2,
    IDX_TAU    = 3
};

//Use a method described by Abramowitz and Stegun:
double gaussrand_Stegun()
{
    static double U, V;
    static int phase = 0;
    double Z;

    if(phase == 0) {
        U = (rand() + 1.) / (RAND_MAX + 2.);
        V = rand() / (RAND_MAX + 1.);
        Z = sqrt(-2 * log(U)) * sin(2 * M_PI * V);
    } else
        Z = sqrt(-2 * log(U)) * cos(2 * M_PI * V);

    phase = 1 - phase;

    return Z;
}

//Use a method discussed in Knuth and due originally to Marsaglia:

double gaussrand_Knuth()
{
    static double V1, V2, S;
    static int phase = 0;
    double X;

    if(phase == 0) {
        do {
            double U1 = (double)rand() / RAND_MAX;
            double U2 = (double)rand() / RAND_MAX;

            V1 = 2 * U1 - 1;
            V2 = 2 * U2 - 1;
            S = V1 * V1 + V2 * V2;
        } while(S >= 1 || S == 0);

        X = V1 * sqrt(-2 * log(S) / S);
    } else
        X = V2 * sqrt(-2 * log(S) / S);

    phase = 1 - phase;

    return X;
}

// global options
int        nmin      = CFG::Cluster::nmin;
double     eps       = 1e-6;
double     fac_eps   = 1e-6;
double     shift     = 1e-7;
bool       use_ldl   = false;

//
// read dataset from file
//
void
read_data ( const std::string &       datafile,
            std::vector< T2Point > &  vertices,
            BLAS::Vector< double > &  Z_data )
{
    std::ifstream  in( datafile );

    if ( ! in ) // error
        exit( 1 );

    size_t  N_vtx = 0;

    #if 1

    std::string  line;

    std::getline( in, line );

    if ( line == "x,y" )
    {
        std::list< T2Point >  pos;
        std::list< double >   vals;

        while ( std::getline( in, line ) )
        {
            auto    parts = split( line, "," );
            double  x = atof( parts[0].c_str() );
            double  y = atof( parts[1].c_str() );

            pos.push_back( T2Point( x, y ) );
        }// while

        N_vtx = pos.size();

        std::cout << "learning dataset" << std::endl;
        std::cout << N_vtx << std::endl;

        vertices.resize( N_vtx );
        Z_data = BLAS::Vector< double >( N_vtx );

        int  i = 0;

        for ( auto  p : pos )
            vertices[ i++ ] = p;

        i = 0;

        for ( idx_t  i = 0; i < idx_t(N_vtx); ++i )
           Z_data( i++ ) =  gaussrand_Knuth(); //gaussrand_Stegun() ;
    }// if
    else
    {
        std::cout << "you should not be here, something is wrong with the input file" << std::endl;
        HERROR( ERR_NOT_IMPL, "", "" );
    }

    #else

    in >> N_vtx;

    std::cout << "reading " << N_vtx << " datapoints" << std::endl;

    vertices.resize( N_vtx );
    Z_data = BLAS::Vector< double >( N_vtx );

    for ( idx_t  i = 0; i < idx_t(N_vtx); ++i )
    {
        int     index = i;
        double  x, y, z;
        // double  v     = 0.0;

        in >> index >> x >> y >> z;
        // in >> index >> x >> y >> z >> v;

        vertices[ index ] = T2Point( x, y );
        //Z_data( index )   = v;
    }// for

    #endif

    //
    // for visualization of data, export 2D points with v value in csv file
    //

    // std::ofstream  out( "data.csv" );

    // out << "x,y,z,v" << std::endl;
    // out << "x,y,z" << std::endl;

    //for ( uint  i = 0; i < N_vtx; ++i )
    //   out << vertices[i].x() << "," << vertices[i].y() << ",0" << std::endl;
//      out << vertices[i].x() << "," << vertices[i].y() << ",0," << Z_data( i ) << std::endl;
}

//
// define PredictionProblem to forecast unknown values in new locations
//
struct GeneratingProblem
{
    std::vector< T2Point >                vertices;
    std::unique_ptr< TCoordinate >        coord;
    std::unique_ptr< TClusterTree >       ct;
    std::unique_ptr< TBlockClusterTree >  bct;
    std::unique_ptr< TVector >            Z;

    GeneratingProblem ( const std::string &  datafile )
    {
        init( datafile );
    }

    void
    init ( const std::string &  datafile )
    {
        BLAS::Vector< double >  Z_data;

        read_data( datafile, vertices, Z_data );
        std::cout << "the grid is successfully read" << std::endl;
        coord = std::make_unique< TCoordinate >( vertices );

        TAutoBSPPartStrat  part_strat;
        TBSPCTBuilder      ct_builder( & part_strat, nmin );

        ct = ct_builder.build( coord.get() );
        //print_vtk( & coord, "ct_coord" );
        //print_vtk( & coord_predict, "ct_coord_predict" );

        TStdGeomAdmCond    adm_cond( 2.0, use_min_diam );
        TBCBuilder         bct_builder;

        bct = bct_builder.build( ct.get(), ct.get(), & adm_cond );
        Z   = std::make_unique< TScalarVector >( *ct->root(), std::move( Z_data ) );
        ct->perm_e2i()->permute( Z.get() );
  }

    //BLAS::Vector< double >
    std::unique_ptr< TVector >
    eval ( const double  sigma,
           const double  length,
           const double  nu,
           const double tau )
    {

        TMaternCovCoeffFn< T2Point >  matern_coefffn( sigma, length, nu,  vertices );
        TPermCoeffFn< double >        coefffn( & matern_coefffn, ct->perm_i2e(), ct->perm_i2e() );

        TACAPlus< double >            aca( & coefffn );
        auto                          acc = fixed_prec( eps );
        TDenseMatBuilder< double >    h_builder( & coefffn, & aca );

        auto                          C        = h_builder.build( bct.get(), acc );
        TPSMatrixVis  mvis;

         //mvis.svd(true).print( C.get(), "myC" );

//        print_ps(bct->root(), "bct.eps");
//        print_ps(bct_predict->root(), "bct_predict.eps");

        //       mvis.svd(true).print( C_predict.get(), "myC_predict" );

        //if ( shift != 0.0 )
        //    add_identity( C.get(), tau*tau );

        auto                          fac_acc  = fixed_prec( fac_eps );
        auto                          C_fac    = C->copy();
        auto                          fac_opts = fac_options_t{ point_wise, CFG::Arith::storage_type, false };

        chol( C_fac.get(), fac_acc );

        //std::cout << "    |С|_F             = " << norm_F( C.get() ) << std::endl;
        //std::cout << "    |L|_F             = " << norm_F( C_fac.get() ) << std::endl;
        //std::cout << "    |С|_2             = " << norm_2( C.get() ) << std::endl;
        //std::cout << "    |L|_2             = " << norm_2( C_fac.get() ) << std::endl;
        //mvis.svd(true).print( C_fac.get(), "myL" );

        //std::unique_ptr< TFacInvMatrix >   C_inv;

        const size_t                  N     = vertices.size();

        auto                          Z_generated = C->row_vector();

      //  std::cout << "  size of C = " << C->rows() << "x"<< C->cols() << std::endl;
      //  std::cout << "  size of C_predict = " << C_predict->rows() <<"x"<<  C_predict->cols() << std::endl;
      //  std::cout << "  sol size = " << sol->size() << std::endl;
      //  std::cout << "  ||sol|| = " << sol->norm2() << std::endl;
      //  std::cout << "  ||Z_predict|| = " << Z_predict->norm2() << std::endl;

        //auto                          ZdotCZ = std::real( Z->dot( sol.get() ) );
        //Z_predict = C_predict * sol.get();
        mul_vec( real_t(1), C_fac.get(), Z.get(), real_t(0), Z_generated.get(), apply_normal );
        //C_predict->mul_vec( 1.0, sol.get(), 0.0, Z_predict.get(), apply_normal );

        ct->perm_i2e()->permute( Z_generated.get() );
        std::cout << "  ||Z_generated|| = " << Z_generated->norm2() << std::endl;

        //TMatlabVectorIO  vio;

        //vio.write( Z_predict,  "x.mat", "x" );

        FILE* f1;
        f1 = fopen("111gen_d.txt", "w");

        for ( size_t  i = 0; i < Z_generated->size(); i++ )
          fprintf(f1," %6.6e, %6.6e, %6.6e\n",   vertices[i].x(),  vertices[i].y(), Z_generated->entry(i));
        fclose(f1);
        return std::move( Z_generated );
    }
};

//
// wrapper from GSL to LogLikeliHoodProblem
//
/*double
  eval_logli ( const gsl_vector *  param,
  void *              data )
  {
  double sigma  = gsl_vector_get( param, IDX_SIGMA );
  double length = gsl_vector_get( param, IDX_LENGTH );
  double nu     = gsl_vector_get( param, IDX_NU );
  double tau    = gsl_vector_get( param, IDX_TAU );

  LogLikeliHoodProblem *  problem = static_cast< LogLikeliHoodProblem * >( data );

  return - problem->eval( sigma, length, nu, tau );
  }
*/
//
// optimization function using GSL
//

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

    CFG::set_verbosity( 3 );
    INIT();

    //std::string  datafile = "datafile.txt";
    //std::string  datafile_predict = "datafile_predict.txt";
    std::string  datafile = "grid.txt";

    //
    // define command line options
    //

    options_description             all_opts;
    options_description             vis_opts( "usage: generatig [options] datafile\n  where options include" );
    options_description             hid_opts( "Hidden options" );
    positional_options_description  pos_opts;
    variables_map                   vm;

    // standard options
    vis_opts.add_options()
        ( "help,h",                       ": print this help text" )
        ( "threads,t",   value<int>(),    ": number of parallel threads" )
        ( "verbosity,v", value<int>(),    ": verbosity level" )
        ( "nmin",        value<int>(),    ": set minimal cluster size" )
        ( "eps,e",       value<double>(), ": set H accuracy" )
        ( "epslu",       value<double>(), ": set only H factorization accuracy" )
        ( "shift",       value<double>(), ": regularization parameter" )
        ( "ldl",                          ": use LDL factorization" )
        ;

    hid_opts.add_options()
        ( "data",        value<std::string>(), ": datafile " );

    // options for command line parsing
    all_opts.add( vis_opts ).add( hid_opts );

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

    //
    // parse command line options
    //

    try
    {
        store( command_line_parser( argc, argv ).options( all_opts ).positional( pos_opts ).run(), vm );
        notify( vm );
    }// try
    catch ( required_option &  e )
    {
        std::cout << e.get_option_name() << " requires an argument, try \"-h\"" << std::endl;
        exit( 1 );
    }// catch
    catch ( unknown_option &  e )
    {
        std::cout << e.what() << ", try \"-h\"" << std::endl;
        exit( 1 );
    }// catch

    //
    // eval command line options
    //

    if ( vm.count( "help") )
    {
        std::cout << vis_opts << std::endl;
        exit( 1 );
    }// if

    if ( vm.count( "nmin"      ) ) nmin     = vm["nmin"].as<int>();
    if ( vm.count( "eps"       ) ) eps      = vm["eps"].as<double>();
    if ( vm.count( "epslu"     ) ) fac_eps  = vm["epslu"].as<double>();
    if ( vm.count( "shift"     ) ) shift    = vm["shift"].as<double>();
    if ( vm.count( "threads"   ) ) CFG::set_nthreads( vm["threads"].as<int>() );
    if ( vm.count( "verbosity" ) ) CFG::set_verbosity( vm["verbosity"].as<int>() );
    if ( vm.count( "ldl"       ) ) use_ldl  = true;

    // default to general eps
    if ( fac_eps == -1 )
        fac_eps = eps;

    if ( vm.count( "data" ) )
        datafile = vm["data"].as<std::string>();
    else
    {
        std::cout << "usage: generating [options] datafile" << std::endl;
        exit( 1 );
    }// if

    double  sigma  = 2.0; //take these values from previous experiments (Part 1a)
    double  length = 0.1;
    double  nu     = 0.5;
    double  tau    = 0.0;

    GeneratingProblem  problem( datafile );
    problem.eval( sigma, length, nu, tau);

    DONE();
}


// ---------------------------------------------------------------------
//
// Copyright (c) 2019 - 2021 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Config files

#include <SAMRAI_config.h>

// Headers for basic libMesh objects
#include <libmesh/boundary_info.h>
#include <libmesh/boundary_mesh.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_refinement.h>

// Headers for application-specific algorithm/data structure objects
#include <ibtk/AppInitializer.h>
#include <ibtk/FEMapping.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/QuadratureCache.h>
#include <ibtk/libmesh_utilities.h>

// Set up application namespace declarations
#include <boost/multi_array.hpp>

#include <ibamr/app_namespaces.h>

#include "../tests.h"

// Verify that FEMapping and descendants output the same values as libMesh::FEMap.
using key_type = quadrature_key_type;

template <int dim, int spacedim>
void
test_cube(LibMeshInit& init,
          FEMapping<dim, spacedim>& jc_1,
          FEMapping<dim, spacedim>& jc_2,
          FEMapping<dim - 1, spacedim>& jc_boundary,
          const key_type key)
{
    const auto elem_type = std::get<0>(key);

    FEMapCache map_cache(dim);
    QuadratureCache quad_cache(dim);

    ReplicatedMesh mesh(init.comm(), dim);
    if (dim == 2)
        MeshTools::Generation::build_square(mesh, 10, 10, 0.0, 0.5, 0.0, 2.0, elem_type);
    else if (dim == 3)
        MeshTools::Generation::build_cube(mesh, 10, 10, 10, 0.0, 0.5, 0.0, 0.25, 0.0, 8.0, elem_type);
    else
        TBOX_ASSERT(false);

    // check that we get the same thing with both calculators and libMesh's general code:
    jc_1.reinit(*mesh.active_local_elements_begin());
    const std::vector<double>& JxW = jc_1.getJxW();
    for (const double jxw : JxW) plog << std::setprecision(12) << jxw << '\n';

    double volume = 0;
    double volume_2 = 0;
    for (auto elem_iter = mesh.active_local_elements_begin(); elem_iter != mesh.active_local_elements_end();
         ++elem_iter)
    {
        FEMap& fe_map = map_cache[key];
        QBase& quad = quad_cache[key];
        fe_map.compute_map(dim, quad.get_weights(), *elem_iter, false);
        // all computed JxW values should agree
        jc_1.reinit(*elem_iter);
        jc_2.reinit(*elem_iter);
        const std::vector<double>& JxW = jc_1.getJxW();
        const std::vector<double>& JxW_2 = jc_2.getJxW();
        const std::vector<double>& JxW_3 = fe_map.get_JxW();
        for (unsigned int i = 0; i < JxW.size(); ++i)
        {
            TBOX_ASSERT(std::abs(JxW[i] - JxW_2[i]) < 1e-14 * std::max(1.0, std::abs(JxW[i])));
            TBOX_ASSERT(std::abs(JxW[i] - JxW_3[i]) < 1e-14 * std::max(1.0, std::abs(JxW[i])));
        }
        volume += std::accumulate(JxW.begin(), JxW.end(), 0.0);
        volume_2 += std::accumulate(JxW_2.begin(), JxW_2.end(), 0.0);
    }
    TBOX_ASSERT(std::abs(volume - volume_2) < 1e-15 * volume);
    plog << "volume is " << volume << '\n';

    // also test the surface mesh:
    {
        BoundaryMesh boundary_mesh(mesh.comm(), mesh.mesh_dimension() - 1);
        BoundaryInfo& boundary_info = mesh.get_boundary_info();
        boundary_info.sync(boundary_mesh);
        boundary_mesh.prepare_for_use();
        TBOX_ASSERT(boundary_mesh.spatial_dimension() == mesh.mesh_dimension());
        TBOX_ASSERT(boundary_mesh.mesh_dimension() == mesh.mesh_dimension() - 1);

        FEMapCache boundary_map_cache(dim - 1);
        QuadratureCache boundary_quad_cache(dim - 1);

        for (auto elem_iter = boundary_mesh.active_local_elements_begin();
             elem_iter != boundary_mesh.active_local_elements_end();
             ++elem_iter)
        {
            key_type boundary_key = key;
            std::get<0>(boundary_key) = (*elem_iter)->type();

            FEMap& fe_map = boundary_map_cache[boundary_key];
            QBase& quad = boundary_quad_cache[boundary_key];
            fe_map.compute_map(dim - 1, quad.get_weights(), *elem_iter, false);
            // all computed JxW values should agree
            jc_boundary.reinit(*elem_iter);
            const std::vector<double>& JxW = jc_boundary.getJxW();
            const std::vector<double>& JxW_2 = fe_map.get_JxW();
            for (unsigned int i = 0; i < JxW.size(); ++i)
            {
                TBOX_ASSERT(std::abs(JxW[i] - JxW_2[i]) < 1e-14 * std::max(1.0, std::abs(JxW[i])));
            }
        }
    }
}

template <int dim, int spacedim>
void
test_circle(LibMeshInit& init,
            FEMapping<dim, spacedim>& jc_1,
            FEMapping<dim, spacedim>& jc_2,
            FEMapping<dim - 1, spacedim>& jc_boundary,
            const int n_refines,
            const key_type key)
{
    const auto elem_type = std::get<0>(key);

    FEMapCache map_cache(dim);
    QuadratureCache quad_cache(dim);

    const double radius = 1.0;
    ReplicatedMesh mesh(init.comm(), dim);
    MeshTools::Generation::build_sphere(mesh, radius, n_refines, elem_type);

    // Ensure nodes on the surface are on the analytic boundary.
    MeshBase::element_iterator el_end = mesh.elements_end();
    for (MeshBase::element_iterator el = mesh.elements_begin(); el != el_end; ++el)
    {
        Elem* const elem = *el;
        for (unsigned int side = 0; side < elem->n_sides(); ++side)
        {
            const bool at_mesh_bdry = !elem->neighbor_ptr(side);
            if (!at_mesh_bdry) continue;
            for (unsigned int k = 0; k < elem->n_nodes(); ++k)
            {
                if (!elem->is_node_on_side(k, side)) continue;
                Node& n = elem->node_ref(k);
                n = radius * n.unit();
            }
        }
    }
    mesh.prepare_for_use();

    // check that we get the same thing with both calculators and libMesh's general code:
    jc_1.reinit(*mesh.active_local_elements_begin());
    const std::vector<double>& JxW = jc_1.getJxW();
    for (const double jxw : JxW) plog << std::setprecision(12) << jxw << '\n';

    double volume = 0;
    double volume_2 = 0;
    for (auto elem_iter = mesh.active_local_elements_begin(); elem_iter != mesh.active_local_elements_end();
         ++elem_iter)
    {
        FEMap& fe_map = map_cache[key];
        QBase& quad = quad_cache[key];
        fe_map.compute_map(dim, quad.get_weights(), *elem_iter, false);
        // all computed JxW values should agree
        jc_1.reinit(*elem_iter);
        jc_2.reinit(*elem_iter);
        const std::vector<double>& JxW = jc_1.getJxW();
        const std::vector<double>& JxW_2 = jc_2.getJxW();
        const std::vector<double>& JxW_3 = fe_map.get_JxW();
        for (unsigned int i = 0; i < JxW.size(); ++i)
        {
            TBOX_ASSERT(std::abs(JxW[i] - JxW_2[i]) < 1e-14 * std::max(1.0, std::abs(JxW[i])));
            TBOX_ASSERT(std::abs(JxW[i] - JxW_3[i]) < 1e-14 * std::max(1.0, std::abs(JxW[i])));
        }
        volume += std::accumulate(JxW.begin(), JxW.end(), 0.0);
        volume_2 += std::accumulate(JxW_2.begin(), JxW_2.end(), 0.0);
    }
    TBOX_ASSERT(std::abs(volume - volume_2) < 1e-15 * volume);
    plog << "volume is " << volume << '\n';

    // also test the surface mesh:
    {
        BoundaryMesh boundary_mesh(mesh.comm(), mesh.mesh_dimension() - 1);
        BoundaryInfo& boundary_info = mesh.get_boundary_info();
        boundary_info.sync(boundary_mesh);
        boundary_mesh.prepare_for_use();
        TBOX_ASSERT(boundary_mesh.spatial_dimension() == mesh.mesh_dimension());
        TBOX_ASSERT(boundary_mesh.mesh_dimension() == mesh.mesh_dimension() - 1);

        FEMapCache boundary_map_cache(dim - 1);
        QuadratureCache boundary_quad_cache(dim - 1);

        for (auto elem_iter = boundary_mesh.active_local_elements_begin();
             elem_iter != boundary_mesh.active_local_elements_end();
             ++elem_iter)
        {
            key_type boundary_key = key;
            std::get<0>(boundary_key) = (*elem_iter)->type();

            FEMap& fe_map = boundary_map_cache[boundary_key];
            QBase& quad = boundary_quad_cache[boundary_key];
            fe_map.compute_map(dim - 1, quad.get_weights(), *elem_iter, false);
            // all computed JxW values should agree
            jc_boundary.reinit(*elem_iter);
            const std::vector<double>& JxW = jc_boundary.getJxW();
            const std::vector<double>& JxW_2 = fe_map.get_JxW();
            for (unsigned int i = 0; i < JxW.size(); ++i)
            {
                TBOX_ASSERT(std::abs(JxW[i] - JxW_2[i]) < 1e-14 * std::max(1.0, std::abs(JxW[i])));
            }
        }
    }
}

int
main(int argc, char** argv)
{
    // Initialize IBAMR and libraries. Deinitialization is handled by this object as well.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);
    LibMeshInit& init = ibtk_init.getLibMeshInit();

    {
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");

        Pointer<Database> input_db = app_initializer->getInputDatabase();

        unsigned int test_n = 1;
        {
            plog << "Test " << test_n << ": TRI3 square" << std::endl;
            const key_type key(TRI3, QGAUSS, THIRD, true);
            Tri3Mapping jac_calc_1(key, FEUpdateFlags::update_JxW);
            FELagrangeMapping<2> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(EDGE2, QGAUSS, THIRD, true);
            FELagrangeMapping<1, 2> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_cube(init, jac_calc_1, jac_calc_2, jac_calc_b, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": TRI3 square" << std::endl;
            const key_type key(TRI3, QGAUSS, THIRD, true);
            Tri3Mapping jac_calc_1(key, FEUpdateFlags::update_JxW);
            FELagrangeMapping<2> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(EDGE2, QGAUSS, THIRD, true);
            FELagrangeMapping<1, 2> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_cube(init, jac_calc_1, jac_calc_2, jac_calc_b, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": TRI6 square" << std::endl;
            const key_type key(TRI6, QGAUSS, FIFTH, true);
            Tri6Mapping jac_calc_1(key, FEUpdateFlags::update_JxW);
            FELagrangeMapping<2> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(EDGE2, QGAUSS, FIFTH, true);
            FELagrangeMapping<1, 2> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_cube(init, jac_calc_1, jac_calc_2, jac_calc_b, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": TRI6 square" << std::endl;
            const key_type key(TRI6, QGAUSS, FIFTH, true);
            Tri6Mapping jac_calc_1(key, FEUpdateFlags::update_JxW);
            FELagrangeMapping<2> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(EDGE2, QGAUSS, FIFTH, true);
            FELagrangeMapping<1, 2> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_cube(init, jac_calc_1, jac_calc_2, jac_calc_b, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": Quad4 square" << std::endl;
            const key_type key(QUAD4, QGAUSS, THIRD, true);
            Quad4Mapping jac_calc_1(key, FEUpdateFlags::update_JxW);
            FELagrangeMapping<2> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(EDGE2, QGAUSS, THIRD, true);
            FELagrangeMapping<1, 2> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_cube(init, jac_calc_1, jac_calc_2, jac_calc_b, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": Quad4 circle" << std::endl;
            const key_type key(QUAD4, QGAUSS, THIRD, true);
            Quad4Mapping jac_calc_1(key, FEUpdateFlags::update_JxW);
            FELagrangeMapping<2> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(EDGE2, QGAUSS, THIRD, true);
            FELagrangeMapping<1, 2> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_circle(init, jac_calc_1, jac_calc_2, jac_calc_b, 5, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": Quad9 square" << std::endl;
            const key_type key(QUAD9, QGAUSS, FOURTH, true);
            Quad9Mapping jac_calc_1(key, FEUpdateFlags::update_JxW);
            FELagrangeMapping<2> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(EDGE3, QGAUSS, FOURTH, true);
            FELagrangeMapping<1, 2> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_cube(init, jac_calc_1, jac_calc_2, jac_calc_b, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": Quad9 circle" << std::endl;
            const key_type key(QUAD9, QGAUSS, FOURTH, true);
            Quad9Mapping jac_calc_1(key, FEUpdateFlags::update_JxW);
            FELagrangeMapping<2> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(EDGE3, QGAUSS, FOURTH, true);
            FELagrangeMapping<1, 2> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_circle(init, jac_calc_1, jac_calc_2, jac_calc_b, 4, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": TET4 cube" << std::endl;
            const key_type key(TET4, QGAUSS, THIRD, true);
            Tet4Mapping jac_calc_1(key, FEUpdateFlags::update_JxW);
            FELagrangeMapping<3> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(TRI3, QGAUSS, THIRD, true);
            FELagrangeMapping<2, 3> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_cube(init, jac_calc_1, jac_calc_2, jac_calc_b, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": HEX8 square" << std::endl;
            const key_type key(HEX8, QGAUSS, THIRD, true);
            // HEX8 doesn't have a custom calculator yet
            FELagrangeMapping<3> jac_calc_1(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            FELagrangeMapping<3> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(QUAD4, QGAUSS, THIRD, true);
            FELagrangeMapping<2, 3> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_cube(init, jac_calc_1, jac_calc_2, jac_calc_b, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": HEX8 circle" << std::endl;
            const key_type key(HEX8, QGAUSS, THIRD, true);
            // HEX8 doesn't have a custom calculator yet
            FELagrangeMapping<3> jac_calc_1(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            FELagrangeMapping<3> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(QUAD4, QGAUSS, THIRD, true);
            FELagrangeMapping<2, 3> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_circle(init, jac_calc_1, jac_calc_2, jac_calc_b, 4, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": HEX27 square" << std::endl;
            const key_type key(HEX27, QGAUSS, FOURTH, true);
            // HEX27 doesn't have a custom calculator yet
            FELagrangeMapping<3> jac_calc_1(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            FELagrangeMapping<3> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(QUAD9, QGAUSS, FOURTH, true);
            FELagrangeMapping<2, 3> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_cube(init, jac_calc_1, jac_calc_2, jac_calc_b, key);
            ++test_n;
        }

        {
            plog << "Test " << test_n << ": HEX27 circle" << std::endl;
            const key_type key(HEX27, QGAUSS, FOURTH, true);
            // HEX27 doesn't have a custom calculator yet
            FELagrangeMapping<3> jac_calc_1(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            FELagrangeMapping<3> jac_calc_2(key, std::get<0>(key), FEUpdateFlags::update_JxW);
            const key_type boundary_key(QUAD9, QGAUSS, FOURTH, true);
            FELagrangeMapping<2, 3> jac_calc_b(boundary_key, std::get<0>(boundary_key), FEUpdateFlags::update_JxW);
            test_circle(init, jac_calc_1, jac_calc_2, jac_calc_b, 2, key);
            ++test_n;
        }
    }
} // main


#include <boost/mp11/mpl.hpp>
#include <type_traits>

template <int I>
using int_ = std::integral_constant<int, I>;

using namespace boost::mp11;

template <typename Sequence, typename Value>
struct index_of_impl
{
	using index = mp_find<Sequence, Value>;
	using size = mp_size<Sequence>;
	using index_smaller_than_size = mp_less<index, size>;
	using type = mp_if<index_smaller_than_size, index, int_<-1>>;
};

template <typename Sequence, typename Value>
using index_of = typename index_of_impl<Sequence, Value>::type;

int main()
{
	using l = mp_list_c<int, 5, 2, 3, 1, 4>;

	constexpr int r1 = index_of<l, int_<3>>::value;
	static_assert(r1 == 2);

	constexpr int r2 = index_of<l, int_<6>>::value;
	static_assert(r2 == -1);
}


#include "Common.h"
#include "CPS4.h"
#include "PropertiesHolder/PropertiesHolder.h"
#include "Material.h"
#include <Eigen/Dense>

#include "GaussQuadrature.h"

Eigen::Matrix<float, 4, 4> CPS4::m_C;
Eigen::Matrix<float, 4, 4> CPS4::m_IC;

float XI[4] = { -1.0, 1.0, 1.0, -1.0 };
float ETA[4] = { -1.0, -1.0, 1.0, 1.0 };

void CPS4::Init()
{
	//x0: -1; x1: 1; x2: 1; x3: -1
	//y0: -1; y1: -1; y2: 1; y3: 1
	m_C <<	Eigen::Vector4f(1.0,				1.0,				1.0,				1.0),
			Eigen::Vector4f(XI[0],				XI[1],				XI[2],				XI[3]),			// x0, x1, x2, x3
			Eigen::Vector4f(ETA[0],				ETA[1],				ETA[2],				ETA[3]),		// y0, y1, y2, y3
			Eigen::Vector4f(XI[0] * ETA[0],		XI[1] * ETA[1],		XI[2] * ETA[2],		XI[3] * ETA[3]);		// x0y0, x1y1, x2y2, x3y3
	m_IC = m_C.inverse();
}

void CPS4::SetIndices(const std::vector<int>& indices)
{
	assert(indices.size() == 4);
	m_nodes[0] = indices[0];
	m_nodes[1] = indices[1];
	m_nodes[2] = indices[2];
	m_nodes[3] = indices[3];
}

std::vector<int> CPS4::GetIndices() const
{
	std::vector<int> indices(4);
	indices[0] = m_nodes[0];
	indices[1] = m_nodes[1];
	indices[2] = m_nodes[2];
	indices[3] = m_nodes[3];
	return indices;
}

std::vector<Eigen::Vector3f> CPS4::GetFunctionValuesAtNodes(const Eigen::VectorXf& deforms)const
{
	Eigen::Matrix<float, 8, 1> uv;
	std::vector<Eigen::Vector3f> output;
	for (int i = 0; i < 4; ++i)
	{
		uv[2 * i + 0] = deforms[2 * m_nodes[i] + 0];
		uv[2 * i + 1] = deforms[2 * m_nodes[i] + 1];
	}

	for (int i = 0; i < 4; i++)
	{
		Eigen::Matrix<float, 3, 8> B = GetB(XI[i], ETA[i] );
		Eigen::Vector3f strain = B * uv;
		output.push_back(strain);
	}
	return output;
}

void CPS4::CalcK(const StrideDataArray& nodes, const tfem::MaterialPtr mat, std::vector<Eigen::Triplet<float> >& tripletVector)
{
	m_mat = mat;
	Eigen::Vector4f X;
	Eigen::Vector4f Y;
	for (int i = 0; i < 4; ++i)
	{
		X[i] = nodes(m_nodes[i], 0);
		Y[i] = nodes(m_nodes[i], 1);
	}

	m_KX = m_IC * X;
	m_KY = m_IC * Y;

	float area = 0;
	Eigen::Matrix<float, 8, 8> K;
	K.setZero();

	float xi, eta, w1, w2;
	for (int i = 0; GaussQuadrature::GetWeights<2>(i, xi, w1); i++)
	{
		for (int j = 0; GaussQuadrature::GetWeights<2>(j, eta, w2); j++)
		{
			float w = w1 * w2;
			Eigen::Matrix<float, 2, 2> J = GetJ(xi, eta);

			Eigen::Matrix<float, 3, 8> B = GetB(xi, eta);

			K += B.transpose() * mat->GetElasticityMatrix(fem::PT_FlatStress) * B * J.determinant() * w;
			area += J.determinant() * w;
		}
	}

	GrabTriplets(K, tripletVector);
}

void CPS4::GrabTriplets(const Eigen::Matrix<float, 8, 8>& K, std::vector<Eigen::Triplet<float> >& tripletVector) const
{
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			Eigen::Triplet<float> trplt11(2 * m_nodes[i] + 0, 2 * m_nodes[j] + 0, K(2 * i + 0, 2 * j + 0));
			Eigen::Triplet<float> trplt12(2 * m_nodes[i] + 0, 2 * m_nodes[j] + 1, K(2 * i + 0, 2 * j + 1));
			Eigen::Triplet<float> trplt21(2 * m_nodes[i] + 1, 2 * m_nodes[j] + 0, K(2 * i + 1, 2 * j + 0));
			Eigen::Triplet<float> trplt22(2 * m_nodes[i] + 1, 2 * m_nodes[j] + 1, K(2 * i + 1, 2 * j + 1));

			tripletVector.push_back(trplt11);
			tripletVector.push_back(trplt12);
			tripletVector.push_back(trplt21);
			tripletVector.push_back(trplt22);
		}
	}
}

tfem::Material* CPS4::GetMaterial()
{
	return m_mat.get();
}

IElement* CPS4::Create()
{
	return new CPS4;
}

CPS4::CPS4()
{

}

Eigen::Matrix<float, 1, 4> CPS4::GetP(float xi, float eta) const
{
	return Eigen::Matrix<float, 1, 4>(1, xi, eta, xi * eta);
}

Eigen::Matrix<float, 1, 4> CPS4::GetdPdxi(float xi, float eta) const
{
	return Eigen::Matrix<float, 1, 4>(0, 1, 0, eta);
}

Eigen::Matrix<float, 1, 4> CPS4::GetdPdeta(float xi, float eta) const
{
	return Eigen::Matrix<float, 1, 4>(0, 0, 1, xi);
}

Eigen::Matrix<float, 2, 2> CPS4::GetJ(float xi, float eta) const
{
	float dxdxi = GetdPdxi(xi, eta) * m_KX;
	float dydxi = GetdPdxi(xi, eta) * m_KY;
	float dxdeta = GetdPdeta(xi, eta) * m_KX;
	float dydeta = GetdPdeta(xi, eta) * m_KY;
	Eigen::Matrix<float, 2, 2> result;
	result << dxdxi, dydxi, dxdeta, dydeta;
	return result;
}

Eigen::Matrix<float, 3, 8> CPS4::GetB(float xi, float eta) const
{
	Eigen::Matrix<float, 3, 8> B;
	Eigen::Matrix<float, 2, 2> J = GetJ(xi, eta);
	Eigen::Matrix<float, 2, 2> IJ = J.inverse();

	Eigen::Matrix<float, 1, 4> dNdxi = GetdPdxi(xi, eta) * m_IC;
	Eigen::Matrix<float, 1, 4> dNdeta = GetdPdeta(xi, eta) * m_IC;

	for (int k = 0; k < 4; ++k)
	{
		Eigen::Matrix<float, 2, 1> dNkdxieta(dNdxi[k], dNdeta[k]);
		Eigen::Matrix<float, 2, 1> dNkdxy = IJ * dNkdxieta;
		B(0, 2 * k + 0) = dNkdxy[0];
		B(0, 2 * k + 1) = 0;
		B(1, 2 * k + 0) = 0;
		B(1, 2 * k + 1) = dNkdxy[1];
		B(2, 2 * k + 0) = dNkdxy[1];
		B(2, 2 * k + 1) = dNkdxy[0];
	}
	return B;
}


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

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

using Pair = std::pair<int, double>;
using std::vector;

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

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

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

/*
 * Finds all primes in {2,...,n} using the fact that if a natural number
 * has a divisor it also has at least one prime divisor. the resulting vector
 * contains the numbers -1.
 */
vector<int> ShortestPathHeuristic::findPrimes(int n) {
	assert(n >= 2);

	vector<int> primes;
	primes.push_back(2);

	for (int i = 3; i < n; ++i) {
		bool isPrime = true;
		for (unsigned int j = 0; j < primes.size() && primes[j]*primes[j] <= i; j++) {
			if (i % primes[j] == 0) {
				isPrime = false;
				break;
			}
		}
		if (isPrime) {
			primes.push_back(i);
		}
	}

	for (unsigned int i = 0; i < primes.size(); ++i) {
		primes[i]--;
	}

	return primes;
}

double ShortestPathHeuristic::constructSteinerTree(Graph& g, int numVertices, int source) {
	assert(numVertices >= 2);

	using Heap = heap::fibonacci_heap<heap_data>;

	vector<int> primes = findPrimes(numVertices);
	int numTerminals = primes.size();

	double** distances = new double*[numTerminals];
	int** predecessors = new int*[numTerminals];

	/*shortest paths from all terminals to all other nodes are calculated*/
	for (int i = 0; i < numTerminals; ++i) {
		if (primes[i] != source) {
			distances[i] = new double[numVertices];
			predecessors[i] = new int[numVertices];
			MyDijkstra::computeShortestPaths(g, numVertices, primes[i], distances[i], predecessors[i]);
		} else {
			distances[i] = new double[0];
			predecessors[i] = new int[0];
		}
	}

	/*we use a priority queue to keep track of remaining and closest terminals*/
	Heap heap;
	Heap::handle_type *handles = new Heap::handle_type[numTerminals];

	//all terminals except for the source are added to heap
	for (int i = 0; i < numTerminals; ++i) {
		if (primes[i] != source) {
			handles[i] = heap.push(std::make_pair(i, distances[i][source]));
		}
	}

	//we also keep track of the closest node in the tree for each terminal
	double objectiveValue = 0;
	int nearestNodes[numTerminals];
	for (int i = 0; i < numTerminals; i++) {
		nearestNodes[i] = source;
	}

	while (!heap.empty()) {
		Pair nextTerminal = heap.top().pair;
		int connectionNode = nearestNodes[nextTerminal.first];
		heap.pop();			//top element in heap always is the closest to tree

		objectiveValue += (distances[nextTerminal.first])[connectionNode];

		//now we iterate over all remaining terminals to check whether
		//they have become closer to the new tree
		for (Heap::iterator it = heap.begin(); it != heap.end(); ++it) {
			Pair curr = (*it).pair;
			double min = curr.second;
			int pred = connectionNode;
			bool reachedEnd = false;

			//for each terminal we iterate over all vertices on newly added path
			while (!reachedEnd){
				reachedEnd = (pred == primes[nextTerminal.first]);
				double tmp = (distances[curr.first])[pred];
				if (tmp < min) {
					min = tmp;
					nearestNodes[curr.first] = pred;
				}
				pred = predecessors[nextTerminal.first][pred];
			}

			//update of heap if neccessary
			if (min < curr.second) {
				(*handles[curr.first]).pair.second = min;
				heap.increase(handles[curr.first]);
			}
		} //end of iteration through heap

	} //end of algorithm

	for (int i = 0; i < numTerminals; i++) {
		delete[] distances[i];
		delete[] predecessors[i];
	}
	delete[] distances;
	delete[] predecessors;
	delete[] handles;

	return objectiveValue;

}

double ShortestPathHeuristic::constructSteinerTree(Graph& g, int numVertices) {
	return constructSteinerTree(g, numVertices, 1);
}


#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Stream_lines_2.h>
#include <CGAL/Runge_kutta_integrator_2.h>
#include <CGAL/Regular_grid_2.h>

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/StreamLinesGraphicsItem.h>
#include <CGAL/Qt/RegularGridVectorFieldGraphicsItem.h>

// for viewportsBbox
#include <CGAL/Qt/utility.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif
// the two base classes
#include "ui_Stream_lines_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;

typedef CGAL::Regular_grid_2<K> Regular_grid;
typedef CGAL::Runge_kutta_integrator_2<Regular_grid> Runge_kutta_integrator;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator> Stream_lines;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator>::Stream_line_iterator_2 Stream_line_iterator;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator>::Point_iterator_2 Point_iterator;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator>::Point_2 Point_2;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator>::Vector_2 Vector;

typedef K::Iso_rectangle_2 Iso_rectangle_2;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Stream_lines_2
{
  Q_OBJECT

private:
  Stream_lines * stream_lines;
  Runge_kutta_integrator * runge_kutta_integrator;
  Regular_grid * regular_grid;
  double density;
  double ratio;
  double integrating;
  int sampling;
  QGraphicsScene scene;

  CGAL::Qt::StreamLinesGraphicsItem<Stream_lines,K> * sli;
  CGAL::Qt::RegularGridVectorFieldGraphicsItem<Regular_grid,K> * rgi;

public:
  MainWindow();

public Q_SLOTS:

  void on_actionLoadPoints_triggered();

  void on_actionClear_triggered();

  void on_actionSavePoints_triggered();

  void on_actionRecenter_triggered();

  virtual void open(QString fileName);

private:
  void generate();

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow(), density(12.0), ratio(1.6), integrating(1.0), sampling(1)
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);

  // Manual handling of actions
  //

  QObject::connect(this->actionQuit, SIGNAL(triggered()),
		   this, SLOT(close()));

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);

  // Turn the vertical axis upside down
  this->graphicsView->matrix().scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Stream_lines_2.html");
  this->addAboutCGAL();

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
	  this, SLOT(open(QString)));
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */

void
MainWindow::on_actionClear_triggered()
{
  Q_EMIT( changed());
}

void
MainWindow::generate()
{
  stream_lines = new Stream_lines(*regular_grid, *runge_kutta_integrator, density, ratio, sampling);

  sli = new CGAL::Qt::StreamLinesGraphicsItem<Stream_lines, K>(stream_lines);
  rgi = new CGAL::Qt::RegularGridVectorFieldGraphicsItem<Regular_grid, K>(regular_grid);

  QObject::connect(this, SIGNAL(changed()),
		   sli, SLOT(modelChanged()));

  rgi->setVerticesPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  rgi->setEdgesPen(QPen(Qt::gray, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  sli->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(sli);
  scene.addItem(rgi);

  on_actionRecenter_triggered();
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadPoints_triggered()
{
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#endif
  QString fileName = QFileDialog::getOpenFileName(this,
						  tr("Open grid file"),
						  "."
						#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
						,tr("WKT files (*.wkt *.WKT)")
						#endif
                                                  );
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::ifstream ifs(qPrintable(fileName));

  runge_kutta_integrator = new Runge_kutta_integrator(integrating);
  double iXSize, iYSize;
  iXSize = iYSize = 512;
  if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
  {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    std::vector<std::vector<Point_2> > mp;
    int size= -1;
    do
    {
      std::vector<Point_2> ps;
      CGAL::read_multi_point_WKT(ifs, ps);
      if(size == -1)
        size = static_cast<int>(ps.size());
      else if(ps.size() > 0 && size != static_cast<int>(ps.size()))
        ps.resize(size);
      else if(ps.size() == 0)
        continue;
      mp.push_back(ps);
    }while(ifs.good() && !ifs.eof());
    regular_grid = new Regular_grid(size, static_cast<int>(mp.size()), iXSize, iYSize);
    /*fill the grid with the appropriate values*/
    for (unsigned int i=0;i<static_cast<unsigned int>(size);++i)
      for (unsigned int j=0;j<mp.size();++j)
      {
        regular_grid->set_field(i, j, Vector(mp[j][i].x(), mp[j][i].y()));
      }
#else
    QApplication::restoreOverrideCursor();
    return;
#endif
  }
  else{
    unsigned int x_samples, y_samples;
    ifs >> x_samples;
    ifs >> y_samples;
    regular_grid = new Regular_grid(x_samples, y_samples, iXSize, iYSize);
    /*fill the grid with the appropriate values*/
    for (unsigned int i=0;i<x_samples;i++)
      for (unsigned int j=0;j<y_samples;j++)
      {
        double xval, yval;
        ifs >> xval;
        ifs >> yval;
        regular_grid->set_field(i, j, Vector(xval, yval));
      }
  }
  ifs.close();
  // default cursor
  QApplication::restoreOverrideCursor();
  this->addToRecentFiles(fileName);
  generate();
  Q_EMIT( changed());

}

void
MainWindow::on_actionSavePoints_triggered()
{
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
  QString fileName = QFileDialog::getSaveFileName(this,
						  tr("Save points"),
						  ".",
                                                  tr("WKT files (*.wkt *.WKT)"));
  if(! fileName.isEmpty()){
    std::ofstream ofs(qPrintable(fileName));

    std::vector<std::vector<Point_2> >mp;
    mp.resize(regular_grid->get_dimension().second);
    for (int i=0;i<regular_grid->get_dimension().first;++i)
    {
      mp[i].reserve(regular_grid->get_dimension().second);
      for (int j=0;j<regular_grid->get_dimension().second;++j)
      {
        mp[i].push_back(Point_2(regular_grid->get_field(j,i).x(),
                                regular_grid->get_field(j,i).y()));
      }
      CGAL::write_multi_point_WKT(ofs, mp[i]);
    }
    ofs.close();
  }
#endif
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(rgi->boundingRect());
  this->graphicsView->fitInView(rgi->boundingRect(), Qt::KeepAspectRatio);
}

#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Stream_lines_2 demo");

  // Import resources from libCGAL (Qt5).
  // See https://doc.qt.io/qt-5/qdir.html#Q_INIT_RESOURCE
  CGAL_QT_INIT_RESOURCES;
  Q_INIT_RESOURCE(Stream_lines_2);

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}

#include "Stream_lines_2.moc"


// 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 "theia/sfm/pose/six_point_radial_distortion_homography.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Jacobi>

namespace theia {

using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Vector3d;
using Eigen::Vector2d;
using Vector6d = Eigen::Matrix<double, 6, 1>;
using Array51d = Eigen::Array<double, 5, 1>;
using Matrix68d = Eigen::Matrix<double, 6, 8>;
using Matrix62d = Eigen::Matrix<double, 6, 2>;
using Matrix65d = Eigen::Matrix<double, 6, 5>;
using Eigen::Matrix3d;

bool IsNearZero(double val) {
  return (+val < (100.0 * std::numeric_limits<double>::epsilon())) &&
         (-val < (100.0 * std::numeric_limits<double>::epsilon()));
}

bool SixPointRadialDistortionHomography(
    const std::vector<Eigen::Vector2d>& normalized_feature_points_left,
    const std::vector<Eigen::Vector2d>& normalized_feature_points_right,
    std::vector<RadialHomographyResult>* results, const double lmin,
    const double lmax) {
  Matrix62d X;
  Matrix62d U;

  for (int i = 0; i < 6; ++i) {
    X.row(i) = normalized_feature_points_left[i];
    U.row(i) = normalized_feature_points_right[i];
  }

  Matrix68d M;
  Vector6d u2 = U.col(0).array().square() + U.col(1).array().square();

  M.col(0) = -X.col(1).array() * U.col(0).array();
  M.col(1) = -X.col(1).array() * U.col(1).array();
  M.col(2) = -X.col(1);
  M.col(3) = X.col(0).array() * U.col(0).array();
  M.col(4) = X.col(0).array() * U.col(1).array();
  M.col(5) = X.col(0);
  M.col(6) = -X.col(1).array() * u2.array();
  M.col(7) = X.col(0).array() * u2.array();

  Eigen::JacobiSVD<Matrix68d, Eigen::FullPivHouseholderQRPreconditioner> Svd1(
      M, Eigen::ComputeFullV);
  const Eigen::Matrix<double, 8, 8>& V1 = Svd1.matrixV();

  const double a = -V1(2, 6) * V1(7, 6) + V1(5, 6) * V1(6, 6);
  const double b = -V1(2, 6) * V1(7, 7) - V1(2, 7) * V1(7, 6) +
                   V1(5, 6) * V1(6, 7) + V1(5, 7) * V1(6, 6);
  const double c = -V1(2, 7) * V1(7, 7) + V1(5, 7) * V1(6, 7);
  const double d = b * b - 4.0 * a * c;

  int nsols = 0;
  Vector2d rs;

  if (IsNearZero(d)) {
    nsols = 1;
    rs(0) = (-b) / (2.0 * a);
  } else if (d > 0.0) {
    nsols = 2;
    double d2 = std::sqrt(d);
    rs(0) = (-b + d2) / (2.0 * a);
    rs(1) = (-b - d2) / (2.0 * a);
  } else {
    return false;
  }

  const Vector6d x2 = X.col(0).array().square() + X.col(1).array().square();
  Vector6d u3, r;
  Matrix<double, 8, 1> n;
  Matrix65d T;
  T.col(0) = -M.col(3);
  T.col(1) = -M.col(4);

  for (int i = 0; i < nsols; i++) {
    n = rs(i) * V1.col(6) + V1.col(7);
    const double l2 = n(6) / n(2);
    // skip this solution early if radial distortion is spurious
    if (l2 < lmin || l2 > lmax) {
      continue;
    }

    u3 = u3.Ones() + l2 * u2;
    r = n(0) * U.col(0) + n(1) * U.col(1) + n(2) * u3;

    T.col(2) = -X.col(0).array() * u3.array();
    T.col(3) = x2.array() * r.array();
    T.col(4) = r;

    Eigen::JacobiSVD<Matrix65d> Svd2(T, Eigen::ComputeFullV);
    Matrix<double, 5, 1> v2 = Svd2.matrixV().col(4);

    v2.head(4) /= v2(4);
    const double l1 = v2(3);
    // skip this solution early if radial distortion is spurious
    if (l1 < lmin || l1 > lmax) {
      continue;
    }

    RadialHomographyResult res;
    // fill homograhapy
    res.H << n(0), n(1), n(2), n(3), n(4), n(5), v2(0), v2(1), v2(2);
    // fill radial distortion values
    res.l1 = l1;
    res.l2 = l2;
    results->push_back(res);
  }

  return nsols > 0;
}
}


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_geometry_ellipsoid_hpp__
#define __multicontact_api_geometry_ellipsoid_hpp__

#include <Eigen/Dense>
#include <iostream>

#include "multicontact-api/geometry/fwd.hpp"

namespace multicontact_api {
namespace geometry {
template <typename _Scalar, int _dim, int _Options>
struct Ellipsoid {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef _Scalar Scalar;
  enum { dim = _dim };
  enum { Options = _Options };

  typedef Eigen::Matrix<Scalar, dim, dim, Options> Matrix;
  typedef Eigen::Matrix<Scalar, dim, 1, Options> Vector;

  Ellipsoid(const Matrix& A, const Vector& center) : m_A(A), m_center(center) {}

  Scalar lhsValue(const Vector& point) const { return (m_A * (point - m_center)).norm(); }

  const Matrix& A() const { return m_A; }
  Matrix& A() { return m_A; }
  const Vector& center() const { return m_center; }
  Vector& center() { return m_center; }

  void disp(std::ostream& os) const {
    os << "A:\n" << m_A << std::endl << "center: " << m_center.transpose() << std::endl;
  }

  friend std::ostream& operator<<(std::ostream& os, const Ellipsoid& E) {
    E.disp(os);
    return os;
  }

 protected:
  /// \brief
  Matrix m_A;

  /// \brief Center of the ellipsoid expressed in the global frame.
  Vector m_center;
};
}  // namespace geometry
}  // namespace multicontact_api

#endif  // ifndef __multicontact_api_geometry_ellipsoid_hpp__


#include "discrete_exponential_map.h"

#include <set>
#include <queue>

#include <Eigen/Dense>

#include <igl/hessian_energy.h>
#include <igl/massmatrix.h>
#include <igl/cotmatrix.h>

#include <geometry/patch.h>
#include <shape_signatures/shape_signature.h>
#include <algorithms/shortest_path.h>
#include <matching/surface_stroke.h>

using namespace shortest_path;

DiscreteExponentialMap::DiscreteExponentialMap():
	_TBN(Eigen::Matrix3d::Identity()),
	_TBN_inv(Eigen::Matrix3d::Identity()),
	_geometry(nullptr) {

}

// Unintuitively, p_vid is actually the index of the vertex in relation to patch->origin_mesh()
DiscreteExponentialMap::DiscreteExponentialMap(std::shared_ptr<Patch> patch, Eigen::DenseIndex p_vid, const Eigen::MatrixXd* guide_points) {
	// Implementation of Discrete Exponential Map from,
	// "Part-Based Representation and Editing of 3D Surface Models", Ryan Schmidt, 2011

	auto mesh = patch->origin_mesh();

	const Eigen::MatrixXd& V = mesh->vertices();
	const Eigen::MatrixXd& N_orig = mesh->vertex_normals();
	Eigen::MatrixXd N(N_orig.rows(), N_orig.cols());

	if (V.size() <= 0 || N.size() <= 0) {
		return;
	}

	std::map<Eigen::DenseIndex, std::shared_ptr<DjikstraVertexNode>> nodes = djikstras_algorithm(patch, p_vid);

	if (nodes.size() == 0) {
		// djikstra's failed??
		return;
	}

	// Smooth the normals a bit before generating the map
	Eigen::SparseMatrix<double> M2;
	Eigen::SparseMatrix<double> QH;

	Eigen::MatrixXd V3 = V.leftCols<3>();
	Eigen::MatrixXi F3 = mesh->faces().leftCols<3>();

	igl::massmatrix(V3, F3, igl::MASSMATRIX_TYPE_BARYCENTRIC, M2);
	igl::hessian_energy(V3, F3, QH);

    // Smoothing -- 0.0 is no smoothing, 1.0 is full
	const double alpha = 0.25;

	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> hessSolver(alpha * QH + (1.0 - alpha) * M2);
	N << hessSolver.solve((1.0 - alpha) * M2 * N_orig);

	Eigen::Vector3d p = V.row(p_vid).block<1, 3>(0, 0).transpose();
	Eigen::Vector3d Np = N.row(p_vid).block<1, 3>(0, 0).transpose().normalized();

	Eigen::Matrix3d Tp_TBN = basis_from_plane_normal(Np);

	// For each point in patch, create a chain of points also within the patch leading back to the center by the shortest path
	std::priority_queue<std::shared_ptr<DjikstraVertexNode>, std::vector<std::shared_ptr<DjikstraVertexNode>>, DjikstraDist> Q;

	for (auto it = nodes.begin(); it != nodes.end(); ++it) {
		Q.push(it->second);
	}

	// Create a set of planar undirected edges representing the discrete exponential map
	std::map<Eigen::DenseIndex, Eigen::Vector2d> DEM_points;
	DEM_points.insert(std::make_pair(Q.top()->_vid, Eigen::Vector2d(0.0, 0.0)));
	Q.pop();

	while (!Q.empty()) {
		auto node = Q.top();
		Q.pop();

		assert(node->_prev != nullptr);
		assert(node->_dist > 0.0);
		assert(node->_vid >= 0);

		// Run from each neighbor already present in DEM_points, as an "Upwind Average"
		// Find all of q's neighbors already in the DEM that share locality with node->_prev
		std::vector<Eigen::DenseIndex> neighbors = patch->origin_mesh()->one_ring(node->_vid);
		std::vector<std::pair<Eigen::Vector2d, double>> upwind;
		Eigen::Vector2d from_parent;
		double parent_dist = 0.0;

		for (Eigen::DenseIndex prev_vid : neighbors) {
			auto prev = DEM_points.find(prev_vid);

			if (prev == DEM_points.end()) {
				continue;
			}

			Eigen::Vector3d q = V.row(node->_vid).block<1, 3>(0, 0).transpose();

			if (prev_vid == p_vid) {
				Eigen::Vector2d Tpq = local_log_map(p, Tp_TBN, q).topRows<2>();

				double weight = (q - p).squaredNorm() + 1e-7;

				upwind.push_back(std::make_pair(Tpq, weight));

				if (prev_vid == node->_prev->_vid) {
					// Store away for special locality test when averaging upwind points
					parent_dist = Tpq.norm();
					from_parent = Tpq;
				}

				continue;
			}

			Eigen::Vector3d r = V.row(prev_vid).block<1, 3>(0, 0).transpose();
			Eigen::Vector2d Tpr = prev->second;

			// A vector parallel to two planes is the cross product of the two normals.
			Eigen::Vector3d Nr = N.row(prev_vid).block<1, 3>(0, 0).transpose().normalized();
			Eigen::Matrix3d Tr_TBN = basis_from_plane_normal(Nr);

			// Do not pass through the intermediate planes -- instead, just transform directly into Tp after log_r(q)
			Eigen::Vector2d Trq = local_log_map(r, Tr_TBN, q).topRows<2>();

			// 3D rotation Mn
			Eigen::Vector3d rot_axis = Nr.cross(Np);

			if (rot_axis.isZero(1e-7)) {
				rot_axis = Tp_TBN.col(0);
			}

			rot_axis.normalize();

			double rot_angle_3d = std::acos(std::min(1.0, std::max(-1.0, Nr.dot(Np))));
			Eigen::AngleAxis<double> R(rot_angle_3d, rot_axis);

			// 2D rotation for planar basis alignment
			Eigen::Vector3d er = (R * Tr_TBN.col(0)).normalized();
			Eigen::Vector3d ep = Tp_TBN.col(0);

			assert(er.dot(Np) < 1e-7);

			double rot_angle_2d = std::acos(std::min(1.0, std::max(-1.0, er.dot(ep))));

			if (!er.cross(ep).isZero(1e-7) && er.cross(ep).normalized().dot(Np) > 0.0) {
				rot_angle_2d *= -1.0;
			}

			Eigen::Rotation2D<double> E(rot_angle_2d);

			Eigen::Vector2d Tpq = Tpr + E * Trq;

			double weight = 1.0 / ((q - r).squaredNorm() + 1e-7);

			if (prev_vid == node->_prev->_vid) {
				// Store away for special locality test when averaging upwind points
				parent_dist = (Tpr - Tpq).norm();
				from_parent = Tpq;
			}

			upwind.push_back(std::make_pair(Tpq, weight));
		}

		assert(upwind.size() > 0);

		Eigen::Vector2d Tpq_avg(0.0, 0.0);

		double total_weight = 0.0;
		for (auto pt : upwind) {
			if ((pt.first - from_parent).norm() > parent_dist / 2.0) {
				continue;
			}

			total_weight += pt.second;
		}

		for (auto pt : upwind) {
			if ((pt.first - from_parent).norm() > parent_dist / 2.0) {
				continue;
			}

			Tpq_avg += pt.second * pt.first / total_weight;
		}

		DEM_points.insert(std::pair<Eigen::DenseIndex, Eigen::Vector2d>(node->_vid, Tpq_avg));
	}

	if (!init(p_vid, Tp_TBN, DEM_points, patch)) {
		throw std::invalid_argument("DEM arguments invalid!");
	}
}

DiscreteExponentialMap::DiscreteExponentialMap(const Eigen::DenseIndex center_vid, Eigen::Matrix3d& TBN, const std::map<Eigen::DenseIndex, Eigen::Vector2d>& vertices, std::shared_ptr<Patch> geometry) {
	if (!init(center_vid, TBN, vertices, geometry)) {
		throw std::invalid_argument("DEM arguments invalid!");
	}
}

DiscreteExponentialMap::~DiscreteExponentialMap() {

}

Eigen::MatrixXi DiscreteExponentialMap::get_reindexed_faces() const {
	Eigen::MatrixXi F = _faces;

	std::map<Eigen::DenseIndex, Eigen::DenseIndex> vid_remap;
	Eigen::DenseIndex i = 0;
	for (auto it = _vertices.cbegin(); it != _vertices.cend(); ++it, ++i) {
		vid_remap.insert(std::make_pair(it->first, i));
	}

	for (i = 0; i < F.size(); ++i) {
		F(i) = vid_remap.at(F(i));
	}

	return F;
}

bool DiscreteExponentialMap::init(const Eigen::DenseIndex center_vid, Eigen::Matrix3d& TBN, const std::map<Eigen::DenseIndex, Eigen::Vector2d>& vertices, std::shared_ptr<Patch> geometry) {
	_geometry = geometry;
	_TBN = TBN;
	_TBN_inv = TBN.inverse();
	_vertices = vertices;

	_center_vid = center_vid;

	// Only include faces made up of vertices in the map
	std::set<Eigen::DenseIndex> fids;
	std::shared_ptr<Mesh> mesh = _geometry->origin_mesh();
	const Eigen::MatrixXi& F = mesh->faces();
	const Eigen::MatrixXd& V = mesh->vertices();

	for (Eigen::DenseIndex i = 0; i < F.rows(); ++i) {
		bool included = true;

		for (Eigen::DenseIndex j = 0; j < F.cols(); ++j) {
			if (vertices.find(F(i, j)) == vertices.end()) {
				included = false;
				break;
			}
		}

		if (!included) {
			continue;
		}

		for (Eigen::DenseIndex j = 0; j < F.cols(); ++j) {
			// Check edge length (it should not be severely distorted
			Eigen::DenseIndex next = (j + 1) % F.cols();

			double dist = (V.row(F(i, j)).leftCols<3>() - V.row(F(i, next)).leftCols<3>()).norm();
			double dem_dist = (vertices.at(F(i, j)) - vertices.at(F(i, next))).norm();

			if (dem_dist > 2.0 * dist) {
				included = false;
				break;
			}
		}

		if (included) {
			fids.insert(i);
		}
	}

	_faces = Eigen::MatrixXi(fids.size(), F.cols());

	int fIndex = 0;
	for (auto fid : fids) {
		_fid_remap.insert(std::make_pair(fIndex, fid));
		_faces.row(fIndex++) << F.row(fid);
	}

	std::stringstream ss; ss << geometry->origin_mesh()->resource_dir() << "//matlab//dem_debug.m";
	to_matlab(ss.str());

	std::vector<std::pair<Eigen::DenseIndex, Eigen::Vector2d>> face_centers;

	_center_fid = -1;

	std::vector<Eigen::DenseIndex> center_fids;
	for (Eigen::DenseIndex i = 0; i < _faces.rows(); ++i) {
		for (Eigen::DenseIndex j = 0; j < _faces.cols(); ++j) {
			if (_faces(i, j) == _center_vid) {
				center_fids.push_back(i);
				break;
			}
		}
	}

	for (auto fid : center_fids) {
		Eigen::Vector2d fc = Eigen::Vector2d::Zero();

		for (Eigen::DenseIndex j = 0; j < _faces.cols(); ++j) {
			fc += vertices.at(_faces(fid, j)) / static_cast<double>(_faces.cols());
		}

		face_centers.push_back(std::pair<Eigen::DenseIndex, Eigen::Vector2d>(fid, fc));
	}

	Eigen::DenseIndex centroid_fid = -1;
	double dist = std::numeric_limits<double>::max();

	for (Eigen::DenseIndex i = 0; i < face_centers.size(); ++i) {
		double c_dist = (face_centers[i].second - vertices.at(_center_vid)).norm();

		if (c_dist < dist) {
			dist = c_dist;
			_center_fid = face_centers[i].first;
		}
	}

	if (_center_fid < 0 && _vertices.size() > 2) {
		throw std::domain_error("Invalid _center_fid!");
	}

	return true;
}

Eigen::DenseIndex DiscreteExponentialMap::get_center_vid() const {
	return _center_vid;
}

Eigen::DenseIndex DiscreteExponentialMap::get_center_fid() const {
	return _center_fid;
}

Eigen::MatrixXd DiscreteExponentialMap::get_3d_vertices() const {
	// get_reindexed_faces describes the faces for this vertex ordering
	Eigen::MatrixXd V_3d(_vertices.size(), 3);

	Eigen::DenseIndex index = 0;
	for (auto it = _vertices.cbegin(); it != _vertices.cend(); ++it) {
		Eigen::Vector3d tbn_point = (Eigen::Vector3d() << it->second, 0.0).finished();

		V_3d.row(index++) = _TBN * tbn_point;
	}

	return V_3d;
}

double DiscreteExponentialMap::get_radius() const {
	Eigen::MatrixXd V = get_3d_vertices();

	// the DEM is relative to the frame origin, so the radius is just the greatest magnitude norm
	double radius = 0.0;
	for (Eigen::DenseIndex i = 0; i < V.rows(); ++i) {
		radius = std::max(V.row(i).norm(), radius);
	}

	return radius;
}

Eigen::Vector3d DiscreteExponentialMap::get_normal() const {
	return _TBN.col(2);
}

Eigen::Vector3d DiscreteExponentialMap::get_tangent() const {
	return _TBN.col(0);
}

Eigen::Vector3d DiscreteExponentialMap::get_bitangent() const {
	return _TBN.col(1);
}

Eigen::Vector2d DiscreteExponentialMap::interpolated_polar(Eigen::Vector3d barycentric_coords, const std::vector<Eigen::DenseIndex>& vids) {
	Eigen::Vector2d polar;
	Eigen::MatrixXd points(2,3);

	for (Eigen::DenseIndex i = 0; i < 3; i++) {
		// Just gonna let it throw an exception if the vids are no in the map -- shame on the user!
		points.col(i) = _vertices[vids[i]];
	}

	Eigen::Vector2d xy = points * barycentric_coords;

	polar << std::sqrt(std::pow(xy(0), 2) + std::pow(xy(1), 2)), std::atan2(xy(1), xy(0));

	return polar;
}

Eigen::DenseIndex DiscreteExponentialMap::nearest_vertex_by_polar(const Eigen::Vector2d& polar_point) {
	Eigen::Vector2d xy_point;
	xy_point << polar_point(0) * std::cos(polar_point(1)),
				polar_point(0) * std::sin(polar_point(1));

	Eigen::DenseIndex vid = -1;
	double dist = std::numeric_limits<double>::max();
	for (auto v : _vertices) {
		double t_dist = (v.second - xy_point).norm();

		if (t_dist < dist) {
			vid = v.first;
			dist = t_dist;
		}
	}

	return vid;
}

Eigen::VectorXd DiscreteExponentialMap::query_map_value(const Eigen::Vector2d& xy_point, std::shared_ptr<ShapeSignature> sig) const {
	// Find triangle which contains (x, y)
	unsigned int i = 0;
	for (i = 0; i < _faces.rows(); ++i) {
		Eigen::DenseIndex r = _faces(i, 0);
		Eigen::DenseIndex s = _faces(i, 1);
		Eigen::DenseIndex t = _faces(i, 2);

		Eigen::Vector2d a = _vertices.at(_faces(i, 0));
		Eigen::Vector2d b = _vertices.at(_faces(i, 1));
		Eigen::Vector2d c = _vertices.at(_faces(i, 2));

		if (point_in_triangle(xy_point, a, b, c)) {
			break;
		}
	}

	// In case point isn't in any triangle, return some invalid result (-1.0?)
	if (i >= _faces.rows()) {
		// point is not within any triangle of the map, so return a vector packed with -1.0s
		return Eigen::VectorXd::Constant(sig->feature_dimension(), -1.0);
	}

	// Find barycentric coordinates of the point within the triangle
	// https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
	Eigen::Vector2d v0 = _vertices.at(_faces(i, 1)) - _vertices.at(_faces(i, 0));
	Eigen::Vector2d v1 = _vertices.at(_faces(i, 2)) - _vertices.at(_faces(i, 0));
	Eigen::Vector2d v2 = xy_point - _vertices.at(_faces(i, 0));
	double d00 = v0.dot(v0);
	double d01 = v0.dot(v1);
	double d11 = v1.dot(v1);
	double d20 = v2.dot(v0);
	double d21 = v2.dot(v1);
	double denom = d00 * d11 - d01 * d01;
	double v = (d11 * d20 - d01 * d21) / denom;
	double w = (d00 * d21 - d01 * d20) / denom;
	double u = 1.0f - v - w;

	if (u + v + w - 1.0 > std::numeric_limits<double>::epsilon()) {
		throw std::domain_error("query_map_value(): Invalid barycentric coordinates!");
	}

	// return linearly interpolated feature values from triangle vertices with respect to (x,y)
	//Eigen::VectorXd value = u * features.row(_faces(i, 0)) + v * features.row(_faces(i, 1)) + w * features.row(_faces(i, 2));
	Eigen::VectorXd u_coord = sig->lerpable_coord(_fid_remap.at(i), _faces(i, 0));
	Eigen::VectorXd v_coord = sig->lerpable_coord(_fid_remap.at(i), _faces(i, 1));
	Eigen::VectorXd w_coord = sig->lerpable_coord(_fid_remap.at(i), _faces(i, 2));

	Eigen::VectorXd value = u * u_coord
						  + v * v_coord
						  + w * w_coord;

	value = sig->lerpable_to_signature_value(value);

	return value;
}

Eigen::VectorXd DiscreteExponentialMap::query_map_value_polar(const Eigen::Vector2d& polar_point, std::shared_ptr<ShapeSignature> sig) const {
	Eigen::Vector2d xy_point;
	xy_point << polar_point(0) * std::cos(polar_point(1)),
				polar_point(0) * std::sin(polar_point(1));

	return query_map_value(xy_point, sig);
}

bool DiscreteExponentialMap::to_matlab(std::string script_out_path) {
	std::ofstream m(script_out_path, std::ofstream::out);

	if (m.is_open()) {
		m << "figure;" << std::endl;
		m << "hold on;" << std::endl;
		m << "axis equal;" << std::endl;
		m << "grid on;" << std::endl;

		auto vertices = get_raw_vertices();
		m << "v = [ ..." << std::endl;
		for (auto vert : vertices) {
			m << vert.second.transpose() << "; ..." << std::endl;
		}
		m << "];" << std::endl;

		const Eigen::MatrixXi& faces = get_reindexed_faces();
		m << "f = [ ..." << std::endl;
		for (Eigen::DenseIndex i = 0; i < faces.rows(); ++i) {
			m << faces.row(i) << "; ..." << std::endl;
		}
		m << "];" << std::endl;

		m << "for i=1:size(f,1)" << std::endl;
		m << "a = [v(f(i, 1) + 1, :), 0.0];" << std::endl;
		m << "b = [v(f(i, 2) + 1, :), 0.0];" << std::endl;
		m << "c = [v(f(i, 3) + 1, :), 0.0];" << std::endl;
		m << "cb = (c - b) / norm(c - b);" << std::endl;
		m << "ab = (a - b) / norm(a - b);" << std::endl;
		m << "n = cross(cb, ab);" << std::endl;
		m << "n = n / norm(n);" << std::endl;
		m << "C = 'g';" << std::endl;
		m << "if dot(n, [0, 0, 1]) < 1 - 1e-7" << std::endl;
		m << "	C = 'r';" << std::endl;
		m << "end" << std::endl;
		m << "h = fill([a(1), b(1), c(1)], [a(2), b(2), c(2)], C);" << std::endl;
		m << "set(h, 'facealpha', .5);" << std::endl;
		m << "end" << std::endl;

		m << "scatter(v(:,1), v(:,2), 'mo');" << std::endl;

		m.close();
	}
	else {
		return false;
	}

	return true;
}