/* Copyright (C) 2017 IBM Corp.
 *  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.
 */
/*****************************************************************************
 * TDMatrix.cpp - Matrixes with trapdoors,
 *   implementing the Efficient algorithm SampleD - Algorithm 3 from MP12.
 *****************************************************************************/
#include <stdexcept>
#include <ctime>
#include <NTL/mat_ZZ.h>
#include <NTL/mat_lzz_p.h>
#include <iostream>
#include <fstream>

NTL_CLIENT
#include "utils/timing.h"
#include "TDMatrix.h"
#include "mat_l.h"

//#define DEBUGPRINT
//#define DEBUG
//#define PRINTDOT //show how program progresses

// A static variable for debugging purposes
std::atomic<int> TDMatrix::maxSample(0); // keep the largest sample ever drawn

/***** Some local functions *****/

// Compute the covariance matrix sigmaP = sigmaX*I - sigmaG*[R/I]*(R^t|I)
static void
computeCovarianceMatrix(mat_l& covMat,const mat_l& R,long sigmaG,long sigmaX);

// Multiply a vector by the G matrix modulo the current NTL modulus, y=G*x.
// G is and n-by-m matrix with m=n*e*numOfFactors, and is defind by means
// of the vector of factors and the exponent e, each row of G is mostly zero,
// except for a progression
//    ( 1, f1,...,f1^e,  (f1^e f2), (f1^e f2^2), ..., (f1^e...fk^{e-1}) )
template<class T>
void multByG(vec_zz_p &y, const Vec<T> &x,
             long n, const vec_l& factors, long e)
{
    FHE_TIMER_START;
    long k = factors.length();
    long m = n*k*e;

    assert(x.length()==m);
    y.SetLength(n);

    for (long row=0; row<n; row++)   // Multiply x by next row of G
    {
        long index = row*k*e;      // first non-zero entry in this row
        zz_p val = to_zz_p(1L);  // value in current entry of G
        y[row] = to_zz_p(0L);    // accumulated sum
        for (long f=0; f<k; f++)     // go over all the factors
        {
            zz_p fact = to_zz_p(factors[f]); // convert to zz_p
            for (long i=0; i<e; i++)    // use each factor e times
            {
                zz_p addedTerm = val;
                addedTerm *= x[index++];  // update sum, advance index
                y[row] += addedTerm;
                val *= fact;              // compute next entry in this row of G
            }
        } // end of current factor
    } // end of current row
}
// Explicit instantiations for Vec<long> and Vec<zz_p>
template void multByG<zz_p>(vec_zz_p &y, const Vec<zz_p> &x,
                            long n, const vec_l& factors, long e);
template void multByG<long>(vec_zz_p &y, const Vec<long> &x,
                            long n, const vec_l& factors, long e);

// Choose a (pseudo)random A with a trapdoor R s.t. A*[R/I]=G
// A=(aBar| aPrime ) with aPrime=G-aBar*R isn't computed explicitly here
void TDMatrix::initTDmatrix(TDMatrixParams& prms, const CRTmatrix* abar)
{
    FHE_TIMER_START;
    params = &prms;

    if (abar)   // aBar is specified by the caller
        aBarCRT = *abar;
    else        // Choose aBar at random
        randomFill(aBarCRT, prms.n, prms.mBar, prms);

    mat_l sigmaP;
    do
    {
        // Choose R as a random small matrix
        setSmall(trapDoorMatR, params->mBar,params->wLen, params->r);

        // Compute the covariance matrix sigmaP = sigmaX*I - sigmaG*[R/I]*(R^t|I)

        computeCovarianceMatrix(sigmaP, trapDoorMatR,
                                /*sigmaG=*/prms.r *prms.maxFactor, prms.sigmaX);

#ifdef DEBUGPRINT
        long minVal =0;
        long maxVal=0;
        for (long i = 0; i < sigmaP.NumCols(); i++)
            for (long j = 0; j < sigmaP.NumRows(); j++)
            {
                if (sigmaP[i][j] < minVal)
                    minVal = sigmaP[i][j];
                if (sigmaP[i][j] > maxVal)
                    maxVal = sigmaP[i][j];
             }

        ZZ q = prms.getQ();
        cout << "minVal=" << minVal << endl;
        cout << "maxVal=" << maxVal << endl;
        cout << "q=" << q << endl;
        cout << "after computeCovarianceMatrix"  << endl;
#endif // DEBUGPRINT
    }
    while (!(gaussSamp.InitSampler(sigmaP)));  // error, try again

#ifdef DEBUGPRINT
        cout << "after gaussSamp.InitSampler"  << endl;
#endif // DEBUGPRINT
}

// Compute the covariance matrix sigmaP = sigmaX*I - sigmaG*[R/I]*(R^t|I)
// Note that [R/I]*(R^t|I) is a block matrix with this form:
//    RR = ( R^2 | R^t )
//         (  R  |  I  )
void computeCovarianceMatrix(mat_l& covMat, const mat_l& R, long sigmaG,long sigmaX)
{

    FHE_TIMER_START;

    long mBar = R.NumRows();
    long wLen = R.NumCols();
    long m = mBar + wLen;

    covMat.SetDims(m,m); // allocate space

    mat_l tmp(INIT_SIZE, mBar, mBar);
    square(tmp, R);   // Set the top-left mBar-by-mBar as R*R^t

    // Reset the top-left to sigmaX*I - sigmaG*R*R^t
    for (long i=0; i<mBar; i++) for (long j=0; j<mBar; j++)
        {
            if (i==j)
                covMat[i][j] = sigmaX - sigmaG*tmp[i][j];
            else
                covMat[i][j] = -sigmaG*tmp[i][j];
        }

    // Set the top-right ro -sigmaG*R^t and the bootom-left to -sigmaG*R
    for (long i=0; i<mBar; i++) for (long j=0; j<wLen; j++)
        {
            covMat[i][j+mBar] = covMat[j+mBar][i] = -sigmaG * R[i][j];
        }

    // Finally set the bottom-right to (sigmaX-sigmaG)*I;
    for (long i=mBar; i<m; i++) for (long j=mBar; j<m; j++)
        {
            if (i==j) covMat[i][j] = sigmaX -sigmaG;
            else      covMat[i][j] = 0;
        }
}

// Compute A explicitly, A = ( aBar | G - aBar*R )
void TDMatrix::getA(CRTmatrix& A) const
{
    FHE_TIMER_START;
    A.params = params;

    long n = params->n;
    long m = params->m;
    long kFactors = params->kFactors;
    long mBar = params->mBar;

    // Compute A modulo each factor separately

    zz_pPush ppush; // backup NTL's current modulus
    A.SetLength(kFactors);
    for (long f=0; f<kFactors; f++)
    {
        params->zzp_context[f].restore();
        mat_zz_p& aMod = A[f];
        const mat_zz_p& aBarMod = aBarCRT[f];

        aMod.SetDims(n,m); // allocate space

        // Copy aBar
        for (long i=0; i<n; i++) for (long j=0; j<mBar; j++)
            {
                aMod[i][j] = aBarMod[i][j];
            }

        // Compute aBar * R
        mat_zz_p tmp;
        mat_zz_p rMod;
        conv(rMod, trapDoorMatR); // conver to zz_p format
        mul(tmp, aBarMod, rMod); // tmp = aBar * R (mod P_f)

        // Copy -aBar*R to left part of A
        for (long i=0; i<n; i++) for (long j=mBar; j<m; j++)
            {
                aMod[i][j] = -tmp[i][j-mBar];
            }

        // Add G to left part of A
        for (long i=0; i<n; i++)    // one row at a time
        {
            // Add (1, f1, f1^2, ..., f1^e...fk^{e-1}) to each row

            long index = (i * kFactors * params->e) + mBar; // 1st index to add to

            zz_p val = to_zz_p(1L);      // The value to add to the next entry
            for (long j=0; j < kFactors; j++)
                for (long ei=0; ei<params->e; ei++, index++)
                {
                    aMod[i][index] += val;
                    val *= params->factors[j];// Change value to add before next entry
                }
        }
    }

}

/** sampleWithTrapdoor - Implementation of Efficient algorithm SampleD -
 * Algorithm 3 from MP12. Choose p at random and z to match the needed
 * solution. This debugging function also returns p and z.
 **/
int TDMatrix::sampleWithTrapdoor(vec_l &xOut, vec_l& p, vec_l& z,
                                 Vec<vec_zz_p>& U) const
{
    FHE_TIMER_START;
    long mBar = params->mBar;
    long m = params->m;
    long n = params->n;
    long e = params->e;
    long kFactors = params->kFactors;

    //choose perturbation P from (0,sigma)

    Vec<double> zeroVector(INIT_SIZE, m);
    clear(zeroVector);

    gaussSamp.SampleDiscreteGaussian(p, zeroVector);
#ifdef DEBUGPRINT
    cout << "\np = " << p << endl;
#endif
    // Split the pertubation vector in two
    vec_l p1(INIT_SIZE, mBar);
    for (long i = 0; i < mBar; i++)
        p1[i] = p[i];

    vec_l p2(INIT_SIZE, params->wLen);
    for (long i = mBar; i < m; i++)
        p2[i - mBar] = p[i];

    //compute wBar and w

    /* We need to compute v = u-A*p = u-wBar-w (in CRT representation), where
     *
     *    A*p = (Abar| Aprime ) * [p1/p2] = Abar*p1 + Aprime*p2
     *        = Abar*p1 + (G-Abar*R)*p2   = Abar*(p1-R*p2) +  G*p2
     *                                      \___wBar____/    \_w_/
     */

    // p1-R*p2 is computed over the integers, stored in p1
    vec_l tmp(INIT_SIZE, mBar);
    mul(tmp, trapDoorMatR, p2); // R*p2
    p1 -= tmp;                  // p1-R*p2

    // All other vectors are computed modulo each of the factors

    // FIXME: There's room for parallelization here, but in our case we have
    // thousands of calls to sampleWithTrapdoor that can be run in parallel,
    // so there is no reason to parallelize also at this lower level.

    zz_pPush push; // backup NTL's current modulus

    Vec<vec_zz_p> vVec = U; // allocate space and initialize v=u
    vec_zz_p wMod, wBarMod;
    for (long iE = 0; iE < kFactors; iE++)
    {
        params->zzp_context[iE].restore();

        // Compute Abar*(p1-R*p2) modulo the current factor
        const mat_zz_p& aBarMod = aBarCRT[iE];
        mul(wBarMod, aBarMod, conv<vec_zz_p>(p1));  // wBar = Abar*(p1-R*p2)
        // convert p1-R*p2 to vec_zz_p and multiply by aBar modulo each factor

        vVec[iE] -= wBarMod; // u - wBar

        multByG(wMod, p2, n, params->factors, e); // w = G*p2
        vVec[iE] -= wMod;    // u - wBar - w
    }

    // Now that we computed v modulo all the primes, sample z s.t. G*z=v

    sampleG(z, vVec, params);
#ifdef DEBUGPRINT
    cout << "z = " << z << endl;
#endif
    // finally, return x = p + [R/I]*z = p + [R*z / z]

    xOut = p;
    mul(tmp, trapDoorMatR, z); // add R*z to top mBar entries
#ifdef DEBUGPRINT
    cout << "R*z = " << tmp << endl;
#endif
    for (long i=0; i<mBar; i++)
    {
      xOut[i] += tmp[i];
      long absX = abs(xOut[i]);
      if (absX > TDMatrix::maxSample) TDMatrix::maxSample.store(absX);
         // not quite thread-safe, the stored value may not be the maximum
    }

    // add z to bottom m-mBar entries
    for (long i = mBar; i< params->m; i++)
    {
      xOut[i] += z[i - mBar];
      long absX = abs(xOut[i]);
      if (absX > TDMatrix::maxSample) TDMatrix::maxSample.store(absX);
         // not quite thread-safe, the stored value may not be the maximum
    }
    return 0;
}

//Write TDMatrix different parameters to file. The handle to the open
//file is sent as input to the function. returns number of items read by
//the function
long TDMatrix::writeToFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = 0;

    count += params->writeToFile(handle); // write the params
    count += aBarCRT.writeToFile(handle); // write the Abar matrix

    long numRows = trapDoorMatR.NumRows();
    long numCols = trapDoorMatR.NumCols();
    count += fwrite(&numRows,sizeof(long),1,handle);
    count += fwrite(&numCols,sizeof(long),1,handle);
    for (long i = 0; i < numRows; i++)  // write the rows of R, one at a time
    {
        count+= fwrite(trapDoorMatR[i].elts(), sizeof(long), numCols, handle);
        // each row is implemented as a C vector
    }

    count += gaussSamp.writeToFile(handle); // write the DGaussSampler

    return count;
}

//Read the TDMatrix parameters from a flie. The handle to the open file is
//received as input to this function, as well as a pointer to the TDMatrixParams
//if the pointer is not provided and params as not been initialized before, an
//error occurs, as a pointer to this structure is needed
//returns number of items read by the function

long TDMatrix::readFromFile(FILE* handle, TDMatrixParams* prmBuf)
{
    FHE_TIMER_START;
    assert(params != NULL || prmBuf != NULL); // some pointer must be provided
    long count=0;

    // If buffer is given, make params point to it and don't overwrite from iput
    if (prmBuf != NULL)
    {
        TDMatrixParams p;
        count += p.readFromFile(handle);
        assert(p == *prmBuf); // sanity check
        params = prmBuf;      // point to given params
    }
    else
        count += params->readFromFile(handle); // overwrite params from input

    count += aBarCRT.readFromFile(handle,prmBuf); // read the Abar matrix

    long numRows,numCols;
    count += fread(&numRows,sizeof(long),1,handle);
    count += fread(&numCols,sizeof(long),1,handle);
    trapDoorMatR.SetDims(numRows,numCols);
    for (long i = 0; i < numRows; i++)  // write the rows of R, one at a time
    {
        count+= fread(trapDoorMatR[i].elts(), sizeof(long), numCols, handle);
        // each row is implemented as a C vector
    }

    count += gaussSamp.readFromFile(handle); // read the DGaussSampler

    return count;
}

// operator==, //compares all variables in TDMatrix
bool operator==(const TDMatrix& A, const TDMatrix& B)
{
    FHE_TIMER_START;
    if (A.getParams() != B.getParams())
        return false;

    if (A.getABar() != B.getABar())
        return false;

    if (A.getR() != B.getR())
        return false;

    return true;
}

#ifdef DEBUG
static void printTrapdoor(const mat_l& td, const TDMatrixParams& params)
{
    FHE_TIMER_START;
    double maxR = 0;
    double AvgR=1;
    //  long TotalItems;

    for (long i=0; i < params.mBar; i++)
        for (long j=0; j < params.wLen; j++)
        {
            if (abs(td[i][j]) > maxR)
                maxR = abs(td[i][j]);
            AvgR *= (double)(i*params.wLen+j)/(i*params.wLen+j+1);
            AvgR+=(double)abs(td[i][j]) /(i*params.wLen+j+1);
        }
    cout << "maxR = " << maxR << ", AvgR = " << AvgR << "\n";

    mat_l rSquare;
    //how large is R^2?
    square(rSquare,td);
    //  cout << "trapDoorMatR=" << td << "rSquare= " << rSquare << "\n";

    maxR = 0;
    AvgR=1;
    for (long i=0; i < params.mBar; i++)
        for (long j=0; j < params.mBar; j++)
        {
            if (abs(rSquare[i][j]) > maxR)
                maxR = abs(rSquare[i][j]);
            AvgR *= (double)(i*params.wLen+j)/(i*params.wLen+j+1);
            AvgR+=(double)abs(rSquare[i][j]) /(i*params.wLen+j+1);
        }
    cout << "maxRSquare = " << maxR << ", AvgRSquare = " << AvgR << "\n";
}
#endif //DEBUG


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

#include "NRegularLanguage.hpp"
#include <memory>

using namespace FACore;
using namespace std;

template<unsigned int ALPHABET_SIZE>
NRegularLanguage<ALPHABET_SIZE> EmptyLanguage() {
    typedef NRegularLanguage<ALPHABET_SIZE> Language;

    // By default, it will be the implementation of the empty language
    return Language(new typename Language::Machine(), 0);
}

template<unsigned int ALPHABET_SIZE>
NRegularLanguage<ALPHABET_SIZE> FullLanguage() {
    typedef NRegularLanguage<ALPHABET_SIZE> Language;
    typedef typename Language::Machine Machine;

    Machine* machine = new Machine();

    // By default, it will be the implementation of the empty language
    auto state = machine->AddState(true);

    for(unsigned int c = 0; c < Language::AlphabetSize; c++) {
        machine->AddArc(state, c, state);
    }

    return Language(machine, state);
}

template<class LanguageType>
bool InLanguage(LanguageType &language, std::vector<typename LanguageType::StateId> word) {
    return language.contains(word.begin(), word.end());
}

template<class LanguageType>
bool NotInLanguage(LanguageType &language, std::vector<typename LanguageType::StateId> word) {
    return !language.contains(word.begin(), word.end());
}

BOOST_AUTO_TEST_SUITE( TestNRegularLanguage );

BOOST_AUTO_TEST_CASE( empty_language_alphabet2 )
{
    NRegularLanguage<2> emptyLanguage = EmptyLanguage<2>(); // This is the default

    BOOST_CHECK( NotInLanguage(emptyLanguage, {}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {0}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {1}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {1,0,1}) );
}

BOOST_AUTO_TEST_CASE( empty_language_alphabet3 )
{
    NRegularLanguage<3> emptyLanguage = EmptyLanguage<3>(); // This is the default

    BOOST_CHECK( NotInLanguage(emptyLanguage, {}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {0}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {1}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {1,0,2}) );
}

BOOST_AUTO_TEST_CASE( full_language )
{
    NRegularLanguage<2> emptyLanguage = FullLanguage<2>(); // This is the default

    BOOST_CHECK( InLanguage(emptyLanguage, {}) );
    BOOST_CHECK( InLanguage(emptyLanguage, {0}) );
    BOOST_CHECK( InLanguage(emptyLanguage, {1}) );
    BOOST_CHECK( InLanguage(emptyLanguage, {1,0,1}) );
}

BOOST_AUTO_TEST_CASE( thue_morse )
{
    // The thue morse machine accepts a bit string iff it has an odd number of ones
    NAutomaton<2> *thueMorseMachine = new NAutomaton<2>();
    auto evenState = thueMorseMachine->AddState(false);
    auto oddState = thueMorseMachine->AddState(true);

    thueMorseMachine->AddArc(evenState, 0, evenState);
    thueMorseMachine->AddArc(evenState, 1, oddState);
    thueMorseMachine->AddArc(oddState, 0, oddState);
    thueMorseMachine->AddArc(oddState, 1, evenState);

    NRegularLanguage<2> thueMorseSequence(thueMorseMachine, evenState);

    // Test using the normal as well as integer "contains" function
    BOOST_CHECK( !thueMorseSequence.contains(0) );
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {} ));
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {0} ));
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {0,0} ));

    BOOST_CHECK( !thueMorseSequence.contains(3) );
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {1,1} ));
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {0,1,1} ));

    BOOST_CHECK( !thueMorseSequence.contains(5) );
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {1,0,1} ));

    BOOST_CHECK( thueMorseSequence.contains(1) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {1}) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {0,1}) );

    BOOST_CHECK( thueMorseSequence.contains(2) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {1,0}) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {0,1,0}) );

    BOOST_CHECK( thueMorseSequence.contains(4) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {1,0,0}) );
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Copyright 2015 David A. Boyuka II
 *
 * 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.
 */

/*
 * ii-setops.cpp
 *
 *  Created on: Mar 20, 2014
 *      Author: David A. Boyuka II
 */

#include <vector>

#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>

#include "pique/setops/bitmap/bitmap-setops.hpp"

boost::shared_ptr< BitmapRegionEncoding > BitmapSetOperations::unary_set_op_impl(boost::shared_ptr< const BitmapRegionEncoding > region, UnarySetOperation op) const {
	assert(op == UnarySetOperation::COMPLEMENT);

	boost::shared_ptr< BitmapRegionEncoding > out = boost::make_shared< BitmapRegionEncoding >();
	out->domain_size = region->domain_size;
	out->bits.resize(region->bits.size());

	auto in_it = region->bits.begin();
	auto out_it = out->bits.begin();
	for (; in_it != region->bits.end(); ++in_it, ++out_it)
		*out_it = ~*in_it;

	return out;
}

void BitmapSetOperations::inplace_unary_set_op_impl(boost::shared_ptr< BitmapRegionEncoding > region_and_out, UnarySetOperation op) const {
	assert(op == UnarySetOperation::COMPLEMENT);

	for (auto it = region_and_out->bits.begin(); it != region_and_out->bits.end(); ++it)
		*it = ~*it;
}

typedef BitmapRegionEncoding::block_t block_t;
typedef typename std::vector< BitmapRegionEncoding::block_t >::iterator block_it_t;
typedef typename std::vector< BitmapRegionEncoding::block_t >::const_iterator const_block_it_t;

template<NArySetOperation op>
inline static void apply_binary_op_inner(const block_t &left, const block_t &right, block_t &out);
template<> inline void apply_binary_op_inner<NArySetOperation::UNION>(const block_t &left, const block_t &right, block_t &out)					{ out = left | right; }
template<> inline void apply_binary_op_inner<NArySetOperation::INTERSECTION>(const block_t &left, const block_t &right, block_t &out)			{ out = left & right; }
template<> inline void apply_binary_op_inner<NArySetOperation::DIFFERENCE>(const block_t &left, const block_t &right, block_t &out)				{ out = left & ~right; }
template<> inline void apply_binary_op_inner<NArySetOperation::SYMMETRIC_DIFFERENCE>(const block_t &left, const block_t &right, block_t &out)	{ out = left ^ right; }

template<NArySetOperation op>
inline static void apply_binary_op(const_block_it_t left_it, const_block_it_t left_end_it, const_block_it_t right_it, block_it_t out_it) {
	for (; left_it != left_end_it; ++left_it, ++right_it, ++out_it)
		apply_binary_op_inner<op>(*left_it, *right_it, *out_it);
}

boost::shared_ptr< BitmapRegionEncoding > BitmapSetOperations::binary_set_op_impl(boost::shared_ptr< const BitmapRegionEncoding > left, boost::shared_ptr< const BitmapRegionEncoding > right, NArySetOperation op) const {
	assert(left->domain_size == right->domain_size);

	boost::shared_ptr< BitmapRegionEncoding > out = boost::make_shared< BitmapRegionEncoding >();
	out->domain_size = left->domain_size;
	out->bits.resize(left->bits.size());

	auto left_it = left->bits.cbegin();
	auto left_end_it = left->bits.cend();
	auto right_it = right->bits.cbegin();
	auto out_it = out->bits.begin();

	switch (op) {
	case NArySetOperation::UNION:
		apply_binary_op<NArySetOperation::UNION>(left_it, left_end_it, right_it, out_it);
		break;
	case NArySetOperation::INTERSECTION:
		apply_binary_op<NArySetOperation::INTERSECTION>(left_it, left_end_it, right_it, out_it);
		break;
	case NArySetOperation::DIFFERENCE:
		apply_binary_op<NArySetOperation::DIFFERENCE>(left_it, left_end_it, right_it, out_it);
		break;
	case NArySetOperation::SYMMETRIC_DIFFERENCE:
		apply_binary_op<NArySetOperation::SYMMETRIC_DIFFERENCE>(left_it, left_end_it, right_it, out_it);
		break;
	}

	return out;
}

template<NArySetOperation op>
inline static void apply_inplace_binary_op(block_it_t left_out_it, block_it_t left_out_end_it, const_block_it_t right_it) {
	for (; left_out_it != left_out_end_it; ++left_out_it, ++right_it)
		apply_binary_op_inner<op>(*left_out_it, *right_it, *left_out_it);
}

void BitmapSetOperations::inplace_binary_set_op_impl(boost::shared_ptr< BitmapRegionEncoding > left_and_out, boost::shared_ptr< const BitmapRegionEncoding > right, NArySetOperation op) const {
	assert(left_and_out->domain_size == right->domain_size);

	auto left_out_it = left_and_out->bits.begin();
	auto left_out_end_it = left_and_out->bits.end();
	auto right_it = right->bits.cbegin();

	switch (op) {
	case NArySetOperation::UNION:
		apply_inplace_binary_op<NArySetOperation::UNION>(left_out_it, left_out_end_it, right_it);
		break;
	case NArySetOperation::INTERSECTION:
		apply_inplace_binary_op<NArySetOperation::INTERSECTION>(left_out_it, left_out_end_it, right_it);
		break;
	case NArySetOperation::DIFFERENCE:
		apply_inplace_binary_op<NArySetOperation::DIFFERENCE>(left_out_it, left_out_end_it, right_it);
		break;
	case NArySetOperation::SYMMETRIC_DIFFERENCE:
		apply_inplace_binary_op<NArySetOperation::SYMMETRIC_DIFFERENCE>(left_out_it, left_out_end_it, right_it);
		break;
	}
}

void BitmapSetOperations::inplace_nary_set_op_impl(boost::shared_ptr< BitmapRegionEncoding > first_and_out, RegionEncodingCPtrCIter it, RegionEncodingCPtrCIter end_it, NArySetOperation op) const {
	// Simple implementation: repeatedly apply the in-place binary setop
	// There's no efficiency loss for a linear approach vs. the heap-based approach, since bitmap binary
	// setop complexity is unaffected by region density, so this is the optimial way to implement N-ary in-place setops

	while (it != end_it)
		this->inplace_binary_set_op(first_and_out, *it++, op);
}


/**
 * @file set_cover_check.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-10-09
 */
#ifndef PAAL_SET_COVER_CHECK_HPP
#define PAAL_SET_COVER_CHECK_HPP

#include <boost/algorithm/cxx11/all_of.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/test/unit_test.hpp>

#include <vector>

template <typename Sets, typename SetsToElements, typename Result>
double set_cover_result_check(Sets sets, SetsToElements sets_to_elements,
                              Result result) {
    int max_item = 0;
    size_t max_size = 0;
    for (auto i : sets) {
        max_item = std::max(max_item, *boost::max_element(sets_to_elements(i)));
        max_size = std::max(sets_to_elements(i).size(), max_size);
    }
    std::vector<bool> is_covered(max_item + 1);
    for (auto i : result) {
        for (auto j : sets_to_elements(i)) {
            is_covered[j] = true;
        }
    }
    BOOST_CHECK(boost::algorithm::all_of_equal(is_covered, true));
    double approximation_ratio = 0.;
    for (size_t x = 1; x <= max_size; x++) {
        approximation_ratio += 1. / double(x);
    }
    return approximation_ratio;
}

#endif /* PAAL_SET_COVER_CHECK_HPP */


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   Distributed under the Boost Software License, Version 1.0.
//                        See accompanying file LICENSE.txt or copy at
//                            http://www.boost.org/LICENSE_1_0.txt
// -------------------------------------------------------------------------------------------------

#include <simd_bench.hpp>
#include <boost/simd/function/simd/cast.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/pack.hpp>

namespace nsb = ns::bench;
namespace bs =  boost::simd;
namespace bd =  boost::dispatch;

template<int N>
struct caster
{
  template<class T> bs::pack<float, N> operator()(const bs::pack<T, N> & a) const
  {
    return bs::cast<float>(a);
  }
};

DEFINE_SIMD_BENCH(simd_caster, caster<bs::pack<float>::static_size>());

DEFINE_BENCH_MAIN()
{
  nsb::for_each<simd_caster, /*int8_t, uint8_t, uint16_t, uint16_t, int32_t, uint32_t, int64_t, uint64_t,*/ float/*, double*/>(-10, 10);
}


#ifndef MATHTOOLBOX_BAYESIAN_OPTIMIZATION_HPP
#define MATHTOOLBOX_BAYESIAN_OPTIMIZATION_HPP

#include <Eigen/Core>
#include <memory>
#include <utility>

namespace mathtoolbox
{
    class GaussianProcessRegressor;

    namespace optimization
    {
        enum class KernelType
        {
            ArdSquaredExp,
            ArdMatern52
        };

        /// \brief An optimizer class for managing Bayesian optimization iterations
        ///
        /// \details This optimizer solves a maximization problem with a black-box function.
        class BayesianOptimizer
        {
        public:
            BayesianOptimizer(const std::function<double(const Eigen::VectorXd&)>& f,
                              const Eigen::VectorXd&                               lower_bound,
                              const Eigen::VectorXd&                               upper_bound,
                              const KernelType& kernel_type = KernelType::ArdMatern52);

            /// \brief Perform a single step of the Bayesian optimization algorithm
            /// \return Newly observed data point
            std::pair<Eigen::VectorXd, double> Step();

            /// \brief Calculate the function value
            /// \return Evaluated function value
            double EvaluatePoint(const Eigen::VectorXd& x) const;

            /// \brief Predict the mean value
            /// \return Predicted mean value
            double PredictMean(const Eigen::VectorXd& x) const;

            /// \brief Predict the standard deviation value
            /// \return Predicted standard deviation value
            double PredictStdev(const Eigen::VectorXd& x) const;

            /// \brief Calculate the acquisition function value at the point
            /// \return Acquisition function value at the point
            double CalcAcquisitionValue(const Eigen::VectorXd& x) const;

            /// \brief Retrieve the optimizer found so far
            /// \return Optimizer found so far
            Eigen::VectorXd GetCurrentOptimizer() const;

            /// \brief Get the observed data points and their values
            std::pair<Eigen::MatrixXd, Eigen::VectorXd> GetData() const;

        private:
            const std::function<double(const Eigen::VectorXd&)> m_f;

            const Eigen::VectorXd m_lower_bound;
            const Eigen::VectorXd m_upper_bound;

            const KernelType m_kernel_type;

            Eigen::MatrixXd m_X;
            Eigen::VectorXd m_y;

            std::shared_ptr<GaussianProcessRegressor> m_regressor;

            void AddDataEntry(const Eigen::VectorXd& x_new, const double y_new);
            void ConstructSurrogateFunction();
        };
    } // namespace optimization
} // namespace mathtoolbox

#endif // MATHTOOLBOX_BAYESIAN_OPTIMIZATION_HPP


//  (C) Copyright Raffi Enficiaud 2014.
//  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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE boost_test_bitwise
#include <boost/test/included/unit_test.hpp>
#include <sstream>

BOOST_AUTO_TEST_CASE(test_bitwise)
{
  namespace tt = boost::test_tools;
  int a = 0xAB;
  BOOST_TEST( a == (a & ~1), tt::bitwise() );
  BOOST_TEST( a == a + 1, tt::bitwise() );
  BOOST_TEST( a != a + 1, tt::bitwise() );
  int b = 0x88;
  BOOST_TEST( a == b, tt::bitwise() );
}
//]


#include <three-dim-util/opengl2/widgets/trackball-widget.hpp>
#include <three-dim-util/opengl2/draw-functions.hpp>
#include <Eigen/Core>
#include <QApplication>

class Widget : public threedimutil::TrackballWidget
{
public:
    Widget(QWidget *parent = nullptr) : threedimutil::TrackballWidget(parent) {}

protected:
    void paintGL()
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        setProjectionMatrix();
        setModelViewMatrix();

        threedimutil::draw_points(points_, colors_);
    }

private:
    const Eigen::MatrixXd points_ = Eigen::MatrixXd::Random(3, 3000);
    const Eigen::MatrixXd colors_ = 0.5 * (points_ + Eigen::MatrixXd::Ones(3, 3000));
};

int main(int argc, char** argv)
{
    QApplication app(argc, argv);
    Widget widget;
    widget.show();
    return app.exec();
}


// Linear Models
//
// Special Naming Convention:
//
// All mathematical matrix variables are named by captial letters
// (e.g. X means input feature dataset). Vectors are named by lowercase
// (e.g. w means weights vector). These variables are always from Eigen.
//
// The key feature of this linear model is it was implemented by
// advanced and efficient c++ matrix library Eigen, but only for the
// training stage. After that, the weights are stored in normal c++
// array for fast predicting and robust interface.
//
// @author: Bingqing Qu
//
// Copyright (C) 2014-2015  Bingqing Qu <sylar.qu@gmail.com>
//
// @license: See LICENSE at root directory

#ifndef OPENLINEAR_LINEAR_H_
#define OPENLINEAR_LINEAR_H_

#include <assert.h>
#include <iostream>
#include <string>
#include <vector>
#include <memory>
#include <algorithm>
#include <exception>
#include <stdexcept>
#include <Eigen/SparseCore>
#include <Eigen/Core>
#include <stdio.h>
#include <stdarg.h>
namespace oplin{

// Define Eigen vector and matrix types we will use
typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SpRowMatrix;
typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SpColMatrix;
typedef Eigen::SparseVector<double, Eigen::RowMajor> SpRowVector;
typedef Eigen::SparseVector<double, Eigen::ColMajor> SpColVector;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1      , Eigen::ColMajor> ColVector;
typedef Eigen::Matrix<double, 1      , Eigen::Dynamic, Eigen::RowMajor> RowVector;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMatrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> ColMatrix;

// smart pointers
typedef std::shared_ptr<SpColMatrix> SpColMatrixPtr;
typedef std::shared_ptr<ColMatrix> ColMatrixPtr;
typedef std::shared_ptr<ColVector> ColVectorPtr;

//
void VOUT(const char* fmt, ...);
// very very light-weight and useful structure for <key,value> pair storage
template <class K, class V>
struct KeyValue
{
    K i;
    V v;
};
/// Dataset parameters
struct Dataset
{
    /** number of samples */
    size_t n_samples;
    /** number of classes */
    size_t n_classes;
    /** feature dimension */
    size_t dimension;
    /** targets */
    std::vector<double> y;
    /** target labels */
    std::vector<double> labels;
    /**
     * features w.r.t order of y
     * dimension is dimension * n_samples
     */
    SpColMatrixPtr X;
    double bias;
    Dataset() : bias(-1.){}

};
enum FormulaType
{
    L1R_LR,
    L2R_LR

};
enum SolverType
{
    GD,
    SGD,
    L_BFGS,
    TRON
};

/// Parameters for training
struct Parameter
{
    /** define the solver type for training model */
    int solver_type;
    /** define the objective function for solving */
    int problem_type;
    /**
     * relative tolerance between two continuous iterations
     * for training stop criteria
     */
    double rela_tol;
    /** absolute loss tolerance for iterative method */
    double abs_tol;
    /** learning rate for iterative method */
    double learning_rate;
    /** maximum iteration for iterative method */
    size_t max_epoch;
    /** C */
    // std::vector<double> C;
    double base_C;
    std::vector<KeyValue<double,double> > adjust_C;

    Parameter() : solver_type(0.), problem_type(0.){}
};
typedef std::shared_ptr<Parameter> ParamPtr;
typedef std::shared_ptr<Dataset> DatasetPtr;

/// Model Parameters
///
/// An important note here is the key part of model - weights:
/// the representation of weight during training phase is an Eigen
/// dense matrix under consideration of the matrix manipultion
/// in fomulas. But finally converted to a normal c++ array as storage.
/// The consideration here is firstly, the low level c++ array is though
/// not safe in memory, I can validate the memory after training and
/// once the double* is stored, no other manipulation should have
/// previlige to modify it until it will be released by constructor.
/// All in all, the segmentation fault will not happen here under the
/// help of class encapsulation.
///
/// Secondly, the performance should have not much difference with
/// vector but slightly better. The linear model always means fast in
/// industry and that "slightly" is good for all users.
///
struct Model
{
    /** number of classes */
    size_t n_classes;
    /** dimension of feature */
    size_t dimension;
    /** define a bias, 0 if no bias setting */
    double bias;
    /** labels of classes */
    std::vector<double> labels;
    Model() : W_(NULL),bias_values_(NULL){}
    // destructor must be called for double*
    ~Model()
    {
        delete [] W_;
        delete [] bias_values_;
    }
    void set_bias_values(double* vals)
    {
        if(bias_values_)
            delete [] bias_values_;
        bias_values_ = vals;
    }
    void set_weights(double* w)
    {
        if(W_)
            delete [] W_;
        W_ = w;
    }
// I encapsulate the two pointers to avoid wrong reference in productive env.
private:
    double* W_;
    double* bias_values_;
    /** weights */

    friend class LinearBase;
};

typedef std::vector<KeyValue<size_t,double> > FeatureVector;
// symbolic links for short implementation views
// typedef std::shared_ptr<Model> ModelPtr;

// the current design keep model to only one owner to avoid wrong reference in
// productive env.
typedef std::unique_ptr<Model> ModelUniPtr;

/// Base class for linear models
///
///
///
class LinearBase
{
protected:
    // model instance
    ModelUniPtr model_;
    bool trained_;
    void predict_WTx(const FeatureVector, std::vector<double>&);
    void preprocess_data(const DatasetPtr, std::vector<size_t>&,
                         std::vector<size_t>&, std::vector<size_t>&);
public:

    LinearBase(void);
    explicit LinearBase(ModelUniPtr);
    virtual ~LinearBase(void){};

    virtual bool is_trained();
    virtual size_t get_n_classes();
    virtual std::vector<double> get_labels();
    virtual void load_model(ModelUniPtr);
    virtual ModelUniPtr export_model();
    virtual void export_model_to_file(const std::string&);
    virtual void train(const DatasetPtr, const ParamPtr) = 0;
    virtual double predict(const FeatureVector);
    virtual double predict_proba(const FeatureVector, std::vector<double>&);
};

} // oplin
// using namespace oplin;
// using namespace oplin::model;
#endif //OPENLINEAR_LINEAR_H_


/**
@file SBGATRefFrame.hpp
@class  SBGATRefFrame
@author Benjamin Bercovici
@author Jay McMahon
@date October 2018

@brief  Defines the SBGATRefFrame class
@details Defines the SBGATRefFrame class, a right-handed set of basis vector positioned
at some origin in space. The position of the origin and the orientation of the basis vectors
are specified with respect to the parent frame of the present frame.
@copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef HEADER_REFFRAME
#define HEADER_REFFRAME

#include <memory>
#include <armadillo>
#include <RigidBodyKinematics.hpp>

class SBGATRefFrame {

public:

	/**
	Constructor
	@param name Reference frame name
	*/
	SBGATRefFrame(std::string name);

	/**
	Copy constructor (RO3)
	@param ref_frame Reference frame
	*/
	SBGATRefFrame( const SBGATRefFrame &ref_frame);

	/**
	Assignment operator (RO3)
	*/
	SBGATRefFrame& operator=(const SBGATRefFrame & other);

	/**
	Destructor (RO3)
	*/
	~SBGATRefFrame();

	/**
	Return the reference frame name
	@return name of reference frame
	*/
	std::string get_name() const;

	/**
	Sets the mrp of $this relative to its parent
	to the prescribed value
	@param mrp MRP
	*/
	void set_mrp_from_parent(arma::vec & mrp) ;

	/**
	Sets the origin of $this relative to its parent
	to the prescribed value
	@param origin Origin of $this with respect to its
	parent expressed in the parent frame
	*/
	void set_origin_from_parent(arma::vec & origin) ;

	/**
	Return the mrp of $this with respect to its parent
	@return mrp of $this with respect to its parent
	*/
	const arma::vec::fixed<3> & get_mrp_from_parent() const;

	/**
	Return the dcm of $this with respect to its parent
	@return dcm of $this with respect to its parent
	*/
	const arma::mat::fixed<3,3> & get_dcm_from_parent() const;

	/**
	Return the origin of $this with respect to its parent
	expressed in the parent reference frame
	@return origin of $this with respect to its parent
	*/
	const arma::vec::fixed<3> & get_origin_from_parent() const;

protected:

	std::string name;

	arma::vec::fixed<3> mrp_from_parent;
	arma::vec::fixed<3> origin_from_parent;
	arma::mat::fixed<3,3> dcm_from_parent;

};

#endif


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

    @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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINCOS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINCOS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object computes simultaneouly
    and at lower cost the sine and cosine of the input

    @par Header <boost/simd/function/sincos.hpp>

    @see sincosd,  sincospi

    @par Example:

      @snippet sincos.cpp sincos

    @par Possible output:

      @snippet sincos.txt sincos

  **/
  std::pair<IEEEValue, IEEEValue> sincos(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sincos.hpp>
#include <boost/simd/function/simd/sincos.hpp>

#endif


#include <iostream>
#include <sstream>
#include <fstream>
#include <type_traits>
#include <chrono>
using namespace std;

#include <libsnark/gadgetlib1/gadgets/basic_gadgets.hpp>
#include <libsnark/common/default_types/r1cs_ppzksnark_pp.hpp>
#include <libff/common/utils.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <libsnark/relations/constraint_satisfaction_problems/r1cs/examples/r1cs_examples.hpp>
#include <boost/optional.hpp>
#include <libff/algebra/curves/mnt/mnt4/mnt4_init.hpp>
#include <libff/algebra/curves/mnt/mnt6/mnt6_init.hpp>
#include <libsnark/gadgetlib1/gadgets/pairing/weierstrass_precomputation.hpp>
using namespace libsnark;

#include "gadget.hpp"

// Timer utility functions

chrono::high_resolution_clock::time_point my_start, my_end;

void
my_timer_start ()
{
  my_start = chrono::high_resolution_clock::now ();
}

int
my_timer_end ()
{
  my_end = chrono::high_resolution_clock::now ();
  return std::chrono::duration_cast < std::chrono::milliseconds >
    (my_end - my_start).count ();
}

// Function from pay-to-sudoku
void
convertBytesToVector (const unsigned char *bytes, std::vector < bool > &v)
{
  int
    numBytes = v.size () / 8;
  unsigned char
    c;
  for (int i = 0; i < numBytes; i++)
    {
      c = bytes[i];

      for (int j = 0; j < 8; j++)
	{
	  v.at ((i * 8) + j) = ((c >> (7 - j)) & 1);
	}
    }
}

void
convertBytesVectorToBytes (const std::vector < unsigned char >&v,
			   unsigned char *bytes)
{
  for (size_t i = 0; i < v.size (); i++)
    {
      bytes[i] = v.at (i);
    }
}

void
convertBytesVectorToVector (const std::vector < unsigned char >&bytes,
			    std::vector < bool > &v)
{
  v.resize (bytes.size () * 8);
  unsigned char
  bytesArr[bytes.size ()];
  convertBytesVectorToBytes (bytes, bytesArr);
  convertBytesToVector (bytesArr, v);
}

template < typename ppT >
  bool run_r1cs_ppzksnark (const r1cs_example < libff::Fr < ppT > >&example)
{
  libff::print_header ("R1CS ppzkSNARK Generator");
  r1cs_ppzksnark_keypair < ppT > keypair =
    r1cs_ppzksnark_generator < ppT > (example.constraint_system);

  libff::print_header ("R1CS ppzkSNARK Prover");
  r1cs_ppzksnark_proof < ppT > proof =
    r1cs_ppzksnark_prover < ppT > (keypair.pk, example.primary_input,
				   example.auxiliary_input);

  libff::print_header ("R1CS ppzkSNARK Verifier");
  const bool
    ans =
    r1cs_ppzksnark_verifier_strong_IC < ppT > (keypair.vk,
					       example.primary_input, proof);
  printf ("* The verification result is: %s\n", (ans ? "PASS" : "FAIL"));

  return ans;
}

template < typename ppT > r1cs_example < libff::Fr < ppT >> gen_BLS_example ()
{
  typedef
    libff::Fr <
    ppT >
    FieldT;

  protoboard < FieldT > pb;

  fair_auditing_gadget < ppT > g (pb);
  const int
    num_inputs = g.num_input_variables ();
  g.generate_r1cs_constraints ();

  auto
    cs = pb.get_constraint_system ();

  auto
    sigma = FieldT::random_element () * libff::G1 < other_curve < ppT >>::one ();
  auto
    gen = FieldT::random_element () * libff::G2 < other_curve < ppT >>::one ();
  auto
    M = sigma;
  auto
    y = gen;

  libff::bit_vector
    r;
  libff::bit_vector
    ad;

  const
    vector <
    uint8_t >
  r8bit =
    { 206, 64, 25, 10, 245, 205, 246, 107, 191, 157, 114, 181, 63, 40, 95,
134, 6, 178, 210, 43, 243, 10, 217, 251, 246, 248, 0, 21, 86, 194, 100, 94 };
  const
    vector <
    uint8_t >
  ad8bit =
    { 253, 199, 66, 55, 24, 155, 80, 121, 138, 60, 36, 201, 186, 221, 164,
65, 194, 53, 192, 159, 252, 7, 194, 24, 200, 217, 57, 55, 45, 204, 71, 9 };

  convertBytesVectorToVector (r8bit, r);
  convertBytesVectorToVector (ad8bit, ad);

  g.generate_r1cs_witness (M, y, gen, ad, sigma, r);

  cout << "Num constraints " << cs.num_constraints () << endl;

  return r1cs_example < FieldT > (std::move (cs),
				  std::move (pb.primary_input ()),
				  std::move (pb.auxiliary_input ()));
}

void
single_test ()
{
  libff::init_mnt4_params ();
  default_r1cs_ppzksnark_pp::init_public_params ();

  r1cs_example < libff::Fr < default_r1cs_ppzksnark_pp > >example =
    gen_BLS_example < default_r1cs_ppzksnark_pp > ();

  bool
    it_works = run_r1cs_ppzksnark < default_r1cs_ppzksnark_pp > (example);
  cout << endl;
  cout << (it_works ? "It works!" : "It failed.") << endl;
}

void
benchmark (int numReps)
{

  libff::init_mnt4_params ();
  default_r1cs_ppzksnark_pp::init_public_params ();

  typedef default_r1cs_ppzksnark_pp
    ppT;

  r1cs_example < libff::Fr < default_r1cs_ppzksnark_pp > >example =
    gen_BLS_example < default_r1cs_ppzksnark_pp > ();

  int
    keygen_t,
    prov_t,
    ver_t;
  keygen_t = prov_t = ver_t = 0;

  for (auto i = 1; i <= numReps; i++)
    {
      // Key generation
      my_timer_start ();
      r1cs_ppzksnark_keypair < ppT > keypair =
	r1cs_ppzksnark_generator < ppT > (example.constraint_system);
      keygen_t += my_timer_end ();

      // Proof
      my_timer_start ();
      r1cs_ppzksnark_proof < ppT > proof =
	r1cs_ppzksnark_prover < ppT > (keypair.pk, example.primary_input,
				       example.auxiliary_input);
      prov_t += my_timer_end ();

      // Verification
      my_timer_start ();
      r1cs_ppzksnark_verifier_strong_IC < ppT > (keypair.vk,
						 example.primary_input,
						 proof);
      ver_t += my_timer_end ();
    }

  cout << "Avg Keygen Time: " << keygen_t / numReps << " millis" << endl;
  cout << "Avg Proving Time: " << prov_t / numReps << " millis" << endl;
  cout << "Avg Verification Time: " << ver_t / numReps << " millis" << endl;
}

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

  //benchmark (100);

  return 0;

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef MTL_VECTOR_MAKE_SPARSE_INCLUDE
#define MTL_VECTOR_MAKE_SPARSE_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/size.hpp>
#include <boost/numeric/mtl/operation/update.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>

namespace mtl { namespace vector {

// Commands in Matlab
// S = sparse(i,j,s,m,n,nzmax)
// S = sparse(i,j,s,m,n)
// S = sparse(i,j,s)
// S = sparse(m,n)

template <typename SizeVector, typename ValueVector>
struct make_sparse_trait
{
    typedef typename Collection<SizeVector>::value_type  size_type;
    typedef typename Collection<ValueVector>::value_type value_type;
    typedef matrix::parameters<row_major, index::c_index, mtl::non_fixed::dimensions, false, size_type> paras;
    typedef matrix::compressed2D<value_type, paras>   type;
};

/// Generates an \p m by \p n matrix from the vectors \p rows, \p cols, and \p values.
/** A sparse matrix is created (compressed2D). The value type is the same as the element type
    of the value vector and the size type the same as the entries of the vectors with the row indices.
    Zero entries in \p values are ignored. Entries with same coordinates are added.
    Same as Matlab's sparse function besides that it is zero-indexed.
 **/
template <typename SizeVector1, typename SizeVector2, typename ValueVector>
inline typename make_sparse_trait<SizeVector1, ValueVector>::type
make_sparse(const SizeVector1& rows, const SizeVector2& cols, const ValueVector& values,
	    std::size_t m, std::size_t n)
{
    MTL_THROW_IF(size(rows) != size(cols), incompatible_size());
    MTL_THROW_IF(size(rows) != size(values), incompatible_size());

    typedef make_sparse_trait<SizeVector1, ValueVector> traits;
    typedef typename traits::type       matrix_type;
    typedef typename traits::value_type value_type;
    typedef typename traits::size_type  size_type;

    size_type               ms(m), ns(n);  // shouldn't be needed :-!
    matrix_type             A(ms, ns);
    matrix::inserter<matrix_type, update_plus<value_type> > ins(A, size_type(size(rows) / m + 1));

    for (std::size_t i= 0; i < size(rows); i++)
	if (values[i] != value_type(0))
	    ins[rows[i]][cols[i]] << values[i];
    return A;
}

/// Generates an \p m by \p n matrix from the vectors \p rows, \p cols, and \p values.
/** A sparse matrix is created (compressed2D). The value type is the same as the element type
    of the value vector and the size type the same as the entries of the vectors with the row indices.
    Zero entries in \p values are ignored. Entries with same coordinates are added. Last parameter
    is ignored and can be omitted.
    Same as Matlab's sparse function besides that it is zero-indexed.
 **/
template <typename SizeVector1, typename SizeVector2, typename ValueVector>
inline typename make_sparse_trait<SizeVector1, ValueVector>::type
make_sparse(const SizeVector1& rows, const SizeVector2& cols, const ValueVector& values,
	    std::size_t m, std::size_t n, std::size_t)
{
    return make_sparse(rows, cols, values, m, n);
}

/// Generates a matrix from the vectors \p rows, \p cols, and \p values.
/** A sparse matrix is created (compressed2D).
    The number of rows/columns is one plus the maximum of the entries of \p rows and \p cols.
    The value type is the same as the element type
    of the value vector and the size type the same as the entries of the vectors with the row indices.
    Zero entries in \p values are ignored. Entries with same coordinates are added.
    Same as Matlab's sparse function besides that it is zero-indexed.
 **/
template <typename SizeVector1, typename SizeVector2, typename ValueVector>
inline typename make_sparse_trait<SizeVector1, ValueVector>::type
make_sparse(const SizeVector1& rows, const SizeVector2& cols, const ValueVector& values)
{
    return make_sparse(rows, cols, values, max(rows)+1, max(cols)+1);
}

/// Generates an empty \p m by \p n matrix.
/** A sparse matrix is created (compressed2D<double>).
    Same as Matlab's sparse function besides that it is zero-indexed.
 **/
inline matrix::compressed2D<double> make_sparse(std::size_t m, std::size_t n)
{
    return matrix::compressed2D<double>(m, n);
}

} // namespace :vector

    using vector::make_sparse;

} // namespace mtl

#endif // MTL_VECTOR_MAKE_SPARSE_INCLUDE


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
/* sample.cpp - implementing various sampling routines */
#include <vector>
#include <NTL/ZZX.h>
#include <NTL/ZZ_pX.h>
#include <NTL/BasicThreadPool.h>
#include "NumbTh.h"
#include "FHEContext.h"
#include "sample.h"
#include "norms.h"

#include "powerful.h"
// only used in experimental Hwt sampler

NTL_CLIENT

// Sample a degree-(n-1) poly, with only Hwt nonzero coefficients
void sampleHWt(zzX &poly, long n, long Hwt)
{
  if (n<=0) n=lsize(poly); if (n<=0) return;
  if (Hwt>=n) {
#ifdef DEBUG_PRINTOUT
    std::cerr << "Hwt="<<Hwt<<">=n="<<n<<", is this ok?\n";
#endif
    Hwt = n-1;
  }
  poly.SetLength(n); // allocate space
  for (long i=0; i<n; i++) poly[i] = 0;

  long i=0;
  while (i<Hwt) {  // continue until exactly Hwt nonzero coefficients
    long u = NTL::RandomBnd(n);  // The next coefficient to choose
    if (poly[u]==0) { // if we didn't choose it already
      long b = NTL::RandomBits_long(2)&2; // b random in {0,2}
      poly[u] = b-1;                      //   random in {-1,1}

      i++; // count another nonzero coefficient
    }
  }
}
// Sample a degree-(n-1) ZZX, with only Hwt nonzero coefficients
void sampleHWt(ZZX &poly, long n, long Hwt)
{
  zzX pp;
  sampleHWt(pp, n, Hwt);
  convert(poly, pp);
}

// Sample a degree-(n-1) poly, with -1/0/+1 coefficients.
// Each coefficients is +-1 with probability prob/2 each,
// and 0 with probability 1-prob. By default, pr[nonzero]=1/2.
void sampleSmall(zzX &poly, long n, double prob)
{
  if (n<=0) n=lsize(poly); if (n<=0) return;
  //OLD: assert(prob>3.05e-5 && prob<=1); // prob must be in [2^{-15},1/2]
  helib::assertTrue<helib::InvalidArgument>(prob > 3.05e-5, "prob must be greater than 2^{-15}");
  helib::assertTrue<helib::InvalidArgument>(prob <= 1, "prob must be less than or equal to 1");
  poly.SetLength(n);

  constexpr long bitSize=16;
  constexpr long hiMask = (1<<(bitSize-1)); // top bit = 2^15
  constexpr long loMask = hiMask-1;         // bottom 15 bits

  long threshold = round(hiMask*prob); // threshold/2^15 = Pr[nonzero]

  NTL_EXEC_RANGE(n, first, last)
  for (long i=first; i<last; i++) {
    long u = NTL::RandomBits_long(bitSize); // a random 16-bit number
    long uLo = u & loMask; // bottom 15 bits
    long uHi = u & hiMask; // top bit

    // with probability threshold/2^15, choose between +-1
    if (uLo<threshold) { // compare low 15 bits to threshold
      poly[i] = (uHi>>(bitSize-2))-1; // topBit*2 - 1 \in {+-1}
    }

    // with probability 1-prob, set to zero
    else poly[i] = 0;
  }
  NTL_EXEC_RANGE_END
}
void sampleSmall(ZZX &poly, long n, double prob)
{
  zzX pp;
  sampleSmall(pp, n, prob);
  convert(poly.rep, pp);
  poly.normalize();
}

// Choose a vector of continuous Gaussians
void sampleGaussian(std::vector<double> &dvec, long n, double stdev)
{
  static double const Pi=4.0*atan(1.0);  // Pi=3.1415..
  static double const bignum = LONG_MAX; // convert to double
  // THREADS: C++11 guarantees these are initialized only once

  if (n<=0) n=lsize(dvec); if (n<=0) return;
  dvec.resize(n, 0.0);        // allocate space for n variables

  // Uses the Box-Muller method to get two Normal(0,stdev^2) variables
  for (long i=0; i<n; i+=2) {
    // r1, r2 are "uniform in (0,1)"
    double r1 = (1+NTL::RandomBnd(LONG_MAX))/(bignum+1);
    double r2 = (1+NTL::RandomBnd(LONG_MAX))/(bignum+1);
    double theta=2*Pi*r1;
    double rr= sqrt(-2.0*log(r2))*stdev;
    if (rr > 8*stdev) // sanity-check, trancate at 8 standard deviations
      rr = 8*stdev;

    // Generate two Gaussians RV's
    dvec[i] = rr*cos(theta);
    if (i+1 < n)
      dvec[i+1] = rr*sin(theta);
  }
}

// Sample a degree-(n-1) ZZX, with rounded Gaussian coefficients
void sampleGaussian(zzX &poly, long n, double stdev)
{
  if (n<=0) n=lsize(poly); if (n<=0) return;
  std::vector<double> dvec;
  sampleGaussian(dvec, n, stdev); // sample continuous Gaussians

  // round and copy to coefficients of poly
  clear(poly);
  poly.SetLength(n); // allocate space for degree-(n-1) polynomial
  for (long i=0; i<n; i++)
    poly[i] = long(round(dvec[i])); // round to nearest integer
}
// Sample a degree-(n-1) ZZX, with rounded Gaussian coefficients
void sampleGaussian(ZZX &poly, long n, double stdev)
{
  zzX pp;
  sampleGaussian(pp, n, stdev);
  convert(poly.rep, pp);
  poly.normalize();
}
#if 0
void sampleGaussian(ZZX &poly, long n, double stdev)
{
  static double const Pi=4.0*atan(1.0); // Pi=3.1415..
  static long const bignum = 0xfffffff;
  // THREADS: C++11 guarantees these are initialized only once

  if (n<=0) n=deg(poly)+1; if (n<=0) return;
  poly.SetMaxLength(n); // allocate space for degree-(n-1) polynomial
  for (long i=0; i<n; i++) SetCoeff(poly, i, ZZ::zero());

  // Uses the Box-Muller method to get two Normal(0,stdev^2) variables
  for (long i=0; i<n; i+=2) {
    double r1 = (1+RandomBnd(bignum))/((double)bignum+1);
    double r2 = (1+RandomBnd(bignum))/((double)bignum+1);
    double theta=2*Pi*r1;
    double rr= sqrt(-2.0*log(r2))*stdev;

    //OLD: assert(rr < 8*stdev); // sanity-check, no more than 8 standard deviations
    helib::assertTrue(rr < 8*stdev, "no more than 8 standard deviations");

    // Generate two Gaussians RV's, rounded to integers
    long x = (long) floor(rr*cos(theta) +0.5);
    SetCoeff(poly, i, x);
    if (i+1 < n) {
      x = (long) floor(rr*sin(theta) +0.5);
      SetCoeff(poly, i+1, x);
    }
  }
  poly.normalize(); // need to call this after we work on the coeffs
}
#endif

// Sample a degree-(n-1) zzX, with coefficients uniform in [-B,B]
void sampleUniform(zzX& poly, long n, long B)
{
  //OLD: assert (B>0);
  helib::assertTrue<helib::InvalidArgument>(B>0l, "Invalid coefficient interval");
  if (n<=0) n=lsize(poly); if (n<=0) return;
  poly.SetLength(n); // allocate space for degree-(n-1) polynomial

  for (long i = 0; i < n; i++)
    poly[i] = NTL::RandomBnd(2*B +1) - B;
}

// Sample a degree-(n-1) ZZX, with coefficients uniform in [-B,B]
void sampleUniform(ZZX& poly, long n, const ZZ& B)
{
  //OLD: assert (B>0);
  helib::assertTrue<helib::InvalidArgument>(static_cast<bool>(B>0l), "Invalid coefficient interval");
  if (n<=0) n=deg(poly)+1; if (n<=0) return;
  clear(poly);
  poly.SetMaxLength(n); // allocate space for degree-(n-1) polynomial

  ZZ UB = 2*B +1;
  for (long i = n-1; i >= 0; i--) {
    ZZ tmp = RandomBnd(UB) - B;
    SetCoeff(poly, i, tmp);
  }
}

/********************************************************************
 * Below are versions of the sampling routines that sample modulo
 * X^m-1 and then reduce mod Phi_m(X). The exception is when m is
 * a power of two, where we still sample directly mod Phi_m(X).
 ********************************************************************/
double sampleHWt(zzX &poly, const FHEcontext& context, long Hwt)
{
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    sampleHWt(poly, m, Hwt);
    reduceModPhimX(poly, palg);
    retval = context.noiseBoundForHWt(Hwt, m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    sampleHWt(poly, phim, Hwt);
    retval = context.noiseBoundForHWt(Hwt, phim);
  }

  return retval;
}

double sampleHWtBoundedEffectiveBound(const FHEcontext& context, long Hwt)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  const PAlgebra& palg = context.zMStar;

  long deg_bnd = (palg.getPow2() == 0) ? palg.getM() : palg.getPhiM();

  long log_deg_bnd = long(log(double(deg_bnd))/log(2.0) + 0.5) + 1;
  //  sqrt(2) * deg_bnd  <= 2^{log_deg_bnd} <= 2*sqrt(2) * deg_bnd

  // we use log_deg_bnd as in index into the erfc_inverse table
  // so that we get a noise bound that should be satisfied
  // with probablity at least 1/sqrt(2).

  //OLD: assert(log_deg_bnd < ERFC_INVERSE_SIZE);
  helib::assertTrue(log_deg_bnd < ERFC_INVERSE_SIZE, "log_deg_bnd must be less than ERFC_INVERSE_SIZE");
  double scale = erfc_inverse[log_deg_bnd];
  double bound = scale * sqrt(double(Hwt));

  return bound;
#else
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    retval = context.noiseBoundForHWt(Hwt, m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    retval = context.noiseBoundForHWt(Hwt, phim);
  }
  return retval;
#endif
}

#if 1
double sampleHWtBounded(zzX &poly, const FHEcontext& context, long Hwt)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  double bound = sampleHWtBoundedEffectiveBound(context, Hwt);
  const PAlgebra& palg = context.zMStar;

  double val;
  long count = 0;
  do {
    sampleHWt(poly, context, Hwt);
    val = embeddingLargestCoeff(poly,palg);
    //cerr << "****** " << (val/bound) << "\n";
  }
  while (++count<1000 && val>bound); // repeat until <= bound

  if (val>bound) {
    std::stringstream ss;
    ss << "Error: sampleSmallBounded, after "
         << count<<" trials, still val="<<val
         << '>'<<"bound="<<bound;
    throw helib::RuntimeError(ss.str());
  }
  return bound;
#else
  return sampleHWt(poly, context, Hwt);
#endif

}

#elif 0

// Experimental version

static ZZ
calculate_matrix_norm(const zzX& try_poly, const FHEcontext& context)
{
  const RecryptData& rcData = context.rcData;
  const PowerfulDCRT* p2dConv = rcData.p2dConv;
  const PAlgebra& palg = context.zMStar;
  long phim = palg.getPhiM();
  long m = palg.getM();
  const ZZX& PhimX = palg.getPhimX();

  ZZX poly;
  convert(poly, try_poly);

  IndexSet iset = IndexSet( context.ctxtPrimes.first(),
                            min( context.ctxtPrimes.first()+3,
                                context.ctxtPrimes.last() ) );
  // use a smaller prime set for powerful conversions
  // right now, we just use the first 3  ctxt primes

  ZZ retval {0};

  for (long i: range(phim)) {
    if (i%300 == 0) cerr << ".";
    Vec<ZZ> basis_vec;
    basis_vec.SetLength(phim);
    basis_vec[i] = 1;

    ZZX basis_poly;
    p2dConv->powerfulToZZX(basis_poly, basis_vec, iset);

    basis_poly = basis_poly*poly;

    // reduce basis_poly mod X^m-1, which is enough for ZZXtoPowerful
    long d = basis_poly.rep.length();

    if (d > m) {
      for (long j: range(m, d)) {
        basis_poly.rep[j-m] += basis_poly.rep[j];
      }
      basis_poly.rep.SetLength(m);
      basis_poly.normalize();
    }

    p2dConv->ZZXtoPowerful(basis_vec, basis_poly, iset);

    for (long j: range(phim)) {
      retval += basis_vec[j]*basis_vec[j];
    }
  }

  return retval;
}

double sampleHWtBounded(zzX &poly, const FHEcontext& context, long Hwt)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  double bound = sampleHWtBoundedEffectiveBound(context, Hwt);
  const PAlgebra& palg = context.zMStar;

  ZZ best_matrix_norm;
  zzX best_poly;

  cerr << "*** starting trials\n";

  for (long trials = 0; trials < 20; trials++) {

    zzX try_poly;

    double val;
    long count = 0;
    do {
      sampleHWt(try_poly, context, Hwt);
      val = embeddingLargestCoeff(try_poly,palg);
      //cerr << "****** " << (val/bound) << "\n";
    }
    while (++count<1000 && val>bound); // repeat until <= bound

    if (val>bound) {
      std::stringstream ss;
      ss << "Error: sampleSmallBounded, after "
	   << count<<" trials, still val="<<val
	   << '>'<<"bound="<<bound;
      throw helib::RuntimeError(ss.str());
    }

    ZZ try_matrix_norm = calculate_matrix_norm(try_poly, context);

    if (trials == 0 || try_matrix_norm < best_matrix_norm) {
      best_matrix_norm = try_matrix_norm;
      best_poly = try_poly;
    }

    cerr << try_matrix_norm << "\n";
  }

  cerr << "*** ending trials\n";

  return bound;
#else
  return sampleHWt(poly, context, Hwt);
#endif

}

#else

void sampleHWtAlt(zzX& poly, const FHEcontext& context, long Hwt)
{
  const RecryptData& rcData = context.rcData;
  const PowerfulDCRT* p2dConv = rcData.p2dConv;
  const PAlgebra& palg = context.zMStar;
  long phim = palg.getPhiM();
  long m = palg.getM();

  Vec<ZZ> pwrfl;
  pwrfl.SetLength(phim);

  for (long i=0; i<Hwt; ) {  // continue until exactly Hwt nonzero coefficients
    long u = RandomBnd(phim);  // The next coefficient to choose
    if (pwrfl[u]==0) { // if we didn't choose it already
      long b = RandomBits_long(2)&2; // b random in {0,2}
      pwrfl[u] = b-1;                      //   random in {-1,1}
      i++; // count another nonzero coefficient
    }
  }

  ZZX poly1;
  p2dConv->powerfulToZZX(poly1, pwrfl);

  convert(poly, poly1);
}

// Experimental version

double sampleHWtBounded(zzX &poly, const FHEcontext& context, long Hwt)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  double bound = sampleHWtBoundedEffectiveBound(context, Hwt);
  const PAlgebra& palg = context.zMStar;

  double val;
  long count = 0;
  do {
    sampleHWtAlt(poly, context, Hwt);
    val = embeddingLargestCoeff(poly,palg);
    //cerr << "****** " << (val/bound) << "\n";
  }
  while (++count<1000 && val>bound); // repeat until <= bound

  if (val>bound) {
    std::stringstream ss;
    ss << "Error: sampleSmallBounded, after "
         << count<<" trials, still val="<<val
         << '>'<<"bound="<<bound;
    throw helib::RuntimeError(ss.str());
  }
  return bound;
#else
  return sampleHWt(poly, context, Hwt);
#endif

}

#endif

double sampleSmall(zzX &poly, const FHEcontext& context)
{
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    long phim = palg.getPhiM();
    sampleSmall(poly, m, phim/(2.0*m)); // nonzero with prob phi(m)/2m
    // FIXME: does this probability make sense?  What is the goal there??
    reduceModPhimX(poly, palg);
    retval = context.noiseBoundForSmall(phim/(2.0*m), m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    sampleSmall(poly, phim);
    retval = context.noiseBoundForSmall(0.5, phim);
  }

  return retval;
}

// Same as above, but ensure the result is not too much larger than typical
double sampleSmallBounded(zzX &poly, const FHEcontext& context)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  const PAlgebra& palg = context.zMStar;
  long m = palg.getM();
  long phim = palg.getPhiM();
  // experimental bound, Pr[l-infty(canonical-embedding)>bound]<5%
  double bound = (1+sqrt(phim*log(phim)))*0.85;
  double val;
  long count = 0;
  do {
    sampleSmall(poly, context);
    val = embeddingLargestCoeff(poly,palg);
  }
  while (++count<1000 && val>bound); // repeat until <= bound
  if (val>bound) {
    std::stringstream ss;
    ss << "Error: sampleSmallBounded, after "
         << count<<" trials, still val="<<val
         << '>'<<"bound="<<bound;
    throw helib::RuntimeError(ss.str());
  }
  return bound;
#else
#warning "No FFT, sampleSmallBounded degenerates to sampleSmall"
  return sampleSmall(poly, context);
#endif
}

double sampleGaussian(zzX &poly, const FHEcontext& context, double stdev)
{
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    sampleGaussian(poly, m, stdev);
    reduceModPhimX(poly, palg);
    retval = context.noiseBoundForGaussian(stdev, m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    sampleGaussian(poly, phim, stdev);
    retval = context.noiseBoundForGaussian(stdev, phim);
  }

  return retval;
}
// Same as above, but ensure the result is not too much larger than typical
double sampleGaussianBounded(zzX &poly, const FHEcontext& context, double stdev)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  const PAlgebra& palg = context.zMStar;
  long m = palg.getM();
  long phim = palg.getPhiM();
  // experimental bound, Pr[l-infty(canonical-embedding)>bound]<5%
  double bound
    = stdev*1.15*(2+((palg.getPow2()==0)?
                     sqrt(m*log(phim)) : sqrt(phim*log(phim))));
  double val;
  long count = 0;
  do {
    sampleGaussian(poly, context, stdev);
    val = embeddingLargestCoeff(poly,palg);
  }
  while (++count<1000 && val>bound); // repeat until <=bound
  if (val>bound) {
    std::stringstream ss;
    ss << "Error: sampleGaussianBounded, after "
         << count<<" trials, still val="<<val
         << '>'<<"bound="<<bound;
    throw helib::RuntimeError(ss.str());
  }
  return bound;
#else
#warning "No FFT, sampleGaussianBounded degenerates to sampleGaussian"
  return sampleGaussian(poly, context, stdev);
#endif
}

double sampleUniform(zzX &poly, const FHEcontext& context, long B)
{
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    sampleUniform(poly, m, B);
    reduceModPhimX(poly, palg);
    retval = context.noiseBoundForUniform(B, m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    sampleUniform(poly, phim, B);
    retval = context.noiseBoundForUniform(B, phim);
  }

  return retval;
}

xdouble sampleUniform(ZZX &poly, const FHEcontext& context, const ZZ& B)
{
  const PAlgebra& palg = context.zMStar;
  xdouble retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    sampleUniform(poly, m, B);
    NTL::rem(poly, poly, palg.getPhimX());
    retval = context.noiseBoundForUniform(conv<xdouble>(B), m);
  }
  else {// power of two
    long phim = palg.getPhiM();
    sampleUniform(poly, phim, B);
    retval = context.noiseBoundForUniform(conv<xdouble>(B), phim);
  }

  return retval;
}

// Helper functions, return a bound B such that for random noise
// terms we have Pr[|canonicalEmbed(noise)|_{\infty} > B] < epsilon.
// (The default is epsilon = 2^{-40}.)
double boundFreshNoise(long m, long phim, double sigma, double epsilon)
{
  // The various constants in this function were determined experimentally.

  /* We begin by computing the standard deviation of the magnitude of
   * f(zeta), where f = sampleSmallBounded * sampleGaussian(sigma)
   *                 +  sampleSmall * sampleGaussianBounded(sigma)
   *                 + sampleGaussian(sigma)
   * and zeta is an m'th root-of-unity, which we approximate as:
   */
  double stdev = (sigma+0.1)*0.54*(1+(is2power(m)? sqrt(phim*m): phim));

  /* Then we use the following rules:
   *      Pr[|f(zeta)| > stdev] = 0.644
   *      Pr[|f(zeta)| > 2*stdev] = 0.266
   *      Pr[|f(zeta)| > 3*stdev] = 9.04e-2
   *      Pr[|f(zeta)| > 4*stdev] = 2.76e-2
   *      Pr[|f(zeta)| > 5*stdev] = 7.89e-3
   *      Pr[|f(zeta)| > 6*stdev] = 2.16e-3
   *      Pr[|f(zeta)| > n*stdev] = 2.16e-3 * 4^{6-n} for n>6
   *
   * We return the smallest number of standard deviations n satifying
   *      Pr[|f(zeta)|>(n stdev)] = epsilon / phi(m)
   */
  epsilon /= phim;

  if (epsilon >= 1.87e-3) { // use the values from above
    if (epsilon >= 0.64)        { return stdev; }
    else if (epsilon >= 0.26)   { return 2*stdev; }
    else if (epsilon >= 8.52e-2) { return 3*stdev; }
    else if (epsilon >= 2.54e-2) { return 4*stdev; }
    else if (epsilon >= 7.06e-3) { return 5*stdev; }
    else                         { return 6*stdev; }
  }
  long num = 7;
  for (double prob=(1.87e-3)/4; prob>epsilon; prob /= 4)
    num++;

  return stdev * num;
}
double boundRoundingNoise(long m, long phim, long p2r, double epsilon)
{
  // The various constants in this function were determined experimentally.

  /* We begin by computing the standard deviation of the magnitude of
   * f(zeta), where
   *          f= sampleSmallBounded*sampleUniform(p) +sampleUniform(p),
   * and zeta is an m'th root-of-unity, which we approximate as:
   */
  double stdev = (2*p2r+1)*(phim-2)/8.0;

  /* Then we use the following rules:
   *      Pr[|f(zeta)| > stdev] = 0.514
   *      Pr[|f(zeta)| > 2*stdev] = 0.194
   *      Pr[|f(zeta)| > 3*stdev] = 6.7e-2
   *      Pr[|f(zeta)| > 4*stdev] = 2.23e-2
   *      Pr[|f(zeta)| > 5*stdev] = 7.21e-3
   *      Pr[|f(zeta)| > 6*stdev] = 2.31e-3
   *      Pr[|f(zeta)| > 7*stdev] = 7.25e-4
   *      Pr[|f(zeta)| > n*stdev] = 7.25e-4 * 3.3^{7-n} for n>5
   *
   * We return the smallest number of standard deviations n satifying
   *      Pr[|f(zeta)|>(n stdev)] = epsilon / phi(m)
   */
  epsilon /= phim;

  if (epsilon >= 7.25e-4) { // use the values from above
    if (epsilon >= 0.514)        { return stdev; }
    else if (epsilon >= 0.194)   { return 2*stdev; }
    else if (epsilon >= 6.7e-2)  { return 3*stdev; }
    else if (epsilon >= 2.23e-2) { return 4*stdev; }
    else if (epsilon >= 7.21e-3) { return 5*stdev; }
    else if (epsilon >= 2.31e-3) { return 6*stdev; }
    else                         { return 7*stdev; }
  }
  long num = 8;
  for (double prob=(7.25e-4)/3.2; prob>epsilon; prob /= 3.2)
    num++;

  return stdev * num;
}


/*
 * Chrono: measure time duration between two points with boost.datetime
 *
 * Copyright (c) 2012 Jérémie Decock
 *
 * Required: boost.datetime library
 * Usage: g++ time_budget_loop.cc
 *
 */

#include <iostream>
#include <boost/date_time/posix_time/posix_time.hpp>

int main() {

    double wait_seconds = 0.005;
    boost::posix_time::ptime end_time = boost::posix_time::microsec_clock::local_time() + boost::posix_time::microseconds(wait_seconds * 1000000.);

    while(boost::posix_time::microsec_clock::local_time() < end_time) {
        // do something
        std::cout << boost::posix_time::microsec_clock::local_time() << std::endl;
    }

    return 0;
}


#include <iostream>
#include <vector>
#include <string>
#include <boost/unordered_map.hpp>

using namespace std;

class Solution {
public:
  vector<vector<string>> groupAnagrams(vector<string>& strs) {
    boost::unordered_map<vector<int>, vector<string> > map;
    for(auto str : strs){
      vector<int> list(26, 0);
      for(char i : str){
        list[i-'a'] += 1;
      }
      if(map.count(list) == 0)
        map.insert({list, vector<string>()});
      map[list].push_back(str);
    }
    vector< vector<string> > res;
    for(auto itr : map)
      res.push_back(itr.second);
    return res;
  }
};

int main(){
  Solution a;
  vector<string> list = {"aaa", "bba", "abb", "aba", "bab"};
  vector< vector<string> > res = a.groupAnagrams(list);
  for(vector<string>& j : res){
    for(string& i : j)
      cout << i << " ";
    cout << endl;
  }

  //unordered_map<int, vector<int> > dd;
  //cout << dd[2] << endl;
  /*

  */
  return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   GaussianConditional.cpp
 * @brief  Conditional Gaussian Base class
 * @author Christian Potthast, Frank Dellaert
 */

#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/Sampler.h>

#include <boost/format.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif

#include <functional>
#include <list>
#include <string>

// In Wrappers we have no access to this so have a default ready
static std::mt19937_64 kRandomNumberGenerator(42);

using namespace std;

namespace gtsam {

  /* ************************************************************************* */
  GaussianConditional::GaussianConditional(
    Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) :
  BaseFactor(key, R, d, sigmas), BaseConditional(1) {}

  /* ************************************************************************ */
  GaussianConditional::GaussianConditional(Key key, const Vector& d,
                                           const Matrix& R, Key parent1,
                                           const Matrix& S,
                                           const SharedDiagonal& sigmas)
      : BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {}

  /* ************************************************************************ */
  GaussianConditional::GaussianConditional(Key key, const Vector& d,
                                           const Matrix& R, Key parent1,
                                           const Matrix& S, Key parent2,
                                           const Matrix& T,
                                           const SharedDiagonal& sigmas)
      : BaseFactor(key, R, parent1, S, parent2, T, d, sigmas),
        BaseConditional(1) {}

  /* ************************************************************************ */
  GaussianConditional GaussianConditional::FromMeanAndStddev(
      Key key, const Matrix& A, Key parent, const Vector& b, double sigma) {
    // |Rx + Sy - d| = |x-(Ay + b)|/sigma
    const Matrix R = Matrix::Identity(b.size(), b.size());
    const Matrix S = -A;
    const Vector d = b;
    return GaussianConditional(key, d, R, parent, S,
                               noiseModel::Isotropic::Sigma(b.size(), sigma));
  }

  /* ************************************************************************ */
  GaussianConditional GaussianConditional::FromMeanAndStddev(
      Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2,
      const Vector& b, double sigma) {
    // |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma
    const Matrix R = Matrix::Identity(b.size(), b.size());
    const Matrix S = -A1;
    const Matrix T = -A2;
    const Vector d = b;
    return GaussianConditional(key, d, R, parent1, S, parent2, T,
                               noiseModel::Isotropic::Sigma(b.size(), sigma));
  }

  /* ************************************************************************ */
  void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
    cout << s << " p(";
    for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
      cout << (boost::format("%1%") % (formatter(*it))).str()
           << (nrFrontals() > 1 ? " " : "");
    }

    if (nrParents()) {
      cout << " |";
      for (const_iterator it = beginParents(); it != endParents(); ++it) {
        cout << " " << (boost::format("%1%") % (formatter(*it))).str();
      }
    }
    cout << ")" << endl;

    cout << formatMatrixIndented("  R = ", R()) << endl;
    for (const_iterator it = beginParents() ; it != endParents() ; ++it) {
      cout << formatMatrixIndented((boost::format("  S[%1%] = ")%(formatter(*it))).str(), getA(it))
        << endl;
    }
    cout << formatMatrixIndented("  d = ", getb(), true) << "\n";
    if (model_)
      model_->print("  Noise model: ");
    else
      cout << "  No noise model" << endl;
  }

  /* ************************************************************************* */
  bool GaussianConditional::equals(const GaussianFactor& f, double tol) const {
    if (const GaussianConditional* c = dynamic_cast<const GaussianConditional*>(&f)) {
      // check if the size of the parents_ map is the same
      if (parents().size() != c->parents().size())
        return false;

      // check if R_ and d_ are linear independent
      for (DenseIndex i = 0; i < Ab_.rows(); i++) {
        list<Vector> rows1, rows2;
        rows1.push_back(Vector(R().row(i)));
        rows2.push_back(Vector(c->R().row(i)));

        // check if the matrices are the same
        // iterate over the parents_ map
        for (const_iterator it = beginParents(); it != endParents(); ++it) {
          const_iterator it2 = c->beginParents() + (it - beginParents());
          if (*it != *(it2))
            return false;
          rows1.push_back(row(getA(it), i));
          rows2.push_back(row(c->getA(it2), i));
        }

        Vector row1 = concatVectors(rows1);
        Vector row2 = concatVectors(rows2);
        if (!linear_dependent(row1, row2, tol))
          return false;
      }

      // check if sigmas are equal
      if ((model_ && !c->model_) || (!model_ && c->model_)
        || (model_ && c->model_ && !model_->equals(*c->model_, tol)))
        return false;

      return true;
    } else {
      return false;
    }
  }

  /* ************************************************************************* */
  VectorValues GaussianConditional::solve(const VectorValues& x) const {
    // Concatenate all vector values that correspond to parent variables
    const Vector xS = x.vector(KeyVector(beginParents(), endParents()));

    // Update right-hand-side
    const Vector rhs = d() - S() * xS;

    // Solve matrix
    const Vector solution = R().triangularView<Eigen::Upper>().solve(rhs);

    // Check for indeterminant solution
    if (solution.hasNaN()) {
      throw IndeterminantLinearSystemException(keys().front());
    }

    // Insert solution into a VectorValues
    VectorValues result;
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      result.emplace(*frontal, solution.segment(vectorPosition, getDim(frontal)));
      vectorPosition += getDim(frontal);
    }

    return result;
  }

  /* ************************************************************************* */
  VectorValues GaussianConditional::solveOtherRHS(
    const VectorValues& parents, const VectorValues& rhs) const {
    // Concatenate all vector values that correspond to parent variables
    Vector xS = parents.vector(KeyVector(beginParents(), endParents()));

    // Instead of updating getb(), update the right-hand-side from the given rhs
    const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
    xS = rhsR - S() * xS;

    // Solve Matrix
    Vector soln = R().triangularView<Eigen::Upper>().solve(xS);

    // Scale by sigmas
    if (model_)
      soln.array() *= model_->sigmas().array();

    // Insert solution into a VectorValues
    VectorValues result;
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      result.emplace(*frontal, soln.segment(vectorPosition, getDim(frontal)));
      vectorPosition += getDim(frontal);
    }

    return result;
  }

  /* ************************************************************************* */
  void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
    Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
    frontalVec = R().transpose().triangularView<Eigen::Lower>().solve(frontalVec);

    // Check for indeterminant solution
    if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());

    for (const_iterator it = beginParents(); it!= endParents(); it++)
      gy[*it].noalias() += -1.0 * getA(it).transpose() * frontalVec;

    // Scale by sigmas
    if (model_)
      frontalVec.array() *= model_->sigmas().array();

    // Write frontal solution into a VectorValues
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal));
      vectorPosition += getDim(frontal);
    }
  }

  /* ************************************************************************ */
  JacobianFactor::shared_ptr GaussianConditional::likelihood(
      const VectorValues& frontalValues) const {
    // Error is |Rx - (d - Sy - Tz - ...)|^2
    // so when we instantiate x (which has to be completely known) we beget:
    // |Sy + Tz + ... - (d - Rx)|^2
    // The noise model just transfers over!

    // Get frontalValues as vector
    const Vector x =
        frontalValues.vector(KeyVector(beginFrontals(), endFrontals()));

    // Copy the augmented Jacobian matrix:
    auto newAb = Ab_;

    // Restrict view to parent blocks
    newAb.firstBlock() += nrFrontals_;

    // Update right-hand-side (last column)
    auto last = newAb.matrix().cols() - 1;
    const auto RR = R().triangularView<Eigen::Upper>();
    newAb.matrix().col(last) -= RR * x;

    // The keys now do not include the frontal keys:
    KeyVector newKeys;
    newKeys.reserve(nrParents());
    for (auto&& key : parents()) newKeys.push_back(key);

    // Hopefully second newAb copy below is optimized out...
    return boost::make_shared<JacobianFactor>(newKeys, newAb, model_);
  }

  /* **************************************************************************/
  JacobianFactor::shared_ptr GaussianConditional::likelihood(
      const Vector& frontal) const {
    if (nrFrontals() != 1)
      throw std::invalid_argument(
          "GaussianConditional Single value likelihood can only be invoked on "
          "single-variable conditional");
    VectorValues values;
    values.insert(keys_[0], frontal);
    return likelihood(values);
  }

  /* ************************************************************************ */
  VectorValues GaussianConditional::sample(const VectorValues& parentsValues,
                                           std::mt19937_64* rng) const {
    if (nrFrontals() != 1) {
      throw std::invalid_argument(
          "GaussianConditional::sample can only be called on single variable "
          "conditionals");
    }
    if (!model_) {
      throw std::invalid_argument(
          "GaussianConditional::sample can only be called if a diagonal noise "
          "model was specified at construction.");
    }
    VectorValues solution = solve(parentsValues);
    Key key = firstFrontalKey();
    const Vector& sigmas = model_->sigmas();
    solution[key] += Sampler::sampleDiagonal(sigmas, rng);
    return solution;
  }

  VectorValues GaussianConditional::sample(std::mt19937_64* rng) const {
    if (nrParents() != 0)
      throw std::invalid_argument(
          "sample() can only be invoked on no-parent prior");
    VectorValues values;
    return sample(values);
  }

  /* ************************************************************************ */
  VectorValues GaussianConditional::sample() const {
    return sample(&kRandomNumberGenerator);
  }

  VectorValues GaussianConditional::sample(const VectorValues& given) const {
    return sample(given, &kRandomNumberGenerator);
  }

  /* ************************************************************************ */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
  void GTSAM_DEPRECATED
  GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array();
      vectorPosition += getDim(frontal);
    }
  }
#endif

}  // namespace gtsam


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file traits_test.hpp
 * \date October 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include <fl/util/math.hpp>
#include <fl/util/traits.hpp>

#include <cmath>

TEST(TraitsTests, is_dynamic)
{
    EXPECT_TRUE(fl::IsDynamic<Eigen::Dynamic>());
    EXPECT_FALSE(fl::IsDynamic<10>());
}

TEST(TraitsTests, is_fixed)
{
    EXPECT_FALSE(fl::IsFixed<Eigen::Dynamic>());
    EXPECT_TRUE(fl::IsFixed<10>());
}

TEST(TraitsTests, DimensionOf_dynamic)
{
    EXPECT_EQ(fl::DimensionOf<Eigen::MatrixXd>(), 0);
    EXPECT_EQ(fl::DimensionOf<Eigen::VectorXd>(), 0);

    typedef Eigen::Matrix<double, 10, -1> PartiallyDynamicMatrix;
    EXPECT_EQ(fl::DimensionOf<PartiallyDynamicMatrix>(), 0);
}

TEST(TraitsTests, DimensionOf_fixed)
{
    EXPECT_EQ(fl::DimensionOf<Eigen::Matrix3d>(), 3);
    EXPECT_EQ(fl::DimensionOf<Eigen::Vector4d>(), 4);

    typedef Eigen::Matrix<double, 10, 10> FixedMatrix;
    EXPECT_EQ(fl::DimensionOf<FixedMatrix>(), 10);

    typedef Eigen::Matrix<double, 10, 1> FixedVector;
    EXPECT_EQ(fl::DimensionOf<FixedVector>(), 10);
}


#include "factorial_opencl_fixture.h"

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

namespace {
const char* kProgramCode = R"(
ulong FactorialImplementation(int val)
{
    ulong result = 1;
    for(int i = 1; i <= val; i++)
    {
        result *= i;
    }
    return result;
}

__kernel void TrivialFactorial(__global int* input, __global ulong* output)
{
    size_t id = get_global_id(0);
    output[id] = FactorialImplementation(input[id]);
}
)";

constexpr const char* const kCompilerOptions = "-Werror";
}  // namespace

namespace kpv {
FactorialOpenClFixture::FactorialOpenClFixture(
    const std::shared_ptr<cl_benchmark::OpenClDevice>& device, int data_size)
    : device_(device), data_size_(data_size) {}

void FactorialOpenClFixture::Initialize() {
    GenerateData();
    auto program = boost::compute::program::build_with_source(
        kProgramCode, device_->GetContext(), kCompilerOptions);
    kernel_ = program.create_kernel("TrivialFactorial");
}

kpv::cl_benchmark::EventList FactorialOpenClFixture::Execute(
    const cl_benchmark::RuntimeParams& params) {
    boost::compute::context& context = device_->GetContext();
    boost::compute::command_queue& queue = device_->GetQueue();

    kpv::cl_benchmark::EventList event_list;

    // create a vector on the device
    boost::compute::vector<cl_int> input_device_vector(data_size_, context);

    // copy data from the host to the device
    event_list.AddOpenClEvent(
        "Copying input data",
        boost::compute::copy_async(
            input_data_.begin(), input_data_.end(), input_device_vector.begin(), queue));

    boost::compute::vector<cl_ulong> output_device_vector(data_size_, context);
    kernel_.set_arg(0, input_device_vector);
    kernel_.set_arg(1, output_device_vector);

    event_list.AddOpenClEvent(
        "Calculating", queue.enqueue_1d_range_kernel(kernel_, 0, data_size_, 0));

    output_data_.resize(data_size_);
    event_list.AddOpenClEvent(
        "Copying output data",
        boost::compute::copy_async(
            output_device_vector.begin(), output_device_vector.end(), output_data_.begin(), queue));

    return event_list;
}

void FactorialOpenClFixture::VerifyResults() {
    if (output_data_.size() != expected_output_data_.size()) {
        throw std::runtime_error(
            "Result verification has failed for factorial fixture . "
            "Output data count is another from expected one.");
    }
    auto mismatched_values = std::mismatch(
        output_data_.cbegin(), output_data_.cend(), expected_output_data_.cbegin(),
        expected_output_data_.cend());
    if (mismatched_values.first != output_data_.cend()) {
        cl_ulong max_abs_error = *mismatched_values.first - *mismatched_values.second;
        throw std::runtime_error(
            (boost::format("Result verification has failed for trivial factorial fixture. "
                           "Maximum absolute error is %1% for input value %2% "
                           "(exact equality is expected).") %
             max_abs_error % *mismatched_values.first)
                .str());
    }
}

void FactorialOpenClFixture::GenerateData() {
    const cl_int min_input_val = 0;
    const cl_int max_input_val =
        20;  // Max value whose factorial fits into 64-bit unsigned integer number.

    std::random_device random_dev;
    std::mt19937 gen(random_dev());
    std::uniform_int_distribution<cl_int> uniform_dist(min_input_val, max_input_val);

    input_data_.resize(data_size_);
    std::generate_n(
        input_data_.begin(), data_size_, [&gen, &uniform_dist]() { return uniform_dist(gen); });

    expected_output_data_.reserve(data_size_);
    std::transform(
        input_data_.begin(), input_data_.end(), std::back_inserter(expected_output_data_),
        [this](cl_int i) { return correct_factorial_values_.at(i); });
}

const std::unordered_map<cl_int, cl_ulong> FactorialOpenClFixture::correct_factorial_values_ = {
    {0, 1ull},
    {1, 1ull},
    {2, 2ull},
    {3, 6ull},
    {4, 24ull},
    {5, 120ull},
    {6, 720ull},
    {7, 5040ull},
    {8, 40320ull},
    {9, 362880ull},
    {10, 3628800ull},
    {11, 39916800ull},
    {12, 479001600ull},
    {13, 6227020800ull},
    {14, 87178291200ull},
    {15, 1307674368000ull},
    {16, 20922789888000ull},
    {17, 355687428096000ull},
    {18, 6402373705728000ull},
    {19, 121645100408832000ull},
    {20, 2432902008176640000ull},
};
}  // namespace kpv


#include "search/coordinate_to_stop.hpp"

#include "geometric/bounding_box.hpp" // for WGS84Boundin...
#include <algorithm>
#include <boost/geometry/geometries/box.hpp>
#include <iterator> // for back_insert_...

namespace nepomuk
{
namespace search
{

namespace
{

CoordinateToStop::PointT as_point(geometric::WGS84Coordinate const location)
{
    return {geometric::doubleFromLatLon(location.longitude),
            geometric::doubleFromLatLon(location.latitude)};
}

geometric::WGS84Coordinate as_coordinate(CoordinateToStop::PointT const point)
{
    return {geometric::makeLatLonFromDouble<geometric::FixedLongitude>(point.get<0>()),
            geometric::makeLatLonFromDouble<geometric::FixedLatitude>(point.get<1>())};
}

boost::geometry::model::box<CoordinateToStop::PointT>
as_bbox(geometric::WGS84BoundingBox const &bounding_box)
{
    return {as_point(bounding_box.lower_left), as_point(bounding_box.upper_right)};
}

boost::geometry::model::box<CoordinateToStop::PointT>
box_around_circle(geometric::WGS84Coordinate const location, double const radius)
{
    // TODO: ensure this does work at the borders of the map
    auto scale_coordinate_lon =
        geometric::WGS84Coordinate(geometric::makeLatLonFromDouble<geometric::FixedLongitude>(
                                       geometric::doubleFromLatLon(location.longitude) + 0.1),
                                   location.latitude);
    auto scale_coordinate_lat =
        geometric::WGS84Coordinate(location.longitude,
                                   geometric::makeLatLonFromDouble<geometric::FixedLatitude>(
                                       geometric::doubleFromLatLon(location.latitude) + 0.1));

    auto const scale_x = 0.1 / geometric::distance(scale_coordinate_lon, location);
    auto const scale_y = 0.1 / geometric::distance(scale_coordinate_lat, location);

    geometric::WGS84Coordinate lower_left = {
        geometric::makeLatLonFromDouble<geometric::FixedLongitude>(
            geometric::doubleFromLatLon(location.longitude) - radius * scale_x),
        geometric::makeLatLonFromDouble<geometric::FixedLatitude>(
            geometric::doubleFromLatLon(location.latitude) - radius * scale_y)};
    geometric::WGS84Coordinate upper_right = {
        geometric::makeLatLonFromDouble<geometric::FixedLongitude>(
            geometric::doubleFromLatLon(location.longitude) + radius * scale_x),
        geometric::makeLatLonFromDouble<geometric::FixedLatitude>(
            geometric::doubleFromLatLon(location.latitude) + radius * scale_y)};

    return {as_point(lower_left), as_point(upper_right)};
}

} // namespace

CoordinateToStop::CoordinateToStop(
    std::vector<std::pair<geometric::WGS84Coordinate, StopID>> const &coordinates)
{
    auto const add_to_rtree = [this](auto const &coordinate_and_stop) {
        tree.insert(
            std::make_pair(as_point(coordinate_and_stop.first), coordinate_and_stop.second));
    };
    std::for_each(coordinates.begin(), coordinates.end(), add_to_rtree);
}

CoordinateToStop::value_type
CoordinateToStop::nearest(geometric::WGS84Coordinate const &location) const
{
    std::vector<RTreeEntry> result;
    tree.query(boost::geometry::index::nearest(as_point(location), 1), std::back_inserter(result));
    return {result.front().second, as_coordinate(result.front().first)};
}

std::vector<CoordinateToStop::value_type>
CoordinateToStop::nearest(geometric::WGS84Coordinate const &location, std::uint32_t count) const
{
    std::vector<RTreeEntry> result;
    tree.query(boost::geometry::index::nearest(as_point(location), count),
               std::back_inserter(result));
    std::vector<value_type> as_value_type(result.size());
    std::transform(
        result.begin(), result.end(), as_value_type.begin(), [](auto const &pair) -> value_type {
            return {pair.second, as_coordinate(pair.first)};
        });
    return as_value_type;
}

std::vector<CoordinateToStop::value_type>
CoordinateToStop::all(geometric::WGS84Coordinate const &location, double const radius) const
{
    std::vector<RTreeEntry> result;
    tree.query(boost::geometry::index::intersects(box_around_circle(location, 1.1 * radius)),
               std::back_inserter(result));

    // sort results by distance to the location
    std::sort(result.begin(), result.end(), [location](auto const &lhs, auto const &rhs) {
        return geometric::distance(location, as_coordinate(lhs.first)) <
               geometric::distance(location, as_coordinate(rhs.first));
    });

    result.erase(std::find_if(result.begin(),
                              result.end(),
                              [location, radius](auto const &entry) {
                                  return geometric::distance(location, as_coordinate(entry.first)) >
                                         radius;
                              }),
                 result.end());
    std::vector<value_type> as_value_type(result.size());
    std::transform(
        result.begin(), result.end(), as_value_type.begin(), [](auto const &pair) -> value_type {
            return {pair.second, as_coordinate(pair.first)};
        });
    return as_value_type;
}

std::vector<CoordinateToStop::value_type>
CoordinateToStop::all(geometric::WGS84BoundingBox const &bounding_box) const
{
    std::vector<RTreeEntry> result;
    tree.query(boost::geometry::index::intersects(as_bbox(bounding_box)),
               std::back_inserter(result));
    std::vector<value_type> as_value_type(result.size());
    std::transform(
        result.begin(), result.end(), as_value_type.begin(), [](auto const &pair) -> value_type {
            return {pair.second, as_coordinate(pair.first)};
        });
    return as_value_type;
}

} // namespace search
} // namespace nepomuk