/* Generate an MCMC estimate of the edge appearance probabilities of a graph
 * in the directed spanning tree polytope by sampling random spanning trees
 *
 * Author : Rahul G. Krishnan
 * Inst.  : NYU
 */
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <vector>
#include <fstream>
#include <cstdlib>
#include <boost/graph/random_spanning_tree.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/array.hpp>
#include <string>
#include <boost/foreach.hpp>
#include <boost/unordered_map.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/graph/iteration_macros.hpp>
#ifdef DEBUG
#define DEBUG_MSG(str) do { std::cout << str << std::endl; } while( false )
#else
#define DEBUG_MSG(str) do { } while ( false )
#endif

//Parameters for the MC algorithm
int WEIGHTED = 0;
int MAXITS  = 10000;

//Defining types
typedef boost::property<boost::edge_weight_t, double> EdgeWeightProperty;
typedef boost::adjacency_list <
    boost::vecS, boost::vecS, boost::directedS,
    boost::no_property,EdgeWeightProperty > digraph_t;
typedef boost::unordered_map<std::string,double> unordered_map;
boost::random::mt19937 rng;
#define getKey(v1,v2) std::to_string((long long unsigned int)v1)+"->"+std::to_string((long long unsigned int)v2)

void initializeGraph(std::vector<boost::tuple<int,int,double> > edgeList,
                    digraph_t* g,
                    unordered_map* map)
{
    int v1,v2;
    double wt;
    boost::tuple<int,int,double> t;
    BOOST_FOREACH ( t, edgeList)
    {
        v1 = boost::get<0>(t);
        v2 = boost::get<1>(t);
        wt = boost::get<2>(t);
        DEBUG_MSG("Loading: "<<v1<<"->"<<v2<<" : "<<wt);
		//TODO: Investigate why
		//you have to use this hack. Likely assumption
		//in BOOST library
        add_edge(v1,v2,100/wt,*g);
        map->insert(unordered_map::value_type(getKey(v1,v2),0));
    }
}

/*
Given an edgeList, run the test case
*/
void runTest(int n_vertices,
	std::vector<boost::tuple<int,int,double> > edgeList,
	unordered_map* edgeProb,
	std::vector<double>* root_prob)
{
    digraph_t g;

    initializeGraph(edgeList,&g,edgeProb);
    for (unordered_map::iterator it = edgeProb->begin(); it != edgeProb->end(); ++it)
        DEBUG_MSG(it->first << ", " << it->second);

  	#ifdef DEBUG
    BGL_FORALL_EDGES(e, g, digraph_t)
    {
        std::cout<<e<<" W="<<get(boost::edge_weight, g, e)<<std::endl;
    }
	for(int v=0;v<n_vertices;v++)
	{
		double weight_sum = 0;
		BGL_FORALL_OUTEDGES(v, e, g, digraph_t) {std::cout<<e<<", ";weight_sum += get(get(boost::edge_weight,g), e);}
		std::cout<<v<<"->"<<weight_sum<<std::endl;
	}
    #endif
    std::vector<int> predecessors (n_vertices);
	std::vector<double> edgeMap (edgeList.size());

    for(int i=0;i<n_vertices;i++)
    {
        predecessors[i]=0;
        (*root_prob)[i] = 0;
    }
    int root;
    boost::random::uniform_int_distribution<> dist(0, n_vertices-1);
    for(int i=1;i<=MAXITS;i++)
    {
		std::cout<<".";
		std::fill(predecessors.begin(),predecessors.end(),0);
		if(i%500==0)
		{
			std::cout<<std::endl;
		}
        //Sample root uniformly
        root = dist(rng);
        #ifdef DEBUG_L2 //Since the DEBUG_MSG macro prints newline
            std::cout<<i<<"|"<<root<<"|,"<<std::flush;
        #endif
		if(WEIGHTED==1)
		{
        boost::random_spanning_tree(g,rng,
            boost::predecessor_map(
                boost::make_iterator_property_map(
                    predecessors.begin(), get(boost::vertex_index, g)))
            .root_vertex(root)
			.weight_map(get(boost::edge_weight,g))
								);
		}
		else
		{
        boost::random_spanning_tree(g,rng,
            boost::predecessor_map(
                boost::make_iterator_property_map(
                    predecessors.begin(), get(boost::vertex_index, g)))
            .root_vertex(root));
		}
#ifdef DEBUG
		std::cout<<"Printing spanning tree rooted at "<<root<<std::endl;
#endif
        //Update counts
        (*root_prob)[root]+=1;
        for(int i=0;i<n_vertices;i++)
        {
            if(predecessors[i]!=-1)
            {
                edgeProb->at(getKey(predecessors[i],i)) +=1;
#ifdef DEBUG
				std::cout<<predecessors[i]<<"--"<<i<<" Key: "<<getKey(predecessors[i],i)<<std::endl;
#endif
            }
            else
            {
                if(root!=i)
                    std::cout<<"Error. root="<<root<<" i="<<i<<std::endl;
            }

        }

    }
    DEBUG_MSG("");

}

int MCMC_spanning_tree(std::string fileIN,std::string fileOUT)
{
	//Read graph structure from fileIN
	int v1,v2,n_vertices,n_edges = 0;
	std::vector< boost::tuple<int,int,double> > edgeList;
	std::string line;
	double weight;
	std::ifstream inputf (fileIN.c_str());
	if(inputf.is_open())
	{
		getline(inputf,line);
		sscanf(line.c_str(),"%d",&n_vertices);
		while(getline(inputf,line))
		{
			sscanf (line.c_str(),"%d,%d,%lf",&v1,&v2,&weight);
			//Assumes vertices start from 0...N-1
			edgeList.push_back(boost::make_tuple(v1,v2,weight));
			n_edges++;
		}
		inputf.close();
	}
	else
	{
		std::cerr<<"Input file not found. Cannot be opened\n";
		return EXIT_FAILURE;
	}

	std::vector<double> root_prob (n_vertices);
    unordered_map edgeProb;
    runTest(n_vertices,edgeList, &edgeProb, &root_prob);
    DEBUG_MSG("---RESULT---");

    std::ofstream outputf (fileOUT.c_str());

    //Normalize root and edge probabilities
    for (int i=0;i<n_vertices;i++)
    {
    	outputf<<(root_prob[i]/MAXITS);
    	outputf<<" ";
        DEBUG_MSG("Node "<<i<<" : " << (root_prob[i]/MAXITS));
    }
    outputf<<"\n";
    boost::tuple<int,int,double> t;
    BOOST_FOREACH(t,edgeList)
    {
    	line = getKey(boost::get<0>(t),boost::get<1>(t));
    	DEBUG_MSG(boost::get<0>(t)<<"->"<<
    	    	  boost::get<1>(t)<<" : "<<(edgeProb.at(line)/MAXITS));
    	outputf<<(edgeProb.at(line)/MAXITS);
    	outputf<<" ";
    }

	return EXIT_SUCCESS;
}

std::string PNAME = "MCMC_spanning_tree";
int main(int argc,char* argv[])
{
	if (argc < 3)
	{
		std::cerr << "Usage (* indicates optional): " << PNAME
			 << " <input file name> <output file name> <Weighted=0>* <MAXIT=10K>*\n";
		return EXIT_FAILURE;
	}
	//Modify global constants if specified
	if (argc>=4)
	{
		WEIGHTED = atoi(argv[3]);
		std::cout<<"Modifying WEIGHTED to "<<WEIGHTED<<std::endl;
		if(WEIGHTED!=0 && WEIGHTED!=1)
		{
			std::cerr <<"WEIGHTED must be 1/0"<<std::endl;
			return EXIT_FAILURE;
		}
	}
	if (argc==5)
	{
		MAXITS = atoi(argv[4]);
		std::cout<<"Modifying MAXITS to "<<MAXITS<<std::endl;
	}
	DEBUG_MSG("---Calling <MCMC_spanning_tree>---\nINPUT FILE: "<<argv[1]<<"\nOUTPUT FILE: "<<argv[2]);
	return MCMC_spanning_tree(argv[1],argv[2]);

}


//============================================================================
// Name        : benchmark_main.cc
// Author      : Giovanni Azua (bravegag@hotmail.com)
// Since       : 25.07.2013
// Description : Main application for benchmarking Eigen with MAGMA and MKL
//============================================================================

#include <assert.h>
#include <iostream>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <vector>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include <boost/program_options/cmdline.hpp>
#include <boost/program_options/config.hpp>
#include <boost/program_options/environment_iterator.hpp>
#include <boost/program_options/eof_iterator.hpp>
#include <boost/program_options/errors.hpp>
#include <boost/program_options/option.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/positional_options.hpp>
#include <boost/program_options/value_semantic.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/version.hpp>

#include <boost/chrono.hpp>

#include <boost/tokenizer.hpp>

#include <Eigen/Dense>

/**
 * Define reusable Eigen vector and matrix types.
 */
template<typename T>
struct BenchType {
	typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixX;
	typedef Eigen::Matrix<T, Eigen::Dynamic, 1, Eigen::ColMajor> VectorX;
};

typedef BenchType<double>::MatrixX MatrixXd;
typedef BenchType<double>::VectorX VectorXd;

using namespace std;
using namespace boost::accumulators;
using namespace boost::chrono;
namespace po = boost::program_options;

typedef double (*workload_type)(long);

typedef accumulator_set<double, stats<tag::mean, tag::variance> > bench_accumulator;
static bench_accumulator real_time_acc, gflops_acc;

// reusable vector and matrices. Note we need to use pointers to make sure
// that the memory is deallocated *before* MAGMA/CUDA shutdown takes place.
MatrixXd *pA = new MatrixXd(), *pB = new MatrixXd(), *pC = new MatrixXd(), *pL = new MatrixXd();
VectorXd *pa = new VectorXd(), *pb = new VectorXd(), *pc = new VectorXd();
Eigen::ColPivHouseholderQR<MatrixXd> *pAqr = new Eigen::ColPivHouseholderQR<MatrixXd>();

// continue using them as before
MatrixXd &A = *pA, &B = *pB, &C = *pC, &L = *pL;
VectorXd &a = *pa, &b = *pb, &c = *pc;
Eigen::ColPivHouseholderQR<MatrixXd>& Aqr = *pAqr;

/// Generic benchmarking
/**
 * Generic benchmarking, implement the different workload types according to the expected signature.
 */
static void run_benchmark(long N, int warm_ups, int num_runs, workload_type workload) {
	// warm up runs
	for (int i = 0; i < warm_ups; ++i) {
		// invoke workload
		(*workload)(N);

		fprintf(stderr, ".");
		fflush (stderr);
	}

	// actual measurements
	for (int i = 0; i < num_runs; ++i) {
		double real_time, cpu_time, gflops;
		double flop_count;
		system_clock::time_point start;

		// benchmark using high-resolution timer
		start = system_clock::now();

		// invoke workload
		flop_count = (*workload)(N);

		// use high-resolution timer
		duration<double> sec = system_clock::now() - start;
		real_time = sec.count();
		gflops = flop_count / (1e9 * real_time);

		// feed the accumulators
		real_time_acc(real_time);
		gflops_acc(gflops);

		fprintf(stderr, ".");
		fflush (stderr);
	}
}

EIGEN_DONT_INLINE
static double dgemm(long N) {
	C = A * B;
	// flops see http://www.netlib.org/lapack/lawnspdf/lawn41.pdf page 120
	return 2 * N * N * N;
}

EIGEN_DONT_INLINE
static double dgeqp3(long N) {
	Eigen::ColPivHouseholderQR<MatrixXd> qr = A.colPivHouseholderQr();
	// flops see http://www.netlib.org/lapack/lawnspdf/lawn41.pdf page 121
	return N * N * N - (2 / 3) * N * N * N + N * N + N * N + (14 / 3) * N;
}

EIGEN_DONT_INLINE
static double dgeqrf(long N) {
	Eigen::HouseholderQR<MatrixXd> qr = A.householderQr();
	// flops see http://www.netlib.org/lapack/lawnspdf/lawn41.pdf page 121
	return 2 * N * N * N - (2 / 3) * N * N * N + 3 * N * N - N * N + (14 / 3) * N;
}

EIGEN_DONT_INLINE
static double dgemv(long N) {
	C = A * b;
	return 2 * N * N - N;
}

EIGEN_DONT_INLINE
static double dtrsm(long N) {
	C = Aqr.solve(B);
	return N * N * N;
}

EIGEN_DONT_INLINE
static double dpotrf(long N) {
	Eigen::LLT<MatrixXd> lltOfA(A);
	L = lltOfA.matrixL();
	return N * N * N / 3.0 + N * N / 2.0 + N / 6.0;
}

EIGEN_DONT_INLINE
static double dgesvd(long N) {
	Eigen::JacobiSVD<MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
	return 22 * N * N * N;
}

int main(int argc, char** argv) {
#if !defined(NDEBUG) || defined(DEBUG)
	cerr << "Warning: you are running in debug mode - assertions are enabled." << endl;
#endif

	try {
		// program arguments
		string function;
		string range;
		int warm_ups, num_runs, device_id = 0;

		po::options_description desc("Benchmark main options");
		desc.add_options()("help", "produce help message")
			("warm-up-runs", po::value<int>(&warm_ups)->default_value(1), "warm up runs e.g. 1")
			("num-runs", po::value<int>(&num_runs)->default_value(10), "number of runs e.g. 10")
			("function", po::value < string > (&function)->default_value("dgemm"), "Function to test e.g. dgemm, dgeqp3")
			("range", po::value < string > (&range)->default_value("1024:10240:1024"), "N range i.e. start:stop:step")
			("device-id", po::value<int>(&device_id)->default_value(0), "device id e.g. 0");

		po::variables_map vm;
		po::store(po::parse_command_line(argc, argv, desc), vm);
		po::notify(vm);

#if defined(EIGEN_USE_MAGMA_ALL)
		cudaSetDevice(device_id);
		magma_init();
#endif
		if (vm.count("help")) {
			cout << desc << "\n";
			return EXIT_FAILURE;

		} else {
			string temp = range;
			boost::tokenizer<> tok(temp);
			vector<long> range_values;
			for (boost::tokenizer<>::iterator current = tok.begin(); current != tok.end(); ++current) {
				range_values.push_back(boost::lexical_cast<long>(*current));
			}

			if (range_values.size() != 3) {
				throw std::runtime_error("Illegal range input: '" + range + "'");
			}

			workload_type workload;
			if (function == "dgemm") {
				workload = dgemm;
			} else if (function == "dgeqp3") {
				workload = dgeqp3;
			} else if (function == "dgeqrf") {
				workload = dgeqrf;
			} else if (function == "dgemv") {
				workload = dgemv;
			} else if (function == "dtrsm") {
				workload = dtrsm;
			} else if (function == "dpotrf") {
				workload = dpotrf;
			} else if (function == "dgesvd") {
				workload = dgesvd;
			} else {
				throw std::runtime_error("Sorry, the function '" + function + "' is not yet implemented.");
			}

			for (long N = range_values[0]; N <= range_values[1]; N += range_values[2]) {
				// prepare the input data
				A = MatrixXd::Random(N, N);
				B = MatrixXd::Random(N, N);
				b = VectorXd::Random(N);

				// function-specific input data
				if (function == "dtrsm") {
					Aqr = A.colPivHouseholderQr();
				} else if (function == "dpotrf") {
					// make sure A is SPD
					A = A.adjoint() * A;
				}

				real_time_acc = bench_accumulator();
				gflops_acc = bench_accumulator();

				// run the benchmark
				run_benchmark(N, warm_ups, num_runs, workload);

				fprintf(stdout, "%d\t%e\t%e\n", N, mean(real_time_acc), mean(gflops_acc));
				fflush (stdout);
				fprintf(stderr, "%d,%e,%e\n", N, mean(real_time_acc), mean(gflops_acc));
				fflush (stderr);
			}
		}

		// now is safe to clean up
		delete pa;
		delete pb;
		delete pc;

		delete pA;
		delete pB;
		delete pC;
		delete pL;

		delete pAqr;

#if defined(EIGEN_USE_MAGMA_ALL)
		magma_finalize();
#endif
	}
	catch (std::exception& e) {
		cerr << "Exception: " << e.what() << "\n";
		return EXIT_FAILURE;
	}
	catch (...) {
		cerr << "Exception of unknown type!\n";
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_OPERATION_
#define _BOOST_UBLAS_OPERATION_

#include <boost/numeric/ublas/matrix_proxy.hpp>

/** \file operation.hpp
 *  \brief This file contains some specialized products.
 */

// axpy-based products
// Alexei Novakov had a lot of ideas to improve these. Thanks.
// Hendrik Kueck proposed some new kernel. Thanks again.

namespace boost { namespace numeric { namespace ublas {

    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const compressed_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2,
               V &v, row_major_tag) {
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;

        for (size_type i = 0; i < e1.filled1 () -1; ++ i) {
            size_type begin = e1.index1_data () [i];
            size_type end = e1.index1_data () [i + 1];
            value_type t (v (i));
            for (size_type j = begin; j < end; ++ j)
                t += e1.value_data () [j] * e2 () (e1.index2_data () [j]);
            v (i) = t;
        }
        return v;
    }

    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const compressed_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2,
               V &v, column_major_tag) {
        typedef typename V::size_type size_type;

        for (size_type j = 0; j < e1.filled1 () -1; ++ j) {
            size_type begin = e1.index1_data () [j];
            size_type end = e1.index1_data () [j + 1];
            for (size_type i = begin; i < end; ++ i)
                v (e1.index2_data () [i]) += e1.value_data () [i] * e2 () (j);
        }
        return v;
    }

    // Dispatcher
    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const compressed_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2,
               V &v, bool init = true) {
        typedef typename V::value_type value_type;
        typedef typename L1::orientation_category orientation_category;

        if (init)
            v.assign (zero_vector<value_type> (e1.size1 ()));
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_plus_assign> (cv, prod (e1, e2));
#endif
        axpy_prod (e1, e2, v, orientation_category ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }
    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V
    axpy_prod (const compressed_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2) {
        typedef V vector_type;

        vector_type v (e1.size1 ());
        return axpy_prod (e1, e2, v, true);
    }

    template<class V, class T1, class L1, class IA1, class TA1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const coordinate_matrix<T1, L1, 0, IA1, TA1> &e1,
               const vector_expression<E2> &e2,
               V &v, bool init = true) {
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;
        typedef L1 layout_type;

        size_type size1 = e1.size1();
        size_type size2 = e1.size2();

        if (init) {
            noalias(v) = zero_vector<value_type>(size1);
        }

        for (size_type i = 0; i < e1.nnz(); ++i) {
            size_type row_index = layout_type::index_M( e1.index1_data () [i], e1.index2_data () [i] );
            size_type col_index = layout_type::index_m( e1.index1_data () [i], e1.index2_data () [i] );
            v( row_index ) += e1.value_data () [i] * e2 () (col_index);
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag, row_major_tag) {
        typedef const E1 expression1_type;
        typedef typename V::size_type size_type;

        typename expression1_type::const_iterator1 it1 (e1 ().begin1 ());
        typename expression1_type::const_iterator1 it1_end (e1 ().end1 ());
        while (it1 != it1_end) {
            size_type index1 (it1.index1 ());
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression1_type::const_iterator2 it2 (it1.begin ());
            typename expression1_type::const_iterator2 it2_end (it1.end ());
#else
            typename expression1_type::const_iterator2 it2 (boost::numeric::ublas::begin (it1, iterator1_tag ()));
            typename expression1_type::const_iterator2 it2_end (boost::numeric::ublas::end (it1, iterator1_tag ()));
#endif
            while (it2 != it2_end) {
                v (index1) += *it2 * e2 () (it2.index2 ());
                ++ it2;
            }
            ++ it1;
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag, column_major_tag) {
        typedef const E1 expression1_type;
        typedef typename V::size_type size_type;

        typename expression1_type::const_iterator2 it2 (e1 ().begin2 ());
        typename expression1_type::const_iterator2 it2_end (e1 ().end2 ());
        while (it2 != it2_end) {
            size_type index2 (it2.index2 ());
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression1_type::const_iterator1 it1 (it2.begin ());
            typename expression1_type::const_iterator1 it1_end (it2.end ());
#else
            typename expression1_type::const_iterator1 it1 (boost::numeric::ublas::begin (it2, iterator2_tag ()));
            typename expression1_type::const_iterator1 it1_end (boost::numeric::ublas::end (it2, iterator2_tag ()));
#endif
            while (it1 != it1_end) {
                v (it1.index1 ()) += *it1 * e2 () (index2);
                ++ it1;
            }
            ++ it2;
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, sparse_bidirectional_iterator_tag) {
        typedef const E2 expression2_type;

        typename expression2_type::const_iterator it (e2 ().begin ());
        typename expression2_type::const_iterator it_end (e2 ().end ());
        while (it != it_end) {
            v.plus_assign (column (e1 (), it.index ()) * *it);
            ++ it;
        }
        return v;
    }

    // Dispatcher
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag) {
        typedef typename E1::orientation_category orientation_category;
        return axpy_prod (e1, e2, v, packed_random_access_iterator_tag (), orientation_category ());
    }

  /** \brief computes <tt>v += A x</tt> or <tt>v = A x</tt> in an
          optimized fashion.

          \param e1 the matrix expression \c A
          \param e2 the vector expression \c x
          \param v  the result vector \c v
          \param init a boolean parameter

          <tt>axpy_prod(A, x, v, init)</tt> implements the well known
          axpy-product.  Setting \a init to \c true is equivalent to call
          <tt>v.clear()</tt> before <tt>axpy_prod</tt>. Currently \a init
          defaults to \c true, but this may change in the future.

          Up to now there are some specialisation for compressed
          matrices that give a large speed up compared to prod.

          \ingroup blas2

          \internal

          template parameters:
          \param V type of the result vector \c v
          \param E1 type of a matrix expression \c A
          \param E2 type of a vector expression \c x
  */
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2,
               V &v, bool init = true) {
        typedef typename V::value_type value_type;
        typedef typename E2::const_iterator::iterator_category iterator_category;

        if (init)
            v.assign (zero_vector<value_type> (e1 ().size1 ()));
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_plus_assign> (cv, prod (e1, e2));
#endif
        axpy_prod (e1, e2, v, iterator_category ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V
    axpy_prod (const matrix_expression<E1> &e1,
               const vector_expression<E2> &e2) {
        typedef V vector_type;

        vector_type v (e1 ().size1 ());
        return axpy_prod (e1, e2, v, true);
    }

    template<class V, class E1, class T2, class IA2, class TA2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const compressed_matrix<T2, column_major, 0, IA2, TA2> &e2,
               V &v, column_major_tag) {
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;

        for (size_type j = 0; j < e2.filled1 () -1; ++ j) {
            size_type begin = e2.index1_data () [j];
            size_type end = e2.index1_data () [j + 1];
            value_type t (v (j));
            for (size_type i = begin; i < end; ++ i)
                t += e2.value_data () [i] * e1 () (e2.index2_data () [i]);
            v (j) = t;
        }
        return v;
    }

    template<class V, class E1, class T2, class IA2, class TA2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const compressed_matrix<T2, row_major, 0, IA2, TA2> &e2,
               V &v, row_major_tag) {
        typedef typename V::size_type size_type;

        for (size_type i = 0; i < e2.filled1 () -1; ++ i) {
            size_type begin = e2.index1_data () [i];
            size_type end = e2.index1_data () [i + 1];
            for (size_type j = begin; j < end; ++ j)
                v (e2.index2_data () [j]) += e2.value_data () [j] * e1 () (i);
        }
        return v;
    }

    // Dispatcher
    template<class V, class E1, class T2, class L2, class IA2, class TA2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const compressed_matrix<T2, L2, 0, IA2, TA2> &e2,
               V &v, bool init = true) {
        typedef typename V::value_type value_type;
        typedef typename L2::orientation_category orientation_category;

        if (init)
            v.assign (zero_vector<value_type> (e2.size2 ()));
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_plus_assign> (cv, prod (e1, e2));
#endif
        axpy_prod (e1, e2, v, orientation_category ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }
    template<class V, class E1, class T2, class L2, class IA2, class TA2>
    BOOST_UBLAS_INLINE
    V
    axpy_prod (const vector_expression<E1> &e1,
               const compressed_matrix<T2, L2, 0, IA2, TA2> &e2) {
        typedef V vector_type;

        vector_type v (e2.size2 ());
        return axpy_prod (e1, e2, v, true);
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag, column_major_tag) {
        typedef const E2 expression2_type;
        typedef typename V::size_type size_type;

        typename expression2_type::const_iterator2 it2 (e2 ().begin2 ());
        typename expression2_type::const_iterator2 it2_end (e2 ().end2 ());
        while (it2 != it2_end) {
            size_type index2 (it2.index2 ());
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression2_type::const_iterator1 it1 (it2.begin ());
            typename expression2_type::const_iterator1 it1_end (it2.end ());
#else
            typename expression2_type::const_iterator1 it1 (boost::numeric::ublas::begin (it2, iterator2_tag ()));
            typename expression2_type::const_iterator1 it1_end (boost::numeric::ublas::end (it2, iterator2_tag ()));
#endif
            while (it1 != it1_end) {
                v (index2) += *it1 * e1 () (it1.index1 ());
                ++ it1;
            }
            ++ it2;
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag, row_major_tag) {
        typedef const E2 expression2_type;
        typedef typename V::size_type size_type;

        typename expression2_type::const_iterator1 it1 (e2 ().begin1 ());
        typename expression2_type::const_iterator1 it1_end (e2 ().end1 ());
        while (it1 != it1_end) {
            size_type index1 (it1.index1 ());
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression2_type::const_iterator2 it2 (it1.begin ());
            typename expression2_type::const_iterator2 it2_end (it1.end ());
#else
            typename expression2_type::const_iterator2 it2 (boost::numeric::ublas::begin (it1, iterator1_tag ()));
            typename expression2_type::const_iterator2 it2_end (boost::numeric::ublas::end (it1, iterator1_tag ()));
#endif
            while (it2 != it2_end) {
                v (it2.index2 ()) += *it2 * e1 () (index1);
                ++ it2;
            }
            ++ it1;
        }
        return v;
    }

    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, sparse_bidirectional_iterator_tag) {
        typedef const E1 expression1_type;

        typename expression1_type::const_iterator it (e1 ().begin ());
        typename expression1_type::const_iterator it_end (e1 ().end ());
        while (it != it_end) {
            v.plus_assign (*it * row (e2 (), it.index ()));
            ++ it;
        }
        return v;
    }

    // Dispatcher
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, packed_random_access_iterator_tag) {
        typedef typename E2::orientation_category orientation_category;
        return axpy_prod (e1, e2, v, packed_random_access_iterator_tag (), orientation_category ());
    }

  /** \brief computes <tt>v += AT x</tt> or <tt>v = AT x</tt> in an
          optimized fashion.

          \param e1 the vector expression \c x
          \param e2 the matrix expression \c A
          \param v  the result vector \c v
          \param init a boolean parameter

          <tt>axpy_prod(x, A, v, init)</tt> implements the well known
          axpy-product.  Setting \a init to \c true is equivalent to call
          <tt>v.clear()</tt> before <tt>axpy_prod</tt>. Currently \a init
          defaults to \c true, but this may change in the future.

          Up to now there are some specialisation for compressed
          matrices that give a large speed up compared to prod.

          \ingroup blas2

          \internal

          template parameters:
          \param V type of the result vector \c v
          \param E1 type of a vector expression \c x
          \param E2 type of a matrix expression \c A
  */
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V &
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               V &v, bool init = true) {
        typedef typename V::value_type value_type;
        typedef typename E1::const_iterator::iterator_category iterator_category;

        if (init)
            v.assign (zero_vector<value_type> (e2 ().size2 ()));
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_plus_assign> (cv, prod (e1, e2));
#endif
        axpy_prod (e1, e2, v, iterator_category ());
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }
    template<class V, class E1, class E2>
    BOOST_UBLAS_INLINE
    V
    axpy_prod (const vector_expression<E1> &e1,
               const matrix_expression<E2> &e2) {
        typedef V vector_type;

        vector_type v (e2 ().size2 ());
        return axpy_prod (e1, e2, v, true);
    }

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI,
               dense_proxy_tag, row_major_tag) {

        typedef typename M::size_type size_type;

#if BOOST_UBLAS_TYPE_CHECK
        typedef typename M::value_type value_type;
        matrix<value_type, row_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), row_major_tag ());
#endif
        size_type size1 (e1 ().size1 ());
        size_type size2 (e1 ().size2 ());
        for (size_type i = 0; i < size1; ++ i)
            for (size_type j = 0; j < size2; ++ j)
                row (m, i).plus_assign (e1 () (i, j) * row (e2 (), j));
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI,
               sparse_proxy_tag, row_major_tag) {

        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;

#if BOOST_UBLAS_TYPE_CHECK
        typedef typename M::value_type value_type;
        matrix<value_type, row_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), row_major_tag ());
#endif
        typename expression1_type::const_iterator1 it1 (e1 ().begin1 ());
        typename expression1_type::const_iterator1 it1_end (e1 ().end1 ());
        while (it1 != it1_end) {
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression1_type::const_iterator2 it2 (it1.begin ());
            typename expression1_type::const_iterator2 it2_end (it1.end ());
#else
            typename expression1_type::const_iterator2 it2 (boost::numeric::ublas::begin (it1, iterator1_tag ()));
            typename expression1_type::const_iterator2 it2_end (boost::numeric::ublas::end (it1, iterator1_tag ()));
#endif
            while (it2 != it2_end) {
                // row (m, it1.index1 ()).plus_assign (*it2 * row (e2 (), it2.index2 ()));
                matrix_row<expression2_type> mr (e2 (), it2.index2 ());
                typename matrix_row<expression2_type>::const_iterator itr (mr.begin ());
                typename matrix_row<expression2_type>::const_iterator itr_end (mr.end ());
                while (itr != itr_end) {
                    if (triangular_restriction::other (it1.index1 (), itr.index ()))
                        m (it1.index1 (), itr.index ()) += *it2 * *itr;
                    ++ itr;
                }
                ++ it2;
            }
            ++ it1;
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI,
               dense_proxy_tag, column_major_tag) {
        typedef typename M::size_type size_type;

#if BOOST_UBLAS_TYPE_CHECK
        typedef typename M::value_type value_type;
        matrix<value_type, column_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), column_major_tag ());
#endif
        size_type size1 (e2 ().size1 ());
        size_type size2 (e2 ().size2 ());
        for (size_type j = 0; j < size2; ++ j)
            for (size_type i = 0; i < size1; ++ i)
                column (m, j).plus_assign (e2 () (i, j) * column (e1 (), i));
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI,
               sparse_proxy_tag, column_major_tag) {
        typedef TRI triangular_restriction;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;

#if BOOST_UBLAS_TYPE_CHECK
        typedef typename M::value_type value_type;
        matrix<value_type, column_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), column_major_tag ());
#endif
        typename expression2_type::const_iterator2 it2 (e2 ().begin2 ());
        typename expression2_type::const_iterator2 it2_end (e2 ().end2 ());
        while (it2 != it2_end) {
#ifndef BOOST_UBLAS_NO_NESTED_CLASS_RELATION
            typename expression2_type::const_iterator1 it1 (it2.begin ());
            typename expression2_type::const_iterator1 it1_end (it2.end ());
#else
            typename expression2_type::const_iterator1 it1 (boost::numeric::ublas::begin (it2, iterator2_tag ()));
            typename expression2_type::const_iterator1 it1_end (boost::numeric::ublas::end (it2, iterator2_tag ()));
#endif
            while (it1 != it1_end) {
                // column (m, it2.index2 ()).plus_assign (*it1 * column (e1 (), it1.index1 ()));
                matrix_column<expression1_type> mc (e1 (), it1.index1 ());
                typename matrix_column<expression1_type>::const_iterator itc (mc.begin ());
                typename matrix_column<expression1_type>::const_iterator itc_end (mc.end ());
                while (itc != itc_end) {
                    if(triangular_restriction::other (itc.index (), it2.index2 ()))
                       m (itc.index (), it2.index2 ()) += *it1 * *itc;
                    ++ itc;
                }
                ++ it1;
            }
            ++ it2;
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    // Dispatcher
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, TRI, bool init = true) {
        typedef typename M::value_type value_type;
        typedef typename M::storage_category storage_category;
        typedef typename M::orientation_category orientation_category;
        typedef TRI triangular_restriction;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return axpy_prod (e1, e2, m, triangular_restriction (), storage_category (), orientation_category ());
    }
    template<class M, class E1, class E2, class TRI>
    BOOST_UBLAS_INLINE
    M
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               TRI) {
        typedef M matrix_type;
        typedef TRI triangular_restriction;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        return axpy_prod (e1, e2, m, triangular_restriction (), true);
    }

  /** \brief computes <tt>M += A X</tt> or <tt>M = A X</tt> in an
          optimized fashion.

          \param e1 the matrix expression \c A
          \param e2 the matrix expression \c X
          \param m  the result matrix \c M
          \param init a boolean parameter

          <tt>axpy_prod(A, X, M, init)</tt> implements the well known
          axpy-product.  Setting \a init to \c true is equivalent to call
          <tt>M.clear()</tt> before <tt>axpy_prod</tt>. Currently \a init
          defaults to \c true, but this may change in the future.

          Up to now there are no specialisations.

          \ingroup blas3

          \internal

          template parameters:
          \param M type of the result matrix \c M
          \param E1 type of a matrix expression \c A
          \param E2 type of a matrix expression \c X
  */
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2,
               M &m, bool init = true) {
        typedef typename M::value_type value_type;
        typedef typename M::storage_category storage_category;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return axpy_prod (e1, e2, m, full (), storage_category (), orientation_category ());
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    axpy_prod (const matrix_expression<E1> &e1,
               const matrix_expression<E2> &e2) {
        typedef M matrix_type;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        return axpy_prod (e1, e2, m, full (), true);
    }

    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    opb_prod (const matrix_expression<E1> &e1,
              const matrix_expression<E2> &e2,
              M &m,
              dense_proxy_tag, row_major_tag) {
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, row_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), row_major_tag ());
#endif
        size_type size (BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size1 ()));
        for (size_type k = 0; k < size; ++ k) {
            vector<value_type> ce1 (column (e1 (), k));
            vector<value_type> re2 (row (e2 (), k));
            m.plus_assign (outer_prod (ce1, re2));
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    opb_prod (const matrix_expression<E1> &e1,
              const matrix_expression<E2> &e2,
              M &m,
              dense_proxy_tag, column_major_tag) {
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;

#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, column_major> cm (m);
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_plus_assign> (cm, prod (e1, e2), column_major_tag ());
#endif
        size_type size (BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size1 ()));
        for (size_type k = 0; k < size; ++ k) {
            vector<value_type> ce1 (column (e1 (), k));
            vector<value_type> re2 (row (e2 (), k));
            m.plus_assign (outer_prod (ce1, re2));
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    // Dispatcher

  /** \brief computes <tt>M += A X</tt> or <tt>M = A X</tt> in an
          optimized fashion.

          \param e1 the matrix expression \c A
          \param e2 the matrix expression \c X
          \param m  the result matrix \c M
          \param init a boolean parameter

          <tt>opb_prod(A, X, M, init)</tt> implements the well known
          axpy-product. Setting \a init to \c true is equivalent to call
          <tt>M.clear()</tt> before <tt>opb_prod</tt>. Currently \a init
          defaults to \c true, but this may change in the future.

          This function may give a speedup if \c A has less columns than
          rows, because the product is computed as a sum of outer
          products.

          \ingroup blas3

          \internal

          template parameters:
          \param M type of the result matrix \c M
          \param E1 type of a matrix expression \c A
          \param E2 type of a matrix expression \c X
  */
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M &
    opb_prod (const matrix_expression<E1> &e1,
              const matrix_expression<E2> &e2,
              M &m, bool init = true) {
        typedef typename M::value_type value_type;
        typedef typename M::storage_category storage_category;
        typedef typename M::orientation_category orientation_category;

        if (init)
            m.assign (zero_matrix<value_type> (e1 ().size1 (), e2 ().size2 ()));
        return opb_prod (e1, e2, m, storage_category (), orientation_category ());
    }
    template<class M, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    opb_prod (const matrix_expression<E1> &e1,
              const matrix_expression<E2> &e2) {
        typedef M matrix_type;

        matrix_type m (e1 ().size1 (), e2 ().size2 ());
        return opb_prod (e1, e2, m, true);
    }

}}}

#endif


#define BOOST_NO_RTTI
#include <boost/math/tools/roots.hpp>

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <sstream>

constexpr long double Q_E = 1.6021766208e-19;

struct PotLoopBase {
    Float
        phi_t, g_A, psi_a, psi_b, n_i,
        N_A, eps_si, phi_ms, Q_0, g_t, cox;
    std::vector<Float> psi_t{}, N_t{};

    unsigned tol_bits{31}; //result will be exact to 2^(1-tol_bits)
    unsigned long max_iter{2000};

    inline Float exp_phi_t(Float x) const {
        return std::exp(x / phi_t);
    }

    inline Float fs_ea(Float psi_s, Float v_ch) const {
        return 1./(1. + g_A * exp_phi_t(psi_a - psi_s + v_ch));
    }

    inline Float fb_ea() const {
        return 1./(1. + g_A * exp_phi_t(psi_a - psi_b));
    }

    inline Float fs_Et(Float psi_t, Float psi_s, Float v_ch) const {
        return 1. / (1. + g_t * exp_phi_t(+psi_t - psi_s + v_ch));
    }

    Float Q_it(Float psi_s, Float v_ch) const {
        Float ret = 0;

        for(size_t i=0; i < psi_t.size(); ++i) {
            if(g_t == 0)
                ret += (-Q_E) * N_t[i];
            else
                ret += (-Q_E) * N_t[i] * fs_Et(psi_t[i], psi_s, v_ch);
        }
        return ret;
    }

    Float v_fb(Float psi_s, Float v_ch) const {
        return phi_ms + (Q_0 - Q_it(psi_s, v_ch)) / cox;
    }
};

struct PotLoop : public PotLoopBase {
    inline Float Es_x(Float psi_s, Float v_ch, Float fb_ea_) const {
        auto fs_ea_ = fs_ea(psi_s, v_ch);

        auto fac1 = 2. * Q_E / eps_si;
        auto fac2 = exp_phi_t(psi_s - v_ch) + exp_phi_t(-psi_s) - exp_phi_t(psi_b - v_ch) - exp_phi_t(-psi_b);
        auto fac3 = psi_s - psi_b - phi_t * std::log(fs_ea_ / fb_ea_);
        auto es2 = n_i * phi_t * fac1 * fac2 + fac1 * N_A * fac3;
        if(psi_s >= psi_b)
            return std::sqrt(es2);
        else
            return -std::sqrt(es2);
    }

    Float Es(Float psi_s, Float v_ch) const {
        auto fb_ea_ = fb_ea();
        return Es_x(psi_s, v_ch, fb_ea_);
    }

    std::pair<Float, Float> psi_s_x(Float v_ch, Float v_gb, boost::uintmax_t& iter, Float start=1.0) const {
        using boost::math::tools::bracket_and_solve_root;
        using boost::math::tools::eps_tolerance;

        const Float fb_ea_ = fb_ea();  //precalculate

        auto rootfun = [=](Float psi_s) {
            psi_s -= 1.;
            return v_fb(psi_s, v_ch) + eps_si * Es_x(psi_s, v_ch, fb_ea_) / cox + psi_s - psi_b - v_gb;
        };

        //TOMS748 instead?
        return bracket_and_solve_root(rootfun, start, 1.2, true, eps_tolerance<Float>(tol_bits), iter);
    }
};

// fermi-dirac solution
struct PotLoopFD : public PotLoopBase {
    // TODO: are these really necessary?
    Float E_i, E_v, E_c, N_c, N_v;

    inline Float fdint(Float e) const {
        // return fdk(k=0.5, phi=E / (k * T))
        static fdint_method<> fd1h("fd1h");
        return fd1h(e / (Q_E * phi_t));
    }

    inline Float Es(Float psi_s, Float v_ch) const {
        using boost::math::quadrature::gauss_kronrod;
        gauss_kronrod<double, 15> integrator;

        auto fac = 2. * Q_E / eps_si;

        const auto S2PI = 2. * boost::math::constants::one_div_root_pi<Float>();

        auto int_fun = [&](Float psi) {
            auto n_fd = N_c * S2PI * fdint(Q_E * (psi - v_ch) + E_i - E_c);
            auto p_fd = N_v * S2PI * fdint(E_v - Q_E * psi - E_i);
            auto na_min = N_A / (1. + g_A * exp_phi_t(psi_a - psi + v_ch));
            return n_fd - p_fd + na_min;
        };

        Float error, ret;
        try {
            if(psi_s >= psi_b)
                ret = sqrt(fac * integrator.integrate(int_fun, psi_b, psi_s, 5, 1e-18, &error));
            else
                ret = -sqrt(-fac * integrator.integrate(int_fun, psi_s, psi_b, 5, 1e-18, &error));
        } catch(std::domain_error const& ex) {
            std::stringstream msg;
            msg << ex.what() << " psi_s = " << psi_s;
            throw std::domain_error(msg.str());
        }

        return ret;
    }

    std::pair<Float, Float> psi_s_x(Float v_ch, Float v_gb, boost::uintmax_t& iter, Float start=1.0) const {
        using boost::math::tools::bracket_and_solve_root;
        using boost::math::tools::eps_tolerance;

        auto rootfun = [=](Float psi_s) {
            psi_s -= 1.;
            return v_fb(psi_s, v_ch) + eps_si * Es(psi_s, v_ch) / cox + psi_s - psi_b - v_gb;
        };

        //TOMS748 instead?
        return bracket_and_solve_root(rootfun, start, 1.2, true, eps_tolerance<Float>(tol_bits), iter);
    }
};

struct PotLoopGildenblat : public PotLoopBase {
    Float lam_bulk, bulk_n, bulk_p;

    inline Float Es(Float psi_s, Float v_ch) const {
        auto k_0 = std::exp(-v_ch / phi_t);
        auto phi_s = psi_s - psi_b;
        auto u = phi_s / phi_t;
        auto g_fun = 1. / lam_bulk * std::log(1. + lam_bulk * (std::exp(u) - 1));
        auto h2 = std::exp(-u) - 1 + g_fun + bulk_n / bulk_p * k_0 * (std::exp(u) - 1. - g_fun);

        auto es2 = 2 * Q_E * bulk_p * phi_t / eps_si * h2;

        if(psi_s >= psi_b)
            return std::sqrt(es2);
        else
            return -std::sqrt(es2);
    }

    std::pair<Float, Float> psi_s_x(Float v_ch, Float v_gb, boost::uintmax_t& iter, Float start=1.0) const {
        using boost::math::tools::bracket_and_solve_root;
        using boost::math::tools::eps_tolerance;

        const Float fb_ea_ = fb_ea();  //precalculate

        auto rootfun = [=](Float psi_s) {
            psi_s -= 1.;
            return v_fb(psi_s, v_ch) + eps_si * Es(psi_s, v_ch) / cox + psi_s - psi_b - v_gb;
        };

        //TOMS748 instead?
        return bracket_and_solve_root(rootfun, start, 1.2, true, eps_tolerance<Float>(tol_bits), iter);
    }
};

template<class L>
Float psi_s(L& l, Float v_ch, Float v_gb) {
    static Float last_root = 1.0;

    boost::uintmax_t iter = l.max_iter;
    auto root = l.psi_s_x(v_ch, v_gb, iter, last_root);

    if(iter == l.max_iter)
        throw std::runtime_error("no solution found in max_iter iterations");

    if(root.first + root.second < 1e-6)
        throw std::runtime_error("ran into psi_s < -1.0");

    if(!isfinite(root.first) || !isfinite(root.second))
        throw std::runtime_error("no solution found (nan or inf)");

    last_root = (root.first + root.second) / 2;
    return last_root - 1.;
}

template<class L>
py::tuple psi_s2(const L& l, Float v_ch, Float v_gb) {
    boost::uintmax_t iter = l.max_iter;
    auto root = l.psi_s_x(v_ch, v_gb, iter);
    return py::make_tuple(root.first, root.second, iter);
}


// standard includes and external libraries
#include <iostream>
#include <random>
#include <Eigen/Dense>
#include <cmath>
#include <fstream>
#include <nlohmann/json.hpp>
#include <ctime>
// project includes
#include "abstractclasses.h"
#include "bandit.h"
#include "oful.h"
#include "leader.h"
#include "regbalancing.h"
#include "adversarial_master.h"
#include "finitelinrep.h"
#include "utils.h"
#include "gzip.h"

using json = nlohmann::json;
using namespace std;
using namespace Eigen;

size_t PREC = 4;   // for saving numbers are rounded to PREC decimals
size_t EVERY = 1;  // save EVERY round

int main()
{
    std::time_t t = std::time(nullptr);
    char MY_TIME[100];
    std::strftime(MY_TIME, sizeof(MY_TIME), "%Y%m%d%H%M%S", std::localtime(&t));
    std::cout << MY_TIME << '\n';

    typedef std::vector<std::vector<double>> vec2double;

    int seed = time(NULL);
    // seed= 1611835201;
    seed=1611836268;
    srand (seed);
    cout << "seed: " << seed << endl;
    // rng.seed(10000); // warm it up
    int n_runs = 5, T = 10000;
    double delta = 0.01;
    double reg_val = 1.;
    double noise_std = 0.3;
    double bonus_scale = 1.;
    bool adaptive_ci = true;
    //double cor_gamma = 1./T, cor_beta = exp(1./log(T)), cor_lr0 = 20. / sqrt(T);

    std::vector<long> seeds(n_runs);
    std::generate(seeds.begin(), seeds.end(), [] ()
    {
        return rand();
    });

    // FiniteLinearRepresentation rep = make_random(20, 5, 6, true, noise_std, seed);
    // rep.save("linrep.json"); // save current model

    FiniteLinearRepresentation rep = flr_loadjson("linrep3.json", noise_std, seed);
    int dim = rep.features_dim();
    cout << "Dimension: " << dim << endl;

    std::vector<FiniteLinearRepresentation> reps;
    double MMM = rep.features_bound();
    for(int i = 1; i < dim; ++i)
    {
        FiniteLinearRepresentation rr = derank_hls(rep, i, false, true, true);
        // cout << i << ": " << rr.features_bound() << ", " << rr.param_bound() << std::endl;
        rr.normalize_features(MMM);
        // cout << i << ": " << rr.features_bound() << ", " << rr.param_bound() << std::endl;
        reps.push_back(rr);
        bool flag = rr.is_equal(rep);
        if (!flag) {
            std::cout << "Error: non realizable representation" << std::endl;
            exit(1);
        }
    }
    assert(rep.is_equal(rep));
    reps.push_back(rep);
    // cout << dim << ": " << rep.features_bound() << ", " << rep.param_bound() << std::endl;
    assert(reps.size() == dim);

    // //LEADER
    // vec2double regrets(n_runs), pseudo_regrets(n_runs);
    // #pragma omp parallel for
    // for (int i = 0; i < n_runs; ++i)
    // {
    //     std::vector<std::shared_ptr<ContRepresentation<int>>> lreps;
    //     for(auto& ll : reps)
    //     {
    //         auto tmp = std::make_shared<FiniteLinearRepresentation>(ll.copy(seeds[i]));
    //         lreps.push_back(tmp);
    //     }
    //     LEADER<int> localg(lreps, reg_val, noise_std, bonus_scale, delta/lreps.size(), adaptive_ci);
    //     ContBanditProblem<int> prb(*lreps[0], localg);
    //     prb.reset();
    //     prb.run(T);
    //     regrets[i] = prb.instant_regret;
    //     pseudo_regrets[i] = prb.exp_instant_regret;
    //     // delete localg;
    // }
    // // save_vector_csv(regrets, "LEADER_regrets.csv", EVERY, PREC);
    // save_vector_csv_gzip(regrets, "LEADER_regrets.csv.gz", EVERY, PREC);
    // // save_vector_csv(pseudo_regrets, "LEADER_pseudoregrets.csv", EVERY, PREC);
    // save_vector_csv_gzip(pseudo_regrets, "LEADER_pseudoregrets.csv.gz", EVERY, PREC);

    //just OFUL
    for(int j = 0; j < reps.size(); ++j)
    {
        vec2double regrets(n_runs), pseudo_regrets(n_runs);

        #pragma omp parallel for
        for (int i = 0; i < n_runs; ++i)
        {
            FiniteLinearRepresentation lrep = reps[j].copy(seeds[i]);
            OFUL<int> localg(lrep, reg_val, noise_std, bonus_scale, delta, adaptive_ci);
            ContBanditProblem<int> prb(lrep, localg);
            prb.reset();
            prb.run(T);
            regrets[i] = prb.instant_regret;
            pseudo_regrets[i] = prb.exp_instant_regret;
        }
        // save_vector_csv(regrets, "OFUL-rep"+std::to_string(j)+"_regrets.csv", EVERY, PREC);
        save_vector_csv_gzip(regrets, "OFUL-rep"+std::to_string(j)+"_regrets.csv.gz", EVERY, PREC);
        // save_vector_csv(pseudo_regrets, "OFUL-rep"+std::to_string(j)+"_pseudoregrets.csv", EVERY, PREC);
        save_vector_csv_gzip(pseudo_regrets, "OFUL-rep"+std::to_string(j)+"_pseudoregrets.csv.gz", EVERY, PREC);

#if 0
        #pragma omp parallel for
        for (int i = 0; i < n_runs; ++i)
        {
            std::vector<FiniteLinearRepresentation> llr{reps[j].copy(seeds[i])};
            LEADER ddd(llr, reg_val, 1, delta/llr.size(), adaptive_ci);
            LinBanditProblem prb(llr[0], ddd);
            prb.run(T);
            regrets[i] = prb.instant_regret;
            pseudo_regrets[i] = prb.exp_instant_regret;
        }
        save_vector_csv(regrets, "LEADER-rep"+std::to_string(j)+"_regrets.txt");
        save_vector_csv(pseudo_regrets, "LEADER-rep"+std::to_string(j)+"_pseudoregrets.txt");
#endif
    }
    return 0;
}


/*
*  ERD2.cpp
*
*      Author: gstoszek
*/

#include "ERD2.h"
#include "Centrality.h"
#include "../algebraic/CSRMatrix.h"
#include "../numerics/LAMG/Lamg.h"
#include "../auxiliary/Log.h"
#include <chrono>
#include <stdlib.h>
#include <cmath>
#include <armadillo>

namespace NetworKit {

  ERD2::ERD2(const Graph &G): Centrality(G, true) {
        n=G.upperNodeIdBound();
        /* EffectiveResistanceDistance-Matrix*/
        ERD.resize(n);
        /*Laplacian*/
        L.set_size(n,n);
        L.zeros();
        /* */
        vList.resize(n);
        vAdj_List.resize(n);

        G.forNodes([&](node v){
            ERD[v].resize(n);
            ERD[v][v]=0.;
            vAdj_List[v].resize(0);
            vList[v]=v;
            G.forNodes([&](node w){
              if(G.hasEdge(v,w)){
                /*instead of 1 one should use a function for weighted edges*/
                L(v,w)=-1.;
                L(v,v)+=1.;
                vAdj_List[v].push_back(w);
              }
            });
            nEdges+=L(v,v);
        });
      }
    /**************************************************************************/
    std::vector<std::vector<double>> ERD2::getERDMatrix() {
        return ERD;
    }
    /**************************************************************************/
    void ERD2::run(){
      /**Set LEVEL here**/
      upperLevelIdBound =1;
      count current_Level;
      current_Level=0;
      coarsed_List.resize(0);
      auto start = std::chrono::high_resolution_clock::now();
      while(current_Level<upperLevelIdBound){
        coarse_L(current_Level);
        current_Level+=1;
      }
      auto end = std::chrono::high_resolution_clock::now();
      std::chrono::duration<double> diff = end-start;
      std::cout<< "Coarsening finished in: " << diff.count() << "(s)" << "\n";
      /*invert*/
      start = std::chrono::high_resolution_clock::now();
      L=arma::pinv(L, 0.01);
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout<< "Pinv() calculated in: " << diff.count() << "(s)" << "\n";
      /*calculate initial ERD Matrix*/
      for(count i=0;i<vList.size();i++){
        for(count j=i+1;j<vList.size();j++){
          ERD[vList[i]][vList[j]]=L(i,i)+L(j,j)-2.*L(i,j);
          ERD[vList[j]][vList[i]]=ERD[vList[i]][vList[j]];
        }
      }
      for(count i=0;i<coarsed_List.size();i++){
        for(count j=0;j<ERD.size();j++){
          ERD[coarsed_List[i].first][j]=ERD[vAdj_List[coarsed_List[i].first][0]][j]+1.;
          ERD[j][coarsed_List[i].first]=ERD[coarsed_List[i].first][j];
        }
      }
    }
    /***************************************************************************/
    void uncoarse_L(){

    }

    /***************************************************************************/
    void ERD2::coarse_L(count current_Level){
      std::vector<node> coarse_List;
      coarse_List.resize(0);
      for(count i=0;i<L.n_rows;i++){
        if(L(i,i)==current_Level+1){
          coarse_List.push_back(i);
        }
      }
      for(count i=0;i<coarse_List.size();i++){
        for(count j=1;j<vAdj_List[coarse_List[i]].size();j++){
          if(L(vAdj_List[coarse_List[i]][0],vAdj_List[coarse_List[i]][j])!=-1){
            L(vAdj_List[coarse_List[i]][0],vAdj_List[coarse_List[i]][j])=-1.;
            L(vAdj_List[coarse_List[i]][j],vAdj_List[coarse_List[i]][0])=-1.;
            tM[vList[i]][vAdj_List[coarse_List[i]][j]];
            tM[vAdj_List[coarse_List[i]][j]][vList[i]];
            /*Diagonal*/
            L(vAdj_List[coarse_List[i]][0],vAdj_List[coarse_List[i]][0])+=1.;
          }
        }
        L(vAdj_List[coarse_List[i]][0],vAdj_List[coarse_List[i]][0])-=1.;
        coarsed_List.push_back(std::make_pair(vList[coarse_List[i]],current_Level));
      }
      arma::uvec indices(vList.size()-coarse_List.size());
      count k;
      count j;
      k=0;
      j=0;
      for(count i=0;i<vList.size();i++){
        if(coarse_List[k]==i){
          k++;
        }
        else{
          indices(j)=i;
          j++;
        }
      }
      for(count i=0;i<coarse_List.size();i++){
        vList.erase(vList.begin()+coarse_List[i]-i);
      }
      L=L.submat(indices, indices);
    }
    /**************************************************************************/
    /*
    void ERD2::sort_Level_List(std::vector<node> vLevel_List,count LevelId){
      std::vector<std::vector<node>> bucket_List;
      bucket_List.resize(upperLevelIdBound)
      bucket_List.resize(0);
      count k,
      k=0;
      while(k!=vLevel_List.size()){
        if(nAdj_List[vLevel_List[k]].second<upperNodeIdBound){
          bucket_List[nAdj_List[vLevel_List[k]].second-1].push_back vLevel_List[k];
        }
        else{
          bucket_List[upperNodeIdBound].push_back vLevel_List[k];
        }
        k++;
      }
      for(count i=0;i<bucket_List.size();i++){
        for(count j=0;j<bucket_List[i].size();j++){
          vLevel_List=bucket_List[i][j];
        }
      }
    }
    */
    /**************************************************************************/
    /*
    void ERD2::coarse(count samplesize, count LevelID){
      if(LevelID==3){
      }
      else{
        std::pair<node,node> coarsed_edge;
        std::vector<std::pair<bool,double>> Absorber_Edge_List;
        std::vector<std::pair<bool,double>> Submitter_Edge_List;
        Absorber_Edge_List.resize(Adj.size());
        Submitter_Edge_List.resize(Adj.size());
        coarsed_edge=select_edge();
        //coarsed_edge=random_edge();
        Absorber_Edge_List=Adj[coarsed_edge.first];
        Submitter_Edge_List=Adj[coarsed_edge.second];
        coarsening(coarsed_edge);
        coarse(1);
        uncoarsening(Absorber_Edge_List,Submitter_Edge_List,coarsed_edge);
      }
    }
    */
    /**************************************************************************/
    /*
    std::pair<node,node> ERD2::select_edge(){
      return make_pair(Matching_List[vLevel_List[0]].second,vLevel_List[0])
    }
    */
    /**************************************************************************/
    /*
    std::pair<node,node> ERD2::random_edge(){
      count Random_Edge;
      count i;
      count a;
      count b;

      a=1;
      b=nAdj_List.size()-1;

      i=a+(b-a)*0.5;
      Random_Edge= (std::rand() % nEdges)+1;
      while(!((Random_Edge>nAdj_List[i-1].second)&&(Random_Edge<=nAdj_List[i].second))){
        if(Random_Edge<=nAdj_List[i-1].second){
          b=i;
        }
        else{
          a=i;
        }
        i=a+(b-a)*0.5;
      }
      count k=0;
      count j=0;
      while(k<Random_Edge-nAdj_List[i-1].second){
        if(Adj[nAdj_List[i].first][j].first){
            k++;
        }
        j++;
      }
      j--;
      if(j>=G.upperNodeIdBound()){
        std::cout<<"<"<<nAdj_List[i].first<<","<<j<<"> mit "<<Random_Edge<<"\n";
        std::cout<<"No. of Edges="<<nEdges<<"\n";
        std::cout<<"i-1="<<i+1<<"-->"<<"v="<<nAdj_List[i-1].first<<" No.="<< nAdj_List[i-1].second<<"\n";
        std::cout<<"i="<<i<<"-->"<<"v="<<nAdj_List[i].first<<" No.="<< nAdj_List[i].second<<"\n";
        std::cout<<"i+1="<<i+1<<"-->"<<"v="<<nAdj_List[i+1].first<<" No.="<< nAdj_List[i+1].second<<"\n";
        count p;
        for(count k=0;k<Adj.size();k++){
          if(Adj[nAdj_List[i].first][k].first){
            p++;
          }
        }
        std::cout<<"p="<<p<<"\n";
      }
      return std::make_pair(nAdj_List[i].first,j);
    }
    */
    /**************************************************************************/
    /*
    void ERD2::coarsening_2(std::pair<node,node> coarsed_edge){
      std::cout << "coarsening! <a,s>=<" << coarsed_edge.first << "," << coarsed_edge.second <<">"<<"\n";
      vLevel_List.erase (vLevel_List.begin());
      Adj[coarsed_edge.first][coarsed_edge.second].first=false;
      Adj[coarsed_edge.second][coarsed_edge.first].first=false;
    }
    */
    /**************************************************************************/
    /**************************************************************************/
    /*void ERD2::coarsening(std::pair<node,node> coarsed_edge){
      count x;
      count y;
      std::vector<int> transfered_Edges;
      transfered_Edges.resize(Adj.size()+1);
      transfered_Edges[0]=0;
      for(count i=1;i<nAdj_List.size();i++){
        x=nAdj_List[i].first;
        if(Adj[coarsed_edge.second][x].first==true){
          if(Adj[coarsed_edge.first][x].first==false){
            Adj[coarsed_edge.first][x]=std::make_pair(true,Adj[coarsed_edge.second][x].second);
            Adj[x][coarsed_edge.first]=std::make_pair(true,Adj[coarsed_edge.second][x].second);
            transfered_Edges[x+1]++;
            transfered_Edges[coarsed_edge.first+1]++;
          }
          Adj[coarsed_edge.second][x]=std::make_pair(false,0.);
          Adj[x][coarsed_edge.second]=std::make_pair(false,0.);
          transfered_Edges[x+1]--;
          transfered_Edges[coarsed_edge.second+1]--;
        }
      }
      Adj[coarsed_edge.first][coarsed_edge.first]=std::make_pair(false,0);
      transfered_Edges[coarsed_edge.first+1]-=2;
      y=1;
      nAdj_List[1].second+=transfered_Edges[nAdj_List[1].first+1];
      for(count i=2;i<nAdj_List.size();i++){
          transfered_Edges[nAdj_List[i].first+1]+=transfered_Edges[nAdj_List[i-1].first+1];
          nAdj_List[i].second+=transfered_Edges[nAdj_List[i].first+1];
          if(nAdj_List[i].first==coarsed_edge.second){
            y=i;
          }
      }
      nAdj_List.erase(nAdj_List.begin()+y);
      nEdges=nAdj_List[nAdj_List.size()-1].second;
    }
    */
    /**************************************************************************/
    /*
    void ERD2::uncoarsening(std::vector<std::pair<bool,double>> Absorber_Edge_List,
      std::vector<std::pair<bool,double>> Submitter_Edge_List,
      std::pair<node,node> coarsed_edge){
        count x;

        first_join(coarsed_edge);
        nAdj_List.push_back(std::make_pair(coarsed_edge.second,1));
        for(count i=1; i<nAdj_List.size();i++){
          x=nAdj_List[i].first;
          if((Submitter_Edge_List[x].first) && (x!=coarsed_edge.first)){
            if(Absorber_Edge_List[x].first){
              edge_fire(std::make_pair(x,coarsed_edge.second));
            }
            else{
              ERD[coarsed_edge.second][x]=ERD[coarsed_edge.first]
            }
          }
          if((Adj[coarsed_edge.first][x].first)&&(Absorber_Edge_List[x].first!=true)){
            non_bridge_delete(std::make_pair(x,coarsed_edge.first));
          }
        }
      }
      */
    /**************************************************************************/
    /*
    void ERD2::first_join(std::pair<node,node> coarsed_edge){
      for(count i=1;i<nAdj_List.size();i++){
        ERD[nAdj_List[i].first][coarsed_edge.second]=ERD[nAdj_List[i].first][coarsed_edge.first]+1.;
        ERD[coarsed_edge.second][nAdj_List[i].first]=ERD[nAdj_List[i].first][coarsed_edge.second];
      }
      Adj[coarsed_edge.first][coarsed_edge.second]=std::make_pair(true,1.);
      Adj[coarsed_edge.second][coarsed_edge.first]=std::make_pair(true,1.);
    }
    */
    /**************************************************************************/
    /*
    void ERD2::edge_fire(std::pair<node,node> coarsed_edge){
      std::vector<std::vector<double>> ERD2;
      ERD2.resize(ERD.size());
      G.forNodes([&](node v){
        ERD2[v].resize(ERD.size());
      });
      count x;
      count y;

      for(count i=1;i<nAdj_List.size();i++){
        x=nAdj_List[i].first;
        for(count j=i+1;j<nAdj_List.size();j++){
          y=nAdj_List[j].first;
          ERD2[x][y]=ERD[x][coarsed_edge.second]-ERD[x][coarsed_edge.first];
          ERD2[x][y]-=ERD[coarsed_edge.second][y]-ERD[coarsed_edge.first][y];
          ERD2[x][y]*=ERD2[x][y];
          ERD2[x][y]/=4.*(1.+ERD[coarsed_edge.first][coarsed_edge.second]);
          ERD2[x][y]=ERD[x][y]-ERD2[x][y];
          ERD2[y][x]=ERD2[x][y];
        }
      }
      ERD=ERD2;
      Adj[coarsed_edge.first][coarsed_edge.second]=std::make_pair(true,1.);
      Adj[coarsed_edge.second][coarsed_edge.first]=std::make_pair(true,1.);
    }
    */
    /**************************************************************************/
    /*
    void ERD2::non_bridge_delete(std::pair<node,node> coarsed_edge){
      std::vector<std::vector<double>> ERD2;
      ERD2.resize(ERD.size());
      G.forNodes([&](node v){
        ERD2[v].resize(ERD.size());
      });
      count x;
      count y;

      for(count i=1;i<nAdj_List.size();i++){
        x=nAdj_List[i].first;
        for(count j=i+1;j<nAdj_List.size();j++){
          y=nAdj_List[j].first;
          ERD2[x][y]=ERD[x][coarsed_edge.second]-ERD[x][coarsed_edge.first];
          ERD2[x][y]-=ERD[coarsed_edge.second][y]-ERD[coarsed_edge.first][y];
          ERD2[x][y]*=ERD2[x][y];
          ERD2[x][y]/=4.*(1.-ERD[coarsed_edge.first][coarsed_edge.second]);
          ERD2[x][y]=ERD[x][y]+ERD2[x][y];
          ERD2[y][x]=ERD2[x][y];
        }
      }
      ERD=ERD2;
      Adj[coarsed_edge.first][coarsed_edge.second]=std::make_pair(false,0.);
      Adj[coarsed_edge.second][coarsed_edge.first]=std::make_pair(false,0.);
    }
    */
    /**************************************************************************/
} /* namespace NetworKit*/


// Copyright (c) 2016 Matt Overby
//
// MPM-OPTIMIZATION Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
// conditions and the following disclaimer.
// 2. 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.
// THIS SOFTWARE IS PROVIDED "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 UNIVERSITY OF MINNESOTA, DULUTH 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.

#ifndef INTERP_HPP
#define INTERP_HPP 1

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

namespace mpm
{

	// Cubic B-spline
	static inline double cspline(double x)
	{
		if (x==0.0) { x=1e-12; }
		x = fabs(x);
		if (x < 1.0) { return x*x*(x*0.5 - 1.0) + 2.0/3.0; }
		else if (x < 2.0) { return x*(x*(-x/6.0 + 1.0) - 2.0) + 4.0/3.0; }
		return 0.0;
	}

	// Slope of cubic spline
	static inline double d_cspline(double x)
	{
		if (x==0.0) { x=1e-12; }
		double abs_x = fabs(x);
		if (abs_x < 1.0) { return 1.5*x*abs_x - 2.0*x; }
		else if (x < 2.0) { return -x*abs_x*0.5 + 2.0*x - 2.0*x/abs_x; }
		return 0.0;
	}

	// Quadratic spline
	static inline double qspline( double x )
	{
		double fx = fabs(x);
		if (fx < 0.5) { return ( 0.75 - x*x ); }
		else if (fx < 1.5) { return ( 0.5*x*x - 1.5*fx + 9.0/8.0 ); }
		return 0.0;
	}

	// Slope of quadratic spline
	static inline double d_qspline( double x )
	{
		double fx = fabs(x);
		if (fx < 0.5) { return ( -2.0 * x ); }
		else if (fx < 1.5)
		{
			if (x < 0.0) { return fabs( fx - 1.5 ); }
			else { return (fx - 1.5); }
		}
		return 0.0;
	}

	static inline bool isreal(double n)
	{
		return !std::isnan(n) && !std::isinf(n);
	}

} // end namespace mpm

#endif


#pragma once

#include <cmath>
#include <map>

#include <Eigen/SparseLU>

#include "OpenABF/Exceptions.hpp"
#include "OpenABF/HalfEdgeMesh.hpp"
#include "OpenABF/Math.hpp"

namespace OpenABF
{

/**
 * @brief Compute parameterized mesh using Angle-based LSCM
 *
 * Computes a least-squares conformal parameterization of a mesh. Unlike the
 * original LSCM algorithm, this class ignores the 3D vertex positions and
 * instead uses the angle associated with the mesh's edge trait
 * (MeshType::EdgeTraits::alpha) to calculate the initial per-triangle edge
 * lengths. Without previously modifying the angles of the provided mesh, this
 * class produces the same result as a vertex-based LSCM implementation.
 * However, by first processing the mesh with a parameterized angle optimizer,
 * such as ABFPlusPlus, the parameterization can be improved, sometimes
 * significantly.
 *
 * Implements the angle-based variant of "Least squares conformal maps for
 * automatic texture atlas generation" by Lévy _et al._ (2002)
 * \cite levy2002lscm.
 *
 * @tparam T Floating-point type
 * @tparam MeshType HalfEdgeMesh type which implements the default mesh traits
 * @tparam Solver A solver implementing the
 * [Eigen Sparse solver
 * concept](https://eigen.tuxfamily.org/dox-devel/group__TopicSparseSystems.html)
 * and templated on Eigen::SparseMatrix<T>
 */
template <
    typename T,
    class MeshType = HalfEdgeMesh<T>,
    class Solver =
        Eigen::SparseLU<Eigen::SparseMatrix<T>, Eigen::COLAMDOrdering<int>>,
    std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
class AngleBasedLSCM
{
public:
    /** @brief Mesh type alias */
    using Mesh = MeshType;

    /** @copydoc AngleBasedLSCM::Compute */
    void compute(typename Mesh::Pointer& mesh) const { Compute(mesh); }

    /**
     * @brief Compute the parameterized mesh
     *
     * @throws MeshException If pinned vertex is not on boundary.
     * @throws SolverException If matrix cannot be decomposed or if solver fails
     * to find a solution.
     */
    static void Compute(typename Mesh::Pointer& mesh)
    {
        using Triplet = Eigen::Triplet<T>;
        using SparseMatrix = Eigen::SparseMatrix<T>;
        using DenseMatrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

        // Pinned vertex selection
        // Get the end points of a boundary edge
        auto p0 = mesh->vertices_boundary()[0];
        auto e = p0->edge;
        do {
            if (not e->pair) {
                break;
            }
            e = e->pair->next;
        } while (e != p0->edge);
        if (e == p0->edge and e->pair) {
            throw MeshException("Pinned vertex not on boundary");
        }
        auto p1 = e->next->vertex;

        // Map selected edge to closest XY axis
        // Use sign to select direction
        auto pinVec = p1->pos - p0->pos;
        auto dist = norm(pinVec);
        pinVec /= dist;
        p0->pos = {T(0), T(0), T(0)};
        auto maxElem = std::max_element(pinVec.begin(), pinVec.end());
        auto maxAxis = std::distance(pinVec.begin(), maxElem);
        dist = std::copysign(dist, *maxElem);
        if (maxAxis == 0) {
            p1->pos = {dist, T(0), T(0)};
        } else {
            p1->pos = {T(0), dist, T(0)};
        }

        // For convenience
        auto numFaces = mesh->num_faces();
        auto numVerts = mesh->num_vertices();
        auto numFixed = 2;
        auto numFree = numVerts - numFixed;

        // Permutation for free vertices
        // This helps us find a vert's row in the solution matrix
        std::map<std::size_t, std::size_t> freeIdxTable;
        for (const auto& v : mesh->vertices()) {
            if (v == p0 or v == p1) {
                continue;
            }
            auto newIdx = freeIdxTable.size();
            freeIdxTable[v->idx] = newIdx;
        }

        // Setup pinned bFixed
        std::vector<Triplet> tripletsB;
        tripletsB.emplace_back(0, 0, p0->pos[0]);
        tripletsB.emplace_back(1, 0, p0->pos[1]);
        tripletsB.emplace_back(2, 0, p1->pos[0]);
        tripletsB.emplace_back(3, 0, p1->pos[1]);
        SparseMatrix bFixed(2 * numFixed, 1);
        bFixed.reserve(tripletsB.size());
        bFixed.setFromTriplets(tripletsB.begin(), tripletsB.end());

        // Setup variables matrix
        // Are only solving for free vertices, so push pins in special matrix
        std::vector<Triplet> tripletsA;
        tripletsB.clear();
        for (const auto& f : mesh->faces()) {
            auto e0 = f->head;
            auto e1 = e0->next;
            auto e2 = e1->next;
            auto sin0 = std::sin(e0->alpha);
            auto sin1 = std::sin(e1->alpha);
            auto sin2 = std::sin(e2->alpha);

            // Find the max sin idx
            std::vector<T> sins{sin0, sin1, sin2};
            auto sinMaxElem = std::max_element(sins.begin(), sins.end());
            auto sinMaxIdx = std::distance(sins.begin(), sinMaxElem);

            // Rotate the edge order of the face so last angle is largest
            if (sinMaxIdx == 0) {
                auto temp = e0;
                e0 = e1;
                e1 = e2;
                e2 = temp;
                sin0 = sins[1];
                sin1 = sins[2];
                sin2 = sins[0];
            } else if (sinMaxIdx == 1) {
                auto temp = e2;
                e2 = e1;
                e1 = e0;
                e0 = temp;
                sin0 = sins[2];
                sin1 = sins[0];
                sin2 = sins[1];
            }

            auto ratio = (sin2 == T(0)) ? T(1) : sin1 / sin2;
            auto cosine = std::cos(e0->alpha) * ratio;
            auto sine = sin0 * ratio;

            // If pin0 or pin1, put in fixedB matrix, else put in A
            auto row = 2 * f->idx;
            if (e0->vertex == p0) {
                tripletsB.emplace_back(row, 0, cosine - T(1));
                tripletsB.emplace_back(row, 1, -sine);
                tripletsB.emplace_back(row + 1, 0, sine);
                tripletsB.emplace_back(row + 1, 1, cosine - T(1));
            } else if (e0->vertex == p1) {
                tripletsB.emplace_back(row, 2, cosine - T(1));
                tripletsB.emplace_back(row, 3, -sine);
                tripletsB.emplace_back(row + 1, 2, sine);
                tripletsB.emplace_back(row + 1, 3, cosine - T(1));
            } else {
                auto freeIdx = freeIdxTable.at(e0->vertex->idx);
                tripletsA.emplace_back(row, 2 * freeIdx, cosine - T(1));
                tripletsA.emplace_back(row, 2 * freeIdx + 1, -sine);
                tripletsA.emplace_back(row + 1, 2 * freeIdx, sine);
                tripletsA.emplace_back(row + 1, 2 * freeIdx + 1, cosine - T(1));
            }

            if (e1->vertex == p0) {
                tripletsB.emplace_back(row, 0, -cosine);
                tripletsB.emplace_back(row, 1, sine);
                tripletsB.emplace_back(row + 1, 0, -sine);
                tripletsB.emplace_back(row + 1, 1, -cosine);
            } else if (e1->vertex == p1) {
                tripletsB.emplace_back(row, 2, -cosine);
                tripletsB.emplace_back(row, 3, sine);
                tripletsB.emplace_back(row + 1, 2, -sine);
                tripletsB.emplace_back(row + 1, 3, -cosine);
            } else {
                auto freeIdx = freeIdxTable.at(e1->vertex->idx);
                tripletsA.emplace_back(row, 2 * freeIdx, -cosine);
                tripletsA.emplace_back(row, 2 * freeIdx + 1, sine);
                tripletsA.emplace_back(row + 1, 2 * freeIdx, -sine);
                tripletsA.emplace_back(row + 1, 2 * freeIdx + 1, -cosine);
            }

            if (e2->vertex == p0) {
                tripletsB.emplace_back(row, 0, T(1));
                tripletsB.emplace_back(row + 1, 1, T(1));
            } else if (e2->vertex == p1) {
                tripletsB.emplace_back(row, 2, T(1));
                tripletsB.emplace_back(row + 1, 3, T(1));
            } else {
                auto freeIdx = freeIdxTable.at(e2->vertex->idx);
                tripletsA.emplace_back(row, 2 * freeIdx, T(1));
                tripletsA.emplace_back(row + 1, 2 * freeIdx + 1, T(1));
            }
        }
        SparseMatrix A(2 * numFaces, 2 * numFree);
        A.reserve(tripletsA.size());
        A.setFromTriplets(tripletsA.begin(), tripletsA.end());

        SparseMatrix bFree(2 * numFaces, 2 * numFixed);
        bFree.reserve(tripletsB.size());
        bFree.setFromTriplets(tripletsB.begin(), tripletsB.end());

        // Calculate rhs from free and fixed matrices
        SparseMatrix b = bFree * bFixed * -1;

        // Setup AtA and solver
        SparseMatrix AtA = A.transpose() * A;
        AtA.makeCompressed();
        Solver solver;
        solver.compute(AtA);
        if (solver.info() != Eigen::ComputationInfo::Success) {
            throw SolverException(solver.lastErrorMessage());
        }

        // Setup Atb
        SparseMatrix Atb = A.transpose() * b;

        // Solve AtAx = AtAb
        DenseMatrix x = solver.solve(Atb);

        // Assign solution to UV coordinates
        // Pins are already updated, so these are free vertices
        for (const auto& v : mesh->vertices()) {
            if (v == p0 or v == p1) {
                continue;
            }
            auto newIdx = 2 * freeIdxTable.at(v->idx);
            v->pos[0] = x(newIdx, 0);
            v->pos[1] = x(newIdx + 1, 0);
            v->pos[2] = T(0);
        }
    }
};

}  // namespace OpenABF


#include "Derivatives.hpp"

#include <fmt/format.h>
#include <spdlog/spdlog.h>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/irange.hpp>

#include <FeltElements/internal/Format.hpp>
#include "Body.hpp"

namespace
{
using namespace FeltElements;

template <typename T>
int sgn(T val)
{
	return (T(0) < val) - (val < T(0));
}
auto constexpr delta = [](auto const i, auto const j) { return i == j; };

Element::Elasticity const c_lambda = ([]() {  // NOLINT(cert-err58-cpp)
	std::size_t constexpr N = 3;
	Element::Elasticity c{};
	for (std::size_t i = 0; i < N; i++)
		for (std::size_t j = 0; j < N; j++)
			for (std::size_t k = 0; k < N; k++)
				for (std::size_t l = 0; l < N; l++) c(i, j, k, l) = delta(i, j) * delta(k, l);

	return c;
}());

// TODO: constexpr - requires constexpr std::copy (C++20)
Element::Elasticity const c_mu = ([]() {  // NOLINT(cert-err58-cpp)
	std::size_t constexpr N = 3;
	Element::Elasticity c{};
	for (std::size_t i = 0; i < N; i++)
		for (std::size_t j = 0; j < N; j++)
			for (std::size_t k = 0; k < N; k++)
				for (std::size_t l = 0; l < N; l++)
					c(i, j, k, l) = delta(i, k) * delta(j, l) + delta(i, l) * delta(j, k);

	return c;
}());

// TODO: constexpr - requries simd_vector_type to satisfy literal type requirements
Tensor::Matrix<3> const I = ([]() {	 // NOLINT(cert-err58-cpp)
	Tensor::Matrix<3> mat{};
	mat.eye2();
	return mat;
}());
}  // namespace

namespace FeltElements::ex
{
using namespace Tensor;

constexpr auto dX_by_dL = [](auto const & X) {
	return Func::einsum<Idxs<k, i>, Idxs<k, j>>(X, Derivatives::dN_by_dL);
};

constexpr auto dL_by_dX = [](auto const & dX_by_dL_) { return Func::inv(dX_by_dL_); };

constexpr auto dX_by_dS = [](auto const & X) {
	return Func::einsum<Idxs<k, i>, Idxs<k, j>>(X, Derivatives::dN_by_dS);
};

constexpr auto dN_by_dX = [](auto const & dL_by_dX_) {
	// dN/dX^T = dX/dL^(-T) * dN/dL^T => dN/dX = dN/dL * dX/dL^(-1) = dN/dL * dL/dX
	return Func::einsum<Idxs<i, k>, Idxs<k, j>>(Derivatives::dN_by_dL, dL_by_dX_);
};

constexpr auto dx_by_dX = [](auto const & x, auto const & dN_by_dX_) {
	return Func::einsum<Idxs<k, i>, Idxs<k, j>>(x, dN_by_dX_);
};

constexpr auto finger = [](auto const & F) { return Func::einsum<Idxs<i, k>, Idxs<j, k>>(F, F); };

constexpr auto sigma = [](Scalar const J, auto const & b, Scalar const lambda, Scalar const mu) {
	return (mu / J) * (b - I) + (lambda / J) * log(J) * I;
};

constexpr auto T = [](auto const & dN_by_dx, auto const & sigma_) {
	// T = v * sigma * dN/dx^T
	return Func::einsum<Idxs<a, k>, Idxs<i, k>>(dN_by_dx, sigma_);
};

constexpr auto c = [](Scalar J, Scalar lambda, Scalar mu) {
	Scalar const lambda_prime = lambda / J;
	Scalar const mu_prime = (mu - lambda * std::log(J)) / J;

	return lambda_prime * c_lambda + mu_prime * c_mu;
};

constexpr auto Kc = [](auto const & dN_by_dx, auto const & c_) {
	// Kc_ij = v * dN_a/dx_k * c_ikjl * dN_b/dx_l
	return Func::einsum<Idxs<a, k>, Idxs<i, k, j, l>, Idxs<b, l>, Order<a, i, b, j>>(
		dN_by_dx, c_, dN_by_dx);
};

constexpr auto Ks = [](auto const & dN_by_dx, auto const & s) {
	// Ks_ij = v * dN_a/dx_k * sigma_kl * dN_b/dx_l * delta_ij
	return Func::einsum<Idxs<a, k>, Idxs<k, l>, Idxs<b, l>, Idxs<i, j>, Order<a, i, b, j>>(
		dN_by_dx, s, dN_by_dx, I);
};
}  // namespace FeltElements::ex

namespace FeltElements::Derivatives
{
Element::StiffnessResidual KR(
	Element::NodePositions const & x,
	Element::BoundaryVtxhIdxs const & boundary_faces_idxs,
	Element::BoundaryNodePositions const & boundary_faces_x,
	Element::ShapeDerivative const & dN_by_dX,
	Body::Material const & material,
	Body::Forces const & forces)
{
	using Tensor::Func::all;

	Element::Gradient const dx_by_dX = ex::dx_by_dX(x, dN_by_dX);
	auto const b = ex::finger(dx_by_dX);
	Scalar const J = Derivatives::det_dx_by_dX(dx_by_dX);
	if (J < 0)
	{
		std::string const msg =
			fmt::format("Error: J < 0:\nJ = {}\nF = {}\nb = {}", J, dx_by_dX, b);
		spdlog::error(msg);
		throw std::invalid_argument{msg};
	}
	Scalar const v = Derivatives::v(x);	 // TODO: but also v = J*V

	auto const & dx_by_dL = ex::dX_by_dL(x);
	auto const & dL_by_dx = ex::dL_by_dX(dx_by_dL);
	Element::ShapeDerivative const dN_by_dx = ex::dN_by_dX(dL_by_dx);

	Element::Stress const sigma = ex::sigma(J, b, material.lambda, material.mu);

	auto const & c = ex::c(J, material.lambda, material.mu);
	// Elasticity component.
	auto const & Kc = ex::Kc(dN_by_dx, c);
	// Initial stress component.
	auto const & Ks = ex::Ks(dN_by_dx, sigma);
	// Constitutive (traction) component.
	auto const & Kp = Derivatives::Kp(boundary_faces_x, boundary_faces_idxs, forces.p);

	// Tangent stiffness matrix.
	Element::Stiffness K = v * (Kc + Ks) + Kp;

	// Internal forces.
	auto const & T_by_v = ex::T(dN_by_dx, sigma);

	// Residual
	Element::Forces R = v * T_by_v;

	// Body force.
	auto const & F_by_V = forces.F_by_m * material.rho;
	auto const & F_by_v = F_by_V / J;
	auto const & F_by_v_per_node = (1.0 / Element::num_nodes) * F_by_v;
	Node::Force const F_per_node = v * F_by_v_per_node;

	for (const auto node_idx : boost::irange(Element::num_nodes)) R(node_idx, all) -= F_per_node;

	// Traction force.
	for (const auto & [s, idxs] : boost::range::combine(boundary_faces_x, boundary_faces_idxs))
	{
		const Node::Force & t =
			(1.0 / BoundaryElement::num_nodes) * Derivatives::t(forces.p, Derivatives::dX_by_dS(s));
		for (Tensor::Index const node_idx : idxs) R(node_idx, all) -= t;
	}
	/*
		if (!Tensor::Func::all_of(R == R))	// Assert no NaNs
		{
			std::string const & msg = fmt::format(
				"Residual is NaN because: v={}; T_by_v={}; dN_by_dx={}; sigma={}; J={}; b={}; "
				"dx_by_dX={}.  Where J={}; dx_by_dX=\n{}",
				std::isnan(v),
				!Tensor::Func::all_of(T_by_v == T_by_v),
				!Tensor::Func::all_of(dN_by_dx == dN_by_dx),
				!Tensor::Func::all_of(sigma == sigma),
				std::isnan(J),
				!Tensor::Func::all_of(b == b),
				!Tensor::Func::all_of(dx_by_dX == dx_by_dX),
				J,
				dx_by_dX);

			spdlog::error(msg);

			throw std::logic_error{msg};
		};
	*/
	return {K, R};
}

Element::Stiffness Kc(
	Element::ShapeDerivative const & dN_by_dx, Scalar const v, Element::Elasticity const & c)
{
	return v * ex::Kc(dN_by_dx, c);
}

Element::Stiffness Ks(
	Element::ShapeDerivative const & dN_by_dx, Scalar const v, Element::Stress const & s)
{
	return v * ex::Ks(dN_by_dx, s);
}

Element::SurfaceShapeDerivative const dN_by_dS =  // NOLINT(cert-err58-cpp)
	Fastor::evaluate(Fastor::inv(Tensor::Matrix<3, 3>{
		{1, 1, 1},
		{0, 1, 0},
		{0, 0, 1},
	}))(Fastor::all, Fastor::fseq<1, 3>());
// clang-format on

namespace
{
auto const [KpLHS, KpRHS] = ([]() {	 // NOLINT(cert-err58-cpp)
	using Tensor::Func::all;
	Tensor::Matrix<3> KpLHS_, KpRHS_;
	for (auto const a : boost::irange(BoundaryElement::num_nodes))
		for (auto const b : boost::irange(BoundaryElement::num_nodes))
		{
			// Assuming single-point integral, i.e. sample at centre where Na = Nb = 1/3.
			KpLHS_(a, b) = dN_by_dS(a, 1) * (1.0 / 3.0) - dN_by_dS(b, 1) * (1.0 / 3.0);
			KpRHS_(a, b) = dN_by_dS(a, 0) * (1.0 / 3.0) - dN_by_dS(b, 0) * (1.0 / 3.0);
		}

	return std::tuple{KpLHS_, KpRHS_};
}());
}

Element::Stiffness Kp(
	Element::BoundaryNodePositions const & xs,
	const Element::BoundaryVtxhIdxs & S_to_Vs,
	Scalar const p)
{
	using Tensor::Idxs;
	using Tensor::Order;
	using Tensor::Func::all;
	using Tensor::Func::einsum;
	enum
	{
		i,
		j,
		k,
		a,
		b
	};
	Element::Stiffness K = 0;
	for (auto const & [x, S_to_V] : boost::range::combine(xs, S_to_Vs))
	{
		auto const & dx_by_dS = Derivatives::dX_by_dS(x);
		Scalar const A = Derivatives::A(x);

		Tensor::Vector<3> const & dx_by_dS1 = dx_by_dS(all, 0);
		Tensor::Vector<3> const & dx_by_dS2 = dx_by_dS(all, 1);

		BoundaryElement::Stiffness const & K_face = 0.5 * A * p *
			(einsum<Idxs<i, j, k>, Idxs<k>, Idxs<a, b>, Order<a, i, b, j>>(
				 levi_civita, dx_by_dS1, KpLHS) -
			 einsum<Idxs<i, j, k>, Idxs<k>, Idxs<a, b>, Order<a, i, b, j>>(
				 levi_civita, dx_by_dS2, KpRHS));

		for (auto const & [face_idx_a, cell_idx_a] : boost::adaptors::index(S_to_V))
			for (auto const & [face_idx_b, cell_idx_b] : boost::adaptors::index(S_to_V))
				K(cell_idx_a, all, cell_idx_b, all) += K_face(face_idx_a, all, face_idx_b, all);
	}

	return K;
}

Element::Elasticity c(Scalar J, Scalar lambda, Scalar mu)
{
	return ex::c(J, lambda, mu);
}

Node::Force t(Scalar const p, Element::SurfaceGradient const & dX_by_dS)
{
	using Tensor::Func::all;
	using Tensor::Func::fix;
	Node::Force const & dX1_by_dS = dX_by_dS(all, fix<0>);
	Node::Force const & dX2_by_dS = dX_by_dS(all, fix<1>);
	return (1.0 / 2.0) * p * cross(dX1_by_dS, dX2_by_dS);
}

Element::Forces T(
	Element::ShapeDerivative const & dN_by_dx, Scalar const v, Element::Stress const & sigma)
{
	return v * ex::T(dN_by_dx, sigma);
}

Element::Stress sigma(
	Scalar const J, Element::Gradient const & b, Scalar const lambda, Scalar const mu)
{
	return ex::sigma(J, b, lambda, mu);
}

Scalar det_dx_by_dX(Element::Gradient const & dx_by_dX)
{
	return Fastor::det(dx_by_dX);
}

Element::Gradient b(Element::Gradient const & F)
{
	return ex::finger(F);
}

Element::Gradient dx_by_dX(
	Element::NodePositions const & x, Element::ShapeDerivative const & dN_by_dX)
{
	return ex::dx_by_dX(x, dN_by_dX);
}

Element::Gradient dx_by_dX(Element::Gradient const & dx_by_dL, Element::Gradient const & dL_by_dX)
{
	using namespace Tensor;
	return Func::einsum<Idxs<k, i>, Idxs<j, k>>(dx_by_dL, dL_by_dX);
}

Element::ShapeDerivative dN_by_dX(Element::Gradient const & dL_by_dx)
{
	return ex::dN_by_dX(dL_by_dx);
}

Element::ShapeDerivative dN_by_dX(Element::NodePositions const & X)
{
	auto const & dX_by_dL = ex::dX_by_dL(X);
	auto const & dL_by_dX = ex::dL_by_dX(dX_by_dL);
	return ex::dN_by_dX(dL_by_dX);
}

Element::CartesianDerivative dx_by_dN(Element::ShapeCartesianTransform const & N_to_x)
{
	using namespace Tensor::Func;
	return N_to_x(fseq<1, last>{}, all);
}

Element::Gradient dL_by_dX(Element::Gradient const & dX_by_dL)
{
	return ex::dL_by_dX(dX_by_dL);
}

Element::Gradient dX_by_dL(Element::NodePositions const & X)
{
	return ex::dX_by_dL(X);
}

Element::SurfaceGradient dX_by_dS(BoundaryElement::NodePositions const & X)
{
	return ex::dX_by_dS(X);
}

Element::ShapeDerivative dN_by_dX(Element::ShapeCartesianTransform const & N_to_x)
{
	using namespace Tensor::Func;
	// Interpolation: (1, x, y, z)^T = N_to_x * N, where N is 4x natural coordinates (corners).
	// Invert then strip constant terms, leaving just coefficients, i.e. the derivative.
	return evaluate(inv(N_to_x))(all, fseq<1, last>{});
}

Element::ShapeCartesianTransform N_to_x(Element::NodePositions const & X)
{
	Element::ShapeCartesianTransform mat;
	mat(Fastor::ffirst, Fastor::all) = 1.0;
	mat(Fastor::fseq<1, Fastor::last>{}, Fastor::all) = Fastor::transpose(X);
	return mat;
}

Scalar V(Element::NodePositions const & x)
{
	using namespace Tensor::Func;
	auto const & start_3x3 = x(fseq<0, 3>{}, all);
	//	std::clog << x(fix<3>, all).self().size() << "\n";
	Tensor::Vector<3> end_1x3 = x(fix<3>, all);
	Tensor::Matrix<3> end_3x3;
	end_3x3(fix<0>, all) = end_1x3;
	end_3x3(fix<1>, all) = end_1x3;
	end_3x3(fix<2>, all) = end_1x3;
	auto const & delta = start_3x3 - end_3x3;
	return std::abs(Fastor::det(delta) / 6.0);
}

Scalar v(Element::NodePositions const & x)
{
	// Volume in local coords (constant)
	constexpr Scalar v_wrt_L = 1.0 / 6.0;
	return v_wrt_L * det_dx_by_dL(x);
}

Scalar A(BoundaryElement::NodePositions const & s)
{
	using Tensor::Func::all;
	using Tensor::Func::cross;
	using Tensor::Func::norm;
	Tensor::Vector<3> const & v1 = s(0, all) - s(2, all);
	Tensor::Vector<3> const & v2 = s(1, all) - s(2, all);
	return 0.5 * norm(cross(v1, v2));
}

Scalar det_dx_by_dL(Element::NodePositions const & x)
{
	using namespace Tensor;
	using Func::all;
	using Func::einsum;
	using Func::fix;

	//	Scalar det_ = 0;
	//	for (Index i = 0; i < Element::count; i++)
	//		for (Index j = 0; j < Element::count; j++)
	//			for (Index k = 0; k < Element::count; k++)
	//				det_ += det_dN_by_dL(i, j, k) * x(i, 0) * x(j, 1) * x(k, 2);
	//
	//	return det_;

	//	Element::ShapeDerivativeDeterminant xs = ([&x](){
	//		using Tensor::Index;
	//		using Tensor::Func::all;
	//		Element::ShapeDerivativeDeterminant xs_;
	//		for (Index i = 0; i < Element::count; i++)
	//			for (Index j = 0; j < Element::count; j++)
	//				for (Index k = 0; k < Element::count; k++)
	//					xs_(i, j, k) = x(i, 0) * x(j, 1) * x(k, 2);
	//		return xs_;
	//	}());
	//	Tensor::Vector<1> sum = einsum<Idxs<i, j, k>, Idxs<i, j, k>>(
	//		det_dN_by_dL, xs);

	// "Consideration of Body Forces within Finite Element Analysis", Glenk et al., 2018
	Tensor::Vector<4> x0 = x(all, fix<0>);
	Tensor::Vector<4> x1 = x(all, fix<1>);
	Tensor::Vector<4> x2 = x(all, fix<2>);
	Tensor::Vector<1> sum =
		einsum<Idxs<i, j, k>, Idxs<i>, Idxs<j>, Idxs<k>>(Derivatives::det_dN_by_dL, x0, x1, x2);

	return sum(0);
}

// clang-format off
Element::IsoCoordDerivative const dL_by_dN = // NOLINT(cert-err58-cpp)
	Tensor::Matrix<4, 4>{
		{1, 1, 1, 1},
		{0, 1, 0, 0},
		{0, 0, 1, 0},
		{0, 0, 0, 1}}(Fastor::fseq<1, 4>(), Fastor::all);

Element::ShapeDerivative const dN_by_dL = // NOLINT(cert-err58-cpp)
	Fastor::evaluate(Fastor::inv(Tensor::Matrix<4, 4>{
		{1, 1, 1, 1},
		{0, 1, 0, 0},
		{0, 0, 1, 0},
		{0, 0, 0, 1}}))(Fastor::all, Fastor::fseq<1, 4>());

Element::ShapeDerivativeDeterminant const det_dN_by_dL = ([]() {  // NOLINT(cert-err58-cpp)
	using Tensor::Index;
	using Tensor::Func::all;
	using Tensor::Func::det;
	Element::ShapeDerivativeDeterminant det_dN_by_dL_;
	for (Index i = 0; i < Element::num_nodes; i++)
		for (Index j = 0; j < Element::num_nodes; j++)
			for (Index k = 0; k < Element::num_nodes; k++)
			{
				Tensor::Matrix<3> dN_by_dL_ijk;
				dN_by_dL_ijk(0, all) = dN_by_dL(i, all);
				dN_by_dL_ijk(1, all) = dN_by_dL(j, all);
				dN_by_dL_ijk(2, all) = dN_by_dL(k, all);
				det_dN_by_dL_(i, j, k) = det(dN_by_dL_ijk);
			}
	return det_dN_by_dL_;
}());

Tensor::Multi<Node::dim, Node::dim, Node::dim> const levi_civita =	// NOLINT(cert-err58-cpp)
	([]() {
		using LeviCivita = Tensor::Multi<Node::dim, Node::dim, Node::dim>;
		LeviCivita E;
		int constexpr const count = static_cast<int>(Node::dim);
		for (int i = 0; i < count; i++)
			for (int j = 0; j < count; j++)
				for (int k = 0; k < count; k++) E(i, j, k) = sgn(j - i) * sgn(k - i) * sgn(k - j);
		return E;
	}());
}  // namespace FeltElements::Derivatives