// It works if the --p2p option is given!??
#include <iostream>
#include <boost/program_options.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include <ndt_registration/ndt_matcher_d2d.h>
#include <ndt_registration/ndt_matcher_d2d_feature.h>
#include <ndt_map/ndt_map.h>
#include <ndt_map/cell_vector.h>
#include <ndt_map/lazy_grid.h>
#include <pointcloud_vrml/pointcloud_utils.h>

using namespace std;
using namespace perception_oru;
namespace po = boost::program_options;

int main(int argc, char** argv)
{
    cout << "--------------------------------------------------" << endl;
    cout << "Small test program of CellVector + F2F matcher " << endl;
    cout << "--------------------------------------------------" << endl;

    po::options_description desc("Allowed options");
    Eigen::Matrix<double,6,1> pose_increment_v;
    string static_file_name, moving_file_name;
    int nb_clusters, nb_points;
    double std_dev, min, max;
    desc.add_options()
    ("help", "produce help message")
    ("x", po::value<double>(&pose_increment_v(0))->default_value(1.), "x pos gt offset")
    ("y", po::value<double>(&pose_increment_v(1))->default_value(1.), "y pos gt offset")
    ("z", po::value<double>(&pose_increment_v(2))->default_value(1.), "z pos gt offset")
    ("X", po::value<double>(&pose_increment_v(3))->default_value(0.1), "x axis rot gt offset")
    ("Y", po::value<double>(&pose_increment_v(4))->default_value(0.1), "y axis rot gt offset")
    ("Z", po::value<double>(&pose_increment_v(5))->default_value(0.1), "z axis rot gt offset")
    ("nb_clusters", po::value<int>(&nb_clusters)->default_value(20), "number of clusters")
    ("nb_points", po::value<int>(&nb_points)->default_value(10), "number of points per clusters")
    ("std_dev", po::value<double>(&std_dev)->default_value(0.1), "standard deviation of the points drawn from a normal distribution (here independent on the axes")
    ("min", po::value<double>(&min)->default_value(-10), "minimum center point")
    ("max", po::value<double>(&max)->default_value(10), "maximum center point")
    ("lazzy", "use lazzygrid")
    ("p2preg", "calculate NDTMatchF2F using two pointclouds")
    ("irregular_grid", "use irregular grid in the p2p registration")
    ("nfeaturecorr", "feature_f2f should NOT be evaluated (NDTMatcherFeatureF2F)")
    ("singleres", "use single resolution in the 'p2p' matching")
    ("nf2f", "if f2f should NOT be evaluated")
    ("usegt", "if the ground truth should be used as initial estimate")
    ;

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

    if (vm.count("help"))
    {
        cout << desc << "\n";
        return 1;
    }
    bool use_lazzy = vm.count("lazzy");
    bool use_p2preg = vm.count("p2preg");
    bool use_irregular_grid = vm.count("irregular_grid");
    bool use_featurecorr = !vm.count("nfeaturecorr");
    bool use_singleres = vm.count("singleres");
    bool use_f2f = !vm.count("nf2f");
    bool usegt = vm.count("usegt");
    pcl::PointCloud<pcl::PointXYZ> static_pc, moving_pc, tmp_pc;
    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> gt_transform;

    // Generate the static point cloud + indices...
    boost::mt19937 rng;
    boost::uniform_real<> ud(min,max);
    boost::normal_distribution<> nd(0.0, std_dev);
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> > var_uni(rng, ud);
    std::vector<std::pair<int,int> > corresp;

    size_t index = 0;
    std::vector<std::vector<size_t> > all_indices;
    for (int i = 0; i < nb_clusters; i++)
    {
        std::vector<size_t> indices;
        double c_x = var_uni();
        double c_y = var_uni();
        double c_z = var_uni();
        for (int j = 0; j < nb_points; j++)
        {
            static_pc.push_back(pcl::PointXYZ(c_x+var_nor(), c_y+var_nor(), c_z+var_nor()));
            indices.push_back(index);
            index++;
        }
        all_indices.push_back(indices);
        corresp.push_back(std::pair<int,int>(i,nb_clusters-1-i)); // nb_clusters-1-i -> To check the da functions is working.
    }
    std::vector<std::vector<size_t> > all_indices_moving(all_indices.size());  // Reverse the correspondances (To check the da functions)
    std::reverse_copy(all_indices.begin(), all_indices.end(), all_indices_moving.begin());

    for (int i = 0; i < 1/*nb_clusters*/; i++)
    {
        tmp_pc.push_back(pcl::PointXYZ(i,i,i));
    }

    // Specify some offset...
    gt_transform = Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
                   Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
                   Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
                   Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;

    std::vector<double> resolutions;
    if (use_singleres)
        resolutions.push_back(1.);
    NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcher(use_irregular_grid,!use_singleres,resolutions);
    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T_f2f,T_p2p, T_feat;
    T_f2f.setIdentity();
    T_p2p.setIdentity();
    T_feat.setIdentity();
    if (usegt)
    {
        T_f2f = gt_transform;
        T_p2p = gt_transform;
        T_feat = gt_transform;
    }

    moving_pc = transformPointCloud(gt_transform, static_pc);

    if (use_p2preg)
    {
        matcher.match(moving_pc, static_pc, T_p2p);
    }
    {
        double current_resolution = 1;

        SpatialIndex<pcl::PointXYZ>* index = NULL;
        if (use_lazzy)
        {
            index = new LazyGrid<pcl::PointXYZ>(current_resolution);
        }
        else
        {
            index = new CellVector<pcl::PointXYZ>();
        }

        NDTMap<pcl::PointXYZ> ndt(index);
        if (!use_lazzy)
            ndt.loadPointCloud( static_pc, all_indices );
        else
            ndt.loadPointCloud (static_pc );
        ndt.computeNDTCells();

        NDTMap<pcl::PointXYZ> mov(index);
        if (!use_lazzy)
            mov.loadPointCloud( moving_pc, all_indices_moving );
        else
            mov.loadPointCloud( moving_pc );
        mov.computeNDTCells();

        if (use_f2f)
        {
            matcher.match( mov, ndt, T_f2f );
        }
        if (use_featurecorr)
        {
            NDTMatcherFeatureD2D<pcl::PointXYZ,pcl::PointXYZ> matcher_feat(corresp);
            matcher_feat.match( mov, ndt, T_feat );
        }
        delete index;
    }

    std::cout<<"GT translation "<<gt_transform.translation().transpose()
             <<" (norm) "<<gt_transform.translation().norm()<<std::endl;
    std::cout<<"GT rotation "<<gt_transform.rotation().eulerAngles(0,1,2).transpose()
             <<" (norm) "<<gt_transform.rotation().eulerAngles(0,1,2).norm()<<std::endl;

    if (use_f2f)
    {
        std::cout<<"f2f translation "<<T_f2f.translation().transpose()
                 <<" (norm) "<<T_f2f.translation().norm()<<std::endl;
        std::cout<<"f2f rotation "<<T_f2f.rotation().eulerAngles(0,1,2).transpose()
                 <<" (norm) "<<T_f2f.rotation().eulerAngles(0,1,2).norm()<<std::endl;
    }

    if (use_featurecorr)
    {
        std::cout<<"feat translation "<<T_feat.translation().transpose()
                 <<" (norm) "<<T_feat.translation().norm()<<std::endl;
        std::cout<<"feat rotation "<<T_feat.rotation().eulerAngles(0,1,2).transpose()
                 <<" (norm) "<<T_feat.rotation().eulerAngles(0,1,2).norm()<<std::endl;
    }

    if (use_p2preg)
    {
        std::cout<<"p2preg translation "<<T_p2p.translation().transpose()
                 <<" (norm) "<<T_p2p.translation().norm()<<std::endl;
        std::cout<<"p2preg rotation "<<T_p2p.rotation().eulerAngles(0,1,2).transpose()
                 <<" (norm) "<<T_p2p.rotation().eulerAngles(0,1,2).norm()<<std::endl;
    }
    cout << "done." << endl;
}


#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);
}


/*
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 );
}


#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;
}


#include "Lib/spin_system.h"
#include "lib/log_trace.h"
#include "Lib/simulate.h"
#include "lib/grid.h"
#include "lib/constants.h"
#include "lib/P_lists.h"
#include "Lib/load_parameters.h"

#include <omp.h>

#include <iostream>
#include <filesystem>
#include <unsupported/Eigen/MatrixFunctions>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <vector>
#include <fstream>
#include <string>
#include <vector>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <boost/ref.hpp>
#include <math.h>

#include <chrono>

namespace fs = std::experimental::filesystem;

template<class P_list>
P_list create_8_ns_P_list(P_list const & P_list_2ns) {

	/*
	this function creates propagators that capture an 8ns step from propagators that capture a 2ns step
	*/

	//2ns
	P_list temp(P_list_2ns);
	//4ns
	temp.multiply(temp,true);
	//8ns
	temp.multiply(temp, true);

	return temp;
}

template<class P_list>
P_list repeat_P_list(P_list const & P_list_step,double t_end) {

	/*
	this function repeats the propagators in P_list_step upto a time step t_end is captured
	*/

	P_list temp(P_list_step);
	temp.reset();

	while (temp.t_step < t_end ) {
		temp.multiply(P_list_step, true);
	}

	return temp;
}

template<class P_list>
P_list reverse_P_list(P_list const & P_list_start, P_list const & P_list_step, double t_end) {

	/*
	this function calculates propagators that capture a time step of the length t_end by reverting the
	propgatros in P_list_start with the propagators in P_list_step
	*/

	P_list temp(P_list_start);

	while ( temp.t_step > t_end) {
		temp.multiply(P_list_step, false);

	}

	return temp;
}

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

	try {
		if (argc < 4)
		{
			throw std::runtime_error("Not enough arguments given");
		}
	}
	catch (std::runtime_error &er) {
		std::cout << er.what() << "\n";
	}

	double t_step;
	std::vector<double> taus;
	Spin_System_Doublet_Triplet spin_time_2;
	Spin_System_Doublet_Triplet spin_pulse_2;
	std::vector<double> t;

	Spin_System_Doublet spin_time;
	Spin_System_Doublet spin_pulse;

	auto temp = boost::make_tuple(boost::ref(spin_time), boost::ref(spin_pulse), boost::ref(t_step), boost::ref(taus), boost::ref(t));
	temp = load_parameters_doublet(argv[1]);

	auto temp2 = boost::make_tuple(boost::ref(spin_time_2), boost::ref(spin_pulse_2), boost::ref(t_step), boost::ref(taus), boost::ref(t));
	temp2 = load_parameters_Doublet_Triplet(argv[1]);

	double const t0 = t[0];
	double const t1 = t[1];
	double const t2 = t[2];
	double const t3 = t[3];
	double const t4 = t[4];
	double const t5 = t[5];
	double t_pi_2 = t[1];

	double t_tau = taus[1] - taus[0];

	Log_stream log_stream(argv[2], "integral", t_step);

	std::vector<double> phi_doublet;
	std::vector<double> theta_doublet;
	std::vector<double> weights_doublet;
	auto temp_doublet = boost::make_tuple(boost::ref(phi_doublet), boost::ref(theta_doublet), boost::ref(weights_doublet));
	temp_doublet = grid(argv[3],spin_time_2.symmetrie_doublet, spin_time_2.knots_doublet);

	std::vector<double> phi_triplet;
	std::vector<double> theta_triplet;
	std::vector<double> weights_triplet;
	auto temp_triplet = boost::make_tuple(boost::ref(phi_triplet), boost::ref(theta_triplet), boost::ref(weights_triplet));
	temp_triplet = grid(argv[3], spin_time_2.symmetrie_triplet, spin_time_2.knots_triplet);

	std::vector<double> phi_dipolar;
	std::vector<double> theta_dipolar;
	std::vector<double> weights_dipolar;
	auto temp_dipolar = boost::make_tuple(boost::ref(phi_dipolar), boost::ref(theta_dipolar), boost::ref(weights_dipolar));
	temp_dipolar = grid(argv[3], spin_time_2.symmetrie_dipolar, spin_time_2.knots_dipolar);

	//the propagators for the Doublet system (before Laser excitation) are simulated in advance, they are updated later on capture the shift of the laser flash

	P_list_one_rot<Spin_System_Doublet> P_list_time_2ns(spin_time, t_step, theta_doublet, phi_doublet); P_list_time_2ns.create_P_list();
	P_list_one_rot<Spin_System_Doublet> P_list_time_8ns(create_8_ns_P_list(P_list_time_2ns));
	P_list_one_rot<Spin_System_Doublet> P_list_time_fid1(repeat_P_list(P_list_time_8ns, t2 - t1));
	P_list_one_rot<Spin_System_Doublet> P_list_time_fid2(repeat_P_list(P_list_time_8ns, t4 - t3));

	P_list_one_rot<Spin_System_Doublet> P_list_pulse_2ns(spin_pulse, t_step, theta_doublet, phi_doublet); P_list_pulse_2ns.create_P_list();
	P_list_one_rot<Spin_System_Doublet> P_list_pulse_8ns(create_8_ns_P_list(P_list_pulse_2ns));
	P_list_one_rot<Spin_System_Doublet>  P_list_pulse_pi_2(repeat_P_list(P_list_pulse_2ns, t_pi_2));
	P_list_one_rot<Spin_System_Doublet>  P_list_pulse_pi(P_list_pulse_pi_2);  P_list_pulse_pi.multiply(P_list_pulse_pi, true);

	P_list_one_rot<Spin_System_Doublet> P_list_dummy(P_list_pulse_2ns);
	Spin_System_Doublet::operator_type rho_eq = spin_time.get_rho_equ();

	Spin_System_Doublet::operator_type rho_doublet = Spin_System_Doublet::operator_type::Zero();

	std::vector<std::vector<Spin_System_Doublet::operator_type>> rho_doublets;

	for (auto tau : taus) {

		std::vector<Spin_System_Doublet::operator_type > rho_doublet_angles;

		log_stream.save_x(tau);

		std::cout << tau*1e9 << std::endl;

		if (tau < t0) {  //laser before first pulse

		}
		else if (tau >= t0 && tau < t1) {  //laser during first pulse

			if ((tau - t0) < t_tau) {
				P_list_dummy.copy_P_list(repeat_P_list(P_list_pulse_2ns, tau - t0));
			}
			else {
				P_list_dummy.multiply(P_list_pulse_8ns, true);
			}

		}
		else if (tau >= t1 && tau < t2) { //laser during first fid

			if ((tau - t1) < t_tau) {
				P_list_dummy.copy_P_list(repeat_P_list(P_list_time_2ns, tau - t1));
			}
			else {
				P_list_dummy.multiply(P_list_time_8ns, true);
			}
		}
		else if (tau >= t2 && tau < t3) { //laser during refocussing pulse

			if ((tau - t2) < t_tau) {
				P_list_dummy.copy_P_list(repeat_P_list(P_list_pulse_2ns, tau - t2));
			}
			else {
				P_list_dummy.multiply(P_list_pulse_8ns, true);
			}

		}
		else if (tau >= t3 && tau < t4) { //laser during second fid(echo)

			if ((tau - t3) < t_tau) {
				P_list_dummy.copy_P_list(repeat_P_list(P_list_time_2ns, tau - t3));
			}
			else {
				P_list_dummy.multiply(P_list_time_8ns, true);
			}
		}
		else {}

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

			double w_doublet = weights_doublet[i];
			Spin_System_Doublet::operator_type rho;

			if (tau < t0) {  //laser before first pulse

				rho = rho_eq;

			}
			else if (tau >= t0 && tau < t1) {  //laser during first pulse
				rho = simulate_P(spin_pulse, P_list_dummy.P_list[i], t0, tau, w_doublet, rho_eq, Log_trace_none{});

			}
			else if (tau >= t1 && tau < t2) { //laser during first fid
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_dummy.P_list[i], t1, tau, w_doublet, rho, Log_trace_none{});

			}
			else if (tau >= t2 && tau < t3) { //laser during refocussing pulse
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid1.P_list[i], t1, t2, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_pulse, P_list_dummy.P_list[i], t2, tau, w_doublet, rho, Log_trace_none{});

			}
			else if (tau >= t3 && tau < t4) { //laser during second fid(echo)
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid1.P_list[i], t1, t2, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_pulse, P_list_pulse_pi.P_list[i], t2, t3, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_dummy.P_list[i], t3, tau, w_doublet, rho, Log_trace_none{});

			}
			else if (tau >= t4 && tau < t5) { //laser during echo-detection
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid1.P_list[i], t1, t2, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_pulse, P_list_pulse_pi.P_list[i], t2, t3, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid2.P_list[i], t3, t4, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_2ns.P_list[i], t4, tau, w_doublet, rho, log_stream);

			}
			else if (tau >= t5) { //laser after echo
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid1.P_list[i], t1, t2, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_pulse, P_list_pulse_pi.P_list[i], t2, t3, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid2.P_list[i], t3, t4, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_2ns.P_list[i], t4, t5, w_doublet, rho, log_stream);
			}
			else {
			}
			rho_doublet_angles.push_back(rho);
		}

		rho_doublets.push_back(rho_doublet_angles);
		log_stream.next();
	}

	log_stream.reset();

	Eigen::Matrix3cd rho_T;

	spin_time_2.create_rho_T_list(theta_triplet, phi_triplet);

	bool first_part=false;

	//the propagators for the Doublet_Triplet system (after Laser excitation) are simulated in advance, they are updated later on capture the shift of the laser flash

	Spin_System_Doublet_Triplet::operator_type rho = Spin_System_Doublet_Triplet::operator_type::Zero();

	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_time_2ns(spin_time_2, t_step, theta_doublet, phi_doublet, theta_triplet, phi_triplet, theta_dipolar, phi_dipolar); P_list_full_time_2ns.create_P_list();
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_time_8ns(create_8_ns_P_list(P_list_full_time_2ns));
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_time_fid1(repeat_P_list(P_list_full_time_8ns, t2 - t1));
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_time_fid2(repeat_P_list(P_list_full_time_8ns, t4 - t3));

	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_pulse_2ns(spin_pulse_2, t_step, theta_doublet, phi_doublet, theta_triplet, phi_triplet, theta_dipolar, phi_dipolar); P_list_full_pulse_2ns.create_P_list();
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_pulse_8ns(create_8_ns_P_list(P_list_full_pulse_2ns));
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_pulse_pi_2(repeat_P_list(P_list_full_pulse_2ns, t_pi_2));
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_pulse_pi(P_list_full_pulse_pi_2);  P_list_full_pulse_pi.multiply(P_list_full_pulse_pi, true);

	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_dummy(P_list_full_pulse_2ns);

	for (int q = 0; q < taus.size();++q) {
		double tau = taus[q];
		log_stream.save_x(tau);

		//show current tau on screen
		std::cout << tau*1e9 << "\n";

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

			rho_doublet = rho_doublets[q][i];

			for (int k = 0; k < phi_dipolar.size(); ++k) {

				for (int m = 0; m < phi_triplet.size(); ++m) {

					rho_T = spin_time_2.get_rho_T_list(m);

					double w= weights_doublet[i] * weights_dipolar[k] * weights_triplet[m];

					if (tau < t0) {  //laser before first pulse

						if (!first_part) {
							rho = Eigen::kroneckerProduct(rho_eq, rho_T);
							rho = simulate_P(spin_pulse_2, P_list_full_pulse_pi_2.P_list[i][m][k], t0, t1, w, rho, Log_trace_none{});
							rho = simulate_P(spin_time_2, P_list_full_time_fid1.P_list[i][m][k], t1, t2, w, rho, Log_trace_none{});
							rho = simulate_P(spin_pulse_2, P_list_full_pulse_pi.P_list[i][m][k], t2, t3, w, rho, Log_trace_none{});
							rho = simulate_P(spin_time_2, P_list_full_time_fid2.P_list[i][m][k], t3, t4, w, rho, Log_trace_none{});
							rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);

						}
						else {
							log_stream.copy_last_point();
							goto escape_point;
						}

					}
					else if (tau >= t0 && tau < t1) {  //laser during first pulse

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_pulse_2, P_list_full_dummy.P_list[i][m][k], tau, t1, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_fid1.P_list[i][m][k], t1, t2, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_pulse_2, P_list_full_pulse_pi.P_list[i][m][k], t2, t3, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_fid2.P_list[i][m][k], t3, t4, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);
					}
					else if (tau >= t1 && tau < t2) { //laser during first fid

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_time_2, P_list_full_dummy.P_list[i][m][k], tau, t2, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_pulse_2, P_list_full_pulse_pi.P_list[i][m][k], t2, t3, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_fid2.P_list[i][m][k], t3, t4, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);

					}
					else if (tau >= t2 && tau < t3) { //laser during refocussing pulse

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_pulse_2, P_list_full_dummy.P_list[i][m][k], tau, t3, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_fid2.P_list[i][m][k], t3, t4, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);

					}
					else if (tau >= t3 && tau < t4) { //laser during second fid(echo)

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_time_2, P_list_full_dummy.P_list[i][m][k], tau, t4, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);

					}
					else if (tau >= t4 && tau < t5) { //laser during echo-detection

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], tau, t5, w, rho, log_stream);
					}
					else if (tau >= t5) { //laser after echo

					}
					else {	}

				}//end T angle loop
			}//end dipolar angle loop

		}//end obs loop

		first_part = true;
		escape_point:;
		log_stream.next();

		if (tau < t0) {  //laser before first pulse
			if ((t0 - tau) < t_tau) {
				P_list_full_dummy.copy_P_list(reverse_P_list(P_list_full_pulse_pi_2, P_list_full_pulse_2ns, t1  - (tau + t_tau)));
			}
			else {

			}

		}
		else if (tau >= t0 && tau < t1) {  //laser during first pulse

			if ((t1 - tau) < t_tau) {
				P_list_full_dummy.copy_P_list(reverse_P_list(P_list_full_time_fid1, P_list_full_time_2ns, t2  - (tau + t_tau)));
			}
			else {
				P_list_full_dummy.multiply(P_list_full_pulse_8ns, false);
			}
		}
		else if (tau >= t1 && tau < t2) { //laser during first fid

			if ((t2 - tau) < t_tau) {
				P_list_full_dummy.copy_P_list(reverse_P_list(P_list_full_pulse_pi, P_list_full_pulse_2ns, t3 - (tau + t_tau)));
			}
			else {
				P_list_full_dummy.multiply(P_list_full_time_8ns, false);
			}

		}
		else if (tau >= t2 && tau < t3) { //laser during refocussing pulse

			if ((t3 - tau) < t_tau) {
				P_list_full_dummy.copy_P_list(reverse_P_list(P_list_full_time_fid2, P_list_full_time_2ns, t4  - (tau + t_tau)));
			}
			else {
				P_list_full_dummy.multiply(P_list_full_pulse_8ns, false);
			}

		}
		else if (tau >= t3 && tau < t4) { //laser during second fid(echo)

			if ((t4 - tau) < t_tau) {

			}
			else {
				P_list_full_dummy.multiply(P_list_full_time_8ns, false);
			}
		}
		else if (tau >= t4 && tau < t5) { //laser during echo-detection

		}
		else if (tau >= t5) { //laser after echo

		}
		else {}

	} //end tau loop
	log_stream.reset();

	log_stream.save_data();

}


//
// Created by Dmitri Bagaev on 2019-03-22.
//

#include "bayesian.h"

#define USE_NLOPT

#include <nlopt.hpp>

#include <boost/parameter/aux_/void.hpp>

#include <limbo/opt/nlopt_no_grad.hpp>
#include <Eigen/Core>
#include <limbo/kernel/exp.hpp>
#include <limbo/kernel/squared_exp_ard.hpp>
#include <limbo/mean/function_ard.hpp>
#include <limbo/model/gp.hpp>
#include <limbo/model/gp/kernel_lf_opt.hpp>
#include <limbo/tools.hpp>
#include <limbo/tools/macros.hpp>
#include <limbo/bayes_opt/bo_base.hpp>
#include <limbo/bayes_opt/boptimizer.hpp>
#include <limbo/model/gp/kernel_mean_lf_opt.hpp>
#include <limbo/model/multi_gp.hpp>
#include <limbo/acqui/gp_ucb.hpp>
#include <limbo/acqui/ei.hpp>

namespace INMOST {

    BayesianUniformDistribution::BayesianUniformDistribution() : distribution(0.0, 1.0) {
        int rank, size;

        MPI_Comm_rank(MPI_COMM_WORLD, &rank);
        MPI_Comm_size(MPI_COMM_WORLD, &size);

        unsigned int seed = static_cast<unsigned int>(time(NULL));

        if (rank == 0) {
            for (int i = 1; i < size; i++) {
                MPI_Send(&seed, 1, MPI_UNSIGNED, i, 0, MPI_COMM_WORLD);
            }
        } else {
            MPI_Status status;
            MPI_Recv(&seed, 1, MPI_UNSIGNED, 0, 0, MPI_COMM_WORLD, &status);
        }

        generator.seed(seed);
    }

    double BayesianUniformDistribution::next() {
        return distribution(generator);
    }

    unsigned int BayesianUniformDistribution::operator()(unsigned int n) {
        return static_cast<unsigned int>(next() * n);
    }

    unsigned int    BayesianOptimizer::DEFAULT_UNIQUE_POINTS_MAX_COUNT    = 7;
    unsigned int    BayesianOptimizer::DEFAULT_UNIQUE_POINTS_RANDOM_COUNT = 5;
    unsigned int    BayesianOptimizer::DEFAULT_INITIAL_ITERATIONS_COUNT   = 5;
    double          BayesianOptimizer::DEFAULT_INITIAL_ITERATIONS_RADIUS  = 0.2;
    double          BayesianOptimizer::DEFAULT_MAX_JUMP_BARRIER           = 0.1;

    BayesianOptimizer::BayesianOptimizer(const std::string &name, const OptimizationParameters &space, const OptimizerProperties &properties, std::size_t buffer_capacity) :
            OptimizerInterface(name, space, properties, buffer_capacity),
            unique_points_max_count(BayesianOptimizer::DEFAULT_UNIQUE_POINTS_MAX_COUNT),
            unique_points_random_count(BayesianOptimizer::DEFAULT_UNIQUE_POINTS_RANDOM_COUNT),
            initial_iterations_count(BayesianOptimizer::DEFAULT_INITIAL_ITERATIONS_COUNT),
            initial_iterations_radius(BayesianOptimizer::DEFAULT_INITIAL_ITERATIONS_RADIUS),
            max_jump_barrier(BayesianOptimizer::DEFAULT_MAX_JUMP_BARRIER) {

        if (this->HasProperty("unique_points_max_count")) {
            unique_points_max_count = static_cast<unsigned int>(std::atoi(this->GetProperty("unique_points_max_count").c_str()));
        }

        if (this->HasProperty("unique_points_random_count")) {
            unique_points_random_count = static_cast<unsigned int>(std::atoi(this->GetProperty("unique_points_random_count").c_str()));
        }

        if (this->HasProperty("initial_iterations_count")) {
            initial_iterations_count = static_cast<unsigned int>(std::atoi(this->GetProperty("initial_iterations_count").c_str()));
        }

        if (this->HasProperty("initial_iterations_radius")) {
            initial_iterations_radius = std::atof(this->GetProperty("initial_iterations_radius").c_str());
        }

        if (this->HasProperty("max_jump_barrier")) {
            max_jump_barrier = std::atof(this->GetProperty("max_jump_barrier").c_str());
        }

    }

    SuggestionChangedParameters BayesianOptimizer::AlgorithmMakeSuggestion() const {

        auto unique  = results.GetLastUniqueEntries(unique_points_max_count);
        auto entries = parameters.GetParameterEntries();

        std::vector<SuggestionChangedParameter> changed_parameters;

        if (unique.size() == 0) {

            std::size_t index = 0;
            std::for_each(entries.cbegin(), entries.cend(), [&index, &changed_parameters](const OptimizationParametersEntry &entry) {
                changed_parameters.emplace_back(SuggestionChangedParameter(index++, entry.first.GetDefaultValue()));
            });

            return changed_parameters;
        } else if (unique.size() < initial_iterations_count) {

            std::size_t index = 0;
            std::for_each(entries.cbegin(), entries.cend(), [&index, &changed_parameters, this, &unique](const OptimizationParametersEntry &entry) {
                auto parameter = entry.first;

                double min_bound = parameter.GetMinimalValue();
                double max_bound = parameter.GetMaximumValue();

                double r = random.next() * (max_bound - min_bound) * initial_iterations_radius;

                double next = entry.second + r * (2.0 * (unique.size() % 2) - 1.0);

                while (next < min_bound || next > max_bound) {
                    r    = random.next() * (max_bound - min_bound) * initial_iterations_radius;
                    next = entry.second + r * (2.0 * (unique.size() % 2) - 1.0);
                }

                changed_parameters.emplace_back(SuggestionChangedParameter(index++, next));
            });

            return changed_parameters;
        }

        std::random_shuffle(unique.begin() + 1, unique.end(), random);

        struct Params {
            struct kernel {
                // BO_PARAM(double, noise, 0.01); default
                // BO_PARAM(bool, optimize_noise, false); default
                BO_PARAM(double, noise, 0.01);

                BO_PARAM(bool, optimize_noise, false);
            };

            struct kernel_squared_exp_ard : public limbo::defaults::kernel_squared_exp_ard {
                // BO_PARAM(int, k, 0); default
                // BO_PARAM(double, sigma_sq, 1); default

                BO_PARAM(int, k, 4);

                BO_PARAM(double, sigma_sq, 0.2);
            };

            struct opt_rprop : public limbo::defaults::opt_rprop {
            };
            struct opt_nloptnograd : public limbo::defaults::opt_nloptnograd {
            };

            struct acqui_ucb {
                // BO_PARAM(double, alpha, 0.5); default

                BO_PARAM(double, alpha, 0.25);
            };

            struct acqui_gpucb : public limbo::defaults::acqui_gpucb {
            };
            struct acqui_ei : public limbo::defaults::acqui_ei {
            };
        };

        using Kernel2_t = limbo::kernel::SquaredExpARD<Params>;
        using Mean_t = limbo::mean::FunctionARD<Params, limbo::mean::Data<Params>>;
        using GP2_t = limbo::model::GP<Params, Kernel2_t, Mean_t, limbo::model::gp::KernelMeanLFOpt<Params>>;

        GP2_t gp_ard;

        std::vector<Eigen::VectorXd> samples;
        std::vector<Eigen::VectorXd> observations;

        std::for_each(unique.cbegin(), unique.cbegin() + std::min(static_cast<std::size_t>(unique_points_random_count), unique.size()), [this, &samples, &observations]
                (const OptimizationParameterResult &result) {
            Eigen::VectorXd sample(parameters.Size());

            int i = 0;
            std::for_each(result.GetPointsAfter().cbegin(), result.GetPointsAfter().cend(), [&i, &sample, this](const OptimizationParameterPoint &point) {
                auto parameter = parameters.GetParameter(static_cast<size_t>(i));

                double min_bound = parameter.GetMinimalValue();
                double max_bound = parameter.GetMaximumValue();

                double normalized = (parameter.ExtractValueFromPoint(point) - min_bound) / (max_bound - min_bound); // Normalize here to [0, 1]

                sample(i) = normalized;

                i += 1;
            });

            samples.push_back(sample);
            observations.push_back(limbo::tools::make_vector(-1.0 * result.GetMetricsAfter()));
        });

        double min_observation = (*std::min_element(observations.cbegin(), observations.cend(), [](const Eigen::VectorXd &l, const Eigen::VectorXd &r) {
            return l(0) < r(0);
        }))(0);

        double max_observation = (*std::max_element(observations.cbegin(), observations.cend(), [](const Eigen::VectorXd &l, const Eigen::VectorXd &r) {
            return l(0) < r(0);
        }))(0);

        std::transform(observations.cbegin(), observations.cend(), observations.begin(), [min_observation, max_observation](const Eigen::VectorXd &ob) {
            return limbo::tools::make_vector((ob(0) - min_observation) / ((10.0 / 6.0) * (max_observation - min_observation)) + 0.2); // Normalize here to [ 0.2, 0.8 ]
        });

        gp_ard.compute(samples, observations);
        gp_ard.optimize_hyperparams();

        using acquiopt_t = limbo::opt::NLOptNoGrad<Params, nlopt::GN_ISRES>;
        using acquisition_function_t = limbo::acqui::UCB<Params, GP2_t>;

        acquiopt_t             acquiopt;
        acquisition_function_t acqui(gp_ard, 0);

        auto afun               = limbo::FirstElem();
        auto acqui_optimization = [&](const Eigen::VectorXd &x, bool g) { return acqui(x, afun, g); };

        Eigen::VectorXd starting_point(parameters.Size());

        int i = 0;
        std::for_each(parameters.GetParameterEntries().cbegin(), parameters.GetParameterEntries().cend(), [&i, &starting_point](const OptimizationParametersEntry &entry) {

            auto parameter = entry.first;

            double min_bound = parameter.GetMinimalValue();
            double max_bound = parameter.GetMaximumValue();

            starting_point(i) = (entry.second - min_bound) / (max_bound - min_bound);

            i += 1;
        });

        Eigen::VectorXd new_sample = acquiopt(acqui_optimization, starting_point, true);

        for (int k = 0; k < parameters.Size(); ++k) {
            auto parameter = parameters.GetParameter(static_cast<size_t>(k));

            double min_bound = parameter.GetMinimalValue();
            double max_bound = parameter.GetMaximumValue();

            double barrier = max_jump_barrier;
            if (std::abs(new_sample(k) - starting_point(k)) > barrier) {
                new_sample(k) = starting_point(k) + barrier * (new_sample(k) - starting_point(k));
            }

            changed_parameters.emplace_back(SuggestionChangedParameter(k, (new_sample(k) * (max_bound - min_bound)) + min_bound));
        }

        return changed_parameters;
    }

    bool BayesianOptimizer::UpdateSpaceWithLatestResults() {
        const OptimizationParameterResult &last = results.at(0);

        if (last.IsGood() && (last.GetMetricsBefore() < 0.0 || (last.GetMetricsAfter() < last.GetMetricsBefore()))) {
            parameters.Update(last.GetChangedParameters(), last.GetMetricsAfter());
            return true;
        } else {
            return false;
        }
    }

    BayesianOptimizer::~BayesianOptimizer() {}

}