#include <limits>
#include <vector>
#include <gtest/gtest.h>

#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math/tools/promotion.hpp>

#include <stan/math/rev.hpp>
#include <stan/math/prim/fun/sign.hpp>
#include <stan/math/prim/fun/fabs.hpp>
#include <stan/math/prim/fun/log1m.hpp>

template <typename T1, typename T2, typename T3, typename T4>
inline typename boost::math::tools::promote_args<T1, T2, T3, T4>::type
skew_de_cdf_test(const T1& y, const T2& mu, const T3& sigma, const T4& tau) {
  using stan::math::log1m;
  using std::exp;
  using std::log;

  if (y < mu) {
    return log(tau) - 2 / sigma * (1 - tau) * (mu - y);
  } else {
    return log1m((1 - tau) * exp(-2 / sigma * tau * (y - mu)));
  }
}

TEST(ProbDistributionsSkewedDoubleExponential,
     lcdf_computes_correct_gradients) {
  using stan::math::skew_double_exponential_lcdf;

  for (double ys : {-1.7, 0.2, 0.5, 0.9, 1.1, 3.2, 8.3}) {
    for (double mus : {-1.8, 0.1, 0.55, 0.89, 1.3, 4.2, 9.3}) {
      for (double sigmas : {0.1, 1.1, 3.2}) {
        for (double taus : {0.01, 0.1, 0.5, 0.9, 0.99}) {
          stan::math::var y = ys;
          stan::math::var mu = mus;
          stan::math::var sigma = sigmas;
          stan::math::var tau = taus;

          stan::math::var lp = skew_double_exponential_lcdf(y, mu, sigma, tau);
          std::vector<stan::math::var> theta;
          theta.push_back(y);
          theta.push_back(mu);
          theta.push_back(sigma);
          theta.push_back(tau);
          std::vector<double> grads;
          lp.grad(theta, grads);

          stan::math::var y_true = ys;
          stan::math::var mu_true = mus;
          stan::math::var sigma_true = sigmas;
          stan::math::var tau_true = taus;

          stan::math::var lp_test
              = skew_de_cdf_test(y_true, mu_true, sigma_true, tau_true);
          std::vector<stan::math::var> theta_true;
          theta_true.push_back(y_true);
          theta_true.push_back(mu_true);
          theta_true.push_back(sigma_true);
          theta_true.push_back(tau_true);
          std::vector<double> grads_true;
          lp_test.grad(theta_true, grads_true);

          EXPECT_NEAR(grads_true[0], grads[0], 0.001);
          EXPECT_NEAR(grads_true[1], grads[1], 0.001);
          EXPECT_NEAR(grads_true[2], grads[2], 0.001);
          EXPECT_NEAR(grads_true[3], grads[3], 0.001);
        }
      }
    }
  }
}

TEST(ProbDistributionsSkewedDoubleExponential, lcdf_works_on_scalar_arguments) {
  using stan::math::skew_double_exponential_lcdf;

  for (double ys : {0.2, 0.9, 1.1, 3.2}) {
    for (double mus : {0.1, 1.3, 3.0}) {
      for (double sigmas : {0.1, 1.1, 3.2}) {
        for (double taus : {0.01, 0.1, 0.5, 0.9, 0.99}) {
          EXPECT_NEAR(skew_de_cdf_test(ys, mus, sigmas, taus),
                      skew_double_exponential_lcdf(ys, mus, sigmas, taus),
                      0.001);
        }
      }
    }
  }
}

TEST(ProbDistributionsSkewedDoubleExponential, lcdf_works_on_vector_arguments) {
  using stan::math::skew_double_exponential_lcdf;

  std::vector<double> ys{0.2, 0.9, 1.1, 3.2};

  for (double mus : {0.1, 1.3, 3.0}) {
    for (double sigmas : {0.1, 1.1, 3.2}) {
      for (double taus : {0.01, 0.1, 0.5, 0.9, 0.99}) {
        double x = 0.0;
        for (double y : ys)
          x += skew_de_cdf_test(y, mus, sigmas, taus);

        EXPECT_NEAR(x, skew_double_exponential_lcdf(ys, mus, sigmas, taus),
                    0.001);
      }
    }
  }
}

TEST(ProbDistributionsSkewedDoubleExponential,
     lcdf_works_on_vectorial_y_and_mu) {
  using stan::math::skew_double_exponential_lcdf;
  std::vector<double> ys{0.2, 0.9, 1.1};
  std::vector<double> mus{0.1, 1.3, 3.0};

  for (double sigmas : {0.1, 1.1, 3.2}) {
    for (double taus : {0.01, 0.1, 0.5, 0.9, 0.99}) {
      double x = 0.0;
      for (int i = 0; i < 3; i++)
        x += skew_de_cdf_test(ys[i], mus[i], sigmas, taus);

      EXPECT_NEAR(x, skew_double_exponential_lcdf(ys, mus, sigmas, taus),
                  0.001);
    }
  }
}

TEST(ProbDistributionsSkewedDoubleExponential,
     lcdf_works_on_vectorial_y_and_sigma) {
  using stan::math::skew_double_exponential_lcdf;
  std::vector<double> ys{0.2, 0.9, 1.1};
  std::vector<double> sigmas{0.1, 1.1, 3.2};

  for (double mus : {0.1, 1.3, 3.0}) {
    for (double taus : {0.01, 0.1, 0.5, 0.9, 0.99}) {
      double x = 0.0;
      for (int i = 0; i < 3; i++)
        x += skew_de_cdf_test(ys[i], mus, sigmas[i], taus);

      EXPECT_NEAR(x, skew_double_exponential_lcdf(ys, mus, sigmas, taus),
                  0.001);
    }
  }
}

TEST(ProbDistributionsSkewedDoubleExponential,
     lcdf_works_on_vectorial_y_and_tau) {
  using stan::math::skew_double_exponential_lcdf;
  std::vector<double> ys{0.2, 0.9, 1.1};
  std::vector<double> taus{0.1, 0.5, 0.9};

  for (double mus : {0.1, 1.3, 3.0}) {
    for (double sigmas : {0.1, 1.1, 3.2}) {
      double x = 0.0;
      for (int i = 0; i < 3; i++)
        x += skew_de_cdf_test(ys[i], mus, sigmas, taus[i]);

      EXPECT_NEAR(x, skew_double_exponential_lcdf(ys, mus, sigmas, taus),
                  0.001);
    }
  }
}

TEST(ProbDistributionsSkewedDoubleExponential,
     lcdf_works_on_vectorial_mu_sigma_and_tau) {
  using stan::math::skew_double_exponential_lcdf;

  std::vector<double> mus{0.1, 1.3, 3.0};
  std::vector<double> sigmas{0.1, 1.1, 3.2};
  std::vector<double> taus{0.1, 0.5, 0.9};

  for (double ys : {0.1, 1.3, 3.0}) {
    double x = 0.0;
    for (int i = 0; i < 3; i++)
      x += skew_de_cdf_test(ys, mus[i], sigmas[i], taus[i]);
    EXPECT_NEAR(x, skew_double_exponential_lcdf(ys, mus, sigmas, taus), 0.001);
  }
}

TEST(ProbDistributionsSkewedDoubleExponential, lcdf_check_errors) {
  using stan::math::skew_double_exponential_lcdf;
  static double inff = std::numeric_limits<double>::infinity();
  EXPECT_THROW(stan::math::skew_double_exponential_lcdf(1.0, 0.0, -1, 0.5),
               std::domain_error);
  EXPECT_THROW(stan::math::skew_double_exponential_lcdf(1.0, 0.0, 0.1, -0.5),
               std::domain_error);
  EXPECT_THROW(stan::math::skew_double_exponential_lcdf(inff, 0.0, 0.1, 1.5),
               std::domain_error);
  EXPECT_THROW(stan::math::skew_double_exponential_lcdf(1.0, inff, 0.1, 1.5),
               std::domain_error);
}

TEST(ProbDistributionsSkewedDoubleExponential, lcdf_check_inconsistent_size) {
  using stan::math::skew_double_exponential_lcdf;

  std::vector<double> mus{0.1, 1.3, 3.0};
  std::vector<double> sigmas{0.1, 1.1, 3.2, 1.0};
  std::vector<double> taus{0.1, 0.5, 0.9};
  EXPECT_THROW(stan::math::skew_double_exponential_lcdf(1.0, mus, sigmas, taus),
               std::invalid_argument);
}

TEST(ProbDistributionsSkewedDoubleExponential, cdf_log_matches_lcdf) {
  double y = 0.8;
  double mu = 2;
  double sigma = 2.3;
  double tau = 0.1;

  EXPECT_FLOAT_EQ(
      (stan::math::skew_double_exponential_lcdf(y, mu, sigma, tau)),
      (stan::math::skew_double_exponential_cdf_log(y, mu, sigma, tau)));

  EXPECT_FLOAT_EQ(
      (stan::math::skew_double_exponential_lcdf<double, double, double, double>(
          y, mu, sigma, tau)),
      (stan::math::skew_double_exponential_cdf_log<double, double, double,
                                                   double>(y, mu, sigma, tau)));
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>  // have to include Eigen/Core before #include <opencv2/core/eigen.hpp> !!
#include <opencv2/core/eigen.hpp>

#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

using namespace Eigen;
using namespace std;
using namespace cv;

void feature_matching(const Mat& img1, const Mat& img2,
    std::vector<KeyPoint>& keypoints1,
    std::vector<KeyPoint>& keypoints2,
    std::vector< DMatch >& matches)
    {
        Mat descriptors_1, descriptors_2;
        Ptr<FeatureDetector> detector = ORB::create();
        Ptr<DescriptorExtractor> descriptor = ORB::create();

        // compute FAST corners
        detector->detect(img1, keypoints1);
        detector->detect(img2, keypoints2);

        // compute BRIEF descriptor
        descriptor->compute(img1, keypoints1, descriptors_1);
        descriptor->compute(img2, keypoints2, descriptors_2);

        Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(4);
        /*
        FLANNBASED = 1,
        BRUTEFORCE = 2,
        BRUTEFORCE_L1 = 3,
        BRUTEFORCE_HAMMING = 4,
        BRUTEFORCE_HAMMINGLUT = 5,
        BRUTEFORCE_SL2 = 6 */

        vector<DMatch> allmatches;
        matcher->match(descriptors_1, descriptors_2, allmatches);

        // Filter all match points
        // Find maximum and minimum distance
        double min_dist = 1000000, max_dist = 0;
        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            double dist = allmatches[i].distance;
            if ( dist < min_dist ) min_dist = dist;
            if ( dist > max_dist ) max_dist = dist;
        }

        cout << "max distance: " << max_dist << endl;
        cout << "min distance: " << min_dist << endl;

        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            if ( allmatches[i].distance <= max ( 2*min_dist, 30.0 ) )
            {
                matches.push_back ( allmatches[i] );
            }
        }
    }

void pose_estimation_2d2d(
    std::vector<KeyPoint>& keypoints1,
    std::vector<KeyPoint>& keypoints2,
    std::vector< DMatch >& matches,
    Mat &R, Mat&t){

        Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

        vector<Point2f> points1;
        vector<Point2f> points2;

        for ( int i = 0; i < ( int ) matches.size(); i++ )
        {
            points1.push_back ( keypoints1[matches[i].queryIdx].pt );
            points2.push_back ( keypoints2[matches[i].trainIdx].pt );
        }

        Mat fundamental_matrix;
        fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
        cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;

        // Essential matrix
        Point2d principal_point ( 325.1, 249.7 );
        double focal_length = 521;
        Mat essential_matrix;
        essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
        cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;

        // Homography Matrix
        Mat homography_matrix;
        homography_matrix = findHomography ( points1, points2, RANSAC, 3 );
        cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;

        // Calculate R and t
        recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
        cout<<"R is "<<endl<<R<<endl;
        cout<<"t is "<<endl<<t<<endl;
    }

int main(int argc, char** argv){
    Mat img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

    vector<KeyPoint> keypoints1, keypoints2;
    vector<DMatch> matches;
    feature_matching(img1, img2, keypoints1, keypoints2, matches);

    cout<< "Total " << matches.size() << "matched feature points" << endl;

    // pose estimation for two images
    Mat R, t;  // Rotation and translation
    pose_estimation_2d2d(keypoints1, keypoints2, matches, R, t);

    // epipolar geometry
    Mat K = ( Mat_<double> (3, 3) << 529.0, 0, 325.1, 0, 521.0, 249.1, 0, 0, 1);
    Mat t_x = ( Mat_<double> ( 3,3 ) <<
                0,                      -t.at<double> ( 2,0 ),     t.at<double> ( 1,0 ),
                t.at<double> ( 2,0 ),      0,                      -t.at<double> ( 0,0 ),
                -t.at<double> ( 1,0 ),     t.at<double> ( 0,0 ),      0 );

    for(DMatch mm : matches){
        Point2d pt1 = Point2d
           (
               ( keypoints1[ mm.queryIdx ].pt.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( keypoints1[ mm.queryIdx ].pt.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
        Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 );

        Point2d pt2 = Point2d
           (
               ( keypoints2[ mm.trainIdx ].pt.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( keypoints2[ mm.trainIdx ].pt.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
        Mat y2 = ( Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 );
        Mat d = y2.t() * t_x * R * y1;
        cout << "epipolar constraint = " << d << endl;
    }

    Mat E = t_x * R;
    cout << E.size() << endl;
    Eigen::Matrix<float, 3, 3> b;
    cv2eigen(E, b);
    cout << "Essential Matrix Eigenvalues and Eigenvectors" << endl;
    cout << b.eigenvalues().col(0)[0] << endl;
    cout << b.eigenvalues().col(0)[1] << endl;
    cout << b.eigenvalues().col(0)[2] << endl;
    // cout << b.eigenvectors() << endl;
    return 0;
}


#include <boost/lexical_cast.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <string>

using namespace std;
using namespace boost::multiprecision;

string lastN(string input, int n) {
	int inputSize = input.size();
	return (n > 0 && inputSize > n) ? input.substr(inputSize - n) : "";
}

int main(int argc, char *argv[]) {
	cpp_int sum = 0;
	for(cpp_int i = 1; i < 1001; i++) {
		sum += boost::multiprecision::pow(i, (int)i);
	}
	cout << lastN(sum.str(), 10) << endl;
	return 0;
}


/*
 * H1H1.cpp
 *
 *  Created on: 13.03.2018
 *      Author: thies
 */

#include <base/Util.h>
#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/sparse_direct.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/sparsity_pattern.h>
#include <deal.II/lac/vector.h>
#include <norms/H1H1.h>
#include <stddef.h>
#include <cmath>
#include <iostream>
#include <vector>

namespace wavepi {
namespace norms {

using namespace dealii;

inline double square(const double x) { return x * x; }
inline double pow4(const double x) { return x * x * x * x; }

template <int dim>
H1H1<dim>::H1H1(double alpha, double gamma) : alpha_(alpha), gamma_(gamma) {}

template <int dim>
double H1H1<dim>::norm(const DiscretizedFunction<dim>& u) const {
  auto mesh = u.get_mesh();

  // we may be able to use v, but this might introduce inconsistencies in the adjoints
  // Note: this function works even for non-constant meshes.
  auto deriv = u.calculate_derivative();

  double result = 0;

  for (size_t i = 0; i < mesh->length(); i++) {
    double nrm2 = mesh->get_mass_matrix(i)->matrix_norm_square(u[i]) +
                  gamma_ * mesh->get_laplace_matrix(i)->matrix_norm_square(u[i]);
    double nrm2_deriv = mesh->get_mass_matrix(i)->matrix_norm_square(deriv[i]) +
                        gamma_ * mesh->get_laplace_matrix(i)->matrix_norm_square(deriv[i]);

    // + trapezoidal rule in time:
    if (i > 0) result += (nrm2 + alpha_ * nrm2_deriv) / 2 * (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));

    if (i < mesh->length() - 1)
      result += (nrm2 + alpha_ * nrm2_deriv) / 2 * (std::abs(mesh->get_time(i + 1) - mesh->get_time(i)));
  }

  return std::sqrt(result);
}

template <int dim>
double H1H1<dim>::dot(const DiscretizedFunction<dim>& u, const DiscretizedFunction<dim>& v) const {
  auto mesh     = u.get_mesh();
  double result = 0.0;

  // we may be able to use v, but this might introduce inconsistencies in the adjoints
  // Note: this function works even for non-constant meshes.
  auto deriv  = u.calculate_derivative();
  auto Vderiv = v.calculate_derivative();

  for (size_t i = 0; i < mesh->length(); i++) {
    double doti = mesh->get_mass_matrix(i)->matrix_scalar_product(u[i], v[i]) +
                  gamma_ * mesh->get_laplace_matrix(i)->matrix_scalar_product(u[i], v[i]);
    double doti_deriv = mesh->get_mass_matrix(i)->matrix_scalar_product(deriv[i], Vderiv[i]) +
                        gamma_ * mesh->get_laplace_matrix(i)->matrix_scalar_product(deriv[i], Vderiv[i]);

    // + trapezoidal rule in time
    if (i > 0) result += (doti + alpha_ * doti_deriv) / 2 * (std::abs(mesh->get_time(i) - mesh->get_time(i - 1)));

    if (i < mesh->length() - 1)
      result += (doti + alpha_ * doti_deriv) / 2 * (std::abs(mesh->get_time(i + 1) - mesh->get_time(i)));
  }

  return result;
}

template <int dim>
void H1H1<dim>::dot_transform(DiscretizedFunction<dim>& u) {
  auto mesh = u.get_mesh();

  // X = (T + \alpha_ D^t T D) * (M+gamma_ L),
  // M = blocks of mass matrices, D = derivative, T = trapezoidal rule, L = blocks of laplace matrices

  DiscretizedFunction<dim> tmp(mesh, u.get_norm());
  for (size_t i = 0; i < mesh->length(); i++)
    mesh->get_laplace_matrix(i)->vmult(tmp[i], u[i]);

  u.mult_mass();
  u.add(gamma_, tmp);

  auto dx = u.calculate_derivative();

  // trapezoidal rule
  // (has to happen between D and D^t for dx)
  for (size_t i = 0; i < mesh->length(); i++) {
    double factor = 0.0;

    if (i > 0) factor += std::abs(mesh->get_time(i) - mesh->get_time(i - 1)) / 2.0;
    if (i < mesh->length() - 1) factor += std::abs(mesh->get_time(i + 1) - mesh->get_time(i)) / 2.0;

    dx[i] *= factor;
    u[i] *= factor;
  }

  auto dtdx = dx.calculate_derivative_transpose();

  // add derivative term
  for (size_t i = 0; i < mesh->length(); i++) {
    u[i].add(alpha_, dtdx[i]);
  }
}

template <int dim>
void H1H1<dim>::dot_transform_inverse(DiscretizedFunction<dim>& u) {
  LogStream::Prefix p("h1h1_transform");

  auto mesh = u.get_mesh();
  AssertThrow(mesh->length() > 7, ExcInternalError());

  Timer timer;
  timer.start();

  // space part: solve mass+ɣ*Δ
  SparseMatrix<double> system_matrix(*mesh->get_sparsity_pattern(0));
  system_matrix.copy_from(*mesh->get_mass_matrix(0));
  system_matrix.add(gamma_, *mesh->get_laplace_matrix(0));
  Vector<double> sp_tmp(u[0].size());

  for (size_t i = 0; i < mesh->length(); i++) {
    Assert(u[0].size() == u[i].size(), ExcInternalError());
    LogStream::Prefix p("step-" + Utilities::int_to_string(i, 4));

    SolverControl solver_control(2000, 1e-10 * u[i].l2_norm());
    SolverCG<> cg(solver_control);
    PreconditionIdentity precondition = PreconditionIdentity();

    sp_tmp = 0.0;
    cg.solve(system_matrix, sp_tmp, u[i], precondition);
    u[i] = sp_tmp;
  }

  // time part
  if (umfpack.n() != mesh->length()) factorize_matrix(mesh);

  // just to be sure
  for (size_t i = 0; i < mesh->length(); i++)
    Assert(u[i].size() == u[0].size(), ExcInternalError());

  // solve for every DoF
  Vector<double> tmp(mesh->length());

  for (size_t i = 0; i < u[0].size(); i++) {
    for (size_t j = 0; j < mesh->length(); j++)
      tmp[j] = u[j][i];

    umfpack.solve(tmp);

    for (size_t j = 0; j < mesh->length(); j++)
      u[j][i] = tmp[j];
  }

  deallog << "solved in " << Util::format_duration(timer.wall_time()) << std::endl;
}

template <int dim>
void H1H1<dim>::factorize_matrix(std::shared_ptr<SpaceTimeMesh<dim>> mesh) {
  deallog << "factorizing matrix" << std::endl;

  SparsityPattern pattern(mesh->length(), mesh->length(), 3);

  pattern.add(0, 0);
  pattern.add(0, 1);
  pattern.add(1, 0);
  pattern.add(1, 1);

  for (size_t i = 2; i < mesh->length() - 2; i++) {
    // fill row i and column i

    pattern.add(i, i);

    pattern.add(i, i - 2);
    pattern.add(i - 2, i);

    pattern.add(i, i + 2);
    pattern.add(i + 2, i);
  }

  pattern.add(mesh->length() - 2, mesh->length() - 1);
  pattern.add(mesh->length() - 2, mesh->length() - 1);
  pattern.add(mesh->length() - 1, mesh->length() - 2);
  pattern.add(mesh->length() - 1, mesh->length() - 1);

  pattern.compress();

  // coefficients of trapezoidal rule
  std::vector<double> lambdas(mesh->length(), 0.0);

  for (size_t i = 0; i < mesh->length(); i++) {
    if (i > 0) lambdas[i] += std::abs(mesh->get_time(i) - mesh->get_time(i - 1)) / 2.0;

    if (i < mesh->length() - 1) lambdas[i] += std::abs(mesh->get_time(i + 1) - mesh->get_time(i)) / 2.0;
  }

  SparseMatrix<double> matrix(pattern);

  double sq20 = 1.0 / square(mesh->get_time(2) - mesh->get_time(0));
  double sq10 = 1.0 / square(mesh->get_time(1) - mesh->get_time(0));
  double sq31 = 1.0 / square(mesh->get_time(3) - mesh->get_time(1));

  matrix.set(0, 0, lambdas[1] * sq20 + lambdas[0] * sq10);
  matrix.set(1, 1, lambdas[2] * sq31 + lambdas[0] * sq10);
  matrix.set(0, 1, -lambdas[0] * sq10);
  matrix.set(1, 0, -lambdas[0] * sq10);

  for (size_t i = 2; i < mesh->length() - 2; i++) {
    // fill row i and column i

    double sq20  = 1.0 / square(mesh->get_time(i + 2) - mesh->get_time(i));
    double sq0m2 = 1.0 / square(mesh->get_time(i) - mesh->get_time(i - 2));

    matrix.set(i, i, lambdas[i + 1] * sq20 + lambdas[i - 1] * sq0m2);

    matrix.set(i, i - 2, -lambdas[i - 1] * sq0m2);
    matrix.set(i - 2, i, -lambdas[i - 1] * sq0m2);

    matrix.set(i, i + 2, -lambdas[i + 1] * sq20);
    matrix.set(i + 2, i, -lambdas[i + 1] * sq20);
  }

  // (symmetric to the first entries)
  size_t N = mesh->length() - 1;  // makes it easier to read

  sq20 = 1.0 / square(mesh->get_time(N - 2) - mesh->get_time(N));
  sq10 = 1.0 / square(mesh->get_time(N - 1) - mesh->get_time(N));
  sq31 = 1.0 / square(mesh->get_time(N - 3) - mesh->get_time(N - 1));

  matrix.set(N, N - 0, lambdas[N - 1] * sq20 + lambdas[N] * sq10);
  matrix.set(N - 1, N - 1, lambdas[N - 2] * sq31 + lambdas[N] * sq10);
  matrix.set(N, N - 1, -lambdas[N] * sq10);
  matrix.set(N - 1, N, -lambdas[N] * sq10);

  matrix *= alpha_;

  // L2 part (+ trapezoidal rule)
  for (size_t i = 0; i < mesh->length(); i++)
    matrix.add(i, i, lambdas[i]);

  umfpack.factorize(matrix);
}

template <int dim>
void H1H1<dim>::dot_solve_mass_and_transform(DiscretizedFunction<dim>& u) {
  // X = (T + \alpha_ D^t T D) * (M+gamma_ L),
  // M = blocks of mass matrices, D = derivative, T = trapezoidal rule, L = blocks of laplace matrices
  u.solve_mass();
  dot_transform(u);
}

template <int dim>
void H1H1<dim>::dot_mult_mass_and_transform_inverse(DiscretizedFunction<dim>& u) {
  u.mult_mass();
  dot_transform_inverse(u);
}

template <int dim>
std::string H1H1<dim>::name() const {
  return "H¹([0,T], H¹(Ω))";
}

template <int dim>
std::string H1H1<dim>::unique_id() const {
  return "H¹([0,T], H¹(Ω)) with ɣ=" + std::to_string(gamma_) + ", α=" + std::to_string(alpha_);
}

template class H1H1<1>;
template class H1H1<2>;
template class H1H1<3>;

} /* namespace norms */
} /* namespace wavepi */


#include "calibrate.hpp"

#include "parse.hpp"

#include <Eigen/SVD>

#include <iostream>
#include <stdexcept>

// Assuming that the photos have been distributed roughly evenly around the
// circle and that the rotation axis was close to the global vertical axis.
// The returned vector is the camera space +y vector in IMU coordinates. y is
// down on the image.
Eigen::Vector3f get_y_axis(std::vector<Eigen::Vector3f> const& photo_gravity) {
	Eigen::Vector3f vertical_axis = Eigen::Vector3f::Zero();

	for (auto const& g : photo_gravity)
		vertical_axis += g;

	return vertical_axis.normalized();
}

// Assumes that the camera x axis was roughly orthogonal to the world vertcal
// axis and the photos were taken while rotating exactly around the camera x
// axis.
Eigen::Vector3f get_x_axis(std::vector<Eigen::Vector3f> const& photo_gravity) {
	// Simple linear regression
	const int n = photo_gravity.size();

	assert(n >= 4);

	Eigen::MatrixXf m(n, 4);
	for (int i = 0; i < n; ++i) {
		m.block<1, 3>(i, 0) = photo_gravity[i].transpose().normalized();
	}

	m.col(3) = Eigen::VectorXf::Ones(n);

	// We find a plane through the data. The first three components are
	// the normal of the plane, the last component is the offset.
	Eigen::Vector4f w = m.bdcSvd(Eigen::ComputeFullV).matrixV().col(3);
	w /= w.head<3>().norm();

	Eigen::Vector3f normal = w.head<3>();

	// Here we make sure that the rotation axis was close to orthogonal
	// to gravity. The offset of the plane is equal the sin of the
	// angle between the rotation axis and a perfectly horizontal line.
	const float sin_angle = w(3);
	assert(std::abs(sin_angle) < 0.2);

	// We check if the camera was rotated in positive or negative
	// direction around `normal` by checking how the first and last
	// vector split up the circle into sectors and seeing into which
	// of the sectors the middle recording falls.

	std::array<Eigen::Vector3f, 3> vecs_on_plane = {
	    m.block<1, 3>(0, 0), m.block<1, 3>(n / 2, 0), m.block<1, 3>(n - 1, 0)};

	for (auto& v : vecs_on_plane)
		v -= v.dot(normal) * normal;

	const Eigen::Vector3f centre_of_positive_sector =
	    normal.cross(vecs_on_plane[0]) - normal.cross(vecs_on_plane[2]);

	if (centre_of_positive_sector.dot(vecs_on_plane[1]) <
	    centre_of_positive_sector.dot(vecs_on_plane[0]))
		normal *= -1;

	return normal;
}

// Returns the rotation from IMU frame to Camera frame.
Eigen::Quaternionf calibrate(std::ifstream& recording_infile,
                             int x_session_index, int y_session_index,
                             int nr_skip_measurements) {
	int session_index = -1;

	bool x_calibrated = false;
	bool y_calibrated = false;

	if (x_session_index == y_session_index) {
		throw std::invalid_argument(
		    "The indices for calibrating the x and y axes can't be equal.");
	}

	std::vector<Eigen::Vector3f> measurements;
	measurements.reserve(30);

	std::vector<Eigen::Vector3f> photo_gravities;

	Eigen::Vector3f x_axis;
	Eigen::Vector3f y_axis;

	const auto run_calibrations = [&]() {
		if (session_index == x_session_index) {
			x_axis = get_x_axis(photo_gravities);
			x_calibrated = true;
		} else if (session_index == y_session_index) {
			y_axis = get_y_axis(photo_gravities);
			y_calibrated = true;
		}
	};

	log_entry_types entry_type;
	while (read_next_entry(recording_infile, entry_type)) {
		switch (entry_type) {
		case log_entry_types::photo_event:
			parse_photo(recording_infile).millis;

			if (session_index >= 0 && (session_index == x_session_index ||
			                           session_index == y_session_index)) {
				Eigen::Vector3f g = Eigen::Vector3f::Zero();
				int nr_relevant_measurements =
				    measurements.size() - nr_skip_measurements;
				for (int i = 0; i < nr_relevant_measurements; ++i)
					g += measurements[i];
				g /= nr_relevant_measurements;

				photo_gravities.push_back(g);
			}

			measurements.clear();
			break;
		case log_entry_types::startup:

			if (session_index >= 0)
				run_calibrations();

			parse_startup(recording_infile);
			photo_gravities.clear();
			measurements.clear();
			++session_index;
			break;
		case log_entry_types::gravity_vector:
			measurements.push_back(parse_gravity(recording_infile).g);
			break;
		}
	}

	if (session_index >= 0)
		run_calibrations();

	// If only one of the axes was calibrated we still want to print the
	// result, this is useful for testing.
	if (x_calibrated)
		std::cout << "Sensor x axis in IMU coordinates: " << x_axis.transpose()
		          << '\n';
	if (y_calibrated)
		std::cout << "Sensor y axis in IMU coordinates: " << y_axis.transpose()
		          << '\n';

	if (x_calibrated && y_calibrated) {
		// running a complete calibration

		Eigen::Matrix3f rotation_matrix;
		rotation_matrix << x_axis, y_axis, x_axis.cross(y_axis);

		// Invert the rotation because we want to get the conversion from IMU
		// frame to camera.
		return Eigen::Quaternionf(rotation_matrix).conjugate();
	} else {
		if (x_session_index >= session_index)
			throw std::runtime_error("Invalid x session index.");
		if (y_session_index >= session_index)
			throw std::runtime_error("Invalid y session index.");
	}

	throw std::runtime_error("Need both x and y for complete calibration.");
}


/**
 * @file rrts02-nearestNeighbors.cpp
 * @author Can Erdogan
 * @date Feb 04, 2013
 * @brief Checks if the nearest neighbor computation done by flann is correct.
 */

#include <iostream>
#include <gtest/gtest.h>
#include <Eigen/Core>
#include <dart/dart.hpp>
#if HAVE_FLANN
#include <flann/flann.hpp>
#endif // HAVE_FLANN
#include "TestHelpers.hpp"

/* ********************************************************************************************* */
#if HAVE_FLANN
TEST(NEAREST_NEIGHBOR, 2D) {

    // Build the index with the first node
    flann::Index<flann::L2<double> > index (flann::KDTreeSingleIndexParams(10, true));
    Eigen::VectorXd p1 (2);
    p1 << -3.04159, -3.04159;
    index.buildIndex(flann::Matrix<double>((double*)p1.data(), 1, p1.size()));

    // Add two more points
    Eigen::Vector2d p2 (-2.96751, -2.97443), p3 (-2.91946, -2.88672);
    index.addPoints(flann::Matrix<double>((double*)p2.data(), 1, p2.size()));
    index.addPoints(flann::Matrix<double>((double*)p3.data(), 1, p3.size()));

    // Check the size of the tree
    EXPECT_EQ(3, (int)index.size());

    // Get the nearest neighbor index for a sample point
    Eigen::Vector2d sample (-2.26654, 2.2874);
    int nearest;
    double distance;
    const flann::Matrix<double> queryMatrix((double*)sample.data(), 1, sample.size());
    flann::Matrix<int> nearestMatrix(&nearest, 1, 1);
    flann::Matrix<double> distanceMatrix(flann::Matrix<double>(&distance, 1, 1));
    index.knnSearch(queryMatrix, nearestMatrix, distanceMatrix, 1,
        flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
    EXPECT_EQ(2, nearest);

    // Get the nearest neighbor
    double* point = index.getPoint(nearest);
    bool equality = equals(Vector2d(point[0], point[1]), p3, 1e-3);
    EXPECT_TRUE(equality);
}
#endif // HAVE_FLANN

/* ********************************************************************************************* */
int main(int argc, char* argv[]) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/*
 [auto_generated]
 libs/numeric/odeint/test/trivial_state.cpp

 [begin_description]
 This file defines a vector_space 1d class with the appropriate operators.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef VECTOR_SPACE_1D_HPP_INCLUDED
#define VECTOR_SPACE_1D_HPP_INCLUDED

#include <boost/config.hpp>
#include <boost/operators.hpp>

template< class T >
struct vector_space_1d :
    boost::additive1< vector_space_1d< T > ,
    boost::additive2< vector_space_1d< T > , T ,
    boost::multiplicative1< vector_space_1d< T > ,
    boost::multiplicative2< vector_space_1d< T > , T
    > > > >
{
    typedef T value_type;

    value_type m_x;

    vector_space_1d( void ) : m_x( 0.0 ) {}

    vector_space_1d& operator+=( const vector_space_1d& p )
    {
        m_x += p.m_x;
        return *this;
    }

    vector_space_1d& operator-=( const vector_space_1d& p )
    {
        m_x -= p.m_x;
        return *this;
    }

    vector_space_1d& operator*=( const vector_space_1d& p )
    {
        m_x *= p.m_x;
        return *this;
    }

    vector_space_1d& operator/=( const vector_space_1d& p )
    {
        m_x /= p.m_x;
        return *this;
    }

    vector_space_1d& operator+=( const value_type& val )
    {
        m_x += val;
        return *this;
    }

    vector_space_1d& operator-=( const value_type& val )
    {
        m_x -= val;
        return *this;
    }

    vector_space_1d& operator*=( const value_type &val )
    {
        m_x *= val;
        return *this;
    }

    vector_space_1d& operator/=( const value_type &val )
    {
        m_x /= val;
        return *this;
    }
};

template< class T >
vector_space_1d< T > abs( const vector_space_1d< T > &v)
{
    vector_space_1d< T > tmp;
    tmp.m_x = std::abs( v.m_x );
    return tmp;
}

template< class T >
T max BOOST_PREVENT_MACRO_SUBSTITUTION ( const vector_space_1d< T > &v )
{
    return v.m_x;
}

namespace boost {
    namespace numeric {
        namespace odeint {

            template< class T >
            struct vector_space_reduce< vector_space_1d< T > >
            {
                template< class Op >
                T operator()( const vector_space_1d< T > &v , Op op , T value )
                {
                    return v.m_x;
                }
            };

} // odeint
} // numeric
} // boost

#endif // VECTOR_SPACE_1D_HPP_INCLUDED


#include <boost/test/unit_test.hpp>
#include <shz/math/vector.hpp>

/*
	VS2012 Compiler Bug makes impossible to use auto keyword for memory aligned structures.
	See: http://connect.microsoft.com/VisualStudio/feedback/details/775238/visual-c-2012-auto-keyword-ignores-structure-memory-alignment
	Workaround: either declare the variable with the 16bytes alignment or don't use the auto keyword
*/

bool is_aligned(void *p, intptr_t N)
{
	return (intptr_t)p % N == 0;
}

BOOST_AUTO_TEST_CASE(vector4fConstructors)
{
	shz::math::vector<shz::math::f32, 4> v(0.f);
	BOOST_CHECK(is_aligned(&v, 16));
	BOOST_CHECK(v.x == 0.f);
	BOOST_CHECK(v.y  == 0.f);
	BOOST_CHECK(v.z  == 0.f);
	BOOST_CHECK(v.w  == 0.f);

	v = shz::math::vector<shz::math::f32, 4>(1.f);
	BOOST_CHECK(v.data[0]  == 1.f);
	BOOST_CHECK(v.data[1]  == 1.f);

	v = shz::math::vector<shz::math::f32, 4>(1.4f);
	v -= 0.4f;
	BOOST_CHECK(v.data[0]  == 1.f);
	BOOST_CHECK(v.data[1]  == 1.f);
	BOOST_CHECK(v.z  == 1.f);
	BOOST_CHECK(v.w  == 1.f);
}

BOOST_AUTO_TEST_CASE(vector4fAdds)
{
	shz::math::vector<shz::math::f32, 4> v(1.4f);
	BOOST_CHECK(is_aligned(&v, 16));
	v += 0.4f;
	BOOST_CHECK(v.x  == 1.8f);
	BOOST_CHECK(v.y  == 1.8f);
	BOOST_CHECK(v.z  == 1.8f);
	BOOST_CHECK(v.w  == 1.8f);

	shz::math::vector<shz::math::f32, 4> w(-1.4f);
	v += w;
	BOOST_CHECK_CLOSE(v.data[0], .4f,  0.00001f);
	BOOST_CHECK_CLOSE(v.data[1], .4f,  0.00001f);
}

BOOST_AUTO_TEST_CASE(vector4fSubs)
{
	shz::math::vector<shz::math::f32, 4> v(1.4f);
	v -= 0.4f;
	BOOST_CHECK(v.data[0]  == 1.f);
	BOOST_CHECK(v.data[1]  == 1.f);

	shz::math::vector<shz::math::f32, 4> w(-1.4f);
	v -= w;
	BOOST_CHECK_CLOSE(v.data[0], 2.4f,  0.00001f);
	BOOST_CHECK_CLOSE(v.data[1], 2.4f,  0.00001f);
}

BOOST_AUTO_TEST_CASE(vector4fMuls)
{
	shz::math::vector<shz::math::f32, 4> v(1.4f);
	v *= 0.5f;
	BOOST_CHECK(v.data[0]  == 0.7f);
	BOOST_CHECK(v.data[1]  == 0.7f);

	shz::math::vector<shz::math::f32, 4> w(-1.4f);
	v *= w;
	BOOST_CHECK_CLOSE(v.data[0], -0.98f,  0.00001f);
	BOOST_CHECK_CLOSE(v.data[1], -0.98f,  0.00001f);
}

BOOST_AUTO_TEST_CASE(vector4fDivs)
{
	shz::math::vector<shz::math::f32, 4> v(1.4f);
	v /= 0.5f;
	BOOST_CHECK(v.data[0]  == 2.8f);
	BOOST_CHECK(v.data[1]  == 2.8f);

	shz::math::vector<shz::math::f32, 4> w(2.f);
	v /= w;
	BOOST_CHECK_CLOSE(v.data[0], 1.4f,  0.00001f);
	BOOST_CHECK_CLOSE(v.data[1], 1.4f,  0.00001f);
}

BOOST_AUTO_TEST_CASE(vector4fDot)
{
	shz::math::vector<shz::math::f32, 4> v(1.4f);
	shz::math::vector<shz::math::f32, 4> w(0.5f);

	auto value = v.dot(w);
	BOOST_CHECK_CLOSE(value, 2.8f, 0.00001f);
}

BOOST_AUTO_TEST_CASE(vector4fLength)
{
	shz::math::vector<shz::math::f32, 4> v(1.4f);
	BOOST_CHECK_CLOSE(v.length(), 2.8f, 0.01f);
	BOOST_CHECK_CLOSE(v.sqrlength(), 7.84f, 0.00001f);
}

BOOST_AUTO_TEST_CASE(vector4fInverseLength)
{
	shz::math::vector<shz::math::f32, 4> v(1.4f);
	BOOST_CHECK_CLOSE(v.inv_length(), 1.f/2.8f, 0.01f);
	v = shz::math::vector<shz::math::f32, 4>(0.f);
	BOOST_CHECK_CLOSE(v.inv_length(), 0.f, 0.0f);
}

BOOST_AUTO_TEST_CASE(vector4fNormalize)
{
	shz::math::vector<shz::math::f32, 4> v(1.4f);
	shz::math::vector<shz::math::f32, 4> w = v.normalize();
	BOOST_CHECK_CLOSE(w.data[0], 0.5f,  0.01f);
	BOOST_CHECK_CLOSE(w.data[1], 0.5f,  0.01f);

	v = shz::math::vector<shz::math::f32, 4>(.0f);
	w = v.normalize();
	BOOST_CHECK_CLOSE(w.data[0], 0.f,  0.001f);
	BOOST_CHECK_CLOSE(w.data[1], 0.f,  0.001f);
	BOOST_CHECK_CLOSE(w.data[2], 0.f,  0.001f);
}


/**
 * @file emst_test.cpp
 *
 * Test file for EMST methods.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/emst/dtb.hpp>
#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

#include <mlpack/core/tree/cover_tree.hpp>

using namespace mlpack;
using namespace mlpack::emst;
using namespace mlpack::tree;
using namespace mlpack::bound;
using namespace mlpack::metric;

BOOST_AUTO_TEST_SUITE(EMSTTest);

/**
 * Simple emst test with small, synthetic dataset.  This is an
 * exhaustive test, which checks that each method for performing the calculation
 * (dual-tree, naive) produces the correct results.  The dataset is in one
 * dimension for simplicity -- the correct functionality of distance functions
 * is not tested here.
 */
BOOST_AUTO_TEST_CASE(ExhaustiveSyntheticTest)
{
  // Set up our data.
  arma::mat data(1, 11);
  data[0] = 0.05; // Row addressing is unnecessary (they are all 0).
  data[1] = 0.37;
  data[2] = 0.15;
  data[3] = 1.25;
  data[4] = 5.05;
  data[5] = -0.22;
  data[6] = -2.00;
  data[7] = -1.30;
  data[8] = 0.45;
  data[9] = 0.91;
  data[10] = 1.00;

  arma::mat results;

  // Build the tree by hand to get a leaf size of 1.
  typedef KDTree<EuclideanDistance, DTBStat, arma::mat> TreeType;
  std::vector<size_t> oldFromNew;
  std::vector<size_t> newFromOld;
  TreeType tree(data, oldFromNew, newFromOld, 1);

  // Create the DTB object and run the calculation.
  DualTreeBoruvka<> dtb(&tree);
  dtb.ComputeMST(results);

  // Now the exhaustive check for correctness.
  if (newFromOld[1] < newFromOld[8])
  {
    BOOST_REQUIRE_EQUAL(results(0, 0), newFromOld[1]);
    BOOST_REQUIRE_EQUAL(results(1, 0), newFromOld[8]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 0), newFromOld[1]);
    BOOST_REQUIRE_EQUAL(results(0, 0), newFromOld[8]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 0), 0.08, 1e-5);

  if (newFromOld[9] < newFromOld[10])
  {
    BOOST_REQUIRE_EQUAL(results(0, 1), newFromOld[9]);
    BOOST_REQUIRE_EQUAL(results(1, 1), newFromOld[10]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 1), newFromOld[9]);
    BOOST_REQUIRE_EQUAL(results(0, 1), newFromOld[10]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 1), 0.09, 1e-5);

  if (newFromOld[0] < newFromOld[2])
  {
    BOOST_REQUIRE_EQUAL(results(0, 2), newFromOld[0]);
    BOOST_REQUIRE_EQUAL(results(1, 2), newFromOld[2]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 2), newFromOld[0]);
    BOOST_REQUIRE_EQUAL(results(0, 2), newFromOld[2]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 2), 0.1, 1e-5);

  if (newFromOld[1] < newFromOld[2])
  {
    BOOST_REQUIRE_EQUAL(results(0, 3), newFromOld[1]);
    BOOST_REQUIRE_EQUAL(results(1, 3), newFromOld[2]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 3), newFromOld[1]);
    BOOST_REQUIRE_EQUAL(results(0, 3), newFromOld[2]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 3), 0.22, 1e-5);

  if (newFromOld[3] < newFromOld[10])
  {
    BOOST_REQUIRE_EQUAL(results(0, 4), newFromOld[3]);
    BOOST_REQUIRE_EQUAL(results(1, 4), newFromOld[10]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 4), newFromOld[3]);
    BOOST_REQUIRE_EQUAL(results(0, 4), newFromOld[10]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 4), 0.25, 1e-5);

  if (newFromOld[0] < newFromOld[5])
  {
    BOOST_REQUIRE_EQUAL(results(0, 5), newFromOld[0]);
    BOOST_REQUIRE_EQUAL(results(1, 5), newFromOld[5]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 5), newFromOld[0]);
    BOOST_REQUIRE_EQUAL(results(0, 5), newFromOld[5]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 5), 0.27, 1e-5);

  if (newFromOld[8] < newFromOld[9])
  {
    BOOST_REQUIRE_EQUAL(results(0, 6), newFromOld[8]);
    BOOST_REQUIRE_EQUAL(results(1, 6), newFromOld[9]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 6), newFromOld[8]);
    BOOST_REQUIRE_EQUAL(results(0, 6), newFromOld[9]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 6), 0.46, 1e-5);

  if (newFromOld[6] < newFromOld[7])
  {
    BOOST_REQUIRE_EQUAL(results(0, 7), newFromOld[6]);
    BOOST_REQUIRE_EQUAL(results(1, 7), newFromOld[7]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 7), newFromOld[6]);
    BOOST_REQUIRE_EQUAL(results(0, 7), newFromOld[7]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 7), 0.7, 1e-5);

  if (newFromOld[5] < newFromOld[7])
  {
    BOOST_REQUIRE_EQUAL(results(0, 8), newFromOld[5]);
    BOOST_REQUIRE_EQUAL(results(1, 8), newFromOld[7]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 8), newFromOld[5]);
    BOOST_REQUIRE_EQUAL(results(0, 8), newFromOld[7]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 8), 1.08, 1e-5);

  if (newFromOld[3] < newFromOld[4])
  {
    BOOST_REQUIRE_EQUAL(results(0, 9), newFromOld[3]);
    BOOST_REQUIRE_EQUAL(results(1, 9), newFromOld[4]);
  }
  else
  {
    BOOST_REQUIRE_EQUAL(results(1, 9), newFromOld[3]);
    BOOST_REQUIRE_EQUAL(results(0, 9), newFromOld[4]);
  }
  BOOST_REQUIRE_CLOSE(results(2, 9), 3.8, 1e-5);
}

/**
 * Test the dual tree method against the naive computation.
 *
 * Errors are produced if the results are not identical.
 */
BOOST_AUTO_TEST_CASE(DualTreeVsNaive)
{
  arma::mat inputData;

  // Hard-coded filename: bad!
  // Code duplication: also bad!
  if (!data::Load("test_data_3_1000.csv", inputData))
    BOOST_FAIL("Cannot load test dataset test_data_3_1000.csv!");

  // Set up matrices to work with.
  arma::mat dualData = inputData;
  arma::mat naiveData = inputData;

  // Reset parameters from last test.
  DualTreeBoruvka<> dtb(dualData);

  arma::mat dualResults;
  dtb.ComputeMST(dualResults);

  // Set naive mode.
  DualTreeBoruvka<> dtbNaive(naiveData, true);

  arma::mat naiveResults;
  dtbNaive.ComputeMST(naiveResults);

  BOOST_REQUIRE_EQUAL(dualResults.n_cols, naiveResults.n_cols);
  BOOST_REQUIRE_EQUAL(dualResults.n_rows, naiveResults.n_rows);

  for (size_t i = 0; i < dualResults.n_cols; i++)
  {
    BOOST_REQUIRE_EQUAL(dualResults(0, i), naiveResults(0, i));
    BOOST_REQUIRE_EQUAL(dualResults(1, i), naiveResults(1, i));
    BOOST_REQUIRE_CLOSE(dualResults(2, i), naiveResults(2, i), 1e-5);
  }
}

/**
 * Make sure the cover tree works fine.
 */
BOOST_AUTO_TEST_CASE(CoverTreeTest)
{
  arma::mat inputData;
  if (!data::Load("test_data_3_1000.csv", inputData))
    BOOST_FAIL("Cannot load test dataset test_data_3_1000.csv!");

  DualTreeBoruvka<> bst(inputData);
  DualTreeBoruvka<EuclideanDistance, arma::mat, StandardCoverTree>
      ct(inputData);

  arma::mat bstResults;
  arma::mat coverResults;

  // Run the algorithms.
  bst.ComputeMST(bstResults);
  ct.ComputeMST(coverResults);

  for (size_t i = 0; i < bstResults.n_cols; i++)
  {
    BOOST_REQUIRE_EQUAL(bstResults(0, i), coverResults(0, i));
    BOOST_REQUIRE_EQUAL(bstResults(1, i), coverResults(1, i));
    BOOST_REQUIRE_CLOSE(bstResults(2, i), coverResults(2, i), 1e-5);
  }

}

/**
 * Test BinarySpaceTree with Ball Bound.
 */
BOOST_AUTO_TEST_CASE(BallTreeTest)
{
  arma::mat inputData;
  if (!data::Load("test_data_3_1000.csv", inputData))
    BOOST_FAIL("Cannot load test dataset test_data_3_1000.csv!");

  // naive mode.
  DualTreeBoruvka<> bst(inputData, true);
  // Ball tree.
  DualTreeBoruvka<EuclideanDistance, arma::mat, BallTree> ballt(inputData);

  arma::mat bstResults;
  arma::mat ballResults;

  // Run the algorithms.
  bst.ComputeMST(bstResults);
  ballt.ComputeMST(ballResults);

  for (size_t i = 0; i < bstResults.n_cols; i++)
  {
    BOOST_REQUIRE_EQUAL(bstResults(0, i), ballResults(0, i));
    BOOST_REQUIRE_EQUAL(bstResults(1, i), ballResults(1, i));
    BOOST_REQUIRE_CLOSE(bstResults(2, i), ballResults(2, i), 1e-5);
  }

}

BOOST_AUTO_TEST_SUITE_END();


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

#include <boost/simd/arithmetic/functions/divround.hpp>
#include <boost/simd/include/functions/scalar/round.hpp>
#include <boost/simd/include/functions/scalar/copysign.hpp>
#include <boost/simd/include/functions/scalar/is_odd.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( divround_, tag::cpu_
                                    , (A0)
                                    , (scalar_< int64_<A0> >)
                                      (scalar_< int64_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (!a0) return  Zero<result_type>();
      if(a1)
      {
        A0 aa0 = abs(a0);
        A0 aa1 = abs(a1);
        result_type q = aa0/aa1;
        result_type r = aa0-q*aa1;
        if ((r!= 0) && (r-is_odd(aa1) >= (aa1 >> 1))) ++q;
        return copysign(q, a0^a1);
      }
      else
        return ((a0>0) ? Valmax<result_type>() : Valmin<result_type>());
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( divround_, tag::cpu_
                                    , (A0)
                                    , (scalar_< signed_<A0> >)
                                      (scalar_< signed_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if(a1)
        return static_cast<result_type >(round(static_cast<double>(a0)/static_cast<double>(a1)));
      else
      {
        return (a0) ? ((a0>0) ? Valmax<result_type>() : Valmin<result_type>()) : Zero<result_type>();
      }
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( divround_, tag::cpu_
                                    , (A0)
                                    , (scalar_< unsigned_<A0> >)
                                      (scalar_< unsigned_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if(a1)
      {
        result_type q = a0/a1;
        result_type r = a0-q*a1;
        if ((r!= 0) && (r-is_odd(a1) >= (a1 >> 1))) ++q;
        return q;
      }
      else
        return (a0) ? Valmax<result_type>() : Zero<result_type>();
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif
  BOOST_DISPATCH_IMPLEMENT          ( divround_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)
                                      (scalar_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::simd::round(a0/a1);
    }
  };
} } }

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif

#endif


#pragma once

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

#include "flux_worker.hpp"

namespace boltzmann {
namespace impl {

/**
 * @brief \f$\langle< \mathbf{v} \dot \mathbf{n} \phi, f \rangle\f$
 *
 * velocity part (in Lagrange basis)
 *
 * upper hemisphere: outflow
 * lower hemisphere: inflow
 *
 */
class IdentityBD : public flux_worker
{
  using flux_worker::vec_t;
  using flux_worker::mat_t;

 public:
  IdentityBD(const vec_t& hermite_weights, const vec_t& hermite_nodes)
      : w_(hermite_weights)
      , x_(hermite_nodes)
  {
  }

  /**
   *
   *
   * @param out Ordering: out(i,j) = f(x_i, y_j)
   * \remark{Also see documentation of @ref H2N.}
   * @param in
   */
  virtual void apply(mat_t& out, const mat_t& in, const dealii::Point<2>& dummy) const;

 private:
  const vec_t w_;
  const vec_t x_;
};

void
IdentityBD::apply(mat_t& out, const mat_t& in, const dealii::Point<2>& dummy) const
{
  AssertDimension(in.rows(), out.rows());
  AssertDimension(in.cols(), out.cols());
  AssertDimension(in.cols(), w_.size());

  int N = w_.size();

  out = in;

  // multiply with weights
  for (int j = 0; j < N; ++j) {
    for (int i = 0; i < N; ++i) {
      out(i, j) *= x_[i];
    }
  }
}

}  // end namespace impl
}  // end namespace boltzmann


/* multiprecision_float_test.cpp
*
* Copyright John Maddock 2015
* Distributed under the Boost Software License, Version 1.0. (See
* accompanying file LICENSE_1_0.txt or copy at
* http://www.boost.org/LICENSE_1_0.txt)
*
* $Id$
*
* Tests all floating point related generators and distributions with multiprecision types.
*/

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/core/ignore_unused.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/debug_adaptor.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/random.hpp>
#include <sstream>

typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float_100::backend_type, boost::multiprecision::et_on > big_float;
typedef boost::random::subtract_with_carry_01_engine<big_float, 48, 10, 24 > ranlux_big_base_01;
typedef boost::random::independent_bits_engine<boost::random::mt19937, 1024, boost::multiprecision::uint1024_t> large_int_generator;

typedef boost::mpl::list <
   boost::random::lagged_fibonacci_01_engine<big_float, 48, 44497, 21034 >,
   boost::random::discard_block_engine< ranlux_big_base_01, 389, 24 >
> engines;

BOOST_AUTO_TEST_CASE_TEMPLATE(generator_test, engine_type, engines)
{
   typedef typename engine_type::result_type test_type;

   boost::scoped_ptr<engine_type> gen(new engine_type());
   unsigned seeds[] = { 1, 2, 3, 4 };
   unsigned *p1 = seeds, *p2 = seeds + 4;
   BOOST_CHECK_THROW(gen->seed(p1, p2), std::invalid_argument);
   gen->seed();
   gen->seed(2);
   test_type a = gen->min();
   test_type b = gen->max();
   BOOST_CHECK(a < b);
   for(unsigned i = 0; i < 200; ++i)
   {
      test_type r = (*gen)();
      BOOST_CHECK((boost::math::isfinite)(r));
      BOOST_CHECK(a <= r);
      BOOST_CHECK(b >= r);
   }
   gen->discard(20);

   std::stringstream ss;
   ss << std::setprecision(std::numeric_limits<test_type>::digits10 + 3) << *gen;
   boost::scoped_ptr<engine_type> gen2(new engine_type());
   ss >> *gen2;
   BOOST_CHECK(*gen == *gen2);
   (*gen2)();
   BOOST_CHECK(*gen != *gen2);
}

typedef boost::mpl::list <
   boost::random::bernoulli_distribution<big_float>,
   boost::random::beta_distribution<big_float>,
   boost::random::cauchy_distribution<big_float>,
   boost::random::chi_squared_distribution<big_float>,
   boost::random::exponential_distribution<big_float>,
   boost::random::extreme_value_distribution<big_float>,
   boost::random::fisher_f_distribution<big_float>,
   boost::random::gamma_distribution<big_float>,
   boost::random::laplace_distribution<big_float>,
   boost::random::lognormal_distribution<big_float>,
   boost::random::normal_distribution<big_float>,
#ifndef BOOST_NO_CXX11_EXPLICIT_CONVERSION_OPERATORS
   boost::random::piecewise_constant_distribution<big_float>,
   boost::random::piecewise_linear_distribution<big_float>,
#endif
   boost::random::student_t_distribution<big_float>,
   boost::random::triangle_distribution<big_float>,
   //boost::random::uniform_01<big_float>,  // doesn't respect the concept!  But gets used internally anyway.
   boost::random::uniform_real_distribution<big_float>,
   boost::random::uniform_on_sphere<big_float>,
   boost::random::weibull_distribution<big_float>
> distributions;

BOOST_AUTO_TEST_CASE_TEMPLATE(distributions_test, dist_type, distributions)
{
   typedef typename dist_type::result_type result_type;
   dist_type d;
   result_type a = (d.min)();
   result_type b = (d.max)();
   typename dist_type::param_type p = d.param();
   boost::ignore_unused(p);
   d.reset();

   std::stringstream ss;
   ss << std::setprecision(std::numeric_limits<result_type>::digits10 + 3) << d;
   dist_type d2;
   ss >> d2;
   BOOST_CHECK(d == d2);

   boost::random::mt19937 int_gen;

   for(unsigned i = 0; i < 200; ++i)
   {
      result_type r = d(int_gen);
      BOOST_CHECK((boost::math::isfinite)(r));
      BOOST_CHECK(r >= a);
      BOOST_CHECK(r <= b);
   }

#ifndef BOOST_NO_CXX11_EXPLICIT_CONVERSION_OPERATORS
   large_int_generator big_int_gen;

   for(unsigned i = 0; i < 200; ++i)
   {
      result_type r = d(big_int_gen);
      BOOST_CHECK((boost::math::isfinite)(r));
      BOOST_CHECK(r >= a);
      BOOST_CHECK(r <= b);
   }

   boost::random::discard_block_engine< ranlux_big_base_01, 389, 24 > big_float_gen;

   for(unsigned i = 0; i < 200; ++i)
   {
      result_type r = d(big_float_gen);
      BOOST_CHECK((boost::math::isfinite)(r));
      BOOST_CHECK(r >= a);
      BOOST_CHECK(r <= b);
   }
#endif

   boost::random::ranlux64_4_01 float_gen;

   for(unsigned i = 0; i < 200; ++i)
   {
      result_type r = d(float_gen);
      BOOST_CHECK((boost::math::isfinite)(r));
      BOOST_CHECK(r >= a);
      BOOST_CHECK(r <= b);
   }
}

BOOST_AUTO_TEST_CASE(canonical_test)
{
   typedef big_float result_type;

   boost::random::mt19937 int_gen;

   for(unsigned i = 0; i < 200; ++i)
   {
      result_type r = boost::random::generate_canonical<big_float, std::numeric_limits<big_float>::digits>(int_gen);
      BOOST_CHECK((boost::math::isfinite)(r));
      BOOST_CHECK(r >= 0);
      BOOST_CHECK(r <= 1);
   }

   large_int_generator big_int_gen;

   for(unsigned i = 0; i < 200; ++i)
   {
      result_type r = boost::random::generate_canonical<big_float, std::numeric_limits<big_float>::digits>(big_int_gen);
      BOOST_CHECK((boost::math::isfinite)(r));
      BOOST_CHECK(r >= 0);
      BOOST_CHECK(r <= 1);
   }

   boost::random::discard_block_engine< ranlux_big_base_01, 389, 24 > big_float_gen;

   for(unsigned i = 0; i < 200; ++i)
   {
      result_type r = boost::random::generate_canonical<big_float, std::numeric_limits<big_float>::digits>(big_float_gen);
      BOOST_CHECK((boost::math::isfinite)(r));
      BOOST_CHECK(r >= 0);
      BOOST_CHECK(r <= 1);
   }

   boost::random::ranlux64_4_01 float_gen;

   for(unsigned i = 0; i < 200; ++i)
   {
      result_type r = boost::random::generate_canonical<big_float, std::numeric_limits<big_float>::digits>(float_gen);
      BOOST_CHECK((boost::math::isfinite)(r));
      BOOST_CHECK(r >= 0);
      BOOST_CHECK(r <= 1);
   }

}


#include "vds.h"

#include <math.h>
#include <stdio.h>
#include <boost/math/constants/constants.hpp>
#include "vector_td_utilities.h"
#include <boost/range/combine.hpp>
#include <boost/range/algorithm.hpp>
#include "hoNDArray_math.h"

constexpr double GAMMA = 4258.0;        /* Hz/G */
constexpr double PI = boost::math::constants::pi<double>();

/* #define TESTCODE 	For testing as regular C code... */

/*
  %
  %	VARIABLE DENSITY SPIRAL GENERATION:
  %	----------------------------------
  %
  %	This is a general description of how the following C code
  %	works.  This text is taken from a matlab script, vds.m, from
  %	which the C code was derived.  However, note that the C code
  %	runs considerably faster.
  %
  %
  %	Function generates variable density spiral which traces
  %	out the trajectory
  %
  %			k(t) = r(t) exp(i*q(t)), 		[1]
  %
  %	Where q IS THE SAME AS theta, and r IS THE SAME AS kr.
  %
  %		r and q are chosen to satisfy:
  %
  %		1) Maximum gradient amplitudes and slew rates.
  %		2) Maximum gradient due to FOV, where FOV can
  %		   vary with k-space radius r, as
  %
  %			FOV(r) = F0 + F1*r + F2*r*r 		[2]
  %
  %
  %	INPUTS:
  %	-------
  %	smax = maximum slew rate G/cm/s
  %	gmax = maximum gradient G/cm (limited by Gmax or FOV)
  %	T = sampling period (s) for gradient AND acquisition.
  %	N = number of interleaves.
  %	F0,F1,F2 = FOV coefficients with respect to r - see above.
  %	rmax= value of k-space radius at which to stop (cm^-1).
  %		rmax = 1/(2*resolution);
  %
  %
  %	OUTPUTS:
  %	--------
  %	k = k-space trajectory (kx+iky) in cm-1.
  %	g = gradient waveform (Gx+iGy) in G/cm.
  %	s = derivative of g (Sx+iSy) in G/cm/s.
  %	time = time points corresponding to above (s).
  %	r = k-space radius vs time (used to design spiral)
  %	theta = atan2(ky,kx) = k-space angle vs time.
  %
  %
  %	METHODS:
  %	--------
  %	Let r1 and r2 be the first derivatives of r in [1].
  %	Let q1 and q2 be the first derivatives of theta in [1].
  %	Also, r0 = r, and q0 = theta - sometimes both are used.
  %	F = F(r) defined by F0,F1,F2.
  %
  %	Differentiating [1], we can get G = a(r0,r1,q0,q1,F)
  %	and differentiating again, we get S = b(r0,r1,r2,q0,q1,q2,F)
  %
  %	(functions a() and b() are reasonably easy to obtain.)
  %
  %	FOV limits put a constraint between r and q:
  %
  %		dr/dq = N/(2*pi*F)				[3]
  %
  %	We can use [3] and the chain rule to give
  %
  %		q1 = 2*pi*F/N * r1				[4]
  %
  %	and
  %
  %		q2 = 2*pi/N*dF/dr*r1^2 + 2*pi*F/N*r2		[5]
  %
  %
  %
  %	Now using [4] and [5], we can substitute for q1 and q2
  %	in functions a() and b(), giving
  %
  %		G = c(r0,r1,F)
  %	and 	S = d(r0,r1,r2,F,dF/dr)
  %
  %
  %	Using the fact that the spiral should be either limited
  %	by amplitude (Gradient or FOV limit) or slew rate, we can
  %	solve
  %		|c(r0,r1,F)| = |Gmax|  				[6]
  %
  %	analytically for r1, or
  %
  %	  	|d(r0,r1,r2,F,dF/dr)| = |Smax|	 		[7]
  %
  %	analytically for r2.
  %
  %	[7] is a quadratic equation in r2.  The smaller of the
  %	roots is taken, and the real part of the root is used to
  %	avoid possible numeric errors - the roots should be real
  %	always.
  %
  %	The choice of whether or not to use [6] or [7], and the
  %	solving for r2 or r1 is done by calcthetadotdot().
  %
  %	Once the second derivative of theta(q) or r is obtained,
  %	it can be integrated to give q1 and r1, and then integrated
  %	again to give q and r.  The gradient waveforms follow from
  %	q and r.
  %
  %	Brian Hargreaves -- Sept 2000.
  %
  %
*/

namespace Gadgetron {

    /* ----------------------------------------------------------------------- */
    void calcthetadotdot(double slewmax, double gradmax, double kr,
                         double krdot, double Tgsample, double Tdsample, int Ninterleaves,
                         double *fov, int numfov, double *thetadotdot, double *krdotdot)
    /*
     * Function calculates the 2nd derivative of kr and theta at each
     * sample point within calc_vds().  ie, this is the iterative loop
     * for calc_vds.  See the text at the top of this file for more details
     * */

    //double slewmax;		/*	Maximum slew rate, G/cm/s		*/
    //double gradmax;		/* 	maximum gradient amplitude, G/cm	*/
    //double kr;		/* 	Current kr. */
    //double krdot;		/*	Current krdot. */
    //double Tgsample;	/*	Gradient Sample period (s) 	*/
    //double Tdsample;	/*	Data Sample period (s) 		*/
    //int Ninterleaves;	/*	Number of interleaves			*/
    //double *fov;		/*	FOV coefficients		*/
    //int numfov;		/*	Number of FOV coefficients		*/
    //double *thetadotdot;	/*	[output] 2nd derivative of theta.	*/
    //double *krdotdot;	/*	[output] 2nd derivative of kr		*/

    /* ----------------------------------------------------------------------- */
    {
        double fovval = 0;    /* FOV for this value of kr	*/
        double dfovdrval = 0;    /* dFOV/dkr for this value of kr	*/
        double gmaxfov;        /* FOV-limited Gmax.	*/
        double maxkrdot;
        int count;

        double tpf;    /* Used to simplify expressions. */
        double tpfsq;    /* 	" 		"        */

        double qdfA, qdfB, qdfC;    /* Quadratic formula coefficients */
        double rootparta, rootpartb;

        /* Calculate the actual FOV and dFOV/dkr for this R,
         * based on the fact that the FOV is expressed
         * as a polynomial in kr.*/

        for (count = 0; count < numfov; count++) {
            fovval = fovval + fov[count] * pow(kr, count);
            if (count > 0)
                dfovdrval = dfovdrval + count * fov[count] * pow(kr, count - 1);
        }

        /* Calculate FOV limit on gmax.  This is the rate of motion along
         * a trajectory, and really should not be a limitation.  Thus,
         * it is reasonable to comment out the following lines. */

        gmaxfov = 1 / GAMMA / fovval / Tdsample;
        if (gradmax > gmaxfov)
            gradmax = gmaxfov;

        /* Maximum dkr/dt, based on gradient amplitude.  */

        maxkrdot = sqrt(pow(GAMMA * gradmax, 2) / (1 + pow(2 * PI * fovval * kr / Ninterleaves, 2)));

        /* These two are just to simplify expressions below */
        tpf = 2 * PI * fovval / Ninterleaves;
        tpfsq = pow(tpf, 2);

        if (krdot > maxkrdot)    /* Then choose krdotdot so that krdot is in range */
        {
            *krdotdot = (maxkrdot - krdot) / Tgsample;
        } else            /* Choose krdotdot based on max slew rate limit. */
        {

            /* Set up for quadratic formula solution. */

            qdfA = 1 + tpfsq * kr * kr;
            qdfB = 2 * tpfsq * kr * krdot * krdot +
                   2 * tpfsq / fovval * dfovdrval * kr * kr * krdot * krdot;
            qdfC = pow(tpfsq * kr * krdot * krdot, 2) + 4 * tpfsq * pow(krdot, 4) +
                   pow(tpf * dfovdrval / fovval * kr * krdot * krdot, 2) +
                   4 * tpfsq * dfovdrval / fovval * kr * pow(krdot, 4) -
                   pow(GAMMA * slewmax, 2);

            rootparta = -qdfB / (2 * qdfA);
            rootpartb = qdfB * qdfB / (4 * qdfA * qdfA) - qdfC / qdfA;

            if (rootpartb < 0)    /* Safety check - if complex, take real part.*/

                *krdotdot = rootparta;

            else
                *krdotdot = rootparta + sqrt(rootpartb);

            /* Could check resulting slew rate here, as in q2r21.m. */
        }

        /* Calculate thetadotdot */

        *thetadotdot = tpf * dfovdrval / fovval * krdot * krdot + tpf * (*krdotdot);
    }

    /* ----------------------------------------------------------------------- */
    void
    calc_vds(double slewmax, double gradmax, double Tgsample, double Tdsample, int Ninterleaves,
             double *fov, int numfov, double krmax,
             int ngmax, double **xgrad, double **ygrad, int *numgrad)

    /*	Function designs a variable-density spiral gradient waveform
     *	that is defined by a number of interleaves, resolution (or max number
     *	of samples), and field-of-view.
     *	The field-of-view is a polynomial function of the
     *	k-space radius, so fov is an array of coefficients so that
     *
     *	FOV = fov[0]+fov[1]*kr+fov[2]*kr^2+ ... +fov[numfov-1]*kr^(numfov-1)
     *
     * 	Gradient design is subject to a constant-slew-rate-limit model,
     * 	with maximum slew rate slewmax, and maximum gradient amplitude
     * 	of gradmax.
     *
     * 	Tgsample is the gradient sampling rate, and Tdsample is the data
     * 	sampling rate.  It is highly recommended to OVERSAMPLE the gradient
     * 	in the design to make the integration more stable.
     *
     * */

    //double slewmax;		/*	Maximum slew rate, G/cm/s		*/
    //double gradmax;		/* 	maximum gradient amplitude, G/cm	*/
    //double Tgsample;	/*	Gradient Sample period (s)		*/
    //double Tdsample;	/*	Data Sample period (s)			*/
    //int Ninterleaves;	/*	Number of interleaves			*/
    //double *fov;		/*	FOV coefficients		*/
    //int numfov;		/*	Number of FOV coefficients		*/
    //double krmax;		/*	Maximum k-space extent (/cm)		*/
    //int ngmax;		/*	Maximum number of gradient samples	*/
    //double **xgrad;		/* 	[output] X-component of gradient (G/cm) */
    //double **ygrad;		/*	[output] Y-component of gradient (G/cm)	*/
    //int *numgrad;		/* 	[output] Number of gradient samples */

    /* ----------------------------------------------------------------------- */
    {
        int gradcount = 0;

        double kr = 0;            /* Current value of kr	*/
        double krdot = 0;        /* Current value of 1st derivative of kr */
        double krdotdot = 0;        /* Current value of 2nd derivative of kr */

        double theta = 0;            /* Current value of theta */
        double thetadot = 0;        /* Current value of 1st derivative of theta */
        double thetadotdot = 0;        /* Current value of 2nd derivative of theta */

        double lastkx = 0;        /* x-component of last k-location. */
        double lastky = 0;        /* y-component of last k-location */
        double kx, ky;            /* x and y components of current k-location */

        double *gxptr, *gyptr;        /* Pointers to gradient variables. */

        /* First just find the gradient length. */

        while ((kr < krmax) && (gradcount < ngmax)) {
            calcthetadotdot(slewmax, gradmax, kr, krdot, Tgsample, Tdsample,
                            Ninterleaves, fov, numfov, &thetadotdot, &krdotdot);

            /* Integrate to obtain new values of kr, krdot, theta and thetadot:*/

            thetadot = thetadot + thetadotdot * Tgsample;
            theta = theta + thetadot * Tgsample;

            krdot = krdot + krdotdot * Tgsample;
            kr = kr + krdot * Tgsample;

            gradcount++;

        }

        /* Allocate memory for gradients. */

        *numgrad = gradcount;

        //*xgrad = (double *)malloc(*numgrad*sizeof(double));
        //*ygrad = (double *)malloc(*numgrad*sizeof(double));

        *xgrad = new double[*numgrad];
        *ygrad = new double[*numgrad];

        /* Reset parameters */

        kr = 0;
        krdot = 0;
        theta = 0;
        thetadot = 0;
        gradcount = 0;
        gxptr = *xgrad;
        gyptr = *ygrad;

        /* Now re-calculate gradient to find length. */

        while ((kr < krmax) && (gradcount < ngmax)) {
            calcthetadotdot(slewmax, gradmax, kr, krdot, Tgsample, Tdsample,
                            Ninterleaves, fov, numfov, &thetadotdot, &krdotdot);

            /* Integrate to obtain new values of kr, krdot, theta and thetadot:*/

            thetadot = thetadot + thetadotdot * Tgsample;
            theta = theta + thetadot * Tgsample;

            krdot = krdot + krdotdot * Tgsample;
            kr = kr + krdot * Tgsample;

            /* Define current gradient values from kr and theta. */

            kx = kr * cos(theta);
            ky = kr * sin(theta);
            *gxptr++ = (1 / GAMMA / Tgsample) * (kx - lastkx);
            *gyptr++ = (1 / GAMMA / Tgsample) * (ky - lastky);
            lastkx = kx;
            lastky = ky;

            gradcount++;
        }

    }

    /* ----------------------------------------------------------------------- */
    void
    calc_traj(double *xgrad, double *ygrad, int ngrad, int Nints, double Tgsamp, double krmax,
              double **x_trajectory, double **y_trajectory,
              double **weights) //, double** y_weights)
    /*
     *inputs:
     *      xgrad   X gradient waveform
     *      ygrad   Y gradient waveform
     *      ngrad   number of gradient samples
     *      Nints   number of interleaves
     *      Tgsamp  sampling time for gradients
     *
     *outputs:
     *      x_trajectory    X position in k-space
     *      y_trajectory    Y position in k-space
     *      x_weights       X weighting
     *      y_weights       Y weighting
     *
     **/
    {

        *x_trajectory = new double[(ngrad * Nints)];
        *y_trajectory = new double[(ngrad * Nints)];
        *weights = new double[(ngrad * Nints)];

        double *txptr = *x_trajectory;
        double *typtr = *y_trajectory;
        double *wptr = *weights;

        for (int inter = 0; inter < Nints; inter++) {
            double rotation = (inter * 2 * PI) / Nints;
            double x_tr = 0;
            double y_tr = 0;
            float x_temp, y_temp;
            for (int gradcount = 0; gradcount < ngrad; gradcount++) {
                if (gradcount > 0) {
                    x_tr += (GAMMA) * xgrad[gradcount - 1] * Tgsamp;
                    y_tr += (GAMMA) * ygrad[gradcount - 1] * Tgsamp;
                }

                x_temp = (x_tr * cos(rotation)) + (y_tr * sin(rotation));
                y_temp = -(x_tr * sin(rotation)) + (y_tr * cos(rotation));
                *(txptr++) = x_temp / krmax;
                *(typtr++) = y_temp / krmax;

                //abs(g(:)
                double abs_w = sqrt((pow(xgrad[gradcount], 2)) + (pow(ygrad[gradcount], 2)));
                double ang_g = xgrad[gradcount] == 0.0 ? PI/2 : atan2(ygrad[gradcount],xgrad[gradcount]);

                double ang_t = x_tr == 0.0 ? PI/2 : atan2(y_tr,x_tr);

                double tp_w = sin(ang_g - ang_t);
                tp_w = abs_w * abs(tp_w);
                *wptr++ = tp_w;
            }
        }
    }
    hoNDArray<float> calculate_weights(const hoNDArray<floatd2> &gradients, const hoNDArray<floatd2> &trajectories) {

        hoNDArray<float> weights(gradients.dimensions());

        boost::transform(gradients,trajectories,weights.begin(),
                [](const floatd2& gradient,const floatd2& trajectory){
                    auto abs_w = norm(gradient);
                    auto ang_g = atan2(gradient[1],gradient[0]);
                    auto ang_t = atan2(trajectory[1],trajectory[0]);
                    return abs(sin(ang_g-ang_t))*abs_w*2;

        });

        return weights;
    }

    hoNDArray<float> calculate_weights_Hoge(const hoNDArray<floatd2> &gradients, const hoNDArray<floatd2> &trajectories) {

        hoNDArray<float> weights(gradients.dimensions());

        boost::transform(gradients,trajectories,weights.begin(),
                [](const floatd2& gradient,const floatd2& trajectory){
                    auto abs_g = norm(gradient);
                    auto abs_t = norm(trajectory);
                    auto ang_g = atan2(gradient[1],gradient[0]);
                    auto ang_t = atan2(trajectory[1],trajectory[0]);
                    return abs(cos(ang_g-ang_t))*abs_g*abs_t;

        });

        return weights;
    }

    hoNDArray<floatd2> calculate_trajectories(const hoNDArray<floatd2> &gradients, float sample_time, float krmax) {
                const int nints = gradients.get_size(1);
        const int ngrad = gradients.get_size(0);

        auto trajectory = hoNDArray < floatd2 > (gradients.dimensions());

        for (int interleave = 0; interleave < nints; interleave++) {
            trajectory(0, interleave) = floatd2(0, 0);

            for (int gradcount = 1; gradcount < ngrad; gradcount++) {
                trajectory(gradcount, interleave) = trajectory(gradcount - 1, interleave) +
                                                    float(GAMMA) * gradients(gradcount - 1,interleave) * sample_time;
            }
        }

        boost::transform(trajectory,trajectory.begin(),[&](auto t){ return t/krmax;});
        return trajectory;
    }

    hoNDArray<floatd2> calculate_vds(double slewmax, double gradmax, double Tgsample, double Tdsample, int Ninterleaves,
             double *fov, int numfov, double krmax, int ngmax,int max_nsamples){

        double* x_ptr;
        double* y_ptr;
        int Nints;
        calc_vds(slewmax,gradmax,Tgsample,Tdsample,Ninterleaves,fov, numfov,krmax,ngmax,&x_ptr,&y_ptr,&Nints);

        hoNDArray<floatd2> gradient(std::min(Nints,max_nsamples));
        size_t nsamples = gradient.get_number_of_elements();
        std::transform(x_ptr,x_ptr+nsamples,y_ptr,gradient.begin(),[](auto x, auto y){return -floatd2(x,y)/2;});

        delete[] x_ptr;
        delete[] y_ptr;
        return gradient;

    }

    hoNDArray<floatd2>  create_rotations(const hoNDArray<floatd2>& trajectories,int nints) {
        auto dims = trajectories.dimensions();
        dims.push_back(nints);
         hoNDArray < floatd2 > result(dims);

        int ngrad = trajectories.get_number_of_elements();
        for (int inter = 0; inter < nints; inter++) {
            double rotation = (inter * 2 * PI) / nints;
            for (int gradcount = 0; gradcount < ngrad; gradcount++) {
                floatd2 point = trajectories[gradcount];
                result(gradcount, inter) = floatd2(point[0] * cos(rotation) + point[1] * sin(rotation),
                                                  -point[0] * sin(rotation) + point[1] * cos(rotation));
            }
        }
        return result;
    }
}


#include <fstream>
#include <ros/ros.h>
#include <ros/package.h>
#include <Eigen/Dense>

void mapIdToIndex(int id, int& ix_cell, int& iy_cell, int map_width){
  iy_cell = id / map_width;
  ix_cell = id - iy_cell * map_width;
}

std::vector<int> positionToMapIndex(double x, double y,
   unsigned int width, unsigned int height, float resolution){
  std::vector<int> index(2);
  index[0] = floor(x / resolution) + width / 2;
  index[1] = floor(y / resolution) + height / 2;

  return index;
}

void findChange(bool& change, int occupation, int current_occupation){
  if (current_occupation != occupation){
    if (std::abs(current_occupation + occupation) == 1){
      change = true;
    }
  }
}

double xCircleMinDist(int ix, int iy, int n_cells, Eigen::MatrixXi& map){
  // https://www.geeksforgeeks.org/bresenhams-circle-drawing-algorithm/
  float dist_sq = -1;
  int occupation = map(ix, iy);
  int x = n_cells;
  int y = 0;
  int err = 3 - (n_cells << 1);
  bool found_change = false;

  int xc, yc;
  if (occupation != 0){
    xc = x - 1;
    yc = y;
  }
  else {
    xc = x;
    yc = y;
  }

  while (x >= y){
    findChange(found_change, occupation, map(ix + x, iy + y)); // 1. octant
    findChange(found_change, occupation, map(ix + y, iy + x)); // 2. octant
    findChange(found_change, occupation, map(ix - y, iy + x)); // 3. octant
    findChange(found_change, occupation, map(ix - x, iy + y)); // 4. octant
    findChange(found_change, occupation, map(ix - x, iy - y)); // 5. octant
    findChange(found_change, occupation, map(ix - y, iy - x)); // 6. octant
    findChange(found_change, occupation, map(ix + y, iy - x)); // 7. octant
    findChange(found_change, occupation, map(ix + x, iy - y)); // 8. octant

    if (found_change){
      double dist_sq_temp = xc * xc + yc * yc;
      if (dist_sq == -1 || dist_sq > dist_sq_temp){
        dist_sq = dist_sq_temp;
      }
    }

    y++;
    if (err > 0){
      x--;
      err += ((y - x) << 2) + 10; //<< 2 -> 4*
    }
    else {
      err += (y << 2) + 6;
    }

    if (occupation != 0){
      yc = y - 1;
      xc = x - 1;
    }
    else {
      yc = y;
      xc = x;
    }
    found_change = false;
  }
  if (dist_sq != -1) return sqrt(dist_sq);
  else return dist_sq;
}

int main(int argc, char **argv)
{
  // Init params
  int map_width = 2000;
  int map_height = 2000;
  float map_resolution = 0.05;
  Eigen::MatrixXi occ_mat(map_width, map_height);

  // Get path and file name
  std::string package_path = ros::package::getPath("crowdbot_active_slam");
  std::string save_directory_path = package_path + "/" + argv[1];
  std::string map_path = save_directory_path + "/occupancy_grid_map.txt";
  std::string save_path = save_directory_path + "/sdf_map.txt";

  std::ifstream map_file(map_path.c_str());

  std::string line;
  int id = 0;

  if (map_file.is_open()){
    int x_cell, y_cell;
    while (std::getline(map_file, line)){
      mapIdToIndex(id, x_cell, y_cell, map_width);
      std::stringstream ss_ref;
      ss_ref << line;
      double p_ref;
      ss_ref >> p_ref;

      if (p_ref == -1){
        occ_mat(x_cell, y_cell) = -1;
      }
      else if (p_ref >= 50){
        occ_mat(x_cell, y_cell) = 1;
      }
      else {
        occ_mat(x_cell, y_cell) = 0;
      }
      id += 1;
    }
  }
  else {
    ROS_INFO("Failed to open map_file!");
  }

  // SDF calculation
  // World borders (inflated by 1m from wall) for utm_0 world
  double p1[2] = {-5.5, -13.5};
  double p2[2] = {24.5, 21.5};

  std::vector<int> p_start = positionToMapIndex(p1[0], p1[1], map_width, map_height, map_resolution);
  std::vector<int> p_end = positionToMapIndex(p2[0], p2[1], map_width, map_height, map_resolution);

  int sdf_width = p_end[0] - p_start[0] + 1;
  int sdf_height = p_end[1] - p_start[1] + 1;
  Eigen::MatrixXd SDF_mat(sdf_width, sdf_height);

  int border = 0;
  int counter = 0;
  // Calculate SDF
  #pragma omp parallel for
  for (int i = p_start[0]; i <= p_end[0]; i++){
    int n = 1;
    double dist = -1;
    bool not_found = true;
    for (int j = p_start[1]; j <= p_end[1]; j++){
      // Search for shortest distance of (i, j) and assign distance to SDF map
      dist = -1;
      not_found = true;
      while (not_found){
        dist = xCircleMinDist(i, j, n, occ_mat);
        if (dist == -1){
          n += 1;
        }
        else {
          not_found = false;
          double temp_dist = xCircleMinDist(i, j, n + 1, occ_mat);
          if (dist > temp_dist && temp_dist != -1){
            dist = temp_dist;
          }
          if (n > 2) n -= 2;
          else n = 1;
        }
      }

      if (occ_mat(i, j) == 0){
        SDF_mat(i - p_start[0], j - p_start[1]) = dist;
      }
      else {
        SDF_mat(i - p_start[0], j - p_start[1]) = -dist;
      }
    }

    #pragma omp atomic
    counter++;
    int percent = 100 * counter / sdf_width;
    if (percent >= border){
      std::cout << percent << " %" << std::endl;
      #pragma omp atomic
      border += 1;
    }
  }

  // Save map
  std::ofstream sdf_file(save_path.c_str());
  if (sdf_file.is_open()){
    sdf_file << SDF_mat;
    sdf_file.close();
  }
  else{
    ROS_INFO("Could not save sdf_map.txt!");
  }
}


#include <array>
#include <iostream>
#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace boost::numeric::odeint;

using state_type = array<double, 3>;

void lorenz_param(const state_type& x, state_type& dxdt, double t,
                  double sigma, double R, double b) {
    dxdt[0] = sigma*(x[1] - x[0]);
    dxdt[1] = R*x[0] - x[1] - x[0]*x[2];
    dxdt[2] = -b*x[2] + x[0]*x[1];
}

void write_lorenz(const state_type& x, const double t) {
    cout << t << '\t' << x[0] << '\t' << x[1] << '\t' << x[2]
         << endl;
}

int main(int argc, char **argv)
{
    const double sigma = 10.0;
    const double R = 28.0;
    const double b = 8.0/3.0;
    using namespace std::placeholders;
    auto lorenz = bind(lorenz_param, _1, _2, _3, sigma, R, b);
    state_type x = { 10.0, 1.0, 1.0 }; // initial conditions
    integrate(lorenz, x, 0.0, 25.0, 0.1, write_lorenz );
    return 0;
}


#include "students_t.h"

#include <boost/math/distributions/students_t.hpp>
using namespace boost::math;

double get_t_value(double alpha, size_t df)
{
    //http://www.boost.org/doc/libs/1_39_0/libs/math/doc/sf_and_dist/html/
    //       math_toolkit/dist/stat_tut/weg/st_eg/tut_mean_intervals.html
    return quantile(complement(students_t(df), alpha / 2));
}


#include <iomanip>
#include <iostream>
#include <armadillo>

#include "GordonNewell.h"

using namespace std;
using namespace arma;

void writeOutput(ofstream& ofstream, const mat& matrix, int K)
{
    ofstream << "////////////////////////////////////////" << endl;
    ofstream << "////////////////" << " K := " << K << " ////////////////" << endl;
    ofstream << "////////////////////////////////////////" << endl;

    ofstream << endl;

    for (int i = 0; i < matrix.n_rows; i++)
        ofstream << "X_" << i + 1 << " = " << matrix(i, 0) << endl;

    ofstream << endl;
}

void writeOutputBuzen(ofstream& ofstream, const mat& data, const int k, const int n, const double& T)
{
    ofstream << "////////////////////////////////////////" << endl;
    ofstream << "///////////" << " K := " << k << "; N := " << n << "; ///////////" << endl;
    ofstream << "////////////////////////////////////////" << endl;

    ofstream << endl;

    ofstream << "T = " << T << " [s]" << endl;

    ofstream << endl;
    ofstream << left
        << setw(15) << "Server #"
        << setw(15) << "U"
        << setw(15) << "X [1/s]"
        << setw(15) << "n_avg"
        << setw(15) << "R [s]"
        << endl;

    for (int i = 0; i < data.n_rows; i++)
    {
        ofstream << left << setw(15) << i;

        for (int j = 0; j < data.n_cols; j++)
            ofstream << left << setw(15) << data(i, j);

        ofstream << endl;
    }

    ofstream << endl;
}

int main(int argc, char** argv)
{
    ofstream outputStream;
    outputStream.open("potraznje_analiticki.txt");

    ofstream output10, output15, output20;
    output10.open("rezultati_analiticki_10.txt");
    output15.open("rezultati_analiticki_15.txt");
    output20.open("rezultati_analiticki_20.txt");

    for (int k = 2; k <= 8; k++)
    {
        Analyzer gn(k);
        mat result = gn.GetXMatrix();

        writeOutput(outputStream, result, k);

        for (int mprg = 10; mprg <= 20; mprg += 5)
        {
            double T;
            mat data = gn.GetBuzenOutput(result, mprg, T);

            ofstream* oref;
            if (mprg == 10)
                oref = &output10;
            else if (mprg == 15)
                oref = &output15;
            else
                oref = &output20;

            writeOutputBuzen(*oref, data, k, mprg, T);
        }
    }

    outputStream.close();
    output10.close();
    output15.close();
    output20.close();

    return 0;
}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Temple/Adaptors/All.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/constexpr/Numeric.h"

// boost algorithm replacements
#include <boost/algorithm/cxx11/all_of.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/algorithm/transform.hpp>

using namespace Scine::Molassembler;

PURITY_STRONG double divByThree (unsigned a) {
  return static_cast<double>(a) / 3.0;
}

BOOST_AUTO_TEST_CASE(SumTest, *boost::unit_test::label("Temple")) {
  std::vector<unsigned> instance {0, 1, 2, 3};
  auto f = Temple::sum(instance);

  BOOST_CHECK(f == 6);

  BOOST_CHECK(
    boost::accumulate(instance, 0) == 6
  );

  auto mapped = Temple::map(instance, divByThree);
  BOOST_CHECK(mapped == std::vector<double>({0, 1.0/3.0, 2.0/3.0, 1}));

  std::vector<double> bmapped;
  boost::transform(instance, std::back_inserter(bmapped), divByThree);
  BOOST_CHECK(mapped == bmapped);

  auto pairwiseSum = Temple::map(
    Temple::Adaptors::sequentialPairs(instance),
    std::plus<>()
  );

  BOOST_CHECK(pairwiseSum == std::vector<unsigned>({1,3,5}));

  std::vector<unsigned> bpairwiseSum;
  boost::transform(
    instance,
    boost::adaptors::slice(instance, 1, instance.size() - 1),
    std::back_inserter(bpairwiseSum),
    std::plus<>()
  );

  auto pairwiseSmaller = Temple::accumulate(
    Temple::map(
      Temple::Adaptors::sequentialPairs(instance),
      std::less<>()
    ),
    true,
    std::logical_and<>()
  );

  auto bpairwiseSmaller = boost::algorithm::all_of(
    boost::combine(
      boost::adaptors::slice(instance, 0, instance.size() - 2),
      boost::adaptors::slice(instance, 1, instance.size() - 1)
    ),
    [&](const auto& twoTuple) -> bool {
      return Temple::invoke(std::less<>(), twoTuple);
    }
  );

  static_assert(std::is_same<decltype(bpairwiseSmaller), bool>::value, "Not a bool??");
  BOOST_CHECK(bpairwiseSmaller);

  BOOST_CHECK(pairwiseSmaller);

  std::vector<
    std::vector<unsigned>
  > vectorOfVectors {
    {0, 1, 4},
    {4, 5}
  };

  auto mapToSizes = Temple::map(
    vectorOfVectors,
    [](const std::vector<unsigned>& vectorUnsigned) -> unsigned {
      return vectorUnsigned.size();
    }
  );

  std::vector<unsigned> bsizes;
  boost::transform(
    vectorOfVectors,
    std::back_inserter(bsizes),
    [](const std::vector<unsigned>& vectorUnsigned) -> unsigned {
      return vectorUnsigned.size();
    }
  );
  BOOST_CHECK(bsizes == mapToSizes);

  std::vector<unsigned> unsignedVector {1, 2, 3};

  BOOST_CHECK(
    Temple::sum(
      Temple::map(
        Temple::Adaptors::allPairs(unsignedVector),
        [](const unsigned a, const unsigned b) -> unsigned {
          return a + b;
        }
      )
    ) == 12
  );

  BOOST_CHECK(
    !Temple::all_of(
      unsignedVector,
      [](auto i) -> bool {
        return i < 2;
      }
    )
  );

  std::vector<double> doubleVector {1.2, 1.5, 1.9};

  BOOST_CHECK(
    Temple::sum(
      Temple::map(
        Temple::Adaptors::allPairs(doubleVector),
        [](const double a, const double b) -> double {
          return a + b;
        }
      )
    ) == 9.2
  );
}

BOOST_AUTO_TEST_CASE(ReduceTests, *boost::unit_test::label("Temple")) {
  std::vector<unsigned> values {1, 2, 3, 4, 5};
  BOOST_CHECK(
    Temple::accumulate(
      values,
      0U,
      std::plus<>()
    ) == 15U
  );
  BOOST_CHECK(
    Temple::accumulate(
      values,
      1U,
      std::multiplies<>()
    ) == 120U
  );
}

BOOST_AUTO_TEST_CASE(MinMaxTests, *boost::unit_test::label("Temple")) {
  const std::vector<unsigned> values {1, 4, 6, 8};
  BOOST_CHECK(Temple::max(values) == 8);
  BOOST_CHECK(Temple::min(values) == 1);
}

BOOST_AUTO_TEST_CASE(MapToSameContainerTests, *boost::unit_test::label("Temple")) {
  std::set<int> f {5, -1, 9};

  auto fMapped = Temple::map_stl(
    f,
    [](const int& x) -> double {
      return x + 1.3;
    }
  );

  static_assert(
    std::is_same<decltype(fMapped), std::set<double>>::value,
    "Map to same container does not work as expected"
  );

  std::vector<float> values {0, 3.4, 9};
  auto xMapped = Temple::map(
    values,
    [](const float& x) -> unsigned long {
      return static_cast<unsigned long>(x);
    }
  );

  static_assert(
    std::is_same<decltype(xMapped), std::vector<unsigned long>>::value,
    "Map to same container does not work as expected"
  );
}


#include "2019/day14/cpp/include/formula.hpp"

#include <math.h>

#include <boost/algorithm/string.hpp>
#include <iostream>
#include <regex>

#include "utils/cpp/include/utils.hpp"

Formula::Formula(std::string file_name) : m_formula() {
  initialize_formula(file_name);
}

void Formula::initialize_formula(std::string file_name) {
  auto form = utils::read_file(file_name);

  // Regex to separeate left-hand and right-hand side with "=>"
  std::regex rgx("([^=>]+)");
  for (auto &f : form) {
    std::smatch sm;
    std::vector<std::string> parts;
    while (regex_search(f, sm, rgx)) {
      std::string part = sm[0];
      boost::algorithm::trim(part);
      parts.push_back(part);
      f = sm.suffix();
    }

    // Split the left-hand and right-hand sides into parts
    std::vector<std::string> LHS, rhs;
    utils::split_string(parts[0], ",", LHS);
    utils::split_string(parts[1], " ", rhs);

    // Go through the left hand side assign the formula dependency
    for (auto &lhs : LHS) {
      boost::algorithm::trim(lhs);
      std::vector<std::string> t;
      utils::split_string(lhs, " ", t);
      m_formula[t[1]].push_back(
          std::make_tuple(std::stoi(t[0]), std::stoi(rhs[0]), rhs[1]));
    }
  }
}

// Figure out how much fuel is needed to generate a specific "key"
int64_t Formula::compute(std::string key, int64_t amount) {
  if (key == "FUEL") {
    return amount;
  }

  int64_t s = 0;
  for (auto &form : m_formula[key]) {
    auto total = compute(std::get<2>(form), amount);
    s += (std::ceil(total / double(std::get<1>(form))) * std::get<0>(form));
  }
  return s;
}

// Figure out how much fuel can be generated, given <ore>
int64_t Formula::compute_fuel(int64_t ore) {
  int64_t fuel_min(ore / compute("ORE", 1)), fuel_max(ore);

  while ((fuel_max - fuel_min) > 1) {
    int64_t mid = (fuel_min + fuel_max) / int64_t(2);

    if (compute("ORE", mid) <= ore) {
      fuel_min = mid;
    } else {
      fuel_max = mid;
    }
  }
  return fuel_min;
}


/*
 * ErrorEllipse.hpp
 *
 *  Created on: 01.08.2018
 *      Author: tomlucas
 */

#ifndef OSG_VIZ_ERRORELLIPSE_HPP_
#define OSG_VIZ_ERRORELLIPSE_HPP_

#include <osg/AutoTransform>
#include "../Plugins/state_plugin_estimator.hpp"
#include "../Estimators/Estimator.hpp"
#include <osg/PolygonMode>
#include <Eigen/Eigenvalues>
#include <math.h>

#include <Eigen_Utils.hpp>

#include <OSG_Utils.hpp>
#include "OSG_VIZ_Transform.hpp"
namespace zavi
::osg_viz {
	/**
	 * This class is used to visualize the covariance of an estimator
	 *
	 * just use addErrorEllipse() to show the covariance of an estimator
	 * @see addErrorEllipse
	 */
	class ErrorEllipse: public ::osg::PositionAttitudeTransform {

	public:
		/**
		 * the basic radius of the sphere
		 * @return the basic radius of the sphere
		 */
		inline osg::Vec3d getBaseRadModifier() {
			return base_rad;
		}
		/**
		 * the sigma intervall
		 * @return the sigma intervall
		 */
		inline double getSigma() {
			return sigma;
		}
	private:
		std::shared_ptr<zavi::plugin::StatePluginEstimator> plugin;     //< shared pointer holding the ode part
		osg::Vec3d base_rad;//< basic radius of sphere
		double sigma;//sigma intervall
		/**
		 * Callback to align osg object with ode object
		 */
		class estimatorCallback: public osg::NodeCallback {
		public:
			void operator()(osg::Node* node, osg::NodeVisitor* nv) {
				ErrorEllipse *mt = dynamic_cast<ErrorEllipse*>(node);
				auto cov = mt->plugin->getPositionError();
				//zavi::printf(cov);
				Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solv(cov);
				Eigen::Quaterniond quat(solv.eigenvectors());
				mt->setAttitude(zavi::osg_viz::eigenToOSGQuat(quat));
				Eigen::Vector3d scale = solv.eigenvalues().real().array().sqrt();
				mt->setScale(zavi::osg_viz::eigenToOSGVector(scale) * mt->getSigma() + mt->getBaseRadModifier());
				mt->setPivotPoint(osg::Vec3d(0, 0, 0));
				mt->setPosition(mt->plugin->getPositionOSG());
			}
		};
	public:
		/**
		 * Creates a new Transform and adds it to root
		 * @param node  the node which shall be transformed
		 @param estimator estimator to read information
		 * @param root  the group where to add the new transform
		 * @param base_rad the basic radius of the ellipse osg::Vec3d(base_rad,base_rad,base_rad)
		 * @param sigma the sigma intervall default 1
		 */
		static void addErrorEllipse(std::shared_ptr<zavi::estimator::Estimator> estimator,
				osg::ref_ptr<osg::Group> root=zavi::osg_viz::root, double base_rad=1, double sigma = 1) {
			std::shared_ptr<zavi::plugin::StatePluginEstimator> plugin(new zavi::plugin::StatePluginEstimator(estimator));
			addErrorEllipse(plugin,root,base_rad,sigma);
		}
		/**
		 * Creates a new Transform and adds it to root
		 * @param node  the node which shall be transformed
		 * @param plugin The StatePlugin to read Information
		 * @param root  the group where to add the new transform
		 * @param base_rad the basic radius of the ellipse osg::Vec3d(base_rad,base_rad,base_rad)
		 * @param sigma the sigma intervall default 1
		 */
		static void addErrorEllipse(
				std::shared_ptr<zavi::plugin::StatePluginEstimator> plugin,
				osg::ref_ptr<osg::Group> root=zavi::osg_viz::root, double base_rad=1,double sigma=1) {
			osg::ref_ptr<osg::ShapeDrawable> estimate_sphere(new osg::ShapeDrawable());
			estimate_sphere->setShape(new osg::Sphere(osg::Vec3d(0,0,0),1));
			osg::PolygonMode* polymode = new osg::PolygonMode;
			polymode->setMode(osg::PolygonMode::FRONT_AND_BACK,osg::PolygonMode::LINE);
			estimate_sphere->getOrCreateStateSet()->setAttributeAndModes(polymode,osg::StateAttribute::OVERRIDE|osg::StateAttribute::ON);
			osg::ref_ptr<zavi::osg_viz::ErrorEllipse> transform(
					new zavi::osg_viz::ErrorEllipse(plugin,base_rad,sigma));
			transform->addChild(estimate_sphere);
			//zavi::osg_viz::OSG_VIZ_Transform::addTransformedNode(transform,plugin,root,false);
			transform->setReferenceFrame(osg::Transform::ReferenceFrame::RELATIVE_RF);
			root->addChild(transform);
		}

		/**
		 * Creates the transform object
		 * @param plugin the Stateplugin to read pose information
		 * * @param base_rad the basic radius of the ellipse
		 */
		ErrorEllipse(std::shared_ptr<zavi::plugin::StatePluginEstimator> plugin, double base_rad, double sigma) :
		plugin(plugin),base_rad(osg::Vec3d(base_rad,base_rad,base_rad)),sigma(sigma) {
			this->setUpdateCallback(new estimatorCallback());

		}
		~ErrorEllipse()
		{
		}
	}
	;

}
//zavi::osg_viz

#endif /* OSG_VIZ_ERRORELLIPSE_HPP_ */


#include "cryptonote_config.h"
#include "common/italo.h"
#include "epee/int-util.h"
#include <boost/endian/conversion.hpp>
#include <limits>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <cfenv>

#include "service_node_rules.h"

namespace service_nodes {

// TODO(italo): Move to italo_economy, this will also need access to italo::exp2
uint64_t get_staking_requirement(cryptonote::network_type m_nettype, uint64_t height, uint8_t hf_version)
{
  if (m_nettype == cryptonote::TESTNET || m_nettype == cryptonote::FAKECHAIN)
      return COIN * 100;

  // For devnet we use the 10% of mainnet requirement at height (650k + H) so that we follow
  // (proportionally) whatever staking changes happen on mainnet.  (The 650k is because devnet
  // launched at ~600k mainnet height, so this puts it a little ahead).
  if (m_nettype == cryptonote::DEVNET)
      return get_staking_requirement(cryptonote::MAINNET, 600000 + height, hf_version) / 10;

  if (hf_version >= cryptonote::network_version_16_pulse)
    return 15000'000000000;

  if (hf_version >= cryptonote::network_version_13_enforce_checkpoints)
  {
    constexpr int64_t heights[] = {
        385824,
        429024,
        472224,
        515424,
        558624,
        601824,
        645024,
    };

    constexpr int64_t lsr[] = {
        20458'380815527,
        19332'319724305,
        18438'564443912,
        17729'190407764,
        17166'159862153,
        16719'282221956,
        16364'595203882,
    };

    assert(static_cast<int64_t>(height) >= heights[0]);
    constexpr uint64_t LAST_HEIGHT      = heights[italo::array_count(heights) - 1];
    constexpr uint64_t LAST_REQUIREMENT = lsr    [italo::array_count(lsr) - 1];
    if (height >= LAST_HEIGHT)
        return LAST_REQUIREMENT;

    size_t i = 0;
    for (size_t index = 1; index < italo::array_count(heights); index++)
    {
      if (heights[index] > static_cast<int64_t>(height))
      {
        i = (index - 1);
        break;
      }
    }

    int64_t H      = height;
    int64_t result = lsr[i] + (H - heights[i]) * ((lsr[i + 1] - lsr[i]) / (heights[i + 1] - heights[i]));
    return static_cast<uint64_t>(result);
  }

  uint64_t hardfork_height = 101250;
  if (height < hardfork_height) height = hardfork_height;

  uint64_t height_adjusted = height - hardfork_height;
  uint64_t base = 0, variable = 0;
  std::fesetround(FE_TONEAREST);
  if (hf_version >= cryptonote::network_version_11_infinite_staking)
  {
    base     = 15000 * COIN;
    variable = (25007.0 * COIN) / italo::exp2(height_adjusted/129600.0);
  }
  else
  {
    base      = 10000 * COIN;
    variable  = (35000.0 * COIN) / italo::exp2(height_adjusted/129600.0);
  }

  uint64_t result = base + variable;
  return result;
}

uint64_t portions_to_amount(uint64_t portions, uint64_t staking_requirement)
{
  uint64_t hi, lo, resulthi, resultlo;
  lo = mul128(staking_requirement, portions, &hi);
  div128_64(hi, lo, STAKING_PORTIONS, &resulthi, &resultlo);
  return resultlo;
}

bool check_service_node_portions(uint8_t hf_version, const std::vector<uint64_t>& portions)
{
  if (portions.size() > MAX_NUMBER_OF_CONTRIBUTORS) return false;

  uint64_t reserved = 0;
  for (auto i = 0u; i < portions.size(); ++i)
  {
    const uint64_t min_portions = get_min_node_contribution(hf_version, STAKING_PORTIONS, reserved, i);
    if (portions[i] < min_portions) return false;
    reserved += portions[i];
  }

  return reserved <= STAKING_PORTIONS;
}

crypto::hash generate_request_stake_unlock_hash(uint32_t nonce)
{
  static_assert(sizeof(crypto::hash) == 8 * sizeof(uint32_t) && alignof(crypto::hash) >= alignof(uint32_t));
  crypto::hash result;
  boost::endian::native_to_little_inplace(nonce);
  for (size_t i = 0; i < 8; i++)
    reinterpret_cast<uint32_t*>(result.data)[i] = nonce;
  return result;
}

uint64_t get_locked_key_image_unlock_height(cryptonote::network_type nettype, uint64_t node_register_height, uint64_t curr_height)
{
  uint64_t blocks_to_lock = staking_num_lock_blocks(nettype);
  uint64_t result         = curr_height + (blocks_to_lock / 2);
  return result;
}

static uint64_t get_min_node_contribution_pre_v11(uint64_t staking_requirement, uint64_t total_reserved)
{
  return std::min(staking_requirement - total_reserved, staking_requirement / MAX_NUMBER_OF_CONTRIBUTORS);
}

uint64_t get_max_node_contribution(uint8_t version, uint64_t staking_requirement, uint64_t total_reserved)
{
  if (version >= cryptonote::network_version_16_pulse)
    return (staking_requirement - total_reserved) * config::MAXIMUM_ACCEPTABLE_STAKE::num
      / config::MAXIMUM_ACCEPTABLE_STAKE::den;
  return std::numeric_limits<uint64_t>::max();
}

uint64_t get_min_node_contribution(uint8_t version, uint64_t staking_requirement, uint64_t total_reserved, size_t num_contributions)
{
  if (version < cryptonote::network_version_11_infinite_staking)
    return get_min_node_contribution_pre_v11(staking_requirement, total_reserved);

  const uint64_t needed = staking_requirement - total_reserved;
  assert(MAX_NUMBER_OF_CONTRIBUTORS > num_contributions);
  if (MAX_NUMBER_OF_CONTRIBUTORS <= num_contributions) return UINT64_MAX;

  const size_t num_contributions_remaining_avail = MAX_NUMBER_OF_CONTRIBUTORS - num_contributions;
  return needed / num_contributions_remaining_avail;
}

uint64_t get_min_node_contribution_in_portions(uint8_t version, uint64_t staking_requirement, uint64_t total_reserved, size_t num_contributions)
{
  uint64_t atomic_amount = get_min_node_contribution(version, staking_requirement, total_reserved, num_contributions);
  uint64_t result        = (atomic_amount == UINT64_MAX) ? UINT64_MAX : (get_portions_to_make_amount(staking_requirement, atomic_amount));
  return result;
}

uint64_t get_portions_to_make_amount(uint64_t staking_requirement, uint64_t amount, uint64_t max_portions)
{
  uint64_t lo, hi, resulthi, resultlo;
  lo = mul128(amount, max_portions, &hi);
  if (lo > UINT64_MAX - (staking_requirement - 1))
    hi++;
  lo += staking_requirement-1;
  div128_64(hi, lo, staking_requirement, &resulthi, &resultlo);
  return resultlo;
}

static bool get_portions_from_percent(double cur_percent, uint64_t& portions) {
  if(cur_percent < 0.0 || cur_percent > 100.0) return false;

  // Fix for truncation issue when operator cut = 100 for a pool Service Node.
  if (cur_percent == 100.0)
  {
    portions = STAKING_PORTIONS;
  }
  else
  {
    portions = (cur_percent / 100.0) * (double)STAKING_PORTIONS;
  }

  return true;
}

bool get_portions_from_percent_str(std::string cut_str, uint64_t& portions) {

  if(!cut_str.empty() && cut_str.back() == '%')
  {
    cut_str.pop_back();
  }

  double cut_percent;
  try
  {
    cut_percent = boost::lexical_cast<double>(cut_str);
  }
  catch(...)
  {
    return false;
  }

  return get_portions_from_percent(cut_percent, portions);
}

} // namespace service_nodes