#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
typedef long long ll;
typedef vector <int> vi;

int der[14];
int fac[14];

int main(){
    ios_base::sync_with_stdio(false);
    cin.tie(nullptr);
    freopen("in.txt", "r", stdin);
    freopen("out.txt", "w", stdout);
    der[0] = 1; der[1] = 0;
    int res = 1;
    fac[0] = fac[1] = 1;
    for(int i=2; i<13; i++){
        der[i] = (i-1) * (der[i-1] + der[i-2]);
        fac[i] = res = res * i;
    }
    int tc; cin >> tc;
    for(int cas = 1; cas <= tc; cas++){
        int n; cin >> n;
        cout << der[n] << "/" << fac[n] << "\n";
    }
    return 0;
}

Rare Topics/Rare Algorithms/Formulas or Theorems/Hats.cpp


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[intersection_segment
//` Calculate the intersection point (or points) of two segments

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>

#include <boost/foreach.hpp>

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> P;
    boost::geometry::model::segment<P> segment1, segment2;
    boost::geometry::read_wkt("linestring(1 1,2 2)", segment1);
    boost::geometry::read_wkt("linestring(2 1,1 2)", segment2);

    std::vector<P> intersections;
    boost::geometry::intersection(segment1, segment2, intersections);

    BOOST_FOREACH(P const& p, intersections)
    {
        std::cout << " " << boost::geometry::wkt(p);
    }
    std::cout << std::endl;

    return 0;
}

//]

//[intersection_segment_output
/*`
Output:
[pre
 POINT(1.5 1.5)
]
*/
//]


#include "optimization/ransac.h"

#include <Eigen/Dense>
#include <algorithm>
#include <random>

#include "gflags/gflags.h"
#include "glog/logging.h"
#include "glog/stl_logging.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "optimization/ransac_impl.h"

DEFINE_int32(num_test_cases, 10, "Number of test cases to run.");

namespace dungeon {

// A model that fits a straight line to observations in 2D.
class LinearModel : public RansacModelBase {
 public:
  // Given that each observation is a vector in R^2, fits parameters (m, b)
  // to minimize L2 error:
  //   \sum_i || m*x(i, 0) + b - x(i, 1) ||^2
  virtual void Fit(const std::vector<Eigen::MatrixXf>& observations) override {
    Eigen::MatrixXf A(observations.size(), 2);
    Eigen::MatrixXf b(observations.size(), 1);
    for (size_t i = 0; i < observations.size(); ++i) {
      const Eigen::MatrixXf& obs = observations[i];
      CHECK_EQ(2, obs.rows());
      CHECK_EQ(1, obs.cols());

      A(i, 0) = obs(0, 0);
      A(i, 1) = 1.0;
      b(i, 0) = obs(1, 0);
    }
    params_ = A.colPivHouseholderQr().solve(b);
    CHECK_EQ(2, params_.rows());
    CHECK_EQ(1, params_.cols());
  }

  // Returns abs(m*x(0) + b - x(1)) for a given observation x.
  virtual double EvaluateError(const Eigen::MatrixXf& pt) const override {
    CHECK_EQ(2, pt.rows());
    CHECK_EQ(1, pt.cols());
    float x = pt(0, 0);
    float y = pt(1, 0);
    return std::abs(params_(0, 0) * x + params_(1, 0) - y);
  }

  virtual Eigen::MatrixXf params() const override { return params_; }

  // Returns the minimum number of observations needed to fit the model.
  virtual int min_num_observations() const { return 2; }

 private:
  Eigen::MatrixXf params_;
};

// Returns a collection of observations for the above model.
std::vector<Eigen::MatrixXf> CreateObservationCollection(
    const Eigen::MatrixXf& params, int num_observations) {
  CHECK_EQ(2, params.rows());
  CHECK_EQ(1, params.cols());
  const float params_x = params(0, 0);
  const float params_y = params(1, 0);
  std::vector<Eigen::MatrixXf> output;
  for (int i = 0; i < num_observations; ++i) {
    float val = drand48() * 10.0 - 5.0;
    output.push_back(
        Eigen::MatrixXf(Eigen::Vector2f(val, params_x * val + params_y)));
  }
  return output;
}

std::vector<Eigen::MatrixXf> GenerateRandomModelParams(int n) {
  srand(0);
  std::vector<Eigen::MatrixXf> output;
  for (int i = 0; i < n; ++i) {
    output.push_back(Eigen::MatrixXf::Random(2, 1));
  }
  return output;
}

class RansacTest : public ::testing::Test,
                   public ::testing::WithParamInterface<Eigen::MatrixXf> {
 public:
  void SetUp() {
    srand(0);
    opts = RansacOptions{
        .num_iterations = 1000,
        .inlier_threshold = 0.001,
        .min_inlier_fraction = 0.5,
        .random_seed = 0,
    };
    CHECK_GT(kNumInliers / static_cast<double>(kNumInliers + kNumOutliers),
             opts.min_inlier_fraction)
        << "The *actual* inlier ratio is smaller than the inlier ratio "
           "required for RANSAC to find the solution. Your test is specifying "
           "a scenario in which an algorithm will never succeed!!";
  }
  RansacOptions opts;
  static constexpr int kNumInliers = 10;
  static constexpr int kNumOutliers = 3;
};

INSTANTIATE_TEST_CASE_P(
    RansacTestSuite, RansacTest,
    ::testing::ValuesIn(GenerateRandomModelParams(FLAGS_num_test_cases)));

TEST_P(RansacTest, LeastSquareFitWithoutOutliersAndWithoutNoise) {
  // Verify that RANSAC can be used to fit a line in absence of noise and
  // absence of outliers.
  const Eigen::MatrixXf params = GetParam();
  std::vector<Eigen::MatrixXf> observations =
      CreateObservationCollection(params, /*num_observations=*/kNumInliers);
  auto solution =
      RansacFit(std::make_unique<LinearModel>(), observations, opts);
  ASSERT_TRUE(solution.success);
  EXPECT_THAT(solution.inliers, ::testing::Each(true));
  EXPECT_EQ(solution.num_inliers, observations.size());
  EXPECT_LE(solution.error, 1e-3);
  EXPECT_LE((solution.params - params).norm(), 1e-2);
}

TEST_P(RansacTest, LeastSquareFitWithOutliersAndWithoutNoise) {
  // Verify that RANSAC can be used to fit a line in absence of noise but with
  // some outliers.
  const Eigen::MatrixXf params = GetParam();
  std::vector<Eigen::MatrixXf> observations =
      CreateObservationCollection(params, /*num_observations=*/kNumInliers);
  std::vector<bool> inliers(kNumInliers, true);
  // Add several outliers.
  for (int i = 0; i < kNumOutliers; ++i) {
    observations.push_back(10.0 * Eigen::MatrixXf::Random(2, 1));
    inliers.push_back(false);
  }

  auto solution =
      RansacFit(std::make_unique<LinearModel>(), observations, opts);
  ASSERT_TRUE(solution.success);
  EXPECT_EQ(solution.num_inliers, kNumInliers);
  EXPECT_EQ(solution.inliers, inliers);
  EXPECT_LE(solution.error, 1e-3);
  EXPECT_LE((solution.params - params).norm(), 1e-2);
}

TEST(RansacRunOneIteration, Works) {
  LinearModel model;
  std::vector<Eigen::MatrixXf> observations{
      Eigen::MatrixXf(Eigen::Vector2f(0, 0)),
      Eigen::MatrixXf(Eigen::Vector2f(1, 1)),
      Eigen::MatrixXf(Eigen::Vector2f(2, 2)),
  };
  RansacOptions opts{
      .num_iterations = 0,  // unused
      .inlier_threshold = 0.001,
      .min_inlier_fraction = 0.5,
      .random_seed = 0,  // unused
  };
  auto soln = internal::RansacRunOneIteration(
      observations, std::unordered_set<int>{0, 1}, opts, model);
  ASSERT_TRUE(soln.success);
  EXPECT_NEAR(soln.error, 0.0, 1e-2);
  EXPECT_EQ(soln.num_inliers, 3);
}

TEST(RansacIsSolutionBetter, Works) {
  RansacSolution a{.success = true, .error = 1.0};
  RansacSolution b{.success = true, .error = 10.0};
  ASSERT_TRUE(internal::IsSolutionBetter(a, b));
}

}  // namespace dungeon


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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)
*
*****************************************************************************/

#include <rokko/rokko.hpp>
#include <rokko/mapping_bc.hpp>
#include <rokko/collective.hpp>
#include <rokko/utility/solver_name.hpp>
#include <rokko/utility/frank_matrix.hpp>
#include <rokko/utility/check_orthogonality.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>

typedef rokko::matrix_col_major matrix_major;

template<typename T, typename MATRIX_MAJOR>
void function_matrix(rokko::localized_vector<double> const& eigval_tmp, rokko::distributed_matrix<T, MATRIX_MAJOR> const& eigvec, rokko::distributed_matrix<T, MATRIX_MAJOR>& result, rokko::distributed_matrix<T, MATRIX_MAJOR>& tmp) {
  for (int local_j=0; local_j<eigvec.get_n_local(); ++local_j) {
    int global_j = eigvec.translate_l2g_col(local_j);
    double coeff = eigval_tmp(global_j);
    for (int local_i=0; local_i<eigvec.get_m_local(); ++local_i) {
      double value = eigvec.get_local(local_i, local_j);
      tmp.set_local(local_i, local_j, coeff * value);
    }
  }
  product(1, tmp, false, eigvec, true, 0, result);
}

template<typename T, typename MATRIX_MAJOR>
void diagonalize_fixedB(rokko::parallel_dense_solver& solver, rokko::distributed_matrix<T, MATRIX_MAJOR>& A, rokko::distributed_matrix<T, MATRIX_MAJOR>& B,
			rokko::localized_vector<double>& eigval, rokko::distributed_matrix<T, MATRIX_MAJOR>& eigvec, T tol = 0) {
  rokko::distributed_matrix<double, matrix_major> tmp(A.get_mapping()), Binvroot(A.get_mapping()), mat(A.get_mapping());
  rokko::parameters params;
  int myrank = A.get_myrank();
  params.set("routine", "");
  solver.diagonalize(B, eigval, eigvec, params);
  // computation of B^{-1/2}
  for(int i=0; i<eigval.size(); ++i)
    eigval(i) = (eigval(i) > tol) ? sqrt(1/eigval(i)) : 0;
  function_matrix(eigval, eigvec, Binvroot, tmp);

  // computation of B^{-1/2} A B^{-1/2}
  product(1, Binvroot, false, A, false, 0, tmp);
  product(1, tmp, false, Binvroot, false, 0, mat);
  // diagonalization of B^{-1/2} A B^{-1/2}
  solver.diagonalize(mat, eigval, tmp, params);

  // computation of {eigvec of Ax=lambda Bx} = B^{-1/2} {eigvec of B^{-1/2} A B^{-1/2}}
  product(1, Binvroot, false, tmp, false, 0, eigvec);
}

template<typename T, typename MATRIX_MAJOR>
void set_A_B(rokko::localized_matrix<T, MATRIX_MAJOR>& locA, rokko::localized_matrix<T, MATRIX_MAJOR>& locB) {
  if ((locA.rows() != 4) || (locA.cols() != 4) || (locB.rows() != 4) || (locB.cols() != 4)) {
    std::cerr << "error: size must be 4!" << std::endl;
    throw;
  }
  locA << 0.24, 0.39, 0.42, -0.16,
          0.39, -0.11, 0.79, 0.63,
          0.42, 0.79, -0.25, 0.48,
         -0.16, 0.63, 0.48, -0.03;

  locB << 4.16, -3.12, 0.56, -0.10,
         -3.12, 5.03, -0.83, 1.09,
          0.56, -0.83, 0.76, 0.34,
         -0.10, 1.09, 0.34, 1.18;
}

int main(int argc, char *argv[]) {
  int provided;
  MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
  MPI_Comm comm = MPI_COMM_WORLD;
  std::string library_routine(rokko::parallel_dense_solver::default_solver());
  std::string library, routine;
  int dim = 4;
  if (argc >= 2) library_routine = argv[1];
  rokko::split_solver_name(library_routine, library, routine);

  rokko::grid g(comm);
  int myrank = g.get_myrank();

  std::cout.precision(5);

  rokko::parallel_dense_solver solver(library);
  solver.initialize(argc, argv);
  if (myrank == 0)
    std::cout << "Eigenvalue decomposition of Frank matrix" << std::endl
	      << "library:routine = " << library_routine << std::endl
              << "num_procs = " << g.get_nprocs() << std::endl
              #ifdef _OPENMP
              << "num_threads per process = " << omp_get_max_threads() << std::endl
              #endif
	      << "routine = " << routine << std::endl
              << "dimension = " << dim << std::endl;

  rokko::localized_matrix<double, matrix_major> locA(dim, dim), locB(dim, dim);
  set_A_B(locA, locB);
  if (myrank == 0) std::cout << "locA:" << std::endl << locA << std::endl;

  rokko::mapping_bc<matrix_major> map(dim, g, solver);
  rokko::distributed_matrix<double, matrix_major> A(map), B(map), eigvec(map);
  rokko::localized_vector<double> eigval(dim);
  rokko::scatter(locA, A, 0);
  rokko::scatter(locB, B, 0);
  MPI_Barrier(comm);
  if (myrank == 0) std::cout << "A:" << std::endl;
  std::cout << A << std::endl;

  diagonalize_fixedB(solver, A, B, eigval, eigvec);

  rokko::localized_matrix<double, matrix_major> eigvec_loc(dim, dim);
  rokko::gather(eigvec, eigvec_loc, 0);

  set_A_B(locA, locB);

  if (myrank == 0) {
    bool sorted = true;
    for (unsigned int i = 1; i < dim; ++i) sorted &= (eigval(i-1) <= eigval(i));
    if (!sorted) std::cout << "Warning: eigenvalues are not sorted in ascending order!\n";
    std::cout << "largest eigenvalues:";
    for (int i = 0; i < std::min(dim, 10); ++i) std::cout << ' ' << eigval(dim - 1 - i);
    std::cout << std::endl;

    std::cout << "eigenvalues:\n" << eigval.transpose() << std::endl
	      << "eigvectors:\n" << eigvec_loc << std::endl;
    std::cout << "orthogonality of eigenvectors:" << std::endl
	      << eigvec_loc.transpose() * locB * eigvec_loc << std::endl;
    std::cout << "residual of the smallest eigenvalue/vector (A x - lambda B x):" << std::endl
	      << (locA * eigvec_loc.col(0) - eigval(0) * locB * eigvec_loc.col(0)).transpose() << std::endl;
  }

  solver.finalize();
  MPI_Finalize();
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <eve/detail/diff_div.hpp>
#include <eve/function/diff/gegenbauer.hpp>
#include <boost/math/special_functions/gegenbauer.hpp>
#include <type_traits>

TTS_CASE_TPL("Check diff(gegenbauer) return type", EVE_TYPE)
{
    TTS_EXPR_IS(eve::diff(eve::gegenbauer)((unsigned int)(0), T(), T()), T);
}

TTS_CASE_TPL("Check eve::diff(eve::gegenbauer) behavior", EVE_TYPE)
{
  using elt_t = eve::element_type_t<T>;
  elt_t l(-3.0/8.0);
  auto eve__gegenbauer =  [&l](unsigned n, auto x) { return eve::diff(eve::gegenbauer)(n, T(l), x); };
  auto boost_gegenbauer =  [&l](unsigned n, auto x) { return boost::math::gegenbauer_derivative(n, double(l), x, 1u); };

  for(unsigned int i=0; i < 10; ++i)
  {
    TTS_ULP_EQUAL(eve__gegenbauer(i, T(10)), T(boost_gegenbauer(i, 10.0)), 1);
    TTS_ULP_EQUAL(eve__gegenbauer(i, T(5)), T(boost_gegenbauer(i, 5.0)), 1);
    TTS_ULP_EQUAL(eve__gegenbauer(i, T(2)), T(boost_gegenbauer(i, 2.0)), 1);
    TTS_ULP_EQUAL(eve__gegenbauer(i, T(1)), T(boost_gegenbauer(i, 1.0)), 20);
    TTS_ULP_EQUAL(eve__gegenbauer(i, T(0)), T(boost_gegenbauer(i, 0.0)), 1);
  }
}


#include "prime.hpp"

// Miller-Rabin prime test algorithm.
#include <boost/multiprecision/miller_rabin.hpp>
#include <random>

boost::multiprecision::cpp_int cryptb::prime::gen_random(const int num_bytes, random_engine& engine)
{
	if (num_bytes <= 0)
		throw std::invalid_argument("Error in function \"cryptb::prime::gen_random\"."
			" The argument: \"num_bytes\" <= 0. There is no prime number with that number of bytes.");
	boost::multiprecision::cpp_int candidate;
	// TODO: Use seed_seq here to seed the std::mt19937_64 engine better.
	// Also, don't allocate the std::mt19937_64 engine on the stack because
	// it's more than 1000 bytes long.
	const auto seed = engine.operator()(sizeof(std::mt19937_64::result_type));
	std::mt19937_64 miller_rabin_engine(static_cast<std::mt19937_64::result_type>(seed));
	do
	{
		candidate = engine.operator()(num_bytes);
		// 64 Should be enough. The higher the number of trials, the lower the probability is for a false positive.
		// Note: making this number lower will significantly improve performance.
	} while (boost::multiprecision::miller_rabin_test(candidate, 64, miller_rabin_engine) == false);
	return candidate;
}


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

  Program:   Visualization Toolkit
  Module:    vtkVelodyneHDLReader.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkPlaneFitter.h"

#include "vtkObjectFactory.h"
#include "vtkPointSet.h"
#include "vtkSmartPointer.h"
#include "vtkDoubleArray.h"
#include "vtkThreshold.h"
#include "vtkUnstructuredGrid.h"
#include "vtkNew.h"

#include <Eigen/Dense>

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkPlaneFitter);

//-----------------------------------------------------------------------------
vtkPlaneFitter::vtkPlaneFitter()
{
}

//-----------------------------------------------------------------------------
vtkPlaneFitter::~vtkPlaneFitter()
{
}

//-----------------------------------------------------------------------------
void vtkPlaneFitter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
}

//-----------------------------------------------------------------------------
void vtkPlaneFitter::PlaneFit(vtkPointSet* pts, double origin[3], double normal[3],
                              double &minDist, double &maxDist, double &stdDev,
                              double channelMean[32], double channelStdDev[32],
                              vtkIdType channelNpts[32])
{
  using namespace Eigen;

  vtkSmartPointer<vtkDoubleArray> ptdata = vtkSmartPointer<vtkDoubleArray>::New();
  ptdata->DeepCopy(pts->GetPoints()->GetData());

  const vtkIdType n = ptdata->GetNumberOfTuples();
  if(n < 1)
    {
    return;
    }

  assert(ptdata->GetNumberOfComponents() == 3);
  Map<MatrixXd> eigpointsraw(static_cast<double*>(ptdata->GetVoidPointer(0)),
                             ptdata->GetNumberOfComponents(),
                             ptdata->GetNumberOfTuples());

  MatrixXd eigpoints = eigpointsraw.transpose();

  VectorXd mean(3);

  mean = eigpoints.colwise().sum() / n;
  assert(mean.size() == 3);

  for(int i = 0; i < 3; ++i)
    {
    origin[i] = mean[i];
    }

  eigpoints.rowwise() -= mean.transpose();

  JacobiSVD<MatrixXd> svd(eigpoints, ComputeThinU | ComputeThinV);

  VectorXd enormal = svd.matrixV().col(2);
  assert(enormal.size() == 3);
  assert(std::fabs(enormal.norm() - 1.0) < 1.0e-8);

  for(int i = 0; i < 3; ++i)
    {
    normal[i] = enormal[i];
    }

  VectorXd distances = eigpoints * enormal;
  assert(distances.size() == n);

  minDist = distances.minCoeff();
  maxDist = distances.maxCoeff();

  stdDev = std::sqrt(distances.squaredNorm() / (n-1));

  for(int i = 0; i < 32; ++i)
    {
    vtkNew<vtkThreshold> threshold;
    threshold->ThresholdBetween(i,i);
    threshold->SetInputData(pts);
    threshold->SetInputArrayToProcess(0, 0, 0, vtkDataObject::FIELD_ASSOCIATION_POINTS, "laser_id");
    threshold->SetOutputPointsPrecision(vtkAlgorithm::DEFAULT_PRECISION);
    threshold->Update();

    channelNpts[i] = threshold->GetOutput()->GetNumberOfPoints();

    vtkSmartPointer<vtkDoubleArray> threshdata = vtkSmartPointer<vtkDoubleArray>::New();
    threshdata->DeepCopy(threshold->GetOutput()->GetPoints()->GetData());

    const vtkIdType n = threshdata->GetNumberOfTuples();
    if(n < 2)
      {
      channelMean[i] = 0.0;
      channelStdDev[i] = 0.0;
      continue;
      }

    assert(threshdata->GetNumberOfComponents() == 3);
    assert(threshdata->GetNumberOfTuples() >= 2);
    Map<MatrixXd> channelraw(static_cast<double*>(threshdata->GetVoidPointer(0)),
                             threshdata->GetNumberOfComponents(),
                             threshdata->GetNumberOfTuples());

    MatrixXd channelpts = channelraw.transpose();
    channelpts.rowwise() -= mean.transpose();

    VectorXd channelds = channelpts * enormal;

    double cmean = channelds.sum() / channelds.size();
    double cstddev = std::sqrt((channelds.array() - cmean).matrix().squaredNorm() / (channelds.size()-1));

    channelMean[i] = cmean;
    channelStdDev[i] = cstddev;
    }

}


/*
 supercell.hxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#pragma once

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

namespace ublas = boost::numeric::ublas;

class Supercell {

public:

  Supercell(ublas::vector<long> supercell_extension,
            long primitive_number_of_atoms,
            ublas::matrix<double> primitive_lattice_vectors,
            ublas::matrix<double> primitive_basis_vectors,
	    double concentration);

  ublas::vector<long> supercell_extension;
  long number_of_atoms;
  ublas::matrix<double> lattice_vectors;
  ublas::matrix<double> basis_vectors;
  ublas::vector<long> pointers;

private:

  long primitive_number_of_atoms_;
  ublas::matrix<double> primitive_lattice_vectors_;
  ublas::matrix<double> primitive_basis_vectors_;
  double concentration_;

};


/**
 * @file ind2sub_test.cpp
 * @author Nilay Jain
 *
 * Test the backported Armadillo ind2sub() and sub2ind() functions.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

BOOST_AUTO_TEST_SUITE(ind2subTest);

/**
 * This test checks whether ind2sub and sub2ind are
 * compiled successfully and that they function properly.
 */
BOOST_AUTO_TEST_CASE(ind2sub_test)
{
  arma::mat A = arma::randu(4, 5);
  size_t index = 13;
  arma::uvec u = arma::ind2sub(arma::size(A), index);

  BOOST_REQUIRE_EQUAL(u(0), index % A.n_rows);
  BOOST_REQUIRE_EQUAL(u(1), index / A.n_rows);

  index = arma::sub2ind(arma::size(A), u(0), u(1));
  BOOST_REQUIRE_EQUAL(index, u(0) + u(1) * A.n_rows);
}

BOOST_AUTO_TEST_SUITE_END();


//  (C) Copyright Eric Niebler 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/time_series/characteristic_series.hpp>
#include <boost/time_series/constant_series.hpp>
#include <boost/time_series/dense_series.hpp>
#include <boost/time_series/heaviside_series.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/shifted_series.hpp>
#include <boost/time_series/sparse_series.hpp>
#include <boost/time_series/ordered_inserter.hpp>
#include <boost/time_series/numeric/shift.hpp>

using namespace boost;
using namespace unit_test;

namespace seq = boost::sequence;
namespace rrs = boost::range_run_storage;

using time_series::inf;

std::ptrdiff_t min_ = -inf;
std::ptrdiff_t max_ = inf;

time_series::piecewise_constant_series<int> tmp;
#define BOOST_CHECK_RANGE_RUN_EQUAL(x,y)\
    BOOST_CHECK_EQUAL(x, (boost::time_series::make_ordered_inserter(::tmp) y .commit(), ::tmp))

///////////////////////////////////////////////////////////////////////////////
// test_shift
//
template<typename Series>
void test_shift()
{
    Series base;

    time_series::make_ordered_inserter(base)
        (1, -1, 2)
        (2, 3, 5)
        (3, 6, 9)
    .commit();

    // Sanity check
    BOOST_CHECK_RANGE_RUN_EQUAL(base, (1, -1, 2)(2, 3, 5)(3, 6, 9));

    // Test1: shift right 1
    Series test1 = time_series::shift(base, 1);
    BOOST_CHECK_EQUAL(test1.discretization(), 1);
    BOOST_CHECK_RANGE_RUN_EQUAL(test1, (1, 0, 3)(2, 4, 6)(3, 7, 10));

    // Test2: shift left 1
    Series test2 = time_series::shift(base, -1);
    BOOST_CHECK_EQUAL(test2.discretization(), 1);
    BOOST_CHECK_RANGE_RUN_EQUAL(test2, (1, -2, 1)(2, 2, 4)(3, 5, 8));

    // Test3: shift right 5
    Series test3 = time_series::shift(base, 5);
    BOOST_CHECK_EQUAL(test3.discretization(), 1);
    BOOST_CHECK_RANGE_RUN_EQUAL(test3, (1, 4, 7)(2, 8, 10)(3, 11, 14));

    // Test4: shift left 5
    Series test4 = time_series::shift(base, -5);
    BOOST_CHECK_EQUAL(test4.discretization(), 1);
    BOOST_CHECK_RANGE_RUN_EQUAL(test4, (1, -6, -3)(2, -2, 0)(3, 1, 4));

    // Test5: shift right 4, self-assign
    test3 = time_series::shift(test3, 4);
    BOOST_CHECK_EQUAL(test3.discretization(), 1);
    BOOST_CHECK_RANGE_RUN_EQUAL(test3, (1, 8, 11)(2, 12, 14)(3, 15, 18));

    // Test6: shift left 4, self-assign
    test4 = time_series::shift(test4, -4);
    BOOST_CHECK_EQUAL(test4.discretization(), 1);
    BOOST_CHECK_RANGE_RUN_EQUAL(test4, (1, -10, -7)(2, -6, -4)(3, -3, 0));
}

void test_shift2()
{
    using namespace boost;
    using namespace time_series;

    dense_series<int> d(-2, 8, 5);
    shifted_series<dense_series<int> > sd = shift(d, 5);

    characteristic_series<int> ch(3, 13, 5);
    BOOST_CHECK_EQUAL(ch, sd);

    constant_series<int> c(5);
    shifted_series<constant_series<int> > sc = shift(c, -5);

    BOOST_CHECK_EQUAL(0, std::distance(seq::begin(sc), seq::end(sc)));
    BOOST_CHECK_EQUAL(min_, rrs::offset(rrs::pre_run(sc)));
    BOOST_CHECK_EQUAL(max_, rrs::end_offset(rrs::pre_run(sc)));
    BOOST_CHECK_EQUAL(5, rrs::pre_value(sc));
}

void test_heaviside()
{
    using namespace time_series;

    heaviside_unit_series<int> h(start = 5);
    heaviside_unit_series<int> r(start = 4);
    h = shift(h, -1);

    BOOST_CHECK_EQUAL(h, r);

    make_ordered_inserter(r)(1, 6, inf).commit();
    h = shift(h, 2);

    BOOST_CHECK_EQUAL(h, r);
}

void test_floating_point_shift()
{
    using namespace boost;
    using namespace time_series;

    characteristic_series<int,int,double> ch(3.5, 13.5, 5);
    shifted_series<characteristic_series<int,int,double> > sch = shift(ch, -0.5);

    BOOST_CHECK_EQUAL(0, std::distance(seq::begin(sch), seq::end(sch)));
    BOOST_CHECK_CLOSE(3.0, rrs::offset(rrs::pre_run(sch)),0.1);
    BOOST_CHECK_CLOSE(13.0, rrs::end_offset(rrs::pre_run(sch)),0.1);
    BOOST_CHECK_EQUAL(5, rrs::pre_value(sch));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("shift test");

    void (*pfn_dense)() = &test_shift<time_series::dense_series<int> >;
    test->add(BOOST_TEST_CASE(pfn_dense));

    void (*pfn_sparse)() = &test_shift<time_series::sparse_series<int> >;
    test->add(BOOST_TEST_CASE(pfn_sparse));

    void (*pfn_piecewise_constant)() = &test_shift<time_series::piecewise_constant_series<int> >;
    test->add(BOOST_TEST_CASE(pfn_piecewise_constant));

    test->add(BOOST_TEST_CASE(&test_shift2));
    test->add(BOOST_TEST_CASE(&test_heaviside));
    test->add(BOOST_TEST_CASE(&test_floating_point_shift));

    return test;
}


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE1_SIMD_FUNCTION_SORT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE1_SIMD_FUNCTION_SORT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/shuffle.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/min.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( sort_
                          , (typename A0)
                          , bs::sse_
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      // half-permute
      A0 p0 = shuffle<2,3,0,1>(a0);
      A0 mn = min(a0,p0);
      A0 mx = max(a0,p0);
      // cross vector concatenation and reversal
      A0 minmax = shuffle<0,1,6,7>(mn,mx);
      A0 maxmin = shuffle<1,0,7,6>(mn,mx);

      mn = min(minmax,maxmin);
      mx = max(minmax,maxmin);

      // rearrange partial max/min while keeping min and max in place
         p0 = shuffle<0,2,5,7>(mn,mx);
      A0 p1 = shuffle<0,2,1,3>(p0);

      // Bring sorted min/max in the proper place
      return shuffle<0,1,6,7>(min(p1,p0),max(p1,p0));
    }
  };
} } }

#endif


#pragma once

#include <Eigen/Dense>
#include <StateSpace.hpp>

namespace RRT {

/**
 * @brief A 2d plane with continuous states and no obstacles.
 */
class PlaneStateSpace : public StateSpace {
public:
    PlaneStateSpace(double width, double height)
        : _width(width), _height(height) {}

    Eigen::Vector2d randomState() const {
        return Eigen::Vector2d(drand48() * width(), drand48() * height());
    }

    Eigen::Vector2d intermediateState(const Eigen::Vector2d& source,
                                  const Eigen::Vector2d& target,
                                  double stepSize) const {
        Eigen::Vector2d delta = target - source;
        delta = delta / delta.norm();  //  unit vector

        Eigen::Vector2d val = source + delta * stepSize;
        return val;
    }

    double distance(const Eigen::Vector2d& from, const Eigen::Vector2d& to) const {
        Eigen::Vector2d delta = from - to;
        return sqrtf(powf(delta.x(), 2) + powf(delta.y(), 2));
    }

    /**
     * Returns a boolean indicating whether the given point is within bounds.
     */
    bool stateValid(const Eigen::Vector2d& pt) const {
        return pt.x() >= 0 && pt.y() >= 0 && pt.x() < width() &&
               pt.y() < height();
    }

    double width() const { return _width; }
    double height() const { return _height; }

private:
    double _width, _height;
};

}  // namespace RRT


//Authors: Dario Cattaruzza, Alessandro Abate, Peter Schrammel, Daniel Kroening
//University of Oxford 2016
//This code is based on Komei Fukuda's ccd implementation and as such supplied under the GPL license agreement (see license.txt)

/* dplex.c:  dual simplex method c-code
   written by Komei Fukuda, fukuda@ifor.math.ethz.ch
   Version 0.61, December 1, 1997
*/

/* dplex.c : C-Implementation of the dual simplex method for
   solving an LP: max/min  c^T x subject to  x in P, where
   P= {x :  b - A x >= 0}.
   Please read COPYING (GNU General Public Licence) and
   the manual cddman.tex for detail.
*/

#include "DualSimplex.h"

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>

#include <boost/timer.hpp>

namespace abstract {
template <class scalar>  DualSimplex<scalar>::DualSimplex(const int size,const int dimension)  :
    Tableau<scalar>(size,dimension),
    m_auxiliaryRow(1,m_dimension),
    m_orBlockSize(1)
{}

template <class scalar>
bool DualSimplex<scalar>::load(const MatrixS &faces,const MatrixS &supports,const bool transpose)
{
  this->Conversion=LPMax;
  if (!Tableau<scalar>::load (faces,supports,transpose)) return false;
  m_auxiliaryRow.resize(1,m_dimension);
  m_tableau.coeffRef(m_objectiveRow,0)=0;
  return true;
}

template <class scalar>
bool DualSimplex<scalar>::SelectDualSimplexPivot(const bool Phase1,pivot_t &pivot)
{ /* selects a dual simplex pivot (pivot.row, pivot.col) if the current
     basis is dual feasible and not optimal. If not dual feasible,
     the procedure returns false and m_status=LPSundecided.
     If Phase1=true, the RHS column will be considered as the negative
     of the column of the largest variable (==m_size).  For this case, it is assumed
     that the caller used the auxiliary row (with variable m_size) to make the current
     dictionary dual feasible before calling this routine so that the nonbasic
     column for m_size corresponds to the auxiliary variable.
  */
  refScalar maxrat=0,rat=0;
  scalar val=0;
  scalar rcost[m_dimension];

  pivot.col=0;
  m_status=eUndecided;
  for (int j=1; j<m_dimension; j++){//ignore RHSCol
    rcost[j]=entry(m_objectiveRow,j);
    if (func::isPositive(rcost[j])) {
      //The zero case may cause an overapproximation of an empty set to an m_zero^n size hypercube.
      return false;
    }
  }
  //Dual Feasible.
  pivot.row=Phase1 ? findMaxRow(m_basicVars[m_size]) : findMinRow(RHSCol);
  if (pivot.row<0) {
    m_status=eOptimal;
  }
  else {
    for (int j=1; j<m_dimension; j++){// ignore RHSCol
      val=entry(pivot.row,j);
      if (func::isPositive(val)) {
        //The zero case would result in a pivot move by a value inside the interval of the hyperplane
        rat=func::toUpper(rcost[j]/val);
        if ((pivot.col==0) || (rat > maxrat)){
          maxrat=rat;
          pivot.col=j;
        }
      }
    }
    if (pivot.col>0) return true;
    m_status=eInconsistent;
  }
  return false;
}

template <class scalar>
bool DualSimplex<scalar>::SelectOredPivot(const long rowmax, const Set &noPivotRow, const Set &noPivotCol, pivot_t &pivot)
/* Select a position (pivot) in the matrix X.T such that (X.T)[pivot.row][pivot.col] is nonzero
   The choice is feasible, i.e., not on NopivotRow and NopivotCol, and
   best with respect to the specified roworder
 */
{
  long rtemp;
  Set rowexcluded(noPivotRow);
  scalar Xtemp,Xtemp2;
  scalar Ftemp,Ftemp2;
  for (rtemp=rowmax;rtemp<m_size;rtemp++) {
    rowexcluded.add(rtemp);   /* cannot pivot on any row > rmax */
  }
  while(true) {
    rtemp=-1;
    for (int i=1;i<=m_size;i++) {
      if (!rowexcluded.member(m_tableau.zeroOrder(i))){
        rtemp=m_tableau.zeroOrder(i);
        break;
      }
    }
    if (rtemp>=0) {
      rtemp-=(rtemp%m_orBlockSize);
      pivot.row=rtemp;
      for (pivot.col=0;pivot.col < m_dimension;pivot.col++) {
        if (!noPivotCol.member(pivot.col)) {
          Xtemp=entry(pivot.row,pivot.col);
          Ftemp=Xtemp/entry(pivot.row,0);
          int offset=0;
          for (int j=1;j<m_orBlockSize;j++) {
            Xtemp2=entry(pivot.row+j,pivot.col);
            Ftemp2=Xtemp2/entry(pivot.row+j,0);
            if (func::isPositive(Ftemp2-Ftemp)) {
              offset=j;
              Xtemp=Xtemp2;
              Ftemp=Ftemp2;
            }
          }
          pivot.row+=offset;
          char sign=func::softSign(Xtemp);//hardSign(Xtemp);//Zero is ensured by check in entry
          if (sign<0) return true;
        }
      }
      for (int i=0;i<m_orBlockSize;i++) rowexcluded.add(rtemp+i);
    }
    else {
      pivot.row = -1;
      pivot.col = -1;
      return false;
    }
  }
  return true;
}

template <class scalar>
void DualSimplex<scalar>::AuxiliaryPivotAndUpdate(long col)
{
  MatrixS Rtemp=m_auxiliaryRow*m_basisInverse;
  char sign=func::hardSign(Rtemp.coeff(0,col));
  if (sign==0) return;
  scalar Xtemp;
  refScalar Xtemp0 = func::toCentre(Rtemp.coeff(0,col));
  for (int j = 0; j < m_dimension; j++) {
    if (j != col) {
      Xtemp = Rtemp.coeff(0,j) / Xtemp0;
      for (int j1 = 0; j1 < m_dimension; j1++)
        func::msub(m_basisInverse.coeffRef(j1,j),m_basisInverse.coeff(j1,col),Xtemp); //m_basisInverse.coeffRef(j1,j) -= m_basisInverse.coeff(j1,col) * Xtemp;
    }
  }
  m_basisInverse.col(col) /= Xtemp0;
  if (ms_trace_tableau>eTraceTableau) this->logBasis(m_size,col);
  long entering=m_nonBasicRow[col];
  m_basicVars[m_size]=col;              // the nonbasic variable r corresponds to column s
  m_nonBasicRow[col]=m_size;            // the nonbasic variable on s column is r
  if (entering>=0) m_basicVars[entering]=-1; // original variables have negative index and should not affect the row index
}

// Find the corresponding row with the minimum entry value for a given column
template <class scalar>
int DualSimplex<scalar>::findMinRow(const int col)
{
  int row=-1;
  refScalar minval=0;
  refScalar val;
  for (int i=0; i<m_objectiveRow; i++) {
    if (m_basicVars[i]<0) {  /* i is a basic variable */
      val=func::toUpper(entry(i,col)); // for dual Phase I (auxiliary row is non-basic)
      if (val < minval) {
        row=i;
        minval=val;
      }
    }
  }
  return row;
}

// Find the corresponding row with the minimum entry value for a given column
template <class scalar>
int DualSimplex<scalar>::findMaxRow(const int col)
{
  int row=-1;
  refScalar maxval=0;
  refScalar val;
  for (int i=0; i<m_objectiveRow; i++) {
    if (m_basicVars[i]<0) {  /* i is a basic variable */
      // for dual Phase I (auxiliary row is non-basic)
      val=func::toLower(entry(i,col));
      if (val > maxval) {
        row=i;
        maxval=val;
      }
    }
  }
  return row;
}

template <class scalar>
int DualSimplex<scalar>::FindDualFeasibleBasis()
{ /* Find a dual feasible basis using Phase I of Dual Simplex method.
     If the problem is dual feasible,
     the procedure returns m_status=LPSundecided and a dual feasible
     basis.   If the problem is dual infeasible, this returns
     m_status=DualInconsistent and the evidence column.
  */

  long rank=0;
  pivot_t pivot;

  m_status=eUndecided; this->m_evidenceCol=-1;
  if (ms_trace_tableau>eTraceTableau) this->logBasis(m_objectiveRow,-1);
  scalar maxcost=-1;
  int maxReducedCostCol=0;  /* ms will be the index of column which has the largest reduced cost */
  for (int col=1; col<m_tableau.cols(); col++){//ignore RHSCol
    scalar cost=entry(m_objectiveRow,col);
    if (func::toLower(cost) > func::toUpper(maxcost)) {maxcost=cost; maxReducedCostCol = col;}//TODO:might want to check for imprecision
  }
  if (ms_trace_tableau>=eTracePivots) {
    std::stringstream buffer;
    buffer << "Dual feasible Basis. cost=" << ms_logger.MakeNumber(maxcost) << ",c=" << maxReducedCostCol;
    ms_logger.logData(buffer.str());
  }
  if (!func::isPositive(maxcost)) return rank;//Dual feasible
  //The zero case above indicates we are somewhere on the hyperplane which is feasible (or an m_zero overapprox)
  m_auxiliaryRow=MatrixS::Zero(1,m_dimension);
  for (int k=1; k<m_dimension; k++) {
    if (m_nonBasicRow[k]>=0) {
      m_auxiliaryRow-=m_tableau.row(m_nonBasicRow[k]);/* To make the auxiliary row (0,-1,-1,...,-1).  */
    }
  }
  if (ms_trace_tableau>eTracePivots) {
    ms_logger.logData(m_auxiliaryRow,"Auxiliary Row:");
    if (ms_trace_tableau>=eTraceEntries) {
      this->logNonBasic();
      this->logBasic();
      MatrixS matrix=m_auxiliaryRow*m_basisInverse;
      ms_logger.logData(matrix,"Entries");
    }
  }

  /* Pivot on (m_auxiliaryRow, maxReducedCostCol) so that the dual basic solution becomes feasible */
  AuxiliaryPivotAndUpdate(maxReducedCostCol);
  rank++;

  m_status=eUndecided;/* Dual Simplex Phase I */
  while (SelectDualSimplexPivot(true, pivot))  {
    this->ColumnPivotAndUpdate(pivot);
    rank++;
    if (m_basicVars[m_size]<0) return rank;
  }
  /* The current dictionary is terminal.  There are two cases:
     TableauEntry(m_objectiveRow,maxReducedCostCol) is negative or zero.
     The first case implies dual infeasible,
     and the latter implies dual feasible but m_size is still in nonbasis.
     We must pivot in the auxiliary variable m_size. */

  pivot.row=findMinRow(maxReducedCostCol);
  pivot.col=maxReducedCostCol;
  if (pivot.row>=0) {
    this->ColumnPivotAndUpdate(pivot);
    rank++;
  }
  if (func::isNegative(entry(m_objectiveRow, pivot.col))) {
    m_status=eDualInconsistent;
    this->m_evidenceCol=maxReducedCostCol;
  }
  return rank;
}

template <class scalar>
scalar DualSimplex<scalar>::maximise(const std::vector<scalar> &vector,const ResetType_t resetType)
{
  if (vector.size()<this->getDimension()) return func::ms_nan;
  for (int col=1;col<m_tableau.cols();col++) {
    m_tableau.coeffRef(m_objectiveRow,col)=vector.at(col-1);//TODO: check the sign
  }
  return processMaximize(resetType);
}

template <class scalar>
scalar DualSimplex<scalar>::maximise(const MatrixS &vector,const ResetType_t resetType)
{
  m_tableau.block(m_objectiveRow,1,1,vector.cols())=vector;//TODO: check the sign
  return processMaximize(resetType);
}

template <class scalar>
bool DualSimplex<scalar>::maximiseAll(const MatrixS &vectors, MatrixS &supports,AproxType_t aprox)
{
  boost::timer timer;
  try {
    if (aprox==eOverAprox)       this->toOuter();
    else if (aprox==eUnderAprox) this->toInner();
    if ((supports.rows()!=vectors.cols()) || (supports.cols()!=1)) supports.resize(vectors.cols(),1);
    FindFeasBasis(eResetBasis);
    for (int i=0;i<vectors.cols();i++) {
      m_tableau.block(m_objectiveRow,1,1,vectors.rows())=vectors.block(0,i,vectors.rows(),1).transpose();//TODO: check sign
      supports.coeffRef(i,0)=processMaximize(eUseDefaultBasis);
    }
  }
  catch(std::string error) {
    ms_logger.logData(error);
    return false;
  }
  if (this->ms_trace_time) {
    int elapsed=timer.elapsed()*1000;
    if (elapsed>0) {
      ms_logger.logData(this->getName(),false);
      ms_logger.logData(elapsed," Maximise time",true);
    }
  }
  return true;
}

template <class scalar>
scalar DualSimplex<scalar>::processMaximize(const ResetType_t resetType)
/*
When LP is inconsistent then *re returns the evidence row.
When LP is dual-inconsistent then *se returns the evidence column.
*/
{
  boost::timer timer;
  long rank=0;
  long maxpivfactor=70;
  pivot_t pivot;
  this->Error=this->None;
  func::setZero(this->m_zero);
  long maxpivots=maxpivfactor*m_dimension;  // maximum pivots to be performed before cc pivot is applied.
  long rebasepivots=((func::getDefaultPrec()>>6)+1)*m_dimension;
  /* Initializing control variables. */

  this->m_evidenceRow=-1;
  this->m_evidenceCol=-1;

  if (resetType!=eRebaseBasis) m_iterations=0;

  if (ms_trace_tableau>=eTracePivots) {
    ms_logger.logData(m_tableau,"Maximise");
  }

  m_iterations+=this->FindFeasBasis(resetType);
  if (this->ms_trace_time && (ms_trace_tableau>=eTracePivots) && (resetType!=eUseDefaultBasis)) {
    logPivotCount(timer.elapsed()*1000,"Find Feasible:");
  }
  m_status=eUndecided;
  if (this->m_evidenceCol<0) m_iterations+=FindDualFeasibleBasis();
  if ((this->ms_trace_time) && (ms_trace_tableau>=eTracePivots)) {
    logPivotCount(timer.elapsed()*1000,"Find DualFeasible:");
  }

  if (this->m_evidenceCol>=0){
    if (m_status==eUndecided) m_status=eStrucDualInconsistent;// No LP basis is found, and thus Inconsistent.
    // else No dual feasible basis is found, and thus DualInconsistent.
    return entry( m_objectiveRow,RHSCol);
  }

  if (ms_trace_tableau>=eTracePivots) ms_logger.logData("LP max");

  /* Dual Simplex Method */
  while(true) {
    m_status=eUndecided;
    if (rank>rebasepivots) {// && (func::toWidth(entry(m_objectiveRow,RHSCol))*m_dimension*m_dimension>m_zero)) {
      return processMaximize(eRebaseBasis);
    }
    if ((rank<maxpivots) && SelectDualSimplexPivot(false, pivot)) {
      this->ColumnPivotAndUpdate(pivot);
      rank++;
    }
    else if ((m_status==eUndecided) && SelectCrissCrossPivot(pivot)) {
      /* In principle this should not be executed because we already have dual feasibility
         attained and dual simplex pivot should have been chosen.  This might occur
         under floating point computation, or the case of cycling.
      */
      this->ColumnPivotAndUpdate(pivot);
      maxpivots+=maxpivfactor*m_dimension;
      rank++;
    }
    else {
      switch (m_status) {
        case eInconsistent: this->m_evidenceRow=pivot.row;
        case eDualInconsistent: this->m_evidenceCol=pivot.col;
        default: break;
      }
      break;
    }
  }
  m_iterations+=rank;
  if ((this->ms_trace_time) && (ms_trace_tableau>=eTracePivots)) logPivotCount(timer.elapsed()*1000,"Find Support:");
  scalar result=entry( m_objectiveRow,RHSCol);
  if (func::isNan(result))
    return result;
  return result;
}

template <class scalar>
int DualSimplex<scalar>::FindFeasBasis(const ResetType_t resetType)
{
  m_tableau.coeffRef(m_objectiveRow,RHSCol)=0;
  if (resetType==eUseDefaultBasis) {
    m_basisInverse=m_feasBasisInverse;
    m_basicVars=m_feasBasicVars;
    m_nonBasicRow=m_feasNonBasicRow;
    if (ms_trace_tableau>=eTraceTableau) this->logBasis(-1,-1);
    return 0;
  }
  else if (resetType==eRebaseBasis) {
    return this->Rebase();
  }
  this->ComputeRowOrderVector(MinIndex);
  int result=this->FindLPBasis();//TODO: go to feasLP?
  m_feasBasisInverse=m_basisInverse;
  m_feasBasicVars=m_basicVars;
  m_feasNonBasicRow=m_nonBasicRow;
  return result;
}

template <class scalar>
int DualSimplex<scalar>::FindFeasOrBasis(int orBlockSize,const ResetType_t resetType)
{
  normalise(true);
  m_orBlockSize=orBlockSize;
  m_tableau.coeffRef(m_objectiveRow,RHSCol)=0;
  if (resetType==eUseDefaultBasis) {
    m_basisInverse=m_feasBasisInverse;
    m_basicVars=m_feasBasicVars;
    m_nonBasicRow=m_feasNonBasicRow;
    if (ms_trace_tableau>=eTraceTableau) this->logBasis(-1,-1);
    return 0;
  }
  else if (resetType==eRebaseBasis) {
    return this->Rebase();
  }
  this->ComputeRowOrderVector(MinIndex);
  int result=this->FindOrLPBasis();
  m_feasBasisInverse=m_basisInverse;
  m_feasBasicVars=m_basicVars;
  m_feasNonBasicRow=m_nonBasicRow;
  return result;
}

template <class scalar>
int DualSimplex<scalar>::FindOrLPBasis()
{ /* Find a LP basis using Gaussian pivots.
     If the problem has an LP basis,
     the procedure returns m_evidenceCol=-1 if LPSundecided and an LP basis.
     If the constraint matrix A (excluding the rhs and objective) is not
     column indepent, there are two cases.  If the dependency gives a dual
     inconsistency, this returns the evidence column m_evidenceCol.  Otherwise, this returns an LP basis of size less than n_size.  Columns j
     that do not belong to the basis (i.e. cannot be chosen as pivot because
     they are all zero) will be indicated in nbindex vector: nbindex[j] will
     be negative and set to -j.
  */
  if (ms_trace_tableau>=eTracePivots) ms_logger.logData("Ored Feasibility Basis");
  ResetTableau();
  Set RowSelected(m_size);
  Set ColSelected(m_dimension);
  RowSelected.add(m_objectiveRow);
  ColSelected.add(RHSCol);
  pivot_t pivot;
  int rank=m_dimension;
  m_evidenceCol=-1;
  for (int i=0;i<m_dimension;i++) {   /* Find a set of rows for a basis */
    if (!SelectOredPivot(m_size, RowSelected, ColSelected, pivot))
    {
      rank=i;
      for (int j=1;j<m_dimension; j++) {//Skip RHSCol
        if (m_nonBasicRow[j]<0){
          if (!func::isZero(entry(m_objectiveRow,j),m_zero)) {  /* dual inconsistent */
            m_evidenceCol=j;
            break;
          }
        }
      }
      /* dependent columns but not dual inconsistent. */
      break;
    }
    if (ms_trace_tableau>=eTraceEntries) {
      RowSelected.logSet("Available Rows:",true);
      ColSelected.logSet("Available Cols:",true);
    }
    RowSelected.add(pivot.row);
    ColSelected.add(pivot.col);
    ColumnPivotAndUpdate(pivot);
  }
  return rank;
}

template <class scalar>
bool DualSimplex<scalar>::SelectCrissCrossPivot(pivot_t &pivot)
{
  m_status=eUndecided;
  for (int i=0; i<m_size; i++) {
    if (i!=m_objectiveRow && m_basicVars[i]==-1) {  /* i is a basic variable */
      if (func::isNegative(entry(i,RHSCol))) {
        //The zero case above indicates we are somewhere on the hyperplane which is feasible (or an m_zero overapprox)
        pivot.row=i;
        for (int j=0; j<m_size; j++) {
          if (m_basicVars[j] >0) { /* i is nonbasic variable */
             if (func::isPositive(entry(pivot.row,m_basicVars[j]))) {
               pivot.col=m_basicVars[j];
               return true;
             }
          }
        }
        m_status=eInconsistent;
        return false;
      }

    }
    else if (m_basicVars[i] >0) { /* i is nonbasic variable */
      if (func::isPositive(entry(m_objectiveRow,m_basicVars[i]))) {
        //The zero case above indicates we are somewhere on the hyperplane which is feasible (or an m_zero overapprox)
        pivot.col=m_basicVars[i];
        for (int j=0; j<m_size; j++) {
          if (j!=m_objectiveRow && m_basicVars[j]==-1) {  /* i is a basic variable */
            if (func::isNegative(entry(j,pivot.col))) {
              pivot.row=j;
              return true;
            }
          }
        }
        m_status=eDualInconsistent;
        return false;
      }
    }
  }
  m_status=eOptimal;
  return false;
}

/// Normalises the directions of the faces
template <class scalar>
void DualSimplex<scalar>::normalise(bool reload)
{
  if (this->m_isNormalised || m_faces.rows()<=0) return;
  MatrixS norms=m_faces.rowwise().norm();
  for (int i=0;i<m_faces.rows();i++) {
    char sign=func::hardSign(norms.coeff(i));
    if (sign!=0) {
      for (int j=0;j<m_faces.cols();j++) m_faces.coeffRef(i,j)/=norms.coeff(i);
      m_supports.coeffRef(i,0)=m_supports.coeff(i,0)/norms.coeff(i);
    }
  }
  this->m_isNormalised=true;
  if (reload) load(m_faces,m_supports);
}

/// Clears redundant faces in the polyhedra (caused by intersections and reductions)
template <class scalar>
bool DualSimplex<scalar>::removeRedundancies()
{
  int rows=m_faces.rows();
  if (rows<=0) return false;
  bool isRedundant[rows];
  for (int i=0;i<m_faces.rows();i++) isRedundant[i] =true;
  int redundant=m_faces.rows();
  this->m_isNormalised=false;
  for (int row=0;row<m_faces.rows();row++) {
    for (int col=0;col<m_faces.cols();col++) {
      char sign=func::hardSign(m_faces.coeff(row,col));
      if (sign!=0) {
        isRedundant[row]=false;
        redundant--;
        break;
      }
    }
  }
  normalise(false);
  SortedMatrix<scalar> faces(m_faces.rows(),m_faces.cols());
  faces.block(0,0,m_faces.rows(),m_faces.cols())=m_faces;
  faces.ComputeRowOrderVector(LexMin);
  for (int i=1;i<=m_faces.rows();i++)
  {
    int row=faces.zeroOrder(i);
    if (isRedundant[row]) continue;
    int count=m_faces.rows();
    for (int j=i+1;j<=count;j++) {
      int row2=faces.zeroOrder(j);
      if (isRedundant[row2]) continue;
      MatrixS check=m_faces.row(row)-m_faces.row(row2);
      char sign=func::hardSign(check.norm());
      if (sign==0) {
        isRedundant[row2]=true;//TODO: should aggregate width(error) on non-redundant vector
        if (func::isNegative(m_supports.coeff(row2,0)-m_supports.coeff(row,0))) {
          isRedundant[row2]=false;
          isRedundant[row]=true;
          i=j;
          row=row2;
        }
        redundant++;
      }
      else break;
    }
  }

/*  for (int i=0;i<m_faces.rows();i++)
  {
    if (isRedundant[i]) continue;
    int count=m_faces.rows();
    for (int j=i+1;j<count;j++) {
      if (isRedundant[j]) continue;
      MatrixS check=m_faces.row(i)-m_faces.row(j);
      char sign=func::hardSign(check.norm());
      if (sign==0) isRedundant[j]=true;
      if (isRedundant[j]) {
        if (m_supports.coeff(j,0)<m_supports.coeff(i,0)) {
          isRedundant[j]=false;
          isRedundant[i++]=true;
          while ((i<count) && isRedundant[i]) i++;
          j=i+1;
        }
        redundant++;
      }
    }
  }*/
  if (redundant>0) {
    int pos=0;
    for (int i=0;i<m_faces.rows();i++) {
      if (isRedundant[i]) continue;
      m_faces.row(pos)=m_faces.row(i);
      m_supports.coeffRef(pos,0)=m_supports.coeff(i,0);
      pos++;
    }
    m_faces.conservativeResize(pos,m_faces.cols());
    m_supports.conservativeResize(pos,1);
    load(m_faces,m_supports);
    return true;
  }
  load(m_faces,m_supports);
  return false;
}

/// Saves the time and iteration count data for the given process
template <class scalar>
void DualSimplex<scalar>::logPivotCount(int time,std::string process)
{
  ms_logger.logData(time,process,true);
  ms_logger.logData(m_iterations,"Iterations:",true);
}

#ifdef USE_LDOUBLE
  #ifdef USE_SINGLES
    template class DualSimplex<long double>;
  #endif
  #ifdef USE_INTERVALS
    template class DualSimplex<ldinterval>;
  #endif
#endif
#ifdef USE_MPREAL
  #ifdef USE_SINGLES
    template class DualSimplex<mpfr::mpreal>;
  #endif
  #ifdef USE_INTERVALS
    template class DualSimplex<mpinterval>;
  #endif
#endif

}


#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

//declaration of the stencil kinEnergy
BZ_DECLARE_STENCIL2(kinEnergy,A,B)
B=Laplacian3D_stencilop(A);
BZ_END_STENCIL_WITH_SHAPE(shape(-1,-1,-1),shape(+1,+1,+1))

typedef Array<complex<double>,3> array3d;

int main()
{
const int N=5;
array3d A(N,N,N);
array3d B(N,N,N);
// Fill a three-dimensional array with a Gaussian function
firstIndex i;
secondIndex j;
thirdIndex k;
float midpoint = 3.;
float c = - 1.;
//A = exp(c * (sqr(i-midpoint) + sqr(j-midpoint)
//    + sqr(k-midpoint)));
A = zip( exp(c * (sqr(i-midpoint) + sqr(j-midpoint)
    + sqr(k-midpoint))), 0.0, complex<double>());

applyStencil(kinEnergy(),A,B);

Array<complex<double>,1> a_view(A.data(),shape(N*N*N));
cout << a_view;
Array<complex<double>,1> out_view(B.data(),shape(N*N*N));
cout << out_view<<endl;
}


#include <iostream>
#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>
#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/IO/WKT.h>

namespace PS = CGAL::Polyline_simplification_2;

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Polygon_2<K>    Polygon_2;
typedef CGAL::Polygon_with_holes_2<K>    Polygon_with_holes_2;
typedef PS::Vertex_base_2<K>  Vb;
typedef CGAL::Constrained_triangulation_face_base_2<K> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb> TDS;
typedef CGAL::Exact_predicates_tag                          Itag;
typedef CGAL::Constrained_Delaunay_triangulation_2<K,TDS, Itag> CDT;
typedef CGAL::Constrained_triangulation_plus_2<CDT>     CT;
typedef CT::Point                           Point;
typedef CT::Constraint_id                   Constraint_id;
typedef CT::Constraint_iterator             Constraint_iterator;
typedef CT::Vertices_in_constraint_iterator Vertices_in_constraint_iterator;
typedef CT::Points_in_constraint_iterator   Points_in_constraint_iterator;
typedef PS::Stop_below_count_ratio_threshold Stop;
typedef PS::Squared_distance_cost Cost;

void print(const CT& ct, Constraint_id cid)
{
  std::cout << "simplified polyline" <<std::endl;
  for(Vertices_in_constraint_iterator vit =
        ct.vertices_in_constraint_begin(cid);
      vit != ct.vertices_in_constraint_end(cid);
      ++vit){
    std::cout << (*vit)->point() << std::endl ;
  }

  std::cout << "original points" <<std::endl;
  for(Points_in_constraint_iterator pit =
        ct.points_in_constraint_begin(cid);
      pit != ct.points_in_constraint_end(cid);
      ++pit){
    std::cout << *pit << std::endl ;
  }

}

int main(int argc, char* argv[])
{
  std::ifstream ifs( (argc==1)?"data/polygon.wkt":argv[1]);
  const bool remove_points = false;
  CT ct;
  Polygon_with_holes_2 P;
  Constraint_id cid;
  std::size_t largest = 0;
  while(CGAL::IO::read_polygon_WKT(ifs, P)){
    const Polygon_2& poly = P.outer_boundary();
    Constraint_id cid2 = ct.insert_constraint(poly);
    if(poly.size() > largest){
      cid = cid2;
    }
  }

  PS::simplify(ct, cid, Cost(), Stop(0.5), remove_points);
  print(ct, cid);
  PS::simplify(ct, cid, Cost(), Stop(0.5), remove_points);
  ct.remove_points_without_corresponding_vertex(cid);
  print(ct, cid);
  return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_CONSTANTS_HPP
#define STAN_MATH_PRIM_SCAL_FUN_CONSTANTS_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/inv.hpp>
#include <boost/math/constants/constants.hpp>
#include <limits>

namespace stan {
namespace math {

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

/**
 * The base of the natural logarithm,
 * \f$ e \f$.
 */
const double E = boost::math::constants::e<double>();

/**
 * The value of the square root of 2,
 * \f$ \sqrt{2} \f$.
 */
const double SQRT_2 = sqrt(2.0);

/**
 * The value of 1 over the square root of 2,
 * \f$ 1 / \sqrt{2} \f$.
 */
const double INV_SQRT_2 = inv(SQRT_2);

/**
 * The natural logarithm of 2,
 * \f$ \log 2 \f$.
 */
const double LOG_2 = log(2.0);

/**
 * The natural logarithm of 10,
 * \f$ \log 10 \f$.
 */
const double LOG_10 = log(10.0);

/**
 * Positive infinity.
 */
const double INFTY = std::numeric_limits<double>::infinity();

/**
 * Negative infinity.
 */
const double NEGATIVE_INFTY = -INFTY;

/**
 * (Quiet) not-a-number value.
 */
const double NOT_A_NUMBER = std::numeric_limits<double>::quiet_NaN();

/**
 * Smallest positive value.
 */
const double EPSILON = std::numeric_limits<double>::epsilon();

/**
 * Largest negative value (i.e., smallest absolute value).
 */
const double NEGATIVE_EPSILON = -EPSILON;

/**
 * Largest rate parameter allowed in Poisson RNG
 */
const double POISSON_MAX_RATE = pow(2.0, 30);

/**
 * Return the value of pi.
 *
 * @return Pi.
 */
inline double pi() { return boost::math::constants::pi<double>(); }

/**
 * Return the base of the natural logarithm.
 *
 * @return Base of natural logarithm.
 */
inline double e() { return E; }

/**
 * Return the square root of two.
 *
 * @return Square root of two.
 */
inline double sqrt2() { return SQRT_2; }

/**
 * Return natural logarithm of ten.
 *
 * @return Natural logarithm of ten.
 */
inline double log10() { return LOG_10; }

/**
 * Return positive infinity.
 *
 * @return Positive infinity.
 */
inline double positive_infinity() { return INFTY; }

/**
 * Return negative infinity.
 *
 * @return Negative infinity.
 */
inline double negative_infinity() { return NEGATIVE_INFTY; }

/**
 * Return (quiet) not-a-number.
 *
 * @return Quiet not-a-number.
 */
inline double not_a_number() { return NOT_A_NUMBER; }

/**
 * Returns the difference between 1.0 and the next value
 * representable.
 *
 * @return Minimum positive number.
 */
inline double machine_precision() { return EPSILON; }

const double SQRT_PI = sqrt(pi());

const double SQRT_2_TIMES_SQRT_PI = SQRT_2 * SQRT_PI;

const double TWO_OVER_SQRT_PI = 2.0 / SQRT_PI;

const double NEG_TWO_OVER_SQRT_PI = -TWO_OVER_SQRT_PI;

const double SQRT_TWO_PI = sqrt(2.0 * pi());

const double INV_SQRT_TWO_PI = inv(SQRT_TWO_PI);

const double LOG_PI = log(pi());

const double LOG_PI_OVER_FOUR = LOG_PI / 4.0;

const double LOG_SQRT_PI = log(SQRT_PI);

const double LOG_ZERO = log(0.0);

const double LOG_HALF = log(0.5);

const double NEG_LOG_TWO = -LOG_2;

const double NEG_LOG_SQRT_TWO_PI = -log(SQRT_TWO_PI);

const double NEG_LOG_PI = -LOG_PI;

const double NEG_LOG_SQRT_PI = -LOG_SQRT_PI;

const double NEG_LOG_TWO_OVER_TWO = -LOG_2 / 2.0;

const double LOG_TWO_PI = LOG_2 + LOG_PI;

const double NEG_LOG_TWO_PI = -LOG_TWO_PI;

const double LOG_EPSILON = log(EPSILON);
}  // namespace math
}  // namespace stan

#endif


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COLORS_MATRIX_FORM_PRIMARIES_HPP
#define PIC_COLORS_MATRIX_FORM_PRIMARIES_HPP

#include "../base.hpp"

#ifndef PIC_DISABLE_EIGEN
    #ifndef PIC_EIGEN_NOT_BUNDLED
        #include "../externals/Eigen/Dense"
        #include "../externals/Eigen/QR"
    #else
        #include <Eigen/Dense>
        #include <Eigen/QR>
    #endif
#endif

namespace pic {

/**
 * @brief createMatrixFromPrimaries computes a matrix for converting XYZ values into the
 * defined color space (i.e., by defining the three primaries: red, green, and blue).
 * @param red_XYZ is the XYZ values of the red primary
 * @param green_XYZ is the XYZ values of the green primary
 * @param blue_XYZ is the XYZ values of the blue primary
 * @param white_point_XYZ is the XYZ values of the white point primary
 * @return It returns a 3x3 matrix for converting XYZ values into the defined color space
 */
float *createMatrixFromPrimaries(float *red_XYZ,
                                 float *green_XYZ,
                                 float *blue_XYZ,
                                 float *white_point_XYZ,
                                 float *ret = NULL
                                 )
{
    if(red_XYZ == NULL || green_XYZ == NULL || blue_XYZ == NULL) {
        return ret;
    }

    if(ret == NULL) {
        ret = new float[9];
    }

#ifndef PIC_DISABLE_EIGEN

    int w = 0;
    if(white_point_XYZ != NULL) {
        w = 3;
    }

    //set up a liner system A x = b
    int nRow = 9 + w;
    Eigen::MatrixXf A(nRow, 9);
    Eigen::VectorXf b(nRow);

    //A matrix
    A.setZero();

    //red
    for(int j = 0; j < 3; j++) {
        for(int i = 0 ; i < 3; i++) {
            A(j, j * 3 + i) = red_XYZ[i];
        }
    }

    //green`
    for(int j = 0; j < 3; j++) {
        for(int i = 0 ; i < 3; i++) {
            A(j + 3, j * 3 + i) = green_XYZ[i];
        }
    }

    //blue`
    for(int j = 0; j < 3; j++) {
        for(int i = 0 ; i < 3; i++) {
            A(j + 6, j * 3 + i) = blue_XYZ[i];
        }
    }

    //white
    if(w == 3) {
        for(int j = 0; j < 3; j++) {
            for(int i = 0 ; i < 3; i++) {
                A(j + 9, j * 3 + i) = white_point_XYZ[i];
            }
        }
    }

    //b vector
    b(0) = 1.0f;
    b(1) = 0.0f;
    b(2) = 0.0f;

    b(3) = 0.0f;
    b(4) = 1.0f;
    b(5) = 0.0f;

    b(6) = 0.0f;
    b(7) = 0.0f;
    b(8) = 1.0f;

    if(w == 3) {
        b(9) = 1.0f;
        b(10) = 1.0f;
        b(11) = 1.0f;
    }

    //solve Ax=b
    Eigen::VectorXf x = A.colPivHouseholderQr().solve(b);

    for(int i = 0; i < 9; i++) {
        ret[i] = x(i);
    }
#endif
    return ret;
}

} // end namespace pic

#endif /* PIC_COLORS_MATRIX_FORM_PRIMARIES_HPP */


#include <Eigen/Core>
#include <aslam/common/pose-types.h>

#include "maplab-common/interpolation-helpers.h"
#include "maplab-common/test/testing-entrypoint.h"

namespace common {

TEST(MaplabCommon, TestEigenInterpolation) {
  Eigen::Vector3d vector_A;
  vector_A << 1, 2, 3;
  constexpr double kTimeA = 100.0;

  Eigen::Vector3d vector_B;
  vector_B << 4, 5, 6;
  constexpr double kTimeB = 200.0;

  const double interpolation_time = 150.0;

  Eigen::Vector3d interpolated_vector;
  linearInterpolation(
      kTimeA, vector_A, kTimeB, vector_B, interpolation_time,
      &interpolated_vector);

  Eigen::Vector3d interpolated_vector_ground_truth;
  interpolated_vector_ground_truth << 2.5, 3.5, 4.5;

  EXPECT_EQ(interpolated_vector_ground_truth, interpolated_vector);

  // Test boundaries.
  linearInterpolation(
      kTimeA, vector_A, kTimeB, vector_B, kTimeA, &interpolated_vector);
  EXPECT_EQ(vector_A, interpolated_vector);
  linearInterpolation(
      kTimeA, vector_A, kTimeB, vector_B, kTimeB, &interpolated_vector);
  EXPECT_EQ(vector_B, interpolated_vector);
}

TEST(MaplabCommon, TestGenericInterpolation) {
  constexpr char kCharA = ' ';
  constexpr int kTimeA = 32;

  constexpr char kCharB = '~';
  constexpr int kTimeB = 126;

  constexpr int kInterpolationTime = 58;

  char interpolated_char;
  linearInterpolation(
      kTimeA, kCharA, kTimeB, kCharB, kInterpolationTime, &interpolated_char);

  constexpr char kInterpolatedCharGroundTruth = ':';
  EXPECT_EQ(kInterpolatedCharGroundTruth, interpolated_char);

  // Test boundaries.
  linearInterpolation(
      kTimeA, kCharA, kTimeB, kCharB, kTimeA, &interpolated_char);
  EXPECT_EQ(kCharA, interpolated_char);
  linearInterpolation(
      kTimeA, kCharA, kTimeB, kCharB, kTimeB, &interpolated_char);
  EXPECT_EQ(kCharB, interpolated_char);
}

TEST(MaplabCommon, TestRotationInterpolation) {
  const double kSqrt2DividedBy2 = std::sqrt(2) / 2.0;
  aslam::Quaternion quat_A(kSqrt2DividedBy2, 0, kSqrt2DividedBy2, 0);
  quat_A.normalize();

  constexpr double kTimeA = 100.0;

  aslam::Quaternion quat_B(-kSqrt2DividedBy2, 0, kSqrt2DividedBy2, 0);
  quat_B.normalize();

  constexpr double kTimeB = 200.0;

  constexpr double kInterpolationTime = 150.0;

  aslam::Quaternion interpolated_quat;
  interpolateRotation(
      kTimeA, quat_A, kTimeB, quat_B, kInterpolationTime, &interpolated_quat);

  aslam::Quaternion interpolated_quat_ground_truth(0.0, 0.0, 1.0, 0.0);
  interpolated_quat_ground_truth.normalize();

  constexpr double kTolerance = 1e-8;
  EXPECT_NEAR(
      interpolated_quat_ground_truth.x(), interpolated_quat.x(), kTolerance);
  EXPECT_NEAR(
      interpolated_quat_ground_truth.y(), interpolated_quat.y(), kTolerance);
  EXPECT_NEAR(
      interpolated_quat_ground_truth.z(), interpolated_quat.z(), kTolerance);
  EXPECT_NEAR(
      interpolated_quat_ground_truth.w(), interpolated_quat.w(), kTolerance);

  // Test boundaries.
  interpolateRotation(
      kTimeA, quat_A, kTimeB, quat_B, kTimeA, &interpolated_quat);
  EXPECT_NEAR(quat_A.x(), interpolated_quat.x(), kTolerance);
  EXPECT_NEAR(quat_A.y(), interpolated_quat.y(), kTolerance);
  EXPECT_NEAR(quat_A.z(), interpolated_quat.z(), kTolerance);
  EXPECT_NEAR(quat_A.w(), interpolated_quat.w(), kTolerance);
  interpolateRotation(
      kTimeA, quat_A, kTimeB, quat_B, kTimeB, &interpolated_quat);
  EXPECT_NEAR(quat_B.x(), interpolated_quat.x(), kTolerance);
  EXPECT_NEAR(quat_B.y(), interpolated_quat.y(), kTolerance);
  EXPECT_NEAR(quat_B.z(), interpolated_quat.z(), kTolerance);
  EXPECT_NEAR(quat_B.w(), interpolated_quat.w(), kTolerance);
}

}  // namespace common

MAPLAB_UNITTEST_ENTRYPOINT


// Copyright (c) 2018-2019 FRC Team 3512. All Rights Reserved.

#pragma once

#include <grpl/pf.h>

#include <array>
#include <tuple>

#include <Eigen/Core>
#include <frc/Timer.h>
#include <frc/controller/StateSpaceLoop.h>
#include <units.h>

#include "Constants.hpp"
#include "control/DrivetrainCoeffs.hpp"
#include "control/Pathfinder.hpp"
#include "control/Pose.hpp"
#include "control/TrajectoryPoint.hpp"
#include "control/TrapezoidalMotionProfile.hpp"

class DrivetrainController {
public:
    // State tolerances in meters and meters/sec respectively.
    static constexpr double kPositionTolerance = 0.05;
    static constexpr double kVelocityTolerance = 2.0;

    DrivetrainController();

    DrivetrainController(const DrivetrainController&) = delete;
    DrivetrainController& operator=(const DrivetrainController&) = delete;

    void Enable();
    void Disable();

    /**
     * Sets goal pose of drivetrain controller.
     *
     * @param pose The goal pose.
     */
    void SetGoal(const Pose& pose);

    /**
     * Sets goal pose of drivetrain controller.
     *
     * @param pose The goal pose.
     */
    void SetGoal(Pose&& pose);

    /**
     * Returns whether the drivetrain controller is at the goal pose.
     */
    bool AtGoal() const;

    /**
     * Sets the current encoder measurements.
     *
     * @param leftPosition  Velocity of left side in meters.
     * @param rightPosition Velocity of right side in meters.
     * @param heading       Angle of the robot.
     */
    void SetMeasuredStates(double leftVelocity, double rightVelocity,
                           double heading);

    /**
     * Returns the control loop calculated voltage for the left side.
     */
    double ControllerLeftVoltage() const;

    /**
     * Returns the control loop calculated voltage for the left side.
     */
    double ControllerRightVoltage() const;

    /**
     * Returns the estimated left velocity.
     */
    double EstimatedLeftVelocity() const;

    /**
     * Returns the estimated right velocity.
     */
    double EstimatedRightVelocity() const;

    /**
     * Returns the error between the left velocity reference and the left
     * velocity estimate.
     */
    double LeftVelocityError() const;

    /**
     * Returns the error between the right velocity reference and the right
     * velocity estimate.
     */
    double RightVelocityError() const;

    TrajectoryPoint EstimatedPose() const;

    TrajectoryPoint GoalPose() const;

    double LeftVelocityReference();

    double RightVelocityReference();

    /**
     * Executes the control loop for a cycle.
     */
    void Update();

    /**
     * Resets any internal state.
     */
    void Reset();

private:
    // The current sensor measurements.
    Eigen::Matrix<double, 2, 1> m_Y;
    double m_headingMeasurement;

    // The control loop.
    frc::StateSpaceLoop<2, 2, 2> m_loop{MakeDrivetrainLoop()};

    // The motion profiles.
    TrapezoidalMotionProfile::Constraints positionConstraints{kRobotMaxV,
                                                              kRobotMaxA};
    TrapezoidalMotionProfile m_positionProfile{positionConstraints,
                                               {0_m, 0_mps}};
    TrapezoidalMotionProfile::Constraints angleConstraints{
        kRobotMaxRotateRate, kRobotMaxRotateAccel};
    TrapezoidalMotionProfile m_angleProfile{angleConstraints, {0_m, 0_mps}};

    Pathfinder m_pathfinder;

    // TrajectoryPoints that store x, y, theta, v, and omega (w) for usage
    TrajectoryPoint m_goal;
    TrajectoryPoint m_estimatedTrajectory;

    bool m_atReferences = false;

    /**
     * Sets the references
     *
     * @param leftVelocity Velocity of the left side in meters per second.
     * @param rightVelocity Velocity of the right side in meters per second.
     */
    void SetReferences(double leftVelocity, double rightVelocity);

    /**
     * Returns sinc(x) = std::sin(x) / x.
     *
     * @param x
     */
    static double Sinc(double x);

    /**
     * Returns a velocity and angular velocity corrected to account for failures
     * to stay on course.
     *
     * @param pose_desired  Desired x, y, and theta values.
     * @param v_desired     Desired velocity.
     * @param omega_desired Desired angular velocity.
     * @param pose          Current x, y, and theta values.
     * @param b             Tuning parameter; makes convergence more aggressive.
     * @param zeta          Tuning parameter in range (0,1); provides damping.
     */
    std::tuple<double, double> Ramsete(TrajectoryPoint trajectory_desired,
                                       TrajectoryPoint trajectory, double b,
                                       double zeta);

    /**
     * Returns left and right velocities from a central velocity and turning
     * rate
     *
     * @param v     Center velocity
     * @param omega Center turning rate
     * @param d     Trackwidth
     */
    std::tuple<double, double> GetDiffVelocities(double v, double omega,
                                                 double d);
};


#include <iostream>
#include <stdlib.h>

#include <gmpxx.h>
#include <NTL/ZZ.h>

using namespace std;

void usage(char *progname) {
	cout << "This program finds m given m^e and e, as long as "
		"m^e < n." << endl;
	cout << "Usage: " << progname << " c e" << endl;
}

int main(int argc, char *argv[]) {
	if (argc != 3) { usage(argv[0]); return 3; }

	// NTL doesn't provide an integer root function, so we
	// have to use GMP integers
	mpz_class c, root;
	mpz_t r;
	mpz_init(r);
	if(c.set_str(argv[1],0)) {
		cerr << "Invalid message: " << argv[1] << endl;
		return 2;
	}

	unsigned long e;
	if(!(e = strtoul(argv[2], NULL, 0))) {
		cerr << "Invalid public exponent: " << argv[2] << endl;
		return 2;
	}

	// Find c^(1/e)
	if(!mpz_root(r, c.get_mpz_t(), e)) {
		cerr << "Error: m^e is greater than n!" << endl;
		return 1;
	} else {
		cout << mpz_class(r) << endl;
	}
}


// Copyright (C) 2016 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
//
// 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 STAT_NORMAL_DISTRIBUTION_HPP
#define STAT_NORMAL_DISTRIBUTION_HPP

#include <stdexcept>
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/throw_exception.hpp>
#include "power.hpp"
#include "moment.hpp"

namespace stat {

using math::p2;
using math::p3;
using math::p4;

class normal_distribution : public moment<normal_distribution> {
private:
  typedef moment<normal_distribution> super_type;
public:
  normal_distribution(double mu = 0, double sigma = 1) : super_type(*this), mu_(mu), sigma_(sigma) {
    if (sigma_ <= 0)
      boost::throw_exception(std::invalid_argument("stat::normal_distribution"));
  }
  std::string name() const {
    return "Normal Distribution: N(" + boost::lexical_cast<std::string>(mu_) + ","
      + boost::lexical_cast<std::string>(sigma_) + ")";
  }
  double moment1() const { return mu_; }
  double moment2() const { return p2(mu_) + p2(sigma_); }
  double moment3() const { return p3(mu_) + 3 * mu_ * p2(sigma_); }
  double moment4() const { return p4(mu_) + 6 * p2(mu_) * p2(sigma_) + 3 * p4(sigma_); }
private:
  double mu_, sigma_;
};

} // end namespace stat

#endif // STAT_NORMAL_DISTRIBUTION_HPP


// Test ../include/Spectra/LinAlg/UpperHessenbergQR.h and
//      ../include/Spectra/LinAlg/DoubleShiftQR.h
#include <Eigen/Core>
#include <Eigen/QR>
#include <Spectra/LinAlg/UpperHessenbergQR.h>
#include <Spectra/LinAlg/DoubleShiftQR.h>

using namespace Spectra;

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

using Eigen::MatrixXd;
using Eigen::VectorXd;
typedef Eigen::Map<Eigen::MatrixXd> MapMat;

template <typename Solver, typename MatrixType>
void run_test(MatrixType &H, double shift)
{
    Solver decomp(H, shift);
    const int n = H.rows();
    const double tol = 1e-12;
    MatrixXd Hs = H - shift * MatrixXd::Identity(n, n);

    // Obtain Q matrix
    MatrixXd I = MatrixXd::Identity(n, n);
    MatrixXd Q = I;
    decomp.apply_QY(Q);

    // Test orthogonality
    MatrixXd QtQ = Q.transpose() * Q;
    INFO("||Q'Q - I||_inf = " << (QtQ - I).cwiseAbs().maxCoeff());
    REQUIRE((QtQ - I).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    MatrixXd QQt = Q * Q.transpose();
    INFO("||QQ' - I||_inf = " << (QQt - I).cwiseAbs().maxCoeff());
    REQUIRE((QQt - I).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    // Obtain R matrix and test whether it is upper triangular
    MatrixXd R = decomp.matrix_R();
    MatrixXd Rlower = R.triangularView<Eigen::StrictlyLower>();
    INFO("Whether R is upper triangular, error = " << Rlower.cwiseAbs().maxCoeff());
    REQUIRE(Rlower.cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    // Compare Hs = H - s * I and QR
    INFO("||Hs - QR||_inf = " << (Hs - Q * R).cwiseAbs().maxCoeff());
    REQUIRE((Hs - Q * R).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    // Obtain Q'HQ
    MatrixXd QtHQ_true = Q.transpose() * H * Q;
    MatrixXd QtHQ;
    decomp.matrix_QtHQ(QtHQ);
    INFO("max error of Q'HQ = " << (QtHQ - QtHQ_true).cwiseAbs().maxCoeff());
    REQUIRE((QtHQ - QtHQ_true).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    // Test "apply" functions
    MatrixXd Y = MatrixXd::Random(n, n);

    MatrixXd QY = Y;
    decomp.apply_QY(QY);
    INFO("max error of QY = " << (QY - Q * Y).cwiseAbs().maxCoeff());
    REQUIRE((QY - Q * Y).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    MatrixXd YQ = Y;
    decomp.apply_YQ(YQ);
    INFO("max error of YQ = " << (YQ - Y * Q).cwiseAbs().maxCoeff());
    REQUIRE((YQ - Y * Q).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    MatrixXd QtY = Y;
    decomp.apply_QtY(QtY);
    INFO("max error of Q'Y = " << (QtY - Q.transpose() * Y).cwiseAbs().maxCoeff());
    REQUIRE((QtY - Q.transpose() * Y).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    MatrixXd YQt = Y;
    decomp.apply_YQt(YQt);
    INFO("max error of YQ' = " << (YQt - Y * Q.transpose()).cwiseAbs().maxCoeff());
    REQUIRE((YQt - Y * Q.transpose()).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    // Test "apply" functions for vectors
    VectorXd y = VectorXd::Random(n);

    VectorXd Qy = y;
    decomp.apply_QY(Qy);
    INFO("max error of Qy = " << (Qy - Q * y).cwiseAbs().maxCoeff());
    REQUIRE((Qy - Q * y).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    VectorXd Qty = y;
    decomp.apply_QtY(Qty);
    INFO("max error of Q'y = " << (Qty - Q.transpose() * y).cwiseAbs().maxCoeff());
    REQUIRE((Qty - Q.transpose() * y).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));
}

TEST_CASE("QR of upper Hessenberg matrix", "[QR]")
{
    std::srand(123);
    int n = 100;
    MatrixXd m = MatrixXd::Random(n, n);
    m.array() -= 0.5;
    MatrixXd H = m.triangularView<Eigen::Upper>();
    H.diagonal(-1) = m.diagonal(-1);

    run_test<UpperHessenbergQR<double> >(H, 1.2345);

    MapMat Hmap(H.data(), H.rows(), H.cols());
    run_test<UpperHessenbergQR<double> >(Hmap, 0.6789);
}

TEST_CASE("QR of Tridiagonal matrix", "[QR]")
{
    std::srand(123);
    int n = 100;
    MatrixXd m = MatrixXd::Random(n, n);
    m.array() -= 0.5;
    MatrixXd H = MatrixXd::Zero(n, n);
    H.diagonal() = m.diagonal();
    H.diagonal(-1) = m.diagonal(-1);
    H.diagonal(1) = m.diagonal(-1);

    run_test<TridiagQR<double> >(H, 1.2345);

    MapMat Hmap(H.data(), H.rows(), H.cols());
    run_test<TridiagQR<double> >(Hmap, 0.6789);
}

TEST_CASE("QR decomposition with double shifts", "[QR]")
{
    std::srand(123);
    const int n = 100;
    const double tol = 1e-12;

    MatrixXd m = MatrixXd::Random(n, n);
    m.array() -= 0.5;
    MatrixXd H = m.triangularView<Eigen::Upper>();
    H.diagonal(-1) = m.diagonal(-1);
    H(1, 0) = 0;  // Test for the case when sub-diagonal element is zero

    const double s = 2, t = 3;

    MatrixXd M = H * H - s * H + t * MatrixXd::Identity(n, n);
    Eigen::HouseholderQR<MatrixXd> qr(M);
    MatrixXd Q0 = qr.householderQ();

    DoubleShiftQR<double> decomp(H, s, t);
    MatrixXd Q = MatrixXd::Identity(n, n);
    decomp.apply_YQ(Q);

    // Equal up to signs
    INFO("max error of Q = " << (Q.cwiseAbs() - Q0.cwiseAbs()).cwiseAbs().maxCoeff());
    REQUIRE((Q.cwiseAbs() - Q0.cwiseAbs()).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    // Test Q'HQ
    MatrixXd QtHQ;
    decomp.matrix_QtHQ(QtHQ);
    INFO("max error of Q'HQ = " << (QtHQ - Q.transpose() * H * Q).cwiseAbs().maxCoeff());
    REQUIRE((QtHQ - Q.transpose() * H * Q).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    // Test apply functions
    VectorXd y = VectorXd::Random(n);
    MatrixXd Y = MatrixXd::Random(n / 2, n);

    VectorXd Qty = y;
    decomp.apply_QtY(Qty);
    INFO("max error of Q'y = " << (Qty - Q.transpose() * y).cwiseAbs().maxCoeff());
    REQUIRE((Qty - Q.transpose() * y).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));

    MatrixXd YQ = Y;
    decomp.apply_YQ(YQ);
    INFO("max error of YQ = " << (YQ - Y * Q).cwiseAbs().maxCoeff());
    REQUIRE((YQ - Y * Q).cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));
}


#ifndef NEURAL_NET_HPP
#define NEURAL_NET_HPP

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

// NN Utilities
namespace NN {
    // RNGs
    extern std::mt19937 gen;
    extern std::normal_distribution<double> randn;
    extern std::uniform_real_distribution<double> rand;

    // Random initializers
    Eigen::VectorXd getBias(int);
    Eigen::MatrixXd getWeight(int, int);
}; // namespace NN

// Activation functions
enum ActFunc {
    SIGMOID,
    RELU
};

/*
 * Feedforward Neural Network
 */
class NeuralNet {
private:
    std::vector<int> sizes;        // layer sizes
    int layers;                    // number of layers
    std::vector<ActFunc> actFuncs; // activation functions for each layer

    void weightInitializer();                                    // random initializer
    Eigen::VectorXd activate(const Eigen::VectorXd &, int = -1); // apply activation functions

public:
    std::vector<Eigen::VectorXd> biases;  // NN biases
    std::vector<Eigen::MatrixXd> weights; // NN weights

    NeuralNet();
    NeuralNet(const std::vector<int> &);
    NeuralNet(const std::vector<int> &, const std::vector<Eigen::MatrixXd> &w, const std::vector<Eigen::VectorXd> &b);
    Eigen::VectorXd feedforward(Eigen::VectorXd); // get feedforward outputs
    void saveToFile(int fileOffset = 0);          // save network to file
    void loadFromFile(int fileOffset = 0);        // load network from file
};

#endif


#include <cassert> // assert

#include <boost/random/mersenne_twister.hpp> // boost::random::mt19937
#include <boost/assign/list_of.hpp> // boost::assign::list_of()

#include <mutation_accumulation/random/multinomial_distribution.h> // random::multinomial_distribution
#include <mutation_accumulation/probability/cdf.h> // probability::CDF_discrete

#include "unit_test.h"

typedef int int_type; // probability::CDF_discrete complains at compile-time if int_type is not int (for example long long int)
typedef mutation_accumulation::random::multinomial_distribution<int_type, double> mn_type;

/*************************************************************************/

void unit_test::unit_test_2Dmultinomial() {

    std::cout << "generate 2D multinomial CDF..." << std::endl;

    /* PRNG engine */
    boost::random::mt19937 urng;

    /* parameters of multinomial_distribution */
    const int_type NN = 10;
    const double pp = 0.75;
    const std::vector<double> probabilities = boost::assign::list_of(pp)(1-pp);
    std::ofstream fout("mn_2D_parameters.in");
    fout << "N = " << NN << std::endl;
    fout << "p = " << pp << std::endl;

    /* create 2D multinomial_distribution object */
    mn_type mn_rnd(NN, probabilities);

    /* number of samples to draw from distribution */
    const int number_trials = 1e3;

    /* sample space */
    std::vector<int_type> sample_space(NN + 1, -1.0);
    for (int_type ii = 0; ii < sample_space.size(); ii++) {
        sample_space.at(ii) = ii;
    }

    /* create CDF object */
    probability::CDF_discrete mn_cdf(sample_space, "mn_2D_cdf.dat");

    /* repeatedly add samples to the CDF */
    for (int ii = 0; ii < number_trials; ii++)
        mn_cdf.update(mn_rnd(urng).at(0));

    std::cout << "... CDF generated" << std::endl;

}

/*************************************************************************/

void unit_test::unit_test__sum_random_vector_elements() {

    std::cout << "testing sum of elements in random vector..." << std::endl;

    /* PRNG engine */
    boost::random::mt19937 urng;

    /* parameters of multinomial_distribution */
    const int_type NN = 10;
    const std::vector<double> probabilities = boost::assign::list_of(0.2)(0.2)(0.2)(0.4);

    /* create multinomial_distribution object */
    mn_type mn_rnd(NN, probabilities);
    std::cout << "generated " << probabilities.size() << "D multinomial distribution" << std::endl;

    /* number of samples to draw from distribution */
    const int number_trials = 1e2;

    /* sample multinomial distribution */
    for (int ii = 0; ii < number_trials; ii++) {

        std::vector<int_type> random_vector = mn_rnd(urng);

        int_type sum = static_cast<int_type>(0);
        for (int ii = 0; ii < random_vector.size(); ii++) {
            sum += random_vector.at(ii);
        }

        assert(sum == NN);

    }

    std::cout << "... finished testing sum of elements in random vector" << std::endl;

}

/*************************************************************************/

void unit_test::unit_test__3Dmultinomial() {

    std::cout << "sampling from 3D multinomial ... " << std::endl;

    /* PRNG engine */
    boost::random::mt19937 urng;

    /* parameters of multinomial_distribution */
    const int_type NN = 20;
    const std::vector<double> probabilities = boost::assign::list_of(0.8)(0.1)(0.1);
    std::ofstream fin("mn_3D_parameters.in");
    fin << "N = " << NN << std::endl;
    for (int ii = 0; ii < probabilities.size(); ii++)
        fin << "p" << ii << " = " << probabilities.at(ii) << std::endl;

    /* create multinomial_distribution object */
    mn_type mn_rnd(NN, probabilities);
    std::cout << "generated " << probabilities.size() << "D multinomial distribution" << std::endl;

    /* number of samples to draw from distribution */
    const int number_trials = 1e5;

    /* open file to write samples to */
    std::ofstream fout("mn_3D_samples.dat");

    /* sample multinomial distribution */
    for (int ii = 0; ii < number_trials; ii++) {

        std::vector<int_type> random_vector = mn_rnd(urng);

        for (int ii = 0; ii < random_vector.size(); ii++) {
            fout << random_vector.at(ii) << " ";
        }
        fout << std::endl;

    }

    std::cout << "... finished sampling from 3D multinomial" << std::endl;

}

/*************************************************************************/

void unit_test::unit_test__4Dmultinomial() {

    std::cout << "sampling from 4D multinomial ... " << std::endl;

    /* PRNG engine */
    boost::random::mt19937 urng;

    /* parameters of multinomial_distribution */
    const int_type NN = 10;
    const std::vector<double> probabilities = boost::assign::list_of(0.6)(0.1)(0.1)(0.2);
    std::ofstream fin("mn_4D_parameters.in");
    fin << "N = " << NN << std::endl;
    for (int ii = 0; ii < probabilities.size(); ii++)
        fin << "p" << ii << " = " << probabilities.at(ii) << std::endl;

    /* create multinomial_distribution object */
    mn_type mn_rnd(NN, probabilities);
    std::cout << "generated " << probabilities.size() << "D multinomial distribution" << std::endl;

    /* number of samples to draw from distribution */
    const int number_trials = 1e5;

    /* open file to write samples to */
    std::ofstream fout("mn_4D_samples.dat");

    /* sample multinomial distribution */
    for (int ii = 0; ii < number_trials; ii++) {

        std::vector<int_type> random_vector = mn_rnd(urng);

        for (int ii = 0; ii < random_vector.size(); ii++) {
            fout << random_vector.at(ii) << " ";
        }
        fout << std::endl;

    }

    std::cout << "... finished sampling from 4D multinomial" << std::endl;

}

/*************************************************************************/

void unit_test::unit_test__3Dmultinomial_edgeCase() {

    std::cout << "sampling from 3D multinomial (edge case)... " << std::endl;

    /* PRNG engine */
    boost::random::mt19937 urng;

    /* parameters of multinomial_distribution */
    const int_type NN = 5;
    const std::vector<double> probabilities = boost::assign::list_of(0.0)(0.0)(1.0);
    std::ofstream fin("mn_3D_edgeCase_parameters.in");
    fin << "N = " << NN << std::endl;
    for (int ii = 0; ii < probabilities.size(); ii++)
        fin << "p" << ii << " = " << probabilities.at(ii) << std::endl;

    /* create multinomial_distribution object */
    mn_type mn_rnd(NN, probabilities);
    std::cout << "generated " << probabilities.size() << "D multinomial distribution" << std::endl;

    /* number of samples to draw from distribution */
    const int number_trials = 1e5;

    /* sample multinomial distribution */
    for (int ii = 0; ii < number_trials; ii++) {

        std::vector<int_type> random_vector = mn_rnd(urng);

        assert(random_vector.at(0) == 0);
        assert(random_vector.at(1) == 0);
        assert(random_vector.at(2) == NN);

//        for (int ii = 0; ii < random_vector.size(); ii++) {
//            std::cout << random_vector.at(ii) << " ";
//        }
//        std::cout << std::endl;

    }

    std::cout << "... finished sampling from 3D multinomial (edge case)" << std::endl;

}

/*************************************************************************/

void unit_test::unit_test__3Dmultinomial_zeroN() {

    std::cout << "sampling from 3D multinomial (N = 0)... " << std::endl;

    /* PRNG engine */
    boost::random::mt19937 urng;

    /* parameters of multinomial_distribution */
    const int_type NN = 0;
    const std::vector<double> probabilities = boost::assign::list_of(0.1)(0.1)(0.8);
    std::ofstream fin("mn_3D_zeroN_parameters.in");
    fin << "N = " << NN << std::endl;
    for (int ii = 0; ii < probabilities.size(); ii++)
        fin << "p" << ii << " = " << probabilities.at(ii) << std::endl;

    /* create multinomial_distribution object */
    mn_type mn_rnd(NN, probabilities);
    std::cout << "generated " << probabilities.size() << "D multinomial distribution" << std::endl;

    /* number of samples to draw from distribution */
    const int number_trials = 1e5;

    /* sample multinomial distribution */
    for (int ii = 0; ii < number_trials; ii++) {

        std::vector<int_type> random_vector = mn_rnd(urng);

        assert(random_vector.at(0) == 0);
        assert(random_vector.at(1) == 0);
        assert(random_vector.at(2) == 0);

//        for (int ii = 0; ii < random_vector.size(); ii++) {
//            std::cout << random_vector.at(ii) << " ";
//        }
//        std::cout << std::endl;

    }

    std::cout << "... finished sampling from 3D multinomial (N = 0)" << std::endl;

}