text:
#include <iostream>
#include <fstream>
#include <regex>
#include <math.h>
#include <immintrin.h>
#include <mpi.h>
#include "utilities.hpp"
#include "phenotype.hpp"
#include "dotp_lut.hpp"
#include "na_lut.hpp"
#include <boost/range/algorithm.hpp>
#include <boost/random/uniform_int.hpp>
#include <filesystem>

namespace fs = std::filesystem;

Phenotype::Phenotype(std::string fp, const Options& opt, const int N, const int M) :
    filepath(fp),
    N(N),
    M(M),
    im4(N%4 == 0 ? N/4 : N/4+1),
    K(opt.get_nmixtures()),
    G(opt.get_ngroups()) {

    mave = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
    check_malloc(mave, __LINE__, __FILE__);
    msig = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
    check_malloc(msig, __LINE__, __FILE__);
    epsilon_ = (double*) _mm_malloc(size_t(im4*4) * sizeof(double), 32);
    check_malloc(epsilon_, __LINE__, __FILE__);

    if (!opt.predict()) {
        cass = (int*) _mm_malloc(G * K * sizeof(int), 32);
        check_malloc(cass, __LINE__, __FILE__);

        betas.assign(M, 0.0);
        acum.assign(M, 0.0);
        muk.assign(K, 0.0);
        denom.resize(K - 1);
        logl.resize(K);
        comp.assign(M, 0);
        beta_sqn.resize(G);
        m0.resize(G);
        sigmag.resize(G);

        dirich.clear();
        for (int i=0; i<K; i++) dirich.push_back(1.0);
    } else {
        set_prediction_filenames(opt.get_out_dir());
    }

    set_output_filenames(opt.get_out_dir());
    read_file(opt);
}

//copy ctor
Phenotype::Phenotype(const Phenotype& rhs) :
    dist_m(rhs.dist_m),
    dist_d(rhs.dist_d),
    filepath(rhs.filepath),
    inbet_fp(rhs.inbet_fp),
    outbet_fp(rhs.outbet_fp),
    outcpn_fp(rhs.outcpn_fp),
    outcsv_fp(rhs.outcsv_fp),
    outmlma_fp(rhs.outmlma_fp),
    nonas(rhs.nonas),
    nas(rhs.nas),
    im4(rhs.im4),
    N(rhs.N),
    M(rhs.M),
    G(rhs.G),
    K(rhs.K),
    betas(rhs.betas),
    data(rhs.data),
    mask4(rhs.mask4),
    midx(rhs.midx),
    denom(rhs.denom),
    muk(rhs.muk),
    logl(rhs.logl),
    acum(rhs.acum),
    comp(rhs.comp),
    beta_sqn(rhs.beta_sqn),
    m0(rhs.m0),
    pi_est(rhs.pi_est),
    dirich(rhs.dirich),
    epssum(rhs.epssum),
    sigmae_(rhs.sigmae_),
    sigmag(rhs.sigmag),
    mu(rhs.mu) {
    epsilon_ = (double*) _mm_malloc(size_t(im4*4) * sizeof(double), 32);
    check_malloc(epsilon_, __LINE__, __FILE__);
    for (int i=0; i<im4*4; i++)
        epsilon_[i] = rhs.epsilon_[i];
    mave = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
    check_malloc(mave, __LINE__, __FILE__);
    msig = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
    check_malloc(msig, __LINE__, __FILE__);
    for (int i=0; i<M; i++) {
        mave[i] = rhs.mave[i];
        msig[i] = rhs.msig[i];
    }
    cass = (int*) _mm_malloc(size_t(K * G) * sizeof(int), 32);
    check_malloc(cass, __LINE__, __FILE__);
}

int Phenotype::get_m0_sum() {
    int sum = 0;
    for (int i=0; i<m0.size(); i++)
        sum += m0.at(i);
    return sum;
}

// Set input filenames based on input phen file (as per output)
void Phenotype::set_prediction_filenames(const std::string out_dir) {
    fs::path pphen = filepath;
    fs::path base  = out_dir;
    base /= pphen.stem();
    fs::path pibet = base;
    pibet.replace_extension(".bet");
    inbet_fp = pibet.string();
    //std::cout << "inbet_fp = " << inbet_fp << std::endl;

    fs::path pmlma = base;
    pmlma += ".mlma";
    outmlma_fp = pmlma.string();
}

void Phenotype::set_output_filenames(const std::string out_dir) {
    fs::path pphen = filepath;
    fs::path base  = out_dir;
    base /= pphen.stem();
    fs::path pbet = base;
    pbet += ".bet";
    fs::path pcpn = base;
    pcpn += ".cpn";
    fs::path pcsv = base;
    pcsv += ".csv";

    outbet_fp = pbet.string();
    outcpn_fp = pcpn.string();
    outcsv_fp = pcsv.string();
}

// Input and output
void Phenotype::open_prediction_files() {

    check_mpi(MPI_File_open(MPI_COMM_WORLD,
                            get_inbet_fp().c_str(),
                            MPI_MODE_RDONLY,
                            MPI_INFO_NULL,
                            get_inbet_fh()),
              __LINE__, __FILE__);

    check_mpi(MPI_File_open(MPI_COMM_WORLD,
                            get_outmlma_fp().c_str(),
                            MPI_MODE_CREATE | MPI_MODE_WRONLY | MPI_MODE_EXCL,
                            MPI_INFO_NULL,
                            get_outmlma_fh()),
              __LINE__, __FILE__);
}

void Phenotype::close_prediction_files() {
    check_mpi(MPI_File_close(get_inbet_fh()), __LINE__, __FILE__);
    check_mpi(MPI_File_close(get_outmlma_fh()), __LINE__, __FILE__);
}

void Phenotype::delete_output_prediction_files() {
    MPI_File_delete(get_outmlma_fp().c_str(), MPI_INFO_NULL);
}

void Phenotype::open_output_files() {
    check_mpi(MPI_File_open(MPI_COMM_WORLD,
                            get_outbet_fp().c_str(),
                            MPI_MODE_CREATE | MPI_MODE_WRONLY | MPI_MODE_EXCL,
                            MPI_INFO_NULL,
                            get_outbet_fh()),
              __LINE__, __FILE__);
    check_mpi(MPI_File_open(MPI_COMM_WORLD,
                            get_outcpn_fp().c_str(),
                            MPI_MODE_CREATE | MPI_MODE_WRONLY | MPI_MODE_EXCL,
                            MPI_INFO_NULL,
                            get_outcpn_fh()),
              __LINE__, __FILE__);
    check_mpi(MPI_File_open(MPI_COMM_WORLD,
                            get_outcsv_fp().c_str(),
                            MPI_MODE_CREATE | MPI_MODE_WRONLY | MPI_MODE_EXCL,
                            MPI_INFO_NULL,
                            get_outcsv_fh()),
              __LINE__, __FILE__);
}

void Phenotype::close_output_files() {
    check_mpi(MPI_File_close(get_outbet_fh()), __LINE__, __FILE__);
    check_mpi(MPI_File_close(get_outcpn_fh()), __LINE__, __FILE__);
    check_mpi(MPI_File_close(get_outcsv_fh()), __LINE__, __FILE__);
}

void Phenotype::delete_output_files() {
    MPI_File_delete(get_outbet_fp().c_str(), MPI_INFO_NULL);
    MPI_File_delete(get_outcpn_fp().c_str(), MPI_INFO_NULL);
    MPI_File_delete(get_outcsv_fp().c_str(), MPI_INFO_NULL);
}

void Phenotype::print_cass() {
    printf("INFO   : cass for phenotype [...]\n");
    for (int i=0; i<G; i++) {
        printf("         %2d : ", i);
        for (int j=0; j<K; j++) {
            printf("%7d", cass[i*K+j]);
        }
        printf("\n");
    }
}

void Phenotype::print_cass(const std::vector<int>& mtotgrp) {
    printf("INFO   : cass for phenotype [...]\n");
    for (int i=0; i<G; i++) {
        printf("         group %2d: %7d | cass: ", i, mtotgrp.at(i));
        for (int j=0; j<K; j++) {
            printf("%7d", cass[i*K + j]);
        }
        printf("\n");
    }
}

void Phenotype::update_pi_est_dirichlet(const int g) {
    std::vector<double> tmp;
    double sum = 0.0;
    for (int i=0; i<K; i++) {
        double val = dist_d.rgamma((double)cass[g * K + i] + dirich[i], 1.0);
        set_pi_est(g, i, val);
        sum += val;
    }
    for (int i=0; i<K; i++)
        set_pi_est(g, i, get_pi_est(g, i) / sum);
}

double Phenotype::epsilon_sum() {
    double* epsilon = get_epsilon();
    double sum = 0.0;
#ifdef _OPENMP
#pragma omp parallel for simd aligned(epsilon:32) reduction(+:sum)
#endif
    for (int i=0; i<N; i++) {
        sum += epsilon[i];
    }
    return sum;
}

double Phenotype::epsilon_sumsqr() {
    double* epsilon = get_epsilon();
    double sumsqr = 0.0;
#ifdef _OPENMP
#pragma omp parallel for simd aligned(epsilon:32) reduction(+:sumsqr)
#endif
    for (int i=0; i<N; i++) {
        sumsqr += epsilon[i] * epsilon[i];
    }
    return sumsqr;
}

void Phenotype::increment_beta_sqn(const int group, const double val) {
    beta_sqn.at(group) += val;
}

int Phenotype::get_marker_local_index(const int shuff_idx) {
    return midx[shuff_idx];
}

double Phenotype::sample_inv_scaled_chisq_rng(const double a, const double b) {
    return dist_d.inv_scaled_chisq_rng(a, b);
}

double Phenotype::sample_norm_rng(const double a, const double b) {
    return dist_d.norm_rng(a, b);
}

double Phenotype::sample_norm_rng() {
    //printf("sampling mu with epssum = %20.15f and sigmae = %20.15f; nonas = %d\n", epssum, sigmae, nonas);
    return dist_d.norm_rng(epssum / double(nonas), get_sigmae() / double(nonas));
}

double Phenotype::sample_beta_rng(const double a, const double b) {
    return dist_d.beta_rng(a, b);
}

double Phenotype::sample_unif_rng() {
    return dist_d.unif_rng();
}

void Phenotype::sample_for_free(const int n) {
    for (int i=0; i<n; i++)
        double fake = dist_d.unif_rng();
}

//void Phenotype::sample_sigmag_beta_rng() {
//    sigmag = dist.beta_rng(epssum / double(nonas), sigmae / double(nonas));
//}

void Phenotype::set_prng_m(const unsigned int seed) {
    dist_m.set_prng(seed);
}
void Phenotype::set_prng_d(const unsigned int seed) {
    dist_d.set_prng(seed);
}

void Phenotype::set_midx() {
    midx.clear();
    for (int i=0; i<M; ++i)
        midx.push_back(i);
}

void Phenotype::shuffle_midx(const bool mimic_hydra) {
    boost::uniform_int<> unii(0, M-1);
    if (mimic_hydra) {
        boost::variate_generator< boost::mt19937&, boost::uniform_int<> > generator(dist_d.get_rng(), unii);
        boost::range::random_shuffle(midx, generator);
    } else {
        boost::variate_generator< boost::mt19937&, boost::uniform_int<> > generator(dist_m.get_rng(), unii);
        boost::range::random_shuffle(midx, generator);
    }
}

// Add contributions to base epsilon
void Phenotype::update_epsilon(const double* dbeta, const unsigned char* bed) {

    const double bs_ = dbeta[0] * dbeta[2]; // lambda = dbeta / marker_sig
    const double mdb = -dbeta[1];
    //printf(" Phenotype::update_epsilon with dbeta = %20.15f, ave = %20.15f, bet/sig = %20.15f\n", dbeta[0], dbeta[1], bs_);

    double* epsilon = get_epsilon();

#ifdef MANVECT
    const __m256d mu = _mm256_set1_pd(-1.0 * dbeta[1]);
    const __m256d bs = _mm256_set1_pd(bs_);
#ifdef __INTEL_COMPILER
    __assume_aligned(epsilon, 32);
    __assume_aligned(&na_lut, 32);
    __assume_aligned(&dotp_lut_a, 32);
    __assume_aligned(&dotp_lut_b, 32);
#endif
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i=0; i<im4; i++) {
        __m256d luta  = _mm256_load_pd(&dotp_lut_ab[bed[i] * 8]);
        __m256d lutb  = _mm256_load_pd(&dotp_lut_ab[bed[i] * 8 + 4]);
        __m256d tmp1  = _mm256_fmadd_pd(mu, lutb, luta);
        __m256d lutna = _mm256_load_pd(&na_lut[mask4[i] * 4]);
        __m256d tmp2  = _mm256_mul_pd(bs, tmp1);
        __m256d eps4  = _mm256_load_pd(&epsilon[i*4]);
        __m256d tmp3  = _mm256_fmadd_pd(lutna, tmp2, eps4);
        _mm256_store_pd(&epsilon[i*4], tmp3);
    }
/*
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
    for (int i=0; i<im4; i++) {
        eps4  = _mm256_load_pd(&epsilon[i*4]);
        lutna = _mm256_load_pd(&na_lut[mask4[i] * 4]);
        luta  = _mm256_load_pd(&dotp_lut_a[bed[i] * 4]);
        lutb  = _mm256_load_pd(&dotp_lut_b[bed[i] * 4]);
        deps4 = _mm256_fmadd_pd(mu, lutb, luta);
        deps4 = _mm256_mul_pd(deps4, bs);
        deps4 = _mm256_mul_pd(deps4, lutna);
        eps4  = _mm256_add_pd(eps4, deps4);
        _mm256_store_pd(&epsilon[i*4], eps4);
    }
*/

#else

#ifdef _OPENMP
#pragma omp parallel for
#endif
    for (int i=0; i<im4; i++) {
        const int bedi = bed[i] * 4;
        const int masi = mask4[i] * 4;
#ifdef _OPENMP
#pragma omp simd aligned(epsilon, dotp_lut_a, dotp_lut_b, na_lut : 32) simdlen(4)
#endif
        for (int j=0; j<4; j++) {
            double a = dotp_lut_a[bedi + j];
            double b = dotp_lut_b[bedi + j];
            double m = na_lut[masi + j];
            epsilon[i*4 + j] += (mdb * b + a) * bs_ * m;
        }
    }

#endif
}

void Phenotype::offset_epsilon(const double offset) {

    double* epsilon = get_epsilon();

#ifdef _OPENMP
#pragma omp parallel for
#endif
    for (int i=0; i<im4; i++) {
        const int masi = mask4[i] * 4;
#ifdef _OPENMP
#pragma omp simd aligned(epsilon, na_lut : 32) simdlen(4)
#endif
        for (int j=0; j<4; j++) {
            epsilon[i*4 + j] += offset * na_lut[masi + j];
        }
    }
}

/*
void Phenotype::update_epsilon_sum() {
    __m256d eps4, sig4, lutna;
    __m256d sums = _mm256_set1_pd(0.0);

#ifdef _OPENMP
#pragma omp parallel for schedule(static) reduction(addpd4:sums)
#endif
    for (int i=0; i<im4; i++) {
        eps4  = _mm256_load_pd(&epsilon[i*4]);
        lutna = _mm256_load_pd(&na_lut[mask4[i] * 4]);
        eps4  = _mm256_mul_pd(eps4, lutna);
        sums  = _mm256_add_pd(sums, eps4);
    }
    epssum = sums[0] + sums[1] + sums[2] + sums[3];
}
*/

// Only depends on NAs
void Phenotype::update_epsilon_sigma() {
    double* epsilon = get_epsilon();
#ifdef MANVECT
    __m256d sume = _mm256_set1_pd(0.0);
#ifdef _OPENMP
#pragma omp parallel for schedule(static) reduction(addpd4:sume)
#endif
    for (int i=0; i<im4; i++) {
        __m256d eps4  = _mm256_load_pd(&epsilon[i*4]);
        __m256d lutna = _mm256_load_pd(&na_lut[mask4[i] * 4]);
        eps4  = _mm256_mul_pd(eps4, lutna);
        eps4  = _mm256_mul_pd(eps4, eps4);
        sume  = _mm256_add_pd(sume, eps4);
    }
    set_sigmae((sume[0] + sume[1] + sume[2] + sume[3]) / double(nonas) * 0.5);
#else
    double sigmae = 0.0;
#ifdef _OPENMP
#pragma omp parallel for simd reduction(+:sigmae)
#endif
    for (int i=0; i<im4; i++) {
        for (int j=0; j<4; j++) {
            sigmae += epsilon[i*4 + j] * epsilon[i*4 + j] * na_lut[mask4[i] * 4 + j];
        }
    }
    set_sigmae(sigmae / double(nonas) * 0.5);
#endif
}

// Compute mean and associated standard deviation for markers
// for each of the phenotypes (stats are NA dependent)
// ! one byte of bed  contains information for 4 individuals
// ! one byte of phen contains information for 8 individuals
void PhenMgr::compute_markers_statistics(const unsigned char* bed, const int N, const int M, const int mbytes) {

    int rank;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);

    for (auto& phen : get_phens()) {

        if (rank == 0)
            phen.print_info();

        const std::vector<unsigned char> mask4 = phen.get_mask4();
        const int im4 = phen.get_im4();

        double* mave = phen.get_mave();
        double* msig = phen.get_msig();

        //double start = MPI_Wtime();
#ifdef MANVECT
#ifdef _OPENMP
#pragma omp parallel for
#endif
        for (int i=0; i<M; i++) {
            __m256d suma = _mm256_set1_pd(0.0);
            __m256d sumb = _mm256_set1_pd(0.0);
            __m256d luta, lutb, lutna;
            size_t bedix = size_t(i) * size_t(mbytes);
            const unsigned char* bedm = &bed[bedix];
            for (int j=0; j<mbytes; j++) {
                luta  = _mm256_load_pd(&dotp_lut_a[bedm[j] * 4]);
                lutb  = _mm256_load_pd(&dotp_lut_b[bedm[j] * 4]);
                lutna = _mm256_load_pd(&na_lut[mask4[j] * 4]);
                luta  = _mm256_mul_pd(luta, lutna);
                lutb  = _mm256_mul_pd(lutb, lutna);
                suma  = _mm256_add_pd(suma, luta);
                sumb  = _mm256_add_pd(sumb, lutb);
            }
            double asum = suma[0] + suma[1] + suma[2] + suma[3];
            double bsum = sumb[0] + sumb[1] + sumb[2] + sumb[3];
            double avg  = asum / bsum;

            __m256d vave = _mm256_set1_pd(-avg);
            __m256d sums = _mm256_set1_pd(0.0);
            for (int j=0; j<mbytes; j++) {
                luta  = _mm256_load_pd(&dotp_lut_a[bedm[j] * 4]);
                lutb  = _mm256_load_pd(&dotp_lut_b[bedm[j] * 4]);
                lutna = _mm256_load_pd(&na_lut[mask4[j] * 4]);
                luta  = _mm256_add_pd(luta, vave);    // - mu
                luta  = _mm256_mul_pd(luta, lutb);    // M -> 0.0
                luta  = _mm256_mul_pd(luta, lutna);   // NAs
                luta  = _mm256_mul_pd(luta, luta);    // ^2
                sums  = _mm256_add_pd(sums, luta);    // sum
            }
            double sig = 1.0 / sqrt((sums[0] + sums[1] + sums[2] + sums[3]) / (double(phen.get_nonas()) - 1.0));
            mave[i] = avg;
            msig[i] = sig;
            //if (i<10)
            //    printf("marker %d: %20.15f +/- %20.15f, %20.15f / %20.15f\n", i, mave[i], msig[i], asum, bsum);
        }
#else
#ifdef _OPENMP
#pragma omp parallel for
#endif
        for (int i=0; i<M; i++) {
            size_t bedix = size_t(i) * size_t(mbytes);
            const unsigned char* bedm = &bed[bedix];
            double suma = 0.0;
            double sumb = 0.0;
            for (int j=0; j<im4; j++) {

                for (int k=0; k<4; k++) {
                    suma += dotp_lut_a[bedm[j] * 4 + k] * na_lut[mask4[j] * 4 + k];
                    sumb += dotp_lut_b[bedm[j] * 4 + k] * na_lut[mask4[j] * 4 + k];
                }
            }
            mave[i] = suma / sumb;
            double sumsqr = 0.0;
            for (int j=0; j<im4; j++) {
                for (int k=0; k<4; k++) {
                    double val = (dotp_lut_a[bedm[j] * 4 + k] - mave[i]) * dotp_lut_b[bedm[j] * 4 + k] * na_lut[mask4[j] * 4 + k];
                    sumsqr += val * val;
                }
            }
            msig[i] = 1.0 / sqrt(sumsqr / (double(phen.get_nonas()) - 1.0));
            //printf("marker %8d: %20.15f +/- %20.15f\n", i, mave[i], msig[i]);
        }

#endif
        //double end = MPI_Wtime();
        //std::cout << "statistics took " << end - start << " seconds to run." << std::endl;
    }
}

void PhenMgr::display_markers_statistics(const int n) {
    for (auto& phen : get_phens()) {
        phen.print_info();
        for (int i=0; i<n; i++) {
            printf("avg for marker %d = %20.15f +/- %20.15f  1/sig = %20.15f\n", i, phen.get_mave()[i], phen.get_msig()[i], 1.0 / phen.get_msig()[i]);
        }
    }
}

void PhenMgr::print_info() {
    for (auto& phen : get_phens())
        phen.print_info();
}

void PhenMgr::read_phen_files(const Options& opt, const int N, const int M) {
    std::vector<std::string> phen_files = opt.get_phen_files();
    int pi = 0;
    for (auto fp = phen_files.begin(); fp != phen_files.end(); ++fp) {
        pi++;
        if (opt.verbosity_level(3))
            std::cout << "Reading phenotype file " << pi << ": " << *fp << std::endl;
        phens.emplace_back(*fp, opt, N, M);
        //std::cout << "emplace_back for pi " << pi << ": " << *fp << std::endl;
    }
}

// Read phenotype file assuming PLINK format:
// Family ID, Individual ID, Phenotype; One row per individual
void Phenotype::read_file(const Options& opt) {

    std::ifstream infile(filepath);
    std::string line;
    std::regex re("\\s+");

    double* epsilon = get_epsilon();
    double sum = 0.0;

    if (infile.is_open()) {
        int line_n = 0;
        nonas = 0, nas = 0;
        while (getline(infile, line)) {
            //std::cout << line << std::endl;
            int m4 = line_n % 4;
            if (m4 == 0)  mask4.push_back(0b00001111);

            std::sregex_token_iterator first{line.begin(), line.end(), re, -1}, last;
            std::vector<std::string> tokens{first, last};

            if (tokens[2] == "NA") {
                nas += 1;
                data.push_back(std::numeric_limits<double>::max());
                if (opt.verbosity_level(2)) {
                    std::cout << " ... found NA on line " << line_n << ", m4 = " << m4 << " on byte " << int(line_n / 4) << std::endl;
                    fflush(stdout);
                }
                mask4.at(int(line_n / 4)) &= ~(0b1 << m4);
            } else {
                nonas += 1;
                data.push_back(atof(tokens[2].c_str()));
                sum += atof(tokens[2].c_str());
            }

            line_n += 1;

            if (opt.verbosity_level(3)) {
                if (line_n % 4 == 0 && line_n > 3 && line_n < 30) {
                    std::cout << "mask4[" << int(line_n / 4) - 1 << "] = " << unsigned(mask4.at(int(line_n / 4) - 1)) << std::endl;
                }
            }
        }
        infile.close();

        assert(nas + nonas == N);

        // Set last bits to 0 if ninds % 4 != 0
        const int m4 = line_n % 4;
        if (m4 != 0) {
            for (int i=m4; i<4; i++) {
                mask4.at(int(line_n / 4)) &= ~(0b1 << i);
            }
            //printf("line_n = %d\n", line_n);
            //printf("last byte starts for indiv %d\n", int(N/4)*4);
            //printf("set up to indiv %d\n", int(N/4 + 1) * 4);
            std::cout << "Setting last " << 4 - m4 << " bits to NAs" << std::endl;
            //std::cout << "fatal: missing implementation" << std::endl;
            //exit(1);
        }

        // Center and scale
        double avg = sum / double(nonas);
        if (opt.verbosity_level(3))
            printf("phen avg = %20.15f\n", avg);

        double sqn = 0.0;
        for (int i=0; i<data.size(); i++) {
            if (opt.verbosity_level(3) && i < 10)
                std::cout << data[i] - avg  << std::endl;
            if (data[i] == std::numeric_limits<double>::max()) {
                epsilon[i] = 0.0;
            } else {
                epsilon[i] = data[i] - avg;
                sqn += epsilon[i] * epsilon[i];
            }
        }
        sqn = sqrt(double(nonas-1) / sqn);
        if (opt.verbosity_level(3))
            printf("phen sqn = %20.15f\n", sqn);
        for (int i=0; i<data.size(); i++)
            epsilon[i] *= sqn;

    } else {
        std::cout << "FATAL: could not open phenotype file: " << filepath << std::endl;
        exit(EXIT_FAILURE);
    }
}

void Phenotype::print_info() const {
    printf("INFO   : %s has %d NAs and %d non-NAs.\n", get_filepath().c_str(), nas, nonas);
}

void Phenotype::set_nas_to_zero(double* y, const int N) {
    assert(N % 4 == 0);
    for (int j=0; j<N/4; j++) {
        for (int k=0; k<4; k++) {
            if (na_lut[mask4[j] * 4 + k] == 0.0) {
				assert(y[j*4 + k] == 0.0);
                //printf("found NA on %d. Correct? %20.15f\n", j * 4 + k, y[j*4 + k]);
                //y[j*4 + k] = 0.0;
            }
        }
    }
}

max_stars_repo_name: medical-genomics-group/gmrm

max_issues_repo_name: medical-genomics-group/gmrm

max_forks_repo_name: medical-genomics-group/gmrm


text:
/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::span;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenMatRowIndRangeColIndRangeGenDouble : public Expected {
    public:
      ExpectedInPlaceGenMatRowIndRangeColIndRangeGenDouble() {
        cout << "Compute ExpectedInPlaceGenMatRowIndRangeColIndRangeGenDouble(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenMat,
            InputClass::RowIndRange,
            InputClass::ColIndRange,
            InputClass::GenDouble
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genMat = *static_cast<Mat<double>*>(value.second);
                  break;
                case 1:
                  _fileSuffix += "," + value.first;
                  _rowIndRange = *static_cast<span*>(value.second);
                  break;
                case 2:
                  _fileSuffix += "," + value.first;
                  _colIndRange = *static_cast<span*>(value.second);
                  break;
                case 3:
                  _fileSuffix += "," + value.first;
                  _genDouble = *static_cast<double*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenMatA = _genMat;
            _copyOfRowIndRange = _rowIndRange;
            _copyOfGenMatB = _colIndRange;
            _copyOfGenDouble = _genDouble;

            expectedMatSubmatPlus();

            _genMat = _copyOfGenMatA;
            _rowIndRange = _copyOfRowIndRange;
            _colIndRange = _copyOfGenMatB;
            _genDouble = _copyOfGenDouble;
            expectedMatSubmatMinus();

            _genMat = _copyOfGenMatA;
            _rowIndRange = _copyOfRowIndRange;
            _colIndRange = _copyOfGenMatB;
            _genDouble = _copyOfGenDouble;
            expectedMatSubmatTimes();

            _genMat = _copyOfGenMatA;
            _rowIndRange = _copyOfRowIndRange;
            _colIndRange = _copyOfGenMatB;
            _genDouble = _copyOfGenDouble;
            expectedMatSubmatDivide();
          }

          cout << "done." << endl;
        }

    protected:
      Mat<double> _genMat;
      Mat<double> _copyOfGenMatA;

      span _rowIndRange;
      span _copyOfRowIndRange;

      span _colIndRange;
      span _copyOfGenMatB;

      double _genDouble;
      double _copyOfGenDouble;

      void expectedMatSubmatPlus() {
        if(!_genMat.in_range(_rowIndRange, _colIndRange)) {
          return;
        }

        cout << "- Compute expectedMatSubmatPlus() ... ";

        _genMat.submat(_rowIndRange, _colIndRange) += _genDouble;
        save<double>("Mat.submatPlus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatSubmatMinus() {
        if(!_genMat.in_range(_rowIndRange, _colIndRange)) {
          return;
        }

        cout << "- Compute expectedMatSubmatMinus() ... ";

        _genMat.submat(_rowIndRange, _colIndRange) -= _genDouble;
        save<double>("Mat.submatMinus", _genMat);

        cout << "done." << endl;
      }

      void expectedMatSubmatTimes() {
        if(!_genMat.in_range(_rowIndRange, _colIndRange)) {
          return;
        }

        cout << "- Compute expectedMatSubmatTimes() ... ";

        _genMat.submat(_rowIndRange, _colIndRange) *= _genDouble;
        save<double>("Mat.submatTimes", _genMat);

        cout << "done." << endl;
      }

      void expectedMatSubmatDivide() {
        if(!_genMat.in_range(_rowIndRange, _colIndRange)) {
          return;
        }

        cout << "- Compute expectedMatSubmatDivide() ... ";

        _genMat.submat(_rowIndRange, _colIndRange) /= _genDouble;
        save<double>("Mat.submatDivide", _genMat);

        cout << "done." << endl;
      }
  };
}

ext: cpp

max_stars_repo_name: SebastianNiemann/ArmadilloJava

max_issues_repo_name: sebiniemann/ArmadilloJava

max_forks_repo_name: sebiniemann/ArmadilloJava


text:
//
// Created by Sandra Alfaro on 24/05/18.
//

#include <vector>
#include <chrono>
#include <atomic>
#include <Eigen/Geometry>                 // MatrixBase.homogeneous()
#include <Eigen/SVD>
#include <Eigen/Core>                     // Transform.computeRotationScaling()

#ifdef OpenGR_USE_OPENMP
#include <omp.h>
#endif

#include "gr/utils/sampling.h"
#include "gr/utils/shared.h"
#include "gr/utils/logger.h"
#include "gr/accelerators/kdtree.h"
#include "match4pcsBase.h"

#ifdef TEST_GLOBAL_TIMINGS
#   include "gr/utils/timer.h"
#endif

namespace gr {
    template <template <typename, typename, typename> class _Functor,
              typename PointType,
              typename TransformVisitor,
              typename PairFilteringFunctor,
              template < class, class > class PFO>
    Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::Match4pcsBase (const OptionsType& options
            , const Utils::Logger& logger)
            : MatchBaseType(options,logger)
            , fun_(MatchBaseType::sampled_Q_3D_,MatchBaseType::base_3D_,MatchBaseType::options_)
    {
    }

    template <template <typename, typename, typename> class _Functor,
              typename PointType,
              typename TransformVisitor,
              typename PairFilteringFunctor,
              template < class, class > class PFO>
    Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::~Match4pcsBase() {}

    template <template <typename, typename, typename> class _Functor,
              typename PointType,
              typename TransformVisitor,
              typename PairFilteringFunctor,
              template < class, class > class PFO>
    bool Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::TryQuadrilateral(
        typename PointType::Scalar &invariant1,
        typename PointType::Scalar &invariant2,
        int &id1, int &id2, int &id3, int &id4) {

        Scalar min_distance = std::numeric_limits<Scalar>::max();
        int best1, best2, best3, best4;
        best1 = best2 = best3 = best4 = -1;
        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                if (i == j) continue;
                int k = 0;
                while (k == i || k == j) k++;
                int l = 0;
                while (l == i || l == j || l == k) l++;
                Scalar local_invariant1;
                Scalar local_invariant2;
                // Compute the closest points on both segments, the corresponding
                // invariants and the distance between the closest points.
                Scalar segment_distance = distSegmentToSegment(
                        MatchBaseType::base_3D_[i]->pos(), MatchBaseType::base_3D_[j]->pos(),
                        MatchBaseType::base_3D_[k]->pos(), MatchBaseType::base_3D_[l]->pos(),
                        local_invariant1, local_invariant2);
                // Retail the smallest distance and the best order so far.
                if (segment_distance < min_distance) {
                    min_distance = segment_distance;
                    best1 = i;
                    best2 = j;
                    best3 = k;
                    best4 = l;
                    invariant1 = local_invariant1;
                    invariant2 = local_invariant2;
                }
            }
        }

        if(best1 < 0 || best2 < 0 || best3 < 0 || best4 < 0 ) return false;

        Coordinates tmp = MatchBaseType::base_3D_;
        MatchBaseType::base_3D_[0] = tmp[best1];
        MatchBaseType::base_3D_[1] = tmp[best2];
        MatchBaseType::base_3D_[2] = tmp[best3];
        MatchBaseType::base_3D_[3] = tmp[best4];

        CongruentBaseType tmpId = {id1, id2, id3, id4};
        id1 = tmpId[best1];
        id2 = tmpId[best2];
        id3 = tmpId[best3];
        id4 = tmpId[best4];

        return true;
    }

    template <template <typename, typename, typename> class _Functor,
              typename PointType,
              typename TransformVisitor,
              typename PairFilteringFunctor,
              template < class, class > class PFO>
    bool Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::SelectQuadrilateral(
        Scalar &invariant1,
        Scalar &invariant2,
        int& base1, int& base2, int& base3, int& base4)  {

        const Scalar kBaseTooSmall (0.2);
        int current_trial = 0;

        // Try fix number of times.
        while (current_trial < MatchBaseType::kNumberOfDiameterTrials) {
            // Select a triangle if possible. otherwise fail.
            if (!MatchBaseType::SelectRandomTriangle(MatchBaseType::max_base_diameter_, base1, base2, base3)){
                return false;
            }

            const auto& b0 = *(MatchBaseType::base_3D_[0] = &MatchBaseType::sampled_P_3D_[base1]);
            const auto& b1 = *(MatchBaseType::base_3D_[1] = &MatchBaseType::sampled_P_3D_[base2]);
            const auto& b2 = *(MatchBaseType::base_3D_[2] = &MatchBaseType::sampled_P_3D_[base3]);

            // The 4th point will be a one that is close to be planar to the other 3
            // while still not too close to them.
            const double x1 = b0.pos()(0);
            const double y1 = b0.pos()(1);
            const double z1 = b0.pos()(2);
            const double x2 = b1.pos()(0);
            const double y2 = b1.pos()(1);
            const double z2 = b1.pos()(2);
            const double x3 = b2.pos()(0);
            const double y3 = b2.pos()(1);
            const double z3 = b2.pos()(2);

            // Fit a plan.
            Scalar denom = (-x3 * y2 * z1 + x2 * y3 * z1 + x3 * y1 * z2 - x1 * y3 * z2 -
                            x2 * y1 * z3 + x1 * y2 * z3);

            if (denom != 0) {
                Scalar A =
                        (-y2 * z1 + y3 * z1 + y1 * z2 - y3 * z2 - y1 * z3 + y2 * z3) / denom;
                Scalar B =
                        (x2 * z1 - x3 * z1 - x1 * z2 + x3 * z2 + x1 * z3 - x2 * z3) / denom;
                Scalar C =
                        (-x2 * y1 + x3 * y1 + x1 * y2 - x3 * y2 - x1 * y3 + x2 * y3) / denom;
                base4 = -1;
                Scalar best_distance = std::numeric_limits<Scalar>::max();
                // Go over all points in P.
                const Scalar too_small = std::pow(MatchBaseType::max_base_diameter_ * kBaseTooSmall, 2);
                for (unsigned int i = 0; i < MatchBaseType::sampled_P_3D_.size(); ++i) {
                    const auto &p = MatchBaseType::sampled_P_3D_[i];
                    if ((p.pos() - b0.pos()).squaredNorm() >= too_small &&
                        (p.pos() - b1.pos()).squaredNorm() >= too_small &&
                        (p.pos() - b2.pos()).squaredNorm() >= too_small) {
                        // Not too close to any of the first 3.
                        const Scalar distance =
                                std::abs(A * p.pos()(0) + B * p.pos()(1) + C * p.pos()(2) - 1.0);
                        // Search for the most planar.
                        if (distance < best_distance) {
                            best_distance = distance;
                            base4 = int(i);
                        }
                    }
                }
                // If we have a good one we can quit.
                if (base4 != -1) {
                    MatchBaseType::base_3D_[3] = &MatchBaseType::sampled_P_3D_[base4];
                    if(TryQuadrilateral(invariant1, invariant2, base1, base2, base3, base4))
                        return true;
                }
            }
            current_trial++;
        }

        // We failed to find good enough base..
        return false;
    }

    template <template <typename, typename, typename> class _Functor,
              typename PointType,
              typename TransformVisitor,
              typename PairFilteringFunctor,
              template < class, class > class PFO>
    // Initialize all internal data structures and data members.
    void Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::Initialize() {
        fun_.Initialize();
    }

    template <template <typename, typename, typename> class _Functor,
              typename PointType,
              typename TransformVisitor,
              typename PairFilteringFunctor,
              template < class, class > class PFO>
    bool Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::generateCongruents (
        CongruentBaseType &base, Set& congruent_quads) {
//      std::cout << "------------------" << std::endl;
        Scalar invariant1, invariant2;

        if(!initBase(base, invariant1, invariant2)) return false;

//        std::cout << "Found a new base !" << std::endl;
        const auto& b0 = *MatchBaseType::base_3D_[0];
        const auto& b1 = *MatchBaseType::base_3D_[1];
        const auto& b2 = *MatchBaseType::base_3D_[2];
        const auto& b3 = *MatchBaseType::base_3D_[3];

        // Computes distance between pairs.
        const Scalar distance1 = (b0.pos()- b1.pos()).norm();
        const Scalar distance2 = (b2.pos()- b3.pos()).norm();

        std::vector<std::pair<int, int>> pairs1, pairs2;

        // Compute normal angles.
        const Scalar normal_angle1 = (b0.normal() - b1.normal()).norm();
        const Scalar normal_angle2 = (b2.normal() - b3.normal()).norm();

        fun_.ExtractPairs(distance1, normal_angle1, MatchBaseType::distance_factor * MatchBaseType::options_.delta, 0, 1, &pairs1);
        fun_.ExtractPairs(distance2, normal_angle2, MatchBaseType::distance_factor * MatchBaseType::options_.delta, 2, 3, &pairs2);

//        std::cout << "Pair set 1 has " << pairs1.size() << " elements" << std::endl;
//        std::cout << "Pair set 2 has " << pairs2.size() << " elements" << std::endl;

//  Log<LogLevel::Verbose>( "Pair creation ouput: ", pairs1.size(), " - ", pairs2.size());

        if (pairs1.size() == 0 || pairs2.size() == 0) {
            return false;
        }

        if (!fun_.FindCongruentQuadrilaterals(invariant1, invariant2,
                                         MatchBaseType::distance_factor * MatchBaseType::options_.delta,
                                         MatchBaseType::distance_factor * MatchBaseType::options_.delta,
                                         pairs1,
                                         pairs2,
                                         &congruent_quads)) {
            return false;
        }

        return true;
    }

    template <template <typename, typename, typename> class _Functor,
              typename PointType,
              typename TransformVisitor,
              typename PairFilteringFunctor,
              template < class, class > class PFO>
    bool Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::initBase (CongruentBaseType &base)
    {
        Scalar invariant1, invariant2; // dummy

        return initBase(base, invariant1, invariant2);
    }

    template <template <typename, typename, typename> class _Functor,
              typename PointType,
              typename TransformVisitor,
              typename PairFilteringFunctor,
              template < class, class > class PFO>
    bool Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::initBase (CongruentBaseType &base, Scalar& invariant1, Scalar& invariant2)
    {
        #ifdef STATIC_BASE
        static bool first_time = true;

        if (first_time){
            std::cerr << "Warning: Running with static base" << std::endl;
            base[0] = 0;
            base[1] = 3;
            base[2] = 1;
            base[3] = 4;

            MatchBaseType::base_3D_[0] = &MatchBaseType::sampled_P_3D_ [base[0]];
            MatchBaseType::base_3D_[1] = &MatchBaseType::sampled_P_3D_ [base[1]];
            MatchBaseType::base_3D_[2] = &MatchBaseType::sampled_P_3D_ [base[2]];
            MatchBaseType::base_3D_[3] = &MatchBaseType::sampled_P_3D_ [base[3]];
            TryQuadrilateral(invariant1, invariant2, base[0], base[1], base[2], base[3]);

            first_time = false;
        }
        else
            return false;

        #else
            if (!SelectQuadrilateral(invariant1, invariant2, base[0], base[1], base[2], base[3])) {
                // std::cout << "Skipping wrong base" << std::endl;
                return false;
            }
        #endif

        return true;
    }

    template <template <typename, typename, typename> class _Functor,
              typename PointType,
              typename TransformVisitor,
              typename PairFilteringFunctor,
              template < class, class > class PFO>
    typename Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::Scalar
    Match4pcsBase<_Functor, PointType, TransformVisitor, PairFilteringFunctor, PFO>::distSegmentToSegment(
        const VectorType& p1, const VectorType& p2,
        const VectorType& q1, const VectorType& q2,
        Scalar& invariant1, Scalar& invariant2) {

        static const Scalar kSmallNumber = 0.0001;
        VectorType u = p2 - p1;
        VectorType v = q2 - q1;
        VectorType w = p1 - q1;
        Scalar a = u.dot(u);
        Scalar b = u.dot(v);
        Scalar c = v.dot(v);
        Scalar d = u.dot(w);
        Scalar e = v.dot(w);
        Scalar f = a * c - b * b;
        // s1,s2 and t1,t2 are the parametric representation of the intersection.
        // they will be the invariants at the end of this simple computation.
        Scalar s1 = 0.0;
        Scalar s2 = f;
        Scalar t1 = 0.0;
        Scalar t2 = f;

        if (f < kSmallNumber) {
            s1 = 0.0;
            s2 = 1.0;
            t1 = e;
            t2 = c;
        } else {
            s1 = (b * e - c * d);
            t1 = (a * e - b * d);
            if (s1 < 0.0) {
                s1 = 0.0;
                t1 = e;
                t2 = c;
            } else if (s1 > s2) {
                s1 = s2;
                t1 = e + b;
                t2 = c;
            }
        }

        if (t1 < 0.0) {
            t1 = 0.0;
            if (-d < 0.0)
                s1 = 0.0;
            else if (-d > a)
                s1 = s2;
            else {
                s1 = -d;
                s2 = a;
            }
        } else if (t1 > t2) {
            t1 = t2;
            if ((-d + b) < 0.0)
                s1 = 0;
            else if ((-d + b) > a)
                s1 = s2;
            else {
                s1 = (-d + b);
                s2 = a;
            }
        }
        invariant1 = (std::abs(s1) < kSmallNumber ? 0.0 : s1 / s2);
        invariant2 = (std::abs(t1) < kSmallNumber ? 0.0 : t1 / t2);

        return ( w + (invariant1 * u) - (invariant2 * v)).norm();
    }
}

ext: hpp

max_stars_repo_name: maxGimeno/OpenGR

max_issues_repo_name: maxGimeno/OpenGR

max_forks_repo_name: maxGimeno/OpenGR


text:
//
// Copyright Copyright 2009-2021, AMT – The Association For Manufacturing Technology (“AMT”)
// All rights reserved.
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
//

#include "entity/xml_parser.hpp"

#include "xml_helper.hpp"

#include <dlib/logger.h>

#include <libxml/parser.h>
#include <libxml/xpath.h>
#include <libxml/xpathInternals.h>

using namespace std;

namespace mtconnect
{
  namespace entity
  {
    static dlib::logger g_logger("entity.xml_parser");

    extern "C" void XMLCDECL entityXMLErrorFunc(void *ctx ATTRIBUTE_UNUSED, const char *msg, ...)
    {
      va_list args;

      char buffer[2048] = {0};
      va_start(args, msg);
      vsnprintf(buffer, 2046u, msg, args);
      buffer[2047] = '0';
      va_end(args);

      g_logger << dlib::LERROR << "XML: " << buffer;
    }

    inline QName nodeQName(xmlNodePtr node)
    {
      QName qname((const char *)node->name);

      if (node->ns && node->ns->prefix &&
          strncmp((const char *)node->ns->href, "urn:mtconnect.org:MTConnectDevices", 34u))
      {
        qname.setNs((const char *)node->ns->prefix);
      }

      return qname;
    }

    inline void trim(string &s)
    {
      auto beg = s.find_first_not_of(" \t\n");
      if (beg > 0)
        s.erase(0, beg);
      if (!s.empty())
      {
        auto end = s.find_last_not_of(" \t\n");
        if (end < s.size() - 1)
          s.erase(end + 1);
      }
    }

    static Value parseRawNode(xmlNodePtr node)
    {
      stringstream str;
      for (xmlNodePtr child = node->children; child; child = child->next)
      {
        xmlBufferPtr buf;
        THROW_IF_XML2_NULL(buf = xmlBufferCreate());
        auto count = xmlNodeDump(buf, child->doc, child, 0, 0);
        if (count > 0)
        {
          str << (const char *)buf->content;
        }
        xmlBufferFree(buf);
      }
      Value content {nullptr};
      if (str.tellp() > 0)
        content = str.str();
      return content;
    }

    EntityPtr XmlParser::parseXmlNode(FactoryPtr factory, xmlNodePtr node, ErrorList &errors,
                                      bool parseNamespaces)
    {
      auto qname = nodeQName(node);
      auto ef = factory->factoryFor(qname);
      if (ef)
      {
        Properties properties;
        EntityList *l {nullptr};
        if (ef->isList())
        {
          l = &properties["LIST"].emplace<EntityList>();
        }

        for (xmlAttrPtr attr = node->properties; attr; attr = attr->next)
        {
          if (attr->type == XML_ATTRIBUTE_NODE)
          {
            properties.insert(
                {(const char *)attr->name, string((const char *)attr->children->content)});
          }
        }

        if (parseNamespaces && node->nsDef)
        {
          auto def = node->nsDef;
          string name {string("xmlns:") + (const char *)def->prefix};
          properties.insert({name, string((const char *)def->href)});
        }

        if (ef->hasRaw())
        {
          auto value = parseRawNode(node);
          if (holds_alternative<string>(value))
            properties.insert({"RAW", value});
        }
        else
        {
          for (xmlNodePtr child = node->children; child; child = child->next)
          {
            if (child->type == XML_ELEMENT_NODE)
            {
              auto name = nodeQName(child);
              if (ef->isSimpleProperty(name))
              {
                if (child->children != nullptr && child->children->content != nullptr)
                {
                  string s((const char *)child->children->content);
                  trim(s);
                  if (!s.empty())
                    properties.insert({nodeQName(child), s});
                }
              }
              else
              {
                auto ent = parseXmlNode(ef, child, errors);
                if (ent)
                {
                  if (ef->isList())
                  {
                    l->emplace_back(ent);
                  }
                  else if (ef->isPropertySet(ent->getName()))
                  {
                    auto res = properties.try_emplace(ent->getName(), EntityList {});
                    get<EntityList>(res.first->second).emplace_back(ent);
                  }
                  else
                  {
                    properties.insert({ent->getName(), ent});
                  }
                }
                else
                {
                  g_logger << dlib::LWARN << "Unexpected element: " << nodeQName(child);
                  errors.emplace_back(
                      new EntityError("Invalid element '" + nodeQName(child) + "'", qname));
                }
              }
            }
            else if (child->type == XML_TEXT_NODE)
            {
              string s((const char *)child->content);
              trim(s);
              if (!s.empty())
                properties.insert({"VALUE", s});
            }
          }
        }

        try
        {
          auto entity = ef->make(qname, properties, errors);
          return entity;
        }
        catch (EntityError &e)
        {
          e.setEntity(qname);
          errors.emplace_back(e.dup());
        }
      }

      return nullptr;
    }

    EntityPtr XmlParser::parse(FactoryPtr factory, const string &document, const string &version,
                               ErrorList &errors, bool parseNamespaces)
    {
      EntityPtr entity;
      try
      {
        xmlInitParser();
        xmlXPathInit();
        xmlSetGenericErrorFunc(nullptr, entityXMLErrorFunc);

        unique_ptr<xmlDoc, function<void(xmlDocPtr)>> doc(
            xmlReadMemory(document.c_str(), document.length(), "document.xml", nullptr,
                          XML_PARSE_NOBLANKS),
            [](xmlDocPtr d) { xmlFreeDoc(d); });
        xmlNodePtr root = xmlDocGetRootElement(doc.get());
        if (root != nullptr)
          entity = parseXmlNode(factory, root, errors, parseNamespaces);
        else
          errors.emplace_back(new EntityError("Cannot parse asset"));
      }

      catch (EntityError e)
      {
        g_logger << dlib::LERROR << "Cannot parse XML document: " << e.what();
        errors.emplace_back(e.dup());
        entity.reset();
      }

      catch (XmlError e)
      {
        g_logger << dlib::LERROR << "Cannot parse XML document: " << e.what();
        errors.emplace_back(new EntityError(e.what()));
        entity.reset();
      }

      catch (...)
      {
        g_logger << dlib::LERROR << "Cannot parse XML document: unknown error";
        errors.emplace_back(new EntityError("Unknown Error"));
        entity.reset();
      }

      return entity;
    }
  }  // namespace entity
}  // namespace mtconnect

max_stars_repo_name: mtconnect/cppagent

max_issues_repo_name: mtconnect/cppagent

max_forks_repo_name: mtconnect/cppagent


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

#include <rw/core/RobWork.hpp>

#include <rwlibs/assembly/AssemblyRegistry.hpp>
#include <rwlibs/assembly/AssemblyResult.hpp>
#include <rwlibs/assembly/AssemblyTask.hpp>

#include <rwsim/contacts/ContactDetector.hpp>
//#include <rwsim/contacts/ContactStrategyCylinderTube.hpp>
#include <rwsim/loaders/DynamicWorkCellLoader.hpp>
#include <rwsim/simulator/AssemblySimulator.hpp>
#include <rwsim/simulator/PhysicsEngine.hpp>

#define BOOST_FILESYSTEM_VERSION 3
#include <boost/filesystem.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/parsers.hpp>

using namespace rw::core;
using namespace rw::proximity;
using namespace rw::math;
using namespace rwlibs::assembly;

using namespace rwsim::contacts;
using namespace rwsim::dynamics;
using namespace rwsim::loaders;
using namespace rwsim::simulator;

using namespace boost::filesystem;
using namespace boost::program_options;

int main(int argc, char** argv) {
	//Random::seed( TimerUtil::currentTimeMs() );
	options_description desc("Allowed options");
	desc.add_options()
	        		("help", "Produce this help message.")
	        		("output,o", value<std::string>()->default_value("results.assembly.xml"), "The result output file.")
	        		("dwc,d", value<std::string>(), "The dynamic workcell (requried).")
	        		("input,i", value<std::vector<std::string> >(), "The input task files (required).")
	        		;
    positional_options_description optionDesc;
    optionDesc.add("input",-1);

	variables_map vm;
    store(command_line_parser(argc, argv).
              options(desc).positional(optionDesc).run(), vm);
	notify(vm);

	if (vm.count("help")) {
		std::cout << "Usage:\n\n"
				<< "\t" << argv[0] <<" [options]\n"
				<< "\n";
		std::cout << desc << "\n";
		return 1;
	}

	if (!vm.count("dwc")) RW_THROW("Please set the dwc parameter!");
	DynamicWorkCell::Ptr dwc = DynamicWorkCellLoader::load(vm["dwc"].as<std::string>());

	Log::log().setLevel(Log::Debug);
	RobWork::getInstance()->initialize();
	Log::log().setLevel(Log::Info);
	const std::vector<std::string> engines = PhysicsEngine::Factory::getEngineIDs();
	std::cout << "Engines available: " << engines.size() << std::endl;
	for(const std::string& str : engines) {
		std::cout << str << std::endl;
	}

    std::vector<std::string> infiles;
    if(vm.count("input") ){
        const std::vector<std::string> &inputs = vm["input"].as<std::vector<std::string> >();
        for(std::string input : inputs) {
            path ip(input);
            if( is_directory(ip) ){
                infiles = IOUtil::getFilesInFolder( ip.string(), false, true);
            } else {
                infiles.push_back( ip.string() );
            }
        }
    }

	AssemblyRegistry::Ptr registry = ownedPtr(new AssemblyRegistry());
	std::vector<AssemblyTask::Ptr> tasks = AssemblyTask::load(infiles,registry);

	//ContactDetector::Ptr detector = ownedPtr(new ContactDetector(dwc->getWorkcell()));
	//detector->setDefaultStrategies(dwc->getEngineSettings());
	//detector->printStrategyTable();
	//AssemblySimulator::Ptr sim = ownedPtr(new AssemblySimulator(dwc,"TNTIsland",detector));
	AssemblySimulator::Ptr sim = ownedPtr(new AssemblySimulator(dwc,"ODE",NULL));
	sim->setDt(0.002);
	sim->setMaxSimTime(10);
	sim->setTasks(tasks);
	sim->setStoreExecutionData(true);
	sim->start();
	std::vector<AssemblyResult::Ptr> results = sim->getResults();
	AssemblyResult::saveRWResult(results,vm["output"].as<std::string>());
	std::cout << "Done" << std::endl;

	return 0;
}


text:
// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE StepIOTest

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <thread>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/names.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/l1.hpp>
#include <popart/session.hpp>
#include <popart/tensordata.hpp>
#include <popart/testdevice.hpp>

using namespace popart;

namespace {

std::shared_ptr<popart::DeviceInfo> acquireIpu() {
  // keep trying to attach to a device until one is available (this may not
  // always be the case as other tests might be running in parallel).
  while (true) {
    if (auto d = createTestDevice(TEST_TARGET, 1, 1216)) {
      return d;
    }

    std::this_thread::sleep_for(std::chrono::seconds(1));
  }

  return nullptr;
}

} // unnamed namespace

BOOST_AUTO_TEST_CASE(StepIOTest_BufferInput) {

  Shape inShape = {2, 5};
  TensorInfo inInfo{"INT32", inShape};

  std::vector<int> rawConstInputData(5 * 2);
  std::iota(rawConstInputData.begin(), rawConstInputData.end(), 1);

  popart::NDArrayWrapper<int> constData(rawConstInputData.data(), {5, 2});

  ConstVoidData constShapeData = {rawConstInputData.data(),
                                  {"INT32", constData.shape()}};

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  auto constId = aiOnnx.constant(constShapeData, "out0ShapeData");
  auto inId    = builder->addInputTensor(inInfo);

  auto outShapeId = aiOnnx.transpose({constId}, {});
  auto out        = aiOnnx.add({outShapeId, inId});
  builder->addOutputTensor(out);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art      = AnchorReturnType("All");
  auto dataFlow = DataFlow(1, {{out, art}});

  auto device = popart::createTestDevice(TEST_TARGET);

  auto session = popart::InferenceSession::createFromOnnxModel(
      proto,
      dataFlow,
      device,
      popart::InputShapeInfo(),
      {},
      popart::Patterns::create({"PostNRepl"}).enableRuntimeAsserts(false));

  // prepare the anchors
  int rawOutputData[10];
  popart::NDArrayWrapper<int> outData(rawOutputData, {2, 5});

  std::map<popart::TensorId, popart::IArray &> anchors = {
      {out, outData},
  };

  session->prepareDevice();

  int rawInputData[10] = {
      0,
  };
  popart::NDArrayWrapper<int> inData(rawInputData, {2, 5});
  std::map<popart::TensorId, popart::IArray &> inputs = {{inId, inData}};

  popart::StepIO stepio(inputs, anchors);

  session->run(stepio);

  popart::logging::info("const : {}", constData);
  popart::logging::info("input : {}", inData);
  popart::logging::info("output : {}", outData);

  int expectedOutput[10] = {1, 3, 5, 7, 9, 2, 4, 6, 8, 10};
  BOOST_CHECK(std::equal(&expectedOutput[0],
                         &expectedOutput[10],
                         &rawOutputData[0]) == true);
}

bool ipu_available(boost::unit_test::test_unit_id) {
  auto devices =
      popart::DeviceManager::createDeviceManager().enumerateDevices();
  return devices.size() > 0;
}

BOOST_AUTO_TEST_CASE(StepIOTest_BufferInput_Ipu,
                     *boost::unit_test::precondition(ipu_available)) {

  Shape inShape = {2, 5};
  TensorInfo inInfo{"INT32", inShape};

  Shape constShape = {5, 2};
  std::vector<int> rawConstInputData(5 * 2);
  std::iota(rawConstInputData.begin(), rawConstInputData.end(), 1);

  popart::NDArrayWrapper<int> constData(rawConstInputData.data(), {5, 2});

  ConstVoidData constShapeData = {rawConstInputData.data(),
                                  {"INT32", constShape}};

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  auto constId = aiOnnx.constant(constShapeData, "out0ShapeData");
  auto inId    = builder->addInputTensor(inInfo);

  auto outShapeId = aiOnnx.transpose({constId}, {});
  auto out        = aiOnnx.add({outShapeId, inId});
  builder->addOutputTensor(out);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art      = AnchorReturnType("All");
  auto dataFlow = DataFlow(1, {{out, art}});

  auto ipuDevice = acquireIpu();

  auto session = popart::InferenceSession::createFromOnnxModel(
      proto,
      dataFlow,
      ipuDevice,
      popart::InputShapeInfo(),
      {},
      popart::Patterns::create({"PostNRepl"}).enableRuntimeAsserts(false));

  // prepare the anchors
  int rawOutputData[10];
  popart::NDArrayWrapper<int> outData(rawOutputData, {2, 5});

  std::map<popart::TensorId, popart::IArray &> anchors = {
      {out, outData},
  };

  session->prepareDevice();

  int rawInputData[10] = {
      0,
  };
  popart::NDArrayWrapper<int> inData(rawInputData, {2, 5});
  std::map<popart::TensorId, popart::IArray &> inputs = {{inId, inData}};

  popart::StepIO stepio(inputs, anchors);

  session->run(stepio);

  popart::logging::info("const : {}", constData);
  popart::logging::info("input : {}", inData);
  popart::logging::info("output : {}", outData);

  int expectedOutput[10] = {1, 3, 5, 7, 9, 2, 4, 6, 8, 10};
  BOOST_CHECK(std::equal(&expectedOutput[0],
                         &expectedOutput[10],
                         &rawOutputData[0]) == true);
}

BOOST_AUTO_TEST_CASE(StepIOTest_CallbackInput) {

  Shape inShape = {2, 5};
  TensorInfo inInfo{"INT32", inShape};

  Shape constShape = {5, 2};
  std::vector<int> rawConstInputData(5 * 2);
  std::iota(rawConstInputData.begin(), rawConstInputData.end(), 1);

  popart::NDArrayWrapper<int> constData(rawConstInputData.data(), {5, 2});

  ConstVoidData constShapeData = {rawConstInputData.data(),
                                  {"INT32", constShape}};

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  auto constId = aiOnnx.constant(constShapeData, "out0ShapeData");
  auto inId    = builder->addInputTensor(inInfo);

  auto outShapeId = aiOnnx.transpose({constId}, {});
  auto out        = aiOnnx.add({outShapeId, inId});
  builder->addOutputTensor(out);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art      = AnchorReturnType("All");
  auto dataFlow = DataFlow(1, {{out, art}});

  auto device = popart::createTestDevice(TEST_TARGET);

  auto session = popart::InferenceSession::createFromOnnxModel(
      proto,
      dataFlow,
      device,
      popart::InputShapeInfo(),
      {},
      popart::Patterns::create({"PostNRepl"}).enableRuntimeAsserts(false));

  // prepare the anchors
  int rawOutputData[10];
  popart::NDArrayWrapper<int> outData(rawOutputData, {2, 5});

  std::map<popart::TensorId, popart::IArray &> anchors = {
      {out, outData},
  };

  session->prepareDevice();

  int rawInputData[10] = {
      0,
  };

  popart::StepIOCallback::InputCallback input_callback =
      [&](TensorId id, bool prefetch) -> ConstVoidData {
    popart::logging::info("input callback called {}", id);
    (void)prefetch;
    popart::NDArrayWrapper<int> inData(rawInputData, {2, 5});

    ConstVoidData data;
    data.data = inData.data();
    data.info = TensorInfo(DataType::INT32, {2, 5});
    return data;
  };

  popart::StepIOCallback::InputCompleteCallback input_complete_callback =
      [](TensorId id) -> void {
    popart::logging::info("input complete callback called {}", id);
  };

  popart::StepIOCallback::OutputCallback output_callback =
      [&](TensorId id) -> MutableVoidData {
    popart::logging::info("output callback called {}", id);

    popart::NDArrayWrapper<int> outData(rawOutputData, {2, 5});

    MutableVoidData data;
    data.data = outData.data();
    data.info = TensorInfo(DataType::INT32, {2, 5});
    return data;
  };

  popart::StepIOCallback::OutputCompleteCallback output_complete_callback =
      [](TensorId id) -> void {
    popart::logging::info("output complete callback called {}", id);
  };

  popart::StepIOCallback stepio(input_callback,
                                input_complete_callback,
                                output_callback,
                                output_complete_callback);

  session->run(stepio);

  int expectedOutput[10] = {1, 3, 5, 7, 9, 2, 4, 6, 8, 10};
  BOOST_CHECK(std::equal(&expectedOutput[0],
                         &expectedOutput[10],
                         &rawOutputData[0]) == true);
}

BOOST_AUTO_TEST_CASE(StepIOTest_CallbackInput_Ipu,
                     *boost::unit_test::precondition(ipu_available)) {

  Shape inShape = {2, 5};
  TensorInfo inInfo{"INT32", inShape};

  Shape constShape = {5, 2};
  std::vector<int> rawConstInputData(5 * 2);
  std::iota(rawConstInputData.begin(), rawConstInputData.end(), 1);

  popart::NDArrayWrapper<int> constData(rawConstInputData.data(), {5, 2});

  ConstVoidData constShapeData = {rawConstInputData.data(),
                                  {"INT32", constShape}};

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  auto constId = aiOnnx.constant(constShapeData, "out0ShapeData");
  auto inId    = builder->addInputTensor(inInfo);

  auto outShapeId = aiOnnx.transpose({constId}, {});
  auto out        = aiOnnx.add({outShapeId, inId});
  builder->addOutputTensor(out);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art      = AnchorReturnType("All");
  auto dataFlow = DataFlow(1, {{out, art}});

  auto ipuDevice = acquireIpu();

  auto session = popart::InferenceSession::createFromOnnxModel(
      proto,
      dataFlow,
      ipuDevice,
      popart::InputShapeInfo(),
      {},
      popart::Patterns::create({"PostNRepl"}).enableRuntimeAsserts(false));

  // prepare the anchors
  int rawOutputData[10];
  popart::NDArrayWrapper<int> outData(rawOutputData, {2, 5});

  std::map<popart::TensorId, popart::IArray &> anchors = {
      {out, outData},
  };

  session->prepareDevice();

  int rawInputData[10] = {
      0,
  };

  popart::StepIOCallback::InputCallback input_callback =
      [&](TensorId id, bool prefetch) -> ConstVoidData {
    popart::logging::info("input callback called {}", id);
    (void)prefetch;
    popart::NDArrayWrapper<int> inData(rawInputData, {2, 5});

    ConstVoidData data;
    data.data = inData.data();
    data.info = TensorInfo(DataType::INT32, {2, 5});
    return data;
  };

  popart::StepIOCallback::InputCompleteCallback input_complete_callback =
      [](TensorId id) -> void {
    popart::logging::info("input complete callback called {}", id);
  };

  popart::StepIOCallback::OutputCallback output_callback =
      [&](TensorId id) -> MutableVoidData {
    popart::logging::info("output callback called {}", id);

    popart::NDArrayWrapper<int> outData(rawOutputData, {2, 5});

    MutableVoidData data;
    data.data = outData.data();
    data.info = TensorInfo(DataType::INT32, {2, 5});
    return data;
  };

  popart::StepIOCallback::OutputCompleteCallback output_complete_callback =
      [](TensorId id) -> void {
    popart::logging::info("output complete callback called {}", id);
  };

  popart::StepIOCallback stepio(input_callback,
                                input_complete_callback,
                                output_callback,
                                output_complete_callback);

  session->run(stepio);

  int expectedOutput[10] = {1, 3, 5, 7, 9, 2, 4, 6, 8, 10};
  BOOST_CHECK(std::equal(&expectedOutput[0],
                         &expectedOutput[10],
                         &rawOutputData[0]) == true);
}


text:
#include <boost/test/unit_test.hpp>

#include <string>
#include <util/string/RangeTokenizer.h>
#include <iostream>

using namespace izenelib::util;

namespace { // {anonymous}

// test func to return first n char as tokenizer
struct FirstNCharTokenizerFunc
{
    // 0 is unlimited
    explicit FirstNCharTokenizerFunc(unsigned n = 0)
    : count_(0), n_(n)
    {}

    template<typename Iterator>
    bool operator()(Iterator& next,
                    Iterator end,
                    std::pair<Iterator, Iterator>& tok)
    {
        if (next == end || (n_ > 0 && count_ >= n_))
        {
            return false;
        }

        tok.first = next;
        tok.second = ++next;
        ++count_;
        return true;
    }

    void reset()
    {
        count_ = 0;
    }

private:
    unsigned count_;
    unsigned n_;
};

struct FirstNCharTokenizerFuncFixture
{
    typedef FirstNCharTokenizerFunc tokenizer_func;
    typedef std::string::iterator iterator;
    typedef RangeTokenizer<tokenizer_func, iterator> tokenizer;
    typedef tokenizer::iterator token_iterator;
};

} // namespace {annonymous}

BOOST_FIXTURE_TEST_SUITE(RangeTokenIterator_test, FirstNCharTokenizerFuncFixture)

BOOST_AUTO_TEST_CASE(RangeConstructor_test)
{
    std::string str("123");
    tokenizer tokens(str.begin(), str.end());

    token_iterator it = tokens.begin();
    token_iterator end = tokens.end();

    BOOST_CHECK(it != end);
    BOOST_CHECK(it->first == str.begin() + 0);
    BOOST_CHECK(it->second == str.begin() + 1);

    ++it;
    BOOST_CHECK(it != end);
    BOOST_CHECK(it->first == str.begin() + 1);
    BOOST_CHECK(it->second == str.begin() + 2);

    ++it;
    BOOST_CHECK(it != end);
    BOOST_CHECK(it->first == str.begin() + 2);
    BOOST_CHECK(it->second == str.begin() + 3);

    ++it;
    BOOST_CHECK(it == end);
}

BOOST_AUTO_TEST_CASE(ContainerConstructor_test)
{
    std::string str("123");
    tokenizer tokens(str);

    token_iterator it = tokens.begin();
    token_iterator end = tokens.end();

    BOOST_CHECK(it != end);
    BOOST_CHECK(it->first == str.begin() + 0);
    BOOST_CHECK(it->second == str.begin() + 1);

    ++it;
    BOOST_CHECK(it != end);
    BOOST_CHECK(it->first == str.begin() + 1);
    BOOST_CHECK(it->second == str.begin() + 2);

    ++it;
    BOOST_CHECK(it != end);
    BOOST_CHECK(it->first == str.begin() + 2);
    BOOST_CHECK(it->second == str.begin() + 3);

    ++it;
    BOOST_CHECK(it == end);
}

BOOST_AUTO_TEST_CASE(RangeFuncConstructor_test)
{
    std::string str("123");
    tokenizer_func func(1);
    tokenizer tokens(str.begin(), str.end(), func);

    token_iterator it = tokens.begin();
    token_iterator end = tokens.end();

    BOOST_CHECK(it != end);
    BOOST_CHECK(it->first == str.begin() + 0);
    BOOST_CHECK(it->second == str.begin() + 1);

    ++it;
    BOOST_CHECK(it == end);
}

BOOST_AUTO_TEST_CASE(ContainerFuncConstructor_test)
{
    std::string str("123");
    tokenizer_func func(1);
    tokenizer tokens(str, func);

    token_iterator it = tokens.begin();
    token_iterator end = tokens.end();

    BOOST_CHECK(it != end);
    BOOST_CHECK(it->first == str.begin() + 0);
    BOOST_CHECK(it->second == str.begin() + 1);

    ++it;
    BOOST_CHECK(it == end);
}

BOOST_AUTO_TEST_SUITE_END() // RangeTokenIterator_test

max_stars_repo_name: izenecloud/izenelib

max_issues_repo_name: izenecloud/izenelib

max_forks_repo_name: izenecloud/izenelib


text:
#include "log.h"
#include "jsonreader.h"
#include "ifile.h"
#include "osfile.h"
#include <cstdio>
#include <cstdarg>
#include <memory>
#include <iosfwd>
#include <cctype>
#include <boost/format.hpp>
#include "game_clock.h"

namespace platform {

LogEntryDesc LogEntryDesc::create( int l, std::string const& ss, int li, std::string const& f, std::string const& fu )
{
    LogEntryDesc desc;
    desc.level = l;
    desc.subsystem = ss;
    desc.line = li;
    desc.file = f;
    desc.function = fu;
    desc.timestamp = Clock::Now();
    return desc;
}

LogEntry::LogEntry( LogEntryDesc const& desc )
    : mDesc( desc )
{
}

LogEntry::~LogEntry()
{
    Logger::Get().Log( *this );
}

LogEntryDesc const& LogEntry::desc() const
{
    return mDesc;
}

namespace {
bool needsWS( std::stringstream& mContent )
{
    if( mContent.rdbuf()->in_avail() < 1 )
    {
        return false;
    }
    mContent.seekg( -1, mContent.end );
    return std::isspace( mContent.peek() );
}
}

void LogEntry::beforeWrite()
{
    if( needsWS( mContent ) )
    {
        mContent << " ";
    }
}

void LogEntry::afterWrite()
{
}

std::string LogEntry::print() const
{
    return boost::str( boost::format( "[%1%] %2% %3% %4% %5% [%6%] : %7%\n" ) % mDesc.timestamp % mDesc.subsystem % mDesc.level % mDesc.file % mDesc.line % mDesc.function % mContent.str() );
}

void Logger::Log( LogEntry const& entry )
{
    if( mDisabledLevels & ( 1 << entry.desc().level ) || mLogFile.mFile == nullptr )
    {
        return;
    }
    std::string const& log = entry.print();
    fprintf( mLogFile.mFile, "%s", log.c_str() );
}

void Logger::Log( int Level, char const* format, ... )
{
    if( mDisabledLevels & ( 1 << Level )||mLogFile.mFile==nullptr )
    {
        return;
    }
    va_list arg;
    int done;
    fprintf( mLogFile.mFile, "[%f]", Clock::Now() );
    va_start ( arg, format );
    done = vfprintf ( mLogFile.mFile, format, arg );
#if BOOST_OS_WINDOWS
    // Temp. Currently using va_args two times does crash using gcc.
    if(Level==1) done = vfprintf ( stderr, format, arg );
#endif
    fflush( mLogFile.mFile );
    va_end ( arg );
}

Logger::Logger()
    : mLogFile()
    , mDisabledLevels( 0 )
{
    std::auto_ptr<File> f( new OsFile( "debug.json" ) );
    if( !f.get() || !f->IsValid() )
    {
        return;
    }
    JsonReader Reader( *f.get() );
    if( !Reader.IsValid() )
    {
        return;
    }
    Json::Value& Root = Reader.GetRoot();
    if( !Root.isObject() )
    {
        return;
    }
    Json::Value& DisabledLogs = Root["disabled_logs"];
    if( !DisabledLogs.isArray() )
    {
        return;
    }
    for( Json::Value::iterator it = DisabledLogs.begin(), e = DisabledLogs.end(); it != e; ++it )
    {
        Json::Value const& DisLog = *it;
        uint32_t Val;
        if( !Json::GetUInt( DisLog, Val ) )
        {
            continue;
        }
        mDisabledLevels |= ( 1 << Val );
    }
}

void Logger::SetFileName( std::string filename )
{
    if (mLogFile.mFile)
    {
        fclose( mLogFile.mFile );
    }
    mLogFile.mFile = fopen( filename.c_str(), "w" );
}

AutoNormalFile::AutoNormalFile( const char* name, const char* mode )
{
    mFile = fopen ( name, mode );
}

AutoNormalFile::AutoNormalFile()
{

}

AutoNormalFile::~AutoNormalFile()
{
    if (mFile)
    {
        fclose( mFile );
    }
}

} // namespace platform


text:
#include "Configuration.h"

#include <util/SmallVector.h>
#include <vector>
#include <algorithm>
#include <utility>

#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>

using namespace std;
using namespace boost;

#ifndef countof
	#define countof(array) (sizeof(array)/sizeof((array)[0]))
#endif

#define COMMENT_TOKEN	"#"
#define SEPARATOR_TOKEN "="

// These characters are ignored in the parsing.
static const char whitespaceCharacters[] = {
	' ',
	'\t',
	'\n'
};

typedef pair<size_t /* lineNumber */, string /* line */> Line;
typedef pair<string /* key */, string /* value */> MapElem;

typedef SmallVector<Line, 128> VecOfLines;

// Read up on MapReduce for info on what this is about.
template <typename ContainerType, typename Functor>
static void map_func(ContainerType& container, Functor func)
{
	for_each(container.begin(), container.end(), func);
}

template <typename Predicate>
static string filter(const string& cIn, Predicate pred)
{
	string cOut;
	cOut.reserve(cIn.length());

	string::const_iterator i   = cIn.begin();
	string::const_iterator end = cIn.end();

	for(; i != end; ++i)
		if(pred(*i)) cOut += *i;

	return cOut;
}

static string my_getline(istream& stream, char sep)
{
	string line;
	getline(stream, line, sep);
	return line;
}

static VecOfLines get_lines(istream& stream)
{
	VecOfLines lines;

	for(size_t i = 1; stream.good(); ++i)
		lines.push_back(make_pair(i, my_getline(stream, '\n')));

	return lines;
}

static Line strip_comments(const Line& in)
{
	const string& str = in.second;
	return make_pair(in.first, str.substr(0, str.find(COMMENT_TOKEN)));
}

static bool is_not_whitespace_char(char c)
{
	const char* begin = whitespaceCharacters;
	const char* end   = begin + countof(whitespaceCharacters);

	return find(begin, end, c) == end;
}

static Line strip_whitespace(const Line& in)
{
	return make_pair(in.first, filter(in.second, is_not_whitespace_char));
}

static void preprocess(VecOfLines& lines)
{
	transform(lines.begin(), lines.end(), lines.begin(), strip_comments);
	transform(lines.begin(), lines.end(), lines.begin(), strip_whitespace);
}

static MapElem parse_pure(const Line& toParse)
{
	const size_t& lineNumber = get<0>(toParse);
	const string& str        = get<1>(toParse);

	const size_t locOfSep = str.find(SEPARATOR_TOKEN);

	if(locOfSep == string::npos)
		throw SyntaxError("No separator found!", lineNumber);

	const MapElem ret = make_pair(str.substr(0, locOfSep), str.substr(locOfSep + 1, string::npos));

	if(get<0>(ret) == "")
		throw SyntaxError("No key found!", lineNumber);

	// We purposely allow blank values. Those are totally legit.

	return ret;
}

static bool is_blank(const Line& line)
{
	return get<1>(line).length() == 0;
}

static void parse(const Line& toParse, unordered_map<string, string>* target)
{
	if(is_blank(toParse))
		return;

	target->insert(parse_pure(toParse));
}

#ifdef BOOST_HAS_RVALUE_REFS
static unordered_map<string, string> init_config(VecOfLines&& lines)
#else
static unordered_map<string, string> init_config(VecOfLines lines)
#endif
{
	unordered_map<string, string> values;

	preprocess(lines);
	map_func(lines, boost::bind(parse, _1, &values));

	return values;
}

Configuration::Configuration(std::istream& stream)
	: values(init_config(get_lines(stream)))
{
}

Configuration::Configuration(const Configuration& rhs)
	: values(rhs.values)
{
}

#ifdef BOOST_HAS_RVALUE_REFS
Configuration::Configuration(std::istream&& stream)
	: values(init_config(get_lines(stream)))
{
}

Configuration::Configuration(const Configuration&& rhs)
	: values(std::move(rhs.values))
{
}
#endif

SyntaxError::SyntaxError(const char* message, size_t _lineNumber)
	: runtime_error(message), lineNumber(_lineNumber)
{
}

Configuration::~Configuration()
{
}

max_stars_repo_name: gogo-dev/GoGo

max_issues_repo_name: gogo-dev/GoGo

max_forks_repo_name: gogo-dev/GoGo


text:
#include "txt_reader.hpp"
#include <decomp_util/ellipsoid_decomp.h>
#include <decomp_geometry/geometric_utils.h>

#include <fstream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

int main(int argc, char **argv) {
  if (argc != 2) {
    printf(ANSI_COLOR_RED "Input txt file required!\n" ANSI_COLOR_RESET);
    return -1;
  }

  // Read obstacles
  vec_Vec2f obs;
  if(!read_obs<2>(argv[1], obs)) {
    printf(ANSI_COLOR_RED "Cannot find input file [%s]!\n" ANSI_COLOR_RESET,
           argv[1]);
    return -1;
  }

  // Set map size
  const Vec2f origin(-2, -2);
  const Vec2f range(4, 4);

  // Path to dilate
  vec_Vec2f path;
  path.push_back(Vec2f(1, 1.0));
  path.push_back(Vec2f(0.0, 0.0));
  path.push_back(Vec2f(-1, 1.0));

  // Initialize SeedDecomp2D
  EllipsoidDecomp2D decomp(origin, range);
  decomp.set_obs(obs);
  decomp.set_local_bbox(Vec2f(2, 2));
  decomp.dilate(path, 0);

  // Plot the result in svg image
  typedef boost::geometry::model::d2::point_xy<double> point_2d;
  std::ofstream svg("output.svg");
  // Declare a stream and an SVG mapper
  boost::geometry::svg_mapper<point_2d> mapper(svg, 1000, 1000);

  // Draw the canvas 4 x 4m
  boost::geometry::model::polygon<point_2d> bound;
  std::vector<point_2d> points;
  points.push_back(point_2d(origin(0), origin(1)));
  points.push_back(point_2d(origin(0), origin(1) + range(1)));
  points.push_back(point_2d(origin(0) + range(0), origin(1) + range(1)));
  points.push_back(point_2d(origin(0) + range(0), origin(1)));
  points.push_back(point_2d(origin(0), origin(1)));
  boost::geometry::assign_points(bound, points);
  boost::geometry::correct(bound);

  mapper.add(bound);
  mapper.map(bound, "fill-opacity:1.0;fill:rgb(255,255,255);stroke:rgb(0,0,0);"
                    "stroke-width:2"); // White
  // Draw obstacles
  for(const auto& it: obs) {
    point_2d pt;
    boost::geometry::assign_values(pt, it(0), it(1));
    mapper.add(pt);
    mapper.map(pt, "fill-opacity:1.0;fill:rgb(255,0,0);", 10); // Red
  }

  // Draw ellispoid
  {
    for(const auto& E: decomp.get_ellipsoids()) {
      int num = 40; // number of points on trajectory to draw
      boost::geometry::model::linestring<point_2d> line;
      for (const auto& it: E.sample(num))
        line.push_back(point_2d(it(0), it(1)));
      line.push_back(line.front());
      mapper.add(line);
      mapper.map(line,
                 "opacity:0.4;fill:none;stroke:rgb(118,215,234);stroke-width:5");
    }
  }

  // Draw polygon
  {
    for(const auto& poly: decomp.get_polyhedrons()) {
      const auto vertices = cal_vertices(poly);
      std::string ss("POLYGON((");
      for (size_t i = 0; i < vertices.size(); i++) {
        ss += std::to_string(vertices[i](0)) + " " +
          std::to_string(vertices[i](1));
        if(i == vertices.size() - 1)
          ss += "))";
        else
          ss += ",";
      }

      boost::geometry::model::polygon<point_2d> p;
      boost::geometry::read_wkt(ss, p);
      mapper.add(p);
      mapper.map(p, "fill-opacity:0.2;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:2");
    }
  }

  // Draw path
  {
    boost::geometry::model::linestring<point_2d> line;
    for(const auto& it: path)
      line.push_back(point_2d(it(0), it(1)));
    mapper.add(line);
    mapper.map(line,
               "opacity:0.8;fill:none;stroke:rgb(255,0,0);stroke-width:5");
  }

  // Write title at the lower right corner on canvas
  mapper.text(point_2d(1.0, -1.8), "test_ellipsoid_decomp",
              "fill-opacity:1.0;fill:rgb(10,10,250);");

  return 0;
}

max_stars_repo_name: shubham-shahh/mader

max_issues_repo_name: shubham-shahh/mader

max_forks_repo_name: shubham-shahh/mader


text:
/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#include "consensus/validation/babe_block_validator.hpp"

#include <algorithm>
#include <boost/assert.hpp>

#include "common/mp_utils.hpp"
#include "consensus/babe/impl/babe_digests_util.hpp"
#include "consensus/validation/prepare_transcript.hpp"
#include "crypto/sr25519_provider.hpp"
#include "scale/scale.hpp"

OUTCOME_CPP_DEFINE_CATEGORY(kagome::consensus,
                            BabeBlockValidator::ValidationError,
                            e) {
  using E = kagome::consensus::BabeBlockValidator::ValidationError;
  switch (e) {
    case E::NO_AUTHORITIES:
      return "no authorities are provided for the validation";
    case E::INVALID_SIGNATURE:
      return "SR25519 signature, which is in BABE header, is invalid";
    case E::INVALID_VRF:
      return "VRF value and output are invalid";
    case E::TWO_BLOCKS_IN_SLOT:
      return "peer tried to distribute several blocks in one slot";
  }
  return "unknown error";
}

namespace kagome::consensus {
  using common::Buffer;

  BabeBlockValidator::BabeBlockValidator(
      std::shared_ptr<blockchain::BlockTree> block_tree,
      std::shared_ptr<runtime::TaggedTransactionQueue> tx_queue,
      std::shared_ptr<crypto::Hasher> hasher,
      std::shared_ptr<crypto::VRFProvider> vrf_provider,
      std::shared_ptr<crypto::Sr25519Provider> sr25519_provider,
      std::shared_ptr<primitives::BabeConfiguration> configuration)
      : block_tree_{std::move(block_tree)},
        tx_queue_{std::move(tx_queue)},
        hasher_{std::move(hasher)},
        vrf_provider_{std::move(vrf_provider)},
        sr25519_provider_{std::move(sr25519_provider)},
        configuration_{std::move(configuration)},
        log_{common::createLogger("BabeBlockValidator")} {
    BOOST_ASSERT(block_tree_);
    BOOST_ASSERT(tx_queue_);
    BOOST_ASSERT(vrf_provider_);
    BOOST_ASSERT(sr25519_provider_);
  }

  outcome::result<void> BabeBlockValidator::validateHeader(
      const primitives::BlockHeader &header,
      const EpochNumber epoch_number,
      const primitives::AuthorityId &authority_id,
      const Threshold &threshold,
      const Randomness &randomness) const {
    log_->debug("Validates block signed by authority: {}",
                authority_id.id.toHex());

    // get BABE-specific digests, which must be inside of this block
    OUTCOME_TRY(babe_digests, getBabeDigests(header));
    const auto &[seal, babe_header] = babe_digests;

    // signature in seal of the header must be valid
    if (!verifySignature(header,
                         babe_header,
                         seal,
                         primitives::BabeSessionKey{authority_id.id})) {
      return ValidationError::INVALID_SIGNATURE;
    }

    // VRF must prove that the peer is the leader of the slot
    if (babe_header.needVRFCheck()
        && !verifyVRF(babe_header,
                      epoch_number,
                      primitives::BabeSessionKey{authority_id.id},
                      threshold,
                      randomness,
                      babe_header.needVRFWithThresholdCheck())) {
      return ValidationError::INVALID_VRF;
    }

    return outcome::success();
  }

  bool BabeBlockValidator::verifySignature(
      const primitives::BlockHeader &header,
      const BabeBlockHeader &babe_header,
      const Seal &seal,
      const primitives::BabeSessionKey &public_key) const {
    // firstly, take hash of the block's header without Seal, which is the last
    // digest
    auto unsealed_header = header;
    unsealed_header.digest.pop_back();

    auto unsealed_header_encoded = scale::encode(unsealed_header).value();

    auto block_hash = hasher_->blake2b_256(unsealed_header_encoded);

    // secondly, use verify function to check the signature
    auto res =
        sr25519_provider_->verify(seal.signature, block_hash, public_key);
    return res && res.value();
  }

  bool BabeBlockValidator::verifyVRF(
      const BabeBlockHeader &babe_header,
      const EpochNumber epoch_number,
      const primitives::BabeSessionKey &public_key,
      const Threshold &threshold,
      const Randomness &randomness,
      const bool checkThreshold) const {
    primitives::Transcript transcript;
    prepareTranscript(
        transcript, randomness, babe_header.slot_number, epoch_number);
    log_->debug(
        "prepareTranscript (verifyVRF): randomness {}, slot {}, epoch {}",
        randomness,
        babe_header.slot_number,
        epoch_number);

    auto verify_res = vrf_provider_->verifyTranscript(
        transcript, babe_header.vrf_output, public_key, threshold);
    if (not verify_res.is_valid) {
      log_->error("VRF proof in block is not valid");
      return false;
    }

    // verify threshold
    if (checkThreshold && not verify_res.is_less) {
      log_->error("VRF value is not less than the threshold");
      return false;
    }

    return true;
  }
}  // namespace kagome::consensus

max_stars_repo_name: Harrm/kagome

max_issues_repo_name: Harrm/kagome

max_forks_repo_name: Harrm/kagome


text:
/**
 * Copyright 2004-present Facebook. All Rights Reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <boost/algorithm/string.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include "source/gpu/GlfwUtil.h"
#include "source/render/RigScene.h"
#include "source/render/VideoFile.h"
#include "source/util/SystemUtil.h"

using namespace fb360_dep;

const std::string kUsageMessage = R"(
  - OpenGL-based viewer for binary 6dof data files.

  Keyboard navigation:
  - w, a, s, d as well as the arrow keyes  will rotate the view.
  - z, and x move forward and backward.

  Mouse navigation:
  - Left button drag the mouse to rotate.
  - Right button drag the mouse to pan.

  Misc:
  - Hit 'r' to reset the view to what was on the command line.
  - Hit 'p' to dump the current view parameters in the command line format.

  - Example:
    ./GlViewer \
    --rig=/path/to/output/fused/rig_calibrated.json \
    --catalog=/path/to/output/fused/fused.json \
    --strip_files=/path/to/output/fused/fused_0.bin
  )";

DEFINE_string(catalog, "", "json file describing strip files");
DEFINE_string(strip_files, "", "comma-separated list of strip files");
DEFINE_int32(readahead, 3, "how many frames to read ahead");
DEFINE_string(rig, "", "path to rig .json file (required)");

static const float kEffectIncrement = 1; // meters per frame
static const float kEffectMax = 15; // meters
// Rig scene use a flipped coordinate system relative to canopy scene
// so we permute the coordinates on the way to the graphics pipeline
// match opengl's coordinate system.
static const Eigen::Matrix4f kPermutationMatrix(
    (Eigen::Matrix4f() << 1, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 1).finished());

class GlViewer : public GlWindow {
 private:
 public:
  RigScene scene;
  std::unique_ptr<AsyncLoader> asyncLoader;
  std::unique_ptr<VideoFile> videoFile;

  GlViewer() : GlWindow("GL viewer", 512, 512), scene(RigScene(FLAGS_rig)) {
    // Initialize the viewer
    CHECK_NE(FLAGS_strip_files, "");
    std::vector<std::string> disks;
    boost::split(disks, FLAGS_strip_files, boost::is_any_of(","));
    videoFile = std::make_unique<VideoFile>(FLAGS_catalog, disks);
    if (videoFile->frames.size() == 1) {
      videoFile->readBegin(scene);
      scene.subframes = videoFile->readEnd(scene);
    } else {
      for (int i = 0; i < FLAGS_readahead; ++i) {
        videoFile->readBegin(scene);
      }
    }
  }

  void keyPress(int key, int s, int action, int mods) override {
    GlWindow::keyPress(key, s, action, mods);

    if (action != GLFW_PRESS) {
      switch (key) {
        case GLFW_KEY_0:
          scene.debug = 0;
          break;

        case GLFW_KEY_1:
          scene.debug = 1;
          break;

        case GLFW_KEY_2:
          scene.debug = 2;
          break;

        case GLFW_KEY_3:
          scene.debug = 3;
          break;

        case GLFW_KEY_4:
          scene.debug = 4;
          break;

        case GLFW_KEY_5:
          scene.debug = 5;
          break;

        case GLFW_KEY_6:
          scene.debug = 6;
          break;

        case GLFW_KEY_7:
          scene.debug = 7;
          break;

        case GLFW_KEY_8:
          scene.debug = 8;
          break;

        case GLFW_KEY_9:
          scene.debug = 9;
          break;

        case GLFW_KEY_L:
          effectBegin();
          break;
      }
    }
  }

  void effectBegin() {
    scene.effect = kEffectIncrement;
  }

  void effectUpdate() {
    if (scene.effect != 0) {
      scene.effect += kEffectIncrement;
    }
    if (scene.effect > kEffectMax) {
      scene.effect = 0;
    }
  }

  void display() override {
    if (videoFile->frames.size() > 1) {
      scene.destroyFrame(scene.subframes);
      scene.subframes = videoFile->readEnd(scene);
      videoFile->readBegin(scene, true);
    }

    // Loop effect
    effectUpdate();

    // draw the scene
    scene.render(projection * transform.matrix() * kPermutationMatrix, 0, true, wireframe);

    // Let the read thread drain if when we finish
    if (done && asyncLoader) {
      asyncLoader->wait();
    }
  }
};

int main(int argc, char* argv[]) {
  FLAGS_stderrthreshold = 0;
  FLAGS_logtostderr = 0;

  system_util::initDep(argc, argv, kUsageMessage);

  CHECK_NE(FLAGS_rig, "");
  CHECK_NE(FLAGS_catalog, "");
  GlViewer glViewer;
  GlWindow::mainLoop();

  return EXIT_SUCCESS;
}


text:
#pragma once
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <vector>
#include <string>
#include <unordered_map>
#include <boost/shared_ptr.hpp>

#include "leveldb/db.h"
namespace util {

// An extension of google flags. It is a singleton that stores 1) google flags
// and 2) other lightweight global flags. Underlying data structure is map of
// string and string, similar to google::CommandLineFlagInfo.
class Context {
public:
  static Context& get_instance();

  int get_int32(std::string key);
  double get_double(std::string key);
  bool get_bool(std::string key);
  std::string get_string(std::string key);

  void set(std::string key, int value);
  void set(std::string key, double value);
  void set(std::string key, bool value);
  void set(std::string key, std::string value);

  static const int num_app_threads() { return get_instance().num_app_threads_; }
  static const int num_rows_per_table() { return get_instance().num_rows_per_table_; }

  static std::vector<boost::shared_ptr<leveldb::DB> >& test_dbs() {
    return get_instance().test_dbs_;
  }
  static boost::shared_ptr<leveldb::DB>& global_db(int id) {
    if (id == -1) {
      return get_instance().db_;
    } else if (id >= 0) {
      CHECK_GT(get_instance().test_dbs_.size(), id);
      return get_instance().test_dbs_[id];
    } else {
      LOG(FATAL) << "Invalid net id " << id
          << "; Valid net id is >= -1 and <= "
          << get_instance().test_dbs_.size() - 1;
    }
  }

private:
  // Private constructor. Store all the gflags values.
  Context();
  // Underlying data structure
  std::unordered_map<std::string, std::string> ctx_;

  int num_app_threads_;
  int num_rows_per_table_;
  // LEVELDB
  // different nets cannot use the same leveldb
  boost::shared_ptr<leveldb::DB> db_;
  std::vector<boost::shared_ptr<leveldb::DB> > test_dbs_;
};

}   // namespace util


text:
#include <CGAL/algorithm.h>
#include <CGAL/function_objects.h>
#include <vector>
#include <iostream>
#include <boost/functional.hpp>

int main()
{
  std::vector< int > v;
  v.push_back(3);
  v.push_back(5);
  v.push_back(2);
  std::cout << "min_odd = "
            << *CGAL::min_element_if(v.begin(),
                                     v.end(),
                                     CGAL::compose1_1(boost::bind2nd(std::greater< int >(), 0),
                                                      boost::bind2nd(std::modulus< int >(), 2)))
            << std::endl;
  return 0;
}

max_stars_repo_name: gaschler/cgal

max_issues_repo_name: gaschler/cgal

max_forks_repo_name: gaschler/cgal


text:
#include <decorator_sync.h>

#include <boost/interprocess/shared_memory_object.hpp>

using namespace boost::interprocess;

BT::DecoratorSync::DecoratorSync(std::string name, std::string link_name) : ControlNode::ControlNode(name)
{

    link_ = tick_engine.CreateSyncLink(link_name);
    tick_id_ = 0;

}

BT::ReturnStatus BT::DecoratorSync::Tick()
{
    if (children_nodes_[0]->get_type() == BT::ACTION_NODE)
    {
        throw std::string("DecoratorSync can only have actions as children");
    }
    if(link_->is_tick_id_ready(tick_id_))
    {
        BT::ReturnStatus status = children_nodes_[0]->TickSync(tick_id_);
        tick_id_++;
        if (status == BT::SUCCESS || status == BT::FAILURE)
        {
            set_status(status);
            return status;
        }
    }
    set_status(BT::RUNNING);
    return BT::RUNNING;
}

void BT::DecoratorSync::Halt()
{

    tick_id_ = 0;
    ControlNode::Halt();

}

int BT::DecoratorSync::DrawType()
{
    // Lock acquistion

    return BT::DECORATOR;
}

max_stars_repo_name: miccol/YARP-Behavior-Trees

max_issues_repo_name: miccol/YARP-Behavior-Trees

max_forks_repo_name: miccol/YARP-Behavior-Trees