//------------------------------------------------------------------------------
// \file Recursion_tests.cpp
//------------------------------------------------------------------------------
#include "Algorithms/Recursion.h"

#include "Tools/CaptureCout.h"

#include <boost/test/unit_test.hpp>
#include <string>

using Algorithms::Recursion::Fibonacci::fib_recursive;
using Algorithms::Recursion::Fibonacci::fib_with_table;
using Algorithms::Recursion::TowersOfHanoi::towers;
using Algorithms::Recursion::HackerRank::CrosswordPuzzle::split_string;
using
  Algorithms::Recursion::HackerRank::DavisStaircases::
    recursive_step_permutations;
using
  Algorithms::Recursion::HackerRank::DavisStaircases::cached_step_permutations;
using Algorithms::Recursion::increasing_decreasing_values;
using Tools::CaptureCoutFixture;
using std::string;

BOOST_AUTO_TEST_SUITE(Algorithms)
BOOST_AUTO_TEST_SUITE(Recursion_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_FIXTURE_TEST_CASE(IncreasingDecreasingValueDemonstratesRecursiveStacks,
  CaptureCoutFixture)
{
  const string expected {
    "200 400 800 unwinding 800unwinding 400unwinding 200"
  };

  increasing_decreasing_values(200);

  BOOST_TEST(local_oss_.str() == expected);
}

BOOST_AUTO_TEST_SUITE(Fibonacci_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(FibonacciFunctionsComputeFibonacciNumbers)
{
  BOOST_TEST(fib_recursive(1) == 1);
  BOOST_TEST(fib_recursive(2) == 1);
  BOOST_TEST(fib_recursive(13) == 233);
  BOOST_TEST(fib_recursive(14) == 377);
  BOOST_TEST(fib_with_table(1) == 2);
  BOOST_TEST(fib_with_table(2) == 3);
  BOOST_TEST(fib_with_table(13) == 377);
  BOOST_TEST(fib_with_table(14) == 610);
}

BOOST_AUTO_TEST_SUITE_END() // Fibonacci_tests

BOOST_AUTO_TEST_SUITE(TowersOfHanoi_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TowersReturnsStringOfMoves)
{
  const string result {towers(2, "P1", "P2", "P3")};

  BOOST_TEST(
    result == "move from P1 to P3move from P1 to P2move from P3 to P2");
}

BOOST_AUTO_TEST_SUITE_END() // TowersOfHanoi_tests

BOOST_AUTO_TEST_SUITE(HackerRank_tests)

BOOST_AUTO_TEST_SUITE(DavisStaircases_tests)

// cf. https://www.hackerrank.com/challenges/ctci-recursive-staircase/problem?h_l=interview&playlist_slugs%5B%5D=interview-preparation-kit&playlist_slugs%5B%5D=recursion-backtracking

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RecursiveStepPermutationCalculatesForLowValueIntegers)
{
  // Test case 0
  BOOST_TEST(recursive_step_permutations(1) == 1);
  BOOST_TEST(recursive_step_permutations(3) == 4);
  BOOST_TEST(recursive_step_permutations(7) == 44);

  // Test case 9, HackerRank
  BOOST_TEST(recursive_step_permutations(5) == 13);
  BOOST_TEST(recursive_step_permutations(8) == 81);
  // Test case 10, HackerRank
  BOOST_TEST(recursive_step_permutations(15) == 5768);
  BOOST_TEST(recursive_step_permutations(20) == 121415);
  BOOST_TEST(recursive_step_permutations(27) == 8646064);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(CachingStepPermutationCalculatesManyIntegers)
{
  // Test case 0
  BOOST_TEST(cached_step_permutations(1) == 1);
  BOOST_TEST(cached_step_permutations(3) == 4);
  BOOST_TEST(cached_step_permutations(7) == 44);

  // Test case 9, HackerRank
  BOOST_TEST(cached_step_permutations(5) == 13);
  BOOST_TEST(cached_step_permutations(8) == 81);
  // Test case 10, HackerRank
  BOOST_TEST(cached_step_permutations(15) == 5768);
  BOOST_TEST(cached_step_permutations(20) == 121415);
  BOOST_TEST(cached_step_permutations(27) == 8646064);
}

BOOST_AUTO_TEST_SUITE_END() // DavisStaircases_tests

// cf. https://www.hackerrank.com/challenges/crossword-puzzle/problem?h_l=interview&playlist_slugs%5B%5D=interview-preparation-kit&playlist_slugs%5B%5D=recursion-backtracking

BOOST_AUTO_TEST_SUITE(DavisStaircases_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SplitStringSplitsSubstringsByDelimiter)
{
  string list_of_words {"POLAND;LHASA;SPAIN;INDIA"};
  const string delimiter {";"};
  const auto split_strings = split_string(list_of_words, delimiter);
  BOOST_TEST(split_strings[0] == "POLAND");
  BOOST_TEST(split_strings[1] == "LHASA");
  BOOST_TEST(split_strings[2] == "SPAIN");
  BOOST_TEST(split_strings[3] == "INDIA");
}

BOOST_AUTO_TEST_SUITE_END() // DavisStaircases_tests

/*

Test case 0

    +-++++++++

    +-++++++++

    +-++++++++

    +-----++++

    +-+++-++++

    +-+++-++++

    +++++-++++

    ++------++

    +++++-++++

    +++++-++++

    LONDON;DELHI;ICELAND;ANKARA

Expected Output
Download

    +L++++++++

    +O++++++++

    +N++++++++

    +DELHI++++

    +O+++C++++

    +N+++E++++

    +++++L++++

    ++ANKARA++

    +++++N++++

    +++++D++++

Test case 1

    +-++++++++

    +-++++++++

    +-------++

    +-++++++++

    +-++++++++

    +------+++

    +-+++-++++

    +++++-++++

    +++++-++++

    ++++++++++

    AGRA;NORWAY;ENGLAND;GWALIOR

Expected Output
Download

    +E++++++++

    +N++++++++

    +GWALIOR++

    +L++++++++

    +A++++++++

    +NORWAY+++

    +D+++G++++

    +++++R++++

    +++++A++++

    ++++++++++

Test case 7

    XXXXXX-XXX

    XX------XX

    XXXXXX-XXX

    XXXXXX-XXX

    XXX------X

    XXXXXX-X-X

    XXXXXX-X-X

    XXXXXXXX-X

    XXXXXXXX-X

    XXXXXXXX-X

    ICELAND;MEXICO;PANAMA;ALMATY

Expected Output
Download

    XXXXXXIXXX

    XXMEXICOXX

    XXXXXXEXXX

    XXXXXXLXXX

    XXXPANAMAX

    XXXXXXNXLX

    XXXXXXDXMX

    XXXXXXXXAX

    XXXXXXXXTX

    XXXXXXXXYX

*/

BOOST_AUTO_TEST_SUITE_END() // HackerRank_tests

BOOST_AUTO_TEST_SUITE_END() // Recursion_tests
BOOST_AUTO_TEST_SUITE_END() // Algorithms


#include <Eigen/Dense>
#include <Eigen/LU>

#include <iostream>
#include "timer.h"

//! \brief Circular shift (downwards) of b
//! \param[in,out] b Vector is nx1, shifted downwards
template <class Vector>
void shift(Vector & b) {
    typedef typename Vector::Scalar Scalar;
    int n = b.size();

    Scalar temp = b(n-1);
    for(int k = n-2; k >= 0; --k) {
        b(k+1) = b(k);
    }
    b(0) = temp;
}

//! \brief Compute X = inv(A)*[b_1,...,b_n], b_i = i-th cyclic shift of b
//! \param[in] A Matrix is nx
//! \param[in] b Vector is nx1
//! \param[out] X solution nxn matrix X = inv(A)*[b_1,...,b_n]
template <class Matrix, class Vector>
void solvpermb(const Matrix & A, Vector & b, Matrix & X) {
    // Size of b, which is size of A
    int n = b.size();
    if( n != A.cols() or n != A.rows() ) {
        // Size check, error if do not match
        std::cerr << "Error: size mismatch!" << std::endl;
        return;
    }
    X.resize(n,n);

    for(int l = 0; l < n; ++l) {
        X.col(l) = A.fullPivLu().solve(b);

        shift(b);
    }
}

//! \brief Compute X = inv(A)*[b_1,...,b_n], b_i = i-th cyclic shift of b in O(n^3)
//! \param[in] A Matrix is nx
//! \param[in] b Vector is nx1
//! \param[out] X solution nxn matrix X = inv(A)*[b_1,...,b_n]
template <class Matrix, class Vector>
void solvpermb_on3(const Matrix & A, Vector & b, Matrix & X) {
    // Size of b, which is size of A
    int n = b.size();
    if( n != A.cols() or n != A.rows() ) {
        // Size check, error if do not match
        std::cerr << "Error: size mismatch!" << std::endl;
        return;
    }
    X.resize(n,n);

    // Notice here we reuse the LU factorization
    Eigen::FullPivLU<Matrix> LU = A.fullPivLu();

    for(int l = 0; l < n; ++l) {
        X.col(l) = LU.solve(b);

        shift(b);
    }
}

int main() {
    unsigned int n = 9;
    // Compute with both LU and reuse LU
    std::cout << "*** Check correctness of permutation solver" << std::endl;
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(n,n);
    Eigen::VectorXd b = Eigen::VectorXd::Random(n);
    Eigen::MatrixXd X;
    std::cout << "b = " << std::endl << b << std::endl;
    solvpermb(A,b,X);
    std::cout << "Direct porting from MALTAB: " << std::endl << X << std::endl;
    std::cout << "A*X = " << std::endl << A*X << std::endl;
    solvpermb_on3(A,b,X);
    std::cout << "Reusing LU: " << std::endl << X << std::endl;
    std::cout << "A*X = " << std::endl << A*X << std::endl;

    // Compute runtime of different implementations of kron
    std::cout << "*** Runtime comparisons of LU vs reuse LU" << std::endl;
    unsigned int repeats = 3;
    timer<> tm_lu, tm_reuse_lu;

    for(unsigned int p = 2; p <= 7; p++) {
        tm_lu.reset();
        tm_reuse_lu.reset();
        for(unsigned int r = 0; r < repeats; ++r) {
            unsigned int M = pow(2,p);
            A = Eigen::MatrixXd::Random(M,M);
            b = Eigen::VectorXd::Random(M);

            tm_lu.start();
            solvpermb(A,b,X);
            tm_lu.stop();

            tm_reuse_lu.start();
            solvpermb_on3(A,b,X);
            tm_reuse_lu.stop();
        }

        std::cout << "LU took:       " << tm_lu.avg().count() / 1000000. << " ms" << std::endl;
        std::cout << "Reuse LU took: " << tm_reuse_lu.avg().count() / 1000000. << " ms" << std::endl;
    }
}

Nummerical Methods for CSE/solutions/solution_2/solvepermb.cpp


#include "MatricesAndVectors.h"
#include "QuatRotEuler.h"
#include <math.h>
#include <Eigen/Dense>
using Eigen::Matrix;

//Rotation matrix around x
Matrix<float, 3, 3> Rotx(double theta){
	Matrix<float, 3, 3> R;

	R << 1,          0,           0,
	     0, cos(theta), -sin(theta),
	     0, sin(theta),  cos(theta);

	return R;
}

//Rotation matrix around y
Matrix<float, 3, 3> Roty(double theta){
	Matrix<float, 3, 3> R;

	R <<  cos(theta), 0, sin(theta),
	               0, 1,          0,
	     -sin(theta), 0, cos(theta);

	return R;
}

//Rotation matrix around z
Matrix<float, 3, 3> Rotz(double theta){
	Matrix<float, 3, 3> R;

	R <<  cos(theta), -sin(theta), 0,
	      sin(theta),  cos(theta), 0,
	               0,           0, 1;

	return R;
}

//Convert Euler Angles (Roll-pitch-yaw) to Rotation Matrix
Matrix<float, 3, 3> RPY2Rot(double roll, double pitch, double yaw){
	Matrix<float, 3, 3> Rx = Rotx(roll);
	Matrix<float, 3, 3> Ry = Roty(pitch);
	Matrix<float, 3, 3> Rz = Rotz(yaw);

	return Rz*Ry*Rx; //Rout = Rz*Ry*Rx

}

//Convert quaternion to rotation matrix
Matrix<float, 3, 3> Quat2rot(Matrix<float, 4, 1> q){

	Matrix<float, 3, 3> R;
	double qw = q(0);
	double qx = q(1);
	double qy = q(2);
	double qz = q(3);

	R <<  1 - 2 * pow(qy, 2) - 2 * pow(qz, 2),               2 * qx*qy - 2 * qz*qw,               2 * qx*qz + 2 * qy*qw,
	                    2 * qx*qy + 2 * qz*qw, 1 - 2 * pow(qx, 2) - 2 * pow(qz, 2),               2 * qy*qz - 2 * qx*qw,
	                    2 * qx*qz - 2 * qy*qw,               2 * qy*qz + 2 * qx*qw, 1 - 2 * pow(qx, 2) - 2 * pow(qy, 2);

	return R;
}

//Convert rotation matrix to quaternion
//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
Matrix<float, 4, 1> Rot2quat(Matrix<float, 3, 3> M){
	double trace = M.trace();
	Matrix<float, 4, 1> q;
	if (trace > 0) {// M_EPSILON = 0
		double s = 0.5 / sqrt(trace + 1.0);
		q << 0.25 / s,
			(M(2,1) - M(1,2)) * s,
			(M(0,2) - M(2,0)) * s,
			(M(1,0) - M(0,1)) * s;
	}
	else {
		if (M(0,0) > M(1,1) && M(0,0) > M(2,2)) {
			double s = 2.0 * sqrt(1.0 + M(0,0) - M(1,1) - M(2,2));
			q << (M(2,1) - M(1,2)) / s,
				 0.25 * s,
				 (M(0,1) + M(1,0)) / s,
				 (M(0,2) + M(2,0)) / s;
		}
		else if (M(1,1) > M(2,2)) {
			double s = 2.0 * sqrt(1.0 + M(1,1) - M(0,0) - M(2,2));
			q << (M(0,2) - M(2,0)) / s;
			     (M(0,1) + M(1,0)) / s;
			     0.25 * s;
			     (M(1,2) + M(2,1)) / s;
		}
		else {
			double s = 2.0 * sqrt(1.0 + M(2,2) - M(0,0) - M(1,1));
			q << (M(1,0) - M(0,1)) / s;
			     (M(0,2) + M(2,0)) / s;
			     (M(1,2) + M(2,1)) / s;
			     0.25 * s;
		}
	}
        return q;
}

//Convert quaternion to Roll-Pitch-Yaw
Matrix<float, 3, 1> Quat2RPY(Matrix<float, 4, 1> q){

	Matrix<float, 3, 1> RPY;

	RPY << atan2(2*(q(0)*q(1) + q(2)*q(3)) , 1 - 2*(q(1)*q(1) + q(2)*q(2)) ),	//Roll
		   asin(2*(q(0)*q(2) - q(3)*q(1))),				//Pitch
		   atan2(2*(q(0)*q(3) + q(1)*q(2)),1 - 2*(q(2)*q(2) + q(3)*q(3)) );	//Yaw

	return RPY;
}

// Quaternion Multiplication
Matrix<float, 4, 1> QuaternionProduct(Matrix<float, 4, 1> q1, Matrix<float, 4, 1> q2){
	Matrix<float, 4, 4> H;
	H << q1(0), -q1(1), -q1(2), -q1(3),
	     q1(1),  q1(0), -q1(3),  q1(2),
	     q1(2),  q1(3),  q1(0), -q1(1),
	     q1(3), -q1(2),  q1(1),  q1(0);

	return H*q2;
}

//Triad Algorithm for Finding attitude from two vectors
// https://en.wikipedia.org/wiki/Triad_method
Matrix<float, 3, 3> Triad(Matrix<float, 3, 1> V1_body, Matrix<float, 3, 1> V2_body, Matrix<float, 3, 1> V1_inertial, Matrix<float, 3, 1> V2_inertial){
	Matrix<float, 3, 3> R, r;
	Matrix<float, 3, 1> r1, r2, r3, R1, R2, R3, V1_bodyNorm, V2_bodyNorm, V1_inertialNorm, V2_inertialNorm;

	//Normalize vectors and make them orthogonal
	// V1_bodyNorm = ScaleVec3(V1_body, 1.0 / p_normVec3(V1_body, 2));
	// V2_bodyNorm = ScaleVec3(V2_body, 1.0 / p_normVec3(V2_body, 2));
	// V1_inertialNorm = ScaleVec3(V1_inertial, 1.0 / p_normVec3(V1_inertial, 2));
	// V2_inertialNorm = ScaleVec3(V2_inertial, 1.0 / p_normVec3(V2_inertial, 2));

	V1_bodyNorm = V1_body * (1.0 / V1_body.norm());
	V2_bodyNorm = V2_body * (1.0 / V2_body.norm());
	V1_inertialNorm = V1_inertial * (1.0 / V1_inertial.norm());
	V2_inertialNorm = V2_inertial * (1.0 / V2_inertial.norm());

	//Set orthogonal vectors from body frame
	r1 = V1_bodyNorm;
	r2 = V1_bodyNorm.cross(V2_bodyNorm) * (1.0/V1_bodyNorm.cross(V2_bodyNorm).norm()); //(V1 x V2)/norm(V1 x V2)
	r3 = r1.cross(r2);

	//Set orthogonal vectors from inertial frame
	R1 = V1_inertialNorm;
	R2 = V1_inertialNorm.cross(V2_inertialNorm) * (1.0/V1_inertialNorm.cross(V2_inertialNorm).norm()); //(V1 x V2)/norm(V1 x V2)
	R3= R1.cross(R2);

	//We need to find the rotation matrix that solves [R1:R2:R3]=Rot.[r1:r2:r3]
	// ==> Rot = [R1:R2:R3].[r1:r2:r3]'
	// Matrix<float, 3, 3> R = Concatenate3Vec3_2_Mat3x3(R1, R2, R3); //Creates matrix R = [R1:R2:R3]
	R << R1, R2, R3;
	//Matrix<float, 3, 3> r = Concatenate3Vec3_2_Mat3x3(r1, r2, r3); //Creates matrix r = [r1:r2:r3]
	r << r1, r2, r3;
	return R*r.transpose();
}

//Propagation quaternion for propagating quaternion with gyroscope data
//Equation 34 in https://users.aalto.fi/~ssarkka/pub/quat.pdf
Matrix<float, 4, 1> propagationQuat(Matrix<float, 3, 1> w, double dt){
	Matrix<float, 4, 1> dq; //delta quaternion
	double norm_w = w.norm();
	double angle = dt*norm_w / 2;
	double sin_angle = sin(angle);

	dq(0,0) = cos(angle);
	if (norm_w != 0){
		// dq(1,0) << w(0) * sin_angle / norm_w,
		// dq(2,0) = w(1) * sin_angle / norm_w;
		// dq(3,0) = w(2) * sin_angle / norm_w;
		dq.tail(3) = w * (sin_angle / norm_w);
	}
	else{
		dq.tail(3) << 0,
					  0,
					  0;
	}
	return dq;
}

//Propagate rotation matrix from initial point through angular velocity
//Equation 33 in https://users.aalto.fi/~ssarkka/pub/quat.pdf, but instead of
// using quaternion multiplication, we use rotation matrix multiplication
Matrix<float, 3, 3> propagateRotMat(Matrix<float, 3, 3> M0, Matrix<float, 3, 1> w, double dt){
	Matrix<float, 4, 1> dq = propagationQuat(w, dt);
	Matrix<float, 3, 3> dRot = Quat2rot(dq);

	return M0*dRot;
}


// normal_misc_examples.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 of using normal distribution.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[normal_basic1
/*`
First we need some includes to access the normal distribution
(and some std output of course).
*/

#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.

#include <iostream>
  using std::cout; using std::endl; using std::left; using std::showpoint; using std::noshowpoint;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;

int main()
{
  cout << "Example: Normal distribution, Miscellaneous Applications.";

  try
  {
    { // Traditional tables and values.
/*`Let's start by printing some traditional tables.
*/
      double step = 1.; // in z
      double range = 4; // min and max z = -range to +range.
      int precision = 17; // traditional tables are only computed to much lower precision.

      // Construct a standard normal distribution s
        normal s; // (default mean = zero, and standard deviation = unity)
        cout << "Standard normal distribution, mean = "<< s.mean()
          << ", standard deviation = " << s.standard_deviation() << endl;

/*` First the probability distribution function (pdf).
*/
      cout << "Probability distribution function values" << endl;
      cout << "  z " "      pdf " << endl;
      cout.precision(5);
      for (double z = -range; z < range + step; z += step)
      {
        cout << left << setprecision(3) << setw(6) << z << " "
          << setprecision(precision) << setw(12) << pdf(s, z) << endl;
      }
      cout.precision(6); // default
      /*`And the area under the normal curve from -[infin] up to z,
      the cumulative distribution function (cdf).
*/
      // For a standard normal distribution
      cout << "Standard normal mean = "<< s.mean()
        << ", standard deviation = " << s.standard_deviation() << endl;
      cout << "Integral (area under the curve) from - infinity up to z " << endl;
      cout << "  z " "      cdf " << endl;
      for (double z = -range; z < range + step; z += step)
      {
        cout << left << setprecision(3) << setw(6) << z << " "
          << setprecision(precision) << setw(12) << cdf(s, z) << endl;
      }
      cout.precision(6); // default

/*`And all this you can do with a nanoscopic amount of work compared to
the team of *human computers* toiling with Milton Abramovitz and Irene Stegen
at the US National Bureau of Standards (now [@http://www.nist.gov NIST]).
Starting in 1938, their "Handbook of Mathematical Functions with Formulas, Graphs and Mathematical Tables",
was eventually published in 1964, and has been reprinted numerous times since.
(A major replacement is planned at [@http://dlmf.nist.gov Digital Library of Mathematical Functions]).

Pretty-printing a traditional 2-dimensional table is left as an exercise for the student,
but why bother now that the Math Toolkit lets you write
*/
    double z = 2.;
    cout << "Area for z = " << z << " is " << cdf(s, z) << endl; // to get the area for z.
/*`
Correspondingly, we can obtain the traditional 'critical' values for significance levels.
For the 95% confidence level, the significance level usually called alpha,
is 0.05 = 1 - 0.95 (for a one-sided test), so we can write
*/
     cout << "95% of area has a z below " << quantile(s, 0.95) << endl;
   // 95% of area has a z below 1.64485
/*`and a two-sided test (a comparison between two levels, rather than a one-sided test)

*/
     cout << "95% of area has a z between " << quantile(s, 0.975)
       << " and " << -quantile(s, 0.975) << endl;
   // 95% of area has a z between 1.95996 and -1.95996
/*`

First, define a table of significance levels: these are the probabilities
that the true occurrence frequency lies outside the calculated interval.

It is convenient to have an alpha level for the probability that z lies outside just one standard deviation.
This will not be some nice neat number like 0.05, but we can easily calculate it,
*/
    double alpha1 = cdf(s, -1) * 2; // 0.3173105078629142
    cout << setprecision(17) << "Significance level for z == 1 is " << alpha1 << endl;
/*`
    and place in our array of favorite alpha values.
*/
    double alpha[] = {0.3173105078629142, // z for 1 standard deviation.
      0.20, 0.1, 0.05, 0.01, 0.001, 0.0001, 0.00001 };
/*`

Confidence value as % is (1 - alpha) * 100 (so alpha 0.05 == 95% confidence)
that the true occurrence frequency lies *inside* the calculated interval.

*/
    cout << "level of significance (alpha)" << setprecision(4) << endl;
    cout << "2-sided       1 -sided          z(alpha) " << endl;
    for (int i = 0; i < sizeof(alpha)/sizeof(alpha[0]); ++i)
    {
      cout << setw(15) << alpha[i] << setw(15) << alpha[i] /2 << setw(10) << quantile(complement(s,  alpha[i]/2)) << endl;
      // Use quantile(complement(s, alpha[i]/2)) to avoid potential loss of accuracy from quantile(s,  1 - alpha[i]/2)
    }
    cout << endl;

/*`Notice the distinction between one-sided (also called one-tailed)
where we are using a > *or* < test (and not both)
and considering the area of the tail (integral) from z up to +[infin],
and a two-sided test where we are using two > *and* < tests, and thus considering two tails,
from -[infin] up to z low and z high up to +[infin].

So the 2-sided values alpha[i] are calculated using alpha[i]/2.

If we consider a simple example of alpha = 0.05, then for a two-sided test,
the lower tail area from -[infin] up to -1.96 is 0.025 (alpha/2)
and the upper tail area from +z up to +1.96 is also  0.025 (alpha/2),
and the area between -1.96 up to 12.96 is alpha = 0.95.
and the sum of the two tails is 0.025 + 0.025 = 0.05,

*/
//] [/[normal_basic1]

//[normal_basic2

/*`Armed with the cumulative distribution function, we can easily calculate the
easy to remember proportion of values that lie within 1, 2 and 3 standard deviations from the mean.

*/
    cout.precision(3);
    cout << showpoint << "cdf(s, s.standard_deviation()) = "
      << cdf(s, s.standard_deviation()) << endl;  // from -infinity to 1 sd
    cout << "cdf(complement(s, s.standard_deviation())) = "
      << cdf(complement(s, s.standard_deviation())) << endl;
    cout << "Fraction 1 standard deviation within either side of mean is "
      << 1 -  cdf(complement(s, s.standard_deviation())) * 2 << endl;
    cout << "Fraction 2 standard deviations within either side of mean is "
      << 1 -  cdf(complement(s, 2 * s.standard_deviation())) * 2 << endl;
    cout << "Fraction 3 standard deviations within either side of mean is "
      << 1 -  cdf(complement(s, 3 * s.standard_deviation())) * 2 << endl;

/*`
To a useful precision, the 1, 2 & 3 percentages are 68, 95 and 99.7,
and these are worth memorising as useful 'rules of thumb', as, for example, in
[@http://en.wikipedia.org/wiki/Standard_deviation standard deviation]:

[pre
Fraction 1 standard deviation within either side of mean is 0.683
Fraction 2 standard deviations within either side of mean is 0.954
Fraction 3 standard deviations within either side of mean is 0.997
]

We could of course get some really accurate values for these
[@http://en.wikipedia.org/wiki/Confidence_interval confidence intervals]
by using cout.precision(15);

[pre
Fraction 1 standard deviation within either side of mean is 0.682689492137086
Fraction 2 standard deviations within either side of mean is 0.954499736103642
Fraction 3 standard deviations within either side of mean is 0.997300203936740
]

But before you get too excited about this impressive precision,
don't forget that the *confidence intervals of the standard deviation* are surprisingly wide,
especially if you have estimated the standard deviation from only a few measurements.
*/
//] [/[normal_basic2]

//[normal_bulbs_example1
/*`
Examples from K. Krishnamoorthy, Handbook of Statistical Distributions with Applications,
ISBN 1 58488 635 8, page 125... implemented using the Math Toolkit library.

A few very simple examples are shown here:
*/
// K. Krishnamoorthy, Handbook of Statistical Distributions with Applications,
 // ISBN 1 58488 635 8, page 125, example 10.3.5
/*`Mean lifespan of 100 W bulbs is 1100 h with standard deviation of 100 h.
Assuming, perhaps with little evidence and much faith, that the distribution is normal,
we construct a normal distribution called /bulbs/ with these values:
*/
    double mean_life = 1100.;
    double life_standard_deviation = 100.;
    normal bulbs(mean_life, life_standard_deviation);
    double expected_life = 1000.;

/*`The we can use the Cumulative distribution function to predict fractions
(or percentages, if * 100) that will last various lifetimes.
*/
    cout << "Fraction of bulbs that will last at best (<=) " // P(X <= 1000)
      << expected_life << " is "<< cdf(bulbs, expected_life) << endl;
    cout << "Fraction of bulbs that will last at least (>) " // P(X > 1000)
      << expected_life << " is "<< cdf(complement(bulbs, expected_life)) << endl;
    double min_life = 900;
    double max_life = 1200;
    cout << "Fraction of bulbs that will last between "
      << min_life << " and " << max_life << " is "
      << cdf(bulbs, max_life)  // P(X <= 1200)
       - cdf(bulbs, min_life) << endl; // P(X <= 900)
/*`
[note Real-life failures are often very ab-normal,
with a significant number that 'dead-on-arrival' or suffer failure very early in their life:
the lifetime of the survivors of 'early mortality' may be well described by the normal distribution.]
*/
//] [/normal_bulbs_example1 Quickbook end]
  }
  {
    // K. Krishnamoorthy, Handbook of Statistical Distributions with Applications,
    // ISBN 1 58488 635 8, page 125, Example 10.3.6

//[normal_bulbs_example3
/*`Weekly demand for 5 lb sacks of onions at a store is normally distributed with mean 140 sacks and standard deviation 10.
*/
  double mean = 140.; // sacks per week.
  double standard_deviation = 10;
  normal sacks(mean, standard_deviation);

  double stock = 160.; // per week.
  cout << "Percentage of weeks overstocked "
    << cdf(sacks, stock) * 100. << endl; // P(X <=160)
  // Percentage of weeks overstocked 97.7

/*`So there will be lots of mouldy onions!
So we should be able to say what stock level will meet demand 95% of the weeks.
*/
  double stock_95 = quantile(sacks, 0.95);
  cout << "Store should stock " << int(stock_95) << " sacks to meet 95% of demands." << endl;
/*`And it is easy to estimate how to meet 80% of demand, and waste even less.
*/
  double stock_80 = quantile(sacks, 0.80);
  cout << "Store should stock " << int(stock_80) << " sacks to meet 8 out of 10 demands." << endl;
//] [/normal_bulbs_example3 Quickbook end]
  }
  { // K. Krishnamoorthy, Handbook of Statistical Distributions with Applications,
    // ISBN 1 58488 635 8, page 125, Example 10.3.7

//[normal_bulbs_example4

/*`A machine is set to pack 3 kg of ground beef per pack.
Over a long period of time it is found that the average packed was 3 kg
with a standard deviation of 0.1 kg.
Assuming the packing is normally distributed,
we can find the fraction (or %) of packages that weigh more than 3.1 kg.
*/

double mean = 3.; // kg
double standard_deviation = 0.1; // kg
normal packs(mean, standard_deviation);

double max_weight = 3.1; // kg
cout << "Percentage of packs > " << max_weight << " is "
<< cdf(complement(packs, max_weight)) << endl; // P(X > 3.1)

double under_weight = 2.9;
cout <<"fraction of packs <= " << under_weight << " with a mean of " << mean
  << " is " << cdf(complement(packs, under_weight)) << endl;
// fraction of packs <= 2.9 with a mean of 3 is 0.841345
// This is 0.84 - more than the target 0.95
// Want 95% to be over this weight, so what should we set the mean weight to be?
// KK StatCalc says:
double over_mean = 3.0664;
normal xpacks(over_mean, standard_deviation);
cout << "fraction of packs >= " << under_weight
<< " with a mean of " << xpacks.mean()
  << " is " << cdf(complement(xpacks, under_weight)) << endl;
// fraction of packs >= 2.9 with a mean of 3.06449 is 0.950005
double under_fraction = 0.05;  // so 95% are above the minimum weight mean - sd = 2.9
double low_limit = standard_deviation;
double offset = mean - low_limit - quantile(packs, under_fraction);
double nominal_mean = mean + offset;

normal nominal_packs(nominal_mean, standard_deviation);
cout << "Setting the packer to " << nominal_mean << " will mean that "
  << "fraction of packs >= " << under_weight
  << " is " << cdf(complement(nominal_packs, under_weight)) << endl;

/*`
Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95.

Setting the packer to 3.13263 will mean that fraction of packs >= 2.9 is 0.99,
but will more than double the mean loss from 0.0644 to 0.133.

Alternatively, we could invest in a better (more precise) packer with a lower standard deviation.

To estimate how much better (how much smaller standard deviation) it would have to be,
we need to get the 5% quantile to be located at the under_weight limit, 2.9
*/
double p = 0.05; // wanted p th quantile.
cout << "Quantile of " << p << " = " << quantile(packs, p)
  << ", mean = " << packs.mean() << ", sd = " << packs.standard_deviation() << endl; //
/*`
Quantile of 0.05 = 2.83551, mean = 3, sd = 0.1

With the current packer (mean = 3, sd = 0.1), the 5% quantile is at 2.8551 kg,
a little below our target of 2.9 kg.
So we know that the standard deviation is going to have to be smaller.

Let's start by guessing that it (now 0.1) needs to be halved, to a standard deviation of 0.05
*/
normal pack05(mean, 0.05);
cout << "Quantile of " << p << " = " << quantile(pack05, p)
  << ", mean = " << pack05.mean() << ", sd = " << pack05.standard_deviation() << endl;

cout <<"Fraction of packs >= " << under_weight << " with a mean of " << mean
  << " and standard deviation of " << pack05.standard_deviation()
  << " is " << cdf(complement(pack05, under_weight)) << endl;
//
/*`
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.05 is 0.9772

So 0.05 was quite a good guess, but we are a little over the 2.9 target,
so the standard deviation could be a tiny bit more. So we could do some
more guessing to get closer, say by increasing to 0.06
*/

normal pack06(mean, 0.06);
cout << "Quantile of " << p << " = " << quantile(pack06, p)
  << ", mean = " << pack06.mean() << ", sd = " << pack06.standard_deviation() << endl;

cout <<"Fraction of packs >= " << under_weight << " with a mean of " << mean
  << " and standard deviation of " << pack06.standard_deviation()
  << " is " << cdf(complement(pack06, under_weight)) << endl;
/*`
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.06 is 0.9522

Now we are getting really close, but to do the job properly,
we could use root finding method, for example the tools provided, and used elsewhere,
in the Math Toolkit, see
[link math_toolkit.toolkit.internals1.roots2  Root Finding Without Derivatives].

But in this normal distribution case, we could be even smarter and make a direct calculation.
*/

normal s; // For standard normal distribution,
double sd = 0.1;
double x = 2.9; // Our required limit.
// then probability p = N((x - mean) / sd)
// So if we want to find the standard deviation that would be required to meet this limit,
// so that the p th quantile is located at x,
// in this case the 0.95 (95%) quantile at 2.9 kg pack weight, when the mean is 3 kg.

double prob =  pdf(s, (x - mean) / sd);
double qp = quantile(s, 0.95);
cout << "prob = " << prob << ", quantile(p) " << qp << endl; // p = 0.241971, quantile(p) 1.64485
// Rearranging, we can directly calculate the required standard deviation:
double sd95 = abs((x - mean)) / qp;

cout << "If we want the "<< p << " th quantile to be located at "
  << x << ", would need a standard deviation of " << sd95 << endl;

normal pack95(mean, sd95);  // Distribution of the 'ideal better' packer.
cout <<"Fraction of packs >= " << under_weight << " with a mean of " << mean
  << " and standard deviation of " << pack95.standard_deviation()
  << " is " << cdf(complement(pack95, under_weight)) << endl;

// Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.0608 is 0.95

/*`Notice that these two deceptively simple questions
(do we over-fill or measure better) are actually very common.
The weight of beef might be replaced by a measurement of more or less anything.
But the calculations rely on the accuracy of the standard deviation - something
that is almost always less good than we might wish,
especially if based on a few measurements.
*/

//] [/normal_bulbs_example4 Quickbook end]
  }

  { // K. Krishnamoorthy, Handbook of Statistical Distributions with Applications,
    // ISBN 1 58488 635 8, page 125, example 10.3.8
//[normal_bulbs_example5
/*`A bolt is usable if between 3.9 and 4.1 long.
From a large batch of bolts, a sample of 50 show a
mean length of 3.95 with standard deviation 0.1.
Assuming a normal distribution, what proportion is usable?
The true sample mean is unknown,
but we can use the sample mean and standard deviation to find approximate solutions.
*/

    normal bolts(3.95, 0.1);
    double top = 4.1;
    double bottom = 3.9;

cout << "Fraction long enough [ P(X <= " << top << ") ] is " << cdf(bolts, top) << endl;
cout << "Fraction too short [ P(X <= " << bottom << ") ] is " << cdf(bolts, bottom) << endl;
cout << "Fraction OK  -between " << bottom << " and " << top
  << "[ P(X <= " << top  << ") - P(X<= " << bottom << " ) ] is "
  << cdf(bolts, top) - cdf(bolts, bottom) << endl;

cout << "Fraction too long [ P(X > " << top << ") ] is "
  << cdf(complement(bolts, top)) << endl;

cout << "95% of bolts are shorter than " << quantile(bolts, 0.95) << endl;

//] [/normal_bulbs_example5 Quickbook end]
  }
  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

/*

Output is:

Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\normal_misc_examples.exe"
Example: Normal distribution, Miscellaneous Applications.Standard normal distribution, mean = 0, standard deviation = 1
Probability distribution function values
  z       pdf
-4     0.00013383022576488537
-3     0.0044318484119380075
-2     0.053990966513188063
-1     0.24197072451914337
0      0.3989422804014327
1      0.24197072451914337
2      0.053990966513188063
3      0.0044318484119380075
4      0.00013383022576488537
Standard normal mean = 0, standard deviation = 1
Integral (area under the curve) from - infinity up to z
  z       cdf
-4     3.1671241833119979e-005
-3     0.0013498980316300959
-2     0.022750131948179219
-1     0.1586552539314571
0      0.5
1      0.84134474606854293
2      0.97724986805182079
3      0.9986501019683699
4      0.99996832875816688
Area for z = 2 is 0.97725
95% of area has a z below 1.64485
95% of area has a z between 1.95996 and -1.95996
Significance level for z == 1 is 0.3173105078629142
level of significance (alpha)
2-sided       1 -sided          z(alpha)
0.3173         0.1587         1
0.2            0.1            1.282
0.1            0.05           1.645
0.05           0.025          1.96
0.01           0.005          2.576
0.001          0.0005         3.291
0.0001         5e-005         3.891
1e-005         5e-006         4.417
cdf(s, s.standard_deviation()) = 0.841
cdf(complement(s, s.standard_deviation())) = 0.159
Fraction 1 standard deviation within either side of mean is 0.683
Fraction 2 standard deviations within either side of mean is 0.954
Fraction 3 standard deviations within either side of mean is 0.997
Fraction of bulbs that will last at best (<=) 1.00e+003 is 0.159
Fraction of bulbs that will last at least (>) 1.00e+003 is 0.841
Fraction of bulbs that will last between 900. and 1.20e+003 is 0.819
Percentage of weeks overstocked 97.7
Store should stock 156 sacks to meet 95% of demands.
Store should stock 148 sacks to meet 8 out of 10 demands.
Percentage of packs > 3.10 is 0.159
fraction of packs <= 2.90 with a mean of 3.00 is 0.841
fraction of packs >= 2.90 with a mean of 3.07 is 0.952
Setting the packer to 3.06 will mean that fraction of packs >= 2.90 is 0.950
Quantile of 0.0500 = 2.84, mean = 3.00, sd = 0.100
Quantile of 0.0500 = 2.92, mean = 3.00, sd = 0.0500
Fraction of packs >= 2.90 with a mean of 3.00 and standard deviation of 0.0500 is 0.977
Quantile of 0.0500 = 2.90, mean = 3.00, sd = 0.0600
Fraction of packs >= 2.90 with a mean of 3.00 and standard deviation of 0.0600 is 0.952
prob = 0.242, quantile(p) 1.64
If we want the 0.0500 th quantile to be located at 2.90, would need a standard deviation of 0.0608
Fraction of packs >= 2.90 with a mean of 3.00 and standard deviation of 0.0608 is 0.950
Fraction long enough [ P(X <= 4.10) ] is 0.933
Fraction too short [ P(X <= 3.90) ] is 0.309
Fraction OK  -between 3.90 and 4.10[ P(X <= 4.10) - P(X<= 3.90 ) ] is 0.625
Fraction too long [ P(X > 4.10) ] is 0.0668
95% of bolts are shorter than 4.11

*/


/**
 * @file gaussian.hpp
 * @author Vahid Bastani
 *
 * Multivariate Gaussian distribution
 */
#ifndef SSMPACK_DISTRIBUTION_GAUSSIAN_HPP
#define SSMPACK_DISTRIBUTION_GAUSSIAN_HPP

#include "ssmkit/random/generator.hpp"

#include <armadillo>

#include <cmath>

namespace ssmkit {
namespace distribution {

/** A D-dimensional multivariate Gaussian distribution.
 * \f[ \mathcal{N}(\mathbf{x}| \mu, \Sigma) =
 * \frac{1}{(2\pi)^{D/2}\sqrt{|\Sigma|}}
 * \exp(-\frac{1}{2}\mathbf{x}^T\Sigma^{-1}\mathbf{x})\f]
 */
class Gaussian {

 public:
  //! Data type of the parameter variable \f$ \theta = \{\mu, \Sigma\} \f$.
  using TParameterVAR = std::tuple<arma::vec, arma::mat>;

 private:
  //! mean vector \f$\mu\f$.
  arma::vec mean_;
  //! covariance matrix \f$\Sigma\f$.
  arma::mat covariance_;
  /** a normal distribution random generator \f$\mathcal{N}(0,1)\f$ */
  std::normal_distribution<double> normal_;
  //! \f$\pi\f$
  static constexpr double pi = 3.1415926535897;
  //! inverse of covariance matrix \f$\Sigma^{-1}\f$.
  arma::mat inv_cov_;
  //! partitioning function \f$\frac{1}{(2\pi)^{D/2}\sqrt{|\Sigma|}}\f$.
  double part_;
  //! Cholesky decomposition of covariance matrix, i.e. \f$L\f$ such that  \f$LL^T=\Sigma\f$.
  arma::mat chol_dec_;
  //! Dimension \f$D\f$
  int dim_;

 private:
  /**
   * Calculates useful constant values for avoiding recalculation for every
   * call to likelihood().
   */
  void calcDistConstants() {
    dim_ = mean_.n_rows;
    inv_cov_ = arma::inv(covariance_);
    // calculate partition function
    double det_cov = arma::det(covariance_);
    double den_pi = 1 / std::sqrt(std::pow(2 * pi, dim_));
    part_ = den_pi * (1 / std::sqrt(det_cov));
    // Cholesky decomposition
    chol_dec_ = arma::chol(covariance_, "lower");
  }

 public:
  Gaussian() = delete;
  /** Default constructor.
   * Returns D dimensional Gaussian distribution with zero mean and identity
   * covariance matrix.
   */
  Gaussian(int dim) : Gaussian(arma::zeros(dim), arma::eye(dim, dim)) {}

  /**
   * Returns D dimensional Gaussian with given mean and covariance. The
   * covariance should be positive definite.
   *
   * @param mean The mean vector.
   * @param covariance The covariance matrix.
   */
  Gaussian(arma::vec mean, arma::mat covariance)
      : mean_(std::move(mean)), covariance_(std::move(covariance)) {
    calcDistConstants();
  }

  /** Returns a random variable from the distribution.
   * \f[ \mathbf{x} \sim \mathcal{N}(\mu, \Sigma) \f]
   * @return The random vector \f$\mathbf{x}\f$
   */
  arma::vec random() {
    arma::vec rnd(dim_); // how much overload the vector construction has?
    rnd.imbue(
        [&]() { return normal_(random::Generator::get().getGenerator()); });
    return mean_ + chol_dec_ * rnd;
  }

  /** Returns the likelihood of a given random variable.
   * \f[p(\mathbf{x}) = \frac{1}{(2\pi)^{D/2}\sqrt{|\Sigma|}}
   * \exp(-\frac{1}{2}\mathbf{x}^T\Sigma^{-1}\mathbf{x})\f]
   * @param rv The random variable \f$\mathbf{x}\f$ for which likelihood is calculated.
   * @return \f$p(\mathbf{x})\f$.
   */
  double likelihood(const arma::vec &rv) const {
    const auto diff = rv - mean_;
    const arma::vec tmp = diff.t() * inv_cov_ * diff;
    const double expt = -tmp(0) / 2;
    return part_ * std::exp(expt);
  }

  /** Changes the mean and covariance of the distribution with the given
   * parameters.
   * @param parameters A tuple containing mean and covariance.
   * @return Reference to the current instance.
   */
  Gaussian &parameterize(const TParameterVAR &parameters) {
    return parameterize(std::get<0>(parameters), std::get<1>(parameters));
  }

  /** Changes the mean and covariance of the distribution with the given values.
   * @param mean The mean vector.
   * @param covariance The covariance matrix.
   * @return Reference to the current instance.
   */
  Gaussian &parameterize(const arma::vec &mean,
                         const arma::mat &covariance) {
    mean_ = mean;
    covariance_ = covariance;
    calcDistConstants();
    return (*this);
  }
  //! Returns the mean vector
  const arma::vec& getMean() const { return mean_; }
  //! Returns the covariance matrix
  const arma::mat& getCovariance() const { return covariance_; }
};

} // namespace distribution
} // namespace ssmkit

#endif // SSMPACK_DISTRIBUTION_GAUSSIAN_HPP


/**
 * @file initcondlv_main.cc
 * @brief NPDE homework InitCondLV code
 * @author lfilippo, tille, jgacon, dcasati
 * @copyright Developed at ETH Zurich
 */

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

#include "initcondlv.h"

int main() {
  /* Compute initial conditions u0, v0 such that solution has period T=5
   * using inital approximation [3,2]^T */
  /* SAM_LISTING_BEGIN_2 */
  Eigen::Vector2d y(3, 2);  // Initial guess
  double T = 25;             // Period

  Eigen::Vector2d F;

  while(true) {
	auto [phi, W] = InitCondLV::PhiAndW(y[0], y[1], T);

	F = phi - y;
	Eigen::Matrix2d DF = W - Eigen::Matrix2d::Identity();

	if(F.norm() > 1e-10) {
		y -= DF.lu().solve(F);
	} else {
		break;
	}

  }

  // Calculate the evolution of the Lotka-Volterra ODE up to
  // time 100, using the initial value y, found by the Newton iterations above.
  // If the evolution is indeed 5-periodic, then the solution at time 100
  // should have the value y. Print a warning message if this is not the case.
  //====================
  // Your code goes here
  //====================

  std::cout << y << std::endl;

  // Testing obtained solution against reference "exact" solution
  Eigen::Vector2d ref(3.1098751029156, 2.08097564048345);
  double tol = 1.0e-8;
  double error = (ref - y).lpNorm<Eigen::Infinity>();
  if (std::abs(error) > tol) {
    std::cout
        << " Error w.r.t. to reference solution is GREATER than tol=1.0e-8."
        << std::endl;
  } else {
    std::cout
        << " Error w.r.t. to reference solution is SMALLER than tol=1.0e-8."
        << std::endl;
  }

  /* SAM_LISTING_END_2 */
  return 0;
}


// TestPI.cpp
//
// Calculation of PI using random numbers; it's analogous
// to throwing darts.
//
// (C) Datasim Education BV 2010-2011
//

#include <boost/random.hpp> // Convenience header file
#include <iostream>
#include <ctime>			// std::time
using namespace std;

#include <boost/limits.hpp>

int main()
{
	// A. Define the lagged Fibonacci and Normal objects
	boost::lagged_fibonacci607 rng;
	rng.seed(static_cast<boost::uint32_t> (std::time(0)));
	boost::uniform_real<> uni(0.0,1.0);

	// B. Produce Uniform (0, 1)
	boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_real<> >
							uniRng(rng, uni);

	// C. Choose the desired accuracy
	cout << "How many darts to throw? "; long N; cin >> N;

	// D. Thow the dart; does it fall in the circle or outside
	// Start throwing darts and count where they land
	long hits = 0;
	double x, y, distance;
	for (long n = 1; n <= N; ++n)
	{
		x = uniRng(); y = uniRng();
		distance = sqrt(x*x + y*y);

		if ( distance <=1)
		{
			hits++;
		}
	}

	//E.  Produce the answer
	cout << "Numbers of hits, PI is: " << hits << ", " << 4.0 * double(hits) / double (N) << endl;

	return 0;

}int outcome;

Level_8/V. An Introduction to the Boost C++ Libraries/04b - Random - PI calculation/TestPI.cpp


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

// Copyright Paul A. Bristow 2015.
// Copyright Christopher Kormanyos 2015.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

//! \brief Example program showing a polynomial approximation of cos(negatable).

// Below are snippets of code that are included into Quickbook file fixed_point.qbk.

#include <iomanip>
#include <iostream>

#include <boost/fixed_point/fixed_point.hpp>

namespace local
{
  template<typename NumericType>
  NumericType cos(const NumericType& x)
  {
    // This subroutine computes cos(x) using a beautiful
    // approximation obtained by Laguerre in 1880 which
    // achieves about 3-4 decimal digits of precision
    // in the range 0 < x < +pi/2.

    // See J. F. Hart et al., Computer Approximations
    // (John Wiley and Sons, Inc., 1968), Eq. 2.4.4
    // in Sect. 2.4 on page 27.
    // In other words:
    //  cos(pi x/2) = 1 - chi^2 / {chi - (chi - 1) sqrt[(2 - chi) / 3]},
    // where a scaled argument chi is used with
    //  chi = x / (pi/2).

    const NumericType chi  = (x * 2) / NumericType(3.14159265358979323846F);

    return 1 - ((chi * chi) / (chi - ((chi - 1) * sqrt((2 - chi) / 3))));
  }
}

int main()
{
  // This example computes approximations of cos(x) in the
  // range 0 < x < pi/2. Comparison are made between
  // fixed-point calculations and floating-point calculations.
  // A 16-bit fixed-point type is used, as may be well-suited
  // for a tiny bare-metal embedded system.

  // The arguments used in the comparisons are:
  //   x = (approx.) 1/4, 1/2, 3/4, 1, 5/4, 3/2.

  // Here we compute cos(n/4) for 1 <= n < 7 for
  // a fixed-point negatable type.
  std::cout << "fixed-point:  ";
  for(int n = 1; n < 7; ++n)
  {
    typedef boost::fixed_point::negatable<4, -11> fixed_point_type;

    const fixed_point_type x = fixed_point_type(n) / 4;

    std::cout << std::setprecision(4)
              << std::fixed
              << local::cos(x)
              << " ";
  }
  std::cout << std::endl;

  // Compare with the control values of cos(n/4)
  // computed with a built-in floating-point type.
  std::cout << "float-point:  ";
  for(int n = 1; n < 7; ++n)
  {
    using std::cos;

    const float x = float(n) / 4;

    std::cout << std::setprecision(4)
              << std::fixed
              << cos(x)
              << " ";
  }
  std::cout << std::endl;
}


/* a zero-sum game example
 payoff matrix
 4 1 8
 2 3 1
 0 4 3
 player's optimal strategy
 I's  (.25,.75,0)
 II's (.5,.5,0)
 Nash equilibrium value 5/2

 Another zero-sum game example
 payoff matrix
 2 3 1 5
 4 1 6 0
 player's optimal strategy
 I's  (0.7143,0.2857)
 II's (0,0.7143,0.2857,0)
 Nash equilibrium value 2.42857
*/

#include <armadillo>
#include "zeroSumGame.h"

using namespace arma;
using namespace std;

int main()
{
//    Mat<double> payoffMat(3,3);
//    payoffMat << 4 << 1 << 8 << endr
//    << 2 << 3 << 1 << endr
//    << 0 << 4 << 3 << endr;

    Mat<double> payoffMat(2,4);
    payoffMat << 2 << 3 << 1 << 5 << endr
              << 4 << 1 << 6 << 0 << endr;

    double gameValue = 0;
    //Col<double> p(payoffMat.n_rows);
    //p.fill(0.0);
    //Col<double> q(payoffMat.n_cols);
    //q.fill(0.0);
    Col<double> p;
    Col<double> q;
    double equilibriumThreshold = 1e-4;
    bool checkNonnegative = true;

    double maxAbsValue = max(max(abs(payoffMat)));
    Mat<double> normPayoffMat = payoffMat/maxAbsValue+1.0;
    if(ZeroSumGame::solveGame(normPayoffMat,gameValue,p,q,equilibriumThreshold,checkNonnegative)){
        gameValue = (gameValue-1)*maxAbsValue;
        cout << gameValue << endl;
        p.print("p:");
        q.print("q:");
    }
    return 0;
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial: Least Squares problem for matrices from ViennaCL or Boost.uBLAS (least-squares.cpp and least-squares.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*   See Example 2 at http://tutorial.math.lamar.edu/Classes/LinAlg/QRDecomposition.aspx for a reference solution.
*
*/

// activate ublas support in ViennaCL
#define VIENNACL_WITH_UBLAS

//
// include necessary system headers
//
#include <iostream>

//
// Boost includes
//
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

//
// ViennaCL includes
//
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/matrix_proxy.hpp"
#include "viennacl/linalg/qr.hpp"
#include "viennacl/linalg/lu.hpp"

/*
*   Tutorial: Least Squares problem for matrices from ViennaCL or Boost.uBLAS
*
*   See Example 2 at http://tutorial.math.lamar.edu/Classes/LinAlg/QRDecomposition.aspx for a reference solution.
*/

int main (int, const char **)
{
  typedef float               ScalarType;     //feel free to change this to 'double' if supported by your hardware
  typedef boost::numeric::ublas::matrix<ScalarType>              MatrixType;
  typedef boost::numeric::ublas::vector<ScalarType>              VectorType;
  typedef viennacl::matrix<ScalarType, viennacl::column_major>   VCLMatrixType;
  typedef viennacl::vector<ScalarType>                           VCLVectorType;

  //
  // Create vectors and matrices with data, cf. http://tutorial.math.lamar.edu/Classes/LinAlg/QRDecomposition.aspx
  //
  VectorType ublas_b(4);
  ublas_b(0) = -4;
  ublas_b(1) =  2;
  ublas_b(2) =  5;
  ublas_b(3) = -1;

  MatrixType ublas_A(4, 3);
  MatrixType Q = boost::numeric::ublas::zero_matrix<ScalarType>(4, 4);
  MatrixType R = boost::numeric::ublas::zero_matrix<ScalarType>(4, 3);

  ublas_A(0, 0) =  2; ublas_A(0, 1) = -1; ublas_A(0, 2) =  1;
  ublas_A(1, 0) =  1; ublas_A(1, 1) = -5; ublas_A(1, 2) =  2;
  ublas_A(2, 0) = -3; ublas_A(2, 1) =  1; ublas_A(2, 2) = -4;
  ublas_A(3, 0) =  1; ublas_A(3, 1) = -1; ublas_A(3, 2) =  1;

  //
  // Setup the matrix in ViennaCL:
  //
  VCLVectorType vcl_b(ublas_b.size());
  VCLMatrixType vcl_A(ublas_A.size1(), ublas_A.size2());

  viennacl::copy(ublas_b, vcl_b);
  viennacl::copy(ublas_A, vcl_A);

  //////////// Part 1: Use Boost.uBLAS for all computations ////////////////

  std::cout << "--- Boost.uBLAS ---" << std::endl;
  std::vector<ScalarType> ublas_betas = viennacl::linalg::inplace_qr(ublas_A);  //computes the QR factorization

  // compute modified RHS of the minimization problem:
  // b' := Q^T b
  viennacl::linalg::inplace_qr_apply_trans_Q(ublas_A, ublas_betas, ublas_b);

  // Final step: triangular solve: Rx = b'', where b'' are the first three entries in b'
  // We only need the upper left square part of A, which defines the upper triangular matrix R
  boost::numeric::ublas::range ublas_range(0, 3);
  boost::numeric::ublas::matrix_range<MatrixType> ublas_R(ublas_A, ublas_range, ublas_range);
  boost::numeric::ublas::vector_range<VectorType> ublas_b2(ublas_b, ublas_range);
  boost::numeric::ublas::inplace_solve(ublas_R, ublas_b2, boost::numeric::ublas::upper_tag());

  std::cout << "Result: " << ublas_b2 << std::endl;

  //////////// Part 2: Use ViennaCL types for BLAS 3 computations, but use Boost.uBLAS for the panel factorization ////////////////

  std::cout << "--- ViennaCL (hybrid implementation)  ---" << std::endl;
  std::vector<ScalarType> hybrid_betas = viennacl::linalg::inplace_qr(vcl_A);

  // compute modified RHS of the minimization problem:
  // b := Q^T b
  viennacl::linalg::inplace_qr_apply_trans_Q(vcl_A, hybrid_betas, vcl_b);

  // Final step: triangular solve: Rx = b'.
  // We only need the upper part of A such that R is a square matrix
  viennacl::range vcl_range(0, 3);
  viennacl::matrix_range<VCLMatrixType> vcl_R(vcl_A, vcl_range, vcl_range);
  viennacl::vector_range<VCLVectorType> vcl_b2(vcl_b, vcl_range);
  viennacl::linalg::inplace_solve(vcl_R, vcl_b2, viennacl::linalg::upper_tag());

  std::cout << "Result: " << vcl_b2 << std::endl;

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


#include <ctime>
#include <iostream>

#include <Eigen/Core>
// 稠密矩阵的代数运算(逆、特征值等)
#include <Eigen/Dense>

using namespace std;

#define MATRIX_SIZE 50

int main() {
    // define
    Eigen::Matrix<float, 2, 3> matrix_23;
    Eigen::Vector3d v_3d;
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matrix_x;

    // output
    cout << "output ..." << endl;
    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout << matrix_23 << endl;

    // access
    cout << "access ..." << endl;
    for (int i = 0; i < 1; ++i) {
        for (int j = 0; j < 2; ++j) {
            cout << matrix_23(i, j) << endl;
        }
    }

    // vector * matrix
    cout << "vector * matrix ..." << endl;
    v_3d << 3, 2, 1;
    // 不能混用不同类型的矩阵，应该转换，并且维度要正确
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    // matrix operation ...
    cout << "matrix operation ..." << endl;
    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl;
    cout << "transponse: \n" << matrix_33.transpose() << endl;
    cout << "sum: \n" << matrix_33.sum() << endl;
    cout << "trace: \n" << matrix_33.trace() << endl;
    cout << "times: \n" << 10 * matrix_33 << endl;
    cout << "inverse: \n" << matrix_33.inverse() << endl;
    cout << "determinant: \n" << matrix_33.determinant() << endl;

    // eigen value
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(matrix_33.transpose() * matrix_33);
    cout << "eigen value: \n" << eigenSolver.eigenvalues() << endl;
    cout << "eigen vectors: \n" << eigenSolver.eigenvectors() << endl;

    // solve: matrix_NN * x = v_Nd：直接求逆与矩形分解计算
    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);

    clock_t time_start = clock();
    Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout << "time use in normal inverse is " << 1000 * (clock() - time_start) / static_cast<double>(CLOCKS_PER_SEC)
         << " ms" << endl;

    time_start = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time use in QR composition is " << 1000 * (clock() - time_start) / static_cast<double>(CLOCKS_PER_SEC)
         << " ms" << endl;

    return 0;
}


// find_scale.cpp

// Copyright Paul A. Bristow 2007, 2010.

// 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 of finding scale (standard deviation) for normal (Gaussian).

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[find_scale1
/*`
First we need some includes to access the __normal_distrib,
the algorithms to find scale (and some std output of course).
*/

#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.
#include <boost/math/distributions/find_scale.hpp>
  using boost::math::find_scale;
  using boost::math::complement; // Needed if you want to use the complement version.
  using boost::math::policies::policy; // Needed to specify the error handling policy.

#include <iostream>
  using std::cout; using std::endl;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;
//] [/find_scale1]

int main()
{
  cout << "Example: Find scale (standard deviation)." << endl;
  try
  {
//[find_scale2
/*`
For this example, we will use the standard __normal_distrib,
with location (mean) zero and standard deviation (scale) unity.
Conveniently, this is also the default for this implementation's constructor.
*/
  normal N01;  // Default 'standard' normal distribution with zero mean
  double sd = 1.; // and standard deviation is 1.
/*`Suppose we want to find a different normal distribution with standard deviation
so that only fraction p (here 0.001 or 0.1%) are below a certain chosen limit
(here -2. standard deviations).
*/
  double z = -2.; // z to give prob p
  double p = 0.001; // only 0.1% below z = -2

  cout << "Normal distribution with mean = " << N01.location()  // aka N01.mean()
    << ", standard deviation " << N01.scale() // aka N01.standard_deviation()
    << ", has " << "fraction <= " << z
    << ", p = "  << cdf(N01, z) << endl;
  cout << "Normal distribution with mean = " << N01.location()
    << ", standard deviation " << N01.scale()
    << ", has " << "fraction > " << z
    << ", p = "  << cdf(complement(N01, z)) << endl; // Note: uses complement.
/*`
[pre
Normal distribution with mean = 0 has fraction <= -2, p = 0.0227501
Normal distribution with mean = 0 has fraction > -2, p = 0.97725
]
Noting that p = 0.02 instead of our target of 0.001,
we can now use `find_scale` to give a new standard deviation.
*/
   double l = N01.location();
   double s = find_scale<normal>(z, p, l);
   cout << "scale (standard deviation) = " << s << endl;
/*`
that outputs:
[pre
scale (standard deviation) = 0.647201
]
showing that we need to reduce the standard deviation from 1. to 0.65.

Then we can check that we have achieved our objective
by constructing a new distribution
with the new standard deviation (but same zero mean):
*/
  normal np001pc(N01.location(), s);
/*`
And re-calculating the fraction below (and above) our chosen limit.
*/
  cout << "Normal distribution with mean = " << l
    << " has " << "fraction <= " << z
    << ", p = "  << cdf(np001pc, z) << endl;
  cout << "Normal distribution with mean = " << l
    << " has " << "fraction > " << z
    << ", p = "  << cdf(complement(np001pc, z)) << endl;
/*`
[pre
Normal distribution with mean = 0 has fraction <= -2, p = 0.001
Normal distribution with mean = 0 has fraction > -2, p = 0.999
]

[h4 Controlling how Errors from find_scale are handled]
We can also control the policy for handling various errors.
For example, we can define a new (possibly unwise)
policy to ignore domain errors ('bad' arguments).

Unless we are using the boost::math namespace, we will need:
*/
  using boost::math::policies::policy;
  using boost::math::policies::domain_error;
  using boost::math::policies::ignore_error;

/*`
Using a typedef is convenient, especially if it is re-used,
although it is not required, as the various examples below show.
*/
  typedef policy<domain_error<ignore_error> > ignore_domain_policy;
  // find_scale with new policy, using typedef.
  l = find_scale<normal>(z, p, l, ignore_domain_policy());
  // Default policy policy<>, needs using boost::math::policies::policy;

  l = find_scale<normal>(z, p, l, policy<>());
  // Default policy, fully specified.
  l = find_scale<normal>(z, p, l, boost::math::policies::policy<>());
  // New policy, without typedef.
  l = find_scale<normal>(z, p, l, policy<domain_error<ignore_error> >());
/*`
If we want to express a probability, say 0.999, that is a complement, `1 - p`
we should not even think of writing `find_scale<normal>(z, 1 - p, l)`,
but use the __complements version (see __why_complements).
*/
  z = -2.;
  double q = 0.999; // = 1 - p; // complement of 0.001.
  sd = find_scale<normal>(complement(z, q, l));

  normal np95pc(l, sd); // Same standard_deviation (scale) but with mean(scale) shifted
  cout << "Normal distribution with mean = " << l << " has "
    << "fraction <= " << z << " = "  << cdf(np95pc, z) << endl;
  cout << "Normal distribution with mean = " << l << " has "
    << "fraction > " << z << " = "  << cdf(complement(np95pc, z)) << endl;

/*`
Sadly, it is all too easy to get probabilities the wrong way round,
when you may get a warning like this:
[pre
Message from thrown exception was:
   Error in function boost::math::find_scale<Dist, Policy>(complement(double, double, double, Policy)):
   Computed scale (-0.48043523852179076) is <= 0! Was the complement intended?
]
The default error handling policy is to throw an exception with this message,
but if you chose a policy to ignore the error,
the (impossible) negative scale is quietly returned.
*/
//] [/find_scale2]
  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

//[find_scale_example_output
/*`
[pre
Example: Find scale (standard deviation).
Normal distribution with mean = 0, standard deviation 1, has fraction <= -2, p = 0.0227501
Normal distribution with mean = 0, standard deviation 1, has fraction > -2, p = 0.97725
scale (standard deviation) = 0.647201
Normal distribution with mean = 0 has fraction <= -2, p = 0.001
Normal distribution with mean = 0 has fraction > -2, p = 0.999
Normal distribution with mean = 0.946339 has fraction <= -2 = 0.001
Normal distribution with mean = 0.946339 has fraction > -2 = 0.999
]
*/
//] [/find_scale_example_output]


#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main(void)
{
  double theta = M_PI/6;
  Matrix2d rot;
  rot <<
    cos(theta), -sin(theta),
    sin(theta), cos(theta);
  auto trans_rot = rot.transpose();
  auto inv_rot = rot.inverse();

  cout << "==================" << endl;
  cout << rot << endl;
  cout << "==================" << endl;
  cout << trans_rot << endl;
  cout << "==================" << endl;
  cout << inv_rot << endl;
  cout << "==================" << endl;
  cout << rot*trans_rot << endl;
  cout << "==================" << endl;

  Matrix2d test;
  test <<
    2,3,
    6,4;
  cout << "==================" << endl;
  cout << test << endl;
  cout << "==================" << endl;
  cout << test.inverse() << endl;
  cout << "==================" << endl;
  cout << test * test.inverse() << endl;
  cout << "==================" << endl;

  return 0;
}


#include "Util.h"

#include <Eigen/SVD>

#include <vector>
#include <cassert>

using namespace std;

float Ubpa::MutualCoherence(const Eigen::MatrixXf& A) {
	size_t N = A.cols();

	vector<float> norms(N);

	for (size_t i = 0; i < N; i++)
		norms[i] = A.col(i).norm();

	float max = 0.f;

	for (size_t i = 0; i < N; i++) {
		for (size_t j = i + 1; j < N; j++) {
			float ai_dot_aj = A.col(i).dot(A.col(j));
			float val = std::abs(ai_dot_aj) / norms[i] / norms[j];
			if (val > max)
				max = val;
		}
	}

	return max;
}

Eigen::VectorXf Ubpa::Shrink(const Eigen::VectorXf& z, float tau) {
	assert(tau > 0);

	size_t N = z.size();

	Eigen::VectorXf x(N);

	for (size_t i = 0; i < N; i++) {
		if (z[i] > tau)
			x[i] = z[i] - tau;
		else if (z[i] < -tau)
			x[i] = z[i] + tau;
		else
			x[i] = 0.f;
	}

	return x;
}

Eigen::VectorXf Ubpa::MoreauYosidaRegularization_Norm1(const Eigen::VectorXf& z, float tau) {
	return Shrink(z, tau);
}

Eigen::VectorXf Ubpa::MoreauYosidaRegularization_Norm2(const Eigen::VectorXf& z, float tau)
{
	assert(tau > 0);

	float normZ = z.norm();
	float coff = std::max(normZ - tau, 0.f);

	Eigen::VectorXf x = coff / normZ * z;

	return x;
}

Eigen::MatrixXf Ubpa::MoreauYosidaRegularization_NormNuclear(const Eigen::MatrixXf& Z, float tau)
{
	// ? : Eigen::ComputeThinU | Eigen::ComputeThinV
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(Z, Eigen::ComputeThinU | Eigen::ComputeThinV);
	// Z = U * S * V^T
	Eigen::MatrixXf U = svd.matrixU();
	Eigen::MatrixXf V = svd.matrixV();
	Eigen::MatrixXf S = U.inverse() * Z * V.transpose().inverse();

	Eigen::VectorXf Z_diag = S.diagonal();
	Eigen::VectorXf X_diag = Shrink(Z_diag, tau);

	Eigen::MatrixXf X_S = Eigen::MatrixXf::Zero(Z_diag.size(), Z_diag.size());
	for (size_t i = 0; i < Z_diag.size(); i++)
		X_S(i, i) = X_diag(i);

	Eigen::MatrixXf X = U * X_S * V.transpose();

	return X;
}

bool Ubpa::ADMM::CheckSettingValid() const {
	if (mu <= 0.f)
		return false;

	if (rho <= 0.f)
		return false;

	if (alpha < 1.f)
		return false;

	if (beta >= 1.f || beta <= 0.f)
		return false;

	if (epsilon <= 0.f)
		return false;

	if (epsilon <= 0.f)
		return false;

	return true;
}

bool Ubpa::ADMM::SetProblem(Eigen::MatrixXf A, Eigen::VectorXf b) {
	if (A.rows() != b.size())
		return false;

	this->A = std::move(A);
	this->b = std::move(b);
	m = this->A.rows();
	n = this->A.cols();

	return true;
}

// set
// - x, y, z, u_y, u_z
// - cur_rho, last_residule
void Ubpa::ADMM::Init() {
	x.setZero(n);
	y.setZero(m);
	z.setZero(n);

	u_y.setZero(m);
	u_z.setZero(n);

	cur_rho = rho;
	last_residule = b.norm();
}

void Ubpa::ADMM::Iterate() {
	// common
	Eigen::MatrixXf AT = A.transpose();
	Eigen::VectorXf Ax = A * x;

	// [[ phase 1 ]]
	// update x, y, z
	// [formula]
	// x := argmin_x (x-z+u_z)^2 + (Ax-y-b+u_y)^2
	// y := argmin_y ||y||_q + rho/2 * (y-Ax+b-u_y)^2
	// z := argmin_z mu * ||z||_p + rho/2 * (z-x-u_z)^2
	// [result]
	// - x
	//      (x-z+u_z) + A^T(Ax-y-b+u_y) = 0
	//   => x = (I + A^T A)^-1 (z - u_z + A^T(y + b - u_y))
	// - y
	//   y = MoreauYosidaRegularization_Norm*(Ax-b+u_y, 1/rho)
	// - z
	//   z = MoreauYosidaRegularization_Norm*(x+u_z, mu/rho)

	x = (Eigen::MatrixXf::Identity(n, n) + AT * A).inverse() * (z - u_z + AT * (y + b - u_y));
	switch (q)
	{
	case Ubpa::ADMM::Norm::One:
		y = MoreauYosidaRegularization_Norm1(Ax - b + u_y, 1 / cur_rho);
		break;
	case Ubpa::ADMM::Norm::Two:
		y = MoreauYosidaRegularization_Norm2(Ax - b + u_y, 1 / cur_rho);
		break;
	default:
		assert(true && "not support");
		break;
	}
	switch (p)
	{
	case Ubpa::ADMM::Norm::One:
		z = MoreauYosidaRegularization_Norm1(x + u_z, mu / cur_rho);
		break;
	case Ubpa::ADMM::Norm::Two:
		z = MoreauYosidaRegularization_Norm2(x + u_z, mu / cur_rho);
		break;
	default:
		assert(true && "not support");
		break;
	}

	// [[ phase 2 ]]
	// update u_y, u_z
	// [formula]
	// u_y := u_y + gamma * (Ax - y - b)
	// u_z := u_z + gamma * (x - z)

	u_y += gamma * (Ax - y - b);
	u_z += gamma * (x - z);

	// [[ phase 3 ]]
	// update current settings: last_residule, cur_rho

	float cur_residule = std::sqrt((x - z).squaredNorm() + (Ax - y - b).squaredNorm());
	if (cur_residule / last_residule >= beta)
		cur_rho *= alpha;

	last_residule = cur_residule;
}

bool Ubpa::ADMM::IsStoppable() {
	return last_residule < epsilon;
}


#include <iostream>
#include <cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

int main()
{
    // Eigen/Geometry provides various representation for rotation and translation
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // rotation vector with AngleAxis, it's not matrix in bottom, but could be calculated as matrix due to overloading
    // rotate 45 deg about z
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1));
    cout.precision(3);
    cout << "rotation matrix = \n" << rotation_vector.matrix() << endl;
    rotation_matrix = rotation_vector.toRotationMatrix();
    // coordinate transformation with AngleAxis
    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_rotated = rotation_vector * v;  // can do this directly?
    cout << "(1, 0, 0) after rotation: " << v_rotated.transpose() << endl;
    // or use rotation matrix
    v_rotated = rotation_matrix * v;
    cout << "(1, 0, 0) after rotation: " << v_rotated.transpose() << endl;

    // Euler angles
    // ZYX order, i.e. yaw pitch roll
    // R = Rz*Ry*Rx from body to fixed frame
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
    cout << "yaw pitch roll: " << euler_angles.transpose() << endl;
    // Euler transformation with Isometry
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();  // called 3d, actually 4x4
    T.rotate(rotation_vector);  // rotate about rotation_vector
    cout << "Transformation matrix: \n" << T.matrix() << endl;
    T.pretranslate(Eigen::Vector3d(1, 3, 4));  // translate by (1, 3, 4);
    cout << "Transformation matrix: \n" << T.matrix() << endl;

    // coordinate transformation with Transformation matrix
    Eigen::Vector3d v_transformed = T*v; // Notice we don't need T.matrix()*v; nor need extend v to be 4x1
    cout << "v transformed: " << v_transformed.transpose() << endl;

    // Eigen::Affine3d and Eigen::Projective3d
    // TODO

    // Quaternion
    // from AngleAxis
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout << "quaternions: \n" << q.coeffs().transpose() << endl;
    // notice, this order is (x, y, z, w) with imaginary coefficients first
    // while in constructing Quaternion from coefficients the required order is (w, x, y, z)
    // based on Eigen doc; but internally the stored coefficients are in oder of (x, y, z, w) - annoying
    // from rotation matrix
    q = Eigen::Quaterniond(rotation_matrix);
    cout << "quaternions: \n" << q.coeffs().transpose() << endl;
    // use quaternion to rotate one vector
    v_rotated = q*v;  // mathematically, qvq^{-1}
    cout << "(1, 0, 0) after rotation: " << v_rotated.transpose() << endl;

    return 0;

    /* Summary of various transformation expression in Eigen
     * rotation matrix (3x3):            Eigen::Matrix3d
     * angle-axis (3x1):                 Eigen::AngleAxisd - interesting one
     * euler angles (3x1):               Eigen::Vector3d
     * quaternion (3x1):                 Eigen::Quaterniond
     * homogeneous transformation (4x4): Eigen::Isometry3d
     * affine (4x4):                     Eigen::Affine3d
     * projection (4x4):                 Eigen::Projective3d
     */
}


/**
 * KalmanFilter.hpp
 * @author : koide
 * 13/08/04
 **/
#ifndef KKL_KALMAN_FILTER_HPP
#define KKL_KALMAN_FILTER_HPP

#include <Eigen/Dense>

namespace kkl{
  namespace alg{

/*****************************************************
 * Kalman filter
 * T : type of scalar
 * stateDim : dimension of the state
 * inputDim : dimension of the input
 * measurementDim : dimension of the observation
*****************************************************/
template<typename T, int stateDim, int inputDim, int measurementDim>
class KalmanFilter{
  const static int N = stateDim;
  const static int M = inputDim;
  const static int K = measurementDim;

  typedef Eigen::Matrix<T, N, 1> VectorN;
  typedef Eigen::Matrix<T, M, 1> VectorM;
  typedef Eigen::Matrix<T, K, 1> VectorK;
  typedef Eigen::Matrix<T, N, N> MatrixNN;
  typedef Eigen::Matrix<T, N, M> MatrixNM;
  typedef Eigen::Matrix<T, K, K> MatrixKK;
  typedef Eigen::Matrix<T, K, N> MatrixKN;
  typedef Eigen::Matrix<T, N, K> MatrixNK;
public:

  /*****************************************************************
   * constructor
   * transition		 : state transition matrix
   * control			 : input response matrix
   * measurement		 : observation matrix
   * processNoise		 : process noise covariance matrix
   * measurementNoise  : measurement noise covariance matrix
   * mean				 : initial mean
   * cov				 : initial covariance matrix
  *****************************************************************/
  KalmanFilter( const MatrixNN& transition, const MatrixNM& control, const MatrixKN& measurement, const MatrixNN& processNoise, const MatrixKK& measurementNoise, const VectorN& mean, const MatrixNN& cov )
    : mean( mean ),
    cov( cov ),
    transitionMatrix( transition ),
    controlMatrix( control ),
    measurementMatrix( measurement ),
    processNoiseCov( processNoise ),
    measurementNoiseCov( measurementNoise ){}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /*****************************************************************
   * predict
   * control : input
  *****************************************************************/
  void predict( const VectorM& control ){
    const auto& A = transitionMatrix;
    const auto& B = controlMatrix;
    const auto& R = processNoiseCov;

    const auto& u = control;

    mean = A * mean + B * u;
    cov = A * cov * A.transpose() + R;
  }

  /*****************************************************************
   * correct
   * measurement : measurement vector
  *****************************************************************/
  void correct( const VectorK& measurement ){
    const auto& C = measurementMatrix;
    const auto& Q = measurementNoiseCov;

    kalmanGain = cov * C.transpose() * ( C * cov * C.transpose() + Q ).inverse();
    const auto& K = kalmanGain;

    mean = mean + K * ( measurement - C * mean );
    cov = ( MatrixNN::Identity() - K * C ) * cov;
  }

  /*			getter			*/
  const VectorN& getMean() const { return mean; }
  const MatrixNN& getCov() const { return cov; }

  const MatrixNN& getTransitionMatrix() const { return transitionMatrix; }
  const MatrixNM& getControlMatrix() const { return controlMatrix; }
  const MatrixKN& getMeasurementMatrix() const { return measurementMatrix; }

  const MatrixNN& getProcessNoiseCov() const { return processNoiseCov; }
  const MatrixKK& getMeasurementNoiseCov() const { return measurementNoiseCov; }

  const MatrixNK& getKalmanGain() const { return kalmanGain; }

  /*			setter			*/
  KalmanFilter& setMean( const VectorN& m ){ mean = m;			return *this; }
  KalmanFilter& setCov( const MatrixNN& s ){ cov = s;			return *this; }

  KalmanFilter& setTransitionMatrix( const MatrixNN& t ){ transitionMatrix = t;	return *this; }
  KalmanFilter& setControlMatrix( const MatrixNM& c ){ controlMatrix = c;			return *this; }
  KalmanFilter& setMeasurementMatrix( const MatrixKN& m ){ measurementMatrix = m;	return *this; }

  KalmanFilter& setProcessNoiseCov( const MatrixNN& p ){ processNoiseCov = p;			return *this; }
  KalmanFilter& setMeasurementNoiseCov( const MatrixKK& m ){ measurementNoiseCov = m;	return *this; }
public:
  VectorN mean;			// mean of the state
  MatrixNN cov;			// covariance of the state

  MatrixNN transitionMatrix;		//
  MatrixNM controlMatrix;			//
  MatrixKN measurementMatrix;		//
  MatrixNN processNoiseCov;		//
  MatrixKK measurementNoiseCov;	//

private:
  MatrixNK kalmanGain;
};
  }
}

#endif


#include "Triangle.h"
#include "Ray.h"
#include <Eigen/Geometry>
#include <iostream>

bool Triangle::intersect(
  const Ray & ray, const double min_t, double & t, Eigen::Vector3d & n) const
{
  ////////////////////////////////////////////////////////////////////////////
  /**
   * NOTE: The following solution is found on page 79 in the textbook.
   */
  bool hit = false;

  // Set up some variables
  Eigen::Vector3d P = std::get<0>(Triangle::corners);       // Point P
  Eigen::Vector3d Q = std::get<1>(Triangle::corners);       // Point Q
  Eigen::Vector3d R = std::get<2>(Triangle::corners);       // Point R
  Eigen::Vector3d PQ = Q - P;                               // direction vector
  Eigen::Vector3d PR = R - P;                               // direction vector
  Eigen::Vector3d v = ray.direction;
  Eigen::Vector3d O = ray.origin;

  // Get the normal of the two vectors in the plane of the triangle
  Eigen::Vector3d normal = PQ.cross(PR);

  // Check if the ray intersects with the plane made by the triangle
  if (v.dot(normal) == 0) {
    return false;
  }

  // If we make it here, then we know that the ray intersects the plane at some
  // point t
  t = (normal.dot(P - O)) / (normal.dot(v));
  if (t < min_t) {
    return false;
  }

  /**
   * Now we need to check if the intersection point is in the triangle
   */
  // Eigen::Matrix3d A;
  // A << -PQ, -PR, v;
  // Eigen::Vector3d y = P - O;           // Ax = y, where x = <beta, gamma, t>

  // Set up the columns of matrix A
  // Column vector (P - Q)
  double a = -PQ[0];
  double b = -PQ[1];
  double c = -PQ[2];
  // Column vector (P - R)
  double d = -PR[0];
  double e = -PR[1];
  double f = -PR[2];
  // Column vector d (direction vector of ray)
  double g = v[0];
  double h = v[1];
  double i = v[2];

  // Column vector y                      Ax = y
  double j = P[0] - O[0];
  double k = P[1] - O[1];
  double l = P[2] - O[2];

  /**
   * Rather than use A.determinant(), we can reduce the number of operations
   * by reusing numbers.
   * double M = A.determinant();
   */
  double ei_minus_hf = e * i - h * f;
  double gf_minus_di = g * f - d * i;
  double dh_minus_eg = d * h - e * g;

  double ak_minus_jb = a * k - j * b;
  double jc_minus_al = j * c - a * l;
  double bl_minus_kc = b * l - k * c;

  // Get the determinant of A
  double M = a * ei_minus_hf + b * gf_minus_di + c * dh_minus_eg;

  // Solve for the values of x = <beta, gamma, t> in the system Ax = y
  double beta = (j * ei_minus_hf + k * gf_minus_di + l * dh_minus_eg) / M;
  double gamma = (i * ak_minus_jb + h * jc_minus_al + g * bl_minus_kc) / M;
  // double t_prime = -((f * ak_minus_jb + e * jc_minus_al + d * bl_minus_kc) / M);
  // assert(t == t_prime);                  // works properly

  if (beta >= 0 && gamma >= 0 && (beta + gamma) <= 1) {
    n = normal.normalized();
    hit = true;
  }

  return hit;
  ////////////////////////////////////////////////////////////////////////////
}


// students_t_example2.cpp

// Copyright Paul A. Bristow 2006.
// 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 2 of using Student's t

// A general guide to Student's t is at
// http://en.wikipedia.org/wiki/Student's_t-test
// (and many other elementary and advanced statistics texts).
// It says:
// The t statistic was invented by William Sealy Gosset
// for cheaply monitoring the quality of beer brews.
// "Student" was his pen name.
// Gosset was statistician for Guinness brewery in Dublin, Ireland,
// hired due to Claude Guinness's innovative policy of recruiting the
// best graduates from Oxford and Cambridge for applying biochemistry
// and statistics to Guinness's industrial processes.
// Gosset published the t test in Biometrika in 1908,
// but was forced to use a pen name by his employer who regarded the fact
// that they were using statistics as a trade secret.
// In fact, Gosset's identity was unknown not only to fellow statisticians
// but to his employer - the company insisted on the pseudonym
// so that it could turn a blind eye to the breach of its rules.

// The Students't distribution function is described at
// http://en.wikipedia.org/wiki/Student%27s_t_distribution

#include <boost/math/distributions/students_t.hpp>
   using boost::math::students_t;  // Probability of students_t(df, t).

#include <iostream>
   using std::cout;
   using std::endl;
#include <iomanip>
   using std::setprecision;
   using std::setw;
#include <cmath>
   using std::sqrt;

// This example of a one-sided test is from:
//
// from Statistics for Analytical Chemistry, 3rd ed. (1994), pp 59-60
// J. C. Miller and J. N. Miller, Ellis Horwood ISBN 0 13 0309907.

// An acid-base titrimetric method has a significant indicator error and
// thus tends to give results with a positive systematic error (+bias).
// To test this an exactly 0.1 M solution of acid is used to titrate
// 25.00 ml of exactly 0.1 M solution of alkali,
// with the following results (ml):

double reference = 25.00; // 'True' result.
const int values = 6; // titrations.
double data [values] = {25.06, 25.18, 24.87, 25.51, 25.34, 25.41};

int main()
{
   cout << "Example2 using Student's t function. ";
#if defined(__FILE__) && defined(__TIMESTAMP__) && defined(_MSC_FULL_VER)
   cout << "  " << __FILE__ << ' ' << __TIMESTAMP__ << ' '<< _MSC_FULL_VER;
#endif
   cout << endl;

   double sum = 0.;
   for (int value = 0; value < values; value++)
   { // Echo data and calculate mean.
      sum += data[value];
      cout << setw(4) << value << ' ' << setw(14) << data[value] << endl;
   }
   double mean = sum /static_cast<double>(values);
   cout << "Mean = " << mean << endl; // 25.2283

   double sd = 0.;
   for (int value = 0; value < values; value++)
   { // Calculate standard deviation.
      sd +=(data[value] - mean) * (data[value] - mean);
   }
   int degrees_of_freedom = values - 1; // Use the n-1 formula.
   sd /= degrees_of_freedom; // == variance.
   sd= sqrt(sd);
   cout << "Standard deviation = " << sd<< endl; // = 0.238279

   double t = (mean - reference) * sqrt(static_cast<double>(values))/ sd; //
   cout << "Student's t = " << t << ", with " << degrees_of_freedom << " degrees of freedom." << endl; // = 2.34725

   cout << "Probability of positive bias is " << cdf(students_t(degrees_of_freedom), t) << "."<< endl; // =  0.967108.
   // A 1-sided test because only testing for a positive bias.
   // If > 0.95 then greater than 1 in 20 conventional (arbitrary) requirement.

   return 0;
}  // int main()

/*

Output is:

------ Build started: Project: students_t_example2, Configuration: Debug Win32 ------
Compiling...
students_t_example2.cpp
Linking...
Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\students_t_example2.exe"
Example2 using Student's t function.   ..\..\..\..\..\..\boost-sandbox\libs\math_functions\example\students_t_example2.cpp Sat Aug 12 16:55:59 2006 140050727
   0          25.06
   1          25.18
   2          24.87
   3          25.51
   4          25.34
   5          25.41
Mean = 25.2283
Standard deviation = 0.238279
Student's t = 2.34725, with 5 degrees of freedom.
Probability of positive bias is 0.967108.
Build Time 0:03
Build log was saved at "file://i:\boost-06-05-03-1300\libs\math\test\Math_test\students_t_example2\Debug\BuildLog.htm"
students_t_example2 - 0 error(s), 0 warning(s)
========== Build: 1 succeeded, 0 failed, 0 up-to-date, 0 skipped ==========

*/


/**
 * @file symplectictimestepping.cc
 * @brief NPDE homework SymplecticTimestepping
 * @copyright Developed at ETH Zurich
 */

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

constexpr double PI = 3.14159265358979323846;

namespace SymplecticTimestepping {

/* SAM_LISTING_BEGIN_0 */
void sympTimestep(double tau, Eigen::Vector2d &pq_j) {
  // Coefficients of the method
  const Eigen::Vector3d a{2. / 3., -2. / 3., 1.};
  const Eigen::Vector3d b{7. / 24., 3. / 4., -1. / 24.};
  // Single step
  //====================
  // Your code goes here
  //====================
}
/* SAM_LISTING_END_0 */

/* SAM_LISTING_BEGIN_1 */
void sympTimesteppingODETest() {
  int nIter = 7;   // total number of iterations
  unsigned int m;  // number of equidistant steps
  Eigen::Vector2d approx_sol;

  // Initial conditions
  Eigen::Vector2d init_cond;
  init_cond << 0.0, 1.0;  // initial conditions

  // Evaluating the error at the final step between the approx solutions as
  // given by the symplectic method and the exact solution computed from
  // the anlytic formula.
  double errors[nIter];  // errors vector for all approx. sols
  double tau;
  for (int k = 0; k < nIter; k++) {
    m = 10 * std::pow(2, k);
    //====================
    // Your code goes here
    //====================
  }
  // Printing results
  std::cout << "Convergence of Symplectic Time Stepping Method:\n";
  std::cout << "\tM\t\terr(M)\n";
  for (int k = 0; k < nIter; k++) {
    std::cout << "\t" << 10 * std::pow(2, k) << "\t\t" << errors[k]
              << std::endl;
  }
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_3 */
Eigen::MatrixXd simulateHamiltonianDynamics(const Eigen::VectorXd &p0,
                                            const Eigen::VectorXd &q0, double T,
                                            unsigned int M) {
  int n = p0.size();
  Eigen::MatrixXd PQ(2 * n, M + 1);

  // Coefficients of the method
  Eigen::VectorXd a(3);
  a << 2. / 3., -2. / 3., 1.;
  Eigen::VectorXd b(3);
  b << 7. / 24., 3. / 4., -1. / 24.;

  double tau = T / M;
  Eigen::VectorXd pj(p0), qj(q0);
  PQ.col(0) << pj, qj;
  //====================
  // Your code goes here
  //====================
  return PQ;
}
/* SAM_LISTING_END_3 */

}  // namespace SymplecticTimestepping


/*
    Math.hpp
    ========
        Math functions implementation.
*/

#pragma once

#include <cmath>
#include <vector>
#include <algorithm>
#include <tuple>
#include <Eigen/Dense>

namespace PDBTools
{

////////////////////////////////////////////////////////////////////////////////
// Using
////////////////////////////////////////////////////////////////////////////////

using std::vector;
using std::min;
using std::max;
using std::tuple;
using Eigen::RowVector3d;
using Eigen::Matrix3d;
using Eigen::Dynamic;
using Eigen::Matrix;
using Eigen::JacobiSVD;
using Eigen::ComputeFullU;
using Eigen::ComputeFullV;

////////////////////////////////////////////////////////////////////////////////
// Convert Radians To Degrees
////////////////////////////////////////////////////////////////////////////////

double degrees(double radiansAngle)
{
    return radiansAngle * 180. / M_PI;
}

////////////////////////////////////////////////////////////////////////////////
// Convert Degrees To Radians
////////////////////////////////////////////////////////////////////////////////

double radians(double degreesAngle)
{
    return degreesAngle * M_PI / 180.;
}

////////////////////////////////////////////////////////////////////////////////
// Calc Vector Angle
////////////////////////////////////////////////////////////////////////////////

double calcVectorAngle(const RowVector3d &coordA, const RowVector3d &coordB)
{
    return acos(min(max(coordA.dot(coordB) / (coordA.norm() * coordB.norm()), -1.), 1.));
}

////////////////////////////////////////////////////////////////////////////////
// Calc Rotation Matrix (Right Multiply Matrix)
////////////////////////////////////////////////////////////////////////////////

Matrix3d calcRotationMatrix(const RowVector3d &rotationAxis, double rotationAngle)
{
    auto normRotationAxis = rotationAxis.normalized();

    double x = normRotationAxis[0], y = normRotationAxis[1], z = normRotationAxis[2],
        s = sin(rotationAngle), c = cos(rotationAngle), _1c = 1. - c;

    return (Matrix3d() <<
        c + x * x * _1c, x * y * _1c + z * s, x * z * _1c - y * s,
        x * y * _1c - z * s, c + y * y * _1c, y * z * _1c + x * s,
        x * z * _1c + y * s, y * z * _1c - x * s, c + z * z * _1c).finished();
}

////////////////////////////////////////////////////////////////////////////////
// Calc Rotation Matrix By Two Vector (Right Multiply Matrix)
////////////////////////////////////////////////////////////////////////////////

Matrix3d calcRotationMatrixByTwoVector(const RowVector3d &refCoord,
    const RowVector3d &tarCoord)
{
    return calcRotationMatrix(tarCoord.cross(refCoord),
        calcVectorAngle(tarCoord, refCoord));
}

////////////////////////////////////////////////////////////////////////////////
// Calc Dihedral Angle
////////////////////////////////////////////////////////////////////////////////

double calcDihedralAngle(const RowVector3d &coordA, const RowVector3d &coordB,
    const RowVector3d &coordC, const RowVector3d &coordD)
{
    RowVector3d AB = coordB - coordA;
    RowVector3d AC = coordC - coordA;
    RowVector3d DB = coordB - coordD;
    RowVector3d DC = coordC - coordD;

    RowVector3d ABAC = AB.cross(AC);
    RowVector3d DBDC = DB.cross(DC);

    double dihedralAngle = calcVectorAngle(ABAC, DBDC);

    // Calc Sign
    RowVector3d OA = coordA - coordB;
    RowVector3d OC = coordC - coordB;
    RowVector3d OD = coordD - coordB;

    double rotationAngle = calcVectorAngle(OC, RowVector3d(1., 0., 0.));

    Matrix3d rotationMatrix = calcRotationMatrix(OC.cross(RowVector3d(1., 0., 0.)),
        rotationAngle);

    OA *= rotationMatrix;
    OD *= rotationMatrix;

    OA[0] = 0.;
    OD[0] = 0.;

    rotationAngle  = calcVectorAngle(OA, RowVector3d(0., 0., 1.));
    rotationMatrix = calcRotationMatrix(OA.cross(RowVector3d(0., 0., 1.)), rotationAngle);

    OD *= rotationMatrix;

    if (OD[1] > 0.)
    {
        dihedralAngle = -dihedralAngle;
    }

    return dihedralAngle;
}

////////////////////////////////////////////////////////////////////////////////
// Calc RMSD (Root-Mean-Square Deviation)
////////////////////////////////////////////////////////////////////////////////

double calcRMSD(const Matrix<double, Dynamic, 3> &coordMatrixA,
    const Matrix<double, Dynamic, 3> &coordMatrixB)
{
    return sqrt((coordMatrixA - coordMatrixB).array().square().sum() / coordMatrixA.rows());
}

////////////////////////////////////////////////////////////////////////////////
// Calc Superimpose Rotation Matrix (Kabsch Algorithm)
////////////////////////////////////////////////////////////////////////////////

tuple<RowVector3d, Matrix3d, RowVector3d> calcSuperimposeRotationMatrix(
    const Matrix<double, Dynamic, 3> &tarCoordMatrix,
    const Matrix<double, Dynamic, 3> &srcCoordMatrix)
{
    RowVector3d srcCenterCoord = srcCoordMatrix.colwise().mean();
    RowVector3d tarCenterCoord = tarCoordMatrix.colwise().mean();

    JacobiSVD<Matrix3d> svd(
        (srcCoordMatrix.rowwise() - srcCenterCoord).transpose() *
        (tarCoordMatrix.rowwise() - tarCenterCoord),
        ComputeFullU | ComputeFullV);

    Matrix3d U = svd.matrixU(), V = svd.matrixV().transpose();

    if (U.determinant() * V.determinant() < 0.)
    {
        U.col(2) = -U.col(2);
    }

    auto rotationMatrix = U * V;

    return {srcCenterCoord, rotationMatrix, tarCenterCoord};
}

////////////////////////////////////////////////////////////////////////////////
// Calc RMSD After Superimpose A <= B
////////////////////////////////////////////////////////////////////////////////

double calcRMSDAfterSuperimpose(
    const Matrix<double, Dynamic, 3> &tarCoordMatrix,
    const Matrix<double, Dynamic, 3> &srcCoordMatrix)
{
    auto [srcCenterCoord, rotationMatrix, tarCenterCoord] =
        calcSuperimposeRotationMatrix(tarCoordMatrix, srcCoordMatrix);

    return calcRMSD(tarCoordMatrix, (((srcCoordMatrix.rowwise() - srcCenterCoord) *
        rotationMatrix).rowwise() + tarCenterCoord).eval());
}

}  // End namespace PDBTools


//functions to convert from ECEF to ENU and vice-versa
#include <armadillo>

using namespace arma;

mat R_ecef_enu(double lat,double lgt,bool give_enu_ecef=false,bool isRadians=false)
{
//in the direct version, we are working with the matrix that
//rotates from ECEF to ENU. If you want from ENU to ECEF, use the third parameter.
//font: https://en.m.wikipedia.org/wiki/Geographic_coordinate_conversion#Molodensky_transformation

    mat res = eye(3,3);

    double c = datum::pi/180;

    if(isRadians)
    {
    c= 1.0f;
    }

    double sLat = sin(lat*c),cLat = cos(lat*c);
    double sLgt = sin(lgt*c),cLgt = cos(lgt*c);

    res(0,0)=-sLgt;        res(0,1)= cLgt;         res(0,2)=0;
    res(1,0)=-sLat*cLgt;   res(1,1)=-sLat*sLgt;    res(1,2)= cLat;
    res(2,0)= cLat*cLgt;   res(2,1)= cLat*sLgt;    res(2,2)= sLat;

    if(give_enu_ecef)
    {
    res = res.t();
    }

    return res;
}

vec3 ECEF_to_ENU(vec3 vECEF,vec3 v0ECEF,double lat0,double lgt0,bool givenRadians = false)
{
    //as output an ENU vector
    mat R = R_ecef_enu(lat0,lgt0,false,givenRadians);
    //cout<<"ECEFtoENU"<<endl<<R<<endl;
    return R*(vECEF-v0ECEF);
}
//

vec3 ENU_to_ECEF(vec3 vENU,vec3 v0ECEF,double lat0,double lgt0,bool givenRadians = false)
{
    //as output an ECEF vector
    mat R = R_ecef_enu(lat0,lgt0,true,givenRadians);
    //cout<<"ENUtoECEF"<<endl<<R*R.t()<<endl;
    return (R*vENU)+v0ECEF;
}

/// end of the file


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

#include "timer.h"

//! \brief Compute the Kronecker product $C = A \otimes B$.
//! \param[in] A Matrix $n \times n$
//! \param[in] B Matrix $n \times n$
//! \param[out] C Kronecker product of A and B of dim $n^2 \times n^2$
template <class Matrix>
void kron(const Matrix & A, const Matrix & B, Matrix & C)
{
    // TODO
}

//! \brief Compute the Kronecker product C = $A \otimes B$. Exploit matrix-vector product.
//! A,B and x must have dimension n \times n resp. n
//! \param[in] A Matrix $n \times n$
//! \param[in] B Matrix $n \times n$
//! \param[in] x Vector of dim $n$
//! \param[out] y Vector y = kron(A,B)*x
template <class Matrix, class Vector>
void kron_fast(const Matrix & A, const Matrix & B, const Vector & x, Vector & y)
{
    // TODO
}

//! \brief Compute the Kronecker product $C = A \otimes B$. Uses fast remapping tecniques (similar to Matlab reshape)
//! A,B and x must have dimension n \times n resp. n*n
//! \param[in] A Matrix $n \times n$
//! \param[in] B Matrix $n \times n$
//! \param[in] x Vector of dim $n$
//! \param[out] y Vector y = kron(A,B)*x
template <class Matrix, class Vector>
void kron_super_fast(const Matrix & A, const Matrix & B, const Vector & x, Vector & y)
{
    // TODO
}

int main(void) {

    // Check if kron works, cf.
    Eigen::MatrixXd A(2,2);
    A << 1, 2, 3, 4;
    Eigen::MatrixXd B(2,2);
    B << 5, 6, 7, 8;
    Eigen::MatrixXd C;

    Eigen::VectorXd x = Eigen::VectorXd::Random(4);
    Eigen::VectorXd y;
    kron(A,B,C);
    y = C*x;
    std::cout << "kron(A,B)=" << std::endl << C << std::endl;
    std::cout << "Using kron: y=       " << std::endl << y << std::endl;

    kron_fast(A,B,x,y);
    std::cout << "Using kron_fast: y=  " << std::endl << y << std::endl;
    kron_super_fast(A,B,x,y);
    std::cout << "Using kron_super_fast: y=  " << std::endl << y << std::endl;

    // Compute runtime of different implementations of kron
    unsigned int repeats = 10;
    timer<> tm_kron, tm_kron_fast, tm_kron_super_fast;
    std::vector<int> times_kron, times_kron_fast, times_kron_super_fast;

    for(unsigned int p = 2; p <= 10; p++) {
        tm_kron.reset();
        tm_kron_fast.reset();
        tm_kron_super_fast.reset();
        for(unsigned int r = 0; r < repeats; ++r) {
            unsigned int M = pow(2,p);
            A = Eigen::MatrixXd::Random(M,M);
            B = Eigen::MatrixXd::Random(M,M);
            x = Eigen::VectorXd::Random(M*M);

            // May be too slow for large p, comment if so
            tm_kron.start();
//             kron(A,B,C);
//             y = C*x;
            tm_kron.stop();

            tm_kron_fast.start();
            kron_fast(A,B,x,y);
            tm_kron_fast.stop();

            tm_kron_super_fast.start();
            kron_super_fast(A,B,x,y);
            tm_kron_super_fast.stop();
        }

        std::cout << "Lazy Kron took:       " << tm_kron.avg().count() / 1000000. << " ms" << std::endl;
        std::cout << "Kron fast took:       " << tm_kron_fast.avg().count() / 1000000. << " ms" << std::endl;
        std::cout << "Kron super fast took: " << tm_kron_super_fast.avg().count() / 1000000. << " ms" << std::endl;
        times_kron.push_back( tm_kron.avg().count() );
        times_kron_fast.push_back( tm_kron_fast.avg().count() );
        times_kron_super_fast.push_back( tm_kron_super_fast.avg().count() );
    }

    for(auto it = times_kron.begin(); it != times_kron.end(); ++it) {
        std::cout << *it << " ";
    }
    std::cout << std::endl;
    for(auto it = times_kron_fast.begin(); it != times_kron_fast.end(); ++it) {
        std::cout << *it << " ";
    }
    std::cout << std::endl;
    for(auto it = times_kron_super_fast.begin(); it != times_kron_super_fast.end(); ++it) {
        std::cout << *it << " ";
    }
    std::cout << std::endl;
}

Nummerical Methods for CSE/PS1/templates/kron.cpp


/**
* Kalman filter header file.
*
* @author: Dhruv Shah, Hayk Martirosyan
* @date: 07/03/2018
*/

#include <Eigen/Dense>

#pragma once

class KalmanFilter {

public:

	/**
	* Create a Kalman filter with the specified matrices.
	*   A - System dynamics matrix
	*   B - Input matrix
	*   C - Output matrix
	*   Q - Process noise covariance
	*   R - Measurement noise covariance
	*   P - Estimate error covariance
	*/
	KalmanFilter(
			const Eigen::MatrixXd& A,
			const Eigen::MatrixXd& B,
			const Eigen::MatrixXd& C,
			const Eigen::MatrixXd& Q,
			const Eigen::MatrixXd& R,
			const Eigen::MatrixXd& P
	);

	/**
	* Initialize the filter with initial states as zero.
	*/
	void init();

	/**
	* Initialize the filter with a guess for initial states.
	*/
	void init(const Eigen::VectorXd& x0);

	/**
	* Update the prediction based on control input.
	*/
	void predict(const Eigen::VectorXd& u);

	/**
	* Update the estimated state based on measured values.
	*/
	void update(const Eigen::VectorXd& y);

	/**
	* Update the dynamics matrix.
	*/
	void update_dynamics(const Eigen::MatrixXd A);

	/**
	* Update the output matrix.
	*/
	void update_output(const Eigen::MatrixXd C);

	/**
	* Return the current state.
	*/
	Eigen::VectorXd state() { return x_hat; };

private:

	// Matrices for computation
	Eigen::MatrixXd A, B, C, Q, R, P, K, P0;

	// System dimensions
	int m, n, c;

	// Is the filter initialized?
	bool initialized = false;

	// n-size identity
	Eigen::MatrixXd I;

	// Estimated states
	Eigen::VectorXd x_hat;
};