//
//  replicates.cpp
//  cufflinks
//
//  Created by Cole Trapnell on 3/11/11.
//  Copyright 2011 Cole Trapnell. All rights reserved.
//

#include <boost/thread.hpp>

extern "C" {
#include "locfit/local.h"
}

#include <boost/random/gamma_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>

#include "replicates.h"
//#include "rounding.h"

#if ENABLE_THREADS
boost::mutex _locfit_lock;
#endif

MassDispersionModel::MassDispersionModel(const std::string& name,
                                         const std::vector<double>& scaled_compatible_mass_means,
                                         const std::vector<double>& scaled_compatible_variances,
                                         const std::vector<double>& scaled_mass_variances)
{
    _name = name;

    if (scaled_compatible_mass_means.size() != scaled_mass_variances.size())
    {
        fprintf (stderr, "Error: dispersion model table is malformed\n");
    }

    double last_val = 0;
    for (size_t i = 0; i < scaled_compatible_mass_means.size(); i++)
    {

        if (last_val > scaled_compatible_mass_means[i])
        {
            fprintf (stderr, "Error: DispersionModel input is malformed\n");
        }

        if ( i == 0 || last_val < scaled_compatible_mass_means[i])
        {
            _scaled_compatible_mass_means.push_back(scaled_compatible_mass_means[i]);
            _scaled_compatible_variances.push_back(scaled_compatible_variances[i]);
            _scaled_mass_variances.push_back(scaled_mass_variances[i]);
        }
        else
        {
            // skip this element if it's equal to what we've already seen
        }

        last_val = scaled_compatible_mass_means[i];
    }
}

double MassDispersionModel::scale_mass_variance(double scaled_mass) const
{
    if (scaled_mass <= 0)
        return 0.0;

    if (_scaled_compatible_mass_means.size() < 2 || _scaled_mass_variances.size() < 2)
    {
        return scaled_mass; // revert to poisson.
    }
    if (scaled_mass > _scaled_compatible_mass_means.back())
    {
        // extrapolate to the right
        // off the right end
        double x1_mean = _scaled_compatible_mass_means[_scaled_compatible_mass_means.size()-2];
        double x2_mean = _scaled_compatible_mass_means[_scaled_compatible_mass_means.size()-1];

        double y1_var = _scaled_mass_variances[_scaled_compatible_mass_means.size()-2];
        double y2_var = _scaled_mass_variances[_scaled_compatible_mass_means.size()-1];
        double slope = 0.0;
        if (x2_mean != x1_mean)
        {
            slope = (y2_var - y1_var) / (x2_mean-x1_mean);
        }
        else if (y1_var == y2_var)
        {
            assert (false); // should have a unique'd table
        }
        double mean_interp = _scaled_mass_variances[_scaled_compatible_mass_means.size()-1] -
        slope*(scaled_mass - _scaled_compatible_mass_means.size()-1);
        if (mean_interp < scaled_mass)
            mean_interp = scaled_mass;
        assert (!isnan(mean_interp) && !isinf(mean_interp));
        return mean_interp;
    }
    else if (scaled_mass < _scaled_compatible_mass_means.front())
    {
        // extrapolate to the left
        // off the left end?
        double x1_mean = _scaled_compatible_mass_means[0];
        double x2_mean = _scaled_compatible_mass_means[1];

        double y1_var = _scaled_mass_variances[0];
        double y2_var = _scaled_mass_variances[1];
        double slope = 0.0;
        if (x2_mean != x1_mean)
        {
            slope = (y2_var - y1_var) / (x2_mean-x1_mean);
        }
        else if (y1_var == y2_var)
        {
            assert (false); // should have a unique'd table
        }
        double mean_interp = _scaled_mass_variances[0] - slope*(_scaled_compatible_mass_means[0] - scaled_mass);
        if (mean_interp < scaled_mass)
            mean_interp = scaled_mass;

        assert (!isnan(mean_interp) && !isinf(mean_interp));
        return mean_interp;
    }

    vector<double>::const_iterator lb;
    lb = lower_bound(_scaled_compatible_mass_means.begin(),
                     _scaled_compatible_mass_means.end(),
                     scaled_mass);
    if (lb < _scaled_compatible_mass_means.end())
    {
        int d = lb - _scaled_compatible_mass_means.begin();
        if (*lb == scaled_mass || lb == _scaled_compatible_mass_means.begin())
        {
            double var = _scaled_mass_variances[d];
            if (var < scaled_mass) // revert to poisson if underdispersed
                var = scaled_mass;
            assert (!isnan(var) && !isinf(var));
            return var;
        }

        //in between two points on the scale.
        d--;

        if (d < 0)
        {
            fprintf(stderr, "ARG d < 0, d = %d \n", d);
        }

        if (d >= _scaled_compatible_mass_means.size())
        {
            fprintf(stderr, "ARG d >= _scaled_compatible_mass_means.size(), d = %d\n", d);
        }
        if (d >= _scaled_mass_variances.size())
        {
            fprintf(stderr, "ARG d >= _scaled_mass_variances.size(), d = %d\n", d);
        }

        double x1_mean = _scaled_compatible_mass_means[d];
        double x2_mean = _scaled_compatible_mass_means[d + 1];

        double y1_var = _scaled_mass_variances[d];
        double y2_var = _scaled_mass_variances[d + 1];
        double slope = 0.0;
        if (x2_mean != x1_mean)
        {
            slope = (y2_var - y1_var) / (x2_mean-x1_mean);
        }
        else if (y1_var == y2_var)
        {
            assert (false); // should have a unique'd table
        }
        double mean_interp = _scaled_mass_variances[d] + slope*(scaled_mass - _scaled_compatible_mass_means[d]);
        if (mean_interp < scaled_mass) // revert to poisson if underdispersed
            mean_interp = scaled_mass;

        assert (!isnan(mean_interp) && !isinf(mean_interp));
        return mean_interp;
    }
    else
    {
        assert (!isnan(scaled_mass) && !isinf(scaled_mass));
        return scaled_mass; // revert to poisson assumption
    }
}

MleErrorModel::MleErrorModel(const std::string& name,
                             const std::vector<double>& scaled_compatible_mass_means,
                             const std::vector<double>& scaled_mle_variances)
{
    _name = name;

    if (scaled_compatible_mass_means.size() != scaled_mle_variances.size())
    {
        fprintf (stderr, "Error: dispersion model table is malformed\n");
    }

    double last_val = 0;
    for (size_t i = 0; i < scaled_compatible_mass_means.size(); i++)
    {

        if (last_val > scaled_compatible_mass_means[i])
        {
            fprintf (stderr, "Error: MLEError input is malformed\n");
        }

        if ( i == 0 || last_val < scaled_compatible_mass_means[i])
        {
            _scaled_compatible_mass_means.push_back(scaled_compatible_mass_means[i]);
            _scaled_mle_variances.push_back(scaled_mle_variances[i]);
        }
        else
        {
            // skip this element if it's equal to what we've already seen
        }

        last_val = scaled_compatible_mass_means[i];
    }
}

double MleErrorModel::scale_mle_variance(double scaled_mass) const
{
    if (scaled_mass <= 0)
        return 0.0;

    if (_scaled_compatible_mass_means.size() < 2 || _scaled_mle_variances.size() < 2)
    {
        return 0; // revert to poisson.
    }
    if (scaled_mass > _scaled_compatible_mass_means.back())
    {
        return 0;
    }
    else if (scaled_mass < _scaled_compatible_mass_means.front())
    {
        return 0; // we won't add anything if we're out of the range of the table
    }

    vector<double>::const_iterator lb;
    lb = lower_bound(_scaled_compatible_mass_means.begin(),
                     _scaled_compatible_mass_means.end(),
                     scaled_mass);
    if (lb < _scaled_compatible_mass_means.end())
    {
        int d = lb - _scaled_compatible_mass_means.begin();
        if (*lb == scaled_mass || lb == _scaled_compatible_mass_means.begin())
        {
            double var = _scaled_mle_variances[d];
            assert (!isnan(var) && !isinf(var));
            if (var < 0)
                var = 0;
            return var;
        }

        //in between two points on the scale.
        d--;

        if (d < 0)
        {
            fprintf(stderr, "ARG d < 0, d = %d \n", d);
        }

        if (d >= _scaled_compatible_mass_means.size())
        {
            fprintf(stderr, "ARG d >= _scaled_compatible_mass_means.size(), d = %d\n", d);
        }
        if (d >= _scaled_mle_variances.size())
        {
            fprintf(stderr, "ARG d >= _scaled_mass_variances.size(), d = %d\n", d);
        }

        double x1_mean = _scaled_compatible_mass_means[d];
        double x2_mean = _scaled_compatible_mass_means[d + 1];

        double y1_var = _scaled_mle_variances[d];
        double y2_var = _scaled_mle_variances[d + 1];
        double slope = 0.0;
        if (x2_mean != x1_mean)
        {
            slope = (y2_var - y1_var) / (x2_mean-x1_mean);
        }
        else if (y1_var == y2_var)
        {
            assert (false); // should have a unique'd table
        }
        double mean_interp = _scaled_mle_variances[d] + slope*(scaled_mass - _scaled_compatible_mass_means[d]);
        if (mean_interp < 0)
            mean_interp = 0;

        assert (!isnan(mean_interp) && !isinf(mean_interp));
        return mean_interp;
    }
    else
    {
        assert (!isnan(scaled_mass) && !isinf(scaled_mass));
        return 0; // revert to poisson assumption
    }
}

void transform_counts_to_common_scale(const vector<double>& scale_factors,
                                      vector<LocusCountList>& sample_compatible_count_table)
{
    // Transform raw counts to the common scale
    for (size_t i = 0; i < sample_compatible_count_table.size(); ++i)
    {
        LocusCountList& p = sample_compatible_count_table[i];
        for (size_t j = 0; j < p.counts.size(); ++j)
        {
            assert (scale_factors.size() > j);
            p.counts[j] *= (1.0 / scale_factors[j]);
        }
    }
}

void calc_geometric_scaling_factors(const vector<LocusCountList>& sample_compatible_count_table,
                                    vector<double>& scale_factors)
{

    vector<double> log_geom_means(sample_compatible_count_table.size(), 0.0);

    for (size_t i = 0; i < sample_compatible_count_table.size(); ++i)
    {
        const LocusCountList& p = sample_compatible_count_table[i];

        for (size_t j = 0; j < p.counts.size(); ++j)
        {
            //assert (log_geom_means.size() > j);
            //if (floor(p.counts[j]) > 0)
            //{
                log_geom_means[i] += (1.0/p.counts.size()) * log(floor(p.counts[j]));
            //}

        }
        //log_geom_means[i] = pow(log_geom_means[i], 1.0/(double)p.counts.size());
    }

    for (size_t j = 0; j < scale_factors.size(); ++j)
    {
        vector<double> tmp_counts;
        for (size_t i = 0; i < sample_compatible_count_table.size(); ++i)
        {
            if (log_geom_means[i] && !isinf(log_geom_means[i]) && !isnan(log_geom_means[i]) && floor(sample_compatible_count_table[i].counts[j]))
            {
                double gm = (double)log(floor(sample_compatible_count_table[i].counts[j])) - log_geom_means[i];
                assert (!isinf(gm));
                tmp_counts.push_back(gm);
            }
        }
        sort(tmp_counts.begin(), tmp_counts.end());
        if (!tmp_counts.empty())
            scale_factors[j] = exp(tmp_counts[tmp_counts.size()/2]);
        else
            scale_factors[j] = 1.0;
    }
}

void calc_classic_fpkm_scaling_factors(const vector<LocusCountList>& sample_compatible_count_table,
                                       vector<double>& scale_factors)
{
    vector<double> total_counts(sample_compatible_count_table.size(), 0.0);

    for (size_t i = 0; i < sample_compatible_count_table.size(); ++i)
    {
        const LocusCountList& p = sample_compatible_count_table[i];

        for (size_t j = 0; j < p.counts.size(); ++j)
        {
            total_counts[j] += floor(p.counts[j]);
        }
    }

    double all_library_counts = accumulate(total_counts.begin(), total_counts.end(), 0.0);
    if (all_library_counts == 0.0)
    {
        for (size_t i = 0; i < scale_factors.size(); ++i)
        {
            scale_factors[i] = 1.0;
        }
        return;
    }
    for (size_t i = 0; i < scale_factors.size(); ++i)
    {
        scale_factors[i] = total_counts[i] / all_library_counts;
    }

    double avg_scaling_factor = accumulate(scale_factors.begin(), scale_factors.end(), 0.0);
    avg_scaling_factor /= scale_factors.size();

    for (size_t i = 0; i < scale_factors.size(); ++i)
    {
        scale_factors[i] = scale_factors[i] / avg_scaling_factor;
    }
}

void calc_estimated_absolute_scaling_factors(vector<boost::shared_ptr<ReadGroupProperties> > & all_read_groups,
                                             vector<double>& scale_factors)
{
    assert (scale_factors.size() == all_read_groups.size());
    for (size_t i = 0; i < scale_factors.size(); ++i)
    {
        double med_cov = all_read_groups[i]->mode_transcript_coverage();
        if (med_cov == 0)
        {
            scale_factors[i] = 0.0;
        }
        else if (med_cov == -1)
        {
            fprintf(stderr, "Error: estimated-absolute requires pre-calculated CXB files\n");
            exit(1);
        }

        scale_factors[i] =  med_cov * 1000000000 / (all_read_groups[i]->normalized_map_mass()) ;
    }

//
//    void adjust_fpkms_by_median_coverage(const vector<vector<double > >& median_coverage,
//                                         FPKMTrackingTable& tracking)
//    {
//        for (FPKMTrackingTable::iterator itr = tracking.begin(); itr != tracking.end(); ++itr)
//        {
//            if (itr != tracking.end())
//            {
//                FPKMTracking& track = itr->second;
//                vector<FPKMContext>& fpkms = track.fpkm_series;
//
//                for (size_t i = 0; i < fpkms.size(); ++i)
//                {
//                    for (size_t j = 0; j != fpkms[i].tracking_info_per_rep.size();
//                         ++j)
//                    {
//                        double& FPKM = fpkms[i].tracking_info_per_rep[j].fpkm;
//                        FPKM /= median_coverage[i][j];
//                        FPKM *= fpkms[i].tracking_info_per_rep[j].rg_props->normalized_map_mass();
//                        FPKM /= 1000000000;
//                    }
//                }
//            }
//        }
//    }
//
//
}

void calc_quartile_scaling_factors(const vector<LocusCountList>& sample_compatible_count_table,
                                   vector<double>& scale_factors)
{

    if (sample_compatible_count_table.empty())
        return;

    vector<double> upper_quartiles(sample_compatible_count_table.front().counts.size(), 0.0);
    vector<double> total_common_masses(sample_compatible_count_table.front().counts.size(), 0.0);

    for (size_t i = 0; i < sample_compatible_count_table.front().counts.size(); ++i)
    {
        vector<double> common_scaled_counts;
        double total_common = 0.0;

        for (size_t j = 0; j < sample_compatible_count_table.size(); ++j)
        {

            //boost::shared_ptr<ReadGroupProperties> rg = bundle_factories[fac_idx];
            //double scaled_mass = scale_factors[fac_idx] * rg->total_map_mass();

            total_common += sample_compatible_count_table[j].counts[i];
            common_scaled_counts.push_back(sample_compatible_count_table[j].counts[i]);
        }

        sort(common_scaled_counts.begin(), common_scaled_counts.end());
        if (common_scaled_counts.empty())
            continue;

        int upper_quart_index = common_scaled_counts.size() * 0.75;
        double upper_quart_count = common_scaled_counts[upper_quart_index];
        upper_quartiles[i] = upper_quart_count;
        total_common_masses[i] = total_common;
    }

    long double total_mass = accumulate(total_common_masses.begin(), total_common_masses.end(), 0.0);
    long double total_norm_mass = accumulate(upper_quartiles.begin(), upper_quartiles.end(), 0.0);

    for (size_t i = 0; i < sample_compatible_count_table.front().counts.size(); ++i)
    {
        if (total_mass > 0)
        {
            double scaling_factor = upper_quartiles[i];
            scaling_factor /= (total_norm_mass / upper_quartiles.size());
            scale_factors[i] = scaling_factor;
        }
        else
        {
            scale_factors[i] = 1.0;
        }
    }
}

void calc_tmm_scaling_factors(const vector<LocusCountList>& sample_compatible_count_table,
                              vector<double>& scale_factors)
{
    scale_factors = vector<double>(sample_compatible_count_table.size(), 1.0);
}

static const int min_loci_for_fitting = 30;

struct SCVInterpolator
{
    void add_scv_pair(double est_scv, double true_scv)
    {
        true_scvs.push_back(true_scv);
        est_scvs.push_back(est_scv);
    }

    void finalize()
    {
        vector<pair<double, double> > pv;
        for (size_t i =0; i < true_scvs.size(); ++i)
        {
            pv.push_back(make_pair(est_scvs[i], true_scvs[i]));
        }
        sort(pv.begin(), pv.end());
    }

    // This was built from the dispersion model interpolator - we should refactor these
    // into a single routine.
    double interpolate_scv(double est_scv)
    {
//        if (est_scv <= 0)
//            return 0.0;

        if (est_scvs.size() < 2 || true_scvs.size() < 2)
        {
            return est_scv; // revert to poisson.
        }
        if (est_scv > est_scvs.back())
        {
            //fprintf(stderr, "Warning: extrapolating to the right\n");
            // extrapolate to the right
            // off the right end
            double x1_mean = est_scvs[est_scvs.size()-2];
            double x2_mean = est_scvs[est_scvs.size()-1];

            double y1_var = true_scvs[est_scvs.size()-2];
            double y2_var = true_scvs[est_scvs.size()-1];
            double slope = 0.0;
            if (x2_mean != x1_mean)
            {
                slope = (y2_var - y1_var) / (x2_mean-x1_mean);
            }
            else if (y1_var == y2_var)
            {
                assert (false); // should have a unique'd table
            }
            double mean_interp = true_scvs[est_scvs.size()-1] -
                slope*(est_scv - est_scvs.size()-1);
//            if (mean_interp < est_scv)
//                mean_interp = est_scv;
            assert (!isnan(mean_interp) && !isinf(mean_interp));
            return mean_interp;
        }
        else if (est_scv < est_scvs.front())
        {
            //fprintf(stderr, "Warning: extrapolating to the left\n");

            // If we're extrapolating to the left, our fit is too coarse, but
            // that probably means we don't need SCV bias correction at all.
            return est_scv;
        }

        vector<double>::const_iterator lb;
        lb = lower_bound(est_scvs.begin(),
                         est_scvs.end(),
                         est_scv);
        if (lb < est_scvs.end())
        {
            int d = lb - est_scvs.begin();
            if (*lb == est_scv || lb == est_scvs.begin())
            {
                double var = true_scvs[d];
//                if (var < est_scv) // revert to poisson if underdispersed
//                    var = est_scv;
                assert (!isnan(var) && !isinf(var));
                return var;
            }

            //in between two points on the scale.
            d--;

            if (d < 0)
            {
                fprintf(stderr, "ARG d < 0, d = %d \n", d);
            }

            if (d >= est_scvs.size())
            {
                fprintf(stderr, "ARG d >= est_scvs.size(), d = %d\n", d);
            }
            if (d >= true_scvs.size())
            {
                fprintf(stderr, "ARG d >= true_scvs.size(), d = %d\n", d);
            }

            double x1_mean = est_scvs[d];
            double x2_mean = est_scvs[d + 1];

            double y1_var = true_scvs[d];
            double y2_var = true_scvs[d + 1];
            double slope = 0.0;
            if (x2_mean != x1_mean)
            {
                slope = (y2_var - y1_var) / (x2_mean-x1_mean);
            }
            else if (y1_var == y2_var)
            {
                fprintf(stderr, "Warning: SCV table does not have unique keys\n!");
                assert (false); // should have a unique'd table
            }
            double mean_interp = true_scvs[d] + slope*(est_scv - est_scvs[d]);
//            if (mean_interp < est_scv) // revert to poisson if underdispersed
//                mean_interp = est_scv;

            assert (!isnan(mean_interp) && !isinf(mean_interp));
            return mean_interp;
        }
        else
        {
            assert (!isnan(est_scv) && !isinf(est_scv));
            return est_scv; // revert to poisson assumption
        }

        return est_scv;
    }

private:
    vector<double> true_scvs;
    vector<double> est_scvs;
};

void build_scv_correction_fit(int nreps, int ngenes, int mean_count, SCVInterpolator& true_to_est_scv_table)
{
    setuplf();

    vector<pair<double, double> > alpha_vs_scv;

    boost::mt19937 rng;
    vector<boost::random::negative_binomial_distribution<int, double> > nb_gens;

    vector<double> alpha_range;
    for(double a = 0.0002; a < 2.0; a += 0.0002)
    {
        alpha_range.push_back(a);
    }

    for (double a = 2; a < 100.0; a += 1)
    {
        alpha_range.push_back(a);
    }

    BOOST_FOREACH (double alpha, alpha_range)
    {
        double k = 1.0/alpha;
        double p = k / (k + mean_count);
        double r = (mean_count * p) / (1-p);

        //boost::random::negative_binomial_distribution<int, double> nb(r, p);

        boost::random::gamma_distribution<double> gamma(r, (1-p)/p);

        vector<double> scvs_for_alpha;
        vector<double> draws;
        for (size_t i = 0; i < ngenes; ++i)
        {
            LocusCountList locus_count("", nreps, 1, vector<string>(), vector<string>());
            for (size_t rep_idx = 0; rep_idx < nreps; ++rep_idx)
            {
                double gamma_draw = gamma(rng);
                if (gamma_draw == 0)
                {
                    locus_count.counts[rep_idx] = 0;
                    draws.push_back(0);
                }
                else
                {
                    boost::random::poisson_distribution<long, double> poisson(gamma_draw);
                    locus_count.counts[rep_idx] = poisson(rng);
                    draws.push_back(locus_count.counts[rep_idx]);
                    //fprintf(stderr, "%lg\t", locus_count.counts[rep_idx]);
                }
            }

            double mean = accumulate(locus_count.counts.begin(), locus_count.counts.end(), 0.0);
            if (mean == 0)
                continue;
            mean /= locus_count.counts.size();
            double var = 0.0;
            BOOST_FOREACH(double c,  locus_count.counts)
            {
                var += (c-mean)*(c-mean);
            }
            var /= locus_count.counts.size();
            var *= locus_count.counts.size() / (locus_count.counts.size() - 1);

            double scv = var / (mean*mean);
            scvs_for_alpha.push_back(scv);
            //fprintf(stderr, " : mean = %lg, var = %lg, scv = %lg\n", mean, var, scv);

            //fprintf(stderr, "\n");
        }

        double mean = accumulate(draws.begin(), draws.end(), 0.0);
        mean /= draws.size();
        double var = 0.0;
        BOOST_FOREACH(int c,  draws)
        {
            var += (c - mean)*(c-mean);
        }
        var /= draws.size();
        var *= draws.size() / (draws.size() - 1);

        //fprintf(stderr, "##########\n");
        //fprintf(stderr, "mean = %lf, var = %lg\n", mean, var);
        if (scvs_for_alpha.size() > 0)
        {
            double mean_scv = accumulate(scvs_for_alpha.begin(),scvs_for_alpha.end(), 0.0);
            mean_scv /= scvs_for_alpha.size();
            //fprintf(stderr, "alpha = %lg scv = %lg\n", alpha, mean_scv);
            alpha_vs_scv.push_back(make_pair(alpha, mean_scv));
        }
    }

    //fprintf(stderr, "$$$$$$$$$\n");

    //sort (alpha_range.begin(), alpha_range.end());

    char namebuf[256];
    sprintf(namebuf, "trueSCV");
    vari* cm = createvar(namebuf,STREGULAR,alpha_vs_scv.size(),VDOUBLE);
    for (size_t i = 0; i < alpha_vs_scv.size(); ++i)
    {
        cm->dpr[i] = alpha_vs_scv[i].first;
    }

    sprintf(namebuf, "estSCV");
    vari* cv = createvar(namebuf,STREGULAR,alpha_vs_scv.size(),VDOUBLE);
    for (size_t i = 0; i < alpha_vs_scv.size(); ++i)
    {
        cv->dpr[i] = alpha_vs_scv[i].second;
    }

    char locfit_cmd[2048];
    sprintf(locfit_cmd, "locfit trueSCV~estSCV");

    locfit_dispatch(locfit_cmd);

    sprintf(namebuf, "domainSCV");
    vari* cd = createvar(namebuf,STREGULAR,alpha_vs_scv.size(),VDOUBLE);
    for (size_t i = 0; i < alpha_vs_scv.size(); ++i)
    {
        cd->dpr[i] = alpha_vs_scv[i].second;
    }

    sprintf(locfit_cmd, "fittedSCV=predict domainSCV");
    locfit_dispatch(locfit_cmd);

    int n = 0;
    sprintf(namebuf, "fittedSCV");
    vari* cp = findvar(namebuf, 1, &n);
    assert(cp != NULL);

    for (size_t i = 0; i < cp->n; ++i)
    {
        //fprintf(stderr, "%lg\t%lg\n",alpha_range[i], cp->dpr[i]);
        true_to_est_scv_table.add_scv_pair(alpha_range[i], cp->dpr[i]);
    }
    true_to_est_scv_table.finalize();
}

void calculate_count_means_and_vars(const vector<LocusCountList>& sample_compatible_count_table,
                                    vector<pair<double, double> >& means_and_vars)
{

    for (size_t i = 0; i < sample_compatible_count_table.size(); ++i)
    {
        const LocusCountList& p = sample_compatible_count_table[i];
        double mean = accumulate(p.counts.begin(), p.counts.end(), 0.0);
        if (mean > 0.0 && p.counts.size() > 0)
            mean /= p.counts.size();

        double var = 0.0;
        double num_non_zero = 0;
        BOOST_FOREACH (double d, p.counts)
        {
            if (d > 0)
                num_non_zero++;
            var += (d - mean) * (d - mean);
        }
        if (var > 0.0 && p.counts.size())
        {
            var /= p.counts.size();
            var *= p.counts.size() / (p.counts.size() - 1);
        }
        means_and_vars.push_back(make_pair(mean, var));
    }
}

boost::shared_ptr<MassDispersionModel>
fit_dispersion_model_helper(const string& condition_name,
                            const vector<double>& scale_factors,
                            const vector<LocusCountList>& sample_compatible_count_table)
{
    vector<pair<double, double> > compatible_means_and_vars;

    SCVInterpolator true_to_est_scv_table;

    int num_samples = sample_compatible_count_table.front().counts.size();
    if (no_scv_correction == false)
    {
        build_scv_correction_fit(num_samples, 10000, 100000, true_to_est_scv_table);
    }

    setuplf();

    calculate_count_means_and_vars(sample_compatible_count_table, compatible_means_and_vars);

    sort(compatible_means_and_vars.begin(), compatible_means_and_vars.end());

    vector<double> compatible_count_means;
    vector<double> raw_variances;

    for(size_t i = 0; i < compatible_means_and_vars.size(); ++i)
    {
        if (compatible_means_and_vars[i].first > 0 && compatible_means_and_vars[i].second > 0.0)
        {
            compatible_count_means.push_back(compatible_means_and_vars[i].first);
            raw_variances.push_back(compatible_means_and_vars[i].second);
        }
    }

    if (compatible_count_means.size() < min_loci_for_fitting)
    {
        boost::shared_ptr<MassDispersionModel> disperser;
        disperser = boost::shared_ptr<MassDispersionModel>(new PoissonDispersionModel(condition_name));

        return disperser;
    }

    vector<double> fitted_values;

    // WARNING: locfit doesn't like undescores - need camel case for
    // variable names

    char namebuf[256];
    sprintf(namebuf, "countMeans");
    vari* cm = createvar(namebuf,STREGULAR,compatible_count_means.size(),VDOUBLE);
    for (size_t i = 0; i < compatible_count_means.size(); ++i)
    {
        cm->dpr[i] = log(compatible_count_means[i]);
    }

    //sprintf(namebuf, "countSCV");
    sprintf(namebuf, "countVariances");
    vari* cv = createvar(namebuf,STREGULAR,raw_variances.size(),VDOUBLE);
    for (size_t i = 0; i < raw_variances.size(); ++i)
    {
        cv->dpr[i] = raw_variances[i];
        //cv->dpr[i] = raw_scvs[i];
    }

    char locfit_cmd[2048];
    //sprintf(locfit_cmd, "locfit countVariances~countMeans family=gamma");
    sprintf(locfit_cmd, "locfit countVariances~countMeans family=gamma");

    locfit_dispatch(locfit_cmd);

    sprintf(locfit_cmd, "fittedVars=predict countMeans");
    locfit_dispatch(locfit_cmd);

    //sprintf(locfit_cmd, "prfit x fhat h nlx");
    //locfit_dispatch(locfit_cmd);

    double xim = 0;
    BOOST_FOREACH(double s, scale_factors)
    {
        if (s)
            xim += 1.0 / s;
    }
    xim /= scale_factors.size();

    int n = 0;
    sprintf(namebuf, "fittedVars");
    vari* cp = findvar(namebuf, 1, &n);
    assert(cp != NULL);
    for (size_t i = 0; i < cp->n; ++i)
    {
//        if (cp->dpr[i] >= 0)
//        {
            double mean = exp(cm->dpr[i]);
            double fitted_scv = (cp->dpr[i] - mean) / (mean * mean);
            double corrected_scv = true_to_est_scv_table.interpolate_scv(fitted_scv);
            double corrected_variance = mean + (corrected_scv * (mean * mean));
            double uncorrected_variance = mean + (fitted_scv * (mean * mean));
            //fitted_values.push_back(mean + (cp->dpr[i] - xim * mean));
            if (no_scv_correction == false && corrected_variance > uncorrected_variance)
                fitted_values.push_back(corrected_variance);
            else if (uncorrected_variance > 0)
                fitted_values.push_back(uncorrected_variance);
            else
                fitted_values.push_back(compatible_count_means[i]);

//        }
//        else
//        {
//            fitted_values.push_back(compatible_count_means[i]);
//        }
    }

    boost::shared_ptr<MassDispersionModel> disperser;
    disperser = boost::shared_ptr<MassDispersionModel>(new MassDispersionModel(condition_name, compatible_count_means, raw_variances, fitted_values));
    if (dispersion_method == POISSON)
        disperser = boost::shared_ptr<MassDispersionModel>(new PoissonDispersionModel(condition_name));

//    for (map<string, pair<double, double> >::iterator itr = labeled_mv_table.begin();
//         itr != labeled_mv_table.end();
//         ++itr)
//    {
//        string label = itr->first;
//        disperser->set_compatible_mean_and_var(itr->first, itr->second);
//    }

    return disperser;
}

boost::shared_ptr<MassDispersionModel>
fit_dispersion_model(const string& condition_name,
                     const vector<double>& scale_factors,
                     const vector<LocusCountList>& sample_compatible_count_table)
{
//
//#if ENABLE_THREADS
//	boost::mutex::scoped_lock lock(_locfit_lock);
//#endif
    for (size_t i = 0; i < sample_compatible_count_table.size(); ++i)
    {
        if (sample_compatible_count_table[i].counts.size() <= 1)
        {
            // only one replicate - no point in fitting variance
            return boost::shared_ptr<MassDispersionModel>(new PoissonDispersionModel(condition_name));
        }
    }
#if ENABLE_THREADS
    _locfit_lock.lock();
#endif

    ProgressBar p_bar("Modeling fragment count overdispersion.",0);

    int max_transcripts = 0;
    BOOST_FOREACH(const LocusCountList& L, sample_compatible_count_table)
    {
        if (L.num_transcripts > max_transcripts)
        {
            max_transcripts = L.num_transcripts;
        }
    }

    boost::shared_ptr<MassDispersionModel>  model = fit_dispersion_model_helper(condition_name, scale_factors, sample_compatible_count_table);

#if ENABLE_THREADS
    _locfit_lock.unlock();
#endif
    return model;
}

void build_norm_table(const vector<LocusCountList>& full_count_table,
                      boost::shared_ptr<const map<string, LibNormStandards> > normalizing_standards,
                      vector<LocusCountList>& norm_table)
{
    // If we're using housekeeping genes or spike-in controls, select the rows we'll be using from the full count table.
    if (normalizing_standards)
    {
        for (size_t i = 0; i < full_count_table.size(); ++i)
        {
            const vector<string>& gene_ids = full_count_table[i].gene_ids;
            const vector<string>& gene_short_names = full_count_table[i].gene_short_names;

            // If the row has an ID that's in the table, take it.
            map<string, LibNormStandards>::const_iterator g_id_itr = normalizing_standards->end();
            map<string, LibNormStandards>::const_iterator g_name_itr = normalizing_standards->end();

            for (size_t j = 0; j < gene_ids.size(); ++j)
            {
                g_id_itr = normalizing_standards->find(gene_ids[j]);
                if (g_id_itr != normalizing_standards->end())
                {
                    break;
                }
            }

            if (g_id_itr != normalizing_standards->end())
            {
                norm_table.push_back(full_count_table[i]);
                continue;
            }

            for (size_t j = 0; j < gene_short_names.size(); ++j)
            {
                g_name_itr = normalizing_standards->find(gene_short_names[j]);
                if (g_name_itr != normalizing_standards->end())
                {
                    break;
                }
            }

            if (g_name_itr != normalizing_standards->end())
            {
                norm_table.push_back(full_count_table[i]);
                continue;
            }

        }
    }
    else // otherwise, just take all rows.
    {
        norm_table = full_count_table;
    }
}

void normalize_counts(vector<boost::shared_ptr<ReadGroupProperties> > & all_read_groups)
{
    vector<LocusCountList> sample_compatible_count_table;
    vector<LocusCountList> sample_total_count_table;

    for (size_t i = 0; i < all_read_groups.size(); ++i)
    {
        boost::shared_ptr<ReadGroupProperties> rg_props = all_read_groups[i];
        const vector<LocusCount>& raw_compatible_counts = rg_props->raw_compatible_counts();
        const vector<LocusCount>& raw_total_counts = rg_props->raw_total_counts();

        for (size_t j = 0; j < raw_compatible_counts.size(); ++j)
        {
            if (sample_compatible_count_table.size() == j)
            {
                const string& locus_id = raw_compatible_counts[j].locus_desc;
                int num_transcripts = raw_compatible_counts[j].num_transcripts;

                const vector<string>& gene_ids = raw_compatible_counts[j].gene_ids;
                const vector<string>& gene_short_names = raw_compatible_counts[j].gene_short_names;

                sample_compatible_count_table.push_back(LocusCountList(locus_id,all_read_groups.size(), num_transcripts, gene_ids, gene_short_names));
                sample_total_count_table.push_back(LocusCountList(locus_id,all_read_groups.size(), num_transcripts, gene_ids, gene_short_names));
            }
            double scaled = raw_compatible_counts[j].count;
            //sample_compatible_count_table[j].counts[i] = scaled * unscaling_factor;
            sample_compatible_count_table[j].counts[i] = floor(scaled);
            sample_total_count_table[j].counts[i] = floor(raw_total_counts[j].count);

            assert(sample_compatible_count_table[j].counts[i] >= 0 && !isinf(sample_compatible_count_table[j].counts[i]));
        }
    }

    vector<double> scale_factors(all_read_groups.size(), 0.0);

    vector<LocusCountList> norm_table;

    if (use_compat_mass)
    {
        build_norm_table(sample_compatible_count_table, lib_norm_standards, norm_table);
    }
    else // use_total_mass
    {
        assert(use_total_mass);
        build_norm_table(sample_total_count_table, lib_norm_standards, norm_table);
    }

    if (lib_norm_method == GEOMETRIC)
    {
        calc_geometric_scaling_factors(norm_table, scale_factors);
    }
    else if (lib_norm_method == CLASSIC_FPKM)
    {
        calc_classic_fpkm_scaling_factors(norm_table, scale_factors);
    }
    else if (lib_norm_method == QUARTILE)
    {
        calc_quartile_scaling_factors(norm_table, scale_factors);
    }
    else if (lib_norm_method == TMM)
    {
        calc_tmm_scaling_factors(norm_table, scale_factors);
    }
    else if (lib_norm_method == ESTIMATED_ABSOLUTE)
    {
        calc_estimated_absolute_scaling_factors(all_read_groups, scale_factors);
    }
    else
    {
        assert (false);
    }

    for (size_t i = 0; i < all_read_groups.size(); ++i)
    {
        boost::shared_ptr<ReadGroupProperties> rg_props = all_read_groups[i];
        rg_props->internal_scale_factor(scale_factors[i]);
    }

    assert(sample_compatible_count_table.size() == sample_total_count_table.size());

    // Transform raw counts to the common scale
    for (size_t i = 0; i < sample_compatible_count_table.size(); ++i)
    {
        LocusCountList& p = sample_compatible_count_table[i];
        for (size_t j = 0; j < p.counts.size(); ++j)
        {
            assert (scale_factors.size() > j);
            p.counts[j] *= (1.0 / scale_factors[j]);
        }

        LocusCountList& t = sample_total_count_table[i];
        for (size_t j = 0; j < t.counts.size(); ++j)
        {
            assert (scale_factors.size() > j);
            t.counts[j] *= (1.0 / scale_factors[j]);
        }
    }

    for (size_t i = 0; i < all_read_groups.size(); ++i)
    {
        boost::shared_ptr<ReadGroupProperties> rg_props = all_read_groups[i];
        vector<LocusCount> scaled_compatible_counts;
        for (size_t j = 0; j < sample_compatible_count_table.size(); ++j)
        {
            string& locus_id = sample_compatible_count_table[j].locus_desc;
            double count = sample_compatible_count_table[j].counts[i];
            int num_transcripts = sample_compatible_count_table[j].num_transcripts;

            const vector<string>& gids = sample_compatible_count_table[j].gene_ids;
            const vector<string>& gnms = sample_compatible_count_table[j].gene_short_names;

            LocusCount locus_count(locus_id, count, num_transcripts, gids, gnms);
            scaled_compatible_counts.push_back(locus_count);
        }
        rg_props->common_scale_compatible_counts(scaled_compatible_counts);
    }

    for (size_t i = 0; i < all_read_groups.size(); ++i)
    {
        boost::shared_ptr<ReadGroupProperties> rg_props = all_read_groups[i];
        vector<LocusCount> scaled_total_counts;
        for (size_t j = 0; j < sample_total_count_table.size(); ++j)
        {
            string& locus_id = sample_total_count_table[j].locus_desc;
            double count = sample_total_count_table[j].counts[i];
            int num_transcripts = sample_total_count_table[j].num_transcripts;

            const vector<string>& gids = sample_total_count_table[j].gene_ids;
            const vector<string>& gnms = sample_total_count_table[j].gene_short_names;

            LocusCount locus_count(locus_id, count, num_transcripts, gids, gnms);
            scaled_total_counts.push_back(locus_count);
        }
        rg_props->common_scale_total_counts(scaled_total_counts);
    }

    double avg_total_common_scaled_count = 0.0;

    for (size_t fac_idx = 0; fac_idx < all_read_groups.size(); ++fac_idx)
    {
        double total_common = 0.0;
        if (use_compat_mass)
        {
            for (size_t j = 0; j < sample_compatible_count_table.size(); ++j)
            {
                total_common += sample_compatible_count_table[j].counts[fac_idx];
            }
        }
        else
        {
            for (size_t j = 0; j < sample_compatible_count_table.size(); ++j)
            {
                total_common += sample_total_count_table[j].counts[fac_idx];
            }

        }

        avg_total_common_scaled_count += (1.0/all_read_groups.size()) * total_common;
    }

    BOOST_FOREACH(boost::shared_ptr<ReadGroupProperties> rg, all_read_groups)
    {
        rg->normalized_map_mass(avg_total_common_scaled_count);
    }
}


#ifndef __abstract_MSSMNoFV_onshell_physical_gm2calc_1_3_0_hpp__
#define __abstract_MSSMNoFV_onshell_physical_gm2calc_1_3_0_hpp__

#include <cstddef>
#include <iostream>
#include <ostream>
#include <Eigen/Core>
#include "gambit/Backends/abstractbase.hpp"
#include "forward_decls_abstract_classes.hpp"
#include "forward_decls_wrapper_classes.hpp"

#include "identification.hpp"

namespace CAT_3(BACKENDNAME,_,SAFE_VERSION)
{

   namespace gm2calc
   {
      class Abstract_MSSMNoFV_onshell_physical : public virtual AbstractBase
      {
         public:

            virtual void clear() =0;

            virtual void convert_to_hk() =0;

            virtual void convert_to_slha() =0;

            virtual void print(::std::basic_ostream<char, std::char_traits<char> >&) const =0;

            virtual double& MVG_ref__BOSS() =0;

            virtual double& MGlu_ref__BOSS() =0;

            virtual double& MVP_ref__BOSS() =0;

            virtual double& MVZ_ref__BOSS() =0;

            virtual double& MFd_ref__BOSS() =0;

            virtual double& MFs_ref__BOSS() =0;

            virtual double& MFb_ref__BOSS() =0;

            virtual double& MFu_ref__BOSS() =0;

            virtual double& MFc_ref__BOSS() =0;

            virtual double& MFt_ref__BOSS() =0;

            virtual double& MFve_ref__BOSS() =0;

            virtual double& MFvm_ref__BOSS() =0;

            virtual double& MFvt_ref__BOSS() =0;

            virtual double& MFe_ref__BOSS() =0;

            virtual double& MFm_ref__BOSS() =0;

            virtual double& MFtau_ref__BOSS() =0;

            virtual double& MSveL_ref__BOSS() =0;

            virtual double& MSvmL_ref__BOSS() =0;

            virtual double& MSvtL_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MSd_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MSu_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MSe_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MSm_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MStau_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MSs_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MSc_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MSb_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MSt_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& Mhh_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MAh_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MHpm_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 4, 1, 0, 4, 1>& MChi_ref__BOSS() =0;

            virtual ::Eigen::Array<double, 2, 1, 0, 2, 1>& MCha_ref__BOSS() =0;

            virtual double& MVWm_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZD_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZU_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZE_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZM_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZTau_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZS_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZC_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZB_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZT_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZH_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZA_ref__BOSS() =0;

            virtual ::Eigen::Matrix<double, 2, 2, 0, 2, 2>& ZP_ref__BOSS() =0;

            virtual ::Eigen::Matrix<std::complex<double>, 4, 4, 0, 4, 4>& ZN_ref__BOSS() =0;

            virtual ::Eigen::Matrix<std::complex<double>, 2, 2, 0, 2, 2>& UM_ref__BOSS() =0;

            virtual ::Eigen::Matrix<std::complex<double>, 2, 2, 0, 2, 2>& UP_ref__BOSS() =0;

         public:
            virtual void pointer_assign__BOSS(Abstract_MSSMNoFV_onshell_physical*) =0;
            virtual Abstract_MSSMNoFV_onshell_physical* pointer_copy__BOSS() =0;

         private:
            MSSMNoFV_onshell_physical* wptr;
            bool delete_wrapper;
         public:
            MSSMNoFV_onshell_physical* get_wptr() { return wptr; }
            void set_wptr(MSSMNoFV_onshell_physical* wptr_in) { wptr = wptr_in; }
            bool get_delete_wrapper() { return delete_wrapper; }
            void set_delete_wrapper(bool del_wrp_in) { delete_wrapper = del_wrp_in; }

         public:
            Abstract_MSSMNoFV_onshell_physical()
            {
               wptr = 0;
               delete_wrapper = false;
            }

            Abstract_MSSMNoFV_onshell_physical(const Abstract_MSSMNoFV_onshell_physical&)
            {
               wptr = 0;
               delete_wrapper = false;
            }

            Abstract_MSSMNoFV_onshell_physical& operator=(const Abstract_MSSMNoFV_onshell_physical&) { return *this; }

            virtual void init_wrapper() =0;

            MSSMNoFV_onshell_physical* get_init_wptr()
            {
               init_wrapper();
               return wptr;
            }

            MSSMNoFV_onshell_physical& get_init_wref()
            {
               init_wrapper();
               return *wptr;
            }

            virtual ~Abstract_MSSMNoFV_onshell_physical() =0;
      };
   }

}

#include "gambit/Backends/backend_undefs.hpp"

#endif /* __abstract_MSSMNoFV_onshell_physical_gm2calc_1_3_0_hpp__ */


#include <numeric>
#include <boost/algorithm/string.hpp>
#include <regex>

#include "PositionStringParser.hpp"
#include "Board.hpp"

Board PositionStringParser::ParsePositionString(const std::string& positionString)
{
	SetPosition(positionString);

	return Board(
		toMove,
		castlingRights,
		remainingPlayers,
		halfMoveClock,
		fullMoveNumber,
		rPawns,
		rKnights,
		rBishops,
		rRooks,
		rQueens,
		rKing,
		bPawns,
		bKnights,
		bBishops,
		bRooks,
		bQueens,
		bKing,
		yPawns,
		yKnights,
		yBishops,
		yRooks,
		yQueens,
		yKing,
		gPawns,
		gKnights,
		gBishops,
		gRooks,
		gQueens,
		gKing);
}

std::string PositionStringParser::GetPositionString(const Board& board) const
{
	return std::string();
}

void PositionStringParser::SetPosition(const std::string& positionString)
{
	if (positionString.empty())
	{
		throw;
	}

	std::vector<std::string> slashTokens;

	boost::split(slashTokens, positionString, [](char c) {return c == '/'; });

	if (slashTokens.size() != SLASH_DELIMITED_TOKENS)
	{
		throw;
	}

	std::vector<std::string> emptySpaceTokens;

	boost::split(emptySpaceTokens, slashTokens.back(), [](char c) {return c == ' '; });

	if (emptySpaceTokens.size() != LAST_TOKEN_TOKENS)
	{
		throw;
	}

	slashTokens.pop_back();

	std::vector<std::string> positionTokens(slashTokens);

	positionTokens.push_back(emptySpaceTokens.front());

	std::regex regex("(\\d+)|(rP)|(rN)|(rB)|(rR)|(rQ)|(rq)|(rK)|(bP)|(bN)|(bB)|(bR)|(bQ)|(bq)|(bK)|(yP)|(yN)|(yB)|(yR)|(yQ)|(yq)|(yK)|(gP)|(gN)|(gB)|(gR)|(gQ)|(gq)|(gK)");

	int position = Board::TOTAL_SQUARES;

	for (auto& token : positionTokens)
	{
		std::vector<int> squares;
		std::vector<std::string> pieces;
		std::smatch match;
		std::string matchable(token);

		while (std::regex_search(matchable, match, regex))
		{
			std::string matchString = match[0].str();
			bool isNumber =
				!matchString.empty() &&
				std::find_if(
					matchString.cbegin(),
					matchString.cend(),
					[](char c) { return !std::isdigit(c); }) == matchString.cend();

			if (isNumber)
			{
				int number = std::stoi(match[0]);
				position -= number;

				if (number <= 0 || number > Board::TOTAL_SQUARES)
				{
					throw;
				}

				squares.push_back(number);
			}
			else
			{
				position--;

				SetSquare(matchString, position);

				pieces.push_back(matchString);
			}

			matchable = match.suffix();
		}

		int totalOccupancy = std::accumulate(squares.cbegin(), squares.cend(), 0) + pieces.size();

		if (totalOccupancy != Board::SQUARES_PER_SIDE)
		{
			throw;
		}
	}

	emptySpaceTokens.erase(emptySpaceTokens.begin());

	SetToMove(emptySpaceTokens.at(0));

	SetCastlingRights(emptySpaceTokens.at(1));

	SetRemainingPlayers(emptySpaceTokens.at(2));

	SetHalfMoveClock(emptySpaceTokens.at(3));

	SetFullMoveNumber(emptySpaceTokens.at(4));
}

void PositionStringParser::SetSquare(const std::string& matchString, int position)
{
	auto mask = (I256{ 1 } << position);

	if (matchString == RED_PAWN)
	{
		rPawns |= mask;
	}
	else if (matchString == RED_KNIGHT)
	{
		rKnights |= mask;
	}
	else if (matchString == RED_BISHOP)
	{
		rBishops |= mask;
	}
	else if (matchString == RED_ROOK)
	{
		rRooks |= mask;
	}
	else if (matchString == RED_QUEEN || matchString == RED_NEW_QUEEN)
	{
		rQueens |= mask;
	}
	else if (matchString == RED_KING)
	{
		rKing |= mask;
	}
	else if (matchString == BLUE_PAWN)
	{
		bPawns |= mask;
	}
	else if (matchString == BLUE_KNIGHT)
	{
		bKnights |= mask;
	}
	else if (matchString == BLUE_BISHOP)
	{
		bBishops |= mask;
	}
	else if (matchString == BLUE_ROOK)
	{
		bRooks |= mask;
	}
	else if (matchString == BLUE_QUEEN || matchString == BLUE_NEW_QUEEN)
	{
		bQueens |= mask;
	}
	else if (matchString == BLUE_KING)
	{
		bKing |= mask;
	}
	else if (matchString == YELLOW_PAWN)
	{
		yPawns |= mask;
	}
	else if (matchString == YELLOW_KNIGHT)
	{
		yKnights |= mask;
	}
	else if (matchString == YELLOW_BISHOP)
	{
		yBishops |= mask;
	}
	else if (matchString == YELLOW_ROOK)
	{
		yRooks |= mask;
	}
	else if (matchString == YELLOW_QUEEN || matchString == YELLOW_NEW_QUEEN)
	{
		yQueens |= mask;
	}
	else if (matchString == YELLOW_KING)
	{
		yKing |= mask;
	}
	else if (matchString == GREEN_PAWN)
	{
		gPawns |= mask;
	}
	else if (matchString == GREEN_KNIGHT)
	{
		gKnights |= mask;
	}
	else if (matchString == GREEN_BISHOP)
	{
		gBishops |= mask;
	}
	else if (matchString == GREEN_ROOK)
	{
		gRooks |= mask;
	}
	else if (matchString == GREEN_QUEEN || matchString == GREEN_NEW_QUEEN)
	{
		gQueens |= mask;
	}
	else if (matchString == GREEN_KING)
	{
		gKing |= mask;
	}
}

void PositionStringParser::SetToMove(const std::string& toMoveString)
{
	if (toMoveString == RED)
	{
		toMove = PlayerColor::Red;
	}
	else if (toMoveString == BLUE)
	{
		toMove = PlayerColor::Blue;
	}
	else if (toMoveString == YELLOW)
	{
		toMove = PlayerColor::Yellow;
	}
	else if (toMoveString == GREEN)
	{
		toMove = PlayerColor::Green;
	}
	else
	{
		throw;
	}
}

void PositionStringParser::SetCastlingRights(const std::string& castleString)
{
	if (castleString.find(RED_KING_CASTLE) != std::string::npos)
	{
		castlingRights |= CastlingRight::RedKingSide;
	}

	if (castleString.find(RED_QUEEN_CASTLE) != std::string::npos)
	{
		castlingRights |= CastlingRight::RedQueenSide;
	}

	if (castleString.find(BLUE_KING_CASTLE) != std::string::npos)
	{
		castlingRights |= CastlingRight::BlueKingSide;
	}

	if (castleString.find(BLUE_QUEEN_CASTLE) != std::string::npos)
	{
		castlingRights |= CastlingRight::BlueQueenSide;
	}

	if (castleString.find(YELLOW_KING_CASTLE) != std::string::npos)
	{
		castlingRights |= CastlingRight::YellowKingSide;
	}

	if (castleString.find(YELLOW_QUEEN_CASTLE) != std::string::npos)
	{
		castlingRights |= CastlingRight::YellowQueenSide;
	}

	if (castleString.find(GREEN_KING_CASTLE) != std::string::npos)
	{
		castlingRights |= CastlingRight::GreenKingSide;
	}

	if (castleString.find(GREEN_QUEEN_CASTLE) != std::string::npos)
	{
		castlingRights |= CastlingRight::GreenQueenSide;
	}
}

void PositionStringParser::SetRemainingPlayers(const std::string& remainingPlayersString)
{
	size_t maximumPlayers = 4;

	if (remainingPlayersString.size() > maximumPlayers)
	{
		throw;
	}

	if (remainingPlayersString.find(RED) != std::string::npos)
	{
		remainingPlayers |= PlayerColor::Red;
	}

	if (remainingPlayersString.find(BLUE) != std::string::npos)
	{
		remainingPlayers |= PlayerColor::Blue;
	}

	if (remainingPlayersString.find(YELLOW) != std::string::npos)
	{
		remainingPlayers |= PlayerColor::Yellow;
	}

	if (remainingPlayersString.find(GREEN) != std::string::npos)
	{
		remainingPlayers |= PlayerColor::Green;
	}
}

void PositionStringParser::SetHalfMoveClock(const std::string& halfMoveClockString)
{
	halfMoveClock = std::stoi(halfMoveClockString);
}

void PositionStringParser::SetFullMoveNumber(const std::string& fullMoveNumberString)
{
	fullMoveNumber = std::stoi(fullMoveNumberString);
}


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/list.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/math/mean_and_variance.h>
#include <xfel/mono_simulation/parameters.h>
#include <xfel/mono_simulation/bandpass_gaussian.h>
#include <xfel/mono_simulation/vector_collection.h>

#include <vector>
#include <map>

using namespace boost::python;
namespace xfel{
namespace boost_python { namespace {

  void
  xfel_mono_sim_init_module() {
    using namespace boost::python;

    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;
    class_<xfel::parameter::parameter_array>(
      "parameter_array",no_init)
      .add_property("x",
       make_getter(&xfel::parameter::parameter_array::parameters, rbv()))
      .add_property("gradients",
       make_getter(&xfel::parameter::parameter_array::gradients,rbv()))
      .add_property("curvatures",
       make_getter(&xfel::parameter::parameter_array::curvatures,rbv()))
    ;
    class_<xfel::parameter::organizer_base>(
      "organizer_base",init<>())
      .def("register",&xfel::parameter::organizer_base::register_array,
        (arg("tag"),arg("ndata"),arg("kdim")=1,arg("data")))
      .def("register_local",&xfel::parameter::organizer_base::register_local_array,
        (arg("tag"),arg("ndata"),arg("kdim")=1,arg("data")))
      .def("as_x_array",&xfel::parameter::organizer_base::as_x_array)
      .def("get_gradient_array",
           (xfel::farray(xfel::parameter::organizer_base::*)()const)
           &xfel::parameter::organizer_base::get_gradient_array)
      .def("set_gradient_array",&xfel::parameter::organizer_base::set_gradient_array)
      .def("get_curvature_array",&xfel::parameter::organizer_base::get_curvature_array)
      .def("from_x_array",&xfel::parameter::organizer_base::from_x_array)
      .def("initialize_gradients_curvatures",
         &xfel::parameter::organizer_base::initialize_gradients_curvatures)
      .def("rezero_gradients_curvatures",
         &xfel::parameter::organizer_base::rezero_gradients_curvatures)
    ;

    class_<xfel::parameter::vector_array >(
      "vector_array",init<>())
      .add_property("gradients",
       make_getter(&xfel::parameter::vector_array::gradients,rbv()))
    ;

    class_<xfel::parameter::vector_collection >(
      "vector_collection",init<>())
      .def(init<xfel::iarray,xfel::marray>())
      .def("collect_vector_information",
        &xfel::parameter::vector_collection::collect_vector_information)
      .def("register",
        &xfel::parameter::vector_collection::register_tag,(arg_("tag")))
    ;

    class_<xfel::parameter::streak_parameters >(
      "streak_parameters",no_init)
      .add_property("rotax",make_getter(
         &xfel::parameter::streak_parameters::rotax, rbv()))
      .add_property("position",make_getter(
         &xfel::parameter::streak_parameters::position, rbv()))
      .add_property("position_to_fictitious",make_getter(
         &xfel::parameter::streak_parameters::position_to_fictitious, rbv()))
      .add_property("rotax_excursion_rad",make_getter(
         &xfel::parameter::streak_parameters::rotax_excursion_rad, rbv()))
      .add_property("rotax_excursion_rad_pvr",make_getter(
         &xfel::parameter::streak_parameters::rotax_excursion_rad_pvr, rbv()))
     ;

    class_<xfel::parameter::bandpass_gaussian>("bandpass_gaussian",
      init<rstbx::bandpass::parameters_bp3 const&>(arg("parameters")))
      .def("set_active_areas", &xfel::parameter::bandpass_gaussian::set_active_areas)
      .def("set_sensor_model", &xfel::parameter::bandpass_gaussian::set_sensor_model,(
         arg("thickness_mm"), arg("mu_rho"), arg("signal_penetration")))
      .def("gaussian_fast_slow",
         &xfel::parameter::bandpass_gaussian::gaussian_fast_slow)
      .add_property("hi_E_limit",make_getter(
         &xfel::parameter::bandpass_gaussian::hi_E_limit, rbv()))
      .add_property("lo_E_limit",make_getter(
         &xfel::parameter::bandpass_gaussian::lo_E_limit, rbv()))
      .add_property("mean_position",make_getter(
         &xfel::parameter::bandpass_gaussian::mean_position, rbv()))
      .add_property("calc_radial_length",make_getter(
         &xfel::parameter::bandpass_gaussian::calc_radial_length, rbv()))
      .add_property("part_distance",make_getter(
         &xfel::parameter::bandpass_gaussian::part_distance, rbv()))
      .add_property("observed_flag",make_getter(
         &xfel::parameter::bandpass_gaussian::observed_flag, rbv()))
      .def("set_subpixel", &xfel::parameter::bandpass_gaussian::set_subpixel,(
           arg("translations"), arg("rotations_deg")))
      .def("set_mosaicity", &xfel::parameter::bandpass_gaussian::set_mosaicity)
      .def("set_domain_size", &xfel::parameter::bandpass_gaussian::set_domain_size)
      .def("set_bandpass", &xfel::parameter::bandpass_gaussian::set_bandpass)
      .def("set_orientation", &xfel::parameter::bandpass_gaussian::set_orientation)
      .def("set_detector_origin",
         &xfel::parameter::bandpass_gaussian::set_detector_origin)
      .def("set_distance", &xfel::parameter::bandpass_gaussian::set_distance)
      .def("set_vector_output_pointers",
         &xfel::parameter::bandpass_gaussian::set_vector_output_pointers,
         (arg_("vector_collection"),arg_("frame_id")))
      .def("measure_bandpass_and_mosaic_parameters", &
      xfel::parameter::bandpass_gaussian::measure_bandpass_and_mosaic_parameters
         ,(arg("radial"), arg("azimut"), arg("domain_sz_inv_ang"),
           arg("lab_frame_obs")
          ))
      .add_property("wavelength_fit_ang",make_getter(
         &xfel::parameter::bandpass_gaussian::wavelength_fit_ang, rbv()))
      .add_property("mosaicity_fit_rad",make_getter(
         &xfel::parameter::bandpass_gaussian::mosaicity_fit_rad, rbv()))
      .add_property("wavelength_fit_ang_sigma",make_getter(
         &xfel::parameter::bandpass_gaussian::wavelength_fit_ang_sigma, rbv()))
      .add_property("mosaicity_fit_rad_sigma",make_getter(
         &xfel::parameter::bandpass_gaussian::mosaicity_fit_rad_sigma, rbv()))
      .def("simple_forward_calculation_spot_position",
           &xfel::parameter::bandpass_gaussian::simple_forward_calculation_spot_position,
           (arg_("wavelength"), arg_("observation_no")))
      .def("simple_part_excursion_part_rotxy",
           &xfel::parameter::bandpass_gaussian::simple_part_excursion_part_rotxy,
           (arg_("wavelength"), arg_("observation_no"),
            arg_("dA_drotxy")))
    ;

    def("best_fit_limit",xfel::parameter::best_fit_limit);
  }

}
}} // namespace xfel::boost_python::<anonymous>

BOOST_PYTHON_MODULE(xfel_mono_sim_ext)
{
  xfel::boost_python::xfel_mono_sim_init_module();

}


/*
 * @Description: lidar localization frontend, interface
 * @Author: Ren Qian
 * @Date: 2020-02-04 18:52:45
 */
#ifndef LIDAR_LOCALIZATION_MATCHING_FRONT_END_MATCHING_HPP_
#define LIDAR_LOCALIZATION_MATCHING_FRONT_END_MATCHING_HPP_

#include <deque>
#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>

#include "lidar_localization/sensor_data/cloud_data.hpp"
#include "lidar_localization/sensor_data/pose_data.hpp"

#include "lidar_localization/models/scan_context_manager/scan_context_manager.hpp"
#include "lidar_localization/models/registration/registration_interface.hpp"
#include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp"
#include "lidar_localization/models/cloud_filter/box_filter.hpp"

namespace lidar_localization {

class Matching {
public:
    Matching();

    bool HasInited(void);
    bool HasNewGlobalMap(void);
    bool HasNewLocalMap(void);

    Eigen::Matrix4f GetInitPose(void);
    CloudData::CLOUD_PTR& GetGlobalMap(void);
    CloudData::CLOUD_PTR& GetLocalMap(void);
    CloudData::CLOUD_PTR& GetCurrentScan(void);

    bool Update(
      const CloudData& cloud_data,
      Eigen::Matrix4f& laser_pose, Eigen::Matrix4f &map_matching_pose
    );

    bool SetGNSSPose(const Eigen::Matrix4f& init_pose);
    bool SetScanContextPose(const CloudData& init_scan);

private:
    bool InitWithConfig();
    //
    // point cloud map & measurement processors:
    //
    bool InitFilter(
      const YAML::Node &config_node, std::string filter_user,
      std::shared_ptr<CloudFilterInterface>& filter_ptr
    );
    bool InitLocalMapSegmenter(const YAML::Node& config_node);
    bool InitPointCloudProcessors(const YAML::Node& config_node);
    //
    // global map:
    //
    bool InitGlobalMap(const YAML::Node& config_node);
    //
    // lidar frontend for relative pose estimation:
    //
    bool InitRegistration(const YAML::Node& config_node, std::shared_ptr<RegistrationInterface>& registration_ptr);
    //
    // map matcher:
    //
    bool InitScanContextManager(const YAML::Node& config_node);

    bool SetInitPose(const Eigen::Matrix4f& init_pose);
    bool ResetLocalMap(float x, float y, float z);

private:
    std::shared_ptr<CloudFilterInterface> global_map_filter_ptr_;

    std::shared_ptr<BoxFilter> local_map_segmenter_ptr_;
    std::shared_ptr<CloudFilterInterface> local_map_filter_ptr_;

    std::shared_ptr<CloudFilterInterface> frame_filter_ptr_;

    CloudData::CLOUD_PTR global_map_ptr_;
    CloudData::CLOUD_PTR local_map_ptr_;
    CloudData::CLOUD_PTR current_scan_ptr_;

    std::shared_ptr<RegistrationInterface> registration_ptr_;

    std::shared_ptr<ScanContextManager> scan_context_manager_ptr_;

    Eigen::Matrix4f current_pose_ = Eigen::Matrix4f::Identity();

    Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f current_gnss_pose_ = Eigen::Matrix4f::Identity();

    bool has_inited_ = false;
    bool has_new_global_map_ = false;
    bool has_new_local_map_ = false;
};

} // namespace lidar_localization

#endif // LIDAR_LOCALIZATION_MATCHING_FRONTEND_MATCHING_HPP_


#include <boost/function.hpp>

namespace learning
{

    boost::function<float (int x) f;

    boost::function grad(boost::function f)
    {
        // right now this would contain a simple grad program
        return 0;
    }

}


/**
 * @file beam_pattern_multi.cc
 */

#include <usml/sensors/beam_pattern_multi.h>
#include <boost/foreach.hpp>

using namespace usml::sensors ;
using namespace boost ;

/**
 * Constructor
 */
beam_pattern_multi::beam_pattern_multi(
    std::list<beam_pattern_model*> beam_list )
    : _beam_list(beam_list)
{

}

/**
 * Destructor
 */
beam_pattern_multi::~beam_pattern_multi()
{
    BOOST_FOREACH( beam_pattern_model* b, _beam_list )
        delete b ;
}

/**
 * Multiplies all beam levels from each beam pattern
 */
void beam_pattern_multi::beam_level(
    double de, double az,
    orientation& orient,
    const vector<double>& frequencies,
    vector<double>* level )
{
    write_lock_guard(_mutex) ;
    vector<double> tmp( frequencies.size(), 1.0 ) ;
    noalias(*level) = vector<double>( frequencies.size(), 1.0 ) ;
    BOOST_FOREACH( beam_pattern_model* b, _beam_list )
    {
        b->beam_level( de, az, orient, frequencies, &tmp ) ;
        *level = element_prod( *level, tmp ) ;
    }
}

/**
 * Multiplies the directivity indices from each beam pattern
 */
void beam_pattern_multi::directivity_index(
    const vector<double>& frequencies,
    vector<double>* level )
{
    write_lock_guard(_mutex) ;
    vector<double> tmp( frequencies.size(), 1.0 ) ;
    noalias(*level) = vector<double>( frequencies.size(), 0.0 ) ;
    BOOST_FOREACH( beam_pattern_model* b, _beam_list )
    {
        b->directivity_index( frequencies, &tmp ) ;
        *level += tmp ;
    }
}


// Copyright 2014 Jonathan Graehl - http://graehl.org/
//
// 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.
#ifndef GRAEHL__SHARED__HASH_CACHE_HPP
#define GRAEHL__SHARED__HASH_CACHE_HPP

#include <boost/functional/hash.hpp>
//#include <graehl/shared/hashtable_fwd.hpp>
#include <vector>
#include <cassert>

#include <graehl/shared/doubling_primes.hpp>

namespace graehl {

/* *Hash is a stateless functor:
   size_t Hash::operator()(const Key &key) const

   *Backing class is stored by value, and must provide:
   void Backing::get(const Key &key,Val *val)

   *Key must provide equality,assignment
   */
template <class Key, class Val, class Backing, class Hash = boost::hash<Key>, size_t default_cache_size = 7919 >
struct hash_cache
{
  Backing backing_store;
  static Hash stateless_hasher;
  unsigned cachesize;
  typedef hash_cache<Key, Val, Backing> Self;
  static unsigned hash(const Key &key)
  {
    return stateless_hasher(key);
  }
  hash_cache(const Key &null_value = Key(), std::size_t cache_size = 10000) { init(null_value); }
  hash_cache(const Key &null_value = Key(), const Backing &back, std::size_t cache_size = 10000) : backing_store(back) { init(null_value); }
  typedef std::pair<Key, Val> entry;
  typedef std::vector<entry> cache;
  void init(const Key &null_value = Key(), std::size_t cache_size = 10000)
  {
    cachesize = prime_upper_bound(cache_size);
    cache.clear();
    cache.resize(cachesize, entry(null_value, Val()));
    assert(cache.size()==cachesize);
    n_miss = n_hit = 0;
  }
  std::size_t n_miss, n_hit;
  // returns [0...1] portion of hits
  double hit_rate()
  {
    return n_hit / ((double)n_miss+n_hit);
  }
  Val &operator[](const Key &key)
  {
    unsigned i = hash(key) % cachesize;
    Key &cached_key = cache[i].first;
    Val &cached_val = cache[i].second;
    if (cached_key != key) {
      cached_key = key;
      ++n_miss;
      backing_store.get(key, &cached_val);
    } else {
      ++n_hit;
    }
    return cached_val;
  }
};

}

#endif


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#ifndef BOOST_SIMD_FUNCTION_SIMD_TANPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_TANPI_HPP_INCLUDED

#include <boost/simd/function/scalar/tanpi.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/tanpi.hpp>

#endif


#pragma once
#include <CreateCoin/protocol/types_fwd.hpp>
#include <CreateCoin/protocol/config.hpp>

#include <CreateCoin/protocol/asset_symbol.hpp>
#include <CreateCoin/protocol/fixed_string.hpp>

#include <fc/container/flat_fwd.hpp>
#include <fc/io/varint.hpp>
#include <fc/io/enum_type.hpp>
#include <fc/crypto/sha224.hpp>
#include <fc/crypto/ripemd160.hpp>
#include <fc/crypto/elliptic.hpp>
#include <fc/reflect/reflect.hpp>
#include <fc/reflect/variant.hpp>
#include <fc/safe.hpp>
#include <fc/optional.hpp>
#include <fc/container/flat.hpp>
#include <fc/string.hpp>
#include <fc/io/raw.hpp>
#include <fc/uint128.hpp>
#include <fc/static_variant.hpp>
#include <fc/smart_ref_fwd.hpp>

#include <boost/multiprecision/cpp_int.hpp>

#include <memory>
#include <vector>
#include <deque>
#include <cstdint>

namespace CreateCoin {

   using                                    fc::uint128_t;
   typedef boost::multiprecision::uint256_t u256;
   typedef boost::multiprecision::uint512_t u512;

   using                               std::map;
   using                               std::vector;
   using                               std::unordered_map;
   using                               std::string;
   using                               std::deque;
   using                               std::shared_ptr;
   using                               std::weak_ptr;
   using                               std::unique_ptr;
   using                               std::set;
   using                               std::pair;
   using                               std::enable_shared_from_this;
   using                               std::tie;
   using                               std::make_pair;

   using                               fc::smart_ref;
   using                               fc::variant_object;
   using                               fc::variant;
   using                               fc::enum_type;
   using                               fc::optional;
   using                               fc::unsigned_int;
   using                               fc::signed_int;
   using                               fc::time_point_sec;
   using                               fc::time_point;
   using                               fc::safe;
   using                               fc::flat_map;
   using                               fc::flat_set;
   using                               fc::static_variant;
   using                               fc::ecc::range_proof_type;
   using                               fc::ecc::range_proof_info;
   using                               fc::ecc::commitment_type;
   struct void_t{};

   namespace protocol {

      typedef fc::ecc::private_key        private_key_type;
      typedef fc::sha256                  chain_id_type;
      typedef fixed_string<16>            account_name_type;
      typedef fc::ripemd160               block_id_type;
      typedef fc::ripemd160               checksum_type;
      typedef fc::ripemd160               transaction_id_type;
      typedef fc::sha256                  digest_type;
      typedef fc::ecc::compact_signature  signature_type;
      typedef safe<int64_t>               share_type;
      typedef uint16_t                    weight_type;
      typedef uint32_t                    contribution_id_type;
      typedef fixed_string<32>            custom_id_type;

      struct public_key_type
      {
            struct binary_key
            {
               binary_key() {}
               uint32_t                 check = 0;
               fc::ecc::public_key_data data;
            };
            fc::ecc::public_key_data key_data;
            public_key_type();
            public_key_type( const fc::ecc::public_key_data& data );
            public_key_type( const fc::ecc::public_key& pubkey );
            explicit public_key_type( const std::string& base58str );
            operator fc::ecc::public_key_data() const;
            operator fc::ecc::public_key() const;
            explicit operator std::string() const;
            friend bool operator == ( const public_key_type& p1, const fc::ecc::public_key& p2);
            friend bool operator == ( const public_key_type& p1, const public_key_type& p2);
            friend bool operator < ( const public_key_type& p1, const public_key_type& p2) { return p1.key_data < p2.key_data; }
            friend bool operator != ( const public_key_type& p1, const public_key_type& p2);
      };

      #define CreateCoin_INIT_PUBLIC_KEY (CreateCoin::protocol::public_key_type(CreateCoin_INIT_PUBLIC_KEY_STR))

      struct extended_public_key_type
      {
         struct binary_key
         {
            binary_key() {}
            uint32_t                   check = 0;
            fc::ecc::extended_key_data data;
         };

         fc::ecc::extended_key_data key_data;

         extended_public_key_type();
         extended_public_key_type( const fc::ecc::extended_key_data& data );
         extended_public_key_type( const fc::ecc::extended_public_key& extpubkey );
         explicit extended_public_key_type( const std::string& base58str );
         operator fc::ecc::extended_public_key() const;
         explicit operator std::string() const;
         friend bool operator == ( const extended_public_key_type& p1, const fc::ecc::extended_public_key& p2);
         friend bool operator == ( const extended_public_key_type& p1, const extended_public_key_type& p2);
         friend bool operator != ( const extended_public_key_type& p1, const extended_public_key_type& p2);
      };

      struct extended_private_key_type
      {
         struct binary_key
         {
            binary_key() {}
            uint32_t                   check = 0;
            fc::ecc::extended_key_data data;
         };

         fc::ecc::extended_key_data key_data;

         extended_private_key_type();
         extended_private_key_type( const fc::ecc::extended_key_data& data );
         extended_private_key_type( const fc::ecc::extended_private_key& extprivkey );
         explicit extended_private_key_type( const std::string& base58str );
         operator fc::ecc::extended_private_key() const;
         explicit operator std::string() const;
         friend bool operator == ( const extended_private_key_type& p1, const fc::ecc::extended_private_key& p2);
         friend bool operator == ( const extended_private_key_type& p1, const extended_private_key_type& p2);
         friend bool operator != ( const extended_private_key_type& p1, const extended_private_key_type& p2);
      };

      chain_id_type generate_chain_id( const std::string& chain_id_name );

} }  // CreateCoin::protocol

namespace fc
{
    void to_variant( const CreateCoin::protocol::public_key_type& var,  fc::variant& vo );
    void from_variant( const fc::variant& var,  CreateCoin::protocol::public_key_type& vo );
    void to_variant( const CreateCoin::protocol::extended_public_key_type& var, fc::variant& vo );
    void from_variant( const fc::variant& var, CreateCoin::protocol::extended_public_key_type& vo );
    void to_variant( const CreateCoin::protocol::extended_private_key_type& var, fc::variant& vo );
    void from_variant( const fc::variant& var, CreateCoin::protocol::extended_private_key_type& vo );
}

FC_REFLECT( CreateCoin::protocol::public_key_type, (key_data) )
FC_REFLECT( CreateCoin::protocol::public_key_type::binary_key, (data)(check) )
FC_REFLECT( CreateCoin::protocol::extended_public_key_type, (key_data) )
FC_REFLECT( CreateCoin::protocol::extended_public_key_type::binary_key, (check)(data) )
FC_REFLECT( CreateCoin::protocol::extended_private_key_type, (key_data) )
FC_REFLECT( CreateCoin::protocol::extended_private_key_type::binary_key, (check)(data) )

FC_REFLECT_TYPENAME( CreateCoin::protocol::share_type )

FC_REFLECT( CreateCoin::void_t, )


/*! *****************************************************************************
 *   \file libSampler.cpp
 *   \author moennen
 *   \brief
 *   \date 2018-04-06
 *   *****************************************************************************/

#include "sampleBuffDataset/libBuffDatasetSampler.h"

#include "utils/cv_utils.h"
#include "utils/imgFileLst.h"
#include "utils/Hop.h"

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>

#include <glm/glm.hpp>

#include <future>

#include <random>
#include <fstream>
#include <string>
#include <vector>
#include <array>

#include <ctime>
#include <iostream>
#include <map>

using namespace std;
using namespace cv;
using namespace glm;

#define TRACE cout << __PRETTY_FUNCTION__ << "@" << __LINE__ << endl;

namespace
{
// copy the input mask to the output and set null the averaged mask values
void correctMask( const Mat& imask, Mat& omask )
{
#pragma omp parallel for
   for ( unsigned y = 0; y < omask.rows; y++ )
   {
      const float* imaskPtr = imask.ptr<float>( y );
      float* omaskPtr = omask.ptr<float>( y );
      for ( unsigned x = 0; x < omask.cols; x++ )
      {
         omaskPtr[x] = ( imaskPtr[x] < 0.999 ? 0.0 : 1.0 );
      }
   }
}

// set to null masked depth entries
void maskDepth( const Mat& matMask, Mat& matDepth )
{
#pragma omp parallel for
   for ( unsigned y = 0; y < matDepth.rows; y++ )
   {
      const float* fPtrMask = matMask.ptr<float>( y );
      float* fPtrDepth = matDepth.ptr<float>( y );
      for ( unsigned x = 0; x < matDepth.cols; x++ )
      {
         if (fPtrMask[x] < 0.999) fPtrDepth[x] = 0.0;
      }
   }
}

struct Sampler final
{
   mt19937 _rng;
   uniform_int_distribution<> _dataGen;
   uniform_real_distribution<> _tsGen;
   normal_distribution<> _rnGen;

   const ivec3 _sampleSz;
   const bool _toLinear;
   const bool _doRescale;

   const unsigned _depthBuffSz;
   const unsigned _maskBuffSz;
   const unsigned _imgBuffSz;

   const unsigned _fullBuffSz;
   future<bool> _asyncSample;
   vector<float> _asyncBuff;

   enum
   {
      nInBuffers = 3,
      nOutBuffers = 3,
      nOutPlanes = 5  // RGB : 3 / Depth : 1 / Mask : 1
   };
   ImgNFileLst _data;

   inline static unsigned getBufferDepth( const unsigned buffId ) { return buffId == 0 ? 3 : 1; }

   Sampler(
       const char* dataSetPath,
       const char* dataPath,
       const ivec3 sampleSz,
       const bool toLinear,
       const bool doRescale,
       const bool doAsync,
       const int seed )
       : _rng( seed ),
         _tsGen( 0.0, 1.0 ),
         _sampleSz( sampleSz ),
         _toLinear( toLinear ),
         _doRescale( doRescale ),
         _depthBuffSz( _sampleSz.y * _sampleSz.z ),
         _maskBuffSz( _depthBuffSz ),
         _imgBuffSz( _depthBuffSz * 3 ),
         _fullBuffSz( _sampleSz.y * _sampleSz.z * nOutPlanes ),
         _data( nInBuffers )
   {
      HOP_PROF_FUNC();

      _data.open( dataSetPath, dataPath );

      if ( _data.size() )
      {
         std::cout << "Read dataset " << dataSetPath << " (" << _data.size() << ") " << std::endl;
      }
      _dataGen = uniform_int_distribution<>( 0, _data.size() - 1 );

      cout << endl << "WARNING ! SAMPLER : NO ERODED MASK PRODUCED !!!!!!! " << endl << endl;

      if ( _data.size() && doAsync )
      {
         _asyncBuff.resize( sampleSz.x * _fullBuffSz );
         _asyncSample = async( launch::async, [&]() { return sample_internal( &_asyncBuff[0] ); } );
      }
   }

   bool sample( float* buff )
   {
      if ( _asyncBuff.empty() )
         return sample_internal( buff );
      else
      {
         bool success = _asyncSample.get();
         if ( success )
         {
#pragma omp parallel for
            for ( int b = 0; b < _sampleSz.x; ++b )
            {
               const size_t off = b * _fullBuffSz;
               memcpy( buff + off, &_asyncBuff[off], sizeof( float ) * _fullBuffSz );
            }
         }
         _asyncSample = async( launch::async, [&]() { return sample_internal( &_asyncBuff[0] ); } );
         return success;
      }
   }

   size_t nSamples() const { return _data.size(); }
   ivec3 sampleSizes() const { return _sampleSz; }

  private:
   bool sample_internal( float* buff )
   {
      HOP_PROF_FUNC();

      float* currBuffImg = buff;
      float* currBuffDepth = buff + _imgBuffSz * _sampleSz.x;
      float* currBuffMask = buff + ( _imgBuffSz + _depthBuffSz ) * _sampleSz.x;
      // float* currBuffErodedMask = buff + ( _imgBuffSz + _depthBuffSz + _maskBuffSz ) *
      // _sampleSz.x;

      std::vector<char> sampled( _sampleSz.x, 0 );
      std::vector<size_t> v_si( _sampleSz.x );
      do
      {
         for ( auto& si : v_si ) si = _dataGen( _rng );

#pragma omp parallel for
         for ( size_t s = 0; s < _sampleSz.x; ++s )
         {
            if ( sampled[s] ) continue;

            const size_t si = v_si[s];

            Mat currImg =
                cv_utils::imread32FC3( _data.filePath( si, 0 ), _toLinear, true /*toRGB*/ );
            Mat currDepth = cv_utils::imread32FC1( _data.filePath( si, 1 ) );
            Mat currMask = cv_utils::imread32FC1( _data.filePath( si, 2 ) );

            ivec2 imgSz( currImg.cols, currImg.rows );

            // ignore failed samples
            if ( currImg.empty() || currDepth.empty() || currMask.empty() ) continue;

            // padd too small samples
            if ( ( imgSz.x < _sampleSz.y ) || ( imgSz.y < _sampleSz.z ) )
            {
               const ivec2 topright(
                   max( ivec2( _sampleSz.y - imgSz.x, _sampleSz.z - imgSz.y ), ivec2( 0 ) ) );
               copyMakeBorder( currImg, currImg, topright.y, 0, 0, topright.x, BORDER_CONSTANT );
               copyMakeBorder(
                   currDepth, currDepth, topright.y, 0, 0, topright.x, BORDER_CONSTANT );
               copyMakeBorder( currMask, currMask, topright.y, 0, 0, topright.x, BORDER_CONSTANT );
               imgSz = ivec2( currImg.cols, currImg.rows );
            }

            // random rescale
            bool rescaled = false;
            if ( _doRescale )
            {
               const float minDs =
                   std::max( (float)_sampleSz.z / imgSz.y, (float)_sampleSz.y / imgSz.x );
               const float ds = minDs; //mix( 1.0f, minDs, _tsGen( _rng ) );
               if ( ds < 1.0 )
               {
                  rescaled = true;
                  resize( currImg, currImg, Size(), ds, ds, INTER_AREA );
                  resize( currDepth, currDepth, Size(), ds, ds, INTER_AREA );
                  resize( currMask, currMask, Size(), ds, ds, INTER_AREA );
                  imgSz = ivec2( currImg.cols, currImg.rows );
               }
            }

            // random translate
            const ivec2 trans(
                std::floor( _tsGen( _rng ) * ( imgSz.x - _sampleSz.y ) ),
                std::floor( _tsGen( _rng ) * ( imgSz.y - _sampleSz.z ) ) );

            // crop
            currImg = currImg( Rect( trans.x, trans.y, _sampleSz.y, _sampleSz.z ) );
            currDepth = currDepth( Rect( trans.x, trans.y, _sampleSz.y, _sampleSz.z ) );
            currMask = currMask( Rect( trans.x, trans.y, _sampleSz.y, _sampleSz.z ) );

            // copy and correct mask
            Mat maskSple( _sampleSz.z, _sampleSz.y, CV_32FC1, currBuffMask + s * _maskBuffSz );
            if ( rescaled )
               correctMask( currMask, maskSple );
            else
               currMask.copyTo( maskSple );
            if ( sum( maskSple )[0] < 20 ) continue;

            // random small blur to remove artifacts + copy to destination
            Mat imgSple( _sampleSz.z, _sampleSz.y, CV_32FC3, currBuffImg + s * _imgBuffSz );
            currImg.copyTo(imgSple);
            /*cv_utils::adjustContrastBrightness<vec3>(
                currImg, ( 1.0f + 0.11f * _rnGen( _rng ) ), 0.11f * _rnGen( _rng ) );
            GaussianBlur( currImg, imgSple, Size( 3, 3 ), 0.31 * abs( _rnGen( _rng ) ) );*/

            // copy and process depth
            Mat depthSple( _sampleSz.z, _sampleSz.y, CV_32FC1, currBuffDepth + s * _depthBuffSz );
            maskSple.convertTo( currMask, CV_8UC1, 255.0, 0.0 );
            currDepth.copyTo(depthSple);
            /*cv::Mat mean, std;
            cv::meanStdDev( currDepth, mean, std, currMask );
            depthSple = ( ( currDepth - mean ) / std );
            // normalize( currDepth, depthSple, 0.0, 1.0, NORM_MINMAX, -1, currMask );
            // this is for debugging !!!
            depthSple = depthSple.mul( maskSple );*/
            // apply log
            cv::log(currDepth, depthSple);
            maskDepth(maskSple,depthSple);

            //transpose(imgSple,imgSple);
            //transpose(depthSple,depthSple);
            //transpose(maskSple,maskSple);

            // copy and process eroded mask
            // Mat erodedMaskSple( _sampleSz.z, _sampleSz.y, CV_32FC1, currBuffErodedMask );
            // erode(maskSple, erodedMaskSple, Mat());

            sampled[s] = 1;
         }
      } while ( accumulate( sampled.begin(), sampled.end(), 0 ) != _sampleSz.x );

      return true;
   }
};

array<unique_ptr<Sampler>, 33> g_samplers;
};

extern "C" int getNbBuffers( const int /*sidx*/ ) { return Sampler::nOutBuffers; }

extern "C" int getBuffersDim( const int sidx, float* dims )
{
   HOP_PROF_FUNC();

   if ( !g_samplers[sidx].get() ) return ERROR_UNINIT;

   const ivec3 sz = g_samplers[sidx]->sampleSizes();
   float* d = dims;
   for ( size_t i = 0; i < Sampler::nOutBuffers; ++i )
   {
      d[0] = sz.z;
      d[1] = sz.y;
      d[2] = Sampler::getBufferDepth( i );
      d += 3;
   }

   return SUCCESS;
}

extern "C" int initBuffersDataSampler(
    const int sidx,
    const char* datasetPath,
    const char* dataPath,
    const int nParams,
    const float* params,
    const int seed )
{
   HOP_PROF_FUNC();

   // check input
   if ( ( nParams < 3 ) || ( sidx > g_samplers.size() ) ) return ERROR_BAD_ARGS;

   // parse params
   const ivec3 sz( params[0], params[2], params[1] );
   const bool toLinear( nParams > 3 ? params[3] > 0.0 : false );
   const bool doRescale( nParams > 4 ? params[4] > 0.0 : false );
   const bool doAsync( nParams > 5 ? params[5] > 0.0 : true );
   g_samplers[sidx].reset(
       new Sampler( datasetPath, dataPath, sz, toLinear, doRescale, doAsync, seed ) );

   return g_samplers[sidx]->nSamples() ? SUCCESS : ERROR_BAD_DB;
}

extern "C" int getBuffersDataSample( const int sidx, float* buff )
{
   HOP_PROF_FUNC();

   if ( !g_samplers[sidx].get() ) return ERROR_UNINIT;

   // sample
   if ( !g_samplers[sidx]->sample( buff ) ) return ERROR_GENERIC;

   return SUCCESS;
}


#include "manycal/FiducialArrayCalibrator.h"
#include "argus_utils/utils/ParamUtils.h"

#include <boost/foreach.hpp>

namespace argus
{

FiducialArrayCalibrator::FiducialArrayCalibrator( ros::NodeHandle& nh,
                                                  ros::NodeHandle& ph )
: _fiducialManager( _lookupInterface ),
  _extrinsicsManager( nh, ph ),
  _detCounter( 0 )
{
	GetParamRequired( ph, "reference_frame", _referenceFrame );
	GetParam( ph, "batch_period", _batchPeriod, (unsigned int) 10 );
	GetParam( ph, "min_detections_per_image", _minDetectionsPerImage, (unsigned int) 2 );
	GetParam( ph, "prior_covariance", _priorCovariance, PoseSE3::CovarianceMatrix::Identity() );

	std::string lookupNamespace;
	GetParam<std::string>( ph, "lookup_namespace", lookupNamespace, "/lookup" );
	_lookupInterface.SetLookupNamespace( lookupNamespace );

	// Create optimizer object
	ros::NodeHandle oh( ph.resolveName( "optimizer" ) );
	_optimizer = GraphOptimizer( oh );

	// Create camera intrinsics node
	_cameraIntrinsics = std::make_shared <isam::MonocularIntrinsics_Node>();
	_cameraIntrinsics->init( isam::MonocularIntrinsics( 1.0, 1.0, Eigen::Vector2d( 0, 0 ) ) );

	// Create fiducial reference pose node
	_fiducialReference = std::make_shared<isam::PoseSE3_Node>();
	_fiducialReference->init( isam::PoseSE3() );

	unsigned int buffSize;
	GetParam( ph, "buffer_size", buffSize, (unsigned int) 10 );
	_detSub = nh.subscribe( "detections",
	                        buffSize,
	                        &FiducialArrayCalibrator::DetectionCallback,
	                        this );
}

void FiducialArrayCalibrator::WriteResults()
{
	BOOST_FOREACH( const FiducialRegistry::value_type& item, _fiducialRegistry )
	{
		const std::string& name = item.first;
		const FiducialRegistration& registration = item.second;
		PoseSE3 extrinsics = registration.extrinsics->value().pose;
		ROS_INFO_STREAM( "Fiducial " << name << " pose " << extrinsics );

		// TODO Write to a YAML file
		// ExtrinsicsInfo info;
		// info.referenceFrame = _referenceFrame;
		// info.extrinsics = extrinsics;
		// _extrinsicsManager.WriteMemberInfo( name, info, true );
	}
}

isam::PoseSE3_Node::Ptr
FiducialArrayCalibrator::InitializeCameraPose( const std::vector<FiducialDetection>& detections )
{
	isam::PoseSE3_Node::Ptr cameraNode;
	BOOST_FOREACH( const FiducialDetection& det, detections )
	{
		// Attempt to initialize unregistered fiducials from prior info
		// If we can't, continue to the next detection
		if( _fiducialRegistry.count( det.name ) == 0 &&
		    !InitializeFiducialFromPrior( det.name ) ) { continue; }

		// If we have a registered fiducial, use it to initialize
		if( _fiducialRegistry.count( det.name ) > 0 )
		{
			PoseSE3 relPose = EstimateArrayPose( det,
			                                     _fiducialManager.GetInfo( det.name ) );
			PoseSE3 fiducialPose = _fiducialRegistry[ det.name ].extrinsics->value().pose;
			PoseSE3 cameraPose = fiducialPose * relPose.Inverse();

			cameraNode = std::make_shared <isam::PoseSE3_Node>();
			cameraNode->init( isam::PoseSE3( cameraPose ) );
			break;
		}
	}
	return cameraNode;
}

void FiducialArrayCalibrator::ProcessDetections( const FiducialDetections& detections )
{
	if( detections.size() < _minDetectionsPerImage )
	{
		ROS_WARN_STREAM( "Only " << detections.size() << " detections, less than " <<
		                 _minDetectionsPerImage << " minimum. Skipping..." );
		return;
	}
	_detectionsBuffer.PushBack( detections );
}

void FiducialArrayCalibrator::Spin()
{
	isam::PoseSE3_Node::Ptr cameraNode = InitializeCameraPose( detections );
	if( !cameraNode )
	{
		ROS_WARN_STREAM( "Could not initialize camera pose." );
		return;
	}
	_cameraPoses.push_back( cameraNode );
	_optimizer.GetOptimizer().add_node( cameraNode.get() );

	// Create observation factors for each fiducial detection
	BOOST_FOREACH( const FiducialDetection& det, detections )
	{
		// if the fiducial is not registered and we can't get intrinsics, skip it
		if( _fiducialRegistry.count( det.name ) == 0
		    && !_fiducialManager.CheckMemberInfo( det.name ) ) { continue; }

		const Fiducial& intrinsics = _fiducialManager.GetInfo( det.name );
		PoseSE3 relPose = EstimateArrayPose( det, intrinsics );
		PoseSE3 cameraPose = cameraNode->value().pose;
		PoseSE3 fiducialPose = cameraPose * relPose;
		RegisterFiducial( det.name, fiducialPose, false );

		const FiducialRegistration& reg = _fiducialRegistry[ det.name ];

		// TODO Formalize this error model?
		double imageCoordinateErr = std::pow( 0.03, 2 );
		isam::Noise cov = isam::Covariance( imageCoordinateErr * isam::eye( 2*det.points.size() ) );

		isam::FiducialFactor::Properties props;
		props.optCamReference = true;
		props.optCamIntrinsics = false;
		props.optCamExtrinsics = false;
		props.optFidReference = false;
		props.optFidIntrinsics = false;
		props.optFidExtrinsics = true;

		isam::FiducialFactor::Ptr factor = std::make_shared<isam::FiducialFactor>
		    ( cameraNode.get(),
		      _cameraIntrinsics.get(),
		      nullptr,
		      _fiducialReference.get(),
		      reg.intrinsics.get(),
		      reg.extrinsics.get(),
		      DetectionToIsam( det ),
		      cov,
		      props );
		_optimizer.GetOptimizer().add_factor( factor.get() );
		_observations.push_back( factor );
	}

	if( _detCounter % _batchPeriod == 0 )
	{
		_optimizer.GetOptimizer().batch_optimization();
		_optimizer.GetOptimizer().print_graph();
	}
	else
	{
		_optimizer.GetOptimizer().update();
	}
	_detCounter++;
}

bool FiducialArrayCalibrator::InitializeFiducialFromPrior( const std::string& name )
{
	if( _fiducialRegistry.count( name ) > 0 ) { return true; }

	// If not registered, attempt initialization
	// First check intrinsics
	if( !_fiducialManager.CheckMemberInfo( name ) ) { return false; }
	// Then check extrinsics
	try
	{
		PoseSE3 ext = _extrinsicsManager.GetExtrinsics( name,
		                                                _referenceFrame );
		RegisterFiducial( name, ext, true );
		return true;
	}
	catch( ExtrinsicsException& e ) { return false; }
}

void FiducialArrayCalibrator::RegisterFiducial( const std::string& name,
                                                const PoseSE3& pose,
                                                bool addPrior )
{
	if( _fiducialRegistry.count( name ) > 0 ) { return; }

	ROS_INFO_STREAM( "Registering fiducial " << name << " with pose " << pose );

	FiducialRegistration registration;
	isam::FiducialIntrinsics intrinsics = FiducialToIsam( _fiducialManager.GetInfo( name ) );
	registration.intrinsics =
	    std::make_shared<isam::FiducialIntrinsics_Node>( intrinsics.name(), intrinsics.dim() );
	registration.intrinsics->init( intrinsics );
	// NOTE Not optimizing intrinsics, so we don't have to add it to the SLAM object

	registration.extrinsics = std::make_shared<isam::PoseSE3_Node>();
	registration.extrinsics->init( isam::PoseSE3( pose ) );
	_optimizer.GetOptimizer().add_node( registration.extrinsics.get() );

	if( addPrior )
	{
		isam::Noise priorCov = isam::Covariance( _priorCovariance );
		registration.extrinsicsPrior =
			std::make_shared<isam::PoseSE3_Prior>( registration.extrinsics.get(),
			                                       isam::PoseSE3( pose ),
			                                       priorCov );
		_optimizer.GetOptimizer().add_factor( registration.extrinsicsPrior.get() );
	}

	_fiducialRegistry[ name ] = registration;
}

}