// negative_binomial_example1.cpp

// Copyright Paul A. Bristow 2007.

// Use, modification and distribution are subject to 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)

// Example 1 of using negative_binomial distribution.

//[negative_binomial_eg1_1

/*`
Based on [@http://en.wikipedia.org/wiki/Negative_binomial_distribution
a problem by Dr. Diane Evans,
Professor of Mathematics at Rose-Hulman Institute of Technology].

Pat is required to sell candy bars to raise money for the 6th grade field trip.
There are thirty houses in the neighborhood,
and Pat is not supposed to return home until five candy bars have been sold.
So the child goes door to door, selling candy bars.
At each house, there is a 0.4 probability (40%) of selling one candy bar
and a 0.6 probability (60%) of selling nothing.

What is the probability mass (density) function (pdf) for selling the last (fifth)
candy bar at the nth house?

The Negative Binomial(r, p) distribution describes the probability of k failures
and r successes in k+r Bernoulli(p) trials with success on the last trial.
(A [@http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli trial]
is one with only two possible outcomes, success of failure,
and p is the probability of success).
See also [@ http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli distribution]
and [@http://www.math.uah.edu/stat/bernoulli/Introduction.xhtml Bernoulli applications].

In this example, we will deliberately produce a variety of calculations
and outputs to demonstrate the ways that the negative binomial distribution
can be implemented with this library: it is also deliberately over-commented.

First we need to #define macros to control the error and discrete handling policies.
For this simple example, we want to avoid throwing
an exception (the default policy) and just return infinity.
We want to treat the distribution as if it was continuous,
so we choose a discrete_quantile policy of real,
rather than the default policy integer_round_outwards.
*/
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#define BOOST_MATH_DISCRETE_QUANTILE_POLICY real
/*`
After that we need some includes to provide easy access to the negative binomial distribution,
[caution It is vital to #include distributions etc *after* the above #defines]
and we need some std library iostream, of course.
*/
#include <boost/math/distributions/negative_binomial.hpp>
  // for negative_binomial_distribution
  using boost::math::negative_binomial; // typedef provides default type is double.
  using  ::boost::math::pdf; // Probability mass function.
  using  ::boost::math::cdf; // Cumulative density function.
  using  ::boost::math::quantile;

#include <iostream>
  using std::cout; using std::endl;
  using std::noshowpoint; using std::fixed; using std::right; using std::left;
#include <iomanip>
  using std::setprecision; using std::setw;

#include <limits>
  using std::numeric_limits;
//] [negative_binomial_eg1_1]

int main()
{
  cout <<"Selling candy bars - using the negative binomial distribution."
    << "\nby Dr. Diane Evans,"
    "\nProfessor of Mathematics at Rose-Hulman Institute of Technology,"
    << "\nsee http://en.wikipedia.org/wiki/Negative_binomial_distribution\n"
    << endl;
  cout << endl;
  cout.precision(5);
  // None of the values calculated have a useful accuracy as great this, but
  // INF shows wrongly with < 5 !
  // https://connect.microsoft.com/VisualStudio/feedback/ViewFeedback.aspx?FeedbackID=240227
//[negative_binomial_eg1_2
/*`
It is always sensible to use try and catch blocks because defaults policies are to
throw an exception if anything goes wrong.

A simple catch block (see below) will ensure that you get a
helpful error message instead of an abrupt program abort.
*/
  try
  {
/*`
Selling five candy bars means getting five successes, so successes r = 5.
The total number of trials (n, in this case, houses visited) this takes is therefore
  = sucesses + failures or k + r = k + 5.
*/
    double sales_quota = 5; // Pat's sales quota - successes (r).
/*`
At each house, there is a 0.4 probability (40%) of selling one candy bar
and a 0.6 probability (60%) of selling nothing.
*/
    double success_fraction = 0.4; // success_fraction (p) - so failure_fraction is 0.6.
/*`
The Negative Binomial(r, p) distribution describes the probability of k failures
and r successes in k+r Bernoulli(p) trials with success on the last trial.
(A [@http://en.wikipedia.org/wiki/Bernoulli_distribution Bernoulli trial]
is one with only two possible outcomes, success of failure,
and p is the probability of success).

We therefore start by constructing a negative binomial distribution
with parameters sales_quota (required successes) and probability of success.
*/
    negative_binomial nb(sales_quota, success_fraction); // type double by default.
/*`
To confirm, display the success_fraction & successes parameters of the distribution.
*/
    cout << "Pat has a sales per house success rate of " << success_fraction
      << ".\nTherefore he would, on average, sell " << nb.success_fraction() * 100
      << " bars after trying 100 houses." << endl;

    int all_houses = 30; // The number of houses on the estate.

    cout << "With a success rate of " << nb.success_fraction()
      << ", he might expect, on average,\n"
        "to need to visit about " << success_fraction * all_houses
        << " houses in order to sell all " << nb.successes() << " bars. " << endl;
/*`
[pre
Pat has a sales per house success rate of 0.4.
Therefore he would, on average, sell 40 bars after trying 100 houses.
With a success rate of 0.4, he might expect, on average,
to need to visit about 12 houses in order to sell all 5 bars.
]

The random variable of interest is the number of houses
that must be visited to sell five candy bars,
so we substitute k = n - 5 into a negative_binomial(5, 0.4)
and obtain the [link math.dist.pdf probability mass (density) function (pdf or pmf)]
of the distribution of houses visited.
Obviously, the best possible case is that Pat makes sales on all the first five houses.

We calculate this using the pdf function:
*/
    cout << "Probability that Pat finishes on the " << sales_quota << "th house is "
      << pdf(nb, 5 - sales_quota) << endl; // == pdf(nb, 0)
/*`
Of course, he could not finish on fewer than 5 houses because he must sell 5 candy bars.
So the 5th house is the first that he could possibly finish on.

To finish on or before the 8th house, Pat must finish at the 5th, 6th, 7th or 8th house.
The probability that he will finish on *exactly* ( == ) on any house
is the Probability Density Function (pdf).
*/
    cout << "Probability that Pat finishes on the 6th house is "
      << pdf(nb, 6 - sales_quota) << endl;
    cout << "Probability that Pat finishes on the 7th house is "
      << pdf(nb, 7 - sales_quota) << endl;
    cout << "Probability that Pat finishes on the 8th house is "
      << pdf(nb, 8 - sales_quota) << endl;
/*`
[pre
Probability that Pat finishes on the 6th house is 0.03072
Probability that Pat finishes on the 7th house is 0.055296
Probability that Pat finishes on the 8th house is 0.077414
]

The sum of the probabilities for these houses is the Cumulative Distribution Function (cdf).
We can calculate it by adding the individual probabilities.
*/
    cout << "Probability that Pat finishes on or before the 8th house is sum "
      "\n" << "pdf(sales_quota) + pdf(6) + pdf(7) + pdf(8) = "
      // Sum each of the mass/density probabilities for houses sales_quota = 5, 6, 7, & 8.
      << pdf(nb, 5 - sales_quota) // 0 failures.
        + pdf(nb, 6 - sales_quota) // 1 failure.
        + pdf(nb, 7 - sales_quota) // 2 failures.
        + pdf(nb, 8 - sales_quota) // 3 failures.
      << endl;
/*`[pre
pdf(sales_quota) + pdf(6) + pdf(7) + pdf(8) = 0.17367
]

Or, usually better, by using the negative binomial *cumulative* distribution function.
*/
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 8 << "th house is "
      << cdf(nb, 8 - sales_quota) << endl;
/*`[pre
Probability of selling his quota of 5 bars on or before the 8th house is 0.17367
]*/
    cout << "\nProbability that Pat finishes exactly on the 10th house is "
      << pdf(nb, 10 - sales_quota) << endl;
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 10 << "th house is "
      << cdf(nb, 10 - sales_quota) << endl;
/*`
[pre
Probability that Pat finishes exactly on the 10th house is 0.10033
Probability of selling his quota of 5 bars on or before the 10th house is 0.3669
]*/
    cout << "Probability that Pat finishes exactly on the 11th house is "
      << pdf(nb, 11 - sales_quota) << endl;
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 11 << "th house is "
      << cdf(nb, 11 - sales_quota) << endl;
/*`[pre
Probability that Pat finishes on the 11th house is 0.10033
Probability of selling his quota of 5 candy bars
on or before the 11th house is 0.46723
]*/
    cout << "Probability that Pat finishes exactly on the 12th house is "
      << pdf(nb, 12 - sales_quota) << endl;

    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << 12 << "th house is "
      << cdf(nb, 12 - sales_quota) << endl;
/*`[pre
Probability that Pat finishes on the 12th house is 0.094596
Probability of selling his quota of 5 candy bars
on or before the 12th house is 0.56182
]
Finally consider the risk of Pat not selling his quota of 5 bars
even after visiting all the houses.
Calculate the probability that he /will/ sell on
or before the last house:
Calculate the probability that he would sell all his quota on the very last house.
*/
    cout << "Probability that Pat finishes on the " << all_houses
      << " house is " << pdf(nb, all_houses - sales_quota) << endl;
/*`
Probability of selling his quota of 5 bars on the 30th house is
[pre
Probability that Pat finishes on the 30 house is 0.00069145
]
when he'd be very unlucky indeed!

What is the probability that Pat exhausts all 30 houses in the neighborhood,
and *still* doesn't sell the required 5 candy bars?
*/
    cout << "\nProbability of selling his quota of " << sales_quota
      << " bars\non or before the " << all_houses << "th house is "
      << cdf(nb, all_houses - sales_quota) << endl;
/*`
[pre
Probability of selling his quota of 5 bars
on or before the 30th house is 0.99849
]

/*`So the risk of failing even after visiting all the houses is 1 - this probability,
  ``1 - cdf(nb, all_houses - sales_quota``
But using this expression may cause serious inaccuracy,
so it would be much better to use the complement of the cdf:
So the risk of failing even at, or after, the 31th (non-existent) houses is 1 - this probability,
  ``1 - cdf(nb, all_houses - sales_quota)``
But using this expression may cause serious inaccuracy.
So it would be much better to use the complement of the cdf.
[link why_complements Why complements?]
*/
    cout << "\nProbability of failing to sell his quota of " << sales_quota
      << " bars\neven after visiting all " << all_houses << " houses is "
      << cdf(complement(nb, all_houses - sales_quota)) << endl;
/*`
[pre
Probability of failing to sell his quota of 5 bars
even after visiting all 30 houses is 0.0015101
]
We can also use the quantile (percentile), the inverse of the cdf, to
predict which house Pat will finish on.  So for the 8th house:
*/
 double p = cdf(nb, (8 - sales_quota));
 cout << "Probability of meeting sales quota on or before 8th house is "<< p << endl;
/*`
[pre
Probability of meeting sales quota on or before 8th house is 0.174
]
*/
    cout << "If the confidence of meeting sales quota is " << p
        << ", then the finishing house is " << quantile(nb, p) + sales_quota << endl;

    cout<< " quantile(nb, p) = " << quantile(nb, p) << endl;
/*`
[pre
If the confidence of meeting sales quota is 0.17367, then the finishing house is 8
]
Demanding absolute certainty that all 5 will be sold,
implies an infinite number of trials.
(Of course, there are only 30 houses on the estate,
so he can't ever be *certain* of selling his quota).
*/
    cout << "If the confidence of meeting sales quota is " << 1.
        << ", then the finishing house is " << quantile(nb, 1) + sales_quota << endl;
    //  1.#INF == infinity.
/*`[pre
If the confidence of meeting sales quota is 1, then the finishing house is 1.#INF
]
And similarly for a few other probabilities:
*/
    cout << "If the confidence of meeting sales quota is " << 0.
        << ", then the finishing house is " << quantile(nb, 0.) + sales_quota << endl;

    cout << "If the confidence of meeting sales quota is " << 0.5
        << ", then the finishing house is " << quantile(nb, 0.5) + sales_quota << endl;

    cout << "If the confidence of meeting sales quota is " << 1 - 0.00151 // 30 th
        << ", then the finishing house is " << quantile(nb, 1 - 0.00151) + sales_quota << endl;
/*`
[pre
If the confidence of meeting sales quota is 0, then the finishing house is 5
If the confidence of meeting sales quota is 0.5, then the finishing house is 11.337
If the confidence of meeting sales quota is 0.99849, then the finishing house is 30
]

Notice that because we chose a discrete quantile policy of real,
the result can be an 'unreal' fractional house.

If the opposite is true, we don't want to assume any confidence, then this is tantamount
to assuming that all the first sales_quota trials will be successful sales.
*/
    cout << "If confidence of meeting quota is zero\n(we assume all houses are successful sales)"
      ", then finishing house is " << sales_quota << endl;
/*`
[pre
If confidence of meeting quota is zero (we assume all houses are successful sales), then finishing house is 5
If confidence of meeting quota is 0, then finishing house is 5
]
We can list quantiles for a few probabilities:
*/

    double ps[] = {0., 0.001, 0.01, 0.05, 0.1, 0.5, 0.9, 0.95, 0.99, 0.999, 1.};
    // Confidence as fraction = 1-alpha, as percent =  100 * (1-alpha[i]) %
    cout.precision(3);
    for (int i = 0; i < sizeof(ps)/sizeof(ps[0]); i++)
    {
      cout << "If confidence of meeting quota is " << ps[i]
        << ", then finishing house is " << quantile(nb, ps[i]) + sales_quota
        << endl;
   }

/*`
[pre
If confidence of meeting quota is 0, then finishing house is 5
If confidence of meeting quota is 0.001, then finishing house is 5
If confidence of meeting quota is 0.01, then finishing house is 5
If confidence of meeting quota is 0.05, then finishing house is 6.2
If confidence of meeting quota is 0.1, then finishing house is 7.06
If confidence of meeting quota is 0.5, then finishing house is 11.3
If confidence of meeting quota is 0.9, then finishing house is 17.8
If confidence of meeting quota is 0.95, then finishing house is 20.1
If confidence of meeting quota is 0.99, then finishing house is 24.8
If confidence of meeting quota is 0.999, then finishing house is 31.1
If confidence of meeting quota is 1, then finishing house is 1.#INF
]

We could have applied a ceil function to obtain a 'worst case' integer value for house.
``ceil(quantile(nb, ps[i]))``

Or, if we had used the default discrete quantile policy, integer_outside, by omitting
``#define BOOST_MATH_DISCRETE_QUANTILE_POLICY real``
we would have achieved the same effect.

The real result gives some suggestion which house is most likely.
For example, compare the real and integer_outside for 95% confidence.

[pre
If confidence of meeting quota is 0.95, then finishing house is 20.1
If confidence of meeting quota is 0.95, then finishing house is 21
]
The real value 20.1 is much closer to 20 than 21, so integer_outside is pessimistic.
We could also use integer_round_nearest policy to suggest that 20 is more likely.

Finally, we can tabulate the probability for the last sale being exactly on each house.
*/
   cout << "\nHouse for " << sales_quota << "th (last) sale.  Probability (%)" << endl;
   cout.precision(5);
   for (int i = (int)sales_quota; i < all_houses+1; i++)
   {
     cout << left << setw(3) << i << "                             " << setw(8) << cdf(nb, i - sales_quota)  << endl;
   }
   cout << endl;
/*`
[pre
House for 5 th (last) sale.  Probability (%)
5                               0.01024
6                               0.04096
7                               0.096256
8                               0.17367
9                               0.26657
10                              0.3669
11                              0.46723
12                              0.56182
13                              0.64696
14                              0.72074
15                              0.78272
16                              0.83343
17                              0.874
18                              0.90583
19                              0.93039
20                              0.94905
21                              0.96304
22                              0.97342
23                              0.98103
24                              0.98655
25                              0.99053
26                              0.99337
27                              0.99539
28                              0.99681
29                              0.9978
30                              0.99849
]

As noted above, using a catch block is always a good idea, even if you do not expect to use it.
*/
  }
  catch(const std::exception& e)
  { // Since we have set an overflow policy of ignore_error,
    // an overflow exception should never be thrown.
     std::cout << "\nMessage from thrown exception was:\n " << e.what() << std::endl;
/*`
For example, without a ignore domain error policy, if we asked for ``pdf(nb, -1)`` for example, we would get:
[pre
Message from thrown exception was:
 Error in function boost::math::pdf(const negative_binomial_distribution<double>&, double):
 Number of failures argument is -1, but must be >= 0 !
]
*/
//] [/ negative_binomial_eg1_2]
  }
   return 0;
}  // int main()

/*

Output is:

Selling candy bars - using the negative binomial distribution.
by Dr. Diane Evans,
Professor of Mathematics at Rose-Hulman Institute of Technology,
see http://en.wikipedia.org/wiki/Negative_binomial_distribution
Pat has a sales per house success rate of 0.4.
Therefore he would, on average, sell 40 bars after trying 100 houses.
With a success rate of 0.4, he might expect, on average,
to need to visit about 12 houses in order to sell all 5 bars.
Probability that Pat finishes on the 5th house is 0.01024
Probability that Pat finishes on the 6th house is 0.03072
Probability that Pat finishes on the 7th house is 0.055296
Probability that Pat finishes on the 8th house is 0.077414
Probability that Pat finishes on or before the 8th house is sum
pdf(sales_quota) + pdf(6) + pdf(7) + pdf(8) = 0.17367
Probability of selling his quota of 5 bars
on or before the 8th house is 0.17367
Probability that Pat finishes exactly on the 10th house is 0.10033
Probability of selling his quota of 5 bars
on or before the 10th house is 0.3669
Probability that Pat finishes exactly on the 11th house is 0.10033
Probability of selling his quota of 5 bars
on or before the 11th house is 0.46723
Probability that Pat finishes exactly on the 12th house is 0.094596
Probability of selling his quota of 5 bars
on or before the 12th house is 0.56182
Probability that Pat finishes on the 30 house is 0.00069145
Probability of selling his quota of 5 bars
on or before the 30th house is 0.99849
Probability of failing to sell his quota of 5 bars
even after visiting all 30 houses is 0.0015101
Probability of meeting sales quota on or before 8th house is 0.17367
If the confidence of meeting sales quota is 0.17367, then the finishing house is 8
 quantile(nb, p) = 3
If the confidence of meeting sales quota is 1, then the finishing house is 1.#INF
If the confidence of meeting sales quota is 0, then the finishing house is 5
If the confidence of meeting sales quota is 0.5, then the finishing house is 11.337
If the confidence of meeting sales quota is 0.99849, then the finishing house is 30
If confidence of meeting quota is zero
(we assume all houses are successful sales), then finishing house is 5
If confidence of meeting quota is 0, then finishing house is 5
If confidence of meeting quota is 0.001, then finishing house is 5
If confidence of meeting quota is 0.01, then finishing house is 5
If confidence of meeting quota is 0.05, then finishing house is 6.2
If confidence of meeting quota is 0.1, then finishing house is 7.06
If confidence of meeting quota is 0.5, then finishing house is 11.3
If confidence of meeting quota is 0.9, then finishing house is 17.8
If confidence of meeting quota is 0.95, then finishing house is 20.1
If confidence of meeting quota is 0.99, then finishing house is 24.8
If confidence of meeting quota is 0.999, then finishing house is 31.1
If confidence of meeting quota is 1, then finishing house is 1.#J
House for 5th (last) sale.  Probability (%)
5                               0.01024
6                               0.04096
7                               0.096256
8                               0.17367
9                               0.26657
10                              0.3669
11                              0.46723
12                              0.56182
13                              0.64696
14                              0.72074
15                              0.78272
16                              0.83343
17                              0.874
18                              0.90583
19                              0.93039
20                              0.94905
21                              0.96304
22                              0.97342
23                              0.98103
24                              0.98655
25                              0.99053
26                              0.99337
27                              0.99539
28                              0.99681
29                              0.9978
30                              0.99849

*/


// 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.

#include <iostream>

#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix, typename Vector>
void inline power_iteration(const Matrix& A, Vector& v, double tau)
{
    assert(num_rows(A) == num_cols(A));               // A should be square
    v*= 1. / two_norm(v);                             // Normalize v
    Vector v2(size(v));
    do {
	swap(v, v2);                                  // Keep old value in v2
	v= A * v2;
	v*= 1. / two_norm(v);                         // Normalize
    } while (two_norm(Vector(v - v2)) >= tau);
}

template <typename Matrix, typename Vector>
bool inline check_eigenvector(const Matrix& A, Vector& v, double tau)
{
    Vector w(A * v);
    std::cout << "A * v is " << w << endl;

    typename mtl::Collection<Vector>::value_type alpha= two_norm(w) / two_norm(v);
    std::cout << "Eigenvalue alpha for v is " << alpha << endl;
    Vector w2(alpha * v);
    std::cout << "alpha * v is " << w2 << endl;

    bool close= two_norm(Vector(w - w2)) / two_norm(Vector(w + w2)) < tau;
    std::cout << "The results are " << (close ? "similar.\n" : "different.\n") << endl;
    return close;
}

int main(int, char**)
{
    double a_value[4][4] = {{0, 0, 1, .5},
			    {1/3., 0, 0, 0},
			    {1/3., .5, 0, .5},
			    {1/3., .5, 0, 0}};
    mtl::dense2D<double> A(a_value);
    mtl::dense_vector<double> v(4, 1.0);

    power_iteration(A, v, 0.00001);
    check_eigenvector(A, v, 0.001);

    mtl::compressed2D<double> B(9, 9);
    {
	mtl::mat::inserter<mtl::compressed2D<double> > ins(B);
	ins[0][1] << .2; ins[0][4] << .5;
	ins[1][0] << .5; ins[1][3] << 1; ins[1][4] << .5; ins[1][5] << .25; ins[1][6] << 1/3.;
	ins[2][2] << .1; ins[2][5] << .25;
	ins[3][1] << .2;
	ins[4][0] << .5; ins[4][1] << .2;
	ins[5][1] << .2; ins[5][6] << 1/3.; ins[5][8] << 1;
	ins[6][1] << .2; ins[6][5] << .25; ins[6][7] << 1;
	ins[7][6] << 1/3.;
	ins[8][5] << .25;
    }
    mtl::dense_vector<double> w(9, 1.0);

    power_iteration(B, w, 0.00001);
    check_eigenvector(B, w, 0.001);

    return 0;
}


    #include <Eigen/Dense>
    #include <Eigen/Sparse>
    #include <unsupported/Eigen/AutoDiff>

    #include <iostream>
    #include <cmath>

    template <typename T>
    T fun(Eigen::Matrix<T,Eigen::Dynamic,1> const &x){
       T y;
       y = x(0)*x(0)*x(0)*x(1) + x(0)*x(0)*x(1)*x(1)*x(1)*x(1); // f(x) = x[0]^3 * x[1]  + x[0]^2 * x[1]^4
       return y;
    }

    template <typename T>
	T dist(Eigen::Matrix<T,Eigen::Dynamic,1> const &x, Eigen::Matrix<T,Eigen::Dynamic,1> const &y){
	T r;
	r =x.dot(y);
	return r;
    }

    int main(){
       //normal use of fun
       {

	  typedef double scalar_t;
          typedef Eigen::Matrix<scalar_t,Eigen::Dynamic,1> input_t;
          input_t x(2);
          x.setConstant(1);
          scalar_t y = fun(x);
	  std::cout << "Normal use of function" << std::endl;
	  std::cout << y << std::endl;
       }
	std::cout << std::endl;
       //autodiff use of dist
       {
	typedef Eigen::Matrix<double,Eigen::Dynamic,1> vec;
	typedef Eigen::AutoDiffScalar<vec> AD;
	typedef Eigen::Matrix<AD,Eigen::Dynamic,1> ADvec;

	vec vec1, vec2;
	vec1 = vec::Random(3).normalized();
	vec2 = vec::Random(3).normalized();

	int s1 = vec1.size();
	int s2 = vec2.size();

	ADvec ax(s1);
	ADvec ay(s2);
	ax = vec1.cast<AD>();
	ay = vec2.cast<AD>();

	ax.setZero(s1);
	ay.setZero(s2);

	for(int i=0; i<s1; i++){
	    ax(i).derivatives().resize(s1);
	    ax(i).derivatives()(i)=1;
	    ay(i).derivatives().resize(s2);
	    ay(i).derivatives()(i)=1;
	}

	AD res = dist(ax,ay);
	std::cout << "\n Autodiff Dist function value: \n" << res.value() << std::endl;
        std::cout << "\n x-Derivatives\n " << res.derivatives() << std::endl;
//        std::cout << "\n y-Derivatives\n " << res.derivatives()(1) << std::endl;
       }

	std::cout << std::endl;
       //autodiff use of fun
       {
          typedef Eigen::Matrix<double,Eigen::Dynamic,1> derivative_t;
          typedef Eigen::AutoDiffScalar<derivative_t> scalar_t;
          typedef Eigen::Matrix<scalar_t,Eigen::Dynamic,1> input_t;
          input_t x(2);
          x.setConstant(1);

          //set unit vectors for the derivative directions (partial derivatives of the input vector)
          x(0).derivatives().resize(2);
          x(0).derivatives()(0)=1;
          x(1).derivatives().resize(2);
          x(1).derivatives()(1)=1;

          scalar_t y = fun(x);
	  std::cout << "Autodiff use of function" << std::endl;
          std::cout << "\nFunction:\n " << y.value() << std::endl;
          std::cout << "\nDerivatives\n " << y.derivatives() << std::endl;
       }
	std::cout << std::endl;
       //autodiff second derivative of fun
       {
          typedef Eigen::Matrix<double,Eigen::Dynamic,1> inner_derivative_t;
          typedef Eigen::AutoDiffScalar<inner_derivative_t> inner_scalar_t;
          typedef Eigen::Matrix<inner_scalar_t,Eigen::Dynamic,1> derivative_t;
          typedef Eigen::AutoDiffScalar<derivative_t> scalar_t;
          typedef Eigen::Matrix<scalar_t,Eigen::Dynamic,1> input_t;
          input_t x(2);
          x(0).value()=1;
          x(1).value()=1;

          //set unit vectors for the derivative directions (partial derivatives of the input vector)
          x(0).derivatives().resize(2);
          x(0).derivatives()(0)=1;
          x(1).derivatives().resize(2);
          x(1).derivatives()(1)=1;

          //repeat partial derivatives for the inner AutoDiffScalar
          x(0).value().derivatives() = inner_derivative_t::Unit(2,0);
          x(1).value().derivatives() = inner_derivative_t::Unit(2,1);

          //set the hessian matrix to zero
          for(int idx=0;idx<2;idx++){
             x(0).derivatives()(idx).derivatives()  = inner_derivative_t::Zero(2);
             x(1).derivatives()(idx).derivatives()  = inner_derivative_t::Zero(2);
          }

          scalar_t y = fun(x);
          //std::cout << y.value().value() << std::endl;
          //std::cout << y.value().derivatives() << std::endl;
          //std::cout << y.derivatives()(0).value() << std::endl;
          std::cout << y.derivatives()(0).derivatives() << std::endl;
         // std::cout << y.derivatives()(1).value() << std::endl;
          std::cout << y.derivatives()(1).derivatives() << std::endl;
       }
    }


// Implements non-natives linear algebra classes (Mat and Vec) - Felipe Figueredo Rocha
#ifndef _linalg_hpp
#define _linalg_hpp

#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <vector>
#include <stdlib.h>
#include <sstream>
#include <cmath>
//~ #include <gsl/gsl_matrix.h>
//~ #include <gsl/gsl_vector.h>
//~ #include <gsl/gsl_blas.h>
//~ #include <gsl/gsl_linalg.h>
#include <armadillo>

//~ using namespace arma;
using namespace std;

#define PRECISAO 12

// defines shortcuts for some most useds object types
template <class T> class Vec;
template <class T> class Mat;
typedef double SGPreal; // long double is not supported by GSL
typedef Vec<SGPreal> SGPrealVec;
typedef Mat<SGPreal> SGPrealMat;
typedef Vec<int> intVec;
typedef Mat<int> intMat;

// class numerical vector
template <class T> class Vec{
	public:
	T *v;
	int n;
	Vec() { n = 0; }
	Vec(int nn) { n= nn; v = new T[n]; (*this)=0.0; }
	void readNumberBlock(ifstream &file); // reads a block of n (size of the vector) numbers in a file
	void writeNumberBlock(ofstream &file); // idem to the last, but writes
	void writeNumberBlockRect(ofstream &file, int m);
	void print();
	void printH();
	T max();
	T min();
	T sum();
	T amax();
	T& operator()(int i) {return v[i];}
	T& operator[](int i) {return v[i];}
	void operator=(T a) { for(int i=0; i<n ; i++) v[i]=a;}
	void operator=(Vec &w) { for(int i=0; i<n ; i++) v[i]=w[i];}
	void operator=(arma::vec &w) { for(int i=0; i<n ; i++) v[i]=w[i];}
};

template<class T> void Vec<T>:: print(){
	for(int i = 0; i<n ; i++) cout << v[i] << endl;
}

template<class T> void Vec<T>:: printH(){
	for(int i = 0; i<n ; i++) cout << v[i] << " ";
	cout << endl;
}

template <class T> void Vec<T> :: readNumberBlock(ifstream &file){
	//~ file << fixed << setprecision(PRECISAO);
	for(int i = 0; i<n ; i++) file  >> v[i] ;
}

template <class T> void Vec<T> :: writeNumberBlock(ofstream &file){
	file << scientific << setprecision(PRECISAO);
	for(int i = 0; i<n ; i++) file  << v[i] << endl ;
}

template <class T> void Vec<T> :: writeNumberBlockRect(ofstream &file,int m){
	if(n%m==0){
		int nn = n/m;
		int ip = 0;
		file << scientific << setprecision(PRECISAO);
		for(int i = 0; i<nn ; i++){
			ip = i*m;
			for(int j = 0; j<m ; j++){
				file  << v[ip + j] << " " ;
			}
			file << endl ;
		}
	} else {
		writeNumberBlock(file);
	}
}

template <class T> T Vec<T> :: max(){
	T vmax = -9999.0;

	for(int i = 0; i<n ; i++){
		if(v[i]>vmax) vmax = v[i];
	}

	return vmax;
}

template <class T> T Vec<T> :: sum(){
	T vsum = 0.0;

	for(int i = 0; i<n ; i++) vsum += v[i];

	return vsum;
}

template <class T> T Vec<T> :: amax(){
	T vmax = 0.0;
	T aux;

	for(int i = 0; i<n ; i++){
		aux = abs(v[i]);
		if(aux>vmax) vmax = aux;
	}

	return vmax;
}

// class numerical matrix as a spelization of the Vec. All the elements are in a Vec, but can be acessed with two indices. Row-major convention (C convention)
template <class T> class Mat : public Vec<T>{
	public:
	int m1,m2; // m1 is Nrows, m2 is Ncolumns
	Mat() {}
	Mat(int mm1, int mm2):Vec<T>(mm1*mm2) { m1 = mm1; m2 = mm2;  }
	Mat(Vec<T> &V, int mm1, int mm2) { m1 = mm1; m2 = mm2; this->v = V.v; }

	//~ T& operator()(int i,int j) {return this->v[j*m1 + i];} Fortran style
	T& operator()(int i,int j) {return this->v[i*m2 + j];} // C style, compatibility with GSL
	void operator=(T a) { for(int i=0; i<this->n ; i++) this->v[i]=a;}
	void operator=(Mat &w) { for(int i=0; i<this->n ; i++) this->v[i]=w->v[i];}
	void prettyPrint();
	void prettyPrintClean();
	void solve(SGPrealVec &x,SGPrealVec &b); // solves linear system using GSL
};

template<class T> void Mat<T> :: prettyPrint(){
	//~ cout << m1 << " " << m2 << endl;
	for (int i=0;i<m1;i++){
        for (int j=0;j<m2;j++) cout << "a[" << i << "][" << j << "]=" << (*this)(i,j) << "  " ;
        cout << endl;
    }
}

template<class T> void Mat<T> :: prettyPrintClean(){
	//~ cout << m1 << " " << m2 << endl;
	for (int i=0;i<m1;i++){
        for (int j=0;j<m2;j++) cout << (*this)(i,j) << "  " ;
        cout << endl;
    }
}

template<class T> T dot_product(Vec<T> &u,Vec<T> &w){
	T dot = 0.0;

	for(int i=0; i<u.n ; i++) dot += u(i)*w(i);

	return dot;
}

template<class T> void Mat<T> :: solve(SGPrealVec &x,SGPrealVec &b){
	// creates GSL objects
	//~ gsl_matrix_view gslA=gsl_matrix_view_array(this->v,this->m1,this->m2);
	//~ gsl_vector_view gslB=gsl_vector_view_array(b.v,b.n);
	//~ gsl_vector_view gslX=gsl_vector_view_array(x.v,x.n);
	//~ gsl_permutation *gslP=gsl_permutation_alloc(b.n);
	//~
	//~ int sig;
      //~
    //~ // solve system
	//~ gsl_linalg_LU_decomp(&gslA.matrix,gslP,&sig);
    //~ gsl_linalg_LU_solve(&gslA.matrix,gslP,&gslB.vector,&gslX.vector);
}

double computeError(arma::vec &v, SGPrealVec &v0){
	double error = 0.0;
	arma::vec e(v0.n);

	for(int i=0;i<v0.n;i++) e(i) = v(i) - v0(i);

	double emax = arma::abs(e).max();
	double v0max = v0.amax();

	if(v0max >0.0){
		error = emax/v0max;
	}
	else{
		error = emax;
	}

	return error;
}

double computeError(SGPrealVec &v,SGPrealVec &v0){

	SGPrealVec e(v.n);
	double error = 0.0;

	for(int i=0; i<v.n ; i++) e(i) = v(i) - v0(i);

	double emax = e.amax();
	double v0max = v0.amax();

	if(v0max >0.0){
		error = emax/v0max;
	}
	else{
		error = emax;
	}

	return error;
}

#endif


/**
 * @file matode_main.cc
 * @brief NPDE homework MatODE code
 * @copyright Developed at ETH Zurich
 */

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

#include "matode.h"

int main(int /*argc*/, char** /*argv*/) {
  /* SAM_LISTING_BEGIN_6 */
  double h = 0.01;  // stepsize
  //Eigen::Vector3d norms;
  // Build M
  Eigen::Matrix3d M;
  M << 8, 1, 6, 3, 5, 7, 9, 9, 2;
  // Build A
  Eigen::Matrix3d A;
  A << 0, 1, 1, -1, 0, 1, -1, -1, 0;
  Eigen::MatrixXd I = Eigen::Matrix3d::Identity();
  //====================
  // Your code goes here
  //====================

  using namespace MatODE;

  // y0 is Q in M = QR
  Eigen::MatrixXd Y01, Y02, Y03;
  Eigen::HouseholderQR<Eigen::MatrixXd> qr(M);
  Y01 = Y02 = Y03 = qr.householderQ();

  Eigen::MatrixXd norms = Eigen::MatrixXd::Zero(20, 3); // stores frobenius norms using 3 different methods
  // perform 20 steps using the implemented methods
  for(int i = 0; i < 20; ++i) {
    // explicit Euler step
    Eigen::MatrixXd Y1 = eeulstep(A, Y01, h);
    norms(i, 0) = (Y1.transpose()*Y1 - I).norm();
    Y01 = Y1;
    // implicit Euler step
    Y1 = ieulstep(A, Y02, h);
    norms(i, 1) = (Y1.transpose()*Y1 - I).norm();
    Y02 = Y1;
    // implicit Euler step
    Y1 = impstep(A, Y03, h);
    norms(i, 2) = (Y1.transpose()*Y1 - I).norm();
    Y03 = Y1;
  }

  std::cout << "Norms:" << std::endl;
  std::cout << norms << std::endl;
  /* SAM_LISTING_END_6 */
  return 0;
}


/**
 * @file exponentialintegrator.cc
 * @brief NPDE homework ExponentialIntegrator code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include "exponentialintegrator.h"

#include <Eigen/Core>
#include <cassert>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>

namespace ExponentialIntegrator {

// Function $\phi$ used in the Exponential Euler
// single step method for an autonomous ODE.
Eigen::MatrixXd phim(const Eigen::MatrixXd &Z) {
  int n = Z.cols();
  assert(n == Z.rows() && "Matrix must be square.");
  Eigen::MatrixXd C(2 * n, 2 * n);
  C << Z, Eigen::MatrixXd::Identity(n, n), Eigen::MatrixXd::Zero(n, 2 * n);
  return C.exp().block(0, n, n, n);
}

void testExpEulerLogODE() {
  /* SAM_LISTING_BEGIN_0 */
  // Final time
  double T = 1.0;
  // Initial value
  Eigen::VectorXd y0(1);
  y0 << 0.1;
  // Function and Jacobian and exact solution
  auto f = [](const Eigen::VectorXd &y) { return y(0) * (1.0 - y(0)); };
  auto df = [](const Eigen::VectorXd &y) {
    Eigen::MatrixXd dfy(1, 1);
    dfy << 1.0 - 2.0 * y(0);
    return dfy;
  };
  double exactyT = y0(0) / (y0(0) + (1.0 - y0(0)) * std::exp(-T));

  // Container for errors
  std::vector<double> error(15);

  // Test many step sizes
  for (int j = 0; j < 15; ++j) {
    int M = std::pow(2, j + 1);
    Eigen::VectorXd y = y0;
    double h = T / M;
    //====================
    // Your code goes here
    // TODO: Perform N timesteps with inital data y0 and store the result in y.
    //====================

    error[j] = std::abs(y(0) - exactyT);
    std::cout << std::left << std::setfill(' ') << std::setw(3)
              << "M = " << std::setw(7) << M << std::setw(8)
              << "Error = " << std::setw(13) << error[j];
    if (j > 0) {
      std::cout << std::left << std::setfill(' ') << std::setw(10)
                << "Approximated order = " << std::log2(error[j - 1] / error[j])
                << std::endl;
    } else
      std::cout << std::endl;
  }
  /* SAM_LISTING_END_0 */
}

}  // namespace ExponentialIntegrator


#pragma once

#include <tuple>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include "geometry.hpp"

namespace rt {
	class OnlineCovarianceMatrix2x2 {
	public:
		void addSample(Vec2 x) {
			_00.addSample(x[0], x[0]);
			_11.addSample(x[1], x[1]);
			_01.addSample(x[0], x[1]);
		}
		Eigen::Matrix2d sampleCovarianceMatrix() const {
			Eigen::Matrix2d cov;
			cov(0, 0) = _00.sampleCovariance();
			cov(1, 1) = _11.sampleCovariance();
			cov(0, 1) = cov(1, 0) = _01.sampleCovariance();
			return cov;
		}
	private:
		OnlineCovariance _00;
		OnlineCovariance _11;
		OnlineCovariance _01;
	};
	class OnlineCovarianceMatrix3x3 {
	public:
		void addSample(Vec3 x) {
			_00.addSample(x[0], x[0]);
			_11.addSample(x[1], x[1]);
			_22.addSample(x[2], x[2]);
			_01.addSample(x[0], x[1]);
			_02.addSample(x[0], x[2]);
			_12.addSample(x[1], x[2]);
		}
		Eigen::Matrix3d sampleCovarianceMatrix() const {
			Eigen::Matrix3d cov;
			cov(0, 0) = _00.sampleCovariance();
			cov(1, 1) = _11.sampleCovariance();
			cov(2, 2) = _22.sampleCovariance();
			cov(0, 1) = cov(1, 0) = _01.sampleCovariance();
			cov(0, 2) = cov(2, 0) = _02.sampleCovariance();
			cov(1, 2) = cov(2, 1) = _12.sampleCovariance();
			return cov;
		}
	private:
		OnlineCovariance _00;
		OnlineCovariance _11;
		OnlineCovariance _22;
		OnlineCovariance _01;
		OnlineCovariance _02;
		OnlineCovariance _12;
	};

	/*
	主成分分析 2x2 mat
	*/
	inline std::tuple<Vec2, Vec2> PCA(const Eigen::Matrix2d covarianceMatrix) {
		Eigen::EigenSolver<Eigen::Matrix2d> es(covarianceMatrix);
		auto eigenvectors = es.eigenvectors();
		Vec2 xaxis(
			eigenvectors(0, 0).real(),
			eigenvectors(1, 0).real()
		);
		Vec2 yaxis(
			eigenvectors(0, 1).real(),
			eigenvectors(1, 1).real()
		);
		return{ xaxis, yaxis };
	}

	/*
	主成分分析 3x3 mat
	*/
	inline std::tuple<Vec3, Vec3, Vec3> PCA(const Eigen::Matrix3d covarianceMatrix) {
		Eigen::EigenSolver<Eigen::Matrix3d> es(covarianceMatrix);
		auto eigenvectors = es.eigenvectors();
		Vec3 xaxis(
			eigenvectors(0, 0).real(),
			eigenvectors(1, 0).real(),
			eigenvectors(2, 0).real()
		);
		Vec3 yaxis(
			eigenvectors(0, 1).real(),
			eigenvectors(1, 1).real(),
			eigenvectors(2, 1).real()
		);
		Vec3 zaxis(
			eigenvectors(0, 2).real(),
			eigenvectors(1, 2).real(),
			eigenvectors(2, 2).real()
		);

		return{ xaxis , yaxis, zaxis };
	}
}


/* @copyright The code is licensed under the MIT License
 *            <https://opensource.org/licenses/MIT>,
 *            Copyright (c) 2020 Christian Eskil Vaugelade Berg
 * @author Christian Eskil Vaugelade Berg
*/
#pragma once

#include <orient/axis.hpp>

#include <Eigen/Dense>

namespace orient {

template<Axis axis, typename Scalar>
Eigen::Matrix<Scalar,3,3> rotationMatrixFromEuler(Scalar angle)
{
  const auto s = std::sin(angle);
  const auto c = std::cos(angle);

  constexpr auto i1 = asIndex<axis>;
  constexpr auto i2 = (i1 + 1) % 3;
  constexpr auto i3 = (i2 + 1) % 3;
  Eigen::Matrix<Scalar,3,3> R = Eigen::Matrix<Scalar,3,3>::Zero();
  R(i1, i1) = 1.0;
  R(i2, i2) = c;
  R(i3, i3) = c;
  R(i2, i3) = -s;
  R(i3, i2) = s;
  return R;
}

template<Axis axis, typename Scalar>
std::pair<Eigen::Matrix<Scalar,3,3>, Eigen::Matrix<Scalar,3,3>> rotationMatrixFromEulerWD(Scalar angle)
{
  const auto s = std::sin(angle);
  const auto c = std::cos(angle);

  constexpr auto i1 = asIndex<axis>;
  constexpr auto i2 = (i1 + 1) % 3;
  constexpr auto i3 = (i2 + 1) % 3;
  Eigen::Matrix<Scalar,3,3> J = Eigen::Matrix<Scalar,3,3>::Zero();
  Eigen::Matrix<Scalar,3,3> R = Eigen::Matrix<Scalar,3,3>::Zero();
  R(i1, i1) = 1.0;
  R(i2, i2) = c;
  J(i2, i2) = -s;

  R(i3, i3) = c;
  J(i3, i3) = -s;

  R(i2, i3) = -s;
  J(i2, i3) = -c;

  R(i3, i2) = s;
  J(i3, i2) = c;
  return std::make_pair(R, J);
}

template<Axis A1, Axis A2, Axis A3, typename Scalar>
Eigen::Matrix<Scalar,3,3> rotationMatrixFromEuler(Eigen::Matrix<Scalar,3,1> const& angles)
{
  return
    rotationMatrixFromEuler<A1>(angles[0]) *
    rotationMatrixFromEuler<A2>(angles[1]) *
    rotationMatrixFromEuler<A3>(angles[2]);
}

template<Axis A1, Axis A2, Axis A3, typename Scalar>
std::pair<Eigen::Matrix<Scalar,3,3>, Eigen::Matrix<Scalar, 9, 3>> rotationMatrixFromEulerWD(Eigen::Matrix<Scalar,3,1> const& angles)
{
  const auto [R1, J1] = rotationMatrixFromEulerWD<A1>(angles[0]);
  const auto [R2, J2] = rotationMatrixFromEulerWD<A2>(angles[1]);
  const auto [R3, J3] = rotationMatrixFromEulerWD<A3>(angles[2]);

  Eigen::Matrix<Scalar, 9, 3> J;
  Eigen::Map<Eigen::Matrix<Scalar,3,3>>(J.template block<9,1>(0,0).data(), 3, 3) = J1 * R2 * R3;
  Eigen::Map<Eigen::Matrix<Scalar,3,3>>(J.template block<9,1>(0,1).data(), 3, 3) = R1 * J2 * R3;
  Eigen::Map<Eigen::Matrix<Scalar,3,3>>(J.template block<9,1>(0,2).data(), 3, 3) = R1 * R2 * J3;
  return std::make_pair(R1 * R2 * R3, J);
}

}


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

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

  // Simple initialization.
  Eigen::Matrix<double, 3, 3> square3;
  square3 << 2, 1, 3, 5, 7, 8, 9, 12, 11;
  std::cout << square3 << std::endl;
  std::cout << "Trace: ";
  std::cout << square3.trace() << std::endl;

  std::cout << "Max: ";
  int mr, mc;
  std::cout << square3.maxCoeff(&mr, &mc) << std::endl;
  std::cout << mr << " , " << mc << std::endl;

  std::cout << "Min: ";
  std::cout << square3.minCoeff(&mr, &mc) << " at " << std::endl;
  std::cout << mr << " , " << mc << std::endl;

  // Invert it.
  Eigen::Matrix3d inv_square3 = square3.inverse();
  std::cout << inv_square3 << std::endl;
  std::cout << "Trace: ";
  std::cout << inv_square3.trace() << std::endl;

  // Example of solve.
  Eigen::Matrix2f A, b;
  A << 2, -1, -1, 3;
  b << 1, 2, 3, 1;
  std::cout << "Here is the matrix A:\n" << A << std::endl;
  std::cout << "Here is the right hand side b:\n" << b << std::endl;
  Eigen::Matrix2f x = A.llt().solve(b);
  std::cout << "The solution is:\n" << x << std::endl;

  return 0;
}


#define BOOST_TEST_MODULE kinetic energy multivar normal
#define BOOST_TEST_DYN_LINK
#include <cmath>
#include <random>
#include <limits>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/test/unit_test.hpp>
#include <mpp/hamiltonian/kinetic_energy_multivar_normal.hpp>

template<typename real_scalar_type>
void test_multivariate_normal_diag()
{
    using namespace mpp::hamiltonian;
    using namespace boost::numeric::ublas;

    typedef multivariate_normal<real_scalar_type> multivariate_normal_type;
    typedef vector<real_scalar_type> real_vector_type;
    typedef std::mt19937 rng_type;

    std::size_t const num_dims = 1000000;

    real_vector_type sigma_inv(num_dims);
    for(std::size_t i=0;i<num_dims;++i)
    {
        sigma_inv(i) = real_scalar_type(1);
    }

    multivariate_normal_type mlt_nr(sigma_inv);

    // compute the log_posterior for a ones vector
    real_vector_type p(num_dims);
    for(std::size_t i=0;i<num_dims;++i)
    {
        p(i) = real_scalar_type(1);
    }

    real_scalar_type log_posterior_c = mlt_nr.log_posterior(p);

    real_scalar_type log_posterior_e(0);
    for(std::size_t i=0;i<num_dims;++i)
    {
        log_posterior_e -= p(i)*p(i)*sigma_inv(i);
    }
    log_posterior_e *= real_scalar_type(0.5);

    real_scalar_type eps = std::numeric_limits<real_scalar_type>::epsilon();
    BOOST_CHECK(
        std::abs(log_posterior_c - log_posterior_e) <= eps
    );

    real_vector_type grad_p_c = mlt_nr.grad_log_posterior(p);

    real_vector_type grad_p_e(num_dims);
    for(std::size_t i=0;i<num_dims;++i)
    {
        grad_p_e(i) = -sigma_inv(i)*p(i);
    }

    for(std::size_t i=0;i<num_dims;++i)
    {
        BOOST_CHECK(
            std::abs(grad_p_c(i) - grad_p_e(i) ) <= eps
        );
    }

    rng_type rng;
    real_vector_type samp_p = mlt_nr.generate_sample(rng);

    real_scalar_type sum(0);
    real_scalar_type sum2(0);
    for(std::size_t i=0;i<num_dims;++i)
    {
        sum += samp_p(i);
        sum2 += samp_p(i)*samp_p(i);
    }
    real_scalar_type mean = sum/real_scalar_type(num_dims);
    real_scalar_type std = std::sqrt( sum2/real_scalar_type(num_dims)
        - mean*mean );

    BOOST_CHECK(
        std::abs(mean) <=
            real_scalar_type(1)/std::sqrt(real_scalar_type(num_dims))
    );

    BOOST_CHECK(
        std::abs( std - real_scalar_type(1) ) <= real_scalar_type(1)/std::sqrt(real_scalar_type(num_dims))
    );

}

BOOST_AUTO_TEST_CASE(kinetic_energy_multivar_normal_diag)
{
    test_multivariate_normal_diag<float>();
    test_multivariate_normal_diag<double>();
    test_multivariate_normal_diag<long double>();
}


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

using namespace std;
using namespace Eigen;

int main()
{
  MatrixXf m(4,4);

  m << 1, 2, 3, 4,
       5, 6, 7, 8,
       9, 10,11,12,
       13,14,15,16;

  //print first two columns
  cout << "-- leftCols(2) --" << endl
    << m.leftCols(2) << endl << endl;

  //print last two rows
  cout << "-- bottomRows(2) --" << endl
    << m.bottomRows(2) << endl << endl;

  //print top-left 2x3 corner
  cout << "-- topLeftCorner(2,3) --" << endl
    << m.topLeftCorner(2,3) << endl;
}


#ifndef VECTOR_MATH_HPP
# define VECTOR_MATH_HPP

#include <Eigen/Dense>

using Eigen::Matrix3d;
using Eigen::Vector3d;

const double ANGLE_SMALL = 1e-12;

Matrix3d skew(const Vector3d& a) {
  Matrix3d ax;
  ax << 0.0, -a(2), a(1),
    a(2), 0.0, -a(0),
    -a(1), a(0), 0.0;
  return ax;
}

/** @brief Convert a rotation vector to a DCM.
 *
 * @param[in]   v   rotation vector (unit rotation axis multiplied by
 *                  angle about that vector)
 *
 * @returns A 3D matrix.
 */
Matrix3d rotvec_to_matrix(const Vector3d& v) {
  double v_mag = v.norm();
  double c     = std::cos(v_mag);
  double s     = std::sin(v_mag);

  Vector3d vu = v / v_mag;

  Matrix3d T;
  if (v_mag < ANGLE_SMALL) T = Matrix3d::Identity();
  else {
    // Reference: https://github.com/mohawkjohn/pyquat/blob/master/pyquat/pyquat.c
    // Line 932
    T << c + vu[0]*vu[0] * (1.0 - c),
      vu[0]*vu[1] * (1.0 - c) + vu[2] * s,
      vu[0]*vu[2] * (1.0 - c) - vu[1] * s,
      vu[0]*vu[1] * (1.0 - c) - vu[2] * s,
      c + vu[1]*vu[1] * (1.0 - c),
      vu[1]*vu[2] * (1.0 - c) + vu[0] * s,
      vu[0]*vu[2] * (1.0 - c) + vu[1] * s,
      vu[1]*vu[2] * (1.0 - c) - vu[0] * s,
      c + vu[2]*vu[2] * (1.0 - c);
  }

  return T;
}

#endif


#pragma once
#include "coordinate_transform.hpp"
#include "grad_shape.hpp"
#include "integrate.hpp"
#include "shape.hpp"
#include <Eigen/Core>

//! Makes the matrix corresponding to the neumann boundary conditions
//!
template <class MatrixType, class Point>
void computeBoundaryMatrix(MatrixType & boundaryMatrix,
                           const Point &a,
                           const Point &b,
                           double       gamma) {
	// (write your solution here)
	boundaryMatrix.resize(2, 2);

	for (int i = 0; i < 2; ++i) {
		for (int j = 0; j < 2; ++j) {
			auto f = [&](double t) -> double {
				return lambda(i, t / 2.0 + 1, 0) * lambda(j, t / 2.0 + 1, 0);
			};

			double scaling_factor = (b - a).norm() / 2.0;
			boundaryMatrix(i, j) = scaling_factor * gamma * integrate1d(f);
		}
	}
}


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)
/*
    boost::math::fft example 07

    Real transforms with GSL, FFTW and Boost backends
*/

#include <boost/random.hpp>
#include <boost/math/fft/bsl_backend.hpp>
#include <boost/math/fft/fftw_backend.hpp>
#include <boost/math/fft/gsl_backend.hpp>
#include <boost/math/fft/real_algorithms.hpp>
#include <iostream>
#include <vector>
#include <complex>

template<class T>
void print(const std::vector<T>& V)
{
  std::cout << "size(V) = " << V.size() << "\n";
  std::cout << "[";
  for(auto x: V)
  {
    std::cout << x << ", ";
  }
  std::cout << "]\n";
}

void check(int n)
{
  boost::random::mt19937 rng;
  boost::random::uniform_real_distribution<double> U(0.0,1.0);

  std::vector< double > V(n);
  for(auto& x: V)
      x = U(rng);
  std::cout << "Original data:\n";
  print(V);

  std::vector<double> A,B,C;

  boost::math::fft::fftw_rdft< double > P1(V.size());
  P1.real_to_halfcomplex(V.begin(),V.end(),std::back_inserter(A));
  //std::cout << "FFTW:\n";
  //print(A);

  boost::math::fft::gsl_rdft< double > P2(V.size());
  P2.real_to_halfcomplex(V.begin(),V.end(),std::back_inserter(B));
  //std::cout << "GSL:\n";
  //print(B);

  boost::math::fft::bsl_rdft< double > P3(V.size());
  P3.real_to_halfcomplex(V.begin(),V.end(),std::back_inserter(C));
  //std::cout << "Boost:\n";
  //print(C);

  std::cout << "Boost inverse:\n";
  P3.halfcomplex_to_real(C.begin(),C.end(),C.begin());
  const double inv_n = 1.0/C.size();
  for(auto &x : C) x *= inv_n;
  print(C);

  std::vector< std::complex<double> > cplx_V(V.begin(),V.end()),cplx_C;
  boost::math::fft::bsl_dft< std::complex<double> > P4(V.size());
  P4.forward(cplx_V.begin(),cplx_V.end(),std::back_inserter(cplx_C));
  //std::cout << "Boost complex:\n";
  //print(cplx_C);
}

int main()
{
  check(1);

  check(2);
  check(3);
  check(5);
  check(7);

  check(4);
  check(8);
  check(16);
  check(32);

  check(6);
  check(9);
  check(10);
  check(12);
  return 0;
}


#pragma once

#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>

namespace common_robotics_utilities
{
namespace conversions
{
Eigen::Quaterniond QuaternionFromRPY(const double R,
                                     const double P,
                                     const double Y);

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Quaterniond QuaternionFromUrdfRPY(const double R,
                                         const double P,
                                         const double Y);

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromRotationMatrix(
    const Eigen::Matrix3d& rot_matrix);

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromQuaternion(const Eigen::Quaterniond& quat);

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromIsometry3d(const Eigen::Isometry3d& trans);

Eigen::Isometry3d TransformFromXYZRPY(const double x,
                                      const double y,
                                      const double z,
                                      const double roll,
                                      const double pitch,
                                      const double yaw);

Eigen::Isometry3d TransformFromRPY(const Eigen::Vector3d& translation,
                                   const Eigen::Vector3d& rotation);

Eigen::Isometry3d TransformFromRPY(const Eigen::VectorXd& components);

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfXYZRPY(const double x,
                                          const double y,
                                          const double z,
                                          const double roll,
                                          const double pitch,
                                          const double yaw);

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::Vector3d& translation,
                                       const Eigen::Vector3d& rotation);

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::VectorXd& components);

Eigen::VectorXd TransformToRPY(const Eigen::Isometry3d& transform);

Eigen::Vector3d StdVectorDoubleToEigenVector3d(
    const std::vector<double>& vector);

Eigen::VectorXd StdVectorDoubleToEigenVectorXd(
    const std::vector<double>& vector);

std::vector<double> EigenVector3dToStdVectorDouble(
    const Eigen::Vector3d& point);

std::vector<double> EigenVectorXdToStdVectorDouble(
    const Eigen::VectorXd& eigen_vector);

// Takes <x, y, z, w> as is the ROS custom!
Eigen::Quaterniond StdVectorDoubleToEigenQuaterniond(
    const std::vector<double>& vector);

// Returns <x, y, z, w> as is the ROS custom!
std::vector<double> EigenQuaterniondToStdVectorDouble(
    const Eigen::Quaterniond& quat);

Eigen::Vector3d GeometryPointToEigenVector3d(
    const geometry_msgs::Point& point);

geometry_msgs::Point EigenVector3dToGeometryPoint(
    const Eigen::Vector3d& point);

Eigen::Vector4d GeometryPointToEigenVector4d(
    const geometry_msgs::Point& point);

geometry_msgs::Point EigenVector4dToGeometryPoint(
    const Eigen::Vector4d& point);

geometry_msgs::PointStamped EigenVector3dToGeometryPointStamped(
    const Eigen::Vector3d& point, const std::string& frame_id);

Eigen::Vector3d GeometryVector3ToEigenVector3d(
    const geometry_msgs::Vector3& vector);

geometry_msgs::Vector3 EigenVector3dToGeometryVector3(
    const Eigen::Vector3d& vector);

Eigen::Vector4d GeometryVector3ToEigenVector4d(
    const geometry_msgs::Vector3& vector);

geometry_msgs::Vector3 EigenVector4dToGeometryVector3(
    const Eigen::Vector4d& vector);

Eigen::Quaterniond GeometryQuaternionToEigenQuaterniond(
    const geometry_msgs::Quaternion& quat);

geometry_msgs::Quaternion EigenQuaterniondToGeometryQuaternion(
    const Eigen::Quaterniond& quat);

Eigen::Isometry3d GeometryPoseToEigenIsometry3d(
    const geometry_msgs::Pose& pose);

geometry_msgs::Pose EigenIsometry3dToGeometryPose(
    const Eigen::Isometry3d& transform);

geometry_msgs::PoseStamped EigenIsometry3dToGeometryPoseStamped(
    const Eigen::Isometry3d& transform, const std::string& frame_id);

Eigen::Isometry3d GeometryTransformToEigenIsometry3d(
    const geometry_msgs::Transform& transform);

geometry_msgs::Transform EigenIsometry3dToGeometryTransform(
    const Eigen::Isometry3d& transform);

geometry_msgs::TransformStamped EigenIsometry3dToGeometryTransformStamped(
    const Eigen::Isometry3d& transform, const std::string& frame_id,
    const std::string& child_frame_id);

Eigen::Matrix3Xd VectorGeometryPointToEigenMatrix3Xd(
    const std::vector<geometry_msgs::Point>& vector_geom);

std::vector<geometry_msgs::Point> EigenMatrix3XdToVectorGeometryPoint(
    const Eigen::Matrix3Xd& eigen_matrix);

std::vector<geometry_msgs::Point>
VectorEigenVector3dToVectorGeometryPoint(
    const common_robotics_utilities::math::VectorVector3d& vector_eigen);

common_robotics_utilities::math::VectorVector3d
VectorGeometryPointToVectorEigenVector3d(
    const std::vector<geometry_msgs::Point>& vector_geom);

common_robotics_utilities::math::VectorVector3d
VectorGeometryVector3ToEigenVector3d(
    const std::vector<geometry_msgs::Vector3>& vector_geom);

common_robotics_utilities::math::VectorIsometry3d
VectorGeometryPoseToVectorIsometry3d(
    const std::vector<geometry_msgs::Pose>& vector_geom);

common_robotics_utilities::math::VectorIsometry3d
VectorGeometryPoseToVectorIsometry3d(
    const std::vector<geometry_msgs::Transform>& vector_geom);

std::vector<geometry_msgs::Pose> VectorIsometry3dToVectorGeometryPose(
    const common_robotics_utilities::math::VectorIsometry3d& vector_eigen);

std::vector<geometry_msgs::Transform> VectorIsometry3dToVectorGeometryTransform(
    const common_robotics_utilities::math::VectorIsometry3d& vector_eigen);
}  // namespace conversions
}  // namespace common_robotics_utilities


#include "problemes.h"
#include "arithmetique.h"
#include "timer.h"

#include <boost/range/adaptor/reversed.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

namespace {
    nombre maximum(const vecteur &fibonacci, const nombre &n) {
        for (const auto &f: boost::adaptors::reverse(fibonacci)) {
            if (f < n)
                return f;
        }

        return fibonacci.back();
    }

    nombre Zeckendorf(const vecteur &fibonacci, const nombre n) {
        if (n < 2)
            return 0;

        static std::map<nombre, nombre> cache;

        if (auto it = cache.find(n);it != cache.end())
            return it->second;

        nombre m = maximum(fibonacci, n);

        nombre resultat = n - m + Zeckendorf(fibonacci, m) + Zeckendorf(fibonacci, n - m);

        cache[n] = resultat;
        return resultat;
    }
}

ENREGISTRER_PROBLEME(297, "Zeckendorf Representation") {
    // Each new term in the Fibonacci sequence is generated by adding the previous two terms. Starting with 1 and 2, the
    // first 10 terms will be: 1, 2, 3, 5, 8, 13, 21, 34, 55, 89.
    //
    // Every positive integer can be uniquely written as a sum of nonconsecutive terms of the Fibonacci sequence. For
    // example, 100 = 3 + 8 + 89. Such a sum is called the Zeckendorf representation of the number.
    //
    // For any integer n>0, let z(n) be the number of terms in the Zeckendorf representation of n.
    // Thus, z(5) = 1, z(14) = 2, z(100) = 3 etc.
    // Also, for 0<n<10**6, ∑ z(n) = 7894453.
    //
    // Find ∑ z(n) for 0<n<10**17.
    const auto limite = puissance::puissance<nombre, unsigned>(10, 17);
    vecteur fibonacci{1, 1};
    while (fibonacci.back() < limite) {
        fibonacci.push_back(fibonacci.back() + fibonacci.at(fibonacci.size() - 2));
    }

    nombre resultat = Zeckendorf(fibonacci, limite);
    return std::to_string(resultat);
}


//
// Helper functions for tests.
//

#pragma once

#include <ilqr/ilqr_taylor_expansions.hh>

#include <Eigen/Dense>

#include <vector>

ilqr::DynamicsFunc create_linear_dynamics(const Eigen::MatrixXd &A,
        const Eigen::MatrixXd &B);

// Creates cost function of form 0.5*[(x-x_goal)^T Q (x-x_goal) + u^T R u]
ilqr::CostFunc create_quadratic_cost(const Eigen::MatrixXd &Q,
        const Eigen::MatrixXd &R,
        const Eigen::VectorXd &x_goal);

// Creates cost function of form 0.5*[x^T Q x + u^T R u]
ilqr::CostFunc create_quadratic_cost(const Eigen::MatrixXd &Q,
        const Eigen::MatrixXd &R);

// Computes \sum_i cost(x_i, u_i)
double compute_total_cost(const ilqr::CostFunc &cost,
                          const std::vector<Eigen::VectorXd> &states,
                          const std::vector<Eigen::VectorXd> &controls);

// Returns a randomly generated [dim x dim] PSD matrix with specified
// minimum eigen value.
Eigen::MatrixXd make_random_psd(const int dim, const double min_eig_val);

// Linearly interpolate from x_t0 -> x_T corresponding to time steps t0->T.
std::vector<Eigen::VectorXd> linearly_interpolate(const int t0,
        const Eigen::VectorXd& x_t0, const int T, const Eigen::VectorXd& x_T);


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <boost/format.hpp>
#include "perceptron.h"
#include "utils.h"
#include <omp.h>
#include <ctime>

using namespace std;
using namespace Eigen;

Perceptron::Perceptron(const int& n_iterations, const double& learning_rate, const double& tolerance, const int& seed, const int& early_stopping_round) : n_iterations(n_iterations), learning_rate(learning_rate), tolerance(tolerance), seed(seed), early_stopping_round(early_stopping_round) {}

Perceptron::~Perceptron(){}

void Perceptron::fit(const MatrixXd& X, const VectorXd& y, VectorXd (*activation)(Eigen::VectorXd x), double (*loss)(VectorXd y, VectorXd y_pred), double (*metric)(VectorXd y, VectorXd pred)){
	int n_features = X.cols();
	srand(seed);
	// initialize weights between [-1/sqrt(n_features+1), 1/sqrt(n_features+1)]
	double limit = 1/sqrt(n_features+1);
	W = limit * VectorXd::Random(X.cols()+1);
	MatrixXd X_new(X.rows(), X.cols()+1);
	X_new<<X, MatrixXd::Ones(X.rows(), 1);

	double best_acc = 0.0;
	int become_worse_round = 0;

	for(int iter = 0; iter < n_iterations; iter++){
		// calculate outputs
		VectorXd outputs = X_new*W;
		//cout << "??" << endl;
		VectorXd y_pred = predict_prob(X);

		//cout << "??" << endl;
		VectorXd E = y - y_pred;

		// calcalate the loss gradient w.r.t the input of the activation function
		VectorXd loss_gradient = (-E.array() * Utils::sigmoid(outputs).array()*(VectorXd::Ones(X.rows()) - Utils::sigmoid(outputs)).array()).matrix();
		W = W - learning_rate*X_new.transpose()*loss_gradient;

		y_pred = predict_prob(X);

		double loss = Utils::squareLoss(y, y_pred);
		double acc = metric(y, y_pred);
		cout << boost::format("Iteration: %d, squareloss:%.5f, accuracy:%.5f") %iter %loss %acc << endl;
		if(loss <= tolerance) break;

		if(acc < best_acc){
			become_worse_round +=1;
		}else{
			become_worse_round = 0;
			best_acc = acc;
		}
		if(become_worse_round >= early_stopping_round){
			cout << "Early stopping. the best accuracy: " << best_acc << endl;
			break;
		}
	}
}

VectorXd Perceptron::predict_prob(const MatrixXd& X){
	MatrixXd X_new(X.rows(), X.cols()+1);
	X_new << X, MatrixXd::Ones(X.rows(), 1);
	VectorXd y_pred_prob = Utils::sigmoid(X_new*W);
	return y_pred_prob;
}

VectorXi Perceptron::predict(const MatrixXd& X){
	VectorXd ret_ = predict_prob(X);
	int n = ret_.size();
	VectorXi ret(n);
	#pragma omp parallel for
	for(int i = 0; i < n; i++){
		ret(i) = ret_(i)>0.5?1:0;
	}
	return ret;
}


//####### Test module for vec functions ####################################

//Define Module name
 #define BOOST_TEST_MODULE "vector functions"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

//Units choice. Not relevant here, but avoids compile-time warning
#define PXRMP_USE_SI_UNITS

#include "vec_functions.hpp"

using namespace picsar::multi_physics;

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-10;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-4;

//Templated tolerance
template <typename T>
T tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

// ------------- Tests --------------

//Test norm2 generic
template<typename T>
void vec_functions_norm2()
{
    vec3<T> vv{static_cast<T>(1.0),static_cast<T>(-2.0),static_cast<T>(3.0)};
    T exp = static_cast<T>(1.0 + 4.0 + 9.0);
    BOOST_CHECK_SMALL((norm2(vv)-exp)/exp, tolerance<T>());
}

//Test norm2 in double precision
BOOST_AUTO_TEST_CASE( vec_functions_norm2_double_1 )
{
    vec_functions_norm2<double>();
}

//Test norm2 in single precision
BOOST_AUTO_TEST_CASE( vec_functions_norm2_single_1 )
{
    vec_functions_norm2<float>();
}

//Test norm generic
template<typename T>
void vec_functions_norm()
{
    vec3<T> vv{static_cast<T>(1.0),static_cast<T>(-2.0),static_cast<T>(3.0)};
    T exp =  static_cast<T>(sqrt(1.0 + 4.0 + 9.0));
    BOOST_CHECK_SMALL((norm(vv)-exp)/exp,  tolerance<T>());
}

//Test norm in double precision
BOOST_AUTO_TEST_CASE( vec_functions_norm_double_1 )
{
    vec_functions_norm<double>();
}

//Test norm in single precision
BOOST_AUTO_TEST_CASE( vec_functions_norm_single_1 )
{
    vec_functions_norm<float>();
}

//Test dot generic
template<typename T>
void vec_functions_dot()
{
    vec3<T> vv1{static_cast<T>(1.0),static_cast<T>(-2.0),static_cast<T>(3.0)};
    vec3<T> vv2{static_cast<T>(-1.0),static_cast<T>(0.0),static_cast<T>(5.0)};
    T exp = static_cast<T>(14.0);
    BOOST_CHECK_SMALL((dot(vv1, vv2)-exp)/exp, tolerance<T>());
}

//Test dot in double precision
BOOST_AUTO_TEST_CASE( vec_functions_dot_double_1 )
{
    vec_functions_dot<double>();
}

//Test dot in single precision
BOOST_AUTO_TEST_CASE( vec_functions_dot_single_1 )
{
    vec_functions_dot<float>();
}

//Test cross generic
template<typename T>
void vec_functions_cross()
{
    vec3<T> vv1{static_cast<T>(1./3.),static_cast<T>(-1./4.),static_cast<T>(1./5.)};
    vec3<T> vv2{static_cast<T>(-2./3.),static_cast<T>(-3./4.),static_cast<T>(4./5.)};
    vec3<T> exp{static_cast<T>(-1./20.),static_cast<T>(-2./5.),static_cast<T>(-5./12.)};

    vec3<T> res = cross(vv1,vv2);
    BOOST_CHECK_SMALL((res[0]-exp[0])/exp[0], tolerance<T>());
    BOOST_CHECK_SMALL((res[1]-exp[1])/exp[1], tolerance<T>());
    BOOST_CHECK_SMALL((res[2]-exp[2])/exp[2], tolerance<T>());
}

//Test cross in double precision
BOOST_AUTO_TEST_CASE( vec_functions_cross_double_1 )
{
    vec_functions_cross<double>();
}

//Test cross in single precision
BOOST_AUTO_TEST_CASE( vec_functions_cross_single_1 )
{
    vec_functions_cross<float>();
}

//Test vector times scalar generic
template<typename T>
void vec_functions_vsprod()
{
    vec3<T> vv{static_cast<T>(1.0),static_cast<T>(2.0),static_cast<T>(-3.0)};
    T s = static_cast<T>(-2.0);
    vec3<T> exp{static_cast<T>(-2.0),static_cast<T>(-4.0),static_cast<T>(6.0)};

    vec3<T> r1 = s * vv;
    vec3<T> r2 = vv * s;

    BOOST_CHECK_SMALL((r1[0]-exp[0])/exp[0], tolerance<T>());
    BOOST_CHECK_SMALL((r1[1]-exp[1])/exp[1], tolerance<T>());
    BOOST_CHECK_SMALL((r1[2]-exp[2])/exp[2], tolerance<T>());
    BOOST_CHECK_SMALL((r2[0]-exp[0])/exp[0], tolerance<T>());
    BOOST_CHECK_SMALL((r2[1]-exp[1])/exp[1], tolerance<T>());
    BOOST_CHECK_SMALL((r2[2]-exp[2])/exp[2], tolerance<T>());
}

//Test vector times scalar in double precision
BOOST_AUTO_TEST_CASE( vec_functions_vsprod_double_1 )
{
    vec_functions_vsprod<double>();
}

//Test vector times scalar in single precision
BOOST_AUTO_TEST_CASE( vec_functions_vsprod_float_1 )
{
    vec_functions_vsprod<float>();
}

//Test vector divided by scalar generic
template<typename T>
void vec_functions_vsdiv()
{
    vec3<T> vv{static_cast<T>(2.0),static_cast<T>(4.0),static_cast<T>(-6.0)};
    T s = static_cast<T>(-2.0);
    vec3<T> exp{static_cast<T>(-1.0),static_cast<T>(-2.0),static_cast<T>(3.0)};

    vec3<T> r = vv / s;

    BOOST_CHECK_SMALL((r[0]-exp[0])/exp[0], tolerance<T>());
    BOOST_CHECK_SMALL((r[1]-exp[1])/exp[1], tolerance<T>());
    BOOST_CHECK_SMALL((r[2]-exp[2])/exp[2], tolerance<T>());
}

//Test vector divided by scalar in double precision
BOOST_AUTO_TEST_CASE( vec_functions_vsdiv_double_1 )
{
    vec_functions_vsdiv<double>();
}

//Test vector divided by scalar in single precision
BOOST_AUTO_TEST_CASE( vec_functions_vsdiv_float_1 )
{
    vec_functions_vsdiv<float>();
}

//Test vector add generic
template<typename T>
void vec_functions_vadd()
{
    vec3<T> vv1{static_cast<T>(1.0),static_cast<T>(2.0),static_cast<T>(3.0)};
    vec3<T> vv2{static_cast<T>(1.0),static_cast<T>(-1.0),static_cast<T>(-2.0)};

    vec3<T> exp{static_cast<T>(2.0),static_cast<T>(1.0),static_cast<T>(1.0)};

    vec3<T> r = vv1 + vv2;

    BOOST_CHECK_SMALL((exp[0]-r[0])/exp[0], tolerance<T>());
    BOOST_CHECK_SMALL((exp[1]-r[1])/exp[1], tolerance<T>());
    BOOST_CHECK_SMALL((exp[2]-r[2])/exp[2], tolerance<T>());
}

//Test vector add in double precision
BOOST_AUTO_TEST_CASE( vec_functions_vadd_double_1 )
{
    vec_functions_vadd<double>();
}

//Test vector add in single precision
BOOST_AUTO_TEST_CASE( vec_functions_vadd_single_1 )
{
    vec_functions_vadd<float>();
}

//Test vector diff generic
template<typename T>
void vec_functions_vdiff()
{
    vec3<T> vv1{static_cast<T>(1.0),static_cast<T>(2.0),static_cast<T>(3.0)};
    vec3<T> vv2{static_cast<T>(2.0),static_cast<T>(-1.0),static_cast<T>(-2.0)};

    vec3<T> exp{static_cast<T>(-1.0),static_cast<T>(3.0),static_cast<T>(5.0)};

    vec3<T> r = vv1 - vv2;

    BOOST_CHECK_SMALL((r[0]-exp[0])/exp[0], tolerance<T>());
    BOOST_CHECK_SMALL((r[1]-exp[1])/exp[1], tolerance<T>());
    BOOST_CHECK_SMALL((r[2]-exp[2])/exp[2], tolerance<T>());
}

//Test vector diff in double precision
BOOST_AUTO_TEST_CASE( vec_functions_vdiff_double_1 )
{
    vec_functions_vdiff<double>();
}

//Test vector diff in single precision
BOOST_AUTO_TEST_CASE( vec_functions_vdiff_single_1 )
{
    vec_functions_vdiff<float>();
}


#ifndef MATH_TYPES_HPP_INCLUDED
#define MATH_TYPES_HPP_INCLUDED

#include <Eigen/Core>

namespace gazeestimation {

	/// These types can be swapped out for anything that supports operators +,-,/,* with scalars
	/// and supplies a corresponding header file implementing the operations from Utils.cpp
	/// in addition they must define an operator <<= for matrics compliant with boost ubas's implementation
	typedef Eigen::Vector2d Vec2;
	typedef Eigen::Vector2i Vec2i;
	typedef Eigen::Vector3d Vec3;
	typedef Eigen::Matrix3d Mat3x3;

	inline Vec2 make_vec2(double a, double b)
	{
		return Vec2(a, b);
	}

	inline Vec3 make_vec3(double a, double b, double c)
	{
		return Vec3(a, b, c);
	}

	inline Mat3x3 mat_prod(const Mat3x3& a, const Mat3x3& b)
	{
		return a * b;
	}

	inline double dot(const Vec3& a, const Vec3& b)
	{
		return a.dot(b);
	}

	inline Vec3 mat3vec3_prod(const Mat3x3& a, const Vec3& b)
	{
		return a * b;
	}

	inline Vec3 normalized(const Vec3& a)
	{
		return a.normalized();
	}

	inline double length(const Vec3& a)
	{
		return a.norm();
	}

	inline double length(const Vec2& a)
	{
		return a.norm();
	}

	inline double squared_length(const Vec3& a)
	{
		return a.squaredNorm();
	}

	inline double squared_length_vec2(const Vec2& a)
	{
		return a.squaredNorm();
	}

	inline std::string vec3_to_string(const Vec3& a)
	{
		std::stringstream s;
		s << "(" << a[0] << ", " << a[1] << ", " << a[2] << ")";
		return s.str();
	}

	inline std::string vec2_to_string(const Vec3& a)
	{
		std::stringstream s;
		s << "(" << a[0] << ", " << a[1] << ")";
		return s.str();
	}

	inline Mat3x3 identity_matrix3x3()
	{
		return Eigen::Matrix3d::Identity();
	}

	inline Mat3x3 calculate_extrinsic_rotation_matrix(double alpha, double beta, double gamma)
	{
		Mat3x3 Rx, Ry, Rz;
		Rx << 1, 0, 0,
			0, std::cos(alpha), -std::sin(alpha),
			0, std::sin(alpha), std::cos(alpha);
		Ry << std::cos(beta), 0, std::sin(beta),
			0, 1, 0,
			-std::sin(beta), 0, std::cos(beta);
		Rz << std::cos(gamma), -std::sin(gamma), 0,
			std::sin(gamma), std::cos(gamma), 0,
			0, 0, 1;
		return mat_prod(Rz, mat_prod(Ry, Rx));
	}

	inline Vec3 cross_product(const Vec3& a, const Vec3& b)
	{
		return a.cross(b);
	}

	/// Returns the midpoint of the shortest segment between the two lines o1+a * d1 and o2 + b * d2;
	Vec3 shortest_line_segment(const Vec3& o1, const Vec3& d1, const Vec3& o2, const Vec3& d2);
}

#endif


#ifndef __BS_PRICES_HPP
#define __BS_PRICES_HPP

#include <iostream>
#include <cmath>
#include <armadillo>

double d_j(const int j, const double S, const double K, const double r, const double sigma, const double T) {
  return (log(S/K) + (r + (pow(-1,j-1))*0.5*sigma*sigma)*T)/(sigma*(pow(T,0.5)));
}

double call_price(const double S, const double K, const double r, const double sigma, const double T) {
  return S * arma::normcdf(d_j(1, S, K, r, sigma, T))-K*exp(-r*T) * arma::normcdf(d_j(2, S, K, r, sigma, T));
}

double call_vega(const double S, const double K, const double r, const double sigma, const double T){
	return S*sqrt(T)*arma::normpdf(d_j(1, S, K, r, sigma, T));
}

#endif


#include <Eigen/Core>

Eigen::Vector3d reflect(const Eigen::Vector3d & in, const Eigen::Vector3d & n)
{
  ////////////////////////////////////////////////////////////////////////////
  // Replace with your code here:
  return -in + (2 * n.dot(in) * n);
  ////////////////////////////////////////////////////////////////////////////
}