#include <fstream>
#include "sys.h"
#include "math/Mat.h"
#include <Eigen/Dense>

using namespace std;
using namespace nla3d;
using namespace nla3d::math;

const double eps = 0.000001;
uint16 tn = 100;
char* dir;

template<typename M1, typename M2>
bool eigen_compare(const Eigen::MatrixBase<M1>& ref, const Eigen::MatrixBase<M2>& res, double eps = 0.001) {
  assert(ref.rows() == res.rows());
  assert(ref.cols() == res.cols());
  for (uint16 i = 0; i < ref.rows(); i++) {
    for (uint16 j = 0; j < ref.cols(); j++) {
      if (fabs(ref(i,j) - res(i,j)) > eps) {
        LOG(ERROR) << "Matrices are different";
        exit(1);
        return false;
      }
    }
  }
  return true;
}

bool test_matBVprod () {
	std::ifstream in;
	const uint16 _N = 24;
	const uint16 _M = 9;
	char filename[100];
	Mat<_N,_M> B;
	Vec<_M> V;
	Vec<_N> Rf;
	Vec<_N> R;

  Eigen::MatrixXd e_B;
  Eigen::VectorXd e_V;
  Eigen::VectorXd e_R;

  sprintf_s(filename,100,"%s/matBVprod_%02d%02d",dir,_N,_M);
  in.open(filename);
	for (uint16 gg = 1; gg <= tn; gg++) {
		B.zero();
		V.zero();
		R.zero();
		Rf.zero();

		B.simple_read(in);
		V.simple_read(in);
		Rf.simple_read(in);

		matBVprod(B, V, 1.0, R);

    e_B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > (B.ptr(), _N, _M);
    e_V = Eigen::Map<Eigen::VectorXd> (V.ptr(), _M, 1);

    e_R = e_B*e_V;

		if (!R.compare(Rf, eps)) {
			LOG(ERROR) << "test_matBVprod: n = " << gg;
      exit(1);
		}
    eigen_compare(Eigen::Map<Eigen::MatrixXd> (Rf.ptr(),_N,1),e_R);
		DLOG(DEBUG) << "test_matBVprod: case " << gg << " checked successfuly!";
	}
  in.close();
	return true;
}

bool test_matBTVprod () {
	std::ifstream in;
	const uint16 _N = 24;
	const uint16 _M = 9;
	char filename[100];
	Mat<_N,_M> B;
	Vec<_N> V;
	Vec<_M> Rf;
	Vec<_M> R;

  Eigen::MatrixXd e_B;
  Eigen::VectorXd e_V;
  Eigen::VectorXd e_R;

  sprintf_s(filename,100, "%s/matBTVprod_%02d%02d", dir, _N, _M);
  in.open(filename);
	for (uint16 gg = 1; gg <= tn; gg++) {
		B.zero();
		V.zero();
		R.zero();
		Rf.zero();

		B.simple_read(in);
		V.simple_read(in);
		Rf.simple_read(in);

		matBTVprod(B, V, 1.0, R);

    e_B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > (B.ptr(), _N, _M);
    e_V = Eigen::Map<Eigen::VectorXd> (V.ptr(), _N, 1);

    e_R = e_B.transpose()*e_V;

		if (!R.compare(Rf, eps)) {
			LOG(ERROR) << "test_matBTVprod: n = " << gg;
      exit(1);
		}
    eigen_compare(Eigen::Map<Eigen::MatrixXd > (Rf.ptr(),_M,1), e_R);
		DLOG(DEBUG) << "test_matBTVprod: case " << gg << " checked successfuly!";
	}
  in.close();
	return true;
}

bool test_matABprod () {
  std::ifstream in;
	const uint16 _N = 24;
	const uint16 _M = 9;
	const uint16 _M2= 12;
	char filename[100];
	Mat<_N,_M>  A;
	Mat<_M,_M2> B;
	Mat<_N,_M2> R;
	Mat<_N,_M2> Rf;
  Eigen::MatrixXd e_A, e_B, e_R;
  sprintf_s(filename,100, "%s/matABprod_%02d%02d%02d", dir, _N, _M, _M2);
  in.open(filename);
	for (uint16 gg = 1; gg <= tn; gg++) {
		A.zero();
		B.zero();
		R.zero();
		Rf.zero();

		A.simple_read(in);
		B.simple_read(in);
		Rf.simple_read(in);

		matABprod(A, B, 1.0, R);

    e_A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > (A.ptr(), _N, _M);
    e_B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > (B.ptr(), _M, _M2);
    e_R = e_A * e_B;
		if (!R.compare(Rf, eps)) {
			LOG(ERROR) << "test_matABprod: n = " << gg;
      exit(1);
		}
    eigen_compare(Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > (Rf.ptr(),_N, _M2),
       e_R);
		DLOG(DEBUG) << "test_matABprod: case " << gg << " checked successfuly!";
	}
  in.close();
	return true;
}

bool test_matATBprod () {
  std::ifstream in;
	const uint16 _M = 24;
	const uint16 _N = 9;
	const uint16 _N2= 12;
	char filename[100];
	Mat<_M,_N>  A;
	Mat<_M,_N2> B;
	Mat<_N,_N2> R;
	Mat<_N,_N2> Rf;

  Eigen::MatrixXd e_A, e_B, e_R;
  sprintf_s(filename,100, "%s/matATBprod_%02d%02d%02d", dir, _M, _N, _N2);
  in.open(filename);
	for (uint16 gg = 1; gg <= tn; gg++) {
		A.zero();
		B.zero();
		R.zero();
		Rf.zero();

		A.simple_read(in);
		B.simple_read(in);
		Rf.simple_read(in);

		matATBprod(A, B, 1.0, R);

    e_A = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > (A.ptr(), _M, _N);
    e_B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > (B.ptr(), _M, _N2);
    e_R = e_A.transpose() * e_B;
		if (!R.compare(Rf, eps)) {
			LOG(ERROR) << "test_matATBprod: n = " << gg;
      exit(1);
		}
    eigen_compare(Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > (Rf.ptr(),_N, _N2),
       e_R);
		DLOG(DEBUG) << "test_matATBprod: case " << gg << " checked successfuly!";
	}
  in.close();
	return true;
}

bool test_matBTDBprod () {
  std::ifstream in;
	const uint16 _M = 24;
	const uint16 _N = 9;
	char filename[100];
	Mat<_M,_N>  B;
	MatSym<_M> D;
	MatSym<_N> R;
	MatSym<_N> Rf;
  Eigen::MatrixXd e_D, e_B, e_R;
  sprintf_s(filename,100, "%s/matBTDBprod_%02d%02d", dir, _M, _N);
  in.open(filename);
	for (uint16 gg = 1; gg <= tn; gg++) {
		D.zero();
		B.zero();
		R.zero();
		Rf.zero();

		B.simple_read(in);
		D.simple_read(in);
		Rf.simple_read(in);

		matBTDBprod(B, D, 1.0, R);

    e_D.resize(_M,_M);
    uint16 c = 0;
    for (uint16 i = 0; i < _M; i++) {
      for (uint16 j = i; j < _M; j++) {
        e_D(i,j) = D.data[c];
        c++;
      }
    }
    //cout << "e_D = " << endl << e_D << endl;
    //Eigen::MatrixXd tmp = e_D.selfadjointView<Eigen::Upper>();
    //cout << "e_D.symmetric_view = " << endl << tmp;
    e_B = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > (B.ptr(), _M, _N);
    e_R = e_B.transpose() *  e_D.selfadjointView<Eigen::Upper>() * e_B;
    Eigen::MatrixXd e_Rf(_N, _N);
    c = 0;
    for (uint16 i = 0; i < _N; i++) {
      for (uint16 j = i; j < _N; j++) {
        e_Rf(i,j) = Rf.data[c];
        c++;
      }
    }
    e_Rf = e_Rf.selfadjointView<Eigen::Upper>();
    cout << "e_Rf.rows = " << e_Rf.rows() << ",e_Rf.cols = " << e_Rf.cols() << endl;
    cout << "e_R.rows = " << e_R.rows() << ",e_R.cols = " << e_R.cols() << endl;
    eigen_compare(e_Rf, e_R);
		if (!R.compare(Rf, eps)) {
			LOG(ERROR) << "test_matBTDBprod: n = " << gg;
      exit(1);
		}
		DLOG(DEBUG) << "test_matBTDBprod: case " << gg << " checked successfuly!";
	}
  in.close();
	return true;
}

int main (int argc, char* argv[]) {
  char* tmp = getCmdOption(argv, argv + argc, "-dir");
  if (tmp) {
    dir = tmp;
  } else {
    LOG(FATAL) << "You shoud provide directory with test data";
  }

  tmp = getCmdOption(argv, argv + argc, "-num");
  if (tmp) {
    tn = atoi(tmp);
  }
	test_matBVprod();
	test_matBTVprod();
	test_matABprod();
	test_matATBprod();
	test_matBTDBprod();
}


//    boost asinh.hpp header file

//  (C) Copyright Eric Ford & Hubert Holin 2001. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_ASINH_HPP
#define BOOST_ASINH_HPP

#include <cmath>
#include <limits>
#include <string>
#include <stdexcept>

#include <boost/config.hpp>

// This is the inverse of the hyperbolic sine function.

namespace boost
{
    namespace math
    {
#if defined(__GNUC__) && (__GNUC__ < 3)
        // gcc 2.x ignores function scope using declarations,
        // put them in the scope of the enclosing namespace instead:

        using    ::std::abs;
        using    ::std::sqrt;
        using    ::std::log;

        using    ::std::numeric_limits;
#endif

        template<typename T>
        inline T    asinh(const T x)
        {
            using    ::std::abs;
            using    ::std::sqrt;
            using    ::std::log;

            using    ::std::numeric_limits;

            T const            one = static_cast<T>(1);
            T const            two = static_cast<T>(2);

            static T const    taylor_2_bound = sqrt(numeric_limits<T>::epsilon());
            static T const    taylor_n_bound = sqrt(taylor_2_bound);
            static T const    upper_taylor_2_bound = one/taylor_2_bound;
            static T const    upper_taylor_n_bound = one/taylor_n_bound;

            if        (x >= +taylor_n_bound)
            {
                if        (x > upper_taylor_n_bound)
                {
                    if        (x > upper_taylor_2_bound)
                    {
                        // approximation by laurent series in 1/x at 0+ order from -1 to 0
                        return( log( x * two) );
                    }
                    else
                    {
                        // approximation by laurent series in 1/x at 0+ order from -1 to 1
                        return( log( x*two + (one/(x*two)) ) );
                    }
                }
                else
                {
                    return( log( x + sqrt(x*x+one) ) );
                }
            }
            else if    (x <= -taylor_n_bound)
            {
                return(-asinh(-x));
            }
            else
            {
                // approximation by taylor series in x at 0 up to order 2
                T    result = x;

                if    (abs(x) >= taylor_2_bound)
                {
                    T    x3 = x*x*x;

                    // approximation by taylor series in x at 0 up to order 4
                    result -= x3/static_cast<T>(6);
                }

                return(result);
            }
        }
    }
}

#endif /* BOOST_ASINH_HPP */


/**
 * @file solvecauchyproblem.cc
 * @brief NPDE exam problem summer 2019 "CLEmpiricFlux" code
 * @author Oliver Rietmann
 * @date 19.07.2019
 * @copyright Developed at ETH Zurich
 */

#include "solvecauchyproblem.h"

#include <Eigen/Core>
#include <cmath>

#include "uniformcubicspline.h"

namespace CLEmpiricFlux {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector2d findSupport(const UniformCubicSpline &f,
                            Eigen::Vector2d initsupp, double t) {
  Eigen::Vector2d result;
  //====================
  // Your code goes here
  //====================
  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
template <typename FUNCTOR>
Eigen::VectorXd semiDiscreteRhs(const Eigen::VectorXd &mu0, double h,
                                FUNCTOR &&numFlux) {
  int m = mu0.size();
  Eigen::VectorXd mu1(m);
  //====================
  // Your code goes here
  //====================
  return mu1;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
template <typename FUNCTOR>
Eigen::VectorXd RalstonODESolver(FUNCTOR &&rhs, Eigen::VectorXd mu0, double tau,
                                 int n) {
  //====================
  // Your code goes here
  //====================
  return mu0;
}
/* SAM_LISTING_END_3 */

/* SAM_LISTING_BEGIN_4 */
Eigen::VectorXd solveCauchyProblem(const UniformCubicSpline &f,
                                   const Eigen::VectorXd &mu0, double h,
                                   double T) {
  Eigen::VectorXd muT(mu0.size());
  //====================
  // Your code goes here
  //====================
  return muT;
}
/* SAM_LISTING_END_4 */

}  // namespace CLEmpiricFlux


#include <Eigen/Dense>

Eigen::VectorXd  get_transition_probs_byind(Eigen::VectorXi rhovec_inds, double krnl, int ind_current){
  int nrhos = rhovec_inds.rows();
  Eigen::VectorXd  prop_prob(nrhos);
  for(int i=0; i<nrhos; i++){ prop_prob(i) = exp(-krnl*pow((double)rhovec_inds(i)-(double)ind_current,2.0)); }
  prop_prob(ind_current) = 0.0;
  double sumInv = 1.0 / prop_prob.sum();
  prop_prob = sumInv * prop_prob;
  return prop_prob;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestNormalDistribution
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/count_if.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/random/normal_distribution.hpp>
#include <boost/compute/lambda.hpp>

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

#include "context_setup.hpp"

template <class Stats, class T>
boost::accumulators::accumulator_set<T, Stats>
accumulate_statistics(const boost::compute::vector<T> &vector,
                      boost::compute::command_queue &queue)
{
    // copy vector to the host
    std::vector<T, mi_stl_allocator<T>> host_vector(vector.size());
    boost::compute::copy(
        vector.begin(), vector.end(), host_vector.begin(), queue);

    // compute desired statistics and return accumulator object
    return std::for_each(
        host_vector.begin(),
        host_vector.end(),
        boost::accumulators::accumulator_set<T, Stats>());
}

BOOST_AUTO_TEST_CASE(normal_distribution_doctest)
{
    using boost::compute::lambda::_1;

    boost::compute::vector<float> vec(10, context);

    //! [generate]
    // initialize the default random engine
    boost::compute::default_random_engine engine(queue);

    // setup the normal distribution to produce floats centered at 5
    boost::compute::normal_distribution<float> distribution(5.0f, 1.0f);

    // generate the random values and store them to 'vec'
    distribution.generate(vec.begin(), vec.end(), engine, queue);
    //! [generate]
}

BOOST_AUTO_TEST_CASE(normal_distribution_statistics)
{
    // generate normally distributed random numbers
    const size_t n = 10000;
    boost::compute::vector<float> vec(n, context);
    boost::compute::default_random_engine engine(queue);
    boost::compute::normal_distribution<float> distribution(10.0f, 2.0f);
    distribution.generate(vec.begin(), vec.end(), engine, queue);

    // compute mean and standard deviation
    using namespace boost::accumulators;
    accumulator_set<float, stats<tag::variance>> acc =
        accumulate_statistics<stats<tag::variance>>(vec, queue);

    // check mean and standard deviation are what we expect
    BOOST_CHECK_CLOSE(mean(acc), 10.f, 0.5f);
    BOOST_CHECK_CLOSE(std::sqrt(variance(acc)), 2.f, 0.5f);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @author Elie Khoury <Elie.Khoury@idiap.ch>
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 * @date Tue Apr 2 21:08:00 2013 +0200
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <boost/make_shared.hpp>
#include <bob.math/inv.h>
#include <bob.math/lu.h>
#include <bob.math/stats.h>

#include <bob.learn.linear/wccn.h>

namespace bob { namespace learn { namespace linear {

  WCCNTrainer::WCCNTrainer() {
  }

  WCCNTrainer::WCCNTrainer(const WCCNTrainer& other) {
  }

  WCCNTrainer::~WCCNTrainer() {}

  WCCNTrainer& WCCNTrainer::operator= (const WCCNTrainer& other) {
    return *this;
  }

  bool WCCNTrainer::operator== (const WCCNTrainer& other) const {
    return true;
  }

  bool WCCNTrainer::operator!= (const WCCNTrainer& other) const {
    return !(this->operator==(other));
  }

  void WCCNTrainer::train(Machine& machine,
      const std::vector<blitz::Array<double, 2> >& data) const {

    const size_t n_classes = data.size();
    // if #classes < 2, then throw
    if (n_classes < 2) {
      boost::format m("number of classes should be >= 2, but you passed %u");
      m % n_classes;
      throw std::runtime_error(m.str());
    }

    // checks for data type and shape once
    const int n_features = data[0].extent(1);

    for (size_t cl=0; cl<n_classes; ++cl) {
      if (data[cl].extent(1) != n_features) {
        boost::format m("number of features (columns) of array for class %u (%d) does not match that of array for class 0 (%d)");
        m % cl % data[cl].extent(1) % n_features;
        throw std::runtime_error(m.str());
      }
    }

    // machine dimensions
    const size_t n_inputs = machine.inputSize();
    const size_t n_outputs = machine.outputSize();

    // Checks that the dimensions are matching
    if ((int)n_inputs != n_features) {
      boost::format m("machine input size (%u) does not match the number of columns in input array (%d)");
      m % n_inputs % n_features;
      throw std::runtime_error(m.str());
    }
    if ((int)n_outputs != n_features) {
      boost::format m("machine output size (%u) does not match the number of columns in output array (%d)");
      m % n_outputs % n_features;
      throw std::runtime_error(m.str());
    }

    // 1. Computes the mean vector and the Scatter matrix Sw and Sb
    blitz::Array<double,1> mean(n_features);
    blitz::Array<double,2> buf1(n_features, n_features); // Sw
    blitz::Array<double,2> buf2(n_features, n_features); // Sb
    bob::math::scatters(data, buf1, buf2, mean); // buf1 = Sw; buf2 = Sb

    // 2. Computes the inverse of (1/N * Sw), Sw is the within-class covariance matrix
    buf1 /= n_classes;
    bob::math::inv(buf1, buf2); // buf2 = (1/N * Sw)^{-1}

  // 3. Computes the Cholesky decomposition of the inverse covariance matrix
  bob::math::chol(buf2, buf1); //  buf1 = cholesky(buf2)

  // 4. Updates the linear machine
  machine.setInputSubtraction(0); // we do not substract the mean
  machine.setInputDivision(1.);
  machine.setWeights(buf1);
  machine.setBiases(0);
  machine.setActivation(boost::make_shared<bob::learn::activation::IdentityActivation>());

  }

}}}


#pragma once

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

using namespace std;
using namespace Eigen;

template<class T, unsigned int batch_size, unsigned int input_size, unsigned int output_size>
class Layer
{
private:
  // matrix to store outputs of neurons after activation sigma(W*X + B)
  Matrix<T, batch_size, output_size> output_ {};
  // matrix to store outputs of neurons before activation W*X + B
  Matrix<T, batch_size, output_size> unactivated_output_ {};
  // matrix to store weights of the connections  (W)*X + (B)
  Matrix<T, input_size, output_size> weights_ {};
  // matrix to store biases of the layer W*X + (B)
  Matrix<T, 1, output_size> biases_ {};

  // matrix to store errors at intermediate nodes for given input i.e. target activation - current activation
  Matrix<T, batch_size, output_size> deltas_ {};
  // matrix to store gradients w.r.t. weights
  Matrix<T, input_size, output_size> grad_weights_ {};
  // matrix to store gradients w.r.t. biases
  Matrix<T, 1, output_size> grad_biases_ {};

public:
  Layer() {}

  void initializeWeightsRandomly()
  {
    weights_ = Matrix<T, input_size, output_size>::Random();
    biases_ = Matrix<T, 1, output_size>::Random();
  }

  void apply( const Matrix<T, batch_size, input_size>& input )
  {
    unactivated_output_ = ( input * weights_ ).rowwise() + biases_;
    output_ = unactivated_output_.cwiseMax( 0 );
  }

  void apply_without_activation( const Matrix<T, batch_size, input_size>& input )
  {
    unactivated_output_ = ( input * weights_ ).rowwise() + biases_;
    output_ = unactivated_output_;
  }

  void print( const unsigned int layer_num ) const
  {
    const IOFormat CleanFmt( 4, 0, ", ", "\n", "[", "]" );

    cout << "Layer " << layer_num << endl;
    cout << "input_size: " << input_size << " -> "
         << "output_size: " << output_size << endl
         << endl;

    cout << "weights:" << endl << weights_.format( CleanFmt ) << endl << endl;
    cout << "biases:" << endl << biases_.format( CleanFmt ) << endl << endl;
    cout << "unactivated_output:" << endl << unactivated_output_.format( CleanFmt ) << endl << endl;
    cout << "output:" << endl << output_.format( CleanFmt ) << endl << endl;

    cout << "deltas:" << endl << deltas_.format( CleanFmt ) << endl << endl;
    cout << "grad_weights:" << endl << grad_weights_.format( CleanFmt ) << endl << endl;
    cout << "grad_biases:" << endl << grad_biases_.format( CleanFmt ) << endl << endl << endl;
  }

  void perturbWeight( const unsigned int weight_num, const T epsilon )
  {
    const unsigned int i = weight_num / output_size;
    const unsigned int j = weight_num % output_size;
    if ( i < input_size ) {
      weights_( i, j ) += epsilon;
    } else {
      biases_( 0, j ) += epsilon;
    }
  }

  unsigned int getNumParams() const { return ( input_size + 1 ) * output_size; }
  unsigned int getInputSize() const { return input_size; }
  unsigned int getOutputSize() const { return output_size; }

  T getEvaluatedGradient( const unsigned int paramNum )
  {
    const unsigned int i = paramNum / output_size;
    const unsigned int j = paramNum % output_size;
    if ( i < input_size ) {
      return grad_weights_( i, j );
    } else {
      return grad_biases_( 0, j );
    }
  }

  const Matrix<T, batch_size, input_size> computeDeltas( Matrix<T, batch_size, output_size> nextLayerDeltas )
  {
    // activated nodes is the matrix that stores 0/1 corresponding to whether the output node was activated
    Matrix<T, batch_size, output_size> activated_nodes
      = ( unactivated_output_.array() > 0 ).template cast<T>().matrix();
    deltas_ = nextLayerDeltas.cwiseProduct( activated_nodes );
    return deltas_ * weights_.transpose();
  }

  const Matrix<T, batch_size, input_size> computeDeltasLastLayer(
    Matrix<T, batch_size, output_size> nextLayerDeltas )
  {
    deltas_ = nextLayerDeltas;
    return deltas_ * weights_.transpose();
  }

  void evaluateGradients( const Matrix<T, batch_size, input_size>& input )
  {
    grad_weights_ = Matrix<T, input_size, output_size>::Zero();
    // grad_biases_ = Matrix<T, 1, output_size>::Zero();
    for ( unsigned int b = 0; b < batch_size; b++ ) {
      // for ( unsigned int j = 0; j < output_size; j++ ) {
      //   // for ( unsigned int i = 0; i < input_size; i++ ) {
      //   //   grad_weights_( i, j ) += input( b, i ) * deltas_( b, j );
      //   // }
      //   grad_weights_.col( j ) += input.row( b ) * deltas_( b, j );
      // }
      grad_weights_.noalias() += input.row( b ).transpose() * deltas_.row( b );
      // grad_biases_.noalias() += deltas_.row( b );
      // noalias is an eigen optimisation - otherwise becomes slower than for loops
    }
    grad_biases_ = deltas_.colwise().sum();
  }

  const Matrix<T, input_size, output_size>& weights() const { return weights_; }
  const Matrix<T, batch_size, output_size>& output() const { return output_; }
  const Matrix<T, 1, output_size>& biases() const { return biases_; }

  // accessors for mutable access to weights and biases
  Matrix<T, input_size, output_size>& weights() { return weights_; }
  Matrix<T, 1, output_size>& biases() { return biases_; }
};


#include "types.h"
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <Eigen/Geometry>
#include <iostream>

using namespace sdrsac;
namespace py = pybind11;

PYBIND11_MODULE(_kabsch, m) {
    m.def("kabsch", [](const Matrix3X& model, const Matrix3X& target) -> std::pair<Matrix3, Vector3> {
        const auto res = Eigen::umeyama(model, target, false);
        return std::make_pair(res.block<3, 3>(0, 0), res.col(3).head<3>());
    });

#ifdef VERSION_INFO
    m.attr("__version__") = VERSION_INFO;
#else
    m.attr("__version__") = "dev";
#endif
}


#include "ElGamalSignature.h"
#include <ctime>
#include <NTL/BasicThreadPool.h>

using namespace ElGamal;

PublicKey::PublicKey(const ZZ &p, const ZZ &alpha, const ZZ &beta)
        : p(p), alpha(alpha), beta(beta) {}

PrivateKey::PrivateKey(const ZZ &a) : a(a) {
}

ElGamalSignature::ElGamalSignature() : pk(nullptr), sk(nullptr) {
    SetSeed(conv<ZZ>(static_cast<long>(time(nullptr))));
}

void ElGamalSignature::generateKeyPair(int len) {
    ZZ p = findPrime(len);
    ZZ alpha = findPrimitiveRoot(p);
    ZZ a = RandomBnd(p - 2) + 1;//[1,p-1]
    ZZ beta = PowerMod(alpha, a, p);
    pk = new PublicKey(p, alpha, beta);
    sk = new PrivateKey(a);
}

ZZ ElGamalSignature::sig(const ZZ &x, PublicKey *pk, PrivateKey *sk) {
    ZZ k = RandomBnd(pk->p - 3) + 1;//[1,p-2]
    while (GCD(k, pk->p - 1) != 1) {
        k = RandomBnd(pk->p - 3) + 1;
    }
    ZZ gamma = PowerMod(pk->alpha, k, pk->p);
    //delta = (x-a*gamma)*k^(-1) mod (p-1)
    ZZ delta = MulMod(x - sk->a * gamma, InvMod(k, pk->p - 1), pk->p - 1);
    return gamma * pk->p + delta;
}

ZZ ElGamalSignature::sig(const ZZ &x) const {
    return this->sig(x, pk, sk);
}

ZZ ElGamalSignature::sig(const string &x) const {
    return this->sig(stringToNumber(x), pk, sk);
}

bool ElGamalSignature::ver(const ZZ &x, const ZZ &y, PublicKey *pk) {
    ZZ gamma = y / pk->p;
    ZZ delta = y % pk->p;
    //beta^(gamma)*gamma^delta==alpha^x (mod p)
    return (PowerMod(pk->beta, gamma, pk->p) * PowerMod(gamma, delta, pk->p)) % pk->p == PowerMod(pk->alpha, x, pk->p);
}

ElGamalSignature::~ElGamalSignature() {
    delete pk;
    delete sk;
}

PublicKey *ElGamalSignature::getPK() const {
    return pk;
}

PrivateKey *ElGamalSignature::getSK() const {
    return sk;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#ifndef MTL_VECTOR_SECULAR_INCLUDE
#define MTL_VECTOR_SECULAR_INCLUDE

#include <cmath>
#include <boost/utility.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/operation/minimal_increase.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace vector {

/// Class for the secular equation( to solve eigenvalue problems)
template <typename Vector>
class secular_f
{
    typedef typename Collection<Vector>::value_type   value_type;
    typedef typename Collection<Vector>::size_type    size_type;

  public:
    /// Constructor needs 2 Vectors z(nummerator), d(dominator) and sigma as factor before the sum
    secular_f(const Vector& z, const Vector& d, value_type sigma)
      : z(z), d(d), sigma(sigma) {}

    /// secular_f equation as function, evaluates the function value
    /** \f$f(x)=1+\sigma * sum_{i=1}^{n}\frac{z_i}{d_i-x} \f$**/
    value_type f(const value_type& lamb)
    {
	value_type fw= 1;
	for(size_type i=0; i<size(z); i++)
	    fw+= sigma*z[i]*z[i]/(d[i]-lamb);
	return fw;
    }

    value_type square(value_type x) const { return x*x; }

    /// gradient of secular_f equation as function, evaluates the gradientfunction value
    /** \f$gradf(x)=\sigma * sum_{i=1}^{n}\frac{z_i}{(d_i-x)^2} \f$**/
    value_type grad_f(const value_type& lamb)
    {
	value_type gfw= 0.0;
	for(size_type i=0; i<size(z); i++)
	    gfw+= square(z[i] / (d[i] - lamb)); // , std::cout << "gfw = " << gfw << '\n';  //TODO
	return sigma*gfw;
    }

    /// Evaluates the roots of secular_f equation =0 with newton algo.
    /** Computes mixed Newton and interval nesting. d must be sorted. **/
    Vector roots()
    {
	assert(size(z) > 1);
	const double tol= 1.0e-6;
	Vector       start(resource(z)), lambda(resource(z));

	for (size_type i= 0; i < size(z); i++) {
	    // Equal poles -> eigenvalue
	    if (i < size(z) - 1 && d[i] == d[i+1]) {
		lambda[i]= d[i]; continue; }

	    // Check if root is too close to pole (i.e. d[i]+eps > 0) then take this because we can't reach the root
	    value_type next= minimal_increase(d[i]), lamb, old;
	    if (f(next) >= value_type(0)){
		lambda[i]= next; continue; }

	    if (i < size(z) - 1)
		old= lamb= start[i]= (d[i] + d[i+1]) / 2;  //start points between pols
	    else
		old= lamb= start[i]= 1.5 * d[i] - 0.5 * d[i-1];  // last start point plus half the distance to second-last

   	    while (std::abs(f(lamb)) > tol) {
		if (lamb <= d[i])
		    start[i]= lamb= (d[i] + start[i]) / 2;
		else
		    lamb-= f(lamb) / grad_f(lamb);
		if (old == lamb) break;
		old= lamb;
	    }
	    lambda[i]= lamb;
	}
	return lambda;
    }

 private:
    Vector     z, d;
    value_type sigma;
};

template <typename Vector, typename Value>
inline Vector secular(const Vector& z, const Vector& d, Value sigma)
{
	vampir_trace<3030> tracer;
    secular_f<Vector> functor(z, d, sigma);
    return functor.roots();
}

}}// namespace vector

#endif // MTL_VECTOR_SECULAR_INCLUDE


/**
@file SBGATPolyhedronGravityModelUQ.hpp
@class  SBGATPolyhedronGravityModelUQ
@author Benjamin Bercovici
@date January 2019

@brief  Evaluation of the formal uncertainty in the potential (variance), acceleration (covariance) caused by a constant-density polyhedron
@details Computes the potential variance, acceleration covariance associated to the gravity deriving from the polyhedron
 of constant density assuming that the underlying shape vertices are outcomes of a Gaussian distribution
 of known mean and covariance
The input must be a topologically-closed polyhedron.

See Werner, R. A., & Scheeres, D. J. (1997). Exterior gravitation of a polyhedron derived and compared with harmonic and mascon gravitation representations of asteroid 4769 Castalia. Celestial Mechanics and Dynamical Astronomy, 65(3), 313–344. https://doi.org/10.1007/BF00053511
for further details. Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
@copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATPolyhedronGravityModelUQ_hpp
#define SBGATPolyhedronGravityModelUQ_hpp

#include <armadillo>
#include "SBGATMassProperties.hpp"
#include "SBGATPolyhedronGravityModel.hpp"
#include "SBGATMassPropertiesUQ.hpp"

class SBGATPolyhedronGravityModelUQ : public SBGATMassPropertiesUQ {
public:

  /**
  Evaluates the Polyhedron Gravity Model potential variance at the specified point assuming
  a constant density
  @param point pointer to coordinates of queried point, expressed in the same frame as
  the polydata
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  @return PGM potential variance evaluated at the queried point (m ^ 4/ s ^4)
  */
  double GetVariancePotential(double const * point,bool hold_mass_constant = false) const;

  /**
  Evaluates the Polyhedron Gravity Model potential variance at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  @return PGM potential variance evaluated at the queried point (m ^ 4 / s ^4)
  */
  double GetVariancePotential(const arma::vec::fixed<3> & point,bool hold_mass_constant = false) const;

  /**
  Evaluates the Polyhedron Gravity Model potential variance and acceleration covariance at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata used to construct the PGM
  @param[out] potential_var PGM potential variance evaluated at the queried point (m ^ 4 / s ^4)
  @param[out] acc_cov PGM acceleration covariance evaluated at the queried point (m^2 / s ^4)
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  */
  void GetVariancePotentialAccelerationCovariance(double const * point,double & potential_var,
    arma::mat::fixed<3,3> & acc_cov,bool hold_mass_constant = false) const;

  /**
  Return the variance of the slope evaluated at the center of the designated facet. This method is NOT thread safe
  @param[in] f facet index
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  @return variance in slope (deg^2)
  */
  double GetVarianceSlope(const unsigned int & f , bool hold_mass_constant = false);

  /**
  Return the variance of the slope evaluated at the center of the designated facets. This method is NOT thread safe (i.e should not be called from multiple threads).
  However, it internaly relies on OpenMP to speed up computations
  @param[out] slope_variances (deg^2)
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  @param[in] facets indices of facets where to evaluate the slope variance
  */
  void GetVarianceSlopes(std::vector<double> & slope_variances,const std::vector<unsigned int> & facets,bool hold_mass_constant = false);

  /**
  Evaluates the Polyhedron Gravity Model potential variance and acceleration covariance at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata used to construct the PGM
  @param[out] potential_var PGM potential variance evaluated at the queried point (m ^ 4 / s ^4)
  @param[out] acc_cov PGM acceleration covariance evaluated at the queried point (m^2 / s ^4)
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  */
  void GetVariancePotentialAccelerationCovariance(const arma::vec::fixed<3> & point,double & potential_var,
    arma::mat::fixed<3,3> & acc_cov,bool hold_mass_constant = false) const;

  /**
  Runs a finite-differencing based test of the implemented PGM partials
  @param input path to obj file used to test the partials
  @param tol relative tolerance
  */
  static void TestPartials(std::string input , double tol, bool shape_in_meters);

  /**
  Obtain the partial derivative of the potential at the prescribed location
  due to a infinitesimal variation in the shape's control points
  @param[in] pos position where to evaluate the partial derivative
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  @return partial derivative of the potential with respect to the variation in the shape's control points
  */

  arma::rowvec GetPartialUPartialC(const arma::vec::fixed<3> & pos,bool hold_mass_constant = false) const;

  /**
  Obtain the partial derivative of the acceleration at the prescribed location
  due to a infinitesimal variation in the shape's control points
  @param[in] pos position where to evaluate the partial derivative
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false

  @return partial derivative of the acceleration with respect to the variation in the shape's control points
  */
  arma::mat GetPartialAPartialC(const arma::vec::fixed<3> & pos,bool hold_mass_constant = false) const;

  /**
  Get covariance in acceleration arising from the uncertain shape
  @param[in] point coordinates where to evaluate the covariance
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  @return covariance of acceleration
  */
  arma::mat::fixed<3,3> GetCovarianceAcceleration(double const * point,bool hold_mass_constant = false) const;

   /**
  Get covariance in acceleration arising from the uncertain shape
  @param[in] point coordinates where to evaluate the covariance
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false

  @return covariance of acceleration
  */
  arma::mat::fixed<3,3> GetCovarianceAcceleration(const arma::vec::fixed<3> & point,bool hold_mass_constant = false) const;

  /**
  Runs a Monte Carlo on the shape and samples accelerations & potentials at the provided positions
  @param[in] path_to_shape path to reference shape
  @param[in] density small body density in kg/m^3
  @param[in] shape_in_meters true if reference shape has its units expressed in meters, false otherwise
  @param[in] hold_mass_constant true if shape mass must be held constant by making density vary (mass = rho * V), false otherwise
  @param[in] C_CC square root of the covariance of the shape vertices coordinates. Must be of dimensions (3N_C * 3N_C) where N_C is the
  number of vertices in the reference shape
  @param[in] N_samples number of shape outcomes to draw
  @param[in] all_positions vector storing all the position where acceleration & potential must be sampled
  @param[in] output_dir path ending in "/" where to save shape-related monte-carlo data. Only used
  if last argument is larger than 0
  @param[in] N_saved_shapes number of shape outcomes to save. must be lesser or equal than N_samples
  @param[out] deviations holds N_samples 3*N_C column vectors storing the deviation applied to the coordinates of the
  reference shape at every sample
  @param[out] densities holds N_samples density samples. If hold_mass_constant is true, then the density samples
  will vary accordingly to the constant mass constraint
  @param[out] all_accelerations holds N_samples vectors, each storing the acceleration evaluated at the specified points
  @param[out] all_potentials holds N_samples vectors, each storing the potential evaluated at the specified points
  */

  static void RunMCUQPotentialAccelerationInertial(std::string path_to_shape,
    const double & density,
    const bool & shape_in_meters,
    const bool & hold_mass_constant,
    const arma::mat & C_CC,
    const unsigned int & N_samples,
    const std::vector<arma::vec::fixed<3> > & all_positions,
    std::string output_dir,
    int N_saved_shapes,
    std::vector<arma::vec> & deviations,
    std::vector<double> & densities,
    std::vector<std::vector<arma::vec::fixed<3> >> & all_accelerations,
    std::vector < std::vector<double> > & all_potentials );

/**
  Runs a Monte Carlo on the shape and samples accelerations at the provided positions
  @param[in] path_to_shape path to reference shape
  @param[in] density small body density in kg/m^3
  @param[in] shape_in_meters true if reference shape has its units expressed in meters, false otherwise
  @param[in] hold_mass_constant true if shape mass must be held constant by making density vary (mass = rho * V), false otherwise
  @param[in] C_CC square root of the covariance of the shape vertices coordinates. Must be of dimensions (3N_C * 3N_C) where N_C is the
  number of vertices in the reference shape
  @param[in] N_samples number of shape outcomes to draw
  @param[in] all_positions vector storing all the position where acceleration & potential must be sampled
  @param[in] output_dir path ending in "/" where to save shape-related monte-carlo data. Only used
  if last argument is larger than 0
  @param[in] N_saved_shapes number of shape outcomes to save. must be lesser or equal than N_samples
  @param[out] deviations holds N_samples 3*N_C column vectors storing the deviation applied to the coordinates of the
  reference shape at every sample
  @param[out] densities holds N_samples density samples. If hold_mass_constant is true, then the density samples
  will vary accordingly to the constant mass constraint
  @param[out] all_accelerations holds N_samples vectors, each storing the acceleration evaluated at the specified points
  */

  static void RunMCUQAccelerationInertial(std::string path_to_shape,
    const double & density,
    const bool & shape_in_meters,
    const bool & hold_mass_constant,
    const arma::mat & C_CC,
    const unsigned int & N_samples,
    const std::vector<arma::vec::fixed<3> > & all_positions,
    std::string output_dir,
    int N_saved_shapes,
    std::vector<arma::vec> & deviations,
    std::vector<double> & densities,
    std::vector<std::vector<arma::vec::fixed<3> >> & all_accelerations);

  /**
  Runs a Monte Carlo on the shape and samples the slopes at the provided facets
  @param[in] path_to_shape path to reference shape
  @param[in] density small body density in kg/m^3
  @param[in] Omega angular velocity of small body in kg/m^3, expressed in the small body frame
  @param[in] shape_in_meters true if reference shape has its units expressed in meters, false otherwise
  @param[in] hold_mass_constant true if shape mass must be held constant by making density vary (mass = rho * V), false otherwise
  @param[in] C_CC square root of the covariance of the shape vertices coordinates. Must be of dimensions (3N_C * 3N_C) where N_C is the
  number of vertices in the reference shape
  @param[in] period_standard_deviation standard deviation of the rotation period in seconds
  @param[in] N_samples number of shape outcomes to draw
  @param[in] all_facets vector storing all the facet indices where the gravitational slopes must be sampled
  @param[in] output_dir path ending in "/" where to save shape-related monte-carlo data. Only used
  if last argument is larger than 0
  @param[in] N_saved_shapes number of shape outcomes to save. must be lesser or equal than N_samples
  @param[out] deviations holds N_samples 3*N_C column vectors storing the deviation applied to the coordinates of the
  reference shape at every sample
  @param[out] densities holds N_samples density samples. If hold_mass_constant is true, then the density samples
  will vary accordingly to the constant mass constraint
  @param[out] period_errors holds N_samples of the error on the rotation period

  @param[out] all_slopes holds N_samples vectors, each storing the slopes evaluated at the specified facets
  */

  static void RunMCUQSlopes(std::string path_to_shape,
    const double & density,
    const arma::vec::fixed<3> & Omega,
    const bool & shape_in_meters,
    const bool & hold_mass_constant,
    const arma::mat & C_CC,
    const double & period_standard_deviation,
    const unsigned int & N_samples,
    const std::vector<unsigned int > & all_facets,
    std::string output_dir,
    int N_saved_shapes,
    std::vector<arma::vec> & deviations,
    std::vector<double> & densities,
    std::vector<double> & period_errors,
    std::vector < std::vector<double> > & all_slopes );

  /**
  Sets the standard deviation of the rotation period
  @param standard deviation of the rotation period (s)
  */
  void SetPeriodErrorStandardDeviation(double rotation_period_sd){
    this -> period_standard_deviation = rotation_period_sd;
  }

  /**
  Return the partial derivative of the slope at the center of facet f relative to
  the angular velocity magnitude and shape vertices coordinates
  @param[in] f facet index
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant
  @return partials
  */
  arma::rowvec GetPartialSlopePartialwPartialC(const int & f,bool hold_mass_constant = false) const;

  /**
  Applies prescribed deviation to all the N_vertices control points and updates model
  @param delta_C deviation (3 * N_vertices x 1)
  */
  virtual void ApplyDeviation(const arma::vec & delta_C);

  /**
  Return the partial derivative of the gravitation slope at the center of face tf relative to
  the shape vertices coordinates
  @param[in] f facet index
  @return partial derivative of the slope at the center of facet f relative to the shape vertices coordinates
  */

  arma::rowvec GetPartialSlopePartialC(const int & f) const;

  /**
  Runs a Monte Carlo on the shape and samples inertial accelerations & potentials at the provided position
  @param[in] path_to_shape path to reference shape
  @param[in] density small body density in kg/m^3
  @param[in] shape_in_meters true if reference shape has its units expressed in meters, false otherwise
  @param[in] hold_mass_constant true if shape mass must be held constant by making density vary (mass = rho * V), false otherwise
  @param[in] C_CC square root of the covariance of the shape vertices coordinates. Must be of dimensions (3N_C * 3N_C) where N_C is the
  number of vertices in the reference shape
  @param[in] N_samples number of shape outcomes to draw
  @param[in] position the position where acceleration & potential must be sampled
  @param[in] output_dir path ending in "/" where to save shape-related monte-carlo data. Only used
  if last argument is larger than 0
  @param[in] N_saved_shapes number of shape outcomes to save. must be lesser or equal than N_samples
  @param[out] deviations holds N_samples 3*N_C column vectors storing the deviation applied to the coordinates of the
  reference shape at every sample
  @param[out] densities holds N_samples density samples. If hold_mass_constant is true, then the density samples
  will vary accordingly to the constant mass constraint
  @param[out] accelerations holds N_samples accelerations evaluated at the specified point
  @param[out] potentials holds N_samples potential evaluated at the specified point
  */
  static void RunMCUQPotentialAccelerationInertial(std::string path_to_shape,
    const double & density,
    const bool & shape_in_meters,
    const bool & hold_mass_constant,
    const arma::mat & C_CC,
    const unsigned int & N_samples,
    const arma::vec::fixed<3> & position,
    std::string output_dir,
    int N_saved_shapes,
    std::vector<arma::vec> & deviations,
    std::vector<double> & densities,
    std::vector<arma::vec::fixed<3> > & accelerations,
    std::vector<double> & potentials);

/**
  Runs a Monte Carlo on the shape and samples inertial accelerations at the provided position
  @param[in] path_to_shape path to reference shape
  @param[in] density small body density in kg/m^3
  @param[in] shape_in_meters true if reference shape has its units expressed in meters, false otherwise
  @param[in] hold_mass_constant true if shape mass must be held constant by making density vary (mass = rho * V), false otherwise
  @param[in] C_CC square root of the covariance of the shape vertices coordinates. Must be of dimensions (3N_C * 3N_C) where N_C is the
  number of vertices in the reference shape
  @param[in] N_samples number of shape outcomes to draw
  @param[in] position the position where acceleration & potential must be sampled
  @param[in] output_dir path ending in "/" where to save shape-related monte-carlo data. Only used
  if last argument is larger than 0
  @param[in] N_saved_shapes number of shape outcomes to save. must be lesser or equal than N_samples
  @param[out] deviations holds N_samples 3*N_C column vectors storing the deviation applied to the coordinates of the
  reference shape at every sample
  @param[out] densities holds N_samples density samples. If hold_mass_constant is true, then the density samples
  will vary accordingly to the constant mass constraint
  @param[out] accelerations holds N_samples accelerations evaluated at the specified point
  */
  static void RunMCUQAccelerationInertial(std::string path_to_shape,
    const double & density,
    const bool & shape_in_meters,
    const bool & hold_mass_constant,
    const arma::mat & C_CC,
    const unsigned int & N_samples,
    const arma::vec::fixed<3> & position,
    std::string output_dir,
    int N_saved_shapes,
    std::vector<arma::vec> & deviations,
    std::vector<double> & densities,
    std::vector<arma::vec::fixed<3> > & accelerations);

  /**
  Runs a Monte Carlo on the shape and samples the gravitational slopes at the provided facets
  @param[in] path_to_shape path to reference shape
  @param[in] density small body density in kg/m^3
  @param[in] Omega angular velocity of small body in kg/m^3, expressed in the small body frame
  @param[in] shape_in_meters true if reference shape has its units expressed in meters, false otherwise
  @param[in] hold_mass_constant true if shape mass must be held constant by making density vary (mass = rho * V), false otherwise
  @param[in] C_CC square root of the covariance of the shape vertices coordinates. Must be of dimensions (3N_C * 3N_C) where N_C is the
  number of vertices in the reference shape
  @param[in] period_standard_deviation standard deviation of the rotation period in seconds
  @param[in] N_samples number of shape outcomes to draw
  @param[in] facet index of the facet where the surface pgm must be sampled
  @param[in] output_dir path ending in "/" where to save shape-related monte-carlo data. Only used
  if last argument is larger than 0
  @param[in] N_saved_shapes number of shape outcomes to save. must be lesser or equal than N_samples
  @param[out] deviations holds N_samples 3*N_C column vectors storing the deviation applied to the coordinates of the
  reference shape at every sample
  @param[out] densities holds N_samples density samples. If hold_mass_constant is true, then the density samples
  will vary accordingly to the constant mass constraint
  @param[out] period_errors holds N_samples of the error on the rotation period
  @param[out] slopes holds N_samples slopes evaluated at the specified facet
  */
  static void RunMCUQSlopes(std::string path_to_shape,
    const double & density,
    const arma::vec::fixed<3> & Omega,
    const bool & shape_in_meters,
    const bool & hold_mass_constant,
    const arma::mat & C_CC,
    const double & period_standard_deviation,
    const unsigned int & N_samples,
    const unsigned int & facet,
    std::string output_dir,
    int N_saved_shapes,
    std::vector<arma::vec> & deviations,
    std::vector<double> & densities,
    std::vector<double> & period_errors,
    std::vector<double> & slopes);

protected:

  arma::vec GetBe() const;

  /**
  Get partial derivative of the angular velocity vector relative to 1) the angular velocity magnitude 2) the shape vertices
  coordinates
  @return partial derivative of Omega relative to its magnitude shape vertices coordinates
  */
  arma::mat PartialOmegaPartialwC() const;

  /**
  Return the partial derivative of the body-fixed angular velocity and the vertices coordinates relative
  to the angular velocity magnitude and shape vertices coordinates
  @return partial
  */
  arma::sp_mat PartialOmegaCPartialwC() const;

/**
Return the partial derivative of the body-fixed acceleration at the center of facet f relative
to the shape coordinates
@param[in] f facet index
@param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
@return partial derivative
*/
  arma::mat PartialBodyFixedAccelerationfPartialC(const int & f,bool hold_mass_constant = false) const;

  /**
  Return the partial derivative of the body-fixed acceleration at the center of facet f with respect to
  the angular velocity and vertices coordinates
  @param[in] f facet index
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  @return partial derivative
  */
  arma::mat PartialBodyFixedAccelerationfPartialOmegaC(const int & f,bool hold_mass_constant = false) const;

  arma::mat::fixed<3,3> PartialBodyFixedAccelerationfPartialOmega(const int & f) const;

  /**
  Adds to the properly initialized vector the partial derivative of the sum of all Ue
  @param[in] pos position where to evaluate the partials
  @param[out] partial partial derivative being evaluated
  */
  void AddPartialSumUePartialC(const arma::vec::fixed<3> & pos,arma::rowvec & partial) const;

   /**
  Add to the properly initialized vector the partial derivative of the sum of all Uf
  @param[in] pos position where to evaluate the partials
  @param[out] partial partial derivative being evaluated
  */
  void AddPartialSumUfPartialC(const arma::vec::fixed<3> & pos,arma::rowvec & partial) const;

  /**
  Adds to the properly initialized vector the partial derivative of the sum of all Acce
  @param[in] pos position where to evaluate the partials
  @param[out] partial partial derivative being evaluated
  */
  void AddPartialSumAccePartialC(const arma::vec::fixed<3> & pos,arma::mat & partial) const;

   /**
  Add to the properly initialized vector the partial derivative of the sum of all Accf
  @param[in] pos position where to evaluate the partials
  @param[out] partial partial derivative being evaluated
  */
  void AddPartialSumAccfPartialC(const arma::vec::fixed<3> & pos,arma::mat & partial) const;

  /**
  Applies deviation to the coordinates of the vertices on the prescribed edge
  and updates the pgm
  @param delta_Ae deviation
  @param e edge index
  */
  void ApplyAeDeviation(arma::vec::fixed<6> delta_Ae,const int & e);

  /**
  Applies deviation to the coordinates of the vertices in the prescribed facet
  and updates the pgm
  @param delta_Tf deviation
  @param[in] f facet index
  */
  void ApplyTfDeviation(arma::vec::fixed<9> delta_Tf,const int & f);

  /**
  Return the partial derivative of an individual edge contribution to the potential (Ue)
  with respect to the Xe^E vector holding the e-th edge dyadic factors
  @param[in] pos position where to evaluate the partial
  @param e edge index
  @return PartialUePartialXe (1x10)
  */
  arma::rowvec::fixed<10> PartialUePartialXe(const arma::vec::fixed<3> & pos,const int & e) const;

  /**
  Return the partial derivative of an individual facet contribution to the potential (Uf)
  with respect to the Xf^F vector holding the f-th facet dyadic factors
  @param[in] pos position where to evaluate the partial
  @param[in] f facet index
  @return PartialUfPartialXf (1x10)

  */
  arma::rowvec::fixed<10> PartialUfPartialXf(const arma::vec::fixed<3> & pos,
    const int & f) const;

  /**
  Return the partial derivative of an individual edge contribution to the acceleration (Acce)
  with respect to the Xe^E vector holding the e-th edge dyadic factors
  @param[in] pos position where to evaluate the partial
  @param e edge index
  @return PartialAccePartialXe (3x10)
  */
  arma::mat::fixed<3,10> PartialAccePartialXe(const arma::vec::fixed<3> & pos,const int & e) const;

  /**
  Return the partial derivative of an individual facet contribution to the acceleration (Accf)
  with respect to the Xf^F vector holding the f-th facet dyadic factors
  @param[in] pos position where to evaluate the partial
  @param[in] f facet index
  @return PartialAccfPartialXf (3x10)
  */
  arma::mat::fixed<3,10> PartialAccfPartialXf(const arma::vec::fixed<3> & pos,const int & f) const;

  /**
  Return the partial derivative of Xf^F, the vector holding the f-th facet dyadic factors,
  with respect to the vertices coordiantes constitutive of the f-th triangle (Tf)
  @param[in] pos position where to evaluate the partial
  @param[in] f facet index
  @return PartialXfPartialTf (10x9)
  */
  arma::mat::fixed<10,9> PartialXfPartialTf(const arma::vec::fixed<3> & pos, const int & f) const;

  /**
  Return the partial derivative of the performance factor omega_f
  with respect to the vertices coordiantes constitutive of the f-th triangle (Tf)
  @param[in] pos position where to evaluate the partial
  @param[in] f facet index
  @return PartialOmegafPartialTf (1x9)
  */
  arma::rowvec::fixed<9> PartialOmegafPartialTf(const arma::vec::fixed<3> & pos,const int & f) const;

  /**
  Return the partial derivative of Z_f = (alpha_f,gamma_f)^T (as in wf = 2 * arctan2(Z_f) )
  with respect to the unit vectors from the field point to the facet vertices
  @param UnitRf 3 unit vectors stacked up
  @return PartialZfPartialUnitRf (2x9)
  */
  static arma::mat::fixed<2,9> PartialZfPartialUnitRf(const arma::vec::fixed<9> & UnitRf);

  /**
  Return the partial derivative of arctan2(Z_f) w/r to Z_f
  with respect to the unit vectors from the field point to the facet vertices
  @param Zf
  @return PartialAtan2PartialZf (1x2)
  */
  static arma::rowvec::fixed<2> PartialAtan2PartialZf(const arma::vec::fixed<2> & Zf);

  /**
  Return the partial derivative of arctan(y/x)
  @param xy input
  @return partial derivative
  */
  static arma::rowvec::fixed<2> PartialOmegafPartialXY(const arma::vec::fixed<2> & xy);

  /**
  Return the partial derivative of the facet dyad parametrization (Ff)
  with respect to the vertices coordinates constitutive of the f-th triangle (Tf)
  @param[in] f facet index
  @return PartialFfPartialTf (6x9)
  */
  arma::mat::fixed<6,9> PartialFfPartialTf(const int & f) const;

  /**
  Return the partial derivative of a normalized vector n relative to the non-normalized
  vector N such that n = N / || N ||
  @param non_normalized_V non-normalized vector used to produce the normalized vector
  @return PartialNormalizedVPartialNonNormalizedV (3x3)
  */
  static arma::mat::fixed<3,3> PartialNormalizedVPartialNonNormalizedV(const arma::vec::fixed<3> & non_normalized_V);

  /**
  Return the partial derivative of the f-th facet dyad parametrization with respect to the
  normalized normal of the f-th facet
  @param nf facet normal
  @return PartialFfPartialnf (6x3)

  */
  static arma::mat::fixed<6,3> PartialFfPartialnf(const arma::vec::fixed<3> & nf);

  /**
  Return the partial derivative of the wire potential Le
  with respect to the coordinates of the two vertices forming the edge (stacked in Ae)
  @param[in] pos position where to evaluate the partial
  @param e edge index
  @return PartialLePartialAe (1x6)
  */
  arma::rowvec::fixed<6> PartialLePartialAe(const arma::vec::fixed<3> & pos,const int & e) const;

  /**
  Return the partial derivative of field-point to edge-point vector
  with respect to the coordinates of the two vertices forming the edge (stacked in Ae)
  @return PartialRadiusEePartialAe (3x6)
  */
  arma::mat::fixed<3,6> PartialRadiusEePartialAe() const;

  /**
  Return the partial derivative of field-point to facet-point vector
  with respect to the coordinates of the three vertices forming the facet (stacked in Tf)
  @return PartialRadiusFfPartialTf (3x9)
  */
  arma::mat::fixed<3,9> PartialRadiusFfPartialTf() const;

  /**
  Return the partial derivative of the parametrization of the Xe dyadic vector
  with respect to the coordinates of the edges points and adjacent facets points
  @param[in] pos position where to evaluate the partial
  @param e edge index
  @return PartialXePartialBe (10x24)
  */
  arma::mat::fixed<10,24> PartialXePartialBe(const arma::vec::fixed<3> & pos,const int & e) const;

  /**
  Return the partial derivative of the edge length le
  with respect to the coordinates of the edges points
  @param e edge index
  @return PartialEdgeLengthPartialAe (10x24)
  */
  arma::rowvec::fixed<6> PartialEdgeLengthPartialAe(const int & e) const;

  /**
  Return the partial derivative of the (q,r) component of the Ee dyad with respect to the
  with respect to the coordinates of the edges points and adjacent facets points
  @param e edge index
  @param q row index
  @param r col index
  @return PartialEqrPartialBe (1x24)
  */
  arma::rowvec::fixed<24> PartialEqrPartialBe(const int & e,const int & q,const int & r) const;

  /**
  Return the partial derivative of the f-th facet slope argument (u as in slope = arcos(-u))
  with respect to the angular velocity and the shape coordinates
  @param[in] f facet index
  @param body_fixed_acc body-fixed acceleration at the center of facet f
  @param[in] hold_mass_constant if true, will make density vary as in drho = - rho V / dV so as to hold mass (mass = rho * V) constant. Default is false
  @return partial derivative
  */
  arma::rowvec PartialSlopeArgumentPartialOmegaC(const int & f,
    const arma::vec::fixed<3> & body_fixed_acc,bool hold_mass_constant = false) const;

  /**
  Return the partial derivative of the Ee dyad parametrization with respect
  to the coordinates of the edges points and adjacent facets points
  @param e edge index
  @return PartialEPartialBe (6x24)
  */
  arma::mat::fixed<6,24> PartialEPartialBe(const int & e) const;

  /**
  Return the connectivity table associated with vector Be
  @param e edge index
  @return connectivity table
  */
  arma::sp_mat  PartialBePartialC(const int & e) const;

  /**
  Return the partial derivative of the slope s == arcos(-u) relative to the slope argument u
  @param u input parameter
  @param partial derivative of slope with respect to u
  */
  static double PartialSlopePartialSlopeArgument(const double & u);

  /**
  Given a prescribed global deviation of all of the shape's N control points,
  applies it and returns the deviation in each of the Be's vector (one per edge in the shape)
  @param delta deviation in all of the shape's N control points (3 x N_vertices)
  @return deviation in all of the shape's Be vectors (24 x N_edges)
  */
  arma::vec ApplyAndGetBeDeviation(const arma::vec & delta);

  static void TestPartialUePartialXe(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialUfPartialXf(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialXfPartialTf(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialOmegafPartialTf(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialZfPartialUnitRf(std::string input , double tol, bool shape_in_meters);
  static void TestPartialFfPartialTf(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialNormalizedVPartialNonNormalizedV(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialAtan2PartialZf(std::string input , double tol, bool shape_in_meters);
  static void TestPartialNfPartialTf(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialFfPartialnf(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialFfPartialNonNormalizedNf(std::string input , double tol, bool shape_in_meters);
  static void TestPartialLePartialAe(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialEePartialAe(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialEePartialTf(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialXePartialBe(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialEdgeLengthPartialAe(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialEPartialBe(std::string input , double tol, bool shape_in_meters) ;
  static void TestPartialUfPartialTf(std::string input , double tol, bool shape_in_meters);
  static void TestPartialUePartialBe(std::string input , double tol, bool shape_in_meters);
  static void TestGetPartialUPartialC(std::string input , double tol, bool shape_in_meters);
  static void TestGetPartialAPartialC(std::string input , double tol, bool shape_in_meters);
  static void TestGetPartialAPartialCConstantMass(std::string input , double tol, bool shape_in_meters);
  static void TestGetPartialUPartialCConstantMass(std::string input , double tol, bool shape_in_meters);

  static void TestPartialUfPartialC(std::string input , double tol, bool shape_in_meters);
  static void TestPartialUePartialC(std::string input , double tol, bool shape_in_meters);
  static void TestAddPartialSumUePartialC(std::string input , double tol, bool shape_in_meters);
  static void TestAddPartialSumUfPartialC(std::string input , double tol, bool shape_in_meters);
  static void TestAddPartialSumAccfPartialC(std::string input , double tol, bool shape_in_meters);
  static void TestAddPartialSumAccePartialC(std::string input , double tol, bool shape_in_meters);
  static void TestPartialBePartialC(std::string input , double tol, bool shape_in_meters);
  static void TestGetPartialSlopePartialwPartialC(std::string input , double tol, bool shape_in_meters);
  static void TestPartialBodyFixedAccelerationfPartialC(std::string input , double tol, bool shape_in_meters);
  static void TestPartialBodyFixedAccelerationfPartialwC(std::string input , double tol, bool shape_in_meters);
  static void TestPartialOmegaPartialwC(std::string input , double tol, bool shape_in_meters);
  static void TestPartialSlopeArgumentPartialOmegaC(std::string input , double tol, bool shape_in_meters);

  double period_standard_deviation;

};

#endif


/*
* General automatic differentiation engine based on a Wengert list
* implementation. Reverse mode only.
*/

#pragma once

#include <vector>
#include <memory>
#include <iostream>

#include <Eigen/Dense>

namespace ts {
	template <typename T> class Node;
	template <typename T> class InputNode;
	template <typename T> class ElementWiseNode;
	template <typename T> class MatProdNode;
	template <typename T> class ScalarNode;

	template <typename T> class WengertList;
	template <typename T> class Tensor;
	template <typename T> class Gradient;

	// This helper function allows us to create Tensor instances without
	// template syntax. This way, the type will be the same as its parent
	// WengertList.

	template <typename T>
	ts::Tensor<T> NewTensor(
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> newValue,
		ts::WengertList<T> * newWList
	);

	template <typename T>
	ts::Tensor<T> operator+(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	template <typename T>
	ts::Tensor<T> operator-(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	template <typename T>
	ts::Tensor<T> operator*(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	template <typename T>
	ts::Tensor<T> operator/(const ts::Tensor<T> &x, const ts::Tensor<T> &y);

	template <typename T>
	ts::Tensor<T> matProd(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	template <typename T>
	ts::Tensor<T> sigmoid(const ts::Tensor<T> &x);
	template <typename T>
	ts::Tensor<T> relu(const ts::Tensor<T> &x);
	template <typename T>
	ts::Tensor<T> leakyRelu(const ts::Tensor<T> &x);
	template <typename T>
	ts::Tensor<T> rescale(const ts::Tensor<T> &x);
	template <typename T>
	ts::Tensor<T> squaredNorm(const ts::Tensor<T> &x);

	// Forward declaration of friends
	// (grad accumulators and other autodiff operations)
	template <typename T> class GaElement;
	template <typename T> class GradientAccumulator;
	template <typename T> class AdamOptimizer;

	enum class ChannelSplit : int;

	template <typename T>
	ts::Tensor<T> convolution(const ts::Tensor<T> &mat, const ts::Tensor<T> &ker);

	template <typename T>
	ts::Tensor<T> maxPooling(const ts::Tensor<T> &x, std::vector<unsigned> pool);

	template <typename T>
	std::vector<ts::Tensor<T>> split(
		const ts::Tensor<T> &x,
		ChannelSplit channelSplit,
		unsigned nInputChannels
	);

	template <typename T>
	ts::Tensor<T> vertCat(const std::vector<ts::Tensor<T>> &x);

	template <typename T>
	ts::Tensor<T> flattening(const ts::Tensor<T> &x);

	template <typename T>
	ts::Tensor<T> im2col(
		const std::vector<ts::Tensor<T>> &x,
		std::vector<unsigned> kernelDim
	);

	template <typename T>
	std::vector<ts::Tensor<T>> col2im(
		const ts::Tensor<T> &x,
		std::vector<unsigned> outputDim
	);
}

	// ts::Node

template <typename T>
class ts::Node {
protected:

	Node() {}

	// Represents an input variable
	Node(std::vector<long> shape);

	// Represents a unary operator
	Node(std::vector<long> shape,
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> xVal, int xDep
	);

	// Represents a binary operator
	Node(
		std::vector<long> shape,
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> xVal, int xDep,
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> yVal, int yDep
	);

	std::vector<int> dependencies{};

	virtual Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	) = 0;

	std::vector< Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> > values{};

	// Shape of the corresponding tensor
	long rows, cols;

public:

	friend ts::Tensor<T>;
	friend ts::WengertList<T>;
	friend ts::GradientAccumulator<T>;
	friend ts::AdamOptimizer<T>;	// Needed to initialize moment estimates

	friend ts::Tensor<T> operator+<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	friend ts::Tensor<T> operator-<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	friend ts::Tensor<T> operator*<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	friend ts::Tensor<T> operator/<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);

	friend ts::Tensor<T> matProd<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	friend ts::Tensor<T> sigmoid<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> relu<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> leakyRelu<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> rescale<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> squaredNorm<>(const ts::Tensor<T> &x);

	friend ts::Tensor<T> convolution<>(const ts::Tensor<T> &mat, const ts::Tensor<T> &ker);
	friend ts::Tensor<T> maxPooling<>(const ts::Tensor<T> &x, std::vector<unsigned> pool);
	friend std::vector<ts::Tensor<T>> split<>(
		const ts::Tensor<T> &x,
		ChannelSplit channelSplit,
		unsigned nInputChannels
	);
	friend ts::Tensor<T> vertCat<>(const std::vector<ts::Tensor<T>> &x);
	friend ts::Tensor<T> flattening<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> im2col<>(
		const std::vector<ts::Tensor<T>> &x,
		std::vector<unsigned> kernelDim
	);
	friend std::vector<ts::Tensor<T>> col2im<>(
		const ts::Tensor<T> &x,
		std::vector<unsigned> outputDim
	);

};

template <typename T>
class ts::InputNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;
	InputNode(std::vector<long> shape, bool model);

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);

	// We will need this to optimize the tensor value in a ts::Model
	ts::Tensor<T> * optimizedTensor = NULL;

	// If true, node won't be removed on wList reset
	bool isModel = false;

public:

	friend ts::WengertList<T>;
	friend ts::Tensor<T>;
	friend ts::GradientAccumulator<T>;
};

template <typename T>
class ts::ElementWiseNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);
};

template <typename T>
class ts::MatProdNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	MatProdNode(
		std::vector<long> shape,
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> xVal, int xDep,
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> yVal, int yDep,
		std::vector<long int> newXSize, std::vector<long int> newYSize
	);

	// Size of the operands to figure out how to increment their partial
	// derivatives
	std::vector<long int> xSize;
	std::vector<long int> ySize;

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);

	friend ts::Tensor<T> matProd<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
};

template <typename T>
class ts::ScalarNode : public ts::Node<T> {
private:
	using ts::Node<T>::Node;

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> incrementGradient(
			Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> &childDerivative,
			unsigned &j
	);
};

	// ts::WengertList

template <typename T>
class ts::WengertList {
private:
	bool elementWiseOnly = true;
	std::vector< std::shared_ptr<ts::Node<T>> > nodes{};

public:
	int size();
	int reset();

	// Make a tensor optimizable
	void toggleOptimize(ts::Tensor<T> * tensor, bool enable);

	friend class ts::Tensor<T>;
	friend class ts::GradientAccumulator<T>;
	friend class ts::AdamOptimizer<T>;	// Needed to initialize moment estimates

	// Other non-element wise operations (to change elementWiseOnly)
	friend ts::Tensor<T> matProd<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	friend ts::Tensor<T> sigmoid<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> relu<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> leakyRelu<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> rescale<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> squaredNorm<>(const ts::Tensor<T> &x);

	friend ts::Tensor<T> convolution<>(const ts::Tensor<T> &mat, const ts::Tensor<T> &ker);
	friend ts::Tensor<T> maxPooling<>(const ts::Tensor<T> &x, std::vector<unsigned> pool);
	friend std::vector<ts::Tensor<T>> split<>(
		const ts::Tensor<T> &x,
		ChannelSplit channelSplit,
		unsigned nInputChannels
	);
	friend ts::Tensor<T> vertCat<>(const std::vector<ts::Tensor<T>> &x);
	friend ts::Tensor<T> flattening<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> im2col<>(
		const std::vector<ts::Tensor<T>> &x,
		std::vector<unsigned> kernelDim
	);
	friend std::vector<ts::Tensor<T>> col2im<>(
		const ts::Tensor<T> &x,
		std::vector<unsigned> outputDim
	);
};

	// ts::Tensor

template <typename T>
class ts::Tensor {
private:
	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> value;
	ts::WengertList<T> * wList = NULL;
	int index;

	// We want this constructor to be private as it is supposed to be called by
	// our friends overloaded operators and functions only. This constructor
	// thus allows us to create a Tensor with dependencies in the Wengert list.
	Tensor(
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> newValue,
		ts::WengertList<T> * newWList, std::shared_ptr<ts::Node<T>> node
	);

public:

	Tensor() {};

	// Input tensor, part of model
	Tensor(
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> newValue,
		ts::WengertList<T> * newWList
	);

	// Non part of model input tensor
	// (equivalent to calling previous constructor with model = false)
	Tensor(
		Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> newValue,
		ts::WengertList<T> * newWList, bool model
	);

	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> getValue();
	ts::Gradient<T> grad();

	friend ts::WengertList<T>;

	friend ts::Gradient<T>;
	friend ts::GaElement<T>;
	friend ts::GradientAccumulator<T>;

	friend ts::Tensor<T> operator+<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	friend ts::Tensor<T> operator-<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	friend ts::Tensor<T> operator*<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	friend ts::Tensor<T> operator/<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);

	friend ts::Tensor<T> matProd<>(const ts::Tensor<T> &x, const ts::Tensor<T> &y);
	friend ts::Tensor<T> sigmoid<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> relu<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> leakyRelu<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> rescale<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> squaredNorm<>(const ts::Tensor<T> &x);

	friend ts::Tensor<T> convolution<>(const ts::Tensor<T> &mat, const ts::Tensor<T> &ker);
	friend ts::Tensor<T> maxPooling<>(const ts::Tensor<T> &x, std::vector<unsigned> pool);
	friend std::vector<ts::Tensor<T>> split<>(
		const ts::Tensor<T> &x,
		ChannelSplit channelSplit,
		unsigned nInputChannels
	);
	friend ts::Tensor<T> vertCat<>(const std::vector<ts::Tensor<T>> &x);
	friend ts::Tensor<T> flattening<>(const ts::Tensor<T> &x);
	friend ts::Tensor<T> im2col<>(
		const std::vector<ts::Tensor<T>> &x,
		std::vector<unsigned> kernelDim
	);
	friend std::vector<ts::Tensor<T>> col2im<>(
		const ts::Tensor<T> &x,
		std::vector<unsigned> outputDim
	);
};

template <typename T>
ts::Tensor<T> NewTensor(
	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> newValue,
	ts::WengertList<T> * newWList
);

	// ts::Gradient

template <typename T>
class ts::Gradient {
private:
	// Constructor is private since we want instances of this class to be
	// generated by the Tensor::grad() method only
	Gradient(
		std::vector< Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> > newDerivatives
	);

	std::vector< Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> > derivatives;

public:
	Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> getValue(ts::Tensor<T> a);
	bool isEmpty();

	friend class ts::Tensor<T>;
	friend class ts::GradientAccumulator<T>;
	friend class ts::AdamOptimizer<T>;
};


//  MIT License
// 	Copyright(c) 2020 ChenKB
//
// 	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.

#include <iostream> // cout
#include <stdio.h>  // sprintf
#include <stdlib.h> // system

#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

// Required: CMake, opencv
// To generate executable:
// cmake . -DOpenCV_DIR="/path/to/opencv/build/folder"
// Afterwards:
// make (Do this to recompile)

using namespace Eigen;
using namespace std;

const int plateSize = 25; // How many balls in each side of the square?

// A ball.
struct grid_unit
{
	Vector3d pos;
	Vector3d v;
	Vector3d f;
	float m;
};

// Set up all the balls' position, velocity and force to 0.
void initialize_layer(int gridSize, grid_unit (*layer)[plateSize][plateSize], float y)
{
	for (int i = 0; i < plateSize; i++)
	{
		for (int j = 0; j < plateSize; j++)
		{
			(*layer)[i][j].pos << i * gridSize, y, j * gridSize;
			(*layer)[i][j].v << 0, 0, 0;
			(*layer)[i][j].f << 0, 0, 0;
			(*layer)[i][j].m = 1;
		}
	}
}

// Calculates layer0[i][j]'s force given one other ball's position
Vector3d spring_f(int i, int j, int i0, int j0,
				  grid_unit (*layer1)[plateSize][plateSize],
				  grid_unit (*layer0)[plateSize][plateSize],
				  bool is_same_lyr, float gridSize)
{
	double length0;
	if (is_same_lyr)
	{
		length0 = gridSize * sqrt(pow(i - i0, 2) + pow(j - j0, 2));
	}
	else
	{
		length0 = gridSize * sqrt(pow(i - i0, 2) + pow(j - j0, 2) + 1);
	}
	Vector3d pdist_v = (*layer1)[i][j].pos - (*layer0)[i0][j0].pos;
	return 1000 * (pdist_v.norm() - length0) * pdist_v / pdist_v.norm();
}

// Resets force each ball of both layer to (0,0,0)
void clearLayerForce(int i, int j, grid_unit (*layer1)[plateSize][plateSize], grid_unit (*layer2)[plateSize][plateSize])
{
	(*layer1)[i][j].f << 0, 0, 0;
	(*layer2)[i][j].f << 0, 0, 0;
}

// Update each ball's force in layer0, given the other layer's position.
// Can apply force to the same layer by giving the same pointer to both layer parameter.
void updateLayerForce(int i, int j, grid_unit (*layer1)[plateSize][plateSize], grid_unit (*layer0)[plateSize][plateSize], float gridSize)
{
	bool isSameLyr = (layer1 == layer0);

	if (!isSameLyr) // directly over / under
	{
		(*layer0)[i][j].f += spring_f(i, j, i, j, layer1, layer0, isSameLyr, gridSize);
	}
	if (i != 0 && j != (plateSize - 1)) // left up
	{
		(*layer0)[i][j].f += spring_f(i - 1, j + 1, i, j, layer1, layer0, isSameLyr, gridSize);
	}
	if (j != (plateSize - 1)) // up
	{
		(*layer0)[i][j].f += spring_f(i, j + 1, i, j, layer1, layer0, isSameLyr, gridSize);
	}
	if (i != (plateSize - 1) && j != (plateSize - 1)) // up right
	{
		(*layer0)[i][j].f += spring_f(i + 1, j + 1, i, j, layer1, layer0, isSameLyr, gridSize);
	}
	if (i != (plateSize - 1)) // right
	{
		(*layer0)[i][j].f += spring_f(i + 1, j, i, j, layer1, layer0, isSameLyr, gridSize);
	}
	if (i != (plateSize - 1) && j != 0) // right down
	{
		(*layer0)[i][j].f += spring_f(i + 1, j - 1, i, j, layer1, layer0, isSameLyr, gridSize);
	}
	if (j != 0) // down
	{
		(*layer0)[i][j].f += spring_f(i, j - 1, i, j, layer1, layer0, isSameLyr, gridSize);
	}
	if (i != 0 && j != 0) // down left
	{
		(*layer0)[i][j].f += spring_f(i - 1, j - 1, i, j, layer1, layer0, isSameLyr, gridSize);
	}
	if (i != 0) // left
	{
		(*layer0)[i][j].f += spring_f(i - 1, j, i, j, layer1, layer0, isSameLyr, gridSize);
	}
}

// Updates one layer's position and velocity.
void updateLayerPV(grid_unit (*layer)[plateSize][plateSize], float dt)
{
	for (int i = 0; i < plateSize; i++)
	{
		for (int j = 0; j < plateSize; j++)
		{
			(*layer)[i][j].v += (*layer)[i][j].f / (*layer)[i][j].m * dt;
			(*layer)[i][j].pos += (*layer)[i][j].v * dt;
		}
	}
}

// Linear mapping, I guess...
// from (pa,pb,x) to (qa,qb,y), outputs y
float map_to(float pa, float pb, float qa, float qb, float x)
{
	return qa + (qb-qa)* (pb-x)/(pb-pa);
}

// output a picture.
// mode 0 = b&w pic, 1 = red&blue
void outputPic(grid_unit (*layer)[plateSize][plateSize], int mode, bool writeFile, int frame)
{

	cv::Mat image(plateSize, plateSize, CV_8UC3);

	if (mode == 0)
	{
		for (int i = 0; i < plateSize; i++)
		{
			for (int j = 0; j < plateSize; j++)
			{
				float value = (*layer)[i][j].pos(1);
				cv::Vec3b &color = image.at<cv::Vec3b>(i, j);
				color[0] = map_to(-3, 3, 255, 0, value);
				color[1] = map_to(-3, 3, 255, 0, value);
				color[2] = map_to(-3, 3, 255, 0, value);
			}
		}
	}
	else
	{
		for (int i = 0; i < plateSize; i++)
		{
			for (int j = 0; j < plateSize; j++)
			{
				float value = (*layer)[i][j].pos(1);
				cv::Vec3b &color = image.at<cv::Vec3b>(i, j);

				if (value > 0)
				{
					color[0] = 0;
					color[1] = 0;
					// color[2] = map_to(0, 10, 255, 0, value);
					if ((int)(abs(value / 3) * 255) <= 255)
					{
						color[2] = (int)(abs(value / 3) * 255);
					}
					else
					{
						color[2] = 255;
					}
				}
				else
				{
					// color[0] = map_to(0, -10, 255, 0, value);
					if ((int)(abs(value / 3) * 255) <= 255)
					{
						color[0] = (int)(abs(value / 3) * 255);
					}
					else
					{
						color[0] = 255;
					}
					color[1] = 0;
					color[2] = 0;
				}
			}
		}
	}

	// cv::eigen2cv(output,output2);
	cv::namedWindow("Chladni", cv::WINDOW_AUTOSIZE);
	cv::resizeWindow("Chladni", 500,500);
	cv::imshow("Chladni", image);
	cv::waitKey(1);

	if(writeFile){
		char buf[50];
		sprintf(buf, "./pics/%04d.png", frame);
		cv::imwrite(buf, image);
	}
}

int main(int argc, char *argv[])
{
	grid_unit layer1[plateSize][plateSize];
	grid_unit layer2[plateSize][plateSize];
	float gridSize = 4; // distance between adjacent balls

	initialize_layer(gridSize, &layer1, 0);
	initialize_layer(gridSize, &layer2, gridSize);

	system("mkdir pics"); // create pics folder if not exist
	system("rm -f pics/*.png"); // clear the floder if it do exist

	// cout << spring_f(5, 5, 5, 6, &layer1, &layer1, true, gridSize) << endl;

	float t = 0;
	const float dt = 0.001;

	int frame = 0;

	float frequency = 2;
	float amplitude = 2.0;
	int test_x = 12;
	int test_y = 12; // vibrating point

	while (frame <= 2200) // main time loop
	{
		t += dt;
		frame += 1;

		if (!(frame % 10))
		{
			cout << "Writing frame " << frame << endl;
		}

		// cout << layer1[11][11].pos << endl;
		// cout << "Layer 2 "<< layer2[11][11].pos(1) << endl;

		for (int i = 0; i < plateSize; i++)
		{
			for (int j = 0; j < plateSize; j++)
			{
				clearLayerForce(i, j, &layer1, &layer2);

				updateLayerForce(i, j, &layer2, &layer1, gridSize);
				updateLayerForce(i, j, &layer1, &layer1, gridSize);
				updateLayerForce(i, j, &layer1, &layer2, gridSize);
				updateLayerForce(i, j, &layer2, &layer2, gridSize);
			}
		}

		updateLayerPV(&layer1, dt);
		updateLayerPV(&layer2, dt);

		// apply vibration
		layer1[test_x][test_y].pos << gridSize * test_x, amplitude * sin(t * 2 * M_PI * frequency), gridSize * test_y;
		layer2[test_x][test_y].pos << gridSize * test_x, amplitude * sin(t * 2 * M_PI * frequency) + gridSize, gridSize * test_y;

		if(!(frame % 10)){
			outputPic(&layer1,1,true,frame);
		}else{
			outputPic(&layer1,1,false,frame);
		}
	}
	cout << "Task done.\07" << endl; // ascii 07 rings a bell
	return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 SunTrust Bank
 Copyright (C) 2010 Cavit Hafizoglu

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file generalizedornsteinuhlenbeckprocess.hpp
    \brief Ornstein-Uhlenbeck process with piecewise linear coefficients
*/

#ifndef quantlib_generalized_ornstein_uhlenbeck_process_hpp
#define quantlib_generalized_ornstein_uhlenbeck_process_hpp

#include <ql/stochasticprocess.hpp>
#include <boost/function.hpp>

namespace QuantLib {

    //! Piecewise linear Ornstein-Uhlenbeck process class
    /*! This class describes the Ornstein-Uhlenbeck process governed by
        \f[
            dx = a (level - x_t) dt + \sigma dW_t
        \f]

        \ingroup processes

        where the coefficients a and sigma are piecewise linear.
    */
    class GeneralizedOrnsteinUhlenbeckProcess : public StochasticProcess1D {
      public:
        GeneralizedOrnsteinUhlenbeckProcess(
              const boost::function<Real (Time)>& speed,
              const boost::function<Real (Time)>& vol,
              Real x0 = 0.0,
              Real level = 0.0);
        //! \name StochasticProcess1D interface
        //@{
        Real x0() const;

        Real drift(Time t, Real x) const;
        Real diffusion(Time t, Real x) const;

        Real expectation(Time t0, Real x0, Time dt) const;
        Real stdDeviation(Time t0, Real x0, Time dt) const;
        Real variance(Time t0, Real x0, Time dt) const;
        //@}

        Real speed(Time t) const;
        Real volatility(Time t) const;
        Real level() const;

      private:
        Real x0_, level_;
        boost::function<Real (Time)> speed_;
        boost::function<Real (Time)> volatility_;
    };

}

#endif


/* boost random/chi_squared_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_CHI_SQUARED_DISTRIBUTION_HPP_INCLUDED
#define BOOST_RANDOM_CHI_SQUARED_DISTRIBUTION_HPP_INCLUDED

#include <iosfwd>
#include <boost/limits.hpp>

#include <boost/random/detail/config.hpp>
#include <boost/random/gamma_distribution.hpp>

namespace boost {
namespace random {

/**
 * The chi squared distribution is a real valued distribution with
 * one parameter, @c n.  The distribution produces values > 0.
 *
 * The distribution function is
 * \f$\displaystyle P(x) = \frac{x^{(n/2)-1}e^{-x/2}}{\Gamma(n/2)2^{n/2}}\f$.
 */
template<class RealType = double>
class chi_squared_distribution {
public:
    typedef RealType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef chi_squared_distribution distribution_type;
        /**
         * Construct a param_type object.  @c n
         * is the parameter of the distribution.
         *
         * Requires: t >=0 && 0 <= p <= 1
         */
        explicit param_type(RealType n_arg = RealType(1))
          : _n(n_arg)
        {}
        /** Returns the @c n parameter of the distribution. */
        RealType n() const { return _n; }
#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
        /** Writes the parameters of the distribution to a @c std::ostream. */
        template<class CharT, class Traits>
        friend std::basic_ostream<CharT,Traits>&
        operator<<(std::basic_ostream<CharT,Traits>& os,
                   const param_type& parm)
        {
            os << parm._n;
            return os;
        }

        /** Reads the parameters of the distribution from a @c std::istream. */
        template<class CharT, class Traits>
        friend std::basic_istream<CharT,Traits>&
        operator>>(std::basic_istream<CharT,Traits>& is, param_type& parm)
        {
            is >> parm._n;
            return is;
        }
#endif
        /** Returns true if the parameters have the same values. */
        friend bool operator==(const param_type& lhs, const param_type& rhs)
        {
            return lhs._n == rhs._n;
        }
        /** Returns true if the parameters have different values. */
        friend bool operator!=(const param_type& lhs, const param_type& rhs)
        {
            return !(lhs == rhs);
        }
    private:
        RealType _n;
    };

    /**
     * Construct a @c chi_squared_distribution object. @c n
     * is the parameter of the distribution.
     *
     * Requires: t >=0 && 0 <= p <= 1
     */
    explicit chi_squared_distribution(RealType n_arg = RealType(1))
      : _impl(static_cast<RealType>(n_arg / 2))
    {
    }

    /**
     * Construct an @c chi_squared_distribution object from the
     * parameters.
     */
    explicit chi_squared_distribution(const param_type& parm)
      : _impl(static_cast<RealType>(parm.n() / 2))
    {
    }

    /**
     * Returns a random variate distributed according to the
     * chi squared distribution.
     */
    template<class URNG>
    RealType operator()(URNG& urng)
    {
        return 2 * _impl(urng);
    }

    /**
     * Returns a random variate distributed according to the
     * chi squared distribution with parameters specified by @c param.
     */
    template<class URNG>
    RealType operator()(URNG& urng, const param_type& parm) const
    {
        return chi_squared_distribution(parm)(urng);
    }

    /** Returns the @c n parameter of the distribution. */
    RealType n() const { return 2 * _impl.alpha(); }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION() const { return 0; }
    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION() const
    { return (std::numeric_limits<RealType>::infinity)(); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(n()); }
    /** Sets parameters of the distribution. */
    void param(const param_type& parm)
    {
        typedef gamma_distribution<RealType> impl_type;
        typename impl_type::param_type impl_parm(static_cast<RealType>(parm.n() / 2));
        _impl.param(impl_parm);
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { _impl.reset(); }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
    /** Writes the parameters of the distribution to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT,Traits>&
    operator<<(std::basic_ostream<CharT,Traits>& os,
               const chi_squared_distribution& c2d)
    {
        os << c2d.param();
        return os;
    }

    /** Reads the parameters of the distribution from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT,Traits>&
    operator>>(std::basic_istream<CharT,Traits>& is,
               chi_squared_distribution& c2d)
    {
        c2d.read(is);
        return is;
    }
#endif

    /** Returns true if the two distributions will produce the same
        sequence of values, given equal generators. */
    friend bool operator==(const chi_squared_distribution& lhs,
                           const chi_squared_distribution& rhs)
    {
        return lhs._impl == rhs._impl;
    }
    /** Returns true if the two distributions could produce different
        sequences of values, given equal generators. */
    friend bool operator!=(const chi_squared_distribution& lhs,
                           const chi_squared_distribution& rhs)
    {
        return !(lhs == rhs);
    }

private:

    /// @cond show_private

    template<class CharT, class Traits>
    void read(std::basic_istream<CharT, Traits>& is) {
        param_type parm;
        if(is >> parm) {
            param(parm);
        }
    }

    gamma_distribution<RealType> _impl;

    /// @endcond
};

}

}

#endif


/*
    This file is part of control-lib.

    Copyright (c) 2020, 2021, 2022 Bernardo Fichera <bernardo.fichera@gmail.com>

    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 CONTROLLIB_TOOLS_MATH_HPP
#define CONTROLLIB_TOOLS_MATH_HPP

#include <Eigen/Core>
namespace control_lib {
    namespace tools {
        inline Eigen::Vector3d eulerError(const Eigen::Vector3d& curr, const Eigen::Vector3d& ref);

        inline Eigen::Vector3d rotationError(const Eigen::Vector3d& curr, const Eigen::Vector3d& ref);

        inline Eigen::Vector4d quaternionError(const Eigen::Vector4d& curr, const Eigen::Vector4d& ref);

        inline Eigen::MatrixXd kronecker(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);

        Eigen::MatrixXd solveVectorized(const Eigen::MatrixXd& A, const Eigen::MatrixXd& W);

        Eigen::MatrixXd bartelsStewart(const Eigen::MatrixXd& A, const Eigen::MatrixXd& W);
    } // namespace tools
} // namespace control_lib

#endif // CONTROLLIB_TOOLS_MATH_HPP