#include <boost/python.hpp>
#include <lightspeed/tensor.hpp>

using namespace lightspeed;
using namespace boost::python;

/**
 * Compute the UED or X-Ray elastic scattering intensity for a molecule,
 * averaging over a molecular orientation quadrature grid, and accounting for
 * anisotropy and/or modifications to the scattering intensity definition. Uses
 * the independent atom model.
 *
 * First, the scattering angles theta are computed according to the elastic
 * scattering formula,
 *
 * s = 4 pi / lambda * sin(theta / 2) => theta = 2 arcsin(s * lambda / (4 * pi))
 *
 * This will throw if s * lambda / (4 * pi) is outside [+1, -1]
 *
 * Next, the scattering vectors are computed according to,
 *
 * \vec s = {sx, sy, sz} = s { cos(theta/2)sin(eta), sin(theta/2), cos(theta/2)cos(eta) }
 *
 * Next the (isotropic) atomic scattering intensity is computed according to,
 *
 * D(s) = \sum_{A} | f_A (s) |^2
 *
 * Next, the scattering intensity is computed for each point in the molecular
 * orientation quadrature { R, w_R }, and summed up, including the possible
 * anisotropy weighting factor a(R).
 *
 * I(s, eta) = \sum_{R} w_{R} a(R) I_R (s, eta)
 *
 * Here the raw scattering intensity for a given molecular orientation is computed as,
 *
 * I_R (s, eta) = | \sum_{A} exp(i \vec s * (R^\dagger r_{A})) |^2
 *
 * e.g., the diffraction pattern of the rotated molecular geometry.
 *
 * If the "anisotropy" option is set to true, the factor a(R) will be computed as
 * the square of the projection of the rotated z axis on the original z axis
 * (cos^2 zeta perpendicular one-photon pump-probe anisotropy)
 *
 * If the "mod" option is set to true, the modified scattering intensity will
 * replace the raw scattering intensity in the result:
 *
 * M_R (s, eta) = [I_R (s, eta) - D(s)] / D(s)
 *
 * @param lambda (double) - probe wavelength (e.g., deBroglie wavelength) [Angstrom].
 * @param ss (Tensor of shape (ns,)) - s values to collocate to [Angstrom^-1]
 * @param etas (Tensor of shape (neta,)) - eta values to collocate to [Radian]
 * @param xyzs (Tensor of shape (natom, 3)) - x, y, z centers of atoms [Anstrom]
 * @param fs (Tensor of shape (natom, ns)) - atomic form factors f_A (s)
 *  computed for the above atoms and s values. Specialization to UED or X-Ray
 *  can be made outside of the present code by computing these factors
 *  appropriately. E.g., f_A^UED (s) = 1 / s^2 (Z_A - f_A^XRAY (s)).
 * @param Rs (Tensor of shape (nrot, 3, 3)) - rotation matrices in the
 *  molecular orientation quadrature grid.
 * @param ws (Tensor of shape (nrot,)) - weights of rotation matrices in the
 *  molecular orientation quadrature grid.
 * @param anisotropy (bool) - a(R) will be computed as R_zz'^2 if true
 *  (perpendicular one-photon anisotropy), else a(R) will be set to 1 if false
 *  (parallel arrangement or isotropic pump-probe experiment).
 * @param mod (bool) - compute M(s,eta) if true, else compute I(s,eta) if false.
 *
 * @return I (Tensor of shape (ns, neta)) - molecular scattering intensity I(s, eta)
 *  or M(s, eta)
 **/
lightspeed::shared_ptr<Tensor> compute_diffraction(
    double lambda,
    const lightspeed::shared_ptr<Tensor>& ss,
    const lightspeed::shared_ptr<Tensor>& etas,
    const lightspeed::shared_ptr<Tensor>& xyzs,
    const lightspeed::shared_ptr<Tensor>& fs,
    const lightspeed::shared_ptr<Tensor>& Rs,
    const lightspeed::shared_ptr<Tensor>& ws,
    bool anisotropy,
    bool mod
    )
{
    // Validity checks
    ss->ndim_error(1);
    etas->ndim_error(1);
    xyzs->ndim_error(2);
    xyzs->shape_error({xyzs->shape()[0], 3});
    fs->shape_error({xyzs->shape()[0], ss->shape()[0]});
    Rs->ndim_error(3);
    Rs->shape_error({Rs->shape()[0], 3, 3});
    ws->shape_error({Rs->shape()[0]});

    // Sizes
    size_t ns = ss->shape()[0];
    size_t neta = etas->shape()[0];
    size_t nA = xyzs->shape()[0];
    size_t nR = Rs->shape()[0];

    // Pointers
    const double* sp = ss->data().data();
    const double* etap = etas->data().data();
    const double* xyzp = xyzs->data().data();
    const double* fp = fs->data().data();
    const double* Rp = Rs->data().data();
    const double* wp = ws->data().data();

    // Scattering vectors
    lightspeed::shared_ptr<Tensor> sxyz(new Tensor({ns, neta, 3}));
    double* sxyzp = sxyz->data().data();
    for (size_t sind = 0; sind < ns; sind++) {
        double arg = lambda * sp[sind] / (4.0 * M_PI);
        if (arg > 1.0 or arg < -1.0) throw std::runtime_error("Invalid s:" + std::to_string(sp[sind]));
        double theta = 2.0 * asin(arg);
        for (size_t eind = 0; eind < neta; eind++) {
            sxyzp[3 * (sind * neta + eind) + 0] = sp[sind] * cos(theta / 2.0) * sin(etap[eind]);
            sxyzp[3 * (sind * neta + eind) + 1] = sp[sind] * sin(theta / 2.0);
            sxyzp[3 * (sind * neta + eind) + 2] = sp[sind] * cos(theta / 2.0) * cos(etap[eind]);
        }
    }

    // Atomic scattering intensity
    lightspeed::shared_ptr<Tensor> Ds(new Tensor({ns}));
    double* Dp = Ds->data().data();
    for (size_t sind = 0; sind < ns; sind++) {
        for (size_t A = 0; A < nA; A++) {
            Dp[sind] += pow(fp[A * ns + sind], 2);
        }
    }

    // Target
    lightspeed::shared_ptr<Tensor> Is(new Tensor({ns, neta}));
    double* Ip = Is->data().data();

    // #pragma omp parallel for schedule(static, 16)
    #pragma omp parallel for schedule(static)
    for (size_t P = 0; P < ns * neta; P++) {
        size_t sind = P / neta;
        size_t eind = P % neta;
        double sx = sxyzp[3*P + 0];
        double sy = sxyzp[3*P + 1];
        double sz = sxyzp[3*P + 2];
        double D = Dp[sind];
        double I = 0.0;
        for (size_t Rind = 0; Rind < Rs->shape()[0]; Rind++) {
            // Rotation quadrature
            const double* R2p = Rp + Rind * 9;
            double w = wp[Rind];
            // Anisotropy weight
            if (anisotropy) {
                w *= pow(R2p[8], 2);
            }
            // Diffraction cross section
            double NR = 0.0;
            double NI = 0.0;
            for (size_t A = 0; A < xyzs->shape()[0]; A++) {
                // Rotate coordinates to frame
                double x = R2p[0] * xyzp[3*A + 0] + R2p[1] * xyzp[3*A + 1] + R2p[2] * xyzp[3*A + 2];
                double y = R2p[3] * xyzp[3*A + 0] + R2p[4] * xyzp[3*A + 1] + R2p[5] * xyzp[3*A + 2];
                double z = R2p[6] * xyzp[3*A + 0] + R2p[7] * xyzp[3*A + 1] + R2p[8] * xyzp[3*A + 2];
                double theta = sx * x + sy * y + sz * z;
                double fA = fp[A * ns + sind];
                NR += fA * cos(theta);
                NI += fA * sin(theta);
            }
            double F = NR * NR + NI * NI;
            if (mod) {
                F = (F - D) / D;
            }
            I += w * F;
        }
        Ip[P] = I;
    }

    return Is;
}

BOOST_PYTHON_MODULE(pyplugin)
{
    def("compute_diffraction", compute_diffraction);
}


#include <boost/histogram.hpp>
#include <iostream>

int main() {
    namespace bh = boost::histogram;

    auto h = bh::make_histogram(bh::axis::category<int>{});

    std::cout << bh::algorithm::sum(h) << std::endl;
}


#include <Eigen/Dense>
#include <iostream>
#include "PowerSpectrum.h"
#include "MFCC/MFCC.h"

PowerSpectrum::PowerSpectrum(Analyser * analyser)
    : analyser(analyser)
{
    SpecFrame frame;
    frame.nfft = 1;
    frame.fs = 16000;
    frame.spec.setOnes(1);

    Eigen::ArrayXd one;
    one.setOnes(1);

    holdLength = 50;
    hold.resize(holdLength, frame);
    holdIndex = 0;

    minGain = -60;
    maxGain = 20;

    connect(&timer, &QTimer::timeout, [this]() {
        repaint();
    });
    timer.setTimerType(Qt::PreciseTimer);
    timer.start(1000.0 / 60.0);
}

PowerSpectrum::~PowerSpectrum()
{
}

void PowerSpectrum::renderSpectrum(const int nframe, const int nNew, const double maximumFrequency, std::deque<SpecFrame>::const_iterator begin, std::deque<SpecFrame>::const_iterator end)
{
    using Eigen::ArrayXd;

    const double fs = begin->fs;

    // Advance the hold buffer.
    for (auto it = begin; it != end; ++it) {
        const auto & frame = *it;

        hold[holdIndex] = frame;
        holdIndex = (holdIndex + 1) % holdLength;
    }

    int maxNfft = 1;

    for (const auto & holdFrame : hold) {
        if (holdFrame.nfft > maxNfft) {
            maxNfft = holdFrame.nfft;
        }
    }

    ArrayXd maxHold(maxNfft);
    maxHold.setZero();

    double maxAmplitude = 1e-16;

    for (const auto & holdFrame : hold) {
        // Each frame needs to be resampled first.
        int gap = maxNfft / holdFrame.nfft;

        for (int i = 0; i < holdFrame.nfft; ++i) {
            for (int j = gap * i; j < gap * (i + 1); ++j) {

                if (holdFrame.spec(i) > maxHold(j)) {
                    maxHold(j) = holdFrame.spec(i);
                }

            }

            if (abs(holdFrame.spec(i)) > maxAmplitude) {
                maxAmplitude = abs(holdFrame.spec(i));
            }
        }
    }

    const double delta = fs / (2 * maxNfft);

    QPainterPath path;

    for (int i = 0; i < maxNfft; ++i) {
        double freq = i * delta;

        if (freq > maximumFrequency) {
            break;
        }

        double gain = std::clamp<double>(20 * std::log10(abs(maxHold(i))), minGain - 100, maxGain);

        int x = targetWidth - (targetWidth * (maxGain - gain)) / (maxGain - minGain);
        int y = yFromFrequency(freq, maximumFrequency);

        if (i == 0) {
            path.moveTo(x, y);
        }
        else {
            path.lineTo(x, y);
        }
    }

    painter.setPen(QPen(QColor(0xFFA500), 2));
    painter.drawPath(path);

}

double PowerSpectrum::frequencyFromY(const int y, const double maximumFrequency)
{
    const double maxFreqMel = hz2mel(maximumFrequency);

    const double freqMel = ((targetHeight - y) * maxFreqMel) / (double) targetHeight;

    return mel2hz(freqMel);
}

int PowerSpectrum::yFromFrequency(const double freq, const double maximumFrequency)
{
    const double maxFreqMel = hz2mel(maximumFrequency);

    const double freqMel = hz2mel(freq);

    return targetHeight - (targetHeight * freqMel) / maxFreqMel;
}

void PowerSpectrum::paintEvent(QPaintEvent * event)
{
    targetWidth = width();
    targetHeight = height();

    painter.begin(this);

    painter.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing | QPainter::SmoothPixmapTransform);
    painter.fillRect(0, 0, targetWidth, targetHeight, QColor(0xC0C0C0));

    analyser->callIfNewFrames(
            1,
            [](auto&&... ts) {},
            [this](auto&&... ts) { renderSpectrum(std::forward<decltype(ts)>(ts)...); },
            [](auto&&... ts) {}
    );

    painter.end();
}


/*
ID: septicmk
LANG: C++
TASK: Kmeans.cpp
*/
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/queue.hpp>
#include <boost/serialization/export.hpp>
#include <boost/mpi.hpp>
#include <algorithm>
#include <vector>
#include <queue>
#include <cmath>
#include <cstdio>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>

//namespace mpi = boost::mpi;

inline int Random(int mod){
    return static_cast<int> (static_cast<double>(rand())/ RAND_MAX * mod);
}

inline int Round(double r){
    return static_cast<int> ((r > 0.0) ? floor(r + 0.5) : ceil(r - 0.5));
}

class MLalgorithm{
    private:
        friend class boost::serialization::access;
        template <typename Archive>
        void serialize (Archive &ar, const unsigned int version){
                ar &nrow;
                ar &ncol;
                ar &data;
            }

    public:
        size_t nrow, ncol;
        std::vector<double> data;

        void Init(size_t nrow, size_t ncol){
            this->nrow = nrow;
            this->ncol = ncol;
            data.resize(nrow * ncol);
            std::fill(data.begin(), data.end(), 0.0f);
        }

        void add (std::vector<double> record, size_t i){
            for (size_t j = 0; j < record.size(); ++j){
                data[ i*ncol + j ] += record[j];
            }
        }

        std::vector<double> get_value(size_t i){
            std::vector<double> ret;
            for (size_t j = 0; j < ncol; ++j){
                ret.push_back(data[i*ncol + j]);
            }
            return ret;
        }

        virtual MLalgorithm* operator + (MLalgorithm &rhs)=0;
        virtual void beginDataScan(std::vector<double> records, size_t feat_dim) = 0;
        virtual MLalgorithm* processRecord (std::vector<double> records, size_t feat_dim)=0;
        virtual void endDataScan()=0;
        virtual bool isConverged(MLalgorithm* rhs, size_t feat_dim, double eps)=0;
        virtual void finish(std::vector<double> records, size_t feat_dim)=0;
};

class Kmeans:public MLalgorithm{
    private:
        friend class boost::serialization::access;
        template <typename Archive>
        void serialize (Archive &ar, const unsigned int version){
            ar & boost::serialization::base_object<MLalgorithm>(*this);
        }

    public:
        MLalgorithm* operator +(MLalgorithm &rhs){
            MLalgorithm *pret = new Kmeans();
            pret->Init(this->nrow, this->ncol);
            for(size_t i = 0; i< data.size() ; ++i)
                pret->data[i] = data[i] + rhs.data[i];
            return pret;
        }

        void beginDataScan(std::vector<double> records, size_t feat_dim){
            std::priority_queue<int> q;
            std::vector<int> weight;
            size_t n_records = records.size();
            size_t n_cluster = this->nrow;
            for (size_t i = 0; i < n_records; ++ i){
                int x = Random(10007);
                weight.push_back(x);
                if (q.size() < n_cluster){
                    q.push(i);
                }else if (x < weight[q.top()]){
                    q.pop();
                    q.push(i);
                }
            }
            while(!q.empty()){
                std::vector<double> record;
                record.clear();
                size_t i = q.top();
                for(size_t j = 0; j < feat_dim ; ++j){
                    record.push_back(records[i*feat_dim+j]);
                }
                add(record, n_cluster - q.size());
                q.pop();
            }
        }

        void endDataScan(){
            for(size_t i = 0; i < nrow; ++i){
                for(size_t j = 0; j < ncol; ++j){
                    data[i*ncol+j] /= data[i*ncol + ncol-1];
                }
                data[i*ncol + ncol-1] = 1;
            }

            //for(size_t i = 0; i < nrow; ++i){
                //for(size_t j = 0; j < ncol; ++j){
                    //std::cout << data[i*ncol+j] << " ";
                //}
                //std::puts("");
            //}
        }

        MLalgorithm* processRecord(std::vector<double> records, size_t feat_dim){
            MLalgorithm* pret = new Kmeans();
            pret->Init(this->nrow, this->ncol);
            size_t n_records = records.size()/feat_dim;
            for (size_t i = 0 ; i < n_records ; i++){
                std::vector<double> record;
                record.clear();
                for(size_t j = 0; j < feat_dim ; j ++){
                    record.push_back(records[i*feat_dim+j]);
                }
                double dis = (1L<<16)-1;
                int who = -1;
                //std::cout<<"bp"<<std::endl;
                for(size_t k = 0; k < this->nrow; k++){
                    std::vector<double> centroid = this->get_value(k);
                    double tmp = 0;
                    for (size_t u = 0; u < record.size(); ++u){
                        tmp += (centroid[u] - record[u])*(centroid[u] - record[u]);
                    }

                    if (tmp < dis){
                        who = k;
                        dis = tmp;
                    }
                }
                record.push_back(1);
                pret->add(record, who);
            }
            return pret;
        }

        bool isConverged(MLalgorithm* rhs, size_t feat_dim, double eps){
            double diff = 0;
            for(size_t i = 0; i < nrow; i ++){
                double tmp = 0;
                for(size_t j = 0;  j < feat_dim; j++){
                    tmp += (data[i*ncol+j] - rhs->data[i*ncol+j]) * (data[i*ncol+j] - rhs->data[i*ncol+j]);
                }
                tmp = sqrt(tmp);
                diff += tmp;
            }
            if (diff <= eps) return true;
            else return false;

        }

        void finish(std::vector<double> records, size_t feat_dim){
            std::ofstream file;
            file.open("./ans.txt");
            std::vector<float> record;
            size_t n_records = records.size()/feat_dim;
            for(size_t i = 0; i < n_records; ++i){
                std::vector<double> record;
                record.clear();
                for(size_t j = 0; j < feat_dim; ++ j){
                    record.push_back(records[i*feat_dim+j]);
                }
                double dis = (1<<16)-1;
                int who = -1;
                for(size_t k = 0; k < this->nrow; k++){
                    std::vector<double> centroid = this->get_value(k);
                    double tmp = 0;
                    for (size_t u = 0; u < record.size(); ++u){
                        tmp += (centroid[u] - record[u])*(centroid[u] - record[u]);
                    }

                    if (tmp < dis){
                        who = k;
                        dis = tmp;
                    }
                }
                file << who << std::endl;

            }

        }
};
BOOST_CLASS_EXPORT(Kmeans)

class Core{
    public:

        std::string Trim (std::string &str){
            str.erase(0,str.find_first_not_of(" \t\r\n"));
            str.erase(str.find_last_not_of(" \t\r\n") + 1);
            return str;
        }

        inline std::vector<std::string> ReadCSV(std::string pwd){
            std::ifstream fin(pwd.c_str());
            std::string e,line;
            std::vector<std::string> data;
            while (std::getline(fin, line)){
                std::stringstream  lineStream(line);
                while(std::getline(lineStream, e,',')){
                    data.push_back(e);
                }
            }
            return data;
        }

        inline double praser(std::vector<std::string> data, int feat_dim, size_t i, size_t j){
            return atof(data[i*feat_dim+j].c_str());
        }

        inline std::vector<double> partition(std::vector<std::string> origin, size_t number, size_t loc, size_t feat_dim){
            std::vector<double> ret;
            ret.clear();
            size_t n_records = origin.size()/feat_dim;
            int members = n_records/number;
            size_t lim = std::min(loc*members+members, n_records);
            for (size_t i = loc*members; i < lim ; i++){
                for(size_t j = 0 ; j < feat_dim; j ++){
                    ret.push_back(praser(origin, feat_dim, i, j));
                }
            }
            return ret;
        }

        MLalgorithm* mainLoop(int argc, char* argv[],
                MLalgorithm *ptr,
                double eps,
                size_t feat_dim){
            std::vector<std::string> data = ReadCSV("../data/iris_n.csv");
            ptr->beginDataScan(partition(data,1,0,feat_dim), feat_dim);
            boost::mpi::environment env(argc, argv);
            boost::mpi::communicator world;
            int rank = world.rank();
            size_t number = world.size();
            size_t n_records = data.size()/ feat_dim;
            size_t iter_num = 0;
            std::vector<double> records = partition(data, number, rank, feat_dim);
            if (rank == 0) std::cout << "[processor] "<< world.size() << std::endl;

            bool done = 0;
            while(!done){
                boost::mpi::broadcast(world, ptr, 0);
                MLalgorithm* local_update = ptr->processRecord(records, feat_dim);

                world.barrier();
                //if(rank ==0 ) std::cout<<"flag"<<std::endl;
                if(rank == 0){
                    std::cout << "[iteration] " << iter_num++ << std::endl;
                    MLalgorithm* ptmp;
                    MLalgorithm* global_updata = local_update;
                    for(size_t i = 1; i < world.size(); ++i){
                        world.recv(boost::mpi::any_source, i, ptmp);
                        global_updata = *global_updata + *ptmp;
                    }
                    MLalgorithm *pafter = *ptr + *global_updata;
                    pafter->endDataScan();
                    done = pafter->isConverged(ptr, feat_dim, eps);
                    ptr = pafter;
                }else{
                    world.send(0, rank, local_update);
                }
                boost::mpi::broadcast(world, done, 0);
            }
            if(rank == 0){
                std::cout << "done" << std::endl;
                std::vector<double> records;
                for(size_t i = 0; i < data.size(); ++i){
                    records.push_back(atof(data[i].c_str()));
                }
                ptr->finish(records, feat_dim);
            }
        }
};

int main(int argc,char *argv[]){
    Core *p = new Core();
    MLalgorithm *ptr = new Kmeans();
    ptr->Init(3,5);

    p->mainLoop(argc, argv, ptr, 1e-7, 4);
    return 0;
}


/**
 *****************************************************************************
 * @author     This file is part of libsnark, developed by SCIPR Lab
 *             and contributors (see AUTHORS).
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/
#include <fstream>
#include <iostream>
#ifndef MINDEPS
#include <boost/program_options.hpp>
#endif

#include <libsnark/common/default_types/ram_ppzksnark_pp.hpp>
#include <libsnark/relations/ram_computations/rams/tinyram/tinyram_params.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/ram_ppzksnark/ram_ppzksnark.hpp>

#ifndef MINDEPS
namespace po = boost::program_options;

bool process_generator_command_line(
    const int argc,
    const char **argv,
    std::string &architecture_params_fn,
    std::string &computation_bounds_fn,
    std::string &proving_key_fn,
    std::string &verification_key_fn)
{
    try {
        po::options_description desc("Usage");
        desc.add_options()("help", "print this help message")(
            "architecture_params",
            po::value<std::string>(&architecture_params_fn)->required())(
            "computation_bounds",
            po::value<std::string>(&computation_bounds_fn)->required())(
            "proving_key", po::value<std::string>(&proving_key_fn)->required())(
            "verification_key",
            po::value<std::string>(&verification_key_fn)->required());

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);

        if (vm.count("help")) {
            std::cout << desc << "\n";
            return false;
        }

        po::notify(vm);
    } catch (std::exception &e) {
        std::cerr << "Error: " << e.what() << "\n";
        return false;
    }

    return true;
}
#endif

using namespace libsnark;

int main(int argc, const char *argv[])
{
    ram_ppzksnark_snark_pp<default_ram_ppzksnark_pp>::init_public_params();
#ifdef MINDEPS
    std::string architecture_params_fn = "architecture_params.txt";
    std::string computation_bounds_fn = "computation_bounds.txt";
    std::string proving_key_fn = "proving_key.txt";
    std::string verification_key_fn = "verification_key.txt";
#else
    std::string architecture_params_fn;
    std::string computation_bounds_fn;
    std::string proving_key_fn;
    std::string verification_key_fn;

    if (!process_generator_command_line(
            argc,
            argv,
            architecture_params_fn,
            computation_bounds_fn,
            proving_key_fn,
            verification_key_fn)) {
        return 1;
    }
#endif
    libff::start_profiling();

    /* load everything */
    ram_ppzksnark_architecture_params<default_ram_ppzksnark_pp> ap;
    std::ifstream f_ap(architecture_params_fn);
    f_ap >> ap;

    std::ifstream f_rp(computation_bounds_fn);
    size_t tinyram_input_size_bound, tinyram_program_size_bound, time_bound;
    f_rp >> tinyram_input_size_bound >> tinyram_program_size_bound >>
        time_bound;

    const size_t boot_trace_size_bound =
        tinyram_program_size_bound + tinyram_input_size_bound;

    const ram_ppzksnark_keypair<default_ram_ppzksnark_pp> keypair =
        ram_ppzksnark_generator<default_ram_ppzksnark_pp>(
            ap, boot_trace_size_bound, time_bound);

    std::ofstream pk(proving_key_fn);
    pk << keypair.pk;
    pk.close();

    std::ofstream vk(verification_key_fn);
    vk << keypair.vk;
    vk.close();
}


/*
 * Software License Agreement (New BSD License)
 *
 * Copyright (c) 2013, Keith Leung, Felipe Inostroza
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Advanced Mining Technology Center (AMTC), the
 *       Universidad de Chile, nor the names of its contributors may be
 *       used to endorse or promote products derived from this software without
 *       specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AMTC, UNIVERSIDAD DE CHILE, OR THE COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <Eigen/LU>
#include <math.h>
#include "MeasurementModel.hpp"

/******** Implementation of 2d measurement Model (Range and Bearing) **********/

RangeBearingModel::RangeBearingModel(){

  config.probabilityOfDetection_ = 0.95;
  config.uniformClutterIntensity_ = 0.1;
  config.rangeLimMax_ = 5;
  config.rangeLimMin_ = 0.3;
  config.rangeLimBuffer_ = 0.25;
}

RangeBearingModel::RangeBearingModel(Eigen::Matrix2d &covZ){

  setNoise(covZ);
  config.probabilityOfDetection_ = 0.95;
  config.uniformClutterIntensity_ = 0.1;
  config.rangeLimMax_ = 5;
  config.rangeLimMin_ = 0.3;
  config.rangeLimBuffer_ = 0.25;
}

RangeBearingModel::RangeBearingModel(double Sr, double Sb){

  Eigen::Matrix2d covZ;
  covZ <<  Sr, 0, 0, Sb;
  setNoise(covZ);
  config.probabilityOfDetection_ = 0.95;
  config.uniformClutterIntensity_ = 0.1;
  config.rangeLimMax_ = 5;
  config.rangeLimMin_ = 0.3;
  config.rangeLimBuffer_ = 0.25;
}

RangeBearingModel::~RangeBearingModel(){}

bool RangeBearingModel::measure(Pose2d  &pose, Landmark2d &landmark,
				Measurement2d &measurement,
				Eigen::Matrix2d *jacobian){

  Eigen::Vector3d robotPose;
  Eigen::Vector2d mean, landmarkState;
  Eigen::Matrix2d H, landmarkUncertainty, cov;
  double range, bearing;

  pose.get(robotPose);
  landmark.get(landmarkState,landmarkUncertainty);

  range = sqrt(  pow(landmarkState(0) - robotPose(0), 2)
		+pow(landmarkState(1) - robotPose(1), 2) );

  bearing = atan2( landmarkState(1) - robotPose(1) , landmarkState(0) - robotPose(0) ) - robotPose(2);

  while(bearing>PI) bearing-=2*PI;
  while(bearing<-PI) bearing+=2*PI;

  mean << range, bearing ;

  H <<  (landmarkState(0)-robotPose(0))/mean(0)          , (landmarkState(1)-robotPose(1))/mean(0) ,
        -(landmarkState(1)-robotPose(1))/(pow(mean(0),2)), (landmarkState(0)-robotPose(0))/pow(mean(0),2) ;

  cov = H * landmarkUncertainty * H.transpose() + R_;
  measurement.set(mean, cov);

  if(jacobian != NULL)
    *jacobian = H;

  if(range > config.rangeLimMax_ || range < config.rangeLimMin_)
    return false;
  else
    return true;
}

void RangeBearingModel::inverseMeasure(Pose2d &pose, Measurement2d &measurement,
				       Landmark2d &landmark){
  Eigen::Vector3d poseState;
  Eigen::Vector2d measurementState, mean;
  Eigen::Matrix2d measurementUncertainty, covariance, Hinv;
  double t;

  pose.get(poseState);
  measurement.get(measurementState, t);
  this->getNoise(measurementUncertainty);
  mean << poseState(0) + measurementState(0) *cos( poseState(2) + measurementState(1) ),
          poseState(1) + measurementState(0) *sin( poseState(2) + measurementState(1) );

  Hinv << cos(poseState(2)+measurementState(1)) , -measurementState(0)*sin(poseState(2)+measurementState(1)) ,
          sin(poseState(2)+measurementState(1)) , measurementState(0)*cos(poseState(2)+measurementState(1));

  covariance = Hinv * measurementUncertainty *Hinv.transpose();
  landmark.set( mean, covariance );

}

double RangeBearingModel::probabilityOfDetection( Pose2d &pose,
						  Landmark2d &landmark,
						  bool &isCloseToSensingLimit ){

  Pose2d::Vec robotPose;
  Landmark2d::Vec landmarkState;
  double range, Pd;

  isCloseToSensingLimit = false;

  pose.get(robotPose);
  landmark.get(landmarkState);

  range = sqrt(  pow(landmarkState(0) - robotPose(0), 2)
		+pow(landmarkState(1) - robotPose(1), 2) );

  if( range <= config.rangeLimMax_ && range >= config.rangeLimMin_){
    Pd = config.probabilityOfDetection_;
    if( range >= (config.rangeLimMax_ - config.rangeLimBuffer_ ) ||
	range <= (config.rangeLimMin_ + config.rangeLimBuffer_ ) )
      isCloseToSensingLimit = true;
  }else{
    Pd = 0;
    if( range <= (config.rangeLimMax_ + config.rangeLimBuffer_ ) ||
	range >= (config.rangeLimMin_ - config.rangeLimBuffer_ ) )
      isCloseToSensingLimit = true;
  }

  return Pd;
}

double RangeBearingModel::clutterIntensity( Measurement2d &z,
					    int nZ){
  return config.uniformClutterIntensity_;
}

double RangeBearingModel::clutterIntensityIntegral( int nZ ){
  double sensingArea_ = 2 * PI * (config.rangeLimMax_ - config.rangeLimMin_);
  return ( config.uniformClutterIntensity_ * sensingArea_ );
}

/************* Implementation of 1d measurement model **************************/

MeasurementModel1d::MeasurementModel1d(){

  config.probabilityOfDetection_ = 0.95;
  config.uniformClutterIntensity_ = 0.1;
  config.rangeLimMax_ = 5;
  config.rangeLimMin_ = 0.3;
  config.rangeLimBuffer_ = 0.25;
}

MeasurementModel1d::MeasurementModel1d(Eigen::Matrix<double, 1, 1> &Sr){
  setNoise(Sr);
  config.probabilityOfDetection_ = 0.95;
  config.uniformClutterIntensity_ = 0.1;
  config.rangeLimMax_ = 5;
  config.rangeLimMin_ = 0.3;
  config.rangeLimBuffer_ = 0.25;
}

MeasurementModel1d::MeasurementModel1d(double Sr){
  Measurement1d::Mat S;
  S << Sr;
  setNoise(S);
  config.probabilityOfDetection_ = 0.95;
  config.uniformClutterIntensity_ = 0.1;
  config.rangeLimMax_ = 5;
  config.rangeLimMin_ = 0.3;
  config.rangeLimBuffer_ = 0.25;
}

MeasurementModel1d::~MeasurementModel1d(){}

bool MeasurementModel1d::measure(Pose1d &pose, Landmark1d &landmark,
				 Measurement1d &measurement,
				 Eigen::Matrix<double, 1, 1> *jacobian){

  Pose1d::Vec robotPos;
  Landmark1d::Vec lmkPos;
  Landmark1d::Mat lmkPosUncertainty;
  Measurement1d::Vec z;
  Eigen::Matrix<double, 1, 1> H;
  Eigen::Matrix<double, 1, 1> var;

  pose.get(robotPos);
  landmark.get(lmkPos, lmkPosUncertainty);
  z = lmkPos - robotPos;
  H << 1;
  var = H * lmkPosUncertainty * H.transpose() + R_;

  measurement.set(z, var);

  if(jacobian != NULL)
    *jacobian = H;

  if(fabs(z(0)) > config.rangeLimMax_ || fabs(z(0)) < config.rangeLimMin_)
    return false;
  else
    return true;

}

void MeasurementModel1d::inverseMeasure(Pose1d &pose, Measurement1d &measurement,
					Landmark1d &landmark){

  Pose1d::Vec x;
  Measurement1d::Vec z;
  Landmark1d::Vec m;

  pose.get(x);
  measurement.get(z);
  landmark.get(m);

  m = x + z;
  landmark.set( m, R_ );

}

double MeasurementModel1d::probabilityOfDetection( Pose1d &pose,
						   Landmark1d &landmark,
						   bool &isCloseToSensingLimit ){

  Pose1d::Vec robotPose;
  Landmark1d::Vec landmarkState;
  double range, Pd;

  isCloseToSensingLimit = false;

  pose.get(robotPose);
  landmark.get(landmarkState);
  range = fabs( landmarkState(0) - robotPose(0) );

  if( range <= config.rangeLimMax_ && range >= config.rangeLimMin_){
    Pd = config.probabilityOfDetection_;
  }else{
    Pd = 0;
  }

  if( ( range >= (config.rangeLimMax_ - config.rangeLimBuffer_ ) &&
	range <= (config.rangeLimMax_ + config.rangeLimBuffer_ ) ) ||
      ( range >= (config.rangeLimMin_ - config.rangeLimBuffer_ ) &&
	range <= (config.rangeLimMin_ + config.rangeLimBuffer_ ) ) )
    isCloseToSensingLimit = true;

  return Pd;
}

double MeasurementModel1d::clutterIntensity( Measurement1d &z,
					     int nZ){
  return config.uniformClutterIntensity_;
}

double MeasurementModel1d::clutterIntensityIntegral( int nZ ){
  double sensingLength = config.rangeLimMax_ - config.rangeLimMin_;
  return ( config.uniformClutterIntensity_ * sensingLength );
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SCALAR_ACOSPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_ACOSPI_HPP_INCLUDED

#include <boost/simd/function/definition/acospi.hpp>
#include <boost/simd/arch/common/generic/function/acospi.hpp>

#endif


// ------------------------------------------------------------
// Copyright (c) Microsoft Corporation.  All rights reserved.
// Licensed under the MIT License (MIT). See License.txt in the repo root for license information.
// ------------------------------------------------------------

#include "stdafx.h"

#include <boost/test/unit_test.hpp>
#include "Common/boost-taef.h"
#include "Common/TimeSpan.h"
#include "Common/Random.h"

namespace Common
{
    class TestTimeSpan
    {
    };

    static int64  const Int64MaxValue = std::numeric_limits<int64>::max();
    static int64  const Int64MinValue = std::numeric_limits<int64>::min();

    static int64 const TicksPerMillisecond = 10000;
    static int64 const TicksPerSecond = TicksPerMillisecond * 1000;
    static int64 const TicksPerMinute = TicksPerSecond * 60;

    static double const MaxMilliseconds = static_cast<double>(Int64MaxValue / TicksPerMillisecond);
    static double const MaxSeconds = static_cast<double>(Int64MaxValue / TicksPerSecond);
    static double const MaxMinutes = static_cast<double>(Int64MaxValue / TicksPerMinute);

    BOOST_FIXTURE_TEST_SUITE(TestTimeSpanSuite,TestTimeSpan)

    BOOST_AUTO_TEST_CASE(CreateTimeSpanTest)
    {
        //Testing int64.MaxValue
        int64 const maxValue = std::numeric_limits<int64>::max();
        int seconds = 5;
        int milliseconds = 477;
        int minutes = 48;
        int hours = 2;
        int days = 10675199;

        TimeSpan timeSpan = TimeSpan::FromTicks(maxValue);
        VERIFY_IS_TRUE(timeSpan.Seconds == seconds);
        VERIFY_IS_TRUE(timeSpan.Milliseconds == milliseconds);
        VERIFY_IS_TRUE(timeSpan.Minutes == minutes);
        VERIFY_IS_TRUE(timeSpan.Hours == hours);
        VERIFY_IS_TRUE(timeSpan.Days == days);

        //Testing max values for milli, seconds and minutes

        timeSpan = TimeSpan::FromMilliseconds(MaxMilliseconds + 1);
        VERIFY_IS_TRUE(timeSpan == TimeSpan::MaxValue);

        timeSpan = TimeSpan::FromSeconds(MaxSeconds + 1);
        VERIFY_IS_TRUE(timeSpan == TimeSpan::MaxValue);

        timeSpan = TimeSpan::FromMinutes(MaxMinutes + 1);
        VERIFY_IS_TRUE(timeSpan == TimeSpan::MaxValue);

        //Testing negative timespan
        int64 const negativeValue = -837453874837489;
        seconds = -27;
        milliseconds = -483;
        minutes = -36;
        hours = -6;
        days = -969;

        timeSpan = TimeSpan::FromTicks(negativeValue);
        VERIFY_IS_TRUE(timeSpan.Seconds == seconds);
        VERIFY_IS_TRUE(timeSpan.Milliseconds == milliseconds);
        VERIFY_IS_TRUE(timeSpan.Minutes == minutes);
        VERIFY_IS_TRUE(timeSpan.Hours == hours);
        VERIFY_IS_TRUE(timeSpan.Days == days);

        //Testing TimeSpan::Zero
        timeSpan = TimeSpan::FromSeconds(0);
        VERIFY_IS_TRUE(timeSpan == TimeSpan::Zero);
    }

    BOOST_AUTO_TEST_CASE(TotalPositiveMillisecondsTest)
    {
        double seconds = -10;
        double milliseconds = static_cast<double>(Int64MinValue);

        TimeSpan timeSpan1 = TimeSpan::FromSeconds(seconds);
        TimeSpan timeSpan2 = TimeSpan::FromMilliseconds(milliseconds);

        VERIFY_IS_TRUE(timeSpan1.TotalPositiveMilliseconds() == 0);
        VERIFY_IS_TRUE(timeSpan2.TotalPositiveMilliseconds() == 0);
    }

    BOOST_AUTO_TEST_CASE(AddTimeSpanTest)
    {
        Random random = Random();
        //Testing unsafe add.
        double seconds1 = static_cast<double>(random.Next());
        double seconds2 = static_cast<double>(random.Next());

        TimeSpan timeSpan1 = TimeSpan::FromSeconds(seconds1);
        TimeSpan timeSpan2 = TimeSpan::FromSeconds(seconds2);

        TimeSpan result = timeSpan1 + timeSpan2;
        VERIFY_IS_TRUE(result.TotalSeconds() == ((seconds1 * TicksPerSecond + seconds2 * TicksPerSecond)/TicksPerSecond));

        //Testing safe add
        timeSpan1 = TimeSpan::MaxValue;
        timeSpan2 = TimeSpan::FromSeconds(seconds2);

        result = timeSpan1.AddWithMaxAndMinValueCheck(timeSpan2);
        VERIFY_IS_TRUE(result == TimeSpan::MaxValue);
    }

    BOOST_AUTO_TEST_CASE(SubtractTimeSpanTest)
    {
        Random random = Random();
        //Testing unsafe subtract.
        double seconds1 = static_cast<double>(random.Next());
        double seconds2 = static_cast<double>(random.Next());

        TimeSpan timeSpan1 = TimeSpan::FromSeconds(seconds1);
        TimeSpan timeSpan2 = TimeSpan::FromSeconds(seconds2);

        TimeSpan result = timeSpan1 - timeSpan2;
        VERIFY_IS_TRUE(result.TotalSeconds() == ((seconds1 * TicksPerSecond - seconds2 * TicksPerSecond)/TicksPerSecond));

        //Testing safe subtract
        timeSpan1 = TimeSpan::MinValue;
        timeSpan2 = TimeSpan::FromSeconds(seconds2);

        result = timeSpan1.SubtractWithMaxAndMinValueCheck(timeSpan2);
        VERIFY_IS_TRUE(result == TimeSpan::MinValue);

        timeSpan1 = TimeSpan::MinValue;
        timeSpan2 = TimeSpan::MaxValue;
        result = timeSpan1.SubtractWithMaxAndMinValueCheck(timeSpan2);
        VERIFY_IS_TRUE(result == TimeSpan::MinValue);
    }

    BOOST_AUTO_TEST_CASE(TimeSpanOperatorsTest)
    {
        TimeSpan timeSpan1 = TimeSpan::FromMinutes(100);
        TimeSpan timeSpan2 = TimeSpan::FromMinutes(200);
        TimeSpan timeSpan3 = TimeSpan::FromMinutes(200);
        TimeSpan timeSpan4 = TimeSpan::FromMinutes(400);

        VERIFY_IS_TRUE(timeSpan1 < timeSpan2);
        VERIFY_IS_TRUE(timeSpan1 != timeSpan2);
        VERIFY_IS_TRUE(timeSpan3 == timeSpan2);
        VERIFY_IS_TRUE(timeSpan4 > timeSpan2);
        VERIFY_IS_TRUE(timeSpan3 <= timeSpan2);
        VERIFY_IS_TRUE(timeSpan4 >= timeSpan1);
    }

    BOOST_AUTO_TEST_CASE(TimeSpanIsoStringTest)
    {
        TimeSpan timeSpan1 = TimeSpan::FromMinutes(10);
        std::wstring str = timeSpan1.ToIsoString();
        TimeSpan timeSpan2;
        VERIFY_IS_TRUE(TimeSpan::TryFromIsoString(str, timeSpan2));
        VERIFY_ARE_EQUAL(timeSpan1, timeSpan2);

        timeSpan1 = TimeSpan::FromMinutes(59);
        str = timeSpan1.ToIsoString();
        VERIFY_IS_TRUE(TimeSpan::TryFromIsoString(str, timeSpan2));
        VERIFY_ARE_EQUAL(timeSpan1, timeSpan2);

        timeSpan1 = TimeSpan::FromMinutes(59) + TimeSpan::FromSeconds(1);
        str = timeSpan1.ToIsoString();
        VERIFY_IS_TRUE(TimeSpan::TryFromIsoString(str, timeSpan2));
        VERIFY_ARE_EQUAL(timeSpan1, timeSpan2);

        timeSpan1 = TimeSpan::FromMinutes(1) + TimeSpan::FromSeconds(1);
        str = timeSpan1.ToIsoString();
        VERIFY_IS_TRUE(TimeSpan::TryFromIsoString(str, timeSpan2));
        VERIFY_ARE_EQUAL(timeSpan1, timeSpan2);

        timeSpan1 = TimeSpan::FromMinutes(1441);// 1 day 1 minute
        str = timeSpan1.ToIsoString();
        VERIFY_IS_TRUE(TimeSpan::TryFromIsoString(str, timeSpan2));
        VERIFY_ARE_EQUAL(timeSpan1, timeSpan2);

        timeSpan1 = TimeSpan::FromMinutes(2880);// 2 days
        str = timeSpan1.ToIsoString();
        TimeSpan::TryFromIsoString(str, timeSpan2);
        VERIFY_ARE_EQUAL(timeSpan1, timeSpan2);

        timeSpan1 = TimeSpan::FromMinutes(1440 * 30);// 1 month
        str = timeSpan1.ToIsoString();
        TimeSpan::TryFromIsoString(str, timeSpan2);
        VERIFY_ARE_EQUAL(timeSpan1, timeSpan2);

        timeSpan1 = TimeSpan::MaxValue;
        str = timeSpan1.ToIsoString();
        TimeSpan::TryFromIsoString(str, timeSpan2);
        str = timeSpan2.ToIsoString();
        VERIFY_ARE_EQUAL(timeSpan1, timeSpan2);

        timeSpan1 = TimeSpan::MinValue;
        str = timeSpan1.ToIsoString();
        TimeSpan::TryFromIsoString(str, timeSpan2);
        auto str2 = timeSpan2.ToIsoString();
        VERIFY_ARE_EQUAL(timeSpan1, timeSpan2);
        VERIFY_ARE_EQUAL(str, str2);
    }

    BOOST_AUTO_TEST_SUITE_END()
}


#include <sstream>
#include "Ligero.hpp"
#include "Math.hpp"
#include <boost/multiprecision/miller_rabin.hpp>
#include "LatticeEncryption.hpp"
#include "Factoring.hpp"
#include "Common.hpp"

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>

#include <boost/archive/binary_iarchive.hpp>

using namespace ligero;

std::vector<mpz_class> alphasPS, alphasCAN, alphasGCD;
std::vector<size_t> bucketSizePS, bucketSizeCAN, bucketSizeGCD;

constexpr auto degree = 1 << 16;
constexpr auto p = 9;
constexpr auto q = 21;
constexpr auto sigma = 8;
constexpr auto lambda = 128;
constexpr auto tau_limit_bit = 175;
constexpr auto tau = 1000;
constexpr auto numCandidates = 2048;
constexpr auto pbs = 1000;

using Q = nfl::poly_p<uint64_t, degree, q>;

static boost::filesystem::ifstream ifs("script.data", std::ios::in | std::ios::binary);
static boost::archive::binary_iarchive ia(ifs, boost::archive::no_header);

static int numParties;
static int currentStep = 1;
static size_t minRowSize = -1;

std::vector<std::vector<mpz_class>> pruneAndReorderEIT (
        const std::vector<mpz_class>& eit,
        const std::vector<int>& flags,
        int numAlphas) {

    assert (eit.size() == flags.size());

    // Now we can construct a result matrix
    std::vector<std::vector<mpz_class>> result;

    // And finally we fill the result matrix compactly with valid shares from `as`
    int k = 0;
    size_t min_size = 0;
    for (size_t c = 0; c < numAlphas; ++c) {

        std::vector<mpz_class> col;
        for (int r = 0; r < bucketSizePS[c]; ++r) {
            if (flags[k] == 1) {
                col.push_back(eit[k]);
            }
            k++;
        }
        result.push_back(col);
        if (min_size == 0) { min_size = col.size(); }
        if (min_size > col.size()) { min_size = col.size(); }
    }

    std::vector<mpz_class> col(bucketSizePS[0], mpz_class(1));
    result.insert(result.begin(), col);

    minRowSize = min_size;
    // resize and transpose
    std::vector<std::vector<mpz_class>> transposed(min_size);

    for (size_t i = 0; i < result.size(); ++i) {
        for (size_t j = 0; j < min_size; ++j) {
            transposed[j].push_back(result[i][j]);
        }
    }

    return transposed;
}

std::tuple<std::vector<mpz_class>, std::vector<int>, std::vector<mpz_class>> computeEITAndFlags(std::array<mpz_t, degree> c_mpz_t) {

    std::vector<mpz_class> eit(primesPS);

    int pq_size = bucketSizePS[0];
    std::vector<int> flags(primesPS);
    assert(eit.size() == flags.size());
    std::vector<mpz_class> c(c_mpz_t.size());

    for (size_t i = 0; i < c.size(); ++i) {
        c[i] = mpz_class(c_mpz_t[i]);
    }

    mpz_t one;
    mpz_init(one);
    mpz_set_ui(one, 1);

    mpz_t gcdResult;
    mpz_init(gcdResult);

    //assert(eit.size() == alphas.size() * bucketSize);
    int k = 0;
    for (int j = 0; j < alphasPS.size(); ++j) {

        int ick = 0;
        int sum = 0;
        for (int i = 0; i < bucketSizePS[j]; ++i) {

            mpz_gcd(gcdResult, alphasPS[j].get_mpz_t(), c_mpz_t[k]);
            eit[k] = c[k];

            flags[k] = mpz_cmp(gcdResult, one) == 0;
            sum += flags[k];
            /*
            if (flags[k] == 1) {
                index_candidates[k] = ick++;
            } else {
                index_candidates[k] = -1;
            }
            */

            k++;
        }

        if (sum < pq_size) {
            pq_size = sum;
        }
    }

    // free mem
    mpz_clear(one);
    mpz_clear(gcdResult);
    std::for_each(c_mpz_t.begin(), c_mpz_t.end(), mpz_clear);

    return {eit, flags, c};
}

template <typename T>
T validate(MessageType expectedMessageType, std::function<T(T, T)> op, T origAccumulator) {
    T computedAccumulator = origAccumulator;
    //try {
    if (ifs.is_open()) {
        // Now read and validate each round
        MessageType t;
        ia >> t;

        int msgId = int(t);
        //std::cout << "t: " << msgId << std::endl;
        if (msgId > 0) msgId = 1 + msgId - int(MessageType::ID_PARTY);
        std::cout << currentStep++ << ". " << msgs[msgId] << std::endl;

        if (t != expectedMessageType) {
            std::cout << "Unexpected MessageType. Expected message type: " << msgs[1 + int(expectedMessageType) - int(MessageType::ID_PARTY)];
            if (1 + int(t) - int(MessageType::ID_PARTY) < msgs.size()) {
                std::cout << "Got MessageType " << msgs[1 + int(t) - int(MessageType::ID_PARTY)];
            } else {
                std::cout << "Got MessageType" << 1 + int(t) - int(MessageType::ID_PARTY);
            }
            assert(false);
        }
        bool initialized = false;
        for (size_t i = 0; i < numParties; ++i) {
            std::cout << "i = " << i << std::endl;
            if (ifs && ifs.peek() != EOF) {
                std::pair<MessageType, T> zzz;
                ia >> zzz;
                auto [ti, x] = zzz;
                MessageType xt = MessageType(ti);
                if (xt != t) {
                    std::cout << "Unexpected MessageType" << std::endl;
                    std::cout << "Expected: " << ">>" << int(t) << "<<" << " -- "  << msgs[1 + int(t) - int(MessageType::ID_PARTY)];
                    std::cout << "Got: " << ">>" << int(xt) << "<<" << " -- "  << msgs[1 + int(xt) - int(MessageType::ID_PARTY)];
                    assert(false);
                }

                if (!initialized) {
                    initialized = true;
                    computedAccumulator = x;
                } else {
                    computedAccumulator = op(computedAccumulator, x);
                }
            } else {
                LOG(FATAL) << "Something went wrong, not enough data to read...";
            }
        }
        if (computedAccumulator == origAccumulator) {
            LOG(FATAL) << "Computed accumulator did not change, aborting...";
        }

        std::pair<MessageType, T> ea;
        ia >> ea;
        auto [at, expectedAccumulator] = ea;

        if (at != expectedMessageType || expectedAccumulator != computedAccumulator) {
            LOG(FATAL) << "Accumulated result did NOT match expected value";
        }
    } else {
        std::cout << "file is not good" << std::endl;
    }
    return computedAccumulator;
}

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

    //try {
    if (ifs.is_open()) {

        // Read the number of parties
        ia >> numParties;
        std::cout << "numParties: " << numParties << std::endl;

        std::vector<mpz_class> candidatesCAN, candidatesPostSieve, candidatesJacobi;

        ProtocolConfig<uint64_t> config = ProtocolConfig<uint64_t>(numParties, p, q, degree, sigma, lambda, tau_limit_bit, pbs, ProtocolMode::NORMAL);
        auto e = lattice::LatticeEncryption<uint64_t, degree, p, q>(config);

        std::tie(alphasPS, bucketSizePS) = math::balanced_bucket_n_primes(config.pbs(), primesPS, config.tauLimitBit(), 1);
        std::tie(alphasCAN, bucketSizeCAN) = math::fixed_bucket_n_primes(config.pbs()+48, primesCAN, config.tauLimitBit());

        std::vector<size_t> bucketSizeGCD;
        std::tie(alphasGCD, bucketSizeGCD) = math::fixed_bucket_n_primes(3*config.pbs()+210, primesGCD, config.tauLimitBit());

        std::vector<std::vector<mpz_class>> eit_prunedPS;
        std::vector<mpz_class> c_can(primesCAN);
        auto alphasPS_tick = alphasPS;
        std::vector<mpz_class> c_gcd(primesGCD);

        // set equal buckets for bucketSizeCan and bucketSizeGCD
        auto bucketSizeCAN_value = lrint(floor(double(primesCAN) / double(alphasCAN.size())));
        for (int i = 0; i < bucketSizeCAN.size(); ++i) {
            bucketSizeCAN[i] = bucketSizeCAN_value;
        }
        auto bucketSizeGCD_value = lrint(floor(double(primesGCD) / double(alphasGCD.size())));
        for (int i = 0; i < bucketSizeGCD.size(); ++i) {
            bucketSizeGCD[i] = bucketSizeGCD_value;
        }

        // Validate Keygen
        {
            validate<Q>(MessageType::PUBLIC_KEY_A_SHARES, std::plus<Q>(), 0);
            validate<Q>(MessageType::PUBLIC_KEY_B_SHARES, std::plus<Q>(), 0);
            std::cout << "Keygen: [SUCCESS]" << std::endl;
        }

        // Validate pre-sieve
        {
            validate<std::pair<Q, Q>>(
                    MessageType::ENCRYPTED_X_SHARES,
                    lattice::pair_add<Q>,
                    std::pair<Q, Q>{0, 0}
                    );

            validate<std::pair<Q, Q>>(
                    MessageType::ENCRYPTED_XY_PLUS_Z_SHARES,
                    lattice::pair_add<Q>,
                    std::pair<Q, Q>{0, 0}
                    );

            {
                Q computedAccumulator = validate<Q>(
                    MessageType::PARTIAL_XY_MINUS_Z_SHARES,
                    std::plus<Q>(),
                    Q{0}
                    );

                std::vector<mpz_class> eitPS;
                std::vector<int> flags_can;
                Q eit_poly = computedAccumulator;

                // convert eit_poly to eit
                std::vector<mpz_class> tauVector(degree);
                {
                    int tvk = 0;

                    // pre-sieving
                    for (size_t i = 0; i < alphasPS.size(); ++i) {
                        for (size_t j = 0; j < bucketSizePS[i]; ++j) {
                            tauVector[tvk] = alphasPS[i];
                            tvk++;
                        }
                    }

                    DBG("tvk << " << tvk);
                    DBG("primesPS << " << primesPS);
                    assert(tvk == primesPS);
                    tvk = primesPS;
                    // candidate generation
                    //index_candidates.resize(Degree);
                    for (size_t j = 0; j < bucketSizeCAN_value; ++j) {
                        for (size_t i = 0; i < alphasCAN.size(); ++i) {
                            //index_candidates[tvk] = j;
                            tauVector[tvk] = alphasCAN[i];
                            tvk++;
                        }
                    }

                    // GCD
                    for (size_t j = 0; j < bucketSizeGCD_value; ++j) {
                        for (size_t i = 0; i < alphasGCD.size(); ++i) {
                            /*
                            if (j < 1) {
                                index_candidates[tvk] = -2;
                            } else {
                                index_candidates[tvk] = -3;
                            }
                            */
                            tauVector[tvk] = alphasGCD[i];
                            tvk++;
                        }
                    }

                    // fill the rest with dummy values from pre-sieving
                    for (; tvk < degree; ++tvk) {
                        tauVector[tvk] = alphasPS[0];
                    }
                }

                DBG("before eval poly");

                auto c_mpz_t = e.eval_poly(eit_poly, tauVector);

                DBG("after eval poly");

                // Need to compute presieve flags
                std::vector<mpz_class> c;
                std::tie(eitPS, flags_can, c) = computeEITAndFlags(c_mpz_t);

                eit_prunedPS = pruneAndReorderEIT(eitPS, flags_can, alphasPS.size());

                alphasPS_tick.insert(alphasPS_tick.begin(), mpz_class(4));
                for (int i = primesPS; i < primesPS + primesCAN; ++i) {
                    c_can[i - primesPS] = c[i];
                }

                for (int i = primesPS + primesCAN; i < primesPS + primesCAN + primesGCD; ++i) {
                    c_gcd[i - primesPS - primesCAN] = c[i];
                }

                std::vector<int> expectedFlagsCan;
                MessageType expectedFlagsType;
                ia >> expectedFlagsType;

                if (expectedFlagsType != MessageType::PS_SIEVING_FLAGS) {
                    std::cout << "Unexpected MessageType " << int(expectedFlagsType) << std::endl;
                    std::cout << "Expected: " << msgs[1 + int(MessageType::PS_SIEVING_FLAGS) - int(MessageType::ID_PARTY)] << std::endl;
                    std::cout << "Got: " << msgs[1 + int(expectedFlagsType) - int(MessageType::ID_PARTY)];
                    assert(false);
                }

                ia >> expectedFlagsCan;

                if (expectedFlagsCan.size() != flags_can.size()) {
                    std::cout << "Expected eit flags size " << expectedFlagsCan.size()
                              << " does not match compute flags size " << flags_can.size();
                    assert(false);
                }
                for (size_t i = 0; i < expectedFlagsCan.size(); i++) {
                    if (flags_can[i] != expectedFlagsCan[i]) {
                        std::cout << "Expected flag flags_can[" << i << "] = " << expectedFlagsCan[i]
                                  << " does not match computed flags[" << i << "] = " << flags_can[i];
                        assert(false);
                    }
                }
            }

            std::cout << "Pre-sieve: [SUCCESS]" << std::endl;
        }

        // Validate modulus candidate generation
        {
            auto bucketSize = lrint(floor(double(primesCAN) / double(alphasCAN.size())));
            if (bucketSize > eit_prunedPS.size()) {
                bucketSize = eit_prunedPS.size();
            }
            std::vector<mpz_class> alphas_combined(alphasCAN.size() + alphasPS_tick.size());
            int aci = 0;
            for (size_t i = 0; i < alphasCAN.size(); ++i) {
                alphas_combined[aci] = alphasCAN[i];
                ++aci;
            }
            for (size_t i = 0; i < alphasPS_tick.size(); ++i) {
                alphas_combined[aci] = alphasPS_tick[i];
                ++aci;
            }

            using PairOfVec = std::pair<std::vector<mpz_class>, std::vector<mpz_class>>;
            auto acc_pair = std::pair<std::vector<mpz_class>, std::vector<mpz_class>>(std::vector<mpz_class>(primesCAN), std::vector<mpz_class>(primesCAN));
            auto ax_by_sum = validate<PairOfVec>(
                    MessageType::AX_BY_SHARES,
                    [](const PairOfVec& a, const PairOfVec& b) {
                    assert(a.first.size() == b.second.size());
                    assert(a.second.size() == b.first.size());

                    std::vector<mpz_class> rf(a.first.size());
                    std::vector<mpz_class> rs(a.second.size());

                    for(size_t i = 0; i < a.first.size(); ++i) {
                    rf[i] = a.first[i] + b.first[i];
                    rs[i] = a.second[i] + b.second[i];
                    }
                    return std::pair{rf, rs};
                    },
                    acc_pair
                    );

            {
                int k = 0;
                for (int j = 0; j < bucketSize; ++j) {
                    for (int i = 0; i < alphasCAN.size(); ++i) {
                        ax_by_sum.first[k] = ax_by_sum.first[k] % alphasCAN[i];
                        ax_by_sum.second[k] = ax_by_sum.second[k] % alphasCAN[i];
                        ++k;
                    }
                }
            }
            auto ab = validate<std::vector<mpz_class>>(
                    MessageType::AXB_MINUS_BYA_SHARES,
                    [](const std::vector<mpz_class>& a, const std::vector<mpz_class>& b) {
                    assert(a.size() == b.size());

                    std::vector<mpz_class> result(a.size());

                    for(size_t i = 0; i < a.size(); ++i) {
                    result[i] = a[i] + b[i];
                    }
                    return result;
                    },
                    std::vector<mpz_class>(primesCAN)
                    );

            {
                int k = 0;
                for (int j = 0; j < bucketSize; ++j) {
                    for (int i = 0; i < alphasCAN.size(); ++i) {
                        ab[k] = math::mod(ab[k] + c_can[k] - ax_by_sum.first[k] * ax_by_sum.second[k], alphasCAN[i]);
                        ++k;
                    }
                }
            }

            // Validate MODULUS_CANDIDATES
            {

                std::vector<mpz_class> candidates_raw(bucketSize);

                int k = 0;
                auto pq_size = bucketSize;
                if (pq_size > eit_prunedPS.size()) {
                    pq_size = eit_prunedPS.size();
                }
                for (int i = 0; i < pq_size; ++i) {

                    std::vector<mpz_class> x(alphasCAN.size() + alphasPS_tick.size());

                    //DBG("copy from can");
                    for (int zz = 0; zz < alphasCAN.size(); ++zz) {
                        x[zz] = mpz_class(ab[i * alphasCAN.size() + zz]);
                    }
                    //DBG("copy from ps");
                    // copy from ps
                    for (int zz = 0; zz < alphasPS_tick.size(); ++zz) {
                        x[zz + alphasCAN.size()] = mpz_class(eit_prunedPS[i][zz]);
                    }

                    //DBG("before crt_reconstruct");
                    std::vector<mpz_class> coefs;
                    candidates_raw[k] = math::crt_reconstruct(x, coefs, alphas_combined);
                    for(int zz=1; zz < 127; ++zz)
                    {
                        mpz_t result;
                        mpz_init(result);
                        mpz_class prime = boost::math::prime(zz);
                        mpz_gcd(result, candidates_raw[k].get_mpz_t(),prime.get_mpz_t());
                        if(mpz_cmp_ui(result,1) != 0)
                        {
                            LOG(INFO) << "Candidate[" << k << "] = " << candidates_raw[k] << " is divisible by " << boost::math::prime(zz);
                            assert(false);
                        }
                        mpz_clear(result);
                    }
                    //DBG("after crt_reconstruct");
                    k++;
                }

                DBG("k = " << k);

                // Compute pq_size
                std::vector<mpz_class> candidates(k);
                for (int i = 0; i < k; ++i) {
                    candidates[i] = candidates_raw[i];
                }

                // Now compare with the expected one from file
                std::vector<mpz_class> expectedModulusCandidates;
                MessageType expectedModulusCandidatesType;
                ia >> expectedModulusCandidatesType;

                if (expectedModulusCandidatesType != MessageType::MODULUS_CANDIDATE) {
                    std::cout << "Unexpected MessageType " << int(expectedModulusCandidatesType) << std::endl;
                    std::cout << "Expected: " << msgs[1 + int(MessageType::MODULUS_CANDIDATE) - int(MessageType::ID_PARTY)] << std::endl;
                    std::cout << "Got: " << msgs[1 + int(expectedModulusCandidatesType) - int(MessageType::ID_PARTY)];
                    assert(false);
                }
                ia >> expectedModulusCandidates;

                if (expectedModulusCandidates.size() != candidates.size()) {
                    std::cout << "Size does not match" << std::endl;
                    std::cout << "Expected candidates size: " << expectedModulusCandidates.size() << std::endl;
                    std::cout << "Computed candidates size: " << candidates.size();
                    assert(false);
                }
                candidatesCAN = candidates;
            }

            // validate sync step
            validate<int>(
                    MessageType::SYNCHRONIZE_NOW,
                    [](const int& a, const int& b) {
                    return a+b;
                    },
                    0);

            std::cout << "Candidate generation: [SUCCESS]" << std::endl;
        }

        // Validate PostSieve
        {
            // read post sieve flags
            boost::dynamic_bitset<> postSieveFlags;
            MessageType postSieveType;

            ia >> postSieveType;
            if (postSieveType != MessageType::POST_SIEVE) {
                std::cout << "Unexpected MessageType " << int(postSieveType) << std::endl;
                std::cout << "Expected: " << msgs[1 + int(MessageType::POST_SIEVE) - int(MessageType::ID_PARTY)] << std::endl;
                std::cout << "Got: " << msgs[1 + int(postSieveType) - int(MessageType::ID_PARTY)];
                assert(false);
            }

            ia >> postSieveFlags;

            auto [Bs, Ms] = ligero::math::compute_m_b_vec(4096, 104729, 2);
            auto nb_threads = 90;
            auto postSieve = std::bind(ligero::math::test_factorizable_threaded, std::placeholders::_1, Bs, Ms, nb_threads);

            std::vector<mpz_class> nonBiPrimes;
            for (size_t i = 0; i < candidatesCAN.size(); i++) {
                if (postSieveFlags[i] == 1) {
                    nonBiPrimes.push_back(candidatesCAN[i]);
                }
            }

            auto discardFlagsToValidate = postSieve(std::vector<mpz_class>(nonBiPrimes.data(), nonBiPrimes.data() + nonBiPrimes.size()));

            // Check that all nonBiPrimes were eliminated
            if (discardFlagsToValidate.count() != nonBiPrimes.size()) {
                std::cout << "Post sieve validation failed" << std::endl;
                std::cout << "Total amount of non-biprimes which were not eliminated is: " << nonBiPrimes.size() - discardFlagsToValidate.count() << std::endl;
                std::cout << "The following non-biprimes were not eliminated:" << std::endl;
                for (size_t i = 0; i < nonBiPrimes.size(); i++) {
                    if (discardFlagsToValidate[i] == 0) {
                        std::cout << "index: " << i << " = " <<  nonBiPrimes[i] << std::endl;
                        assert(false);
                    }
                }
            }
            candidatesPostSieve = discardCandidates(candidatesCAN, postSieveFlags);
            std::cout << currentStep++ << ". " << "Post-Sieve: [SUCCESS]" << std::endl;
        }

        // Validate jacobi
        {
            auto candidates = candidatesPostSieve;
            validate<mpz_class>(
                    MessageType::GAMMA_RANDOM_SEED_SHARES,
                    [](const mpz_class& a,
                        const mpz_class& b) { return a ^ b; },
                    mpz_class(0)
                    );

            auto ggs = validate<std::vector<mpz_class>>(
                    MessageType::EXPONENTIATED_GAMMA_VALUE,
                    [](const std::vector<mpz_class>& a, const std::vector<mpz_class>& b) {

                    if (a.size() == 0) return b;

                    std::vector<mpz_class> c(b.size());
                    for (size_t i = 0; i < c.size(); ++i) {
                    c[i] = a[i] * b[i];
                    }
                    return c;
                    },
                    std::vector<mpz_class>()
                    );

            boost::dynamic_bitset<> discard (candidates.size());

            int discarded = 0;
            //DBG("my candidates = " << candidates);
            for (int i = 0; i < candidates.size(); ++i) {
                const mpz_class& N = candidates[i];
                mpz_class& gg = ggs[i];
                mpz_class x = gg % N;

                // Elimination
                if (x != 1 && x != (N - 1)) {
                    discard[i] = 1;
                    discarded++;
                }
            }

            boost::dynamic_bitset<> discardFlagsJacobi;
            MessageType discardFlagsJacobiType;
            ia >> discardFlagsJacobiType;
            if (discardFlagsJacobiType != MessageType::DISCARD_FLAGS) {
                std::cout << "Unexpected MessageType " << int(discardFlagsJacobiType) << std::endl;
                std::cout << "Expected: " << msgs[1 + int(MessageType::DISCARD_FLAGS) - int(MessageType::ID_PARTY)] << std::endl;
                std::cout << "Got: " << msgs[1 + int(discardFlagsJacobiType) - int(MessageType::ID_PARTY)];
                assert(false);
            }
            ia >> discardFlagsJacobi;

            for (size_t i = 0; i < candidates.size(); i++) {
                if (discardFlagsJacobi[i] != discard[i]) {
                    std::cout << "Discard flags for jacobi do not match the computed ones" << std::endl;
                    std::cout << "Computed[" << i << "] = " << discard[i] << std::endl;
                    std::cout << "Expected[" << i << "] = " << discardFlagsJacobi[i] << std::endl;
                }
            }

            candidatesJacobi = discardCandidates(candidates, discardFlagsJacobi);
            std::cout << currentStep++ << ". " << "Jacobi flags validation passed" << std::endl;
            std::cout << "Jacobi: [SUCCESS]" << std::endl;
        }

        // Validate GCD and Jacobi
        {
            std::vector<mpz_class> ggsGCD;
            const int bucketSize = lrint(floor(double(primesGCD) / double(alphasGCD.size())));
            auto candidates = candidatesJacobi;
            candidates.resize(1);

            auto acc_pair = std::pair{std::pair{std::vector<mpz_class>(primesGCD), std::vector<mpz_class>(primesGCD)}, mpz_class(0)};

            auto result_value = validate<PairOfVecGCD>
                (
                 MessageType::GCD_AX_BY_SHARES,
                 [](const PairOfVecGCD& a, const PairOfVecGCD& b) {
                 auto [pair_a, at] = a;
                 auto [pair_b, bt] = b;
                 auto [af, as] = pair_a;
                 auto [bf, bs] = pair_b;
                 assert(af.size() == bf.size());
                 assert(as.size() == bs.size());

                 std::vector<mpz_class> rf(af.size());
                 std::vector<mpz_class> rs(as.size());

                 for(size_t i = 0; i < af.size(); ++i) {
                 rf[i] = af[i] + bf[i];
                 rs[i] = as[i] + bs[i];
                 }

                 mpz_class rt = at ^ bt;

                 return std::pair{std::pair{rf, rs}, rt};
                 },
                 acc_pair
                     );

            auto [pair_axby, gammaSeed] = result_value;

            auto [ax_sum, by_sum] = pair_axby;

            {
                int k = 0;
                for (int j = 0; j < bucketSize; ++j) {
                    for (int i = 0; i < alphasGCD.size(); ++i) {
                        ax_sum[k] = ax_sum[k] % alphasGCD[i];
                        by_sum[k] = by_sum[k] % alphasGCD[i];
                        ++k;
                    }
                }
            }
            auto zcrt_value = validate<std::pair<std::vector<mpz_class>, std::vector<mpz_class>>>(
                    MessageType::AXB_MINUS_BYA_SHARES,
                    [](const std::pair<std::vector<mpz_class>, std::vector<mpz_class>>& pa,
                        const std::pair<std::vector<mpz_class>, std::vector<mpz_class>>& pb) {
                    auto [a, expGa] = pa;
                    auto [b, expGb] = pb;

                    if (a.size() == 0) return pb;

                    assert(expGa.size() == expGb.size());

                    std::vector<mpz_class> expResult(expGb.size());
                    for (size_t i = 0; i < expResult.size(); ++i) {
                    expResult[i] = expGa[i] * expGb[i];
                    }

                    assert(a.size() == b.size());

                    std::vector<mpz_class> result(a.size());

                    for(size_t i = 0; i < a.size(); ++i) {
                        result[i] = a[i] + b[i];
                    }
                    return std::pair{result, expResult};
                    },
                        std::pair{std::vector<mpz_class>(), std::vector<mpz_class>()}
            );

            std::vector<mpz_class> zCRTs;

            std::tie(zCRTs, ggsGCD) = zcrt_value;

            {
                int k = 0;
                for (int j = 0; j < bucketSize; ++j) {
                    for (int i = 0; i < alphasGCD.size(); ++i) {
                        zCRTs[k] = math::mod(zCRTs[k] + c_gcd[k] - ax_sum[k] * by_sum[k], alphasGCD[i]);
                        ++k;
                    }
                }
            }
            // reconstruct z from CRT representation
            std::vector<mpz_class> zs(candidates.size());
            int zc = 0;
            for (int i = 0; i < candidates.size(); ++i) {

                std::vector<mpz_class> x(alphasGCD.size());
                for (int zz = 0; zz < alphasGCD.size(); ++zz) {
                    x[zz] = zCRTs[zc];
                    ++zc;
                }

                std::vector<mpz_class> coefs;
                zs[i] = math::crt_reconstruct(x, coefs, alphasGCD);
            }

            // GCD: compute discard vector
            boost::dynamic_bitset<> discardGCD (zs.size());
            {
                {
                    mpz_t one;
                    mpz_init(one);
                    mpz_set_ui(one, 1);

                    mpz_t gcdResult;
                    mpz_init(gcdResult);
                    for (int i = 0; i < zs.size(); ++i) {
                        DBG("i = " << i);
                        const mpz_class& N = candidates[i];
                        DBG("N = " << N);

                        const mpz_class z = zs[i] % N;

                        DBG("z = " << z);
                        DBG("before mpz_gcd");
                        mpz_gcd(gcdResult, N.get_mpz_t(), z.get_mpz_t());

                        if (mpz_cmp(gcdResult, one) != 0) {
                            discardGCD[i] = 1;
                        }

                    }
                    mpz_clear(gcdResult);
                    mpz_clear(one);
                }
            }

            assert(zs.size() == candidates.size());

            // Jacobi: compute discard vector
            boost::dynamic_bitset<> discardJacobi (candidates.size());
            {

                //DBG("my candidates = " << candidates);
                DBG("Taking gamma values modulo N");
                for (int i = 0; i < candidates.size(); ++i) {
                    for (int j = 0; j < config.lambda(); ++j) {
                        const mpz_class& N = candidates[i];
                        mpz_class& gg = ggsGCD[i * config.lambda() + j];
                        mpz_class x = gg % N;

                        DBG("x = " << x);
                        DBG("N = " << N);
                        DBG("ggs[" << i << "] =" << ggsGCD[i * config.lambda() + j]);

                        // Elimination
                        if (x != 1 && x != (N - 1)) {
                            discardJacobi[i] = 1;
                        }
                    }
                }
            }

            // Final step: merge discardGCD and discardJacobi
            boost::dynamic_bitset<> discard (candidates.size());
            DBG("hostGCDandJacobiTest candidates.size() = " << candidates.size());
            for (int i = 0; i < candidates.size(); ++i) {
                DBG("discardGCD[" << i << "] = " << discardGCD[i]);
                DBG("discardJacobi[" << i << "] = " << discardJacobi[i]);
                discard[i] = discardGCD[i] | discardJacobi[i];
            }

            boost::dynamic_bitset<> discardFlagsGCD;
            MessageType discardFlagsGCDType;
            ia >> discardFlagsGCDType;

            if (discardFlagsGCDType != MessageType::DISCARD_FLAGS) {
                std::cout << "Unexpected MessageType " << int(discardFlagsGCDType) << std::endl;
                std::cout << "Expected: " << msgs[1 + int(MessageType::DISCARD_FLAGS) - int(MessageType::ID_PARTY)] << std::endl;
                std::cout << "Got: " << msgs[1 + int(discardFlagsGCDType) - int(MessageType::ID_PARTY)];
                assert(false);
            }

            ia >> discardFlagsGCD;

            for (size_t i = 0; i < candidates.size(); i++) {
                if (discardFlagsGCD[i] != discard[i]) {
                    std::cout << "Discard flags for GCD and jacobi do not match the computed ones" << std::endl;
                    std::cout << "Computed[" << i << "] = " << discard[i] << std::endl;
                    std::cout << "Expected[" << i << "] = " << discardFlagsGCD[i] << std::endl;
                }
            }

            std::cout << currentStep++ << ". " << "GCD Jacobi flags validation passed" << std::endl;
            std::cout << "GCD and Jacobi: [SUCCESS]" << std::endl;
        }

        std::cout << std::endl << ">>>>  VALIDATION PASSED <<<<" << std::endl;
        /*
           while (ifs && ifs.peek() != EOF) {
           MessageType t;
           ia >> t;
           int msgId = int(t);
           std::cout << "t: " << msgId << std::endl;
           if (msgId > 0) msgId = msgId - int(MessageType::ID_PARTY);
           std::cout << "header: " << msgs[msgId] << std::endl;

           }
           */
    }
    /*
       } catch (...) {
       std::cout << "caught exception" << std::endl;
       ifs.close();
       }
       */

    ifs.close();
    return 0;
}


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.
#include "Constraint.hpp"
#include "Scheduler.hpp"
#include <memory>
#define BOOST_TEST_MODULE Less
#include <boost/test/unit_test.hpp>

using namespace popsolver;

BOOST_AUTO_TEST_CASE(PropagateNoChange) {
  Variable a(0), b(1);
  auto less = std::unique_ptr<Less>(new Less(a, b));
  Domains domains;
  domains.push_back({DataType{1}, DataType{5}}); // a
  domains.push_back({DataType{7}, DataType{8}}); // b
  Scheduler scheduler(domains, {less.get()});
  bool success = less->propagate(scheduler);
  BOOST_CHECK(success);
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{1});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{7});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{8});
}

BOOST_AUTO_TEST_CASE(PropagateChangeA) {
  Variable a(0), b(1);
  auto less = std::unique_ptr<Less>(new Less(a, b));
  Domains domains;
  domains.push_back({DataType{5}, DataType{10}}); // a
  domains.push_back({DataType{7}, DataType{8}});  // b
  Scheduler scheduler(domains, {less.get()});
  bool success = less->propagate(scheduler);
  BOOST_CHECK(success);
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{7});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{7});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{8});
}

BOOST_AUTO_TEST_CASE(PropagateChangeB) {
  Variable a(0), b(1);
  auto less = std::unique_ptr<Less>(new Less(a, b));
  Domains domains;
  domains.push_back({DataType{5}, DataType{10}}); // a
  domains.push_back({DataType{1}, DataType{11}}); // b
  Scheduler scheduler(domains, {less.get()});
  bool success = less->propagate(scheduler);
  BOOST_CHECK(success);
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{10});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{6});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{11});
}

BOOST_AUTO_TEST_CASE(PropagateChangeBoth) {
  Variable a(0), b(1);
  auto less = std::unique_ptr<Less>(new Less(a, b));
  Domains domains;
  domains.push_back({DataType{5}, DataType{10}}); // a
  domains.push_back({DataType{1}, DataType{8}});  // b
  Scheduler scheduler(domains, {less.get()});
  bool success = less->propagate(scheduler);
  BOOST_CHECK(success);
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{5});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{7});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{6});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{8});
}

BOOST_AUTO_TEST_CASE(PropagateFail) {
  Variable a(0), b(1);
  Less c(a, b);
  Domains domains;
  domains.push_back({DataType{4}, DataType{6}});
  domains.push_back({DataType{1}, DataType{4}});
  Scheduler scheduler(domains, {&c});
  bool success = c.propagate(scheduler);
  BOOST_CHECK(!success);
}


// Copyright 2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
// Original Author: Ryan Pavlik
//
// IMU test application for the latency tester hardware,
// based on the original design for the OSVR latency tester.

#ifdef APP_IMUTEST
// Must come before Arduino because of abs
#include <Eigen/Core>
#include "apps.h"

using Eigen::Vector3f;

#include <Arduino.h>
#include "defines.h"

#include "gyroProc.h"

bool ledState = false;
unsigned long lastTimestamp = 0;

GyroProc gyroProc;

void imutestLoop(Board& board)
{
    unsigned long timestamp;
    sensors_event_t g;
    if (!board.getGyroData(&timestamp, &g))
    {
        Serial.println("Read failed!?");
        delay(10);
        return;
    }
    bool dataGood = false;
    Vector3f processed;
    std::tie(dataGood, processed) = gyroProc.process(g);
    if (dataGood)
    {

        Serial.print("Gyro X: ");
        Serial.print(processed.x());
        Serial.print(" rad/s");
        Serial.print("\tY: ");
        Serial.print(processed.y());
        Serial.print(" rad/s");
        Serial.print("\tZ: ");
        Serial.print(processed.z());
        Serial.println(" rad/s");

        Serial.println();
        Serial.print("Elapsed time since last reading: ");
        auto elapsed = timestamp - lastTimestamp;
        Serial.print(elapsed);
        Serial.print("us (");
        Serial.print(std::floor(1000000.0f / elapsed));
        Serial.println("Hz)");
    }
    lastTimestamp = timestamp;
    ledState = !ledState;
    digitalWrite(LED_BUILTIN, ledState);
}
#endif


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, Robert Bosch GmbH
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Robert Bosch GmbH nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Leonard Bruns */

#include "ompl/base/samplers/DeterministicStateSampler.h"
#include "ompl/base/samplers/deterministic/HaltonSequence.h"
#include "ompl/base/spaces/SE2StateSpace.h"
#include "ompl/base/spaces/SO2StateSpace.h"
#include "ompl/base/spaces/RealVectorStateSpace.h"

#include <iostream>
#include <boost/math/constants/constants.hpp>

namespace ompl
{
    namespace base
    {
        DeterministicStateSampler::DeterministicStateSampler(const StateSpace *space, DeterministicSamplerType type)
          : StateSampler(space)
        {
            switch (type)
            {
                case HALTON:
                    sequence_ptr_ = std::make_shared<HaltonSequence>(space->getDimension());
                    break;
                default:
                    OMPL_WARN("Unknown deterministic sampler type specified, using Halton instead.");

                    break;
            }
        }

        DeterministicStateSampler::DeterministicStateSampler(const StateSpace *space,
                                                             std::shared_ptr<DeterministicSequence> sequence_ptr)
          : StateSampler(space), sequence_ptr_(sequence_ptr)
        {
        }

        void SO2DeterministicStateSampler::sampleUniform(State *state)
        {
            auto sample = sequence_ptr_->sample();
            state->as<SO2StateSpace::StateType>()->value =
                -boost::math::constants::pi<double>() + sample[0] * 2 * boost::math::constants::pi<double>();
        }

        void SO2DeterministicStateSampler::sampleUniformNear(State *, const State *, double)
        {
            OMPL_WARN("Deterministic sampler does not support near sampling.");
        }

        void SO2DeterministicStateSampler::sampleGaussian(State *, const State *, double)
        {
            OMPL_WARN("Deterministic sampler does not support Gaussian sampling.");
        }

        void RealVectorDeterministicStateSampler::sampleUniform(State *state)
        {
            auto sample = sequence_ptr_->sample();

            const unsigned int dim = space_->getDimension();

            const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace *>(space_)->getBounds();

            if (stretch_)
            {
                auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
                for (unsigned int i = 0; i < dim; ++i)
                    rstate->values[i] = bounds.low[i] + sample[i] * (bounds.high[i] - bounds.low[i]);
            }
            else
            {
                auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
                for (unsigned int i = 0; i < dim; ++i)
                    rstate->values[i] = sample[i];
            }
        }

        void RealVectorDeterministicStateSampler::sampleUniformNear(State *, const State *, double)
        {
            OMPL_WARN("Deterministic sampler does not support near sampling.");
        }

        void RealVectorDeterministicStateSampler::sampleGaussian(State *, const State *, double)
        {
            OMPL_WARN("Deterministic sampler does not support Gaussian sampling.");
        }

        void SE2DeterministicStateSampler::sampleUniform(State *state)
        {
            auto sample = sequence_ptr_->sample();

            const RealVectorBounds &bounds = static_cast<const SE2StateSpace *>(space_)->getBounds();

            auto se2_state_ptr = static_cast<SE2StateSpace::StateType *>(state);
            if (stretch_rv_)
            {
                se2_state_ptr->setX(bounds.low[0] + sample[0] * (bounds.high[0] - bounds.low[0]));
                se2_state_ptr->setY(bounds.low[1] + sample[1] * (bounds.high[1] - bounds.low[1]));
            }
            else
                se2_state_ptr->setXY(sample[0], sample[1]);

            if (stretch_so2_)
                se2_state_ptr->setYaw(-boost::math::constants::pi<double>() +
                                      sample[2] * 2 * boost::math::constants::pi<double>());
            else
                se2_state_ptr->setYaw(sample[2]);
        }

        void SE2DeterministicStateSampler::sampleUniformNear(State *, const State *, double)
        {
            OMPL_WARN("Deterministic sampler does not support near sampling.");
        }

        void SE2DeterministicStateSampler::sampleGaussian(State *, const State *, double)
        {
            OMPL_WARN("Deterministic sampler does not support Gaussian sampling.");
        }
    }  // namespace base
}  // namespace ompl