/*
    Copyright (c) 2014, Philipp Krähenbühl
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:
        * Redistributions of source code must retain the above copyright
        notice, this list of conditions and the following disclaimer.
        * Redistributions in binary form must reproduce the above copyright
        notice, this list of conditions and the following disclaimer in the
        documentation and/or other materials provided with the distribution.
        * Neither the name of the Stanford University nor the
        names of its contributors may be used to endorse or promote products
        derived from this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY Philipp Krähenbühl ''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 Philipp Krähenbühl 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.
*/
#include "util/win_util.h"
#include "util/threading.h"
#include "splitcriterion.h"
#include <random>
#include <stdexcept>
#include <iostream>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

int SplitCriterion::repLabel() const {
	return -1;
}
class LabeledSplit: public SplitCriterion {
protected:
	VectorXi lbl_;
	VectorXf weight_;
	virtual float score( const ArrayXf & p ) const = 0;
public:
	LabeledSplit(){
	}
	LabeledSplit( const RMatrixXf & lbl, const VectorXf & weight ): weight_(weight) {
		if( lbl.cols()!= 1 )
			throw std::invalid_argument( "Only 1d labels supported!" );
		lbl_ = lbl.cast<int>();
	}
	virtual float gain( const VectorXb& is_left ) const {
		const int N = lbl_.maxCoeff()+1;
		ArrayXf wl = 1e-20*ArrayXf::Ones(N), wr = 1e-20*ArrayXf::Ones(N);
		for( int i=0; i<(int)lbl_.size(); i++ )
			if( is_left[i] )
				wl[lbl_[i]] += weight_[i];
			else
				wr[lbl_[i]] += weight_[i];
		const float l = wl.sum(), r = wr.sum();
		const float sl = score(wl/l), sr = score(wr/r);
		printf("%d  %d\n", (int)l, (int)r );
		return score( (wl+wr)/(l+r) ) - (sl*l/(l+r) + sr*r/(l+r));
	}
	virtual float bestThreshold( const VectorXf & f, float * gain ) const {
		const int N = lbl_.maxCoeff()+1;
		const float EPS=1e-6;
		// Create the feature/label pairs
		std::vector< std::pair<float,int> > elements( f.size() );
		for( int i=0; i<f.size(); i++ )
			elements[i] = std::make_pair( f[i], i );
		std::sort(elements.begin(), elements.end() );

		// And compute the probabilities and cirterion
		ArrayXf wl = 1e-20*ArrayXf::Ones(N), wr = 1e-20*ArrayXf::Ones(N);
		for( int i=0; i<lbl_.size(); i++ )
			wr[ lbl_[i] ] += weight_[i];

		// Initialize the thresholds
		float best_gain = 0, tbest = (elements.front().first+elements.back().first)/2, last_t = elements.front().first;
		const float tot_s = score( wr/wr.sum() );
		// Find the best threshold
		for( auto i: elements ) {
			const float t = i.first;
			const int j = i.second;
			// If there is a threshold
			if( t - last_t > EPS ) {
				// Compute the score
				const float l = wl.sum(), r = wr.sum();
				const float sl = score(wl/l), sr = score(wr/r);
				const float g = tot_s - ( sl*l/(l+r) + sr*r/(l+r) );
				if( g > best_gain ) {
					best_gain = g;
					tbest = (last_t+t)/2.;
				}
			}
			// Update the probabilities
			wl[ lbl_[j] ] += weight_[j];
			wr[ lbl_[j] ] -= weight_[j];
			last_t = t;
		}
		if( gain )
			*gain = best_gain;
		return tbest;
	}
	virtual bool is_pure( ) const {
		for( int i=1; i<lbl_.size(); i++ )
			if( lbl_[0] != lbl_[i] )
				return false;
		return true;
	}
	virtual int repLabel() const {
		// Count the label occurence
		const int n_lbl = lbl_.maxCoeff()+1;
		VectorXi lbl_cnt = VectorXi::Zero( n_lbl );
		for( int i=0; i<lbl_.size(); i++ )
			lbl_cnt[lbl_[i]]++;
		// And return the maximum
		int rep_lbl = 0;
		for( int i=0; i<lbl_.size(); i++ )
			if( lbl_cnt[lbl_[rep_lbl]] < lbl_cnt[lbl_[i]] )
				rep_lbl = i;
		return rep_lbl;
	}
};
class GiniSplit: public LabeledSplit {
public:
	template<typename ...ARGS> GiniSplit(ARGS... args) :LabeledSplit(args...) {}
	virtual std::shared_ptr<SplitCriterion> create( const RMatrixXf & lbl, const VectorXf & weight ) const {
		return std::make_shared<GiniSplit>( lbl, weight );
	}
	virtual float score( const ArrayXf & p ) const {
		return (p*(1-p)).sum();
	}
};
class EntropySplit: public LabeledSplit {
public:
	template<typename ...ARGS> EntropySplit(ARGS... args) :LabeledSplit(args...) {}
	virtual std::shared_ptr<SplitCriterion> create( const RMatrixXf & lbl, const VectorXf & weight ) const {
		return std::make_shared<EntropySplit>( lbl, weight );
	}
	virtual float score( const ArrayXf & p ) const {
		return -(p*(p+1e-20).log()).sum();
	}
};
RMatrixXf pairwiseDistance( const RMatrixXf & X, int N ) {
	static std::mt19937 gen;
	// Do a random pairwise projection
	const int M = X.cols();
	RMatrixXf r( X.rows(), N );
	std::vector<int> o1(N), o2(N);
	for( int i=0; i<N; i++ ) {
		o1[i] = 1+(gen()%(M-1));
		o2[i] = gen()%o1[i];
	}
	for( int i=0; i<X.rows(); i++ )
		for( int j=0; j<N; j++ )
			r(i,j) = ((X(i,o1[j]) == X(i,o2[j])));
	return r;
}
// RMatrixXf pairwiseDistance( const RMatrixXf & X, int N ) {
// 	static std::mt19937 gen;
// 	// Do a random pairwise projection
// 	const int M = X.cols();
//
// 	// Figure out if a column has more than one value
// 	VectorXb singleton = VectorXb::Ones( M );
// 	VectorXf v = X.row(0);
// 	for( int i=1; i<X.rows(); i++ )
// 		for( int j=0; j<M; j++ )
// 			if( singleton[j] )
// 				singleton[j] = (v[j] == X(i,j));
// 	int N_ns = (singleton.array() == 0).cast<int>().sum();
// 	if( N_ns == 0 )
// 		return RMatrixXf::Zero( X.rows(), 1 );
//
// 	// Adjust N to the fewer samples in theory by a factor of 1-(1-N_ns/M)^2,
// 	// in practice 2*N_ns/M seems close enough [without the qudartic term]
// 	if( 2*N_ns < M )
// 		N = 2*N*N_ns / M;
//
// 	VectorXi good_o( N_ns );
// 	for( int i=0,j=0; i<M; i++ )
// 		if( !singleton[i] )
// 			good_o[j++] = i;
//
// 	RMatrixXf r( X.rows(), N );
// 	std::vector<int> o1(N), o2(N);
// 	for( int i=0; i<N; i++ ) {
// 		o1[i] = good_o[gen()%N_ns];
// 		o2[i] = gen()%M;
// 	}
// 	for( int i=0; i<X.rows(); i++ )
// 		for( int j=0; j<N; j++ ) {
// 			r(i,j) = (X(i,o1[j]) != X(i,o2[j]));
// 		}
// 	return r;
// }
VectorXf project1D( const RMatrixXf & Y, int * rep_label=NULL ) {
	const bool fast = true, very_fast = true;
	// Remove the DC
	RMatrixXf dY = Y.rowwise() - Y.colwise().mean();
	// ... and use (pc > 0)
	VectorXf lbl = VectorXf::Zero( Y.rows() );
	// Find the largest PC
	if( very_fast ) {
		// Find the PC using poweriterations
		lbl.setRandom();
		float old_v = 0;
		for( int it=0; it<20; it++ ){
			// Normalize
			lbl.array() /= lbl.norm()+std::numeric_limits<float>::min();
			lbl = dY*(dY.transpose()*lbl);
			float v = lbl.norm();
			if( fabs(v-old_v) < 1e-4*fabs(v) )
				break;
			old_v = v;
		}
		VectorXf tmp = dY.transpose()*lbl;
		tmp.array() /= tmp.norm()+std::numeric_limits<float>::min();
		lbl = dY*tmp;
	}
	else if(fast) {
		// Compute the eigen values of the covariance (and project onto the largest eigenvector)
		MatrixXf cov = dY.transpose()*dY;
		SelfAdjointEigenSolver<MatrixXf> eigensolver(0.5*(cov+cov.transpose()));
		MatrixXf ev = eigensolver.eigenvectors();
		lbl = dY * ev.col( ev.cols()-1 );
	}
	else {
		// Use the SVD
		JacobiSVD<RMatrixXf> svd = dY.jacobiSvd(ComputeThinU | ComputeThinV );
		// Project onto the largest PC
		lbl = svd.matrixU().col(0) * svd.singularValues()[0];
	}
	// Find the representative label
	if( rep_label )
		dY.array().square().rowwise().sum().minCoeff( rep_label );

	return (lbl.array() > 0).cast<float>();
}

template<typename Split> class Structured: public Split {
protected:
	int rep_label_;
	int n_struc_samples_;
public:
	Structured( int n_struc_samples = 256 ):n_struc_samples_(n_struc_samples){}
	Structured( const RMatrixXf & lbl, const VectorXf & weight, int n_struc_samples ): Split( project1D(pairwiseDistance(lbl,n_struc_samples),&rep_label_), weight ) {
	}
	virtual std::shared_ptr< SplitCriterion > create( const RMatrixXf & lbl, const VectorXf & weight ) const {
		return std::make_shared< Structured<Split> >( lbl, weight, n_struc_samples_ );
	}
	virtual int repLabel() const {
		return rep_label_;
	}
};
std::shared_ptr<SplitCriterion> entropySplit() {
	return std::make_shared<EntropySplit>();
}
std::shared_ptr<SplitCriterion> giniSplit() {
	return std::make_shared<GiniSplit>();
}
std::shared_ptr<SplitCriterion> structEntropySplit( int n_struc_samples ) {
	return std::make_shared< Structured<EntropySplit> >( n_struc_samples );
}
std::shared_ptr<SplitCriterion> structGiniSplit( int n_struc_samples ) {
	return std::make_shared< Structured<GiniSplit> >( n_struc_samples );
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_DETAILS_BLAS_R_HPP_INCLUDED
#define NT2_LINALG_DETAILS_BLAS_R_HPP_INCLUDED

#include <complex>
#include <boost/preprocessor/cat.hpp>
#include <nt2/linalg/details/blas/blas1.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>

namespace nt2 { namespace details
{
#define NT2_R(T, PREFIX, SUFFIX)                                            \
inline void BOOST_PP_CAT(ger,SUFFIX)( const long int *m, const long int *n  \
                                    , const T *al                           \
                                    , const T *x, const long int *incx      \
                                    , const T *y, const long int *incy      \
                                    , const T *a, const long int *lda       \
                                    )                                       \
{                                                                           \
  NT2_F77NAME(BOOST_PP_CAT(PREFIX,ger))(ta,m,n,al,x,incx,y,incy,a,lda);     \
}                                                                           \
/**/

  // INTERNAL ONLY
  // gerx A <- {t,c}x*y+A  with x =  u, c or nothing in which case x == y
  // _GER  ( M, N, ALPHA, X, INCX, Y, INCY, A, LDA ) S, D
  // _GERU ( M, N, ALPHA, X, INCX, Y, INCY, A, LDA ) C, Z (TRANSPOSE)
  // _GERC ( M, N, ALPHA, X, INCX, Y, INCY, A, LDA ) C, Z (TRANSCONJUGATE)
  NT2_R(double, d, )
  NT2_R(float,  s, )
  NT2_R(std::complex<double>, z, u)
  NT2_R(std::complex<float>,  c, u)
  NT2_R(std::complex<double>, z, c)
  NT2_R(std::complex<float>,  c, c)

#undef NT2_R

#define NT2_R(T, P1, P2)                                                  \
inline void BOOST_PP_CAT(P2,r)( const char * uplo                         \
                              , const long int *n                         \
                              , const T *al                               \
                              , const T *x, const long int *incx          \
                              , T *a      , const long int *lda           \
                              )                                           \
{                                                                         \
  NT2_F77NAME(BOOST_PP_CAT(P1,BOOST_PP_CAT(P2,r)))(ta,n,al,x,incx,a,lda); \
}                                                                         \
/**/

  // INTERNAL ONLY
  // herx A <- hx*y+hy*x+A
  // syrx A <- tx*y+ty*x+A
  // _HER  ( UPLO, N, ALPHA, X, INCX, A, LDA ) C, Z
  // _SYR  ( UPLO, N, ALPHA, X, INCX, A, LDA ) S, D
  NT2_R(std::complex<double>, z, he)
  NT2_R(std::complex<float>,  c, he)
  NT2_R(std::complex<double>, s, sy)
  NT2_R(std::complex<float>,  d, sy)
#undef NT2_R

#define NT2_R(T, P1, P2)                                                    \
inline void BOOST_PP_CAT(P2,r)( const char * uplo                           \
                              , const long int *n                           \
                              , const T *al                                 \
                              , const T *x, const long int *incx            \
                              , T *ap                                       \
                              )                                             \
{                                                                           \
  BOOST_PP_CAT(BOOST_PP_CAT(P1,BOOST_PP_CAT(P2,r),_))(uplo,n,al,x,incx,ap); \
}                                                                           \
/**/

  // INTERNAL ONLY
  // herx A <- hx*y+hy*x+A
  // syrx A <- tx*y+ty*x+A
  // _HER  ( UPLO, N, ALPHA, X, INCX, A, LDA ) C, Z
  // _SYR  ( UPLO, N, ALPHA, X, INCX, A, LDA ) S, D
  NT2_R(std::complex<double>, z, hp)
  NT2_R(std::complex<float>,  c, hp)
  NT2_R(std::complex<double>, s, sp)
  NT2_R(std::complex<float>,  d, sp)
#undef NT2_R

#define NT2_R(T, P1, P2)                                                          \
inline void BOOST_PP_CAT(P2,r2) ( const char * uplo                               \
                                , const long int *n                               \
                                , const T *al                                     \
                                , const T *x, const long int *ix                  \
                                , const T *y, const long int *iy                  \
                                , T *a, const long int *lda                       \
                                )                                                 \
{                                                                                \
  BOOST_PP_CAT(BOOST_PP_CAT(P1,BOOST_PP_CAT(P2,r2),_)(uplo,n,al,x,ix,y,iy,a,lda); \
}                                                                                \
/**/

  // INTERNAL ONLY
  // _HER2 ( UPLO, N, ALPHA, X, INCX, Y, INCY, A, LDA )
  // _SYR2 ( UPLO, N, ALPHA, X, INCX, Y, INCY, A, LDA )
  NT2_R(std::complex<double>, z, he)
  NT2_R(std::complex<float>,  c, he)
  NT2_R(std::complex<double>, s, sy)
  NT2_R(std::complex<float>,  d, sy)
#undef NT2_R

#define NT2_R(T, P1, P2)                                                      \
inline void BOOST_PP_CAT(P2,r)( const char * uplo                             \
                              , const long int *n                             \
                              , const T *al                                   \
                              , const T *x, const long int *incx              \
                              , T *ap                                         \
                              )                                               \
{                                                                             \
  BOOST_PP_CAT(BOOST_PP_CAT(P1,BOOST_PP_CAT(P2,r2),_))(uplo,n,al,x,incx,ap);  \
}                                                                             \
/**/

  // INTERNAL ONLY
  // _HPR  ( UPLO, N, ALPHA, X, INCX, AP )
  // _SPR  ( UPLO, N, ALPHA, X, INCX, AP )
  NT2_R(std::complex<double>, z, hp)
  NT2_R(std::complex<float>,  c, hp)
  NT2_R(std::complex<double>, s, sp)
  NT2_R(std::complex<float>,  d, sp)
#undef NT2_R
} }

#endif


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

#ifndef CRYPTO3_HASH_DETAIL_H2F_FUNCTIONS_HPP
#define CRYPTO3_HASH_DETAIL_H2F_FUNCTIONS_HPP

#include <cstdint>
#include <array>
#include <vector>
#include <iterator>
#include <type_traits>

#include <boost/assert.hpp>
#include <boost/static_assert.hpp>
#include <boost/concept/assert.hpp>

#include <nil/marshalling/endianness.hpp>
#include <nil/marshalling/algorithms/pack.hpp>
#include <nil/crypto3/marshalling/multiprecision/types/integral.hpp>

#include <nil/crypto3/algebra/type_traits.hpp>

#include <nil/crypto3/detail/strxor.hpp>

#include <nil/crypto3/hash/algorithm/hash.hpp>

#include <nil/crypto3/hash/detail/h2c/h2c_policy.hpp>

namespace nil {
    namespace crypto3 {
        namespace hashes {
            namespace detail {
                template<std::size_t k, std::size_t len_in_bytes, typename Hash,
                         /// Hash::digest_type is required to be uint8_t[]
                         typename = typename std::enable_if<std::is_same<
                             std::uint8_t,
                             typename std::iterator_traits<typename Hash::digest_type>::value_type>::value>::type>
                class expand_message_xmd {
                    // https://tools.ietf.org/html/draft-irtf-cfrg-hash-to-curve-10#section-5.4.1
                    static_assert(Hash::block_bits % 8 == 0, "r_in_bytes is not a multiple of 8");
                    static_assert(Hash::digest_bits % 8 == 0, "b_in_bytes is not a multiple of 8");
                    static_assert(Hash::digest_bits >= 2 * k, "k-bit collision resistance is not fulfilled");
                    static_assert(len_in_bytes < 0x10000, "len_in_bytes should be less than 0x10000");

                    constexpr static std::size_t b_in_bytes = Hash::digest_bits / 8;
                    constexpr static std::size_t r_in_bytes = Hash::block_bits / 8;
                    constexpr static std::array<std::uint8_t, 2> l_i_b_str = {
                        static_cast<std::uint8_t>(len_in_bytes >> 8u), static_cast<std::uint8_t>(len_in_bytes % 0x100)};
                    constexpr static std::size_t ell = static_cast<std::size_t>(len_in_bytes / b_in_bytes) +
                                                       static_cast<std::size_t>(len_in_bytes % b_in_bytes != 0);
                    constexpr static const std::array<std::uint8_t, r_in_bytes> Z_pad {0};

                    // https://tools.ietf.org/html/draft-irtf-cfrg-hash-to-curve-10#section-5.4.1
                    static_assert(ell <= 255, "ell should be less than 256");

                public:
                    typedef std::array<std::uint8_t, len_in_bytes> result_type;
                    typedef accumulator_set<Hash> internal_accumulator_type;

                    static inline void init_accumulator(internal_accumulator_type &acc) {
                        hash<Hash>(Z_pad, acc);
                    }

                    template<typename InputRange>
                    static inline void update(internal_accumulator_type &acc, const InputRange &range) {
                        BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<InputRange>));

                        hash<Hash>(range, acc);
                    }

                    template<typename InputIterator>
                    static inline void update(internal_accumulator_type &acc, InputIterator first, InputIterator last) {
                        BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<InputIterator>));

                        hash<Hash>(first, last, acc);
                    }

                    template<typename DstRange>
                    static inline typename std::enable_if<
                        std::is_same<std::uint8_t,
                                     typename std::iterator_traits<typename DstRange::iterator>::value_type>::value,
                        result_type>::type
                        process(internal_accumulator_type &b0_acc, const DstRange &dst) {

                        auto dst_size = std::distance(std::cbegin(dst), std::cend(dst));
                        assert(dst_size >= 16 && dst_size <= 255);

                        hash<Hash>(l_i_b_str, b0_acc);
                        hash<Hash>(std::array<std::uint8_t, 1> {0}, b0_acc);
                        hash<Hash>(dst, b0_acc);
                        hash<Hash>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(dst_size)}, b0_acc);
                        typename Hash::digest_type b0 = ::nil::crypto3::accumulators::extract::hash<Hash>(b0_acc);

                        result_type uniform_bytes;
                        internal_accumulator_type bi_acc;
                        hash<Hash>(b0, bi_acc);
                        hash<Hash>(std::array<std::uint8_t, 1> {1}, bi_acc);
                        hash<Hash>(dst, bi_acc);
                        hash<Hash>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(dst_size)}, bi_acc);
                        typename Hash::digest_type bi = ::nil::crypto3::accumulators::extract::hash<Hash>(bi_acc);
                        std::copy(bi.begin(), bi.end(), uniform_bytes.begin());

                        typename Hash::digest_type xored_b;
                        for (std::size_t i = 2; i <= ell; i++) {
                            internal_accumulator_type bi_acc;
                            ::nil::crypto3::detail::strxor(b0, bi, xored_b.begin());
                            hash<Hash>(xored_b, bi_acc);
                            hash<Hash>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(i)}, bi_acc);
                            hash<Hash>(dst, bi_acc);
                            hash<Hash>(std::array<std::uint8_t, 1> {static_cast<std::uint8_t>(dst_size)}, bi_acc);
                            bi = ::nil::crypto3::accumulators::extract::hash<Hash>(bi_acc);
                            std::copy(bi.begin(), bi.end(), uniform_bytes.begin() + (i - 1) * b_in_bytes);
                        }
                        return uniform_bytes;
                    }
                };
            }    // namespace detail
        }        // namespace hashes
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_HASH_DETAIL_H2F_FUNCTIONS_HPP


#include "APSimulator.hpp"

#include "RegularStimulus.hpp"
#include "SimpleStimulus.hpp"
#include "MultiStimulus.hpp"
#include "AbstractIvpOdeSolver.hpp"
#include <boost/shared_ptr.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include "BoostFilesystem.hpp"
#include "RandomNumberGenerator.hpp"

#include "CheckpointArchiveTypes.hpp"
#include "ArchiveLocationInfo.hpp"
#include "SteadyStateRunner.hpp"

#include "hodgkin_huxley_squid_axon_model_1952_modifiedCvode.hpp"
#include "beeler_reuter_model_1977Cvode.hpp"
#include "luo_rudy_1991Cvode.hpp"
#include "ten_tusscher_model_2004_epiCvode.hpp"
#include "decker_2009Cvode.hpp"
//#include "ten_tusscher_model_2004_epiCvodeOpt.hpp" // trying to catch blip "davies_isap_2012CvodeDataClampOpt.hpp"
#include "ohara_rudy_2011_endoCvodeDataClamp.hpp"
#include "davies_isap_2012CvodeDataClamp.hpp"
//#include "davies_isap_2012Cvode.hpp"
#include "paci_hyttinen_aaltosetala_severi_ventricularVersionCvodeDataClamp.hpp"
#include "gokhale_ex293_2017Cvode.hpp"

APSimulator::APSimulator()
    : mModelNumber(1),
      mNumberOfFailedSolves(0),
      mHowManySolves(1),
      mSolveStart(0),
      mSolveEnd(500),
      mSolveTimestep(0.2),
      mNumTimePts(2501),
      mUseDataClamp(false),
      mDataClampOn(0),
      mDataClampOff(1),
      mHaveRunToSteadyState(false)
{
    //RedirectStdErr();
    std::cerr << "*** INSIDE CONSTRUCTOR (nothing should happen here) ***" << std::endl << std::flush;
}

APSimulator::~APSimulator()
{
    //fclose(stderr);
}

/*
void APSimulator::RedirectStdErr()
{
    boost::filesystem::path stderr_dir = "/home/rossj/stderr/"; // should find a better location for this
    std::string log_file_path = stderr_dir.string() + "stderr.log";

    boost::filesystem::create_directories(stderr_dir);

    std::cerr << stderr_dir << std::endl << std::flush;

    freopen( log_file_path.c_str(), "w", stderr );
}
*/

void APSimulator::DefineStimulus(double stimulus_magnitude, double stimulus_duration, double stimulus_period, double stimulus_start_time)
{
    mpStimulus.reset(new RegularStimulus(stimulus_magnitude,stimulus_duration,stimulus_period,stimulus_start_time));
    mStimPeriod = stimulus_period;
}

void APSimulator::DefineSolveTimes(double solve_start, double solve_end, double solve_timestep)
{
    std::cerr << "About to define solve times" << std::endl << std::flush;
    mSolveStart = solve_start;
    mSolveEnd = solve_end;
    mSolveTimestep = solve_timestep;
    mNumTimePts = (solve_end - solve_start)/solve_timestep + 1;
    std::cerr << "Solve times defined" << std::endl << std::flush;
}

void APSimulator::DefineModel(unsigned model_number)
{
    mModelNumber = model_number;
    boost::shared_ptr<AbstractIvpOdeSolver> p_solver;
    if ( model_number == 1u )
    {
        mpModel.reset(new Cellhodgkin_huxley_squid_axon_model_1952_modifiedFromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance"); // 120
        mParameterMetanames.push_back("membrane_potassium_current_conductance");   // 36
        mParameterMetanames.push_back("membrane_leakage_current_conductance");     // 0.3
    }
    else if ( model_number == 2u )
    {
        mpModel.reset(new Cellbeeler_reuter_model_1977FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                        // 4e-2
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");         // 0.0035
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.008
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                     // 9e-4
    }
    else if ( model_number == 3u )
    {
        mpModel.reset(new Cellluo_rudy_1991FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                  // 23
        mParameterMetanames.push_back("membrane_delayed_rectifier_potassium_current_conductance");  // 0.282
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");   // 0.6047
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");               // 0.09
        mParameterMetanames.push_back("membrane_leakage_current_conductance");                      // 0.03921
        mParameterMetanames.push_back("membrane_plateau_potassium_current_conductance");            // 0.0183
    }
    else if ( model_number == 4u )
    {
        mpModel.reset(new Cellten_tusscher_model_2004_epiFromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                        // 14.838
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                     // 0.000175
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");         // 5.405
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.096
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");   // 0.245
        mParameterMetanames.push_back("membrane_sodium_calcium_exchanger_current_conductance");           // 1000
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");                  // 0.294
        mParameterMetanames.push_back("membrane_background_calcium_current_conductance");                 // 0.000592
        mParameterMetanames.push_back("membrane_background_sodium_current_conductance");                  // 0.00029
        mParameterMetanames.push_back("membrane_calcium_pump_current_conductance");                       // 0.825
        mParameterMetanames.push_back("membrane_potassium_pump_current_conductance");                     // 0.0146
        mParameterMetanames.push_back("membrane_sodium_potassium_pump_current_permeability");             // 1.362
    }
    else if ( model_number == 5u )
    {
        mpModel.reset(new Cellohara_rudy_2011_endoFromCellMLCvodeDataClamp(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                        // 75
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                     // 0.0001
        mParameterMetanames.push_back("membrane_background_potassium_current_conductance");               // 0.003
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");         // 0.1908
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.046
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");   // 0.0034
        mParameterMetanames.push_back("membrane_sodium_calcium_exchanger_current_conductance");           // 0.0008
        mParameterMetanames.push_back("membrane_sodium_potassium_pump_current_permeability");             // 30
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");                  // 0.02
        mParameterMetanames.push_back("membrane_background_calcium_current_conductance");                 // 2.5e-8
        mParameterMetanames.push_back("membrane_background_sodium_current_conductance");                  // 3.75e-10
        mParameterMetanames.push_back("membrane_calcium_pump_current_conductance");                       // 0.0005
        mParameterMetanames.push_back("membrane_persistent_sodium_current_conductance");                  // 0.0075
    }
    else if ( model_number == 6u ) // Davies 2012 (with data clamp)
    {
        mpModel.reset(new Celldavies_isap_2012FromCellMLCvodeDataClamp(p_solver, mpStimulus));
        //mpModel.reset(new Celldavies_isap_2012FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                        // 8.25
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                     // 0.000243
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");         // 0.5
        mParameterMetanames.push_back("membrane_potassium_pump_current_conductance");                     // 0.00276
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");   // 0.00746925
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.0138542
        mParameterMetanames.push_back("membrane_calcium_pump_current_conductance");                       // 0.0575
        mParameterMetanames.push_back("membrane_background_calcium_current_conductance");                 // 0.0000007980336
        mParameterMetanames.push_back("membrane_sodium_calcium_exchanger_current_conductance");           // 5.85
        mParameterMetanames.push_back("membrane_sodium_potassium_pump_current_permeability");             // 0.61875
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");                  // 0.1805
        mParameterMetanames.push_back("membrane_transient_outward_chloride_current_conductance");         // 4e-7
        mParameterMetanames.push_back("membrane_background_chloride_current_conductance");                // 0.000225
        mParameterMetanames.push_back("membrane_persistent_sodium_current_conductance");                  // 0.011
    }
    else if ( model_number == 7u ) // Paci ventricular
    {
        mpModel.reset(new Cellpaci_hyttinen_aaltosetala_severi_ventricularVersionFromCellMLCvodeDataClamp(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");                                            // 3671.2302
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");                                         // 8.635702e-5
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");                             // 28.1492
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");                       // 2.041
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");                      // 29.8667
        mParameterMetanames.push_back("membrane_calcium_pump_current_conductance");                                           // 0.4125
        mParameterMetanames.push_back("membrane_sodium_calcium_exchanger_current_conductance");                               // 4900
        mParameterMetanames.push_back("membrane_background_calcium_current_conductance");                                     // 0.69264
        mParameterMetanames.push_back("membrane_sodium_potassium_pump_current_permeability");                                 // 1.841424
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");                                      // 29.9038
        mParameterMetanames.push_back("membrane_background_sodium_current_conductance");                                      // 0.9
        mParameterMetanames.push_back("membrane_hyperpolarisation_activated_funny_current_potassium_component_conductance");  // 30.10312
    }
    else if ( model_number == 8u ) // Decker dog
    {
        mpModel.reset(new Celldecker_2009FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");  // 9.075
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");  //  0.00015552
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");  // 0.5
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");  // 0.0826
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.0138542
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");  // 0.497458
    }
    else if ( model_number == 9u ) // Gokhale 2017
    {
        mpModel.reset(new Cellgokhale_ex293_2017FromCellMLCvode(p_solver, mpStimulus));
        mParameterMetanames.push_back("membrane_fast_sodium_current_conductance");  // 9.075
        mParameterMetanames.push_back("membrane_L_type_calcium_current_conductance");  //  0.00015552
        mParameterMetanames.push_back("membrane_inward_rectifier_potassium_current_conductance");  // 0.5
        mParameterMetanames.push_back("membrane_slow_delayed_rectifier_potassium_current_conductance");  // 0.0826
        mParameterMetanames.push_back("membrane_rapid_delayed_rectifier_potassium_current_conductance");  // 0.0138542
        mParameterMetanames.push_back("membrane_transient_outward_current_conductance");  // 0.497458
    }
    mpModel->SetMaxSteps(10000);
}

std::vector<std::string> APSimulator::GetParameterMetanames()
{
    return mParameterMetanames;
}

void APSimulator::SetToModelInitialConditions()
{
    mpModel->SetStateVariables(mpModel->GetInitialConditions());
}

std::vector<double> APSimulator::SolveForVoltageTraceWithParamsNoDataClamp(const std::vector<double>& rParams)
{
    //std::cerr << "About to try and solve" << std::endl << std::flush;
    //mpModel->SetStateVariables(mpModel->GetInitialConditions());
    std::vector<double> voltage_trace;
    for (unsigned j=0; j<rParams.size(); j++)
    {
        mpModel->SetParameter(mParameterMetanames[j], rParams[j]);
    }
    //std::cerr << "Just set(ted) parameters" << std::endl << std::flush;
    try
    {
        if (mHowManySolves > 1)
        {
            for (unsigned i=0; i<mHowManySolves-1; i++)
            {
                mpModel->Compute(mSolveStart, mSolveStart+mStimPeriod, mSolveTimestep);
            }
        }
        //std::cerr << "About to actually try and solve" << std::endl << std::flush;
        OdeSolution sol1 = mpModel->Compute(mSolveStart, mSolveEnd, mSolveTimestep);
        //std::cerr << "Just solved" << std::endl << std::flush;
        voltage_trace = sol1.GetAnyVariable("membrane_voltage");
    }
    catch (Exception &e)
    {
        std::cerr << "WARNING: CVODE failed to solve with these parameters" << std::endl << std::flush;
        std::cerr << "error was " << e.GetShortMessage() << std::endl << std::flush;
        voltage_trace = std::vector<double>(mNumTimePts, 0.0);
        mNumberOfFailedSolves++;
    }
    return voltage_trace;
}

std::vector<double> APSimulator::SolveForVoltageTraceWithParamsWithDataClamp(const std::vector<double>& rParams)
{
    mpModel->SetStateVariables(mpModel->GetInitialConditions());
    std::vector<double> voltage_trace;

    for (unsigned j=0; j<rParams.size(); j++)
    {
        mpModel->SetParameter(mParameterMetanames[j], rParams[j]);
    }
    mpModel->SetStateVariable("membrane_voltage",mExptTrace[0]);

    boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOffDataClamp();

    //mpModel->ResetSolver();

    if (mHowManySolves > 1)
    {
        for (unsigned i=0; i<mHowManySolves-1; i++)
        {
            mpModel->Compute(mSolveStart, mDataClampOn, mSolveTimestep);
            mpModel->ResetSolver();
            boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOnDataClamp(200);
            mpModel->Compute(mDataClampOn, mDataClampOff, mSolveTimestep);
            mpModel->ResetSolver();
            boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOffDataClamp();
            mpModel->Compute(mDataClampOff, mSolveEnd, mSolveTimestep);
        }
    }

    OdeSolution sol1 = mpModel->Compute(mSolveStart, mDataClampOn, mSolveTimestep);

    mpModel->ResetSolver();
    boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOnDataClamp(200);
    OdeSolution sol2 = mpModel->Compute(mDataClampOn, mDataClampOff, mSolveTimestep);
    mpModel->ResetSolver();
    boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->TurnOffDataClamp();
    OdeSolution sol3 = mpModel->Compute(mDataClampOff, mSolveEnd, mSolveTimestep);

    voltage_trace = sol1.GetAnyVariable("membrane_voltage");
    std::vector<double> voltage_trace_part_2 = sol2.GetAnyVariable("membrane_voltage");
    std::vector<double> voltage_trace_part_3 = sol3.GetAnyVariable("membrane_voltage");

    voltage_trace.erase(voltage_trace.end()-1);
    voltage_trace.insert( voltage_trace.end(), voltage_trace_part_2.begin(), voltage_trace_part_2.end() );
    voltage_trace.erase(voltage_trace.end()-1);
    voltage_trace.insert( voltage_trace.end(), voltage_trace_part_3.begin(), voltage_trace_part_3.end() );
    return voltage_trace;
}

std::vector<double> APSimulator::SolveForVoltageTraceWithParams(const std::vector<double>& rParams)
{
    std::vector<double> voltage_trace;
    if (mUseDataClamp == false)
    {
        voltage_trace = SolveForVoltageTraceWithParamsNoDataClamp(rParams);
    }
    else
    {
        voltage_trace = SolveForVoltageTraceWithParamsWithDataClamp(rParams);
    }
    return voltage_trace;
}

void APSimulator::SetTolerances(double rel_tol, double abs_tol)
{
    mpModel->SetTolerances(rel_tol, abs_tol);
}

double APSimulator::ExampleLogLikelihoodFunction(const std::vector<double>& test_trace)
{
    double total = 0;
    double temp;
    for (unsigned i=0; i<test_trace.size(); i++)
    {
        temp = test_trace[i];
        //std::cerr << temp << " ";
        total += temp*temp;
    }
    //std::cerr << "APSimulator::ExampleLogLikelihoodFunction: " << total << std::endl << std::flush;
    return total;
}

std::vector<double> APSimulator::GenerateSyntheticExptTrace(const std::vector<double>& rParams, double noise_sd, double c_seed)
{
    std::vector<double> expt_trace = SolveForVoltageTraceWithParams(rParams);
    RandomNumberGenerator::Instance()->Reseed(c_seed);
    double random_number;
    for (unsigned i=0; i<expt_trace.size(); i++)
    {
        random_number = RandomNumberGenerator::Instance()->StandardNormalRandomDeviate();
        expt_trace[i] +=  noise_sd*random_number;
    }
    mExptTrace = expt_trace;
    return expt_trace;
}

void APSimulator::UseDataClamp(double data_clamp_on, double data_clamp_off)
{
    mUseDataClamp = true;
    mDataClampOn = data_clamp_on;
    mDataClampOff = data_clamp_off;
}

void APSimulator::SetExperimentalTraceAndTimesForDataClamp(const std::vector<double>& expt_times, const std::vector<double>& expt_trace)
{
    boost::static_pointer_cast<AbstractCvodeCellWithDataClamp>(mpModel)->SetExperimentalData(expt_times,expt_trace);
    mExptTrace = expt_trace;
}

void APSimulator::SetExtracellularPotassiumConc( double extra_K_conc )
{
    try
    {
        mpModel->SetParameter("extracellular_potassium_concentration", extra_K_conc);
    }
    catch (Exception &e)
    {
        std::cerr << "No extracellular_potassium_concentration parameter in model" << std::endl << std::flush;
        std::cerr << "error was " << e.GetShortMessage() << std::endl << std::flush;
    }
}

void APSimulator::SetNumberOfSolves( unsigned num_solves )
{
    mHowManySolves = num_solves;
}

bool APSimulator::RunToSteadyState()
{
    SteadyStateRunner steady_runner(mpModel);
    unsigned max_paces = 50000u;
    steady_runner.SetMaxNumPaces(max_paces);
    bool result = steady_runner.RunToSteadyState();
    mHaveRunToSteadyState = result;
    return result;
}

void APSimulator::ArchiveStateVariables()
{
    boost::filesystem::path arch_dir;
    if (mHaveRunToSteadyState)
    {
        arch_dir = "projects/RossJ/archived_variables/steady_state/";
    }
    else
    {
        arch_dir = "projects/RossJ/archived_variables/non_steady_state/";
    }
    boost::filesystem::create_directories(arch_dir);
    //OutputFileHandler handler(arch_dir.string(), false);
    //handler.SetArchiveDirectory();
    //std::string arch_name = handler.GetOutputDirectoryFullPath() + "m_"+boost::lexical_cast<std::string>(mModelNumber)+".arch";
    std::ofstream ofs((arch_dir.string() + "m_"+boost::lexical_cast<std::string>(mModelNumber)+".arch").c_str());
    boost::archive::text_oarchive output_arch(ofs);
    output_arch <<  *mpModel;
}

void APSimulator::LoadStateVariables()
{
    std::string arch_file = "~/workspace/RossJ/archived_variables/steady_state/m_"+boost::lexical_cast<std::string>(mModelNumber)+".arch";
    std::cout << arch_file << std::endl << std::flush;
    std::ifstream ifs(arch_file.c_str(), std::ios::binary);
    boost::archive::text_iarchive input_arch(ifs);
    input_arch >> *mpModel;
}


/*
 *            Copyright 2009-2020 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

// Third party includes
#include <boost/format.hpp>

// VOTCA includes
#include <votca/tools/constants.h>

// Local VOTCA includes
#include "votca/xtp/aomatrix.h"
#include "votca/xtp/dftcoupling.h"

namespace votca {
namespace xtp {

using boost::format;
using std::flush;

void DFTcoupling::Initialize(tools::Property& options) {

  degeneracy_ = options.get("degeneracy").as<double>();
  degeneracy_ *= tools::conv::ev2hrt;
  numberofstatesA_ = options.get("levA").as<Index>();
  numberofstatesB_ = options.get("levB").as<Index>();
}

void DFTcoupling::WriteToProperty(tools::Property& type_summary,
                                  const Orbitals& orbitalsA,
                                  const Orbitals& orbitalsB, Index a,
                                  Index b) const {
  double J = getCouplingElement(a, b, orbitalsA, orbitalsB);
  tools::Property& coupling = type_summary.add("coupling", "");
  coupling.setAttribute("levelA", a);
  coupling.setAttribute("levelB", b);
  coupling.setAttribute("j", (format("%1$1.6e") % J).str());
}

void DFTcoupling::Addoutput(tools::Property& type_summary,
                            const Orbitals& orbitalsA,
                            const Orbitals& orbitalsB) const {
  tools::Property& dftcoupling = type_summary.add(Identify(), "");
  dftcoupling.setAttribute("homoA", orbitalsA.getHomo());
  dftcoupling.setAttribute("homoB", orbitalsB.getHomo());
  tools::Property& hole_summary = dftcoupling.add("hole", "");
  // hole hole
  for (Index a = Range_orbA.first; a <= orbitalsA.getHomo(); ++a) {
    for (Index b = Range_orbB.first; b <= orbitalsB.getHomo(); ++b) {
      WriteToProperty(hole_summary, orbitalsA, orbitalsB, a, b);
    }
  }
  tools::Property& electron_summary = dftcoupling.add("electron", "");
  // electron-electron
  for (Index a = orbitalsA.getLumo();
       a <= Range_orbA.first + Range_orbA.second - 1; ++a) {
    for (Index b = orbitalsB.getLumo();
         b <= Range_orbB.first + Range_orbB.second - 1; ++b) {
      WriteToProperty(electron_summary, orbitalsA, orbitalsB, a, b);
    }
  }
  return;
}

std::pair<int, Index> DFTcoupling::DetermineRangeOfStates(
    const Orbitals& orbital, Index numberofstates) const {
  const Eigen::VectorXd& MOEnergies = orbital.MOs().eigenvalues();
  if (std::abs(MOEnergies(orbital.getHomo()) - MOEnergies(orbital.getLumo())) <
      degeneracy_) {
    throw std::runtime_error(
        "Homo Lumo Gap is smaller than degeneracy. "
        "Either your degeneracy is too large or your Homo and Lumo are "
        "degenerate");
  }

  Index minimal = orbital.getHomo() - numberofstates + 1;
  Index maximal = orbital.getLumo() + numberofstates - 1;

  std::vector<Index> deg_min = orbital.CheckDegeneracy(minimal, degeneracy_);
  minimal = *std::min_element(deg_min.begin(), deg_min.end());

  std::vector<Index> deg_max = orbital.CheckDegeneracy(maximal, degeneracy_);
  maximal = *std::max_element(deg_max.begin(), deg_max.end());

  std::pair<Index, Index> result;
  result.first = minimal;                 // start
  result.second = maximal - minimal + 1;  // size

  return result;
}

double DFTcoupling::getCouplingElement(Index levelA, Index levelB,
                                       const Orbitals& orbitalsA,
                                       const Orbitals& orbitalsB) const {

  Index levelsA = Range_orbA.second;
  if (degeneracy_ != 0) {
    std::vector<Index> list_levelsA =
        orbitalsA.CheckDegeneracy(levelA, degeneracy_);
    std::vector<Index> list_levelsB =
        orbitalsB.CheckDegeneracy(levelB, degeneracy_);

    double JAB_sq = 0;

    for (Index iA : list_levelsA) {
      Index indexA = iA - Range_orbA.first;
      for (Index iB : list_levelsB) {
        Index indexB = iB - Range_orbB.first + levelsA;
        double JAB_one_level = JAB(indexA, indexB);
        JAB_sq += JAB_one_level * JAB_one_level;
      }
    }
    return std::sqrt(JAB_sq /
                     double(list_levelsA.size() * list_levelsB.size())) *
           tools::conv::hrt2ev;
  } else {
    Index indexA = levelA - Range_orbA.first;
    Index indexB = levelB - Range_orbB.first + levelsA;
    return JAB(indexA, indexB) * tools::conv::hrt2ev;
  }
}

/**
 * \brief evaluates electronic couplings
 * @param  orbitalsA_ molecular orbitals of molecule A
 * @param  orbitalsB_ molecular orbitals of molecule B
 * @param  orbitalsAB_ molecular orbitals of the dimer AB
 */
void DFTcoupling::CalculateCouplings(const Orbitals& orbitalsA,
                                     const Orbitals& orbitalsB,
                                     const Orbitals& orbitalsAB) {

  XTP_LOG(Log::error, *pLog_) << "Calculating electronic couplings" << flush;

  CheckAtomCoordinates(orbitalsA, orbitalsB, orbitalsAB);

  // constructing the direct product orbA x orbB
  Index basisA = orbitalsA.getBasisSetSize();
  Index basisB = orbitalsB.getBasisSetSize();

  if ((basisA == 0) || (basisB == 0)) {
    throw std::runtime_error("Basis set size is not stored in monomers");
  }

  Range_orbA = DetermineRangeOfStates(orbitalsA, numberofstatesA_);
  Range_orbB = DetermineRangeOfStates(orbitalsB, numberofstatesB_);

  Index levelsA = Range_orbA.second;
  Index levelsB = Range_orbB.second;

  XTP_LOG(Log::error, *pLog_)
      << "Levels:Basis A[" << levelsA << ":" << basisA << "]"
      << " B[" << levelsB << ":" << basisB << "]" << flush;

  if ((levelsA == 0) || (levelsB == 0)) {
    throw std::runtime_error(
        "No information about number of occupied/unoccupied levels is stored");
  }

  // constructing merged orbitals
  auto MOsA = orbitalsA.MOs().eigenvectors().middleCols(Range_orbA.first,
                                                        Range_orbA.second);
  auto MOsB = orbitalsB.MOs().eigenvectors().middleCols(Range_orbB.first,
                                                        Range_orbB.second);

  XTP_LOG(Log::info, *pLog_) << "Calculating overlap matrix for basisset: "
                             << orbitalsAB.getDFTbasisName() << flush;

  Eigen::MatrixXd overlap =
      CalculateOverlapMatrix(orbitalsAB) * orbitalsAB.MOs().eigenvectors();

  XTP_LOG(Log::info, *pLog_)
      << "Projecting monomers onto dimer orbitals" << flush;
  Eigen::MatrixXd A_AB = MOsA.transpose() * overlap.topRows(basisA);
  Eigen::MatrixXd B_AB = MOsB.transpose() * overlap.bottomRows(basisB);
  Eigen::VectorXd mag_A = A_AB.rowwise().squaredNorm();
  if (mag_A.any() < 0.95) {
    XTP_LOG(Log::error, *pLog_)
        << "\nWarning: "
        << "Projection of orbitals of monomer A on dimer is insufficient,mag="
        << mag_A.minCoeff() << flush;
  }
  Eigen::VectorXd mag_B = B_AB.rowwise().squaredNorm();
  if (mag_B.any() < 0.95) {
    XTP_LOG(Log::error, *pLog_)
        << "\nWarning: "
        << "Projection of orbitals of monomer B on dimer is insufficient,mag="
        << mag_B.minCoeff() << flush;
  }

  Eigen::MatrixXd psi_AxB_dimer_basis(A_AB.rows() + B_AB.rows(), A_AB.cols());
  psi_AxB_dimer_basis.topRows(A_AB.rows()) = A_AB;
  psi_AxB_dimer_basis.bottomRows(B_AB.rows()) = B_AB;

  XTP_LOG(Log::info, *pLog_)
      << "Projecting the Fock matrix onto the dimer basis" << flush;
  Eigen::MatrixXd JAB_dimer = psi_AxB_dimer_basis *
                              orbitalsAB.MOs().eigenvalues().asDiagonal() *
                              psi_AxB_dimer_basis.transpose();
  XTP_LOG(Log::info, *pLog_) << "Constructing Overlap matrix" << flush;
  Eigen::MatrixXd S_AxB = psi_AxB_dimer_basis * psi_AxB_dimer_basis.transpose();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(S_AxB);
  Eigen::MatrixXd Sm1 = es.operatorInverseSqrt();
  XTP_LOG(Log::info, *pLog_) << "Smallest eigenvalue of overlap matrix is "
                             << es.eigenvalues()(0) << flush;
  JAB = Sm1 * JAB_dimer * Sm1;
  XTP_LOG(Log::error, *pLog_) << "Done with electronic couplings" << flush;
}

}  // namespace xtp
}  // namespace votca


#include "Elasticity.h"
#include "config.h"
#include "kernels/elasticity/init.h"
#include "kernels/elasticity/kernel.h"
#include "kernels/elasticity/tensor.h"

#include "basis/WarpAndBlend.h"
#include "form/BC.h"
#include "form/DGCurvilinearCommon.h"
#include "form/InverseInequality.h"
#include "form/RefElement.h"
#include "geometry/Curvilinear.h"
#include "quadrules/SimplexQuadratureRule.h"
#include "tensor/EigenMap.h"
#include "util/LinearAllocator.h"
#include "util/Stopwatch.h"

#include <Eigen/LU>
#include <cassert>

namespace tensor = tndm::elasticity::tensor;
namespace init = tndm::elasticity::init;
namespace kernel = tndm::elasticity::kernel;

namespace tndm {

Elasticity::Elasticity(std::shared_ptr<Curvilinear<DomainDimension>> cl, functional_t<1> lam,
                       functional_t<1> mu, std::optional<functional_t<1>> rho, DGMethod method)
    : DGCurvilinearCommon<DomainDimension>(std::move(cl), MinQuadOrder()), method_(method),
      space_(PolynomialDegree, WarpAndBlendFactory<DomainDimension>(), ALIGNMENT),
      materialSpace_(PolynomialDegree, WarpAndBlendFactory<DomainDimension>(), ALIGNMENT),
      fun_lam(make_volume_functional(std::move(lam))),
      fun_mu(make_volume_functional(std::move(mu))),
      fun_rho(rho ? make_volume_functional(std::move(*rho)) : one_volume_function) {

    MhatInv = space_.inverseMassMatrix();
    E_Q = space_.evaluateBasisAt(volRule.points());
    E_Q_T = space_.evaluateBasisAt(volRule.points(), {1, 0});
    Dxi_Q = space_.evaluateGradientAt(volRule.points());
    Dxi_Q_120 = space_.evaluateGradientAt(volRule.points(), {1, 2, 0});

    negative_E_Q_T = Managed<Matrix<double>>(E_Q_T.shape(), std::size_t{ALIGNMENT});
    EigenMap(negative_E_Q_T) = -EigenMap(E_Q_T);

    for (std::size_t f = 0; f < DomainDimension + 1u; ++f) {
        auto points = cl_->facetParam(f, fctRule.points());
        E_q.emplace_back(space_.evaluateBasisAt(points));
        E_q_T.emplace_back(space_.evaluateBasisAt(points, {1, 0}));
        Dxi_q.emplace_back(space_.evaluateGradientAt(points));
        Dxi_q_120.emplace_back(space_.evaluateGradientAt(points, {1, 2, 0}));
        matE_q_T.emplace_back(materialSpace_.evaluateBasisAt(points, {1, 0}));

        negative_E_q.emplace_back(space_.evaluateBasisAt(points));
        auto neg = EigenMap(negative_E_q.back());
        neg = -neg;

        negative_E_q_T.emplace_back(space_.evaluateBasisAt(points, {1, 0}));
        auto negT = EigenMap(negative_E_q_T.back());
        negT = -negT;
    }

    matE_Q_T = materialSpace_.evaluateBasisAt(volRule.points(), {1, 0});
}

void Elasticity::compute_mass_matrix(std::size_t elNo, double* M) const {
    kernel::massMatrix mm;
    mm.E_Q = E_Q.data();
    mm.J = vol[elNo].get<AbsDetJ>().data();
    mm.M = M;
    mm.W = volRule.weights().data();
    mm.execute();
}

void Elasticity::compute_inverse_mass_matrix(std::size_t elNo, double* Minv) const {
    compute_mass_matrix(elNo, Minv);

    using MMat = Eigen::Matrix<double, tensor::M::Shape[0], tensor::M::Shape[1]>;
    using MMap = Eigen::Map<MMat, Eigen::Unaligned,
                            Eigen::OuterStride<init::M::Stop[0] - init::M::Start[0]>>;
    auto Minv_eigen = MMap(Minv);
    auto Minv_lu = Eigen::FullPivLU<MMat>(Minv_eigen);
    Minv_eigen = Minv_lu.inverse();
}

void Elasticity::begin_preparation(std::size_t numElements, std::size_t numLocalElements,
                                   std::size_t numLocalFacets) {
    base::begin_preparation(numElements, numLocalElements, numLocalFacets);

    material.setStorage(
        std::make_shared<material_vol_t>(numElements * materialSpace_.numBasisFunctions()), 0u,
        numElements, materialSpace_.numBasisFunctions());

    volPre.setStorage(std::make_shared<vol_pre_t>(numElements * volRule.size()), 0u, numElements,
                      volRule.size());

    fctPre.setStorage(std::make_shared<fct_pre_t>(numLocalFacets * fctRule.size()), 0u,
                      numLocalFacets, fctRule.size());

    penalty_.resize(numLocalFacets);
    cfl_dt_.resize(numLocalElements, 0.0);
}

void Elasticity::prepare_volume(std::size_t elNo, LinearAllocator<double>& scratch) {
    base::prepare_volume(elNo, scratch);

    alignas(ALIGNMENT) double lam_Q_raw[tensor::lam_Q::size()];
    auto lam_Q = Matrix<double>(lam_Q_raw, 1, volRule.size());
    fun_lam(elNo, lam_Q);

    alignas(ALIGNMENT) double mu_Q_raw[tensor::mu_Q::size()];
    auto mu_Q = Matrix<double>(mu_Q_raw, 1, volRule.size());
    fun_mu(elNo, mu_Q);

    alignas(ALIGNMENT) double rhoInv_Q_raw[tensor::rhoInv_Q::size()];
    auto rhoInv_Q = Matrix<double>(rhoInv_Q_raw, 1, volRule.size());
    fun_rho(elNo, rhoInv_Q);
    for (unsigned q = 0; q < tensor::rhoInv_Q::Shape[0]; ++q) {
        rhoInv_Q(0, q) = 1.0 / rhoInv_Q(0, q);
    }

    alignas(ALIGNMENT) double Mmem[tensor::matM::size()];
    kernel::project_material_lhs krnl_lhs;
    krnl_lhs.matE_Q_T = matE_Q_T.data();
    krnl_lhs.J = vol[elNo].get<AbsDetJ>().data();
    krnl_lhs.matM = Mmem;
    krnl_lhs.W = volRule.weights().data();
    krnl_lhs.execute();

    auto lam_field = material[elNo].get<lam>().data();
    auto mu_field = material[elNo].get<mu>().data();
    auto rhoInv_field = material[elNo].get<rhoInv>().data();
    kernel::project_material_rhs krnl_rhs;
    krnl_rhs.matE_Q_T = matE_Q_T.data();
    krnl_rhs.J = vol[elNo].get<AbsDetJ>().data();
    krnl_rhs.lam = lam_field;
    krnl_rhs.lam_Q = lam_Q_raw;
    krnl_rhs.mu = mu_field;
    krnl_rhs.mu_Q = mu_Q_raw;
    krnl_rhs.rhoInv = rhoInv_field;
    krnl_rhs.rhoInv_Q = rhoInv_Q_raw;
    krnl_rhs.W = volRule.weights().data();
    krnl_rhs.execute();

    using MMap = Eigen::Map<Eigen::Matrix<double, tensor::matM::Shape[0], tensor::matM::Shape[1]>,
                            Eigen::Unaligned,
                            Eigen::OuterStride<init::matM::Stop[0] - init::matM::Start[0]>>;
    using LamMap = Eigen::Map<Eigen::Matrix<double, tensor::lam::Shape[0], 1>, Eigen::Unaligned,
                              Eigen::InnerStride<1>>;
    using MuMap = Eigen::Map<Eigen::Matrix<double, tensor::mu::Shape[0], 1>, Eigen::Unaligned,
                             Eigen::InnerStride<1>>;
    using RhoInvMap = Eigen::Map<Eigen::Matrix<double, tensor::rhoInv::Shape[0], 1>,
                                 Eigen::Unaligned, Eigen::InnerStride<1>>;

    auto proj = MMap(Mmem).fullPivLu();

    auto lam_eigen = LamMap(lam_field);
    lam_eigen = proj.solve(lam_eigen);

    auto mu_eigen = MuMap(mu_field);
    mu_eigen = proj.solve(mu_eigen);

    auto rhoInv_eigen = RhoInvMap(rhoInv_field);
    rhoInv_eigen = proj.solve(rhoInv_eigen);

    auto G_Q = init::G::view::create(vol[elNo].get<JInv>().data()->data());
    auto G_Q_T = init::G_Q_T::view::create(volPre[elNo].get<JInvT>().data()->data());
    for (std::ptrdiff_t q = 0; q < G_Q.shape(2); ++q) {
        for (std::ptrdiff_t i = 0; i < G_Q.shape(0); ++i) {
            for (std::ptrdiff_t j = 0; j < G_Q.shape(1); ++j) {
                G_Q_T(i, j, q) = G_Q(j, i, q);
            }
        }
    }
}

void Elasticity::transpose_JInv(std::size_t fctNo, int side) {
    const auto G_q = (side == 1) ? fct[fctNo].get<JInv1>() : fct[fctNo].get<JInv0>();
    auto G_q_T = (side == 1) ? fctPre[fctNo].get<JInvT1>() : fctPre[fctNo].get<JInvT0>();
    for (std::size_t q = 0; q < fctRule.size(); ++q) {
        for (std::ptrdiff_t i = 0; i < Dim; ++i) {
            for (std::ptrdiff_t j = 0; j < Dim; ++j) {
                G_q_T[q][i + j * Dim] = G_q[q][j + i * Dim];
            }
        }
    }
};

void Elasticity::prepare_skeleton(std::size_t fctNo, FacetInfo const& info,
                                  LinearAllocator<double>& scratch) {
    base::prepare_skeleton(fctNo, info, scratch);

    kernel::precomputeSurface krnl;
    for (unsigned side = 0; side < 2; ++side) {
        krnl.matE_q_T(side) = matE_q_T[info.localNo[side]].data();
    }
    krnl.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
    krnl.lam_q(1) = fctPre[fctNo].get<lam_q_1>().data();
    krnl.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
    krnl.mu_q(1) = fctPre[fctNo].get<mu_q_1>().data();

    for (unsigned side = 0; side < 2; ++side) {
        krnl.lam = material[info.up[side]].get<lam>().data();
        krnl.mu = material[info.up[side]].get<mu>().data();
        krnl.execute(side);
    }

    transpose_JInv(fctNo, 0);
    transpose_JInv(fctNo, 1);
}

void Elasticity::prepare_boundary(std::size_t fctNo, FacetInfo const& info,
                                  LinearAllocator<double>& scratch) {
    base::prepare_boundary(fctNo, info, scratch);

    kernel::precomputeSurface krnl;
    krnl.matE_q_T(0) = matE_q_T[info.localNo[0]].data();
    krnl.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
    krnl.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
    krnl.lam = material[info.up[0]].get<lam>().data();
    krnl.mu = material[info.up[0]].get<mu>().data();
    krnl.execute(0);

    transpose_JInv(fctNo, 0);
}

void Elasticity::prepare_volume_post_skeleton(std::size_t elNo, LinearAllocator<double>& scratch) {
    base::prepare_volume_post_skeleton(elNo, scratch);

    auto lam_field = material[elNo].get<lam>();
    auto mu_field = material[elNo].get<mu>();
    auto rhoInv_field = material[elNo].get<rhoInv>();

    auto J_Q = vol[elNo].get<AbsDetJ>();
    alignas(ALIGNMENT) double Jinv_Q[tensor::Jinv_Q::size()] = {};
    for (unsigned q = 0; q < tensor::Jinv_Q::Shape[0]; ++q) {
        Jinv_Q[q] = 1.0 / J_Q[q];
    }

    kernel::precomputeVolume krnl_pre;
    krnl_pre.matE_Q_T = matE_Q_T.data();
    krnl_pre.J = vol[elNo].get<AbsDetJ>().data();
    krnl_pre.Jinv_Q = Jinv_Q;
    krnl_pre.lam = lam_field.data();
    krnl_pre.lam_W_J_Q = volPre[elNo].template get<lam_W_J_Q>().data();
    krnl_pre.mu = mu_field.data();
    krnl_pre.mu_W_J_Q = volPre[elNo].template get<mu_W_J_Q>().data();
    krnl_pre.rhoInv = rhoInv_field.data();
    krnl_pre.negative_rhoInv_W_Jinv_Q =
        volPre[elNo].template get<negative_rhoInv_W_Jinv_Q>().data();
    krnl_pre.W = volRule.weights().data();
    krnl_pre.execute();
}

std::pair<double, double> Elasticity::stiffness_tensor_bounds(std::size_t elNo) const {
    auto lam_field = material[elNo].get<lam>();
    auto mu_field = material[elNo].get<mu>();
    assert(lam_field.size() == mu_field.size());

    double c0 = std::numeric_limits<double>::max();
    double c1 = std::numeric_limits<double>::lowest();
    for (std::size_t i = 0, n = lam_field.size(); i < n; ++i) {
        c0 = std::min(c0, 2.0 * mu_field[i]);
        c1 = std::max(c1, Dim * lam_field[i] + 2.0 * mu_field[i]);
    }
    return {c0, c1};
}

double Elasticity::inverse_density_upper_bound(std::size_t elNo) const {
    auto rhoInv_field = material[elNo].get<rhoInv>();

    double ir1 = std::numeric_limits<double>::lowest();
    for (std::size_t i = 0, n = rhoInv_field.size(); i < n; ++i) {
        ir1 = std::max(ir1, rhoInv_field[i]);
    }
    return ir1;
}

void Elasticity::prepare_penalty(std::size_t fctNo, FacetInfo const& info,
                                 LinearAllocator<double>&) {
    auto const p = [&](int side) {
        const auto [c0, c1] = stiffness_tensor_bounds(info.up[side]);
        constexpr double c_N_1 = InverseInequality<Dim>::trace_constant(PolynomialDegree - 1);
        return (Dim + 1) * c_N_1 * (area_[fctNo] / volume_[info.up[side]]) * (c1 * c1 / c0);
    };

    if (info.up[0] != info.up[1]) {
        penalty_[fctNo] = (p(0) + p(1)) / 4.0;
    } else {
        penalty_[fctNo] = p(0);
    }
}

void Elasticity::prepare_cfl(std::size_t elNo, mneme::span<SideInfo> info,
                             LinearAllocator<double>&) {
    double l_max = 0.0; // Bound for maximum eigenvalue
    double bnd_area = 0.0;
    for (std::size_t f = 0; f < NumFacets; ++f) {
        const bool is_skeleton_face = elNo != info[f].lid;
        const double gamma = is_skeleton_face ? 2.0 : 1.0;
        const auto fctNo = info[f].fctNo;

        constexpr double c_N = InverseInequality<Dim>::trace_constant(PolynomialDegree);
        l_max += gamma * penalty_[fctNo] * c_N * area_[fctNo] / volume_[elNo];
        bnd_area += area_[fctNo];
    }

    const auto [c0, c1] = stiffness_tensor_bounds(elNo);
    const double ir1 = inverse_density_upper_bound(elNo);
    const double h_1 = bnd_area / volume_[elNo];
    constexpr double C_N = InverseInequality<Dim>::grad_constant(PolynomialDegree);
    l_max += c1 * ir1 * C_N * h_1 * h_1;
    l_max *= 2.0;
    cfl_dt_[elNo] = 1.0 / sqrt(l_max);
}

bool Elasticity::assemble_volume(std::size_t elNo, Matrix<double>& A00,
                                 LinearAllocator<double>& scratch) const {
    alignas(ALIGNMENT) double Dx_Q[tensor::Dx_Q::size()];

    assert(volRule.size() == tensor::W::Shape[0]);
    assert(Dxi_Q.shape(0) == tensor::Dxi_Q::Shape[0]);
    assert(Dxi_Q.shape(1) == tensor::Dxi_Q::Shape[1]);
    assert(Dxi_Q.shape(2) == tensor::Dxi_Q::Shape[2]);

    kernel::Dx_Q dxKrnl;
    dxKrnl.Dx_Q = Dx_Q;
    dxKrnl.Dxi_Q = Dxi_Q.data();
    dxKrnl.G = vol[elNo].get<JInv>().data()->data();
    dxKrnl.execute();

    kernel::assembleVolume krnl;
    krnl.A = A00.data();
    krnl.delta = init::delta::Values;
    krnl.Dx_Q = Dx_Q;
    krnl.lam_W_J_Q = volPre[elNo].get<lam_W_J_Q>().data();
    krnl.mu_W_J_Q = volPre[elNo].get<mu_W_J_Q>().data();
    krnl.execute();
    return true;
}

bool Elasticity::assemble_skeleton(std::size_t fctNo, FacetInfo const& info, Matrix<double>& A00,
                                   Matrix<double>& A01, Matrix<double>& A10, Matrix<double>& A11,
                                   LinearAllocator<double>& scratch) const {
    assert(fctRule.size() == tensor::w::Shape[0]);
    assert(E_q[0].shape(0) == tensor::E_q::Shape[0][0]);
    assert(E_q[0].shape(1) == tensor::E_q::Shape[0][1]);
    assert(Dxi_q[0].shape(0) == tensor::Dxi_q::Shape[0][0]);
    assert(Dxi_q[0].shape(1) == tensor::Dxi_q::Shape[0][1]);
    assert(Dxi_q[0].shape(2) == tensor::Dxi_q::Shape[0][2]);

    alignas(ALIGNMENT) double Dx_q0[tensor::Dx_q::size(0)];
    alignas(ALIGNMENT) double Dx_q1[tensor::Dx_q::size(1)];

    kernel::Dx_q dxKrnl;
    dxKrnl.Dx_q(0) = Dx_q0;
    dxKrnl.Dx_q(1) = Dx_q1;
    dxKrnl.g(0) = fct[fctNo].get<JInv0>().data()->data();
    dxKrnl.g(1) = fct[fctNo].get<JInv1>().data()->data();
    for (unsigned side = 0; side < 2; ++side) {
        dxKrnl.Dxi_q(side) = Dxi_q[info.localNo[side]].data();
        dxKrnl.execute(side);
    }

    alignas(ALIGNMENT) double traction_op_q0[tensor::traction_op_q::size(0)];
    alignas(ALIGNMENT) double traction_op_q1[tensor::traction_op_q::size(1)];

    kernel::assembleTractionOp tOpKrnl;
    tOpKrnl.delta = init::delta::Values;
    tOpKrnl.Dx_q(0) = Dx_q0;
    tOpKrnl.Dx_q(1) = Dx_q1;
    tOpKrnl.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
    tOpKrnl.lam_q(1) = fctPre[fctNo].get<lam_q_1>().data();
    tOpKrnl.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
    tOpKrnl.mu_q(1) = fctPre[fctNo].get<mu_q_1>().data();
    tOpKrnl.n_q = fct[fctNo].get<Normal>().data()->data();
    tOpKrnl.traction_op_q(0) = traction_op_q0;
    tOpKrnl.traction_op_q(1) = traction_op_q1;
    tOpKrnl.execute(0);
    tOpKrnl.execute(1);

    alignas(ALIGNMENT) double L_q0[tensor::L_q::size(0)];
    alignas(ALIGNMENT) double L_q1[tensor::L_q::size(1)];
    auto L_q = std::array<double*, 2>{L_q0, L_q1};

    if (method_ == DGMethod::BR2) {
        alignas(ALIGNMENT) double Lift0[tensor::Lift::size(0)];
        alignas(ALIGNMENT) double Lift1[tensor::Lift::size(1)];
        alignas(ALIGNMENT) double Minv[2][tensor::M::size()];
        for (int i = 0; i < 2; ++i) {
            compute_inverse_mass_matrix(info.up[i], Minv[i]);
        }

        kernel::lift_skeleton lift;
        lift.delta = init::delta::Values;
        lift.Lift(0) = Lift0;
        lift.Lift(1) = Lift1;
        lift.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
        lift.lam_q(1) = fctPre[fctNo].get<lam_q_1>().data();
        lift.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
        lift.mu_q(1) = fctPre[fctNo].get<mu_q_1>().data();
        lift.n_q = fct[fctNo].get<Normal>().data()->data();
        lift.w = fctRule.weights().data();
        for (int i = 0; i < 2; ++i) {
            lift.E_q(i) = E_q[info.localNo[i]].data();
            lift.L_q(i) = L_q[i];
            lift.Minv(i) = Minv[i];
        }
        lift.execute(0);
        lift.execute(1);
    } else { // IP
        kernel::lift_ip lift;
        lift.delta = init::delta::Values;
        lift.nl_q = fct[fctNo].get<NormalLength>().data();
        for (int i = 0; i < 2; ++i) {
            lift.E_q(i) = E_q[info.localNo[i]].data();
            lift.L_q(i) = L_q[i];
        }
        lift.execute(0);
        lift.execute(1);
    }

    kernel::assembleSurface krnl;
    krnl.c00 = -0.5;
    krnl.c01 = -krnl.c00;
    krnl.c10 = epsilon * 0.5;
    krnl.c11 = -krnl.c10;
    krnl.c20 = penalty(fctNo);
    krnl.c21 = -krnl.c20;
    krnl.a(0, 0) = A00.data();
    krnl.a(0, 1) = A01.data();
    krnl.a(1, 0) = A10.data();
    krnl.a(1, 1) = A11.data();
    for (unsigned side = 0; side < 2; ++side) {
        krnl.E_q(side) = E_q[info.localNo[side]].data();
        krnl.L_q(side) = L_q[side];
    }
    krnl.traction_op_q(0) = traction_op_q0;
    krnl.traction_op_q(1) = traction_op_q1;
    krnl.w = fctRule.weights().data();
    krnl.execute(0, 0);
    krnl.execute(0, 1);
    krnl.execute(1, 0);
    krnl.execute(1, 1);

    return true;
}

bool Elasticity::assemble_boundary(std::size_t fctNo, FacetInfo const& info, Matrix<double>& A00,
                                   LinearAllocator<double>& scratch) const {
    if (info.bc == BC::Natural) {
        return false;
    }

    assert(fctRule.size() == tensor::w::Shape[0]);
    assert(E_q[0].shape(0) == tensor::E_q::Shape[0][0]);
    assert(E_q[0].shape(1) == tensor::E_q::Shape[0][1]);
    assert(Dxi_q[0].shape(0) == tensor::Dxi_q::Shape[0][0]);
    assert(Dxi_q[0].shape(1) == tensor::Dxi_q::Shape[0][1]);
    assert(Dxi_q[0].shape(2) == tensor::Dxi_q::Shape[0][2]);

    alignas(ALIGNMENT) double Dx_q0[tensor::Dx_q::size(0)];

    kernel::Dx_q dxKrnl;
    dxKrnl.Dx_q(0) = Dx_q0;
    dxKrnl.g(0) = fct[fctNo].get<JInv0>().data()->data();
    dxKrnl.Dxi_q(0) = Dxi_q[info.localNo[0]].data();
    dxKrnl.execute(0);

    alignas(ALIGNMENT) double traction_op_q0[tensor::traction_op_q::size(0)];

    kernel::assembleTractionOp tOpKrnl;
    tOpKrnl.delta = init::delta::Values;
    tOpKrnl.Dx_q(0) = Dx_q0;
    tOpKrnl.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
    tOpKrnl.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
    tOpKrnl.n_q = fct[fctNo].get<Normal>().data()->data();
    tOpKrnl.traction_op_q(0) = traction_op_q0;
    tOpKrnl.execute(0);

    alignas(ALIGNMENT) double L_q0[tensor::L_q::size(0)];
    if (method_ == DGMethod::BR2) {
        alignas(ALIGNMENT) double Lift0[tensor::Lift::size(0)];
        alignas(ALIGNMENT) double Minv0[tensor::M::size()];
        compute_inverse_mass_matrix(info.up[0], Minv0);

        kernel::lift_boundary lift;
        lift.delta = init::delta::Values;
        lift.Lift(0) = Lift0;
        lift.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
        lift.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
        lift.n_q = fct[fctNo].get<Normal>().data()->data();
        lift.w = fctRule.weights().data();
        lift.E_q(0) = E_q[info.localNo[0]].data();
        lift.L_q(0) = L_q0;
        lift.Minv(0) = Minv0;
        lift.execute();
    } else { // IP
        kernel::lift_ip lift;
        lift.delta = init::delta::Values;
        lift.nl_q = fct[fctNo].get<NormalLength>().data();
        lift.E_q(0) = E_q[info.localNo[0]].data();
        lift.L_q(0) = L_q0;
        lift.execute(0);
    }

    kernel::assembleSurface krnl;
    krnl.c00 = -1.0;
    krnl.c10 = epsilon;
    krnl.c20 = penalty(fctNo);
    krnl.a(0, 0) = A00.data();
    krnl.E_q(0) = E_q[info.localNo[0]].data();
    krnl.L_q(0) = L_q0;
    krnl.traction_op_q(0) = traction_op_q0;
    krnl.w = fctRule.weights().data();
    krnl.execute(0, 0);

    return true;
}

bool Elasticity::rhs_volume(std::size_t elNo, Vector<double>& B,
                            LinearAllocator<double>& scratch) const {
    if (!fun_force) {
        return false;
    }

    assert(tensor::b::Shape[0] == tensor::A::Shape[0]);

    double F_Q_raw[tensor::F_Q::size()];
    assert(tensor::F_Q::Shape[1] == volRule.size());

    auto F_Q = Matrix<double>(F_Q_raw, NumQuantities, volRule.size());
    (*fun_force)(elNo, F_Q);

    kernel::rhsVolume rhs;
    rhs.E_Q = E_Q.data();
    rhs.F_Q = F_Q_raw;
    rhs.J = vol[elNo].get<AbsDetJ>().data();
    rhs.W = volRule.weights().data();
    rhs.b = B.data();
    rhs.execute();
    return true;
}

bool Elasticity::bc_skeleton(std::size_t fctNo, BC bc, double f_q_raw[]) const {
    assert(tensor::f_q::Shape[1] == fctRule.size());
    auto f_q = Matrix<double>(f_q_raw, NumQuantities, fctRule.size());
    if (bc == BC::Fault && fun_slip) {
        (*fun_slip)(fctNo, f_q, false);
    } else if (bc == BC::Dirichlet && fun_dirichlet) {
        (*fun_dirichlet)(fctNo, f_q, false);
    } else {
        return false;
    }
    return true;
}
bool Elasticity::bc_boundary(std::size_t fctNo, BC bc, double f_q_raw[]) const {
    assert(tensor::f_q::Shape[1] == fctRule.size());
    auto f_q = Matrix<double>(f_q_raw, NumQuantities, fctRule.size());
    if (bc == BC::Fault && fun_slip) {
        (*fun_slip)(fctNo, f_q, true);
        for (std::size_t q = 0; q < tensor::f_q::Shape[1]; ++q) {
            for (std::size_t p = 0; p < NumQuantities; ++p) {
                f_q(p, q) *= 0.5;
            }
        }
    } else if (bc == BC::Dirichlet && fun_dirichlet) {
        (*fun_dirichlet)(fctNo, f_q, true);
    } else {
        return false;
    }
    return true;
}

bool Elasticity::rhs_skeleton(std::size_t fctNo, FacetInfo const& info, Vector<double>& B0,
                              Vector<double>& B1, LinearAllocator<double>& scratch) const {
    alignas(ALIGNMENT) double Dx_q[tensor::Dx_q::size(0)];
    alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
    if (!bc_skeleton(fctNo, info.bc, f_q_raw)) {
        return false;
    }

    alignas(ALIGNMENT) double f_lifted_q[tensor::f_lifted_q::size()];
    if (method_ == DGMethod::BR2) {
        alignas(ALIGNMENT) double f_lifted0[tensor::f_lifted::size(0)];
        alignas(ALIGNMENT) double f_lifted1[tensor::f_lifted::size(1)];
        alignas(ALIGNMENT) double Minv[2][tensor::M::size()];
        for (int i = 0; i < 2; ++i) {
            compute_inverse_mass_matrix(info.up[i], Minv[i]);
        }

        kernel::rhs_lift_skeleton lift;
        lift.delta = init::delta::Values;
        lift.f_q = f_q_raw;
        lift.f_lifted(0) = f_lifted0;
        lift.f_lifted(1) = f_lifted1;
        lift.f_lifted_q = f_lifted_q;
        lift.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
        lift.lam_q(1) = fctPre[fctNo].get<lam_q_1>().data();
        lift.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
        lift.mu_q(1) = fctPre[fctNo].get<mu_q_1>().data();
        lift.n_q = fct[fctNo].get<Normal>().data()->data();
        lift.w = fctRule.weights().data();
        for (int i = 0; i < 2; ++i) {
            lift.E_q(i) = E_q[info.localNo[i]].data();
            lift.Minv(i) = Minv[i];
        }
        lift.execute();
    } else { // IP
        kernel::rhs_lift_ip lift;
        lift.f_q = f_q_raw;
        lift.f_lifted_q = f_lifted_q;
        lift.nl_q = fct[fctNo].get<NormalLength>().data();
        lift.execute();
    }

    kernel::rhsFacet rhs;
    rhs.b = B0.data();
    rhs.c10 = 0.5 * epsilon;
    rhs.c20 = penalty(fctNo);
    rhs.Dx_q(0) = Dx_q;
    rhs.Dxi_q(0) = Dxi_q[info.localNo[0]].data();
    rhs.E_q(0) = E_q[info.localNo[0]].data();
    rhs.f_q = f_q_raw;
    rhs.f_lifted_q = f_lifted_q;
    rhs.g(0) = fct[fctNo].get<JInv0>().data()->data();
    rhs.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
    rhs.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
    rhs.n_q = fct[fctNo].get<Normal>().data()->data();
    rhs.w = fctRule.weights().data();
    rhs.execute();

    rhs.b = B1.data();
    rhs.c20 *= -1.0;
    rhs.Dxi_q(0) = Dxi_q[info.localNo[1]].data();
    rhs.E_q(0) = E_q[info.localNo[1]].data();
    rhs.g(0) = fct[fctNo].get<JInv1>().data()->data();
    rhs.lam_q(0) = fctPre[fctNo].get<lam_q_1>().data();
    rhs.mu_q(0) = fctPre[fctNo].get<mu_q_1>().data();
    rhs.execute();

    return true;
}

bool Elasticity::rhs_boundary(std::size_t fctNo, FacetInfo const& info, Vector<double>& B0,
                              LinearAllocator<double>& scratch) const {
    alignas(ALIGNMENT) double Dx_q[tensor::Dx_q::size(0)];
    alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
    if (!bc_boundary(fctNo, info.bc, f_q_raw)) {
        return false;
    }

    alignas(ALIGNMENT) double f_lifted_q[tensor::f_lifted_q::size()];
    if (method_ == DGMethod::BR2) {
        alignas(ALIGNMENT) double f_lifted0[tensor::f_lifted::size(0)];
        alignas(ALIGNMENT) double Minv0[tensor::M::size()];
        compute_inverse_mass_matrix(info.up[0], Minv0);

        kernel::rhs_lift_boundary lift;
        lift.delta = init::delta::Values;
        lift.f_q = f_q_raw;
        lift.f_lifted(0) = f_lifted0;
        lift.f_lifted_q = f_lifted_q;
        lift.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
        lift.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
        lift.n_q = fct[fctNo].get<Normal>().data()->data();
        lift.w = fctRule.weights().data();
        lift.E_q(0) = E_q[info.localNo[0]].data();
        lift.Minv(0) = Minv0;
        lift.execute();
    } else { // IP
        kernel::rhs_lift_ip lift;
        lift.f_q = f_q_raw;
        lift.f_lifted_q = f_lifted_q;
        lift.nl_q = fct[fctNo].get<NormalLength>().data();
        lift.execute();
    }

    kernel::rhsFacet rhs;
    rhs.b = B0.data();
    rhs.c10 = epsilon;
    rhs.c20 = penalty(fctNo);
    rhs.Dx_q(0) = Dx_q;
    rhs.Dxi_q(0) = Dxi_q[info.localNo[0]].data();
    rhs.E_q(0) = E_q[info.localNo[0]].data();
    rhs.f_q = f_q_raw;
    rhs.f_lifted_q = f_lifted_q;
    rhs.g(0) = fct[fctNo].get<JInv0>().data()->data();
    rhs.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
    rhs.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
    rhs.n_q = fct[fctNo].get<Normal>().data()->data();
    rhs.w = fctRule.weights().data();
    rhs.execute();

    return true;
}

template <bool WithRHS>
void Elasticity::apply_(std::size_t elNo, mneme::span<SideInfo> info,
                        Vector<double const> const& x_0,
                        std::array<Vector<double const>, NumFacets> const& x_n,
                        Vector<double>& y_0) const {
    alignas(ALIGNMENT) double Ju_Q[tensor::Ju_Q::size()];
    kernel::apply_volume av;
    av.delta = init::delta::Values;
    av.Dxi_Q = Dxi_Q.data();
    av.Dxi_Q_120 = Dxi_Q_120.data();
    av.Ju_Q = Ju_Q;
    av.G = vol[elNo].get<JInv>().data()->data();
    av.G_Q_T = volPre[elNo].get<JInvT>().data()->data();
    av.lam_W_J_Q = volPre[elNo].get<lam_W_J_Q>().data();
    av.mu_W_J_Q = volPre[elNo].get<mu_W_J_Q>().data();
    av.U = x_0.data();
    av.Unew = y_0.data();
    av.execute();

    alignas(ALIGNMENT) double Ju_q0[tensor::Ju_q::size(0)];
    alignas(ALIGNMENT) double Ju_q1[tensor::Ju_q::size(1)];
    alignas(ALIGNMENT) double n_q_flipped[tensor::n_q::size()];
    alignas(ALIGNMENT) double n_unit_q_flipped[tensor::n_unit_q::size()];
    for (std::size_t f = 0; f < NumFacets; ++f) {
        bool is_skeleton_face = elNo != info[f].lid;
        bool is_fault_or_dirichlet = info[f].bc == BC::Fault || info[f].bc == BC::Dirichlet;

        auto fctNo = info[f].fctNo;
        double const* n_q = fct[fctNo].get<Normal>().data()->data();
        double const* n_unit_q = fct[fctNo].get<UnitNormal>().data()->data();
        double const* lam_q0 = fctPre[fctNo].get<lam_q_0>().data();
        double const* lam_q1 = fctPre[fctNo].get<lam_q_1>().data();
        double const* mu_q0 = fctPre[fctNo].get<mu_q_0>().data();
        double const* mu_q1 = fctPre[fctNo].get<mu_q_1>().data();
        double const* G_q_T0 = fctPre[fctNo].get<JInvT0>().data()->data();
        double const* G_q_T1 = fctPre[fctNo].get<JInvT1>().data()->data();
        if (is_skeleton_face && info[f].side == 1) {
            std::swap(lam_q0, lam_q1);
            std::swap(mu_q0, mu_q1);
            std::swap(G_q_T0, G_q_T1);

            for (int i = 0; i < tensor::n_q::size(); ++i) {
                n_q_flipped[i] = -n_q[i];
            }
            n_q = n_q_flipped;
            for (int i = 0; i < tensor::n_unit_q::size(); ++i) {
                n_unit_q_flipped[i] = -n_unit_q[i];
            }
            n_unit_q = n_unit_q_flipped;
        }

        alignas(ALIGNMENT) double u_hat_minus_u_q[tensor::u_hat_minus_u_q::size()] = {};
        alignas(ALIGNMENT) double sigma_hat_q[tensor::sigma_hat_q::size()] = {};

        if (info[f].bc == BC::None || (is_skeleton_face && is_fault_or_dirichlet)) {
            kernel::flux_u_skeleton fu;
            fu.negative_E_q_T(0) = negative_E_q_T[f].data();
            fu.E_q_T(1) = E_q_T[info[f].localNo].data();
            fu.U = x_0.data();
            fu.U_ext = x_n[f].data();
            fu.u_hat_minus_u_q = u_hat_minus_u_q;
            fu.execute();

            kernel::flux_sigma_skeleton fs;
            fs.c00 = -penalty(fctNo);
            fs.delta = init::delta::Values;
            fs.Dxi_q_120(0) = Dxi_q_120[f].data();
            fs.Dxi_q_120(1) = Dxi_q_120[info[f].localNo].data();
            fs.E_q_T(0) = E_q_T[f].data();
            fs.G_q_T(0) = G_q_T0;
            fs.G_q_T(1) = G_q_T1;
            fs.lam_q(0) = lam_q0;
            fs.lam_q(1) = lam_q1;
            fs.mu_q(0) = mu_q0;
            fs.mu_q(1) = mu_q1;
            fs.negative_E_q_T(1) = negative_E_q_T[info[f].localNo].data();
            fs.U = x_0.data();
            fs.U_ext = x_n[f].data();
            fs.n_unit_q = n_unit_q;
            fs.sigma_hat_q = sigma_hat_q;
            fs.Ju_q(0) = Ju_q0;
            fs.Ju_q(1) = Ju_q1;
            fs.execute();

            if constexpr (WithRHS) {
                alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
                if (bc_skeleton(fctNo, info[f].bc, f_q_raw)) {
                    double sign = info[f].side == 1 ? -1.0 : 1.0;
                    kernel::flux_u_add_bc fub;
                    fub.c00 = 0.5 * sign;
                    fub.f_q = f_q_raw;
                    fub.u_hat_minus_u_q = u_hat_minus_u_q;
                    fub.execute();

                    kernel::flux_sigma_add_bc fsb;
                    fsb.c00 = sign * penalty(fctNo);
                    fsb.f_q = f_q_raw;
                    fsb.n_unit_q = n_unit_q;
                    fsb.sigma_hat_q = sigma_hat_q;
                    fsb.execute();
                }
            }
        } else if (is_fault_or_dirichlet) {
            kernel::flux_u_boundary fu;
            fu.U = x_0.data();
            fu.u_hat_minus_u_q = u_hat_minus_u_q;
            fu.negative_E_q_T(0) = negative_E_q_T[f].data();
            fu.execute();

            kernel::flux_sigma_boundary fs;
            fs.c00 = -penalty(fctNo);
            fs.delta = init::delta::Values;
            fs.Dxi_q_120(0) = Dxi_q_120[f].data();
            fs.G_q_T(0) = G_q_T0;
            fs.E_q_T(0) = E_q_T[f].data();
            fs.lam_q(0) = lam_q0;
            fs.mu_q(0) = mu_q0;
            fs.U = x_0.data();
            fs.n_unit_q = n_unit_q;
            fs.sigma_hat_q = sigma_hat_q;
            fs.Ju_q(0) = Ju_q0;
            fs.execute();

            if constexpr (WithRHS) {
                alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
                if (bc_boundary(fctNo, info[f].bc, f_q_raw)) {
                    kernel::flux_u_add_bc fub;
                    fub.c00 = 1.0;
                    fub.f_q = f_q_raw;
                    fub.u_hat_minus_u_q = u_hat_minus_u_q;
                    fub.execute();

                    kernel::flux_sigma_add_bc fsb;
                    fsb.c00 = penalty(fctNo);
                    fsb.f_q = f_q_raw;
                    fsb.n_unit_q = n_unit_q;
                    fsb.sigma_hat_q = sigma_hat_q;
                    fsb.execute();
                }
            }
        } else {
            continue;
        }

        kernel::apply_facet af;
        af.delta = init::delta::Values;
        af.Dxi_q(0) = Dxi_q[f].data();
        af.negative_E_q(0) = negative_E_q[f].data();
        af.G_q_T(0) = G_q_T0;
        af.lam_q(0) = lam_q0;
        af.mu_q(0) = mu_q0;
        af.n_q = n_q;
        af.sigma_hat_q = sigma_hat_q;
        af.u_hat_minus_u_q = u_hat_minus_u_q;
        af.Unew = y_0.data();
        af.w = fctRule.weights().data();
        af.execute();
    }
}

void Elasticity::apply(std::size_t elNo, mneme::span<SideInfo> info,
                       Vector<double const> const& x_0,
                       std::array<Vector<double const>, NumFacets> const& x_n,
                       Vector<double>& y_0) const {
    apply_<false>(elNo, std::move(info), x_0, x_n, y_0);
}

void Elasticity::wave_rhs(std::size_t elNo, mneme::span<SideInfo> info,
                          Vector<double const> const& x_0,
                          std::array<Vector<double const>, NumFacets> const& x_n,
                          Vector<double>& y_0) const {
    alignas(ALIGNMENT) double rhs_raw[tensor::Unew::size()];
    auto rhs = Vector<double>(rhs_raw, y_0.shape(0));
    apply_<true>(elNo, std::move(info), x_0, x_n, rhs);

    kernel::apply_inverse_mass krnl;
    krnl.E_Q = E_Q.data();
    krnl.MinvRef = MhatInv.data();
    krnl.Jinv_Q = volPre[elNo].get<negative_rhoInv_W_Jinv_Q>().data();
    krnl.U = rhs_raw;
    krnl.Unew = y_0.data();
    krnl.execute();
}

void Elasticity::project(std::size_t elNo, volume_functional_t x, Vector<double>& y) const {
    alignas(ALIGNMENT) double U_Q_raw[tensor::U_Q::size()];
    alignas(ALIGNMENT) double U_raw[tensor::U::size()];

    auto U_Q = Matrix<double>(U_Q_raw, NumQuantities, volRule.size());
    x(elNo, U_Q);

    kernel::project_u_rhs krnl;
    krnl.E_Q = E_Q.data();
    krnl.J = vol[elNo].get<AbsDetJ>().data();
    krnl.U = U_raw;
    krnl.U_Q = U_Q_raw;
    krnl.W = volRule.weights().data();
    krnl.execute();

    auto J_Q = vol[elNo].get<AbsDetJ>();
    auto const& w = volRule.weights();
    alignas(ALIGNMENT) double Jinv_Q[tensor::Jinv_Q::size()] = {};
    for (unsigned q = 0; q < tensor::Jinv_Q::Shape[0]; ++q) {
        Jinv_Q[q] = w[q] / J_Q[q];
    }

    kernel::apply_inverse_mass im;
    im.E_Q = E_Q.data();
    im.MinvRef = MhatInv.data();
    im.Jinv_Q = Jinv_Q;
    im.U = U_raw;
    im.Unew = y.data();
    im.execute();
}

std::size_t Elasticity::flops_apply(std::size_t elNo, mneme::span<SideInfo> info) const {
    std::size_t flops = kernel::apply_volume::HardwareFlops;
    for (std::size_t f = 0; f < NumFacets; ++f) {
        bool is_skeleton_face = elNo != info[f].lid;
        bool is_fault_or_dirichlet = info[f].bc == BC::Fault || info[f].bc == BC::Dirichlet;
        if (info[f].bc == BC::None || (is_skeleton_face && is_fault_or_dirichlet)) {
            flops += kernel::flux_u_skeleton::HardwareFlops;
            flops += kernel::flux_sigma_skeleton::HardwareFlops;
        } else if (is_fault_or_dirichlet) {
            flops += kernel::flux_u_boundary::HardwareFlops;
            flops += kernel::flux_sigma_boundary::HardwareFlops;
        } else {
            continue;
        }
        flops += kernel::apply_facet::HardwareFlops;
    }
    return flops;
}

void Elasticity::coefficients_volume(std::size_t elNo, Matrix<double>& C,
                                     LinearAllocator<double>&) const {
    auto const coeff_lam = material[elNo].get<lam>();
    auto const coeff_mu = material[elNo].get<mu>();
    assert(coeff_lam.size() == C.shape(0));
    assert(coeff_mu.size() == C.shape(0));
    assert(2 == C.shape(1));
    for (std::size_t i = 0; i < C.shape(0); ++i) {
        C(i, 0) = coeff_lam[i];
        C(i, 1) = coeff_mu[i];
    }
}

TensorBase<Matrix<double>> Elasticity::tractionResultInfo() const {
    return TensorBase<Matrix<double>>(tensor::traction_q::Shape[0], tensor::traction_q::Shape[1]);
}

void Elasticity::traction_skeleton(std::size_t fctNo, FacetInfo const& info,
                                   Vector<double const>& u0, Vector<double const>& u1,
                                   Matrix<double>& result) const {
    assert(result.size() == tensor::traction_q::size());
    assert(u0.size() == tensor::u::size(0));
    assert(u1.size() == tensor::u::size(1));

    alignas(ALIGNMENT) double Dx_q0[tensor::Dx_q::size(0)];
    alignas(ALIGNMENT) double Dx_q1[tensor::Dx_q::size(1)];

    kernel::Dx_q dxKrnl;
    dxKrnl.Dx_q(0) = Dx_q0;
    dxKrnl.Dx_q(1) = Dx_q1;
    dxKrnl.g(0) = fct[fctNo].get<JInv0>().data()->data();
    dxKrnl.g(1) = fct[fctNo].get<JInv1>().data()->data();
    for (unsigned side = 0; side < 2; ++side) {
        dxKrnl.Dxi_q(side) = Dxi_q[info.localNo[side]].data();
        dxKrnl.execute(side);
    }

    alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
    bc_skeleton(fctNo, info.bc, f_q_raw);

    kernel::compute_traction krnl;
    krnl.c00 = -penalty(fctNo);
    krnl.Dx_q(0) = Dx_q0;
    krnl.Dx_q(1) = Dx_q1;
    krnl.E_q(0) = E_q[info.localNo[0]].data();
    krnl.E_q(1) = E_q[info.localNo[1]].data();
    krnl.f_q = f_q_raw;
    krnl.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
    krnl.lam_q(1) = fctPre[fctNo].get<lam_q_1>().data();
    krnl.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
    krnl.mu_q(1) = fctPre[fctNo].get<mu_q_1>().data();
    krnl.n_unit_q = fct[fctNo].get<UnitNormal>().data()->data();
    krnl.traction_q = result.data();
    krnl.u(0) = u0.data();
    krnl.u(1) = u1.data();
    krnl.execute();
}

void Elasticity::traction_boundary(std::size_t fctNo, FacetInfo const& info,
                                   Vector<double const>& u0, Matrix<double>& result) const {

    assert(result.size() == tensor::traction_q::size());
    assert(u0.size() == tensor::u::size(0));

    alignas(ALIGNMENT) double Dx_q0[tensor::Dx_q::size(0)];

    kernel::Dx_q dxKrnl;
    dxKrnl.Dx_q(0) = Dx_q0;
    dxKrnl.g(0) = fct[fctNo].get<JInv0>().data()->data();
    dxKrnl.Dxi_q(0) = Dxi_q[info.localNo[0]].data();
    dxKrnl.execute(0);

    alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
    bc_boundary(fctNo, info.bc, f_q_raw);

    kernel::compute_traction_bnd krnl;
    krnl.c00 = -penalty(fctNo);
    krnl.Dx_q(0) = Dx_q0;
    krnl.E_q(0) = E_q[info.localNo[0]].data();
    krnl.f_q = f_q_raw;
    krnl.lam_q(0) = fctPre[fctNo].get<lam_q_0>().data();
    krnl.mu_q(0) = fctPre[fctNo].get<mu_q_0>().data();
    krnl.n_unit_q = fct[fctNo].get<UnitNormal>().data()->data();
    krnl.traction_q = result.data();
    krnl.u(0) = u0.data();
    krnl.execute();
}

} // namespace tndm


/*
	Copyright (C) 2003-2014 by David White <davewx7@gmail.com>

	This software is provided 'as-is', without any express or implied
	warranty. In no event will the authors be held liable for any damages
	arising from the use of this software.

	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:

	   1. The origin of this software must not be misrepresented; you must not
	   claim that you wrote the original software. If you use this software
	   in a product, an acknowledgement in the product documentation would be
	   appreciated but is not required.

	   2. Altered source versions must be plainly marked as such, and must not be
	   misrepresented as being the original software.

	   3. This notice may not be removed or altered from any source
	   distribution.
*/

#pragma once

#include <string>

#include <iosfwd>
#include <boost/cstdint.hpp>
#if defined(TARGET_BLACKBERRY)
#include <math.h>
#endif

static const int64_t DECIMAL_PRECISION = 1000000;
static const int64_t DECIMAL_PLACES = 6;

class decimal
{
public:
	static decimal from_string(const std::string& s);
	static decimal from_int(int v) { return decimal(v); }
	static decimal from_raw_value(int64_t v) { decimal d; d.value_ = v; return d; }
	static decimal epsilon() { return decimal::from_raw_value(static_cast<int64_t>(1)); }
	decimal() : value_(0) {}
	explicit decimal(int value) : value_(static_cast<int64_t>(value)*DECIMAL_PRECISION) {}
#if defined(TARGET_BLACKBERRY)
	explicit decimal(double value) : value_(llround(value*DECIMAL_PRECISION)) {}
#else
	explicit decimal(double value) : value_(static_cast<int64_t>(value*DECIMAL_PRECISION)) {}
#endif

	int64_t value() const { return value_; }
	int as_int() const { return int( value_/DECIMAL_PRECISION ); }
	double as_float() const { return value_/static_cast<double>(DECIMAL_PRECISION); }
	float as_float32() const { return static_cast<float>(as_float()); }
	int64_t fractional() const { return value_%DECIMAL_PRECISION; }

	decimal operator-() const {
		return decimal(from_raw_value(-value_));
	}

	friend decimal operator+(const decimal& a, const decimal& b);
	friend decimal operator-(const decimal& a, const decimal& b);
	friend decimal operator*(const decimal& a, const decimal& b);
	friend decimal operator/(const decimal& a, const decimal& b);

	void operator+=(decimal a) { *this = *this + a; }
	void operator-=(decimal a) { *this = *this - a; }
	void operator*=(decimal a) { *this = *this * a; }
	void operator/=(decimal a) { *this = *this / a; }

	void operator+=(int a) { operator+=(decimal::from_int(a)); }
	void operator-=(int a) { operator-=(decimal::from_int(a)); }
	void operator*=(int a) { operator*=(decimal::from_int(a)); }
	void operator/=(int a) { operator/=(decimal::from_int(a)); }

private:
	int64_t value_;
};

inline decimal operator+(const decimal& a, const decimal& b) {
	return decimal::from_raw_value(a.value() + b.value());
}

inline decimal operator-(const decimal& a, const decimal& b) {
	return decimal::from_raw_value(a.value() - b.value());
}

decimal operator*(const decimal& a, const decimal& b);
decimal operator/(const decimal& a, const decimal& b);

inline bool operator==(const decimal& a, const decimal& b) {
	return a.value() == b.value();
}

inline bool operator!=(const decimal& a, const decimal& b) {
	return !operator==(a, b);
}

inline bool operator<=(const decimal& a, const decimal& b) {
	return a.value() <= b.value();
}

inline bool operator>=(const decimal& a, const decimal& b) {
	return b <= a;
}

inline bool operator<(const decimal& a, const decimal& b) {
	return !(b <= a);
}

inline bool operator>(const decimal& a, const decimal& b) {
	return !(a <= b);
}

inline decimal operator+(decimal a, int b) { return operator+(a, decimal::from_int(b)); }
inline decimal operator-(decimal a, int b) { return operator-(a, decimal::from_int(b)); }
inline decimal operator*(decimal a, int b) { return operator*(a, decimal::from_int(b)); }
inline decimal operator/(decimal a, int b) { return operator/(a, decimal::from_int(b)); }
inline bool operator<(decimal a, int b) { return operator<(a, decimal::from_int(b)); }
inline bool operator>(decimal a, int b) { return operator>(a, decimal::from_int(b)); }
inline bool operator<=(decimal a, int b) { return operator<=(a, decimal::from_int(b)); }
inline bool operator>=(decimal a, int b) { return operator>=(a, decimal::from_int(b)); }

inline decimal operator+(int a, decimal b) { return operator+(decimal::from_int(a), b); }
inline decimal operator-(int a, decimal b) { return operator-(decimal::from_int(a), b); }
inline decimal operator*(int a, decimal b) { return operator*(decimal::from_int(a), b); }
inline decimal operator/(int a, decimal b) { return operator/(decimal::from_int(a), b); }
inline bool operator<(int a, decimal b) { return operator<(decimal::from_int(a), b); }
inline bool operator>(int a, decimal b) { return operator>(decimal::from_int(a), b); }
inline bool operator<=(int a, decimal b) { return operator<=(decimal::from_int(a), b); }
inline bool operator>=(int a, decimal b) { return operator>=(decimal::from_int(a), b); }

std::ostream& operator<<(std::ostream& s, decimal d);