#ifndef BOOST_QVM_DETAIL_DETERMINANT_IMPL_HPP_INCLUDED
#define BOOST_QVM_DETAIL_DETERMINANT_IMPL_HPP_INCLUDED

// Copyright 2008-2022 Emil Dotchevski and Reverge Studios, Inc.

// 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 <boost/qvm/config.hpp>
#include <boost/qvm/mat_traits_array.hpp>
#include <boost/qvm/static_assert.hpp>

namespace boost { namespace qvm {

namespace
qvm_detail
    {
    template <int N>
    struct
    det_size
        {
        };

    template <class M>
    BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
    typename mat_traits<M>::scalar_type
    determinant_impl_( M const & a, det_size<2> )
        {
        return
            mat_traits<M>::template read_element<0,0>(a) * mat_traits<M>::template read_element<1,1>(a) -
            mat_traits<M>::template read_element<1,0>(a) * mat_traits<M>::template read_element<0,1>(a);
        }

    template <class M,int N>
    BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_RECURSION
    typename mat_traits<M>::scalar_type
    determinant_impl_( M const & a, det_size<N> )
        {
        typedef typename mat_traits<M>::scalar_type T;
        T m[N-1][N-1];
        T det=T(0);
        for( int j1=0; j1!=N; ++j1 )
            {
            for( int i=1; i!=N; ++i )
                {
                int j2 = 0;
                for( int j=0; j!=N; ++j )
                    {
                    if( j==j1 )
                        continue;
                    m[i-1][j2] = mat_traits<M>::read_element_idx(i,j,a);
                    ++j2;
                    }
                }
            T d=determinant_impl_(m,det_size<N-1>());
            if( j1&1 )
                d=-d;
            det += mat_traits<M>::read_element_idx(0,j1,a) * d;
            }
        return det;
        }

    template <class M>
    BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL
    typename mat_traits<M>::scalar_type
    determinant_impl( M const & a )
        {
        BOOST_QVM_STATIC_ASSERT(mat_traits<M>::rows==mat_traits<M>::cols);
        return determinant_impl_(a,det_size<mat_traits<M>::rows>());
        }
    }

} }

#endif


#include "random.hpp"

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/random_simplify_point_set.h>
#include <CGAL/property_map.h>
#include <CGAL/IO/read_xyz_points.h>

#include <vector>
#include <fstream>
#include <iostream>

#include <cmath>

#include <boost/tuple/tuple.hpp>

#include "DtcMainHelper.hpp"

// Types
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3 Point;
typedef boost::tuple<Point, float, float, float, float> MYPoint;

void random(int argc, char*argv[]) {
/*
Input:
    argv[0]: this program
    argv[1]: "random"
    argv[2]: dir of output data
    argv[3]: filename of input data under ../tmp/
    argv[4]: keep ratio in pecentage [0, 100]
*/

    if (argc != 5) {
        std::cerr << "[grid.cpp] require argc: 5, input argc: " << argc << std::endl;
        std::cerr << "Format: program random output_dir input_file keep_ratio" << std::endl;
        exit(-1);
    }

    std::string sampler(argv[1]);
    std::string dstDir(argv[2]);
    std::string srcFile(argv[3]);
    double keep_ratio = atof(argv[4]);

    std::vector<MYPoint> points;
    std::string srcFilename = "../tmp/" + srcFile;

    // read points
    // std::cout << "Reading file: " << srcFilename << std::endl;
    std::FILE *pFile = fopen(srcFilename.c_str(), "rb");
    fseek(pFile, 0, SEEK_END);    // file pointer goes to the end of the file
    long fileSize = ftell(pFile); // file size
    rewind(pFile);                // rewind file pointer to the beginning
    float *rawData = new float[fileSize];
    fread(rawData, sizeof(float), fileSize/sizeof(float), pFile);
    int nProperties = 7;
    long number_of_points = fileSize / nProperties / sizeof(float); // x, y, z, r, g, b, label

    for (int i = 0; i < number_of_points; i++) {
        points.push_back(MYPoint(Point(rawData[nProperties*i], rawData[nProperties*i+1], rawData[nProperties*i+2]),
            rawData[nProperties*i+3], rawData[nProperties*i+4], rawData[nProperties*i+5], rawData[nProperties*i+6]));
    }

    // processing time
    uint64_t time_before_sample = DtcMainHelper::getTimestamp();

    points.erase(CGAL::random_simplify_point_set(points.begin(), points.end(), CGAL::Nth_of_tuple_property_map<0, MYPoint>(), 100.0-keep_ratio),
        points.end());

    DtcMainHelper::dataToFile() << DtcMainHelper::getTimestamp() - time_before_sample << std::endl;

    // log processing time to file time.txt
    std::string dstFileSave = dstDir + "/" + srcFile + ".trim";
    // std::cout << dstFileSave << std::endl;

    std::ofstream out(dstFileSave, std::ios_base::binary);
    for (int i = 0; i < points.size(); i++) {

        float x = points[i].get<0>().x();
        float y = points[i].get<0>().y();
        float z = points[i].get<0>().z();
        float r = points[i].get<1>();
        float g = points[i].get<2>();
        float b = points[i].get<3>();
        float label = points[i].get<4>();

        out.write((char *)&x, sizeof(float));
        out.write((char *)&y, sizeof(float));
        out.write((char *)&z, sizeof(float));
        out.write((char *)&r, sizeof(float));
        out.write((char *)&g, sizeof(float));
        out.write((char *)&b, sizeof(float));
        out.write((char *)&label, sizeof(float));
    }
    out.close();
}


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "matrix_utils.hpp"

#include <cmath>
#include <vector>

#include <boost/dynamic_bitset.hpp>
#include <boost/format.hpp>

#include <xtensor/xio.hpp>
#include <xtensor-blas/xlinalg.hpp>

#include <core/utils/bitset_utils.hpp>
#include <core/utils/terminal.hpp>
#include <reversible/pauli_tags.hpp>
#include <reversible/target_tags.hpp>
#include <reversible/simulation/simple_simulation.hpp>

namespace cirkit
{

using namespace std::complex_literals;

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

xt::xarray<complex_t> get_2by2_matrix( complex_t a, complex_t b, complex_t c, complex_t d )
{
  xt::xarray<complex_t> matrix(std::vector<size_t>{2, 2});
  matrix[{0,0}] = a;
  matrix[{0,1}] = b;
  matrix[{1,0}] = c;
  matrix[{1,1}] = d;
  return matrix;
}

xt::xarray<complex_t> kron( const std::vector<xt::xarray<complex_t>>& ms )
{
  assert( !ms.empty() );

  auto r = ms[0u];

  for ( auto i = 1u; i < ms.size(); ++i )
  {
    r = xt::linalg::kron( r, ms[i] );
  }

  return r;
}

xt::xarray<complex_t> identity( unsigned dimension )
{
  xt::xarray<complex_t> matrix(std::vector<size_t>{dimension, dimension});
  for ( auto i = 0u; i < dimension; ++i )
  {
    matrix[{i, i}] = 1.0;
  }
  return matrix;
}

xt::xarray<complex_t> identity_padding( const xt::xarray<complex_t>& matrix, unsigned from, unsigned to, unsigned lines )
{
  std::vector<xt::xarray<complex_t>> ms;

  if ( to + 1u < lines )
  {
    ms.push_back( identity( 1 << ( lines - to - 1u ) ) );
  }

  ms.push_back( matrix );

  if ( from > 0u )
  {
    ms.push_back( identity( 1 << from ) );
  }

  return kron( ms );
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

xt::xarray<complex_t> matrix_from_clifford_t_circuit( const circuit& circ, bool progress )
{
  xt::xarray<complex_t> matrix_X = get_2by2_matrix( 0.0, 1.0, 1.0, 0.0 );
  xt::xarray<complex_t> matrix_H = 1.0 / sqrt( 2.0 ) * get_2by2_matrix( 1.0, 1.0, 1.0, -1.0 );
  xt::xarray<complex_t> matrix_Z = get_2by2_matrix( 1.0, 0.0, 0.0, -1.0 );
  xt::xarray<complex_t> matrix_S = get_2by2_matrix( 1.0, 0.0, 0.0, 1i );
  xt::xarray<complex_t> matrix_Sdag = get_2by2_matrix( 1.0, 0.0, 0.0, -1i );
  xt::xarray<complex_t> matrix_T = get_2by2_matrix( 1.0, 0.0, 0.0, exp( 1i * M_PI / 4.0 ) );
  xt::xarray<complex_t> matrix_Tdag = get_2by2_matrix( 1.0, 0.0, 0.0, exp( -1i * M_PI / 4.0 ) );
  xt::xarray<complex_t> matrix_zc = get_2by2_matrix( 1.0, 0.0, 0.0, 0.0 );
  xt::xarray<complex_t> matrix_oc = get_2by2_matrix( 0.0, 0.0, 0.0, 1.0 );

  const auto n = circ.lines();

  std::vector<xt::xarray<complex_t>> gates;

  for ( const auto& g : circ )
  {
    const auto target = g.targets().front();

    if ( is_toffoli( g ) && g.controls().size() == 0u )
    {
      gates.push_back( identity_padding( matrix_X, target, target, n ) );
    }
    else if ( is_toffoli( g ) && g.controls().size() == 1u && g.controls().front().polarity() )
    {
      const auto control = g.controls().front().line();

      if ( control < target )
      {
        const auto act = target - control;
        const auto gate = xt::linalg::kron( identity( 1 << act ), matrix_zc ) + kron( {matrix_X, identity( 1 << ( act - 1 ) ), matrix_oc} );
        gates.push_back( identity_padding( gate, control, target, n ) );
      }
      else
      {
        const auto act = control - target;
        const auto gate = xt::linalg::kron( matrix_zc, identity( 1 << act ) ) + kron( {matrix_oc, identity( 1 << ( act - 1 ) ), matrix_X} );
        gates.push_back( identity_padding( gate, target, control, n ) );
      }
    }
    else if ( is_hadamard( g ) )
    {
      gates.push_back( identity_padding( matrix_H, target, target, n ) );
    }
    else if ( is_pauli( g ) )
    {
      const auto pauli = boost::any_cast<pauli_tag>( g.type() );
      switch ( pauli.axis )
      {
      case pauli_axis::X:
        if ( pauli.root == 1u )
        {
          gates.push_back( identity_padding( matrix_X, target, target, n ) );
        }
        else
        {
          std::cout << "[w] unsupported X gate" << std::endl;
        }
        break;
      case pauli_axis::Z:
        if ( pauli.root == 1u )
        {
          gates.push_back( identity_padding( matrix_Z, target, target, n ) );
        }
        else if ( pauli.root == 2u )
        {
          gates.push_back( identity_padding( pauli.adjoint ? matrix_Sdag : matrix_S, target, target, n ) );
        }
        else if ( pauli.root == 4u )
        {
          gates.push_back( identity_padding( pauli.adjoint ? matrix_Tdag : matrix_T, target, target, n ) );
        }
        else
        {
          std::cout << "[w] unsupported Z gate with root " << pauli.root << std::endl;
        }
        break;
      default:
        std::cout << "[w] unsupported Pauli gate" << std::endl;
        break;
      }
    }
    else
    {
      std::cout << "[w] unsupported gate" << std::endl;
    }
  }

  if ( gates.empty() )
  {
    return identity( 1 << n );
  }
  else
  {
    progress_line pline( boost::str( boost::format( "[i] (matrix_from_clifford_t_circuit) gate %%d / %d" ) % gates.size() ), progress );

    auto r = gates.front();
    for ( auto i = 1u; i < gates.size(); ++i )
    {
      pline( i + 1u );
      r = xt::linalg::dot( r, gates[i] );
    }

    return r;
  }
}

xt::xarray<complex_t> matrix_from_reversible_circuit( const circuit& circ )
{
  const size_t N = 1 << circ.lines();

  xt::xarray<complex_t> matrix(std::vector<size_t>{N, N});
  foreach_bitset( circ.lines(), [&circ, &matrix]( const boost::dynamic_bitset<>& input ) {
      boost::dynamic_bitset<> output;
      simple_simulation( output, circ, input );
      matrix[{input.to_ulong(), output.to_ulong()}] = 1.0;
    } );

  return matrix;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/*===========================================================================*\

Author: Matthias W. Smith
Email:  mwsmith2@uw.edu
Date:   11/02/14

Detail: This is a new test program for my Fid libraries

\*===========================================================================*/

//--- std includes ----------------------------------------------------------//
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
using std::vector;
using std::cout;
using std::endl;

//--- other includes --------------------------------------------------------//
#include <armadillo>

//--- project includes ------------------------------------------------------//
#include "fid.h"
using namespace fid;

int main(int argc, char** argv)
{
  // set precision
  cout.precision(10);
  cout.setf(std::ios::fixed, std::ios::floatfield);

  // declare variables
  int fid_length = 5000;
  double ti = -1.0;
  double dt = 0.001;
  double ftruth = 23.0;

  vector<double> wf;
  vector<double> tm;
  wf.reserve(fid_length);
  tm.reserve(fid_length);

  std::ofstream out;
  out.precision(10);

  for (int i = 0; i < fid_length; i++){
    tm.push_back(i * dt + ti);
  }

  FidFactory ff;
  ff.SetLarmorFreq(ftruth + sim::mixdown_freq);
  ff.IdealFid(wf, tm);

  for (int i = 0; i < wf.size(); ++i) {
    wf[i] = sin(40 * tm[i]);
  }

  out.open("wvd_test_wf.txt");
  for (auto it = wf.begin(); it != wf.end(); ++it) {
    out << *it << ", ";
  }
  out.close();

  auto wf_im = dsp::hilbert(wf);

  out.open("wvd_test_wf_im.txt");
  for (auto it = wf_im.begin(); it != wf_im.end(); ++it) {
    out << *it << ", ";
  }
  out.close();

  auto res = dsp::wvd(wf);
  res.quiet_save("wvd_test.txt", arma::csv_ascii);

  return 0;
}


/*
assignmentGenerator.cpp - This file is part of the Bayesembler (v1.1.1)

The MIT License (MIT)

Copyright (c) 2014 Lasse Maretty and Jonas Andreas Sibbesen

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <assignmentGenerator.h>
#include <boost/random/discrete_distribution.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_01.hpp>
#include <algorithm>

typedef boost::random::uniform_int_distribution<> uniform_sampler_t;
typedef boost::random::binomial_distribution<> binomial_distribution_t;
typedef boost::random::variate_generator<boost::random::mt19937*, boost::random::binomial_distribution<> > binomial_sampler_t;
typedef boost::random::uniform_01<boost::random::mt19937*> uniform_01_sampler_t;

AssignmentGenerator::AssignmentGenerator(mt_rng_pt_t mt_rng_pt_in, Eigen::MatrixXd * collapsed_map_eigen_in, vector<int> * collapsed_count_vec_in) {

	mt_rng_pt = mt_rng_pt_in;
    collapsed_map_eigen = collapsed_map_eigen_in;
    collapsed_count_vec = collapsed_count_vec_in;
    num_transcripts = collapsed_map_eigen->cols();

}

void AssignmentGenerator::calcMinimumReadCover(stringstream& log_stream) {

    log_stream << "\n[" << getLocalTime() << "] " << "Calculating minimum read cover" << endl;

    tr1::unordered_set<int> minimum_set_temp;

    Eigen::Matrix< int,1,Eigen::Dynamic > logical_vector(collapsed_map_eigen->rows());
    logical_vector.setOnes();

    Eigen::Matrix< int,Eigen::Dynamic,Eigen::Dynamic > binary_collapsed_map_eigen((collapsed_map_eigen->array() >= double_underflow).matrix().cast<int>());

    while (logical_vector.sum() > 0) {

        Eigen::RowVectorXi covered_reads(logical_vector * binary_collapsed_map_eigen);
        int max_val = covered_reads.maxCoeff();
        assert (max_val > 0);

        vector<int> max_indices;
        max_indices.reserve(covered_reads.size());

        for (int i=0; i < covered_reads.size(); i++) {

            if (max_val == covered_reads[i]) {

                max_indices.push_back(i);

            }
        }

        uniform_sampler_t set_index_dist(0,max_indices.size()-1);
        int max_pos = max_indices[set_index_dist(*mt_rng_pt)];

        minimum_set_temp.insert(max_pos);
        logical_vector = (logical_vector.array() - logical_vector.array() * binary_collapsed_map_eigen.col(max_pos).transpose().array()).matrix();
    }

    minimum_set = minimum_set_temp;

    log_stream << "[" << getLocalTime() << "] " << "Number of candidates in minimum read cover: " << minimum_set.size() << "\n" << endl;
}

int AssignmentGenerator::getMinimumReadCoverSize() {

    return minimum_set.size();
}

CountValueContainer AssignmentGenerator::initAssignmentMinimum(stringstream& log_stream) {

    log_stream << "[" << getLocalTime() << "] " << "Initialising read assignments using minimum set cover" << endl;

    CountValueContainer map_counts(num_transcripts);

    for (int i=0; i < collapsed_count_vec->size(); i++) {

        vector<int> minimum_set_vec;
        double norm_const = 0;

		for (int j=0; j < collapsed_map_eigen->cols(); j++) {

			if ((minimum_set.count(j) == 1) and ((*collapsed_map_eigen)(i,j) >= double_underflow)) {

				minimum_set_vec.push_back(j);
                norm_const += (*collapsed_map_eigen)(i,j);

			}
		}

        assert(norm_const >= double_underflow);
        int num_samples = collapsed_count_vec->at(i);
        double divider = 1;

        for (int j = 0; j < minimum_set_vec.size(); j++) {

            double norm_probability = (*collapsed_map_eigen)(i,minimum_set_vec[j])/norm_const;

            if (norm_probability >= double_underflow) {

                binomial_distribution_t binom_dist(num_samples, norm_probability/divider);
                binomial_sampler_t binom_sampler(mt_rng_pt, binom_dist);

                int num_maps = binom_sampler();
                map_counts.addToCount(num_maps,minimum_set_vec[j]);

                if (num_maps > 0) {

                    map_counts.addToPlus(minimum_set_vec[j]);

                }

                num_samples -= num_maps;
            }

            divider -= norm_probability;
        }

        assert (num_samples == 0);
	}

	map_counts.fetchNullSet();
    vector <int> counts_out = map_counts.getCounts();
	// cout << "Initialised assignment with counts:\n" << counts_out << endl;

	return map_counts;

}

CountValueContainer AssignmentGenerator::initEnsembleAssignment(vector<int> indices) {

    CountValueContainer map_counts(num_transcripts);

    for (int i=0; i < collapsed_count_vec->size(); i++) {

        double norm_const = 0;

		for (int j=0; j < indices.size(); j++) {

            norm_const += (*collapsed_map_eigen)(i,indices[j]);
    	}

        assert(norm_const >= double_underflow);
        int num_samples = collapsed_count_vec->at(i);
        double divider = 1;

        for (int j = 0; j < indices.size(); j++) {

            int current_idx = indices[j];
            double norm_probability = (*collapsed_map_eigen)(i, current_idx)/norm_const;

            if (norm_probability >= double_underflow) {

                binomial_distribution_t binom_dist(num_samples, norm_probability/divider);
                binomial_sampler_t binom_sampler(mt_rng_pt, binom_dist);

                int num_maps = binom_sampler();
                map_counts.addToCount(num_maps, current_idx);

                if (num_maps > 0) {

                    map_counts.addToPlus(current_idx);

                }

                num_samples -= num_maps;

            }

            divider -= norm_probability;

        }

        assert (num_samples == 0);
	}

	map_counts.fetchNullSet();
    vector <int> counts_out = map_counts.getCounts();
	// cout << "Initialised assignment with counts:\n" << counts_out << endl;

	return map_counts;

}

CountValueContainer AssignmentGenerator::initAssignment(stringstream& log_stream) {

    log_stream << "[" << getLocalTime() << "] " << "Initialising read assignments uniformly" << endl;

    CountValueContainer map_counts(num_transcripts);

    for (int i=0; i < collapsed_count_vec->size(); i++) {

        int num_samples = collapsed_count_vec->at(i);
        double divider = 1;

        for (int j = 0; j < num_transcripts; j++) {

            double norm_probability = (*collapsed_map_eigen)(i,j);

            if (norm_probability >= double_underflow) {

                binomial_distribution_t binom_dist(num_samples, norm_probability/divider);
                binomial_sampler_t binom_sampler(mt_rng_pt, binom_dist);

                int num_maps = binom_sampler();
                map_counts.addToCount(num_maps,j);

                if (num_maps > 0) {

                    map_counts.addToPlus(j);

                }

                num_samples -= num_maps;

            }

            divider -= norm_probability;

        }

        assert (num_samples == 0);

	}

	map_counts.fetchNullSet();
    vector <int> counts_out = map_counts.getCounts();
	// cout << "Initialised assignment with counts:\n" << counts_out << endl;

	return map_counts;

}

CountValueContainer AssignmentGenerator::generateAssignment(ExpressionValueContainer expression) {

    // Reset assignment map
	CountValueContainer map_counts(num_transcripts);

    // Create expression vector and calculate normalisation constants
	Eigen::RowVectorXd expression_values(num_transcripts);

	for (int i=0; i < num_transcripts; i++) {

		expression_values(i) = expression.getValue(i);
	}

    uniform_01_sampler_t sample_uniform_01(mt_rng_pt);

    for (int i=0; i < collapsed_count_vec->size(); i++) {

        Eigen::RowVectorXd probabilities_unnormalised = collapsed_map_eigen->row(i).array() * expression_values.array();

        double normalisation_constant = probabilities_unnormalised.sum();

        assert (normalisation_constant >= double_underflow);
        assert (probabilities_unnormalised.size() == num_transcripts);

        int num_samples = collapsed_count_vec->at(i);

        if (num_samples < 20) {

            vector <double> uniform_samples(num_samples);

            for (int j = 0; j < num_samples; j++) {

                uniform_samples[j] = sample_uniform_01();
            }

            sort (uniform_samples.begin(), uniform_samples.end());
            double cum_sum = 0;
            int index_num = 0;

            for (int j = 0; j < num_transcripts; j++) {

                cum_sum += probabilities_unnormalised[j]/normalisation_constant;

                if (uniform_samples[index_num] < cum_sum) {

                    int samp_counter = 1;
                    index_num +=1;

                    while (index_num < uniform_samples.size()) {

                        if (uniform_samples[index_num] >= cum_sum) {

                            break;
                        }

                        samp_counter += 1;
                        index_num +=1;

                    }

                    map_counts.addToCount(samp_counter, j);
                    map_counts.addToPlus(j);

                }

                if (index_num == uniform_samples.size()) {

                    break;

                }

            }

        } else {

            double divider = 1;

            for (int j = 0; j < num_transcripts; j++) {

                double norm_probability = probabilities_unnormalised[j]/normalisation_constant;

                if (norm_probability >= double_underflow) {

                    binomial_distribution_t binom_dist(num_samples, norm_probability/divider);
                    binomial_sampler_t binom_sampler(mt_rng_pt, binom_dist);

                    int num_maps = binom_sampler();

                    if (num_maps > 0) {

                        map_counts.addToCount(num_maps, j);
                        map_counts.addToPlus(j);
                    }

                    num_samples -= num_maps;
                }

                divider -= norm_probability;
            }

            assert (num_samples == 0);

        }

	}

	map_counts.fetchNullSet();

	return map_counts;

}


//
// boost/radix/bitmask.hpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// 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 BOOST_RADIX_BITMASK_HPP
#define BOOST_RADIX_BITMASK_HPP

#include <boost/radix/common.hpp>

#include <boost/integer/integer_mask.hpp>
#include <boost/radix/common.hpp>

namespace boost { namespace radix {

template <std::size_t Bits>
struct mask {
  BOOST_STATIC_CONSTANT(
      typename low_bits_mask_t<Bits>::fast,
      value = ~(~low_bits_mask_t<Bits>::sig_bits_fast));
};

template <std::size_t Bits, std::size_t Shift>
struct mask_shift {
  BOOST_STATIC_CONSTANT(
      typename low_bits_mask_t<Bits>::fast,
      value = ~(~low_bits_mask_t<Bits>::sig_bits_fast) << Shift);
};

}} // namespace boost::radix

#endif // BOOST_RADIX_BITMASK_HPP


#include "forward_dynamics.h"

#include <Eigen/Cholesky>
#include <iit/rbd/robcogen_commons.h>

using namespace iit::rbd;

// Initialization of static-const data
const iit::popi::dyn::ForwardDynamics::ExtForces
    iit::popi::dyn::ForwardDynamics::zeroExtForces(Force::Zero());

iit::popi::dyn::ForwardDynamics::ForwardDynamics(InertiaProperties& inertia, MotionTransforms& transforms) :
    inertiaProps( & inertia ),
    motionTransforms( & transforms )
{
    EpauleAVD_v.setZero();
    EpauleAVD_c.setZero();
    HJambeAVD_v.setZero();
    HJambeAVD_c.setZero();
    BJambeAVD_v.setZero();
    BJambeAVD_c.setZero();
    EpauleAVG_v.setZero();
    EpauleAVG_c.setZero();
    HJambeAVG_v.setZero();
    HJambeAVG_c.setZero();
    BJambeAVG_v.setZero();
    BJambeAVG_c.setZero();
    EpauleARD_v.setZero();
    EpauleARD_c.setZero();
    HJambeARD_v.setZero();
    HJambeARD_c.setZero();
    BJambeARD_v.setZero();
    BJambeARD_c.setZero();
    EpauleARG_v.setZero();
    EpauleARG_c.setZero();
    HJambeARG_v.setZero();
    HJambeARG_c.setZero();
    BJambeARG_v.setZero();
    BJambeARG_c.setZero();

    vcross.setZero();
    Ia_r.setZero();

}

void iit::popi::dyn::ForwardDynamics::fd(
    JointState& qdd,
    Acceleration& base_a,
    const Velocity& base_v,
    const Acceleration& g,
    const JointState& qd,
    const JointState& tau,
    const ExtForces& fext/* = zeroExtForces */)
{

    base_AI = inertiaProps->getTensor_base();
    base_p = - fext[BASE];
    EpauleAVD_AI = inertiaProps->getTensor_EpauleAVD();
    EpauleAVD_p = - fext[EPAULEAVD];
    HJambeAVD_AI = inertiaProps->getTensor_HJambeAVD();
    HJambeAVD_p = - fext[HJAMBEAVD];
    BJambeAVD_AI = inertiaProps->getTensor_BJambeAVD();
    BJambeAVD_p = - fext[BJAMBEAVD];
    EpauleAVG_AI = inertiaProps->getTensor_EpauleAVG();
    EpauleAVG_p = - fext[EPAULEAVG];
    HJambeAVG_AI = inertiaProps->getTensor_HJambeAVG();
    HJambeAVG_p = - fext[HJAMBEAVG];
    BJambeAVG_AI = inertiaProps->getTensor_BJambeAVG();
    BJambeAVG_p = - fext[BJAMBEAVG];
    EpauleARD_AI = inertiaProps->getTensor_EpauleARD();
    EpauleARD_p = - fext[EPAULEARD];
    HJambeARD_AI = inertiaProps->getTensor_HJambeARD();
    HJambeARD_p = - fext[HJAMBEARD];
    BJambeARD_AI = inertiaProps->getTensor_BJambeARD();
    BJambeARD_p = - fext[BJAMBEARD];
    EpauleARG_AI = inertiaProps->getTensor_EpauleARG();
    EpauleARG_p = - fext[EPAULEARG];
    HJambeARG_AI = inertiaProps->getTensor_HJambeARG();
    HJambeARG_p = - fext[HJAMBEARG];
    BJambeARG_AI = inertiaProps->getTensor_BJambeARG();
    BJambeARG_p = - fext[BJAMBEARG];
    // ---------------------- FIRST PASS ---------------------- //
    // Note that, during the first pass, the articulated inertias are really
    //  just the spatial inertia of the links (see assignments above).
    //  Afterwards things change, and articulated inertias shall not be used
    //  in functions which work specifically with spatial inertias.

    // + Link EpauleAVD
    //  - The spatial velocity:
    EpauleAVD_v = (motionTransforms-> fr_EpauleAVD_X_fr_base) * base_v;
    EpauleAVD_v(AZ) += qd(RF_HAA_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(EpauleAVD_v, vcross);
    EpauleAVD_c = vcross.col(AZ) * qd(RF_HAA_JOINT);

    //  - The bias force term:
    EpauleAVD_p += vxIv(EpauleAVD_v, EpauleAVD_AI);

    // + Link HJambeAVD
    //  - The spatial velocity:
    HJambeAVD_v = (motionTransforms-> fr_HJambeAVD_X_fr_EpauleAVD) * EpauleAVD_v;
    HJambeAVD_v(AZ) += qd(RF_HFE_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(HJambeAVD_v, vcross);
    HJambeAVD_c = vcross.col(AZ) * qd(RF_HFE_JOINT);

    //  - The bias force term:
    HJambeAVD_p += vxIv(HJambeAVD_v, HJambeAVD_AI);

    // + Link BJambeAVD
    //  - The spatial velocity:
    BJambeAVD_v = (motionTransforms-> fr_BJambeAVD_X_fr_HJambeAVD) * HJambeAVD_v;
    BJambeAVD_v(AZ) += qd(RF_KFE_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(BJambeAVD_v, vcross);
    BJambeAVD_c = vcross.col(AZ) * qd(RF_KFE_JOINT);

    //  - The bias force term:
    BJambeAVD_p += vxIv(BJambeAVD_v, BJambeAVD_AI);

    // + Link EpauleAVG
    //  - The spatial velocity:
    EpauleAVG_v = (motionTransforms-> fr_EpauleAVG_X_fr_base) * base_v;
    EpauleAVG_v(AZ) += qd(LF_HAA_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(EpauleAVG_v, vcross);
    EpauleAVG_c = vcross.col(AZ) * qd(LF_HAA_JOINT);

    //  - The bias force term:
    EpauleAVG_p += vxIv(EpauleAVG_v, EpauleAVG_AI);

    // + Link HJambeAVG
    //  - The spatial velocity:
    HJambeAVG_v = (motionTransforms-> fr_HJambeAVG_X_fr_EpauleAVG) * EpauleAVG_v;
    HJambeAVG_v(AZ) += qd(LF_HFE_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(HJambeAVG_v, vcross);
    HJambeAVG_c = vcross.col(AZ) * qd(LF_HFE_JOINT);

    //  - The bias force term:
    HJambeAVG_p += vxIv(HJambeAVG_v, HJambeAVG_AI);

    // + Link BJambeAVG
    //  - The spatial velocity:
    BJambeAVG_v = (motionTransforms-> fr_BJambeAVG_X_fr_HJambeAVG) * HJambeAVG_v;
    BJambeAVG_v(AZ) += qd(LF_KFE_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(BJambeAVG_v, vcross);
    BJambeAVG_c = vcross.col(AZ) * qd(LF_KFE_JOINT);

    //  - The bias force term:
    BJambeAVG_p += vxIv(BJambeAVG_v, BJambeAVG_AI);

    // + Link EpauleARD
    //  - The spatial velocity:
    EpauleARD_v = (motionTransforms-> fr_EpauleARD_X_fr_base) * base_v;
    EpauleARD_v(AZ) += qd(RH_HAA_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(EpauleARD_v, vcross);
    EpauleARD_c = vcross.col(AZ) * qd(RH_HAA_JOINT);

    //  - The bias force term:
    EpauleARD_p += vxIv(EpauleARD_v, EpauleARD_AI);

    // + Link HJambeARD
    //  - The spatial velocity:
    HJambeARD_v = (motionTransforms-> fr_HJambeARD_X_fr_EpauleARD) * EpauleARD_v;
    HJambeARD_v(AZ) += qd(RH_HFE_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(HJambeARD_v, vcross);
    HJambeARD_c = vcross.col(AZ) * qd(RH_HFE_JOINT);

    //  - The bias force term:
    HJambeARD_p += vxIv(HJambeARD_v, HJambeARD_AI);

    // + Link BJambeARD
    //  - The spatial velocity:
    BJambeARD_v = (motionTransforms-> fr_BJambeARD_X_fr_HJambeARD) * HJambeARD_v;
    BJambeARD_v(AZ) += qd(RH_KFE_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(BJambeARD_v, vcross);
    BJambeARD_c = vcross.col(AZ) * qd(RH_KFE_JOINT);

    //  - The bias force term:
    BJambeARD_p += vxIv(BJambeARD_v, BJambeARD_AI);

    // + Link EpauleARG
    //  - The spatial velocity:
    EpauleARG_v = (motionTransforms-> fr_EpauleARG_X_fr_base) * base_v;
    EpauleARG_v(AZ) += qd(LH_HAA_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(EpauleARG_v, vcross);
    EpauleARG_c = vcross.col(AZ) * qd(LH_HAA_JOINT);

    //  - The bias force term:
    EpauleARG_p += vxIv(EpauleARG_v, EpauleARG_AI);

    // + Link HJambeARG
    //  - The spatial velocity:
    HJambeARG_v = (motionTransforms-> fr_HJambeARG_X_fr_EpauleARG) * EpauleARG_v;
    HJambeARG_v(AZ) += qd(LH_HFE_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(HJambeARG_v, vcross);
    HJambeARG_c = vcross.col(AZ) * qd(LH_HFE_JOINT);

    //  - The bias force term:
    HJambeARG_p += vxIv(HJambeARG_v, HJambeARG_AI);

    // + Link BJambeARG
    //  - The spatial velocity:
    BJambeARG_v = (motionTransforms-> fr_BJambeARG_X_fr_HJambeARG) * HJambeARG_v;
    BJambeARG_v(AZ) += qd(LH_KFE_JOINT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(BJambeARG_v, vcross);
    BJambeARG_c = vcross.col(AZ) * qd(LH_KFE_JOINT);

    //  - The bias force term:
    BJambeARG_p += vxIv(BJambeARG_v, BJambeARG_AI);

    // + The floating base body
    base_p += vxIv(base_v, base_AI);

    // ---------------------- SECOND PASS ---------------------- //
    Matrix66 IaB;
    Force pa;

    // + Link BJambeARG
    BJambeARG_u = tau(LH_KFE_JOINT) - BJambeARG_p(AZ);
    BJambeARG_U = BJambeARG_AI.col(AZ);
    BJambeARG_D = BJambeARG_U(AZ);

    compute_Ia_revolute(BJambeARG_AI, BJambeARG_U, BJambeARG_D, Ia_r);  // same as: Ia_r = BJambeARG_AI - BJambeARG_U/BJambeARG_D * BJambeARG_U.transpose();
    pa = BJambeARG_p + Ia_r * BJambeARG_c + BJambeARG_U * BJambeARG_u/BJambeARG_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_BJambeARG_X_fr_HJambeARG, IaB);
    HJambeARG_AI += IaB;
    HJambeARG_p += (motionTransforms-> fr_BJambeARG_X_fr_HJambeARG).transpose() * pa;

    // + Link HJambeARG
    HJambeARG_u = tau(LH_HFE_JOINT) - HJambeARG_p(AZ);
    HJambeARG_U = HJambeARG_AI.col(AZ);
    HJambeARG_D = HJambeARG_U(AZ);

    compute_Ia_revolute(HJambeARG_AI, HJambeARG_U, HJambeARG_D, Ia_r);  // same as: Ia_r = HJambeARG_AI - HJambeARG_U/HJambeARG_D * HJambeARG_U.transpose();
    pa = HJambeARG_p + Ia_r * HJambeARG_c + HJambeARG_U * HJambeARG_u/HJambeARG_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_HJambeARG_X_fr_EpauleARG, IaB);
    EpauleARG_AI += IaB;
    EpauleARG_p += (motionTransforms-> fr_HJambeARG_X_fr_EpauleARG).transpose() * pa;

    // + Link EpauleARG
    EpauleARG_u = tau(LH_HAA_JOINT) - EpauleARG_p(AZ);
    EpauleARG_U = EpauleARG_AI.col(AZ);
    EpauleARG_D = EpauleARG_U(AZ);

    compute_Ia_revolute(EpauleARG_AI, EpauleARG_U, EpauleARG_D, Ia_r);  // same as: Ia_r = EpauleARG_AI - EpauleARG_U/EpauleARG_D * EpauleARG_U.transpose();
    pa = EpauleARG_p + Ia_r * EpauleARG_c + EpauleARG_U * EpauleARG_u/EpauleARG_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_EpauleARG_X_fr_base, IaB);
    base_AI += IaB;
    base_p += (motionTransforms-> fr_EpauleARG_X_fr_base).transpose() * pa;

    // + Link BJambeARD
    BJambeARD_u = tau(RH_KFE_JOINT) - BJambeARD_p(AZ);
    BJambeARD_U = BJambeARD_AI.col(AZ);
    BJambeARD_D = BJambeARD_U(AZ);

    compute_Ia_revolute(BJambeARD_AI, BJambeARD_U, BJambeARD_D, Ia_r);  // same as: Ia_r = BJambeARD_AI - BJambeARD_U/BJambeARD_D * BJambeARD_U.transpose();
    pa = BJambeARD_p + Ia_r * BJambeARD_c + BJambeARD_U * BJambeARD_u/BJambeARD_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_BJambeARD_X_fr_HJambeARD, IaB);
    HJambeARD_AI += IaB;
    HJambeARD_p += (motionTransforms-> fr_BJambeARD_X_fr_HJambeARD).transpose() * pa;

    // + Link HJambeARD
    HJambeARD_u = tau(RH_HFE_JOINT) - HJambeARD_p(AZ);
    HJambeARD_U = HJambeARD_AI.col(AZ);
    HJambeARD_D = HJambeARD_U(AZ);

    compute_Ia_revolute(HJambeARD_AI, HJambeARD_U, HJambeARD_D, Ia_r);  // same as: Ia_r = HJambeARD_AI - HJambeARD_U/HJambeARD_D * HJambeARD_U.transpose();
    pa = HJambeARD_p + Ia_r * HJambeARD_c + HJambeARD_U * HJambeARD_u/HJambeARD_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_HJambeARD_X_fr_EpauleARD, IaB);
    EpauleARD_AI += IaB;
    EpauleARD_p += (motionTransforms-> fr_HJambeARD_X_fr_EpauleARD).transpose() * pa;

    // + Link EpauleARD
    EpauleARD_u = tau(RH_HAA_JOINT) - EpauleARD_p(AZ);
    EpauleARD_U = EpauleARD_AI.col(AZ);
    EpauleARD_D = EpauleARD_U(AZ);

    compute_Ia_revolute(EpauleARD_AI, EpauleARD_U, EpauleARD_D, Ia_r);  // same as: Ia_r = EpauleARD_AI - EpauleARD_U/EpauleARD_D * EpauleARD_U.transpose();
    pa = EpauleARD_p + Ia_r * EpauleARD_c + EpauleARD_U * EpauleARD_u/EpauleARD_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_EpauleARD_X_fr_base, IaB);
    base_AI += IaB;
    base_p += (motionTransforms-> fr_EpauleARD_X_fr_base).transpose() * pa;

    // + Link BJambeAVG
    BJambeAVG_u = tau(LF_KFE_JOINT) - BJambeAVG_p(AZ);
    BJambeAVG_U = BJambeAVG_AI.col(AZ);
    BJambeAVG_D = BJambeAVG_U(AZ);

    compute_Ia_revolute(BJambeAVG_AI, BJambeAVG_U, BJambeAVG_D, Ia_r);  // same as: Ia_r = BJambeAVG_AI - BJambeAVG_U/BJambeAVG_D * BJambeAVG_U.transpose();
    pa = BJambeAVG_p + Ia_r * BJambeAVG_c + BJambeAVG_U * BJambeAVG_u/BJambeAVG_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_BJambeAVG_X_fr_HJambeAVG, IaB);
    HJambeAVG_AI += IaB;
    HJambeAVG_p += (motionTransforms-> fr_BJambeAVG_X_fr_HJambeAVG).transpose() * pa;

    // + Link HJambeAVG
    HJambeAVG_u = tau(LF_HFE_JOINT) - HJambeAVG_p(AZ);
    HJambeAVG_U = HJambeAVG_AI.col(AZ);
    HJambeAVG_D = HJambeAVG_U(AZ);

    compute_Ia_revolute(HJambeAVG_AI, HJambeAVG_U, HJambeAVG_D, Ia_r);  // same as: Ia_r = HJambeAVG_AI - HJambeAVG_U/HJambeAVG_D * HJambeAVG_U.transpose();
    pa = HJambeAVG_p + Ia_r * HJambeAVG_c + HJambeAVG_U * HJambeAVG_u/HJambeAVG_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_HJambeAVG_X_fr_EpauleAVG, IaB);
    EpauleAVG_AI += IaB;
    EpauleAVG_p += (motionTransforms-> fr_HJambeAVG_X_fr_EpauleAVG).transpose() * pa;

    // + Link EpauleAVG
    EpauleAVG_u = tau(LF_HAA_JOINT) - EpauleAVG_p(AZ);
    EpauleAVG_U = EpauleAVG_AI.col(AZ);
    EpauleAVG_D = EpauleAVG_U(AZ);

    compute_Ia_revolute(EpauleAVG_AI, EpauleAVG_U, EpauleAVG_D, Ia_r);  // same as: Ia_r = EpauleAVG_AI - EpauleAVG_U/EpauleAVG_D * EpauleAVG_U.transpose();
    pa = EpauleAVG_p + Ia_r * EpauleAVG_c + EpauleAVG_U * EpauleAVG_u/EpauleAVG_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_EpauleAVG_X_fr_base, IaB);
    base_AI += IaB;
    base_p += (motionTransforms-> fr_EpauleAVG_X_fr_base).transpose() * pa;

    // + Link BJambeAVD
    BJambeAVD_u = tau(RF_KFE_JOINT) - BJambeAVD_p(AZ);
    BJambeAVD_U = BJambeAVD_AI.col(AZ);
    BJambeAVD_D = BJambeAVD_U(AZ);

    compute_Ia_revolute(BJambeAVD_AI, BJambeAVD_U, BJambeAVD_D, Ia_r);  // same as: Ia_r = BJambeAVD_AI - BJambeAVD_U/BJambeAVD_D * BJambeAVD_U.transpose();
    pa = BJambeAVD_p + Ia_r * BJambeAVD_c + BJambeAVD_U * BJambeAVD_u/BJambeAVD_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_BJambeAVD_X_fr_HJambeAVD, IaB);
    HJambeAVD_AI += IaB;
    HJambeAVD_p += (motionTransforms-> fr_BJambeAVD_X_fr_HJambeAVD).transpose() * pa;

    // + Link HJambeAVD
    HJambeAVD_u = tau(RF_HFE_JOINT) - HJambeAVD_p(AZ);
    HJambeAVD_U = HJambeAVD_AI.col(AZ);
    HJambeAVD_D = HJambeAVD_U(AZ);

    compute_Ia_revolute(HJambeAVD_AI, HJambeAVD_U, HJambeAVD_D, Ia_r);  // same as: Ia_r = HJambeAVD_AI - HJambeAVD_U/HJambeAVD_D * HJambeAVD_U.transpose();
    pa = HJambeAVD_p + Ia_r * HJambeAVD_c + HJambeAVD_U * HJambeAVD_u/HJambeAVD_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_HJambeAVD_X_fr_EpauleAVD, IaB);
    EpauleAVD_AI += IaB;
    EpauleAVD_p += (motionTransforms-> fr_HJambeAVD_X_fr_EpauleAVD).transpose() * pa;

    // + Link EpauleAVD
    EpauleAVD_u = tau(RF_HAA_JOINT) - EpauleAVD_p(AZ);
    EpauleAVD_U = EpauleAVD_AI.col(AZ);
    EpauleAVD_D = EpauleAVD_U(AZ);

    compute_Ia_revolute(EpauleAVD_AI, EpauleAVD_U, EpauleAVD_D, Ia_r);  // same as: Ia_r = EpauleAVD_AI - EpauleAVD_U/EpauleAVD_D * EpauleAVD_U.transpose();
    pa = EpauleAVD_p + Ia_r * EpauleAVD_c + EpauleAVD_U * EpauleAVD_u/EpauleAVD_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_EpauleAVD_X_fr_base, IaB);
    base_AI += IaB;
    base_p += (motionTransforms-> fr_EpauleAVD_X_fr_base).transpose() * pa;

    // + The acceleration of the floating base base, without gravity
    Eigen::LLT<Matrix66d> llt(base_AI);
    base_a = - llt.solve(base_p);  // base_a = - IA^-1 * base_p

    // ---------------------- THIRD PASS ---------------------- //
    EpauleAVD_a = (motionTransforms-> fr_EpauleAVD_X_fr_base) * base_a + EpauleAVD_c;
    qdd(RF_HAA_JOINT) = (EpauleAVD_u - EpauleAVD_U.dot(EpauleAVD_a)) / EpauleAVD_D;
    EpauleAVD_a(AZ) += qdd(RF_HAA_JOINT);

    HJambeAVD_a = (motionTransforms-> fr_HJambeAVD_X_fr_EpauleAVD) * EpauleAVD_a + HJambeAVD_c;
    qdd(RF_HFE_JOINT) = (HJambeAVD_u - HJambeAVD_U.dot(HJambeAVD_a)) / HJambeAVD_D;
    HJambeAVD_a(AZ) += qdd(RF_HFE_JOINT);

    BJambeAVD_a = (motionTransforms-> fr_BJambeAVD_X_fr_HJambeAVD) * HJambeAVD_a + BJambeAVD_c;
    qdd(RF_KFE_JOINT) = (BJambeAVD_u - BJambeAVD_U.dot(BJambeAVD_a)) / BJambeAVD_D;
    BJambeAVD_a(AZ) += qdd(RF_KFE_JOINT);

    EpauleAVG_a = (motionTransforms-> fr_EpauleAVG_X_fr_base) * base_a + EpauleAVG_c;
    qdd(LF_HAA_JOINT) = (EpauleAVG_u - EpauleAVG_U.dot(EpauleAVG_a)) / EpauleAVG_D;
    EpauleAVG_a(AZ) += qdd(LF_HAA_JOINT);

    HJambeAVG_a = (motionTransforms-> fr_HJambeAVG_X_fr_EpauleAVG) * EpauleAVG_a + HJambeAVG_c;
    qdd(LF_HFE_JOINT) = (HJambeAVG_u - HJambeAVG_U.dot(HJambeAVG_a)) / HJambeAVG_D;
    HJambeAVG_a(AZ) += qdd(LF_HFE_JOINT);

    BJambeAVG_a = (motionTransforms-> fr_BJambeAVG_X_fr_HJambeAVG) * HJambeAVG_a + BJambeAVG_c;
    qdd(LF_KFE_JOINT) = (BJambeAVG_u - BJambeAVG_U.dot(BJambeAVG_a)) / BJambeAVG_D;
    BJambeAVG_a(AZ) += qdd(LF_KFE_JOINT);

    EpauleARD_a = (motionTransforms-> fr_EpauleARD_X_fr_base) * base_a + EpauleARD_c;
    qdd(RH_HAA_JOINT) = (EpauleARD_u - EpauleARD_U.dot(EpauleARD_a)) / EpauleARD_D;
    EpauleARD_a(AZ) += qdd(RH_HAA_JOINT);

    HJambeARD_a = (motionTransforms-> fr_HJambeARD_X_fr_EpauleARD) * EpauleARD_a + HJambeARD_c;
    qdd(RH_HFE_JOINT) = (HJambeARD_u - HJambeARD_U.dot(HJambeARD_a)) / HJambeARD_D;
    HJambeARD_a(AZ) += qdd(RH_HFE_JOINT);

    BJambeARD_a = (motionTransforms-> fr_BJambeARD_X_fr_HJambeARD) * HJambeARD_a + BJambeARD_c;
    qdd(RH_KFE_JOINT) = (BJambeARD_u - BJambeARD_U.dot(BJambeARD_a)) / BJambeARD_D;
    BJambeARD_a(AZ) += qdd(RH_KFE_JOINT);

    EpauleARG_a = (motionTransforms-> fr_EpauleARG_X_fr_base) * base_a + EpauleARG_c;
    qdd(LH_HAA_JOINT) = (EpauleARG_u - EpauleARG_U.dot(EpauleARG_a)) / EpauleARG_D;
    EpauleARG_a(AZ) += qdd(LH_HAA_JOINT);

    HJambeARG_a = (motionTransforms-> fr_HJambeARG_X_fr_EpauleARG) * EpauleARG_a + HJambeARG_c;
    qdd(LH_HFE_JOINT) = (HJambeARG_u - HJambeARG_U.dot(HJambeARG_a)) / HJambeARG_D;
    HJambeARG_a(AZ) += qdd(LH_HFE_JOINT);

    BJambeARG_a = (motionTransforms-> fr_BJambeARG_X_fr_HJambeARG) * HJambeARG_a + BJambeARG_c;
    qdd(LH_KFE_JOINT) = (BJambeARG_u - BJambeARG_U.dot(BJambeARG_a)) / BJambeARG_D;
    BJambeARG_a(AZ) += qdd(LH_KFE_JOINT);

    // + Add gravity to the acceleration of the floating base
    base_a += g;
}


#include <fstream>
#include <vector>
#include <string>

#include <rw/geometry/Box.hpp>
#include <rw/geometry/PlainTriMesh.hpp>
#include <rw/geometry/Plane.hpp>
#include <rw/math/Random.hpp>
#include <rw/math/Vector3D.hpp>
#include <rw/loaders/model3d/STLFile.hpp>

#include <boost/lexical_cast.hpp>

using namespace rw::geometry;
using rw::loaders::STLFile;
using namespace rw::math;

int main(int argc, char** argv)
{
	static const unsigned int SEED = static_cast<unsigned int>(time(NULL));
	Random::seed(SEED);
	srand(SEED);

    if( argc < 4 ){
		std::cout << "------ Usage: " << std::endl;
	    std::cout << "- Arg 1 name of col data file" << std::endl;
	    std::cout << "- Arg 2 width of texels in meters\n" << std::endl;
	    std::cout << "- Arg 3 name of output stl file\n" << std::endl;
	    return 0;
	}

	std::string filename(argv[1]);
	double width = boost::lexical_cast<double>(argv[2]);
	std::string outfile(argv[3]);
	std::vector<Vector3D<> > texlets;
	char line[256];
	std::ifstream input(filename.c_str());
	// first line is a plane

	float vals[4];
	input.getline(line, 256);
    sscanf(line, "%f %f %f %f", &vals[0], &vals[1], &vals[2], &vals[3]);

    Vector3D<> n(vals[0], vals[1], vals[2]);

    Plane p( normalize(n), ((vals[3])/n.norm2())*-0.001 );

	// and then there are texlets
	while(!input.eof()){
	    input.getline(line, 256);
	    sscanf(line, "%f %f %f", &vals[0], &vals[1], &vals[2]);
	    Vector3D<> v(vals[0],vals[1],vals[2]);
	    v = v*0.001; // the values are in mm
	    texlets.push_back(v);
	}
	input.close();
	// now create the triangle mesh. Use boxes on the texlets
	Box texletBox(width,width,width);
	TriMesh::Ptr texletMesh = texletBox.getTriMesh();
	TriMesh::Ptr planeMesh = p.getTriMesh();
	size_t meshSize = planeMesh->size()+texletMesh->size()*texlets.size();
	PlainTriMesh<> stlMesh(meshSize);

	// and copy plane triangles into the mesh
	for(size_t i=0;i<planeMesh->size();i++)
	    stlMesh[i] = planeMesh->getTriangle(i);

	// and copy every point into the mesh
	for(size_t i=0;i<texlets.size();i++){
	    Transform3D<> transform(texlets[i], Rotation3D<>::identity());
	    for(size_t j=0;j<texletMesh->size();j++){
	        size_t idx = planeMesh->size()+i*texletMesh->size()+j;
	        if(idx>=stlMesh.size())
	            RW_THROW(idx<< ">=" << stlMesh.size() << "  i:" << i << " " << j);
	        stlMesh[idx] = texletMesh->getTriangle(j).transform( transform );
	    }
	}

	STLFile::save(stlMesh, outfile);
	return 0;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/toint.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/ldexp.hpp>

STF_CASE_TPL (" bs::saturated_(bs::toint) real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using r_t = decltype(bs::saturated_(bs::toint)(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T, signed>));

  // specific values tests
  STF_EQUAL(bs::saturated_(bs::toint)(T(2)*bs::Valmax<r_t>()),  bs::Valmax<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(T(2)*bs::Valmin<r_t>()),  bs::Valmin<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(T(1.5)*bs::Valmax<r_t>()),  bs::Valmax<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(T(1.5)*bs::Valmin<r_t>()),  bs::Valmin<r_t>());

  STF_EQUAL(bs::saturated_(bs::toint)(bs::Inf<T>()),  bs::Inf<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(bs::Minf<T>()), bs::Minf<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(bs::Mone<T>()), bs::Mone<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(bs::Nan<T>()),  bs::Zero<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(bs::One<T>()),  bs::One<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(bs::Zero<T>()), bs::Zero<r_t>());

  T v = T(1);
  r_t iv = 1;
  int N = sizeof(T)*8-1;
  for(int i=0; i < N ; i++, v*= 2, iv <<= 1)
  {
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
    STF_EQUAL(bs::saturated_(bs::toint)(v), iv);
    STF_EQUAL(bs::saturated_(bs::toint)(-v), -iv);
  }
  STF_EQUAL(bs::saturated_(bs::toint)(bs::ldexp(bs::One<T>(), N)), bs::Valmax<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(bs::ldexp(bs::One<T>(), N+1)), bs::Valmax<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(-bs::ldexp(bs::One<T>(), N+1)), bs::Valmin<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(-bs::ldexp(bs::One<T>(), N+1)), bs::Valmin<r_t>());

} // end of test for floating_

STF_CASE_TPL (" bs::saturated_(bs::toint) unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using r_t = decltype(bs::saturated_(bs::toint)(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T, signed>));

  // specific values tests
  STF_EQUAL(bs::saturated_(bs::toint)(bs::One<T>()),  bs::One<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" bs::saturated_(bs::toint) signed",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using r_t = decltype(bs::saturated_(bs::toint)(T()));
  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T, signed>));

  // specific values tests
  STF_EQUAL(bs::saturated_(bs::toint)(bs::Mone<T>()), bs::Mone<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(bs::One<T>()),  bs::One<r_t>());
  STF_EQUAL(bs::saturated_(bs::toint)(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for signed_int_


//
// Created by alex on 26/05/20.
//

#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <numeric>
#include <boost/algorithm/string.hpp>
#include <fstream>
#include "FileUtil.h"

using namespace std;

bool BothAreSpaces(char lhs, char rhs) { return (lhs == rhs) && (lhs == ' '); }

pair<vector<pair<int, int>>, int> FileUtil::load_graph(const std::string &file_path) {

    vector<std::vector<int>> vertices;
    string line;
    ifstream file(file_path);
    vector<std::pair<int, int>> edges_vec;
    int n = -1;
    if (file.is_open()) {

        getline(file, line);
        std::string::iterator new_end = std::unique(line.begin(), line.end(), BothAreSpaces);
        line.erase(new_end, line.end());

        boost::trim_right(line);
        boost::trim_left(line);

        vector<std::string> line_vec;
        boost::split(line_vec, line, boost::is_any_of(" "));

        string ninfo = line_vec[0];
        string minfo = line_vec[1];

        line_vec.clear();
        boost::split(line_vec, ninfo, boost::is_any_of("="));
        n = stoi(line_vec[1]);

        line_vec.clear();
        boost::split(line_vec, minfo, boost::is_any_of("="));
        int m = stoi(line_vec[1]);
        edges_vec.reserve(m);

        while (getline(file, line)) {
            line_vec.clear();
            std::replace(std::begin(line),std::end(line),'\t',' ');
            boost::trim_right(line);
            boost::trim_left(line);
            std::string::iterator new_end = std::unique(line.begin(), line.end(), BothAreSpaces);
            line.erase(new_end, line.end());

            boost::split(line_vec, line, boost::is_any_of(" "));

           int i = stoi(line_vec[0])-1;
           int j = stoi(line_vec[1])-1;
           edges_vec.emplace_back(i,j);
        }
        file.close();
    } else {
        cerr << "Unable to open file" << std::endl;
    }
    return make_pair(edges_vec, n);
}

void FileUtil::floydWarshall(std::vector<std::vector<float>> &G) {
    int n = G.size();
    for (int i = 0; i < n; ++i) {
        for (int j = 0; j < n; ++j) {
            for (int l = 0; l < n; ++l) {
                float cost = (G[i][j] == +INFINITY || G[i][l] == +INFINITY) ?
                             +INFINITY : G[i][j] + G[i][l];
                if (cost < G[j][l]) {
                    G[j][l] = cost;
                }
            }
        }
    }
}

float FileUtil::stdDev(std::vector<float> &items, float average) {
    float std = 0;
    for (float item : items) {
        std += pow(item - average, 2);
    }
    int n = items.size() > 2 ? items.size() - 1 : items.size();
    return sqrt(std / n);
}

bool FileUtil::save(std::string &output_path, std::string &content) {
    std::ofstream output_file(output_path);
//    output_file.write(&content, content.size());
    output_file << content;
    output_file.close();
    return true;
}


#define BOOST_TEST_ALTERNATIVE_INIT_API
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/parameterized_test.hpp>
#include <boost/bind.hpp>

class test_class {
    public:
        void test_method(double d) {
            BOOST_TEST(d * 100 == (double)(int)(d * 100),
                       boost::test_tools::tolerance(0.0001));
        }
} tester;

bool init_unit_test() {
    double params[] = { 1., 1.00001 };

    boost::function<void(double)> test_method =
        boost::bind(&test_class::test_method, &tester, _1);

    boost::unit_test::framework::master_test_suite().
        add(BOOST_PARAM_TEST_CASE(test_method, params, params + 2));

    return true;
}


#include <mtl/dense1D.h>
#include <mtl/mtl.h>
#include <mtl/utils.h>

/*
  example output:

  [4,4,4,4,4,4,4,4,4,4]

  */

int
main()
{
  using namespace mtl;
  //begin
  dense1D<double> x(10,1);
  dense1D<double> y(10);
  double alpha = 4.0;
  mtl::copy(scaled(x, alpha), y);
  //end
  print_vector(y);
  return 0;
}


#include "generator.h"
#include "delaunay.h"
#include "minimum_spanning_tree.h"
#include "validate.h"
#include "visualization.h"
#include <boost/program_options.hpp>

using namespace boost::program_options;

int main(int argc, char *argv[])
{
    //set generate option
    int GENERATE_CASE = 1;

    //set the filename to save these points' & graph's data
    std::string PREFIX, SUFFIX;
    std::string POINTS_FILENAME;
    std::string TRIANGULATION_FILENAME;
    std::string VORONOI_FILENAME;
    std::string MST_FILENAME;

    //save points into this container
    std::vector<Simple_Point> raw_data;

    //save trangulation information into this container
    std::vector<Edge> triangulation_data;

    //save voronoi diagram infomation into this container
    std::vector<std::pair<Simple_Point, Simple_Point> >voronoi_data;

    //save mst infomation into this container
    std::vector<Edge> mst_data;

    //read from command line
    try
    {
        options_description desc("Options");
        desc.add_options()
        ("help,h", "display this help and exit")
        ("number,n", value<int>(&Generator::TOTAL_NUMBER_OF_POINTS)->default_value(10000), "number of points")
        ("circle,c", "reset generation method to random in a circle, default: random in full square")
        ("test,t", value<std::string>(&PREFIX)->default_value("../../testcase/test/"), "test mode & set file path & read data from data.txt")
        ("range,r", value<int>(&Generator::MAX_COORDINATE)->default_value(10000), "coordinate range [0, number)")
        ("distance,d", value<ld>(&Generator::MIN_DISTANCE)->default_value(1e-3), "minimum distance between each pair of points")
        ("img-size,i", value<int>(&Visualization::IMAGE_SIZE)->default_value(2000), "size of image")
        ("suffix,s", value<std::string>(&SUFFIX)->default_value("png"), "set the suffix of output image")
        ("window,w", "display the window")
        ("voronoi,v", "output voronoi diagram infomation");

        variables_map vm;
        store(parse_command_line(argc, argv, desc), vm);
        notify(vm);

        if (vm.count("help"))
        {
            std::cout << "Delaunay triangulation for calculating Euclidean distance minimum spanning tree" << std::endl;
            std::cout << "Made by n+e\t2017-04" << std::endl;
            std::cout << std::endl;
            std::cout << desc << std::endl;
            std::cout << "Examples:" << std::endl;
            std::cout << "Generate new testcase in default file path, 10 points in [0,10), save in jpg file, and show window:" << std::endl;
            std::cout << "\t./main -n 10 -r 10 -s jpg -w" << std::endl;
            std::cout << "Use circle-shape testcase in file \"../../testcase/circle_100000/\" and output voronoi message:" << std::endl;
            std::cout << "\t./main -t ../../testcase/circle_100000/ -v" << std::endl << std::endl;
            return 0;
        }
        if (Generator::TOTAL_NUMBER_OF_POINTS <= 0 || Generator::TOTAL_NUMBER_OF_POINTS >= 5000000)
        {
            std::cerr << "Number of points is invalid!" << std::endl;
            return 1;
        }
        if (Visualization::IMAGE_SIZE <= 0 || Visualization::IMAGE_SIZE > 5000)
        {
            std::cerr << "Size of image is invalid!" << std::endl;
            return 1;
        }
        if (Generator::MAX_COORDINATE <= 1)
        {
            std::cerr << "max coordinate is invalid!" << std::endl;
            return 1;
        }
        if (Generator::MIN_DISTANCE <= 0)
        {
            std::cerr << "min distance is invalid!" << std::endl;
            return 1;
        }
        if (vm.count("circle"))
            Generator::METHOD_CASE = 1;
        if (vm.count("voronoi"))
            Delaunay_Triangulation::VORONOI_CASE = 1;
        if (PREFIX != "../../testcase/test/")
        {
            GENERATE_CASE = 0;
            if (PREFIX[PREFIX.length() - 1] != '/')
                PREFIX += '/';
        }
        POINTS_FILENAME = PREFIX + "data.txt";
        TRIANGULATION_FILENAME = PREFIX + "triangulation.txt";
        VORONOI_FILENAME = PREFIX + "voronoi.txt";
        MST_FILENAME = PREFIX + "mst.txt";

        //generate data
        if (GENERATE_CASE != 0)
            raw_data = Generator().Save(POINTS_FILENAME);
        else
        {
            TIME_BEGIN("Read data from data.txt")
            std::ifstream in;
            in.open(POINTS_FILENAME.c_str());
            ld x, y;
            Generator::MAX_COORDINATE = 0;
            while (in >> x >> y)
            {
                raw_data.push_back(Simple_Point(x, y));
                if (Generator::MAX_COORDINATE < x)Generator::MAX_COORDINATE = x;
                if (Generator::MAX_COORDINATE < y)Generator::MAX_COORDINATE = y;
            }
            std::sort(raw_data.begin(), raw_data.end());
            Generator::TOTAL_NUMBER_OF_POINTS = raw_data.size();
            in.close();
            TIME_END
        }
        //calculate triangulation using CGAL library
        Delaunay_Triangulation cgal_dt(raw_data);
        cgal_dt.Save(TRIANGULATION_FILENAME, VORONOI_FILENAME, triangulation_data, voronoi_data);
        //calculate mst with kruskal
        MST mymst(triangulation_data);
        mst_data = mymst.Save(MST_FILENAME);
        //check delaunay triangulation
        My_Delaunay myd(raw_data);
        //check mst with prim
        Prim prim(raw_data);
        //visualize the result
        Visualization visual(vm.count("window"), PREFIX, SUFFIX, raw_data, triangulation_data, voronoi_data, mst_data);
    }
    catch (const error &e)
    {
        std::cerr << e.what() << std::endl;
        return 1;
    }
    return 0;
}


#include <iostream>
#include <vector>
#include <utility>
#include <iterator>
#include <cstdlib>
#include <limits>
#include <cerrno>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/concept_check.hpp>
#include <boost/operators.hpp>
#include <boost/iterator.hpp>
#include <boost/multi_array.hpp>
#include <boost/random.hpp>
#include "edmonds_optimum_branching.hpp"

//#define DEBUG

using namespace std;
using namespace boost;

struct Line {
    unsigned int from, to, price;

    Line(unsigned int from, unsigned int to, unsigned int price) : from(from), to(to), price(price) {};
};

struct Result {
    bool feasibility;
    unsigned long total_cost;
    unsigned int depot_id;
    std::vector<Line> rec_offers;
};

class GraphProcessor {
public:
    bool load_input();
    void find_hub();
    void send_result();

private:
    Result last_res;
    int cities_cnt;
    std::vector<Line> lines;
};

// definitions of a complete graph that implements the EdgeListGraph
// concept of Boost's graph library.
namespace boost {
    struct complete_graph {
        complete_graph(int n_vertices) : n_vertices(n_vertices) {}
        int n_vertices;

        struct edge_iterator : public input_iterator_helper<edge_iterator, int, std::ptrdiff_t, int const *, int>
        {
            int edge_idx, n_vertices;

            edge_iterator() : edge_idx(0), n_vertices(-1) {}
            edge_iterator(int n_vertices, int edge_idx) : edge_idx(edge_idx), n_vertices(n_vertices) {}
            edge_iterator &operator++()
            {
                if (edge_idx >= n_vertices * n_vertices)
                    return *this;
                ++edge_idx;
                if (edge_idx / n_vertices == edge_idx % n_vertices)
                    ++edge_idx;
                return *this;
            }
            int operator*() const {return edge_idx;}
            bool operator==(const edge_iterator &iter) const
            {
                return edge_idx == iter.edge_idx;
            }
        };
    };

    template<>
    struct graph_traits<complete_graph> {
        typedef int                             vertex_descriptor;
        typedef int                             edge_descriptor;
        typedef directed_tag                    directed_category;
        typedef disallow_parallel_edge_tag      edge_parallel_category;
        typedef edge_list_graph_tag             traversal_category;
        typedef complete_graph::edge_iterator   edge_iterator;
        typedef unsigned                        edges_size_type;

        static vertex_descriptor null_vertex() {return -1;}
    };

    pair<complete_graph::edge_iterator, complete_graph::edge_iterator>
    edges(const complete_graph &g)
    {
        return make_pair(complete_graph::edge_iterator(g.n_vertices, 1),
                         complete_graph::edge_iterator(g.n_vertices, g.n_vertices*g.n_vertices));
    }

    unsigned
    num_edges(const complete_graph &g)
    {
        return (g.n_vertices - 1) * (g.n_vertices - 1);
    }

    int
    source(int edge, const complete_graph &g)
    {
        return edge / g.n_vertices;
    }

    int
    target(int edge, const complete_graph &g)
    {
        return edge % g.n_vertices;
    }
}

typedef graph_traits<complete_graph>::edge_descriptor Edge;
typedef graph_traits<complete_graph>::vertex_descriptor Vertex;


#ifndef BG_UTILS_HPP
#define BG_UTILS_HPP

#include "bg_types.hpp"

#include <boost/geometry/srs/epsg.hpp>
#include <boost/geometry/srs/projection.hpp>
#include <boost/geometry/srs/projections/proj4.hpp>

#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>

template <int ppc, class T>
Multipolygon2D buffer2D(T && g, float width) {
    Multipolygon2D mp2d;
    boost::geometry::strategy::buffer::distance_symmetric<float>
        distance_strategy(width / 2);
    boost::geometry::strategy::buffer::join_round join_strategy(ppc);
    boost::geometry::strategy::buffer::end_flat end_strategy;
    boost::geometry::strategy::buffer::point_circle point_strategy(ppc);
    boost::geometry::strategy::buffer::side_straight side_strategy;
    boost::geometry::buffer(g, mp2d, distance_strategy, side_strategy,
                            join_strategy, end_strategy, point_strategy);
    return mp2d;
}

template <class Point>
MultipolygonGeo buffer_PointGeo(Point && p, float width) {
    boost::geometry::srs::projection<> proj = boost::geometry::srs::proj4(
        "+proj=eqc +ellps=GRS80 +lon_0=" + std::to_string(p.x()) +
        " +lat_0=" + std::to_string(p.y()));

    MultipolygonGeo mp;

    Point2D p2d;
    proj.forward(p, p2d);
    Multipolygon2D mp2d = buffer2D<8>(p2d, width);
    proj.inverse(mp2d, mp);

    return mp;
}

template <class Linestring>
MultipolygonGeo buffer_LinestringGeo(Linestring && l, float width) {
    PointGeo center = boost::geometry::return_centroid<PointGeo>(
        boost::geometry::return_envelope<BoxGeo>(l));
    boost::geometry::srs::projection<> proj = boost::geometry::srs::proj4(
        "+proj=eqc +ellps=GRS80 +lon_0=" + std::to_string(center.x()) +
        " +lat_0=" + std::to_string(center.y()));

    MultipolygonGeo mp;

    Linestring2D l2d;
    proj.forward(l, l2d);

    Linestring2D l2d_simplified;
    boost::geometry::simplify(l2d, l2d_simplified, 0.5);

    l2d_simplified[0] = Point2D(
        (9999 * l2d_simplified[0].get<0>() + l2d_simplified[1].get<0>()) /
            10000,
        (9999 * l2d_simplified[0].get<1>() + l2d_simplified[1].get<1>()) /
            10000);

    const int last_id = l2d_simplified.size() - 1;
    l2d_simplified[last_id] = Point2D((9999 * l2d_simplified[last_id].get<0>() +
                                       l2d_simplified[last_id - 1].get<0>()) /
                                          10000,
                                      (9999 * l2d_simplified[last_id].get<1>() +
                                       l2d_simplified[last_id - 1].get<1>()) /
                                          10000);

    Multipolygon2D mp2d = buffer2D<4>(l2d_simplified, width);

    Multipolygon2D mp2d_simplified;
    boost::geometry::simplify(mp2d, mp2d_simplified, 0.5);

    proj.inverse(mp2d_simplified, mp);

    // if(! boost::geometry::is_valid(mp)) {
    //     std::string m;
    //     boost::geometry::is_valid(mp, m);
    //     throw std::runtime_error{m};
    // }

    return mp;
}

#endif  // BG_UTILS_HPP


/*

Adopted from Thomas Prest's project https://github.com/tprest/Lattice-IBE and edited for this transformation.
Therefore, please respect its license and requirements.

*/

#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>

#include "params.h"
#include "io.h"
#include "FFT.h"
#include "Sampling.h"
#include "Random.h"
#include "Algebra.h"
#include "Scheme.h"

using namespace std;
using namespace NTL;

//==============================================================================
//==============================================================================
//                                  MAIN
//==============================================================================
//==============================================================================

int main()
{
    cout << "\n=======================================================================\n";
    cout << "This program is a proof-of concept for efficient PEKS over lattices.\n";
    cout << "It generates a NTRU lattice of dimension 2N and associated modulus q,\n";
    cout << "and perform benches and tests, for user PEKS and Trapdoor  and Test.";
    cout << "\n=======================================================================\n\n";

    ZZX MSK[4];
    ZZ_pX phiq, MPK;
    unsigned int i;
    float diff;
    MSK_Data * MSKD = new MSK_Data;
    MPK_Data * MPKD = new MPK_Data;
    clock_t t1, t2;
    const ZZX phi = Cyclo();

    srand(rdtsc()); // initialisation of rand
    cout << "N = " << N0 << endl;
    cout << "q = " << q0 << endl;

    ZZ_p::init(q1);
    zz_p::init(q0);

    phiq = conv<ZZ_pX>(phi);
    ZZ_pXModulus PHI(phiq);

    cout << "\n===================================================================\n KEY GENERATION";
    cout << "\n===================================================================\n";
    t1 = clock();
    for(i=0; i<1; i++)
    {
        Keygen(MPK, MSK);
    }

    CompleteMSK(MSKD, MSK);
    CompleteMPK(MPKD, MPK);

    t2 = clock();
    diff = ((float)t2 - (float)t1)/1000000.0F;
    cout << "It took " << diff << " seconds to generate user keys" << endl;

    //==============================================================================
    //Key extraction bench and encryption/decryption bench
    //==============================================================================
    const unsigned int nb_trdb = 1000;
    const unsigned int nb_crypb = 1000;
    //const unsigned int nb_decrypb = 1000;

    cout << "\n===================================================================\n RUNNING PEKS BENCH FOR ";
    cout << nb_crypb << " DIFFERENT KEYWORDS\n===================================================================\n";
    Encrypt_Bench(nb_crypb, MPKD, MSKD);

    cout << "\n===================================================================\n RUNNING TRAPDOOR BENCH FOR ";
    cout << nb_trdb << " DIFFERENT KEYWORDS\n===================================================================\n";
    Trapdoor_Bench(nb_trdb, MSKD);

    ///==============================================================================
    //Key extraction test and encryption/decryption test
    //==============================================================================
    const unsigned int nb_trdt = 100;
    const unsigned int nb_crypt = 100;

    cout << "\n===================================================================\n CHECKING PEKS VALIDITY FOR ";
    cout << nb_crypt << " DIFFERENT KEYWORDS\n===================================================================\n";
    Encrypt_Test(nb_crypt, MPKD, MSKD);

    cout << "\n===================================================================\n CHECKING TRAPDOOR VALIDITY FOR ";
    cout << nb_trdt << " DIFFERENT KEYWORDS\n===================================================================\n";
	Trapdoor_Test(nb_trdt, MSKD);

    free(MSKD);
    free(MPKD);
    return 0;
}


#include <iostream>
#include <string>
#include <sstream>
#include <NTL/ZZ_pX.h>
#include <NTL/mat_ZZ_p.h>
#include <NTL/vec_ZZ_p.h>

using namespace NTL;
std::stringstream outstream;
std::string tStr;

static const char *ret_stream(void);
static char *prep_string(char *s);
extern "C" {
    const char *convert_matrix(char *in);
    const char *compute_laurent(char *t_yP, char *t_r, char *t_s);
    void clear_stringstream(void);
    void init(char *p);
}

void init(char *p) {
    ZZ_p::init(conv<ZZ>(p));
}

void clear_stringstream(void) {
    outstream.clear();
    outstream.str(std::string());
}

static const char *ret_stream(void) {
    tStr = std::move(outstream.str());
    for (auto it = tStr.begin(); it != tStr.end(); it++) {
        if ((*it == '\n') || (*it == ' ')) {
            *it = ',';
        }
    }
    return tStr.c_str();
}

static char *prep_string(char *s) {
    for (unsigned i = 0; s[i] != '\0'; i++) {
        if ((s[i] == ',') || (s[i] == 'L')) {
            s[i] = ' ';
        }
    }
    return s;
}

const char *convert_matrix(char *in) {
    clear_stringstream();

    Mat<ZZ_p> mat{conv<Mat<ZZ_p>>(prep_string(in))};
    outstream << mat;
    return ret_stream();
}

const char *compute_laurent(char *t_yP, char *t_r, char *t_s) {
    clear_stringstream();

    // convert
    Mat<ZZ_p> rT, rPT;
    unsigned n;
#ifdef EXTRATESTS
    Vec<ZZ_p> uv;
#endif
    {
        Vec<ZZ_p> yP{conv<Vec<ZZ_p>>(prep_string(t_yP))};
        Mat<ZZ_p> r{conv<Mat<ZZ_p>>(prep_string(t_r))};
        Mat<ZZ_p> s{conv<Mat<ZZ_p>>(prep_string(t_s)) * 2};

        // compute r' = r o yP + 2s
        Mat<ZZ_p> rP{r};
        for (unsigned i = 0; i < rP.NumRows(); i++) {
            for (unsigned j = 0; j < rP.NumCols(); j++) {
                rP[i][j] *= yP[j];
            }
            rP[i] += s[i];
        }
        rT = transpose(r);
        rPT = transpose(rP);
        n = yP.length();
#ifdef EXTRATESTS
        ZZ_p tmp;
        uv.SetLength(2*r.NumRows() - 1);
        for (unsigned ui = 0; ui < r.NumRows(); ui++) {
            for (unsigned vi = 0; vi < r.NumRows(); vi++) {
                InnerProduct(tmp, r[ui], rP[vi]);
                uv[ui + vi] += tmp;
            }
        }
#endif
    }

    // now compute r * r' using polymul
    ZZ_pX fg;
    {
        ZZ_pX f, g, tmp;
        for (unsigned j = 0; j < n; j++) {
            f.rep = std::move(rT[j]);
            g.rep = std::move(rPT[j]);
            tmp = f * g;
            fg += tmp;
        }
    }

#ifdef EXTRATESTS
    {
        bool correct = true;
        for (unsigned j = 0; j < uv.length(); j++) {
            correct &= uv[j] == fg[j];
        }
        if (!correct) {
            std::cerr << "ERROR: mismatched results\n" << uv << std::endl << fg << std::endl;
        }
    }
#endif

    outstream << fg;
    return ret_stream();
}


#ifndef SM_UNCERTAIN_TRANSFORMATION_HPP
#define SM_UNCERTAIN_TRANSFORMATION_HPP

#include <boost/serialization/base_object.hpp>
#include "Transformation.hpp"
#include "UncertainHomogeneousPoint.hpp"

namespace sm {
namespace kinematics {

class UncertainTransformation : public Transformation {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef Eigen::Matrix<double, 6, 6> covariance_t;
    ///
    /// Default constructor. The transformation and uncertainty will
    /// both be set to identity.
    ///
    UncertainTransformation();

    UncertainTransformation(const Eigen::Matrix4d& T, const covariance_t& U);

    UncertainTransformation(const Eigen::Matrix4d& T, double diagonalTranslationVariance,
                            double diagonalRotationVariance);

    UncertainTransformation(const Eigen::Vector4d& q_a_b, const Eigen::Vector3d& t_a_b_a, const covariance_t& U);

    UncertainTransformation(const Eigen::Vector4d& q_a_b, const Eigen::Vector3d& t_a_b_a,
                            double diagonalTranslationVariance, double diagonalRotationVariance);

    UncertainTransformation(const Transformation& T, const covariance_t& U);

    UncertainTransformation(const Transformation& T, double diagonalTranslationVariance,
                            double diagonalRotationVariance);

    /// \brief Initialize with zero uncertainty
    UncertainTransformation(const Transformation& T);
    /// \brief Initialize with zero uncertainty
    UncertainTransformation(const Eigen::Matrix4d& T);
    /// \brief Initialize with zero uncertainty
    UncertainTransformation(const Eigen::Vector4d& q_a_b, const Eigen::Vector3d& t_a_b_a);

    virtual ~UncertainTransformation();

    virtual UncertainTransformation operator*(const UncertainTransformation& rhs) const;
    UncertainTransformation operator*(const Transformation& rhs) const;
    UncertainHomogeneousPoint operator*(const HomogeneousPoint& rhs) const;

    Eigen::Vector3d operator*(const Eigen::Vector3d& rhs) const { return Transformation::operator*(rhs); }
    Eigen::Vector4d operator*(const Eigen::Vector4d& rhs) const { return Transformation::operator*(rhs); }

    virtual UncertainHomogeneousPoint operator*(const UncertainHomogeneousPoint& rhs) const;

    Transformation toTransformation() const;
    UncertainTransformation inverse() const;

    const covariance_t& U() const;

    /// \brief hhis gets the uncertainty based on the "oplus" function for updating the transformation matrix.
    covariance_t UOplus() const;

    /// \brief This sets the uncertainty directly.
    void setU(const covariance_t& U);

    /// \brief This sets the uncertainty based on the "oplus" function for updating the transformation matrix.
    /// This is a different uncertainty than that carried around by the class, so one should be careful.
    void setUOplus(const covariance_t& oplusU);

    // Set this to a random transformation.
    virtual void setRandom();

    virtual void setRandom(double translationMaxMeters, double rotationMaxRadians);

    bool isBinaryEqual(const UncertainTransformation& rhs) const;

    enum { CLASS_SERIALIZATION_VERSION = 0 };
    BOOST_SERIALIZATION_SPLIT_MEMBER()
    template <class Archive>
    void save(Archive& ar, const unsigned int version) const;
    template <class Archive>
    void load(Archive& ar, const unsigned int version);

  private:
    covariance_t _U;
};

template <class Archive>
void UncertainTransformation::save(Archive& ar, const unsigned int /* version */) const {
    using ::boost::serialization::make_nvp;
    ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Transformation);
    ar << make_nvp("_U", _U);  // BOOST_SERIALIZATION_NVP(_U);
}

template <class Archive>
void UncertainTransformation::load(Archive& ar, const unsigned int version) {
    SM_ASSERT_LE(std::runtime_error, version, (unsigned int)CLASS_SERIALIZATION_VERSION,
                 "Unsupported serialization version");

    using ::boost::serialization::make_nvp;
    ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Transformation);
    ar >> make_nvp("_U", _U);  // BOOST_SERIALIZATION_NVP(_U);
}

}  // namespace kinematics
}  // namespace sm

BOOST_CLASS_VERSION(sm::kinematics::UncertainTransformation,
                    sm::kinematics::UncertainTransformation::CLASS_SERIALIZATION_VERSION);

#endif /* SM_UNCERTAIN_TRANSFORMATION_HPP */


#include "drake/solvers/sos_basis_generator.h"

#include <vector>

#include <Eigen/Core>

#include "drake/solvers/integer_inequality_solver.h"
namespace drake {
namespace solvers {
namespace {
// Anonymous namespace containing collection of utility functions
using Variable = symbolic::Variable;
using Monomial = symbolic::Monomial;
using Variables = symbolic::Variables;
using MonomialVector = VectorX<symbolic::Monomial>;
using Exponent = Eigen::RowVectorXi;
using ExponentList = Eigen::Matrix<int, -1, -1, Eigen::RowMajor>;

// Given a list of exponents and variables, returns a vector of monomials.
// Ex: if exponents = [0, 1;1, 2], and vars = [x(0), x(1)], then the vector
// [x(1); x(0)* x(1)²] is returned.
MonomialVector ExponentsToMonomials(const ExponentList& exponents,
                                    const drake::VectorX<Variable>& vars) {
  MonomialVector monomials(exponents.rows());
  for (int i = 0; i < exponents.rows(); i++) {
    monomials(i) = Monomial(vars, exponents.row(i));
  }
  return monomials;
}

// Returns a list of all exponents that appear in a polynomial p.
// E.g., given p = 1 + 2x₀² + 3x₀*x₁², returns [0, 0; 2, 0; 1, 2];
ExponentList GetPolynomialExponents(const drake::symbolic::Polynomial& p) {
  const Variables& indeterminates{p.indeterminates()};
  ExponentList exponents(p.monomial_to_coefficient_map().size(),
                         indeterminates.size());
  int row = 0;
  for (const auto& m : p.monomial_to_coefficient_map()) {
    int col = 0;
    for (const auto& var : indeterminates) {
      exponents(row, col++) = m.first.degree(var);
    }
    row++;
  }
  return exponents;
}

ExponentList VerticalStack(const ExponentList& A, const ExponentList& B) {
  DRAKE_ASSERT(A.cols() == B.cols());
  if (A.rows() == 0) {
    return B;
  }
  if (B.rows() == 0) {
    return A;
  }
  ExponentList Y(A.rows() + B.rows(), B.cols());
  Y << A, B;
  return Y;
}

ExponentList PairwiseSums(const ExponentList& exponents) {
  int n = exponents.rows();
  ExponentList sums((n * n - n) / 2, exponents.cols());
  int cnt = 0;
  for (int i = 0; i < n; i++) {
    // Note: counter starts at i+1 to omit a+b when a = b.
    for (int j = i + 1; j < n; j++) {
      sums.row(cnt++) = exponents.row(i) + exponents.row(j);
    }
  }
  return sums;
}

// Returns true if the first num_rows of A contains B.
bool ContainsExponent(const ExponentList& A, int num_rows, const Exponent& B) {
  DRAKE_ASSERT(B.rows() == 1 && B.cols() == A.cols());
  DRAKE_ASSERT((num_rows >= 0) && (num_rows <= A.rows()));
  for (int i = 0; i < num_rows; i++) {
    if (A.row(i) == B) {
      return true;
    }
  }
  return false;
}

/* Intersection(A, B) removes duplicate rows from B and any row that doesn't
 * also appear in A.  For example, given A = [1, 0; 0, 1; 1, 1] and B = [1, 0;
 * 1, 1; 1, 1;], it overwrites B with [1, 0; 1, 1]. */
void Intersection(const ExponentList& A, ExponentList* B) {
  DRAKE_ASSERT(A.cols() == B->cols());
  int index = 0;
  for (int i = 0; i < B->rows(); i++) {
    if ((ContainsExponent(A, A.rows(), B->row(i))) &&
        !(ContainsExponent(*B, index, B->row(i)))) {
      B->row(index++) = B->row(i);
    }
  }
  B->conservativeResize(index, Eigen::NoChange);
}

/* Removes exponents of the monomials that aren't diagonally-consistent with
 * respect to the polynomial p and the given monomial basis.  A monomial is
 * diagonally-consistent if its square appears in p, or its square equals a
 * product of monomials in the basis; see, e.g., "Pre- and Post-Processing
 * Sum-of-Squares Programs in Practice Johan Löfberg, IEEE Transactions on
 * Automatic Control, 2009." After execution, all exponents of inconsistent
 * monomials are removed from exponents_of_basis.
*/
void RemoveDiagonallyInconsistentExponents(const ExponentList& exponents_of_p,
                                           ExponentList* exponents_of_basis) {
  while (1) {
    int num_exponents = exponents_of_basis->rows();

    ExponentList valid_squares =
        VerticalStack(PairwiseSums(*exponents_of_basis), exponents_of_p);

    (*exponents_of_basis) = (*exponents_of_basis) * 2;
    Intersection(valid_squares, exponents_of_basis);
    (*exponents_of_basis) = (*exponents_of_basis) / 2;

    if (exponents_of_basis->rows() == num_exponents) {
      break;
    }
  }
  return;
}

struct Hyperplanes {
  Eigen::MatrixXi normal_vectors;  // Each row contains a normal vector.
  Eigen::VectorXi max_dot_product;
  Eigen::VectorXi min_dot_product;
};

// Finding random supporting hyperplanes of 1/2 P, where P is the Newton
// polytope of the polynomial p (i.e., the convex hull of its exponents).
Hyperplanes RandomSupportingHyperplanes(const ExponentList& exponents_of_p,
                                        unsigned int seed) {
  Hyperplanes H;

  // get_random() samples uniformly between normal_vector_component_min/max.
  // Current values of min and max set heuristically.
  const int normal_vector_component_min = -10;
  const int normal_vector_component_max = 10;
  std::default_random_engine generator(seed);
  std::uniform_int_distribution<int> distribution(normal_vector_component_min,
                                                  normal_vector_component_max);
  auto get_random = [&]() { return distribution(generator); };

  // Number of hyperplanes currently picked heuristically.
  int num_hyperplanes = 10 * exponents_of_p.cols();

  H.normal_vectors = Eigen::MatrixXi(num_hyperplanes, exponents_of_p.cols());
  for (int i = 0; i < H.normal_vectors.cols(); i++) {
    H.normal_vectors.col(i)
        << Eigen::VectorXi::NullaryExpr(num_hyperplanes, get_random);
  }

  Eigen::MatrixXi dot_products = H.normal_vectors * exponents_of_p.transpose();
  H.max_dot_product = dot_products.rowwise().maxCoeff() / 2;
  H.min_dot_product = dot_products.rowwise().minCoeff() / 2;

  return H;
}

//  Generates the supporting hyperplanes of the Newton polytope that
//  are induced by the total degree ordering.
Hyperplanes DegreeInducedHyperplanes(const ExponentList& exponents_of_p) {
  Hyperplanes H;

  // The hyperplane for total degree.
  H.normal_vectors.resize(1, exponents_of_p.cols());
  H.normal_vectors.setConstant(1);

  Eigen::MatrixXi dot_products = H.normal_vectors * exponents_of_p.transpose();
  H.max_dot_product = dot_products.rowwise().maxCoeff() / 2;
  H.min_dot_product = dot_products.rowwise().minCoeff() / 2;

  return H;
}

ExponentList EnumerateInitialSet(const ExponentList& exponents_of_p) {
  Eigen::VectorXi lower_bounds = exponents_of_p.colwise().minCoeff() / 2;
  Eigen::VectorXi upper_bounds = exponents_of_p.colwise().maxCoeff() / 2;
  Hyperplanes hyperplanes = DegreeInducedHyperplanes(exponents_of_p);

  // We check the inequalities in two batches to allow for internal
  // infeasibility propagation inside of EnumerateIntegerSolutions,
  // which is done only if A has a column that is elementwise nonnegative
  // or nonpositive. (This condition never holds if we check the
  // inequalities in one batch, since A = [normal_vectors;-normal_vectors].)
  ExponentList basis_exponents_1 = drake::solvers::EnumerateIntegerSolutions(
      hyperplanes.normal_vectors, hyperplanes.max_dot_product, lower_bounds,
      upper_bounds);

  ExponentList basis_exponents = drake::solvers::EnumerateIntegerSolutions(
      -hyperplanes.normal_vectors, -hyperplanes.min_dot_product, lower_bounds,
      upper_bounds);

  Intersection(basis_exponents_1, &basis_exponents);
  return basis_exponents;
}

//  This function removes an element alpha from "basis" if a randomly generated
//  hyperplane separates 2*alpha from the Newton polytope of the polynomial p.
//  Note that this function is actually deterministic since the seed for
//  the random number generator is set to predetermined constants.
void RemoveWithRandomSeparatingHyperplanes(const ExponentList& exponents_of_p,
                                           ExponentList* basis) {
  // Declare this outside the main loop to avoid repeated dynamic memory
  // allocation.
  Eigen::MatrixXi dot_products;
  int random_seed = 0;

  while (1) {
    int next_basis_size = 0;
    int current_basis_size = basis->rows();

    auto H = RandomSupportingHyperplanes(exponents_of_p, random_seed++);

    // Remove monomials that the hyperplanes separate from the
    // Newton polytope.
    dot_products = (*basis) * H.normal_vectors.transpose();
    for (int i = 0; i < current_basis_size; i++) {
      bool keep_monomial = true;
      for (int j = 0; j < dot_products.cols(); j++) {
        if (dot_products(i, j) > H.max_dot_product(j) ||
            dot_products(i, j) < H.min_dot_product(j)) {
          keep_monomial = false;
          break;
        }
      }

      if (keep_monomial) {
        basis->row(next_basis_size++) = basis->row(i);
      }
    }

    basis->conservativeResize(next_basis_size, basis->cols());

    // Quit if the basis is now empty or if its size wasn't reduced
    // enough.
    constexpr double kMinimumPercentReduction = .1;
    if (next_basis_size >
            current_basis_size * (1.0 - kMinimumPercentReduction) ||
        next_basis_size == 0) {
      break;
    }
  }
  return;
}

ExponentList ConstructMonomialBasis(const ExponentList& exponents_of_p) {
  auto basis_exponents = EnumerateInitialSet(exponents_of_p);
  RemoveWithRandomSeparatingHyperplanes(exponents_of_p, &basis_exponents);
  RemoveDiagonallyInconsistentExponents(exponents_of_p, &basis_exponents);
  return basis_exponents;
}

}  // namespace

MonomialVector ConstructMonomialBasis(const drake::symbolic::Polynomial& p) {
  const Variables& indeterminates{p.indeterminates()};
  drake::VectorX<Variable> vars(indeterminates.size());
  int cnt = 0;
  for (auto& var : indeterminates) {
    vars(cnt++) = var;
  }

  auto polynomial_exponents = GetPolynomialExponents(p);
  auto basis_exponents = ConstructMonomialBasis(polynomial_exponents);
  auto monomial_basis = ExponentsToMonomials(basis_exponents, vars);
  return monomial_basis;
}
}  // namespace solvers
}  // namespace drake