//
//  bpp_c.hpp for mammal
//  PhyloAcc_init2
//
//  Created by hzr on 4/19/16.
//  Copyright © 2016 hzr. All rights reserved.
//

#ifndef bpp_c_hpp
#define bpp_c_hpp

#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>
#include <string>
#include <armadillo>
#include <cassert>

#include <cmath>
#include <sys/time.h>

#include <map>
#include <fstream>
#include <vector>
#include <set>
#include <sstream>
#include <algorithm>
#include <random>

#include "newick.h"
#include "profile.h"
#include "bpp.hpp"
#include "utils.h"
#include "genetree.hpp"

using namespace std;
using namespace arma;

struct simpletree
{
    int count;                       // number of occurence
    vector< double > distances;      // average branch distances
    vector<vector<int>> children_gene;
    vector<int> parent_gene;
    int root;
    vector<string> node_names;     //Han*: name internal node by its descendents in newick format
    void printTree(int s, BPP & bpp, std::stringstream & buffer)
    {
        if (children_gene[s][0] == -1)
        {
            buffer << bpp.species_names[s] << ":"<< distances[s];
        }
        else
        {
            buffer << "(";
            for(int i =0;i <2; i++)
            {
                int child = children_gene[s][i];

                printTree(child, bpp, buffer);
                if(i==0) buffer << ",";

            }

            if(parent_gene[s] < bpp.N)
            {
                buffer << "):" << distances[s];
            }else{
                buffer << ");" ;
            }
        }
    }
};

class BPP_C
{
    private:

    int CC;  //current number of elements
    int GG_block;

    int S;
    int N;

    //vector< vector<vec > > ambiguousS_null;  //base * species# * 4;

    int    (*children2)[2];  //children from pruned tree
    int root;
    //Han*: change to vector
    vec pi;
    vec log_pi;
    //vector<double> pi;
    //vector<double> log_pi;
    vector<double> prior_dir_param;
    //Han*: Q a-f:
    vec inst_rate;
    mat eigenvec; vec eigenval;
    mat eigeninv;
    mat submat;

    vector< vector<vec>>  lambda;  //GG * S * vec(4)
    vector < vector<int>> Tg; //current history during each speciation, GG*S

    vector<int> nodes;
    //vector<int> internal_nodes;
    vector<bool> missing;
    vector<int> upper_c;
    vector<int> upper_conserve_c;

   // double prior_glr[3];
    double prior_l_a, prior_l_b;
    double prior_l2_a, prior_l2_b;
    double prior_g_a, prior_g_b;
    double ratio0;
    double ratio1;
    int num_burn;   // num of burn-in updates
    int num_mcmc;   // num of MCMC updates
    int num_thin;   // num of updates between two samples
    int adaptive_freq; //= 100;

    vector<int> Z ; //N *0, accelerate(time,0-1), loss(-1)
    vector<int> fixZ ;

    //buffer P(zt|zt-1,Hg)
    vector<vec> log_prob_back;

    //get P(X|theta)
    vector< vector<double> >  log_emission; //( N-1)*2

    // MCMC updating states
    int m;                                      // current MCMC step

    vector<mat> log_TM_Int;

    vec prior_z;

    // samples to output
    double MaxLoglik;
    int Max_m;
    vector <int > Max_Z;
    string Max_GT;
    //Han*: save running max for pi and acgt counts (for posterior parameters)
    vector<double> Max_pi;

    vector< double >  trace_loglik;  //P(X|Z, TM, r)
    vector< double >  trace_full_loglik;  //P(X,Z,r|TM)
    vector< vector<int> > trace_Z;  //iteration * Species * {0,1,2}, only for init
    vector <double> trace_n_rate;  //iter*element
    vector <double> trace_c_rate;
    vector<double> trace_logNormratio;
    map<string, simpletree> trace_genetree; // record how many time and average branch length

    vector <double> trace_l_rate;  //iter*element
    vector <double> trace_l2_rate;
    vector <double> trace_g_rate;

    //Han*: save MCMC traces
    vector<vector<double>> trace_pi;
    vector<double> trace_indicator;
    vector<int> trace_GTtopChg;

    int accept_n_rate = 0;
    int accept_c_rate = 0;  //how many accepted in current cycle

    double prop_n;  //for adaptive MCMC, changed by acceptance rate
    double prop_c;

    double consToMis;
    double nconsToMis;

    // GSL random number generator
    gsl_rng * RNG;

    time_t last_time;

    unsigned long int seed;
    unsigned long int seed2;
    GTree* gtree;
    friend class GTree;

public:
    int GG; //base pairs current elements
    bool failure = false;
    bool verbose;
    bool verboseGT;
    int idblk_count=0; //length of 1st blck of all identical bp across sp.

    BPP_C(int c, PhyloProf _prof, BPP& bpp, char gapchar, double missing_thres, bool & filter, bool _verbose, bool _verboseGT, double _consToMis, int blocks = 20, bool prune=0, double revgap=0, int min_length =50, double _nconsToMis = 0.5)//, double _indel)
    {

        RNG = bpp.RNG;

        num_burn = bpp.num_burn*bpp.num_thin;   // num of burn-in updates
        num_mcmc = bpp.num_mcmc*bpp.num_thin;   // num of MCMC updates
        if(bpp.num_thin == 1)  // 0517: ???
        {
            adaptive_freq = 100;   // num of updates between two samples, 100
        }else{
            adaptive_freq = 50;
        }
        num_thin = bpp.num_thin;

        consToMis = _consToMis;
        nconsToMis = _nconsToMis;

        N = bpp.N;
        CC = c;
        GG =bpp.element_size[c];
        if(GG < min_length)
        {
            filter = true;
            return;
        }

        root = N-1;   // for gene tree
        S = bpp.S;
        GG_block = blocks; //ceil((double)GG/blocks);
        int tot = ceil((double)(GG - 15)/GG_block);
        // if(tot>5) tot=5;
        // GG_block=ceil((double)GG/(tot));

        //cout << CC << ": " << GG << " bp" << endl;

        lambda = vector< vector<vec> > (GG, vector<vec>(N));
        Tg = vector < vector< int> > (GG, vector<int>(N));

        gtree = new GTree(N, GG, S, RNG);

        ratio0 = bpp.ratio0;  //no use
        ratio1 = bpp.ratio1; //no use

        verbose = _verbose;
        verboseGT = _verboseGT;

        log_TM_Int = vector<mat >(N, zeros<mat>(3,2));

        prior_l_a = bpp.prior_l_a;
        prior_l_b = bpp.prior_l_b;
        prior_g_a = bpp.prior_g_a;
        prior_g_b = bpp.prior_g_b;
        prior_l2_a = bpp.prior_l2_a;
        prior_l2_b = bpp.prior_l2_b;

        prior_z = zeros<vec>(3);
        prior_z[0] = 0.5;  // prior for root
        prior_z[1] = 0.5;
        prior_z = log(prior_z);

        //Han*:
        prior_dir_param=bpp.prior_dir_param;

        if(verbose) cout << "Init lambda" <<endl;
        vector<int> num_missing = vector<int> (S,0);

        //get leaves lambda
        int st = bpp.element_start[CC];
        // shuffle sequence
        vector<int> myvector;
        for (int i=0; i<GG; ++i) myvector.push_back(i);
        std::shuffle ( myvector.begin(), myvector.end(), bpp.twister2);//default_random_engine(bpp.seed)

        vector < vector< int> > Tg2 = vector < vector< int> > (GG, vector<int>(N));
        for(int s=0; s<S; s++){
            string y = _prof.X[s].substr(st, st+GG);
            for(int g=0; g<GG; g++){
                int gs=myvector[g];
                Tg2[g][s] = -1;
                switch (y[gs])
                {
                    case 'a':
                        Tg2[g][s] = 0; break;
                    case 'c':
                        Tg2[g][s] = 1; break;
                    case 'g':
                        Tg2[g][s] = 2; break;
                    case 't':
                        Tg2[g][s] = 3; break;
                    case 'r':
                        break;
                    case 'y':
                        break;
                    case 'k':
                        break;
                    case 'm':
                        break;
                    case 's':
                        break;
                    case 'w':
                        break;
                    default:
                        if(y[gs] == gapchar || y[gs]=='n' || y[gs]=='*') {Tg2[g][s] = 4; break;}
			            Tg2[g][s] = 5;
                }
            }
        }

        vector<int>::iterator ip;
        vector<int> tmp_tg;
        vector<int> myvector2={}; //put all identical bp blocks in front.
        for(int g=0; g<GG; g++){
            tmp_tg=Tg2[g];
            std::sort(tmp_tg.begin(),tmp_tg.end());
            ip=std::unique(tmp_tg.begin(),tmp_tg.end());
            tmp_tg.resize(std::distance(tmp_tg.begin(),ip));
            if(tmp_tg.size()==1){
                myvector2.insert(myvector2.begin(),g);
                idblk_count+=1;
            }else if(tmp_tg.size()==2){
                if(tmp_tg== vector<int>{0,4} || tmp_tg==vector<int>{0,5} || tmp_tg==vector<int>{1,4} ||
                tmp_tg==vector<int>{1,5} || tmp_tg==vector<int>{2,4} || tmp_tg==vector<int>{2,5} ||
                tmp_tg==vector<int>{3,4} || tmp_tg==vector<int>{3,5} || tmp_tg==vector<int>{4,5}){
                    myvector2.insert(myvector2.begin(),g);
                    idblk_count+=1;
                }else{
                    myvector2.push_back(g);
                }
            }else if(tmp_tg==vector<int> {0,4,5} ||tmp_tg==vector<int> {1,4,5} ||tmp_tg==vector<int> {2,4,5} ||tmp_tg==vector<int> {3,4,5}){
                    myvector2.insert(myvector2.begin(),g);
                    idblk_count+=1;
            }else{
                myvector2.push_back(g);
            }
        }
        //cout<<"number of idblk is "<<idblk_count<<". myvec2 length "<<myvector2.size()<<". GG="<<GG<<endl;

        for(int s=0; s<S; s++)
        {
            //const char* y = bpp.X[c][s].c_str();
            string y = _prof.X[s].substr(st, st+GG);

            for(int g=0; g<GG; g++) {
                int gs = myvector2[g];
                lambda[g][s] = zeros<vec>(bpp.num_base);
                lambda[g][s].fill(LOG_ZERO);
                Tg[g][s] = -1;
                switch (y[gs])
                {
                    case 'a':
                        lambda[g][s][0] = 0; Tg[g][s] = 0; break;
                    case 'c':
                        lambda[g][s][1] = 0; Tg[g][s] = 1; break;
                    case 'g':
                        lambda[g][s][2] = 0; Tg[g][s] = 2; break;
                    case 't':
                        lambda[g][s][3] = 0; Tg[g][s] = 3; break;
                    case 'r': // a/g
                        lambda[g][s][0] = 0;
                        lambda[g][s][2] = 0;
                        break;
                    case 'y': // c/t
                        lambda[g][s][1] = 0;
                        lambda[g][s][3] = 0;  break;
                    case 'k': // g/t
                        lambda[g][s][2] = 0;
                        lambda[g][s][3] = 0;  break;
                    case 'm': // a/c
                        lambda[g][s][0] = 0;
                        lambda[g][s][1] = 0;  break;
                    case 's': // g/c
                        lambda[g][s][1] = 0;
                        lambda[g][s][2] = 0;  break;
                    case 'w': // a/t
                        lambda[g][s][0] = 0;
                        lambda[g][s][3] = 0;  break;
                    default:
                        for(int b =0;b<bpp.num_base;b++) lambda[g][s][b] = 0;
                        //if(y[gs] == gapchar) {Tg[g][s] = 4; break;}
                        if(y[gs] == gapchar || y[gs]=='n' || y[gs]=='*') {Tg[g][s] = 4; break;}
			            Tg[g][s] = 5;
                }
            }

        }

        // remove columns with nearly all gaps
        if(revgap < 1)
        {
            //remove bases with 'n'/'*' or gap in more than 80% species
            vector<int> missingBase;
            for(int g=0; g<GG; g++)
            {
                int mis = 0;
                for(int s=0; s<S; s++){
                    if(Tg[g][s] >= 4)
                    {
                        mis++;
                    }
                }
                if(mis > S*revgap)
                {
                    missingBase.push_back(g);
                    if(g<idblk_count) idblk_count = idblk_count -1;
                }

            }

            if(GG - missingBase.size() < min_length)
            {
                filter = true;
                return;
            }

            for(vector<int>::reverse_iterator it = missingBase.rbegin(); it!= missingBase.rend(); it ++)
            {
                lambda.erase(lambda.begin() + *it);
                Tg.erase(Tg.begin() + *it);
            }

            GG = lambda.size();
        }
        //cout<<"After filter, number of idblk is "<<idblk_count<<". GG="<<GG<<endl;
        if(idblk_count<=5) idblk_count=0;

        // reset GG_block if GG is too long
        //int tot = ceil((double)(GG - 10)/GG_block);
        /*
        if(tot > 5)
        {
            GG_block = ceil((double)(GG - 10)/5); // need blocks >=5!!
        }
        */

        // get gap species after filtering
        for(int s=0; s<S; s++)
        {
            for(int g= 0; g < GG; g++)
            {
                if( Tg[g][s] == 4) //10Nov: >=4?
                {
                    num_missing[s] ++;
                }

            }
        }

        children2    = new int[N][2];
        for(int i=0; i<N; i++)
        {
            children2[i][0] = bpp.children[i][0];
            children2[i][1] = bpp.children[i][1];
        }

        // set missing only for extant species and upper
        missing = vector<bool>(N, false);
        // set missing for all, will be used in getUpdateNode in sample_rate
        for(int s=S; s<N; s++)
        {
            int* p = bpp.children[s];
            for(int cc=0;cc<2;cc++)
            {
                int chi = p[cc];
                if(chi<S && num_missing[chi]>missing_thres*GG)
                {
                    missing[chi] = true;
                }
            }
            if(missing[p[0]] && missing[p[1]]) {
                missing[s] = true;
            }
        }

        int ct =0;
        for(vector<int>::iterator it = bpp.conservedgroup.begin(); it<bpp.conservedgroup.end();it++)
        {
            if(missing[*it]){
                ct +=1;
            }
        }

        if(ct > bpp.conserve_prop * bpp.conservedgroup.size())
        {
            filter = true;
            return;
        }

        //prune tree if outgroup not conserved, find root
        if(prune)
        {
            for(int s=S; s<N; s++)
            {
                int* p = bpp.children[s];
                for(int cc =0 ;cc < 2; cc++)
                {
                    if(missing[p[cc]])
                    {
                        children2[s][cc] = -1;
                    }
                }
            }
        }else{
            //similar to above, but only cares about nodes on lineages leading to outgroup
            getUppertree_missing(bpp); // get children2: if kid[s] is missing, then s does not have this kid, (reflected in children2)
        }

         //init gtree lambda/Tg
        for(int s = 0; s< S; s++)
        {
            gtree->lambda[s][s] = vector<mat>(GG);
            gtree->Tg[s][s] = vector<int>(GG);

            for(int g = 0; g < GG; g++)
            {
                gtree->lambda[s][s][g] = zeros<mat>(bpp.num_base, 3);
                gtree->lambda[s][s][g].col(2) = lambda[g][s];
                gtree->Tg[s][s][g] = Tg[g][s];
            }
        }

        lambda.clear();
        Tg.clear();

        if(verbose) cout << "root: " << root << endl;

        getSubtree(root, nodes);  // will use children2 (only here)
        for(vector<int>::iterator it = nodes.begin(); it < nodes.end(); it++)
        {
            if(bpp.upper.find(*it)!=bpp.upper.end())
            {
                upper_c.push_back(*it);
            }

            if(bpp.upper_conserve.find(*it)!=bpp.upper_conserve.end())
            {
                upper_conserve_c.push_back(*it);
            }

        }

        //Han*
        pi=bpp.pi;
        log_pi = bpp.log_pi;
        inst_rate=bpp.inst_rate;
        submat=bpp.submat;
        eigenvec=bpp.eigenvec;
        eigenval=bpp.eigenval;
        eigeninv=bpp.eigeninv;

        // from initMCMC
        trace_loglik = vector<double >(num_burn+num_mcmc, 0); //P(X|Z, r)
        trace_full_loglik = vector<double >(num_burn+num_mcmc, 0); //P(X, Z, r)
        trace_Z = vector<vector<int> >(num_burn+num_mcmc, vector<int>(N,1));
        trace_n_rate = vector<double >(num_burn+num_mcmc, 0);
        trace_c_rate = vector<double >(num_burn+num_mcmc, 0);
        trace_l_rate = vector<double >(num_burn+num_mcmc, bpp.cur_lrate[CC]);
        trace_g_rate = vector<double >(num_burn+num_mcmc, bpp.cur_grate[CC]);
        trace_l2_rate = vector<double >(num_burn+num_mcmc, 0);
        trace_logNormratio = vector<double>(num_burn+num_mcmc, 0);

        //Han*:
        trace_pi=vector<vector<double>> (num_burn+num_mcmc,vector<double>(4,0.25)); //all inialized to pi from input
        //Han**: 0322-debug
        trace_indicator=vector<double>(num_burn+num_mcmc,0);
        trace_GTtopChg = vector<int>(num_burn+num_mcmc,0);

        //initalize Z, all Z are 1 except the root
        for(int res =0; res < 3; res ++)
        {
            bpp.cur_Z[res][CC] = vector<int>(N,-1);
            for(vector<int>::iterator it = nodes.begin();it<nodes.end();it++) {
                bpp.cur_Z[res][CC][*it] = 1;
            }
            bpp.cur_Z[res][CC][root] = 0;
        }
        log_emission = vector<vector<double> >(N, vector<double>(3,0));
    }

    ~BPP_C()
    {
        //gsl_rng_free(RNG);
        delete [] children2;
        delete gtree;
    }

    void getSubtree(int root, vector<int> & visited_init);
    void getUppertree_missing(BPP & bpp);
    void initMCMC(int iter, int max_iter, BPP&bpp, int resZ, bool prune, bool fixtree = false);
    void getUpdateNode(vector<int> changedZ, vector<bool> & visited_init, BPP & bpp);
    void getUpdateNode(bool neut,vector<bool> & visited_init, BPP & bpp);
    void MonitorChain(int m,int iter, int max_iter, BPP &bpp, double ind_prop, const double add_loglik, const int resZ, bool recordtree = true) ;
    double sample_rate(int indictor,int iter, int max_iter, vector<int> lens, int resZ, double old_rate, bool neut, vector<bool> visited, vec & loglik_old, BPP& bpp, int M =1, bool adaptive = true, double adaptive_factor = 0.3);
    void Gibbs(int iter, int max_iter, BPP &bpp, ofstream & outZ, string output_path,string output_path2,int resZ, bool UpR, bool UpHyper, double lrate_prop, double grate_prop, bool WL = true);
    vector<int> Update_Z(int len, BPP &bpp);
    void Output_init(string output_path, string output_path2, BPP& bpp, ofstream& outZ, ofstream& out_tree, int mod_GT);
    void Output_tree(int iter, string outpathG, BPP &bpp, int resZ);
    void Output_sampling(int iter, string output_path2, BPP &bpp, int resZ);
    void getEmission(int len, BPP & bpp);
    vector<int>  Move_Z(int & propConf, int & revConf,  int & changeZ);
    void sample_transition( double  & gr, double  & lr, double  & lr2, BPP& bpp);
    void simulate(BPP& bpp, PhyloProf & profile, char gapchar, bool prune);
    double priorP_Z(BPP & bpp);
    void getQmat(vec & statR, vec & instR);
    int sample_pi(int m, int indictor, vector<int> lens, int resZ, double piA_old, vec &loglik_old, double pi_delta , BPP &bpp, vector<double> obsCount, int resample_burnin);
    void Output_GTsampling(string output_path2,BPP& bpp, int resZ);
    void printSptree(BPP& bpp, int l=0);
};

#endif /* bpp_c_hpp */


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <orea/aggregation/dimcalculator.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/vectorutils.hpp>
#include <ql/errors.hpp>
#include <ql/time/calendars/weekendsonly.hpp>
#include <ql/version.hpp>

#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/generallinearleastsquares.hpp>
#include <ql/math/kernelfunctions.hpp>
#include <ql/methods/montecarlo/lsmbasissystem.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#include <qle/math/nadarayawatson.hpp>
#include <qle/math/stabilisedglls.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>

using namespace std;
using namespace QuantLib;

using namespace boost::accumulators;

namespace ore {
namespace analytics {

DynamicInitialMarginCalculator::DynamicInitialMarginCalculator(
    const boost::shared_ptr<Portfolio>& portfolio, const boost::shared_ptr<NPVCube>& cube,
    const boost::shared_ptr<CubeInterpretation>& cubeInterpretation,
    const boost::shared_ptr<AggregationScenarioData>& scenarioData, Real quantile, Size horizonCalendarDays,
    const std::map<std::string, Real>& currentIM)
    : portfolio_(portfolio), cube_(cube), cubeInterpretation_(cubeInterpretation), scenarioData_(scenarioData),
      quantile_(quantile), horizonCalendarDays_(horizonCalendarDays), currentIM_(currentIM) {

    QL_REQUIRE(portfolio_, "portfolio is null");
    QL_REQUIRE(cube_, "cube is null");
    QL_REQUIRE(cubeInterpretation_, "cube interpretation is null");
    QL_REQUIRE(scenarioData_, "aggregation scenario data is null");

    boost::shared_ptr<RegularCubeInterpretation> regCubeInt =
        boost::dynamic_pointer_cast<RegularCubeInterpretation>(cubeInterpretation_);
    cubeIsRegular_ = (regCubeInt != NULL);
    datesLoopSize_ = cubeIsRegular_ ? cube_->dates().size() - 1 : cube_->dates().size();

    Size dates = cube_->dates().size();
    Size samples = cube_->samples();

    if (!cubeInterpretation_->hasMporFlows(cube_)) {
        WLOG("cube holds no mpor flows, will assume no flows in the dim calculation");
    }

    // initialise aggregate NPV and Flow by date and scenario
    set<string> nettingSets;
    for (Size i = 0; i < portfolio_->size(); ++i) {
        string tradeId = portfolio_->trades()[i]->id();
        string nettingSetId = portfolio_->trades()[i]->envelope().nettingSetId();
        if (nettingSets.find(nettingSetId) == nettingSets.end()) {
            nettingSets.insert(nettingSetId);
            nettingSetNPV_[nettingSetId] = vector<vector<Real>>(dates, vector<Real>(samples, 0.0));
            nettingSetCloseOutNPV_[nettingSetId] = vector<vector<Real>>(dates, vector<Real>(samples, 0.0));
            nettingSetFLOW_[nettingSetId] = vector<vector<Real>>(dates, vector<Real>(samples, 0.0));
            nettingSetDeltaNPV_[nettingSetId] = vector<vector<Real>>(dates, vector<Real>(samples, 0.0));
            nettingSetDIM_[nettingSetId] = vector<vector<Real>>(dates, vector<Real>(samples, 0.0));
            nettingSetExpectedDIM_[nettingSetId] = vector<Real>(dates, 0.0);
        }
        for (Size j = 0; j < datesLoopSize_; ++j) {
            for (Size k = 0; k < samples; ++k) {
                Real defaultNpv = cubeInterpretation_->getDefaultNpv(cube_, i, j, k);
                Real closeOutNpv = cubeInterpretation_->getCloseOutNpv(cube_, i, j, k);
                Real mporFlow =
                    cubeInterpretation_->hasMporFlows(cube_) ? cubeInterpretation_->getMporFlows(cube_, i, j, k) : 0.0;
                nettingSetNPV_[nettingSetId][j][k] += defaultNpv;
                nettingSetCloseOutNPV_[nettingSetId][j][k] += closeOutNpv;
                nettingSetFLOW_[nettingSetId][j][k] += mporFlow;
            }
        }
    }

    for (auto n : nettingSets)
        nettingSetIds_.push_back(n);

    dimCube_ = boost::make_shared<SinglePrecisionInMemoryCube>(cube_->asof(), nettingSetIds_, cube_->dates(),
                                                               cube_->samples());
}

const vector<vector<Real>>& DynamicInitialMarginCalculator::dynamicIM(const std::string& nettingSet) {
    if (nettingSetDIM_.find(nettingSet) != nettingSetDIM_.end())
        return nettingSetDIM_[nettingSet];
    else
        QL_FAIL("netting set " << nettingSet << " not found in DIM results");
}

const vector<Real>& DynamicInitialMarginCalculator::expectedIM(const std::string& nettingSet) {
    if (nettingSetExpectedDIM_.find(nettingSet) != nettingSetExpectedDIM_.end())
        return nettingSetExpectedDIM_[nettingSet];
    else
        QL_FAIL("netting set " << nettingSet << " not found in expected DIM results");
}

const vector<vector<Real>>& DynamicInitialMarginCalculator::cashFlow(const std::string& nettingSet) {
    if (nettingSetFLOW_.find(nettingSet) != nettingSetFLOW_.end())
        return nettingSetFLOW_[nettingSet];
    else
        QL_FAIL("netting set " << nettingSet << " not found in DIM results");
}

void DynamicInitialMarginCalculator::exportDimEvolution(ore::data::Report& dimEvolutionReport) {

    Size samples = dimCube_->samples();
    Size stopDatesLoop = datesLoopSize_;
    Date asof = cube_->asof();

    dimEvolutionReport.addColumn("TimeStep", Size())
        .addColumn("Date", Date())
        .addColumn("DaysInPeriod", Size())
        .addColumn("AverageDIM", Real(), 6)
        .addColumn("AverageFLOW", Real(), 6)
        .addColumn("NettingSet", string())
        .addColumn("Time", Real(), 6);

    for (auto nettingSet : dimCube_->ids()) {

        LOG("Export DIM evolution for netting set " << nettingSet);
        for (Size i = 0; i < stopDatesLoop; ++i) {
            Real expectedFlow = 0.0;
            for (Size j = 0; j < samples; ++j) {
                expectedFlow += nettingSetFLOW_[nettingSet][i][j] / samples;
            }

            Date defaultDate = dimCube_->dates()[i];
	    Time t = ActualActual().yearFraction(asof, defaultDate);
            Size days = cubeInterpretation_->getMporCalendarDays(dimCube_, i);
            dimEvolutionReport.next()
                .add(i)
                .add(defaultDate)
                .add(days)
                .add(nettingSetExpectedDIM_[nettingSet][i])
                .add(expectedFlow)
                .add(nettingSet)
	        .add(t);
        }
    }
    dimEvolutionReport.end();
    LOG("Exporting expected DIM through time done");
}

} // namespace analytics
} // namespace ore


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/as.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <cstdint>

namespace boost { namespace simd
{
  template<typename Type, detail::bits_t<double> Double, detail::bits_t<float> Single>
  BOOST_FORCEINLINE Type Real()
  {
    return detail::constant ( typename detail::constantify< boost::dispatch::scalar_of_t<Type>
                                                          , Double,Single
                                                          >::type{}
                            , boost::simd::as_<Type>{}
                            );
  }

  template<typename T> BOOST_FORCEINLINE
  auto Real(boost::dispatch::as_<T> const&) BOOST_NOEXCEPT_DECLTYPE(Real<T>())
  {
    return Real<T>();
  }
} }

#endif


// Copyright (c) 2016 University of Minnesota
//
// ADMM-Elastic Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other materials
//    provided with the distribution.
// THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR  A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF MINNESOTA, DULUTH OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
// IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef ADMM_DYNAMICCOLLISION_HPP
#define ADMM_DYNAMICCOLLISION_HPP

#include <Eigen/Dense>
#include "MCL/BVH.hpp"
#include "Collider.hpp"

namespace admm {

class TetMeshCollision : public DynamicCollision {
private:
	mcl::bvh::AABBTree<float,3> m_faces_tree;
	mcl::bvh::AABBTree<double,4> m_tets_tree;

	int vert_offset;
	std::vector<mcl::Vec4i> mesh_tets;
	std::vector<mcl::Vec3i> mesh_faces;
	std::vector<mcl::Vec3f> mesh_rest_verts;
	const VecX *mesh_verts;

public:
	typedef Eigen::Matrix<double,3,1> Vec3;
	typedef Eigen::Matrix<double,Eigen::Dynamic,1> VecX;

	// Constructor with vertex offset (to index into global vertex array)
	TetMeshCollision( const std::shared_ptr<mcl::TetMesh> mesh, int v_offset ) : vert_offset(v_offset) {
		if( mesh->faces.size() == 0 ){
			throw std::runtime_error( "**TetMeshCollision Error: TetMesh needs surface faces" );
		}

		// Create a tree of the rest faces
		mesh_faces = mesh->faces;
		mesh_rest_verts = mesh->vertices;
		m_faces_tree.init( &mesh_faces[0][0], &mesh->vertices[0][0], mesh_faces.size() );

		// Update tet indices
		int n_tets = mesh->tets.size();
		mesh_tets = mesh->tets;
		const mcl::Vec4i tet_offset = mcl::Vec4i(vert_offset,vert_offset,vert_offset,vert_offset);
		for( int i=0; i<n_tets; ++i ){ mesh_tets[i] += tet_offset; }

	} // end constructor

	// Update the dynamic tet mesh vertices
	void update( const VecX &x ){
		mesh_verts = &x;
		m_tets_tree.init( &mesh_tets[0][0], mesh_verts->data(), mesh_tets.size() );
	}

	// Compute signed distance on the tet mesh
	void signed_distance( const Vec3 &x, Payload &p ) const {
		if( p.dx < 0 ){ return; }// only resolve one dynamic collision at a time

		// Do a point-in-tet test
		mcl::bvh::PointInTet<double> point_in_tet( x, mesh_verts->data(), &mesh_tets[0][0] );
		point_in_tet.skip_vert_idx.push_back( p.vert_idx );
		bool hit = m_tets_tree.traverse( point_in_tet );

		// If we're inside a tet, find the nearest surface
		if( hit ){

			Vec3 restx;
			{ // Compute point in rest pose
				mcl::Vec4i tet = mesh_tets[ point_in_tet.hit_tet ];
				const Vec3 &p0 = mesh_verts->segment<3>(tet[0]*3);
				const Vec3 &p1 = mesh_verts->segment<3>(tet[1]*3);
				const Vec3 &p2 = mesh_verts->segment<3>(tet[2]*3);
				const Vec3 &p3 = mesh_verts->segment<3>(tet[3]*3);
				mcl::Vec4d hitbarys = mcl::vec::barycoords( x, p0, p1, p2, p3 );
				tet -= mcl::Vec4i(vert_offset,vert_offset,vert_offset,vert_offset);
				restx = hitbarys[0]*mesh_rest_verts[tet[0]].cast<double>() +
					hitbarys[1]*mesh_rest_verts[tet[1]].cast<double>() +
					hitbarys[2]*mesh_rest_verts[tet[2]].cast<double>() +
					hitbarys[3]*mesh_rest_verts[tet[3]].cast<double>();
			}

			mcl::bvh::NearestTriangle<float> nearest_tri( restx.cast<float>(), &mesh_rest_verts[0][0], &mesh_faces[0][0] );
			nearest_tri.skip_vert_idx.push_back( p.vert_idx-vert_offset );
			m_faces_tree.traverse( nearest_tri );
			if( nearest_tri.hit_tri < 0 ){
				throw std::runtime_error("TetMeshCollision Error: Could not find a nearest face");
			}
			mcl::Vec3i hit_face = mesh_faces[ nearest_tri.hit_tri ];
			const mcl::Vec3f &p0 = mesh_rest_verts[hit_face[0]];
			const mcl::Vec3f &p1 = mesh_rest_verts[hit_face[1]];
			const mcl::Vec3f &p2 = mesh_rest_verts[hit_face[2]];
			mcl::Vec3f norm = (p1-p0).cross(p2-p0);
			norm.normalize();

			// Set the payload data
			p.dx = -1.0*(nearest_tri.proj-restx.cast<float>()).norm();
			p.face = hit_face + mcl::Vec3i(vert_offset,vert_offset,vert_offset);
			p.barys = mcl::vec::barycoords(nearest_tri.proj, p0, p1, p2).cast<double>();
			p.normal = norm.cast<double>();

		}

	} // end signed distance

}; // class dynamic collision

} // namespace admm

#endif


// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <carla/geom/BoundingBox.h>
#include <carla/geom/GeoLocation.h>
#include <carla/geom/Location.h>
#include <carla/geom/Rotation.h>
#include <carla/geom/Transform.h>
#include <carla/geom/Vector2D.h>
#include <carla/geom/Vector3D.h>

#include <boost/python/implicit.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <ostream>

namespace carla {
namespace geom {

  template <typename T>
  static void WriteVector2D(std::ostream &out, const char *name, const T &vector2D) {
    out << name
        << "(x=" << std::to_string(vector2D.x)
        << ", y=" << std::to_string(vector2D.y) << ')';
  }

  template <typename T>
  static void WriteVector3D(std::ostream &out, const char *name, const T &vector3D) {
    out << name
        << "(x=" << std::to_string(vector3D.x)
        << ", y=" << std::to_string(vector3D.y)
        << ", z=" << std::to_string(vector3D.z) << ')';
  }

  std::ostream &operator<<(std::ostream &out, const Vector2D &vector2D) {
    WriteVector2D(out, "Vector2D", vector2D);
    return out;
  }

  std::ostream &operator<<(std::ostream &out, const Vector3D &vector3D) {
    WriteVector3D(out, "Vector3D", vector3D);
    return out;
  }

  std::ostream &operator<<(std::ostream &out, const Location &location) {
    WriteVector3D(out, "Location", location);
    return out;
  }

  std::ostream &operator<<(std::ostream &out, const Rotation &rotation) {
    out << "Rotation(pitch=" << std::to_string(rotation.pitch)
        << ", yaw=" << std::to_string(rotation.yaw)
        << ", roll=" << std::to_string(rotation.roll) << ')';
    return out;
  }

  std::ostream &operator<<(std::ostream &out, const Transform &transform) {
    out << "Transform(" << transform.location << ", " << transform.rotation << ')';
    return out;
  }

  std::ostream &operator<<(std::ostream &out, const BoundingBox &box) {
    out << "BoundingBox(" << box.location << ", ";
    WriteVector3D(out, "Extent", box.extent);
    out << ')';
    return out;
  }

  std::ostream &operator<<(std::ostream &out, const GeoLocation &geo_location) {
    out << "GeoLocation(latitude=" << std::to_string(geo_location.latitude)
        << ", longitude=" << std::to_string(geo_location.longitude)
        << ", altitude=" << std::to_string(geo_location.altitude) << ')';
    return out;
  }

} // namespace geom
} // namespace carla

static void TransformList(const carla::geom::Transform &self, boost::python::list &list) {
  auto length = boost::python::len(list);
   for (auto i = 0u; i < length; ++i) {
    self.TransformPoint(boost::python::extract<carla::geom::Vector3D &>(list[i]));
  }
}

static boost::python::list BuildMatrix(const std::array<float, 16> &m) {
  boost::python::list r_out;
  boost::python::list r[4];
  for (uint8_t i = 0; i < 16; ++i) { r[uint8_t(i / 4)].append(m[i]); }
  for (uint8_t i = 0; i < 4; ++i) { r_out.append(r[i]); }
  return r_out;
}

static auto GetTransformMatrix(const carla::geom::Transform &self) {
  return BuildMatrix(self.GetMatrix());
}

static auto GetInverseTransformMatrix(const carla::geom::Transform &self) {
  return BuildMatrix(self.GetInverseMatrix());
}

void export_geom() {
  using namespace boost::python;
  namespace cg = carla::geom;
  class_<std::vector<cg::Vector2D>>("vector_of_vector2D")
      .def(boost::python::vector_indexing_suite<std::vector<cg::Vector2D>>())
      .def(self_ns::str(self_ns::self))
  ;

  class_<cg::Vector2D>("Vector2D")
    .def(init<float, float>((arg("x")=0.0f, arg("y")=0.0f)))
    .def_readwrite("x", &cg::Vector2D::x)
    .def_readwrite("y", &cg::Vector2D::y)
    .def("__eq__", &cg::Vector2D::operator==)
    .def("__ne__", &cg::Vector2D::operator!=)
    .def(self += self)
    .def(self + self)
    .def(self -= self)
    .def(self - self)
    .def(self *= double())
    .def(self * double())
    .def(double() * self)
    .def(self /= double())
    .def(self / double())
    .def(double() / self)
    .def(self_ns::str(self_ns::self))
  ;

  implicitly_convertible<cg::Vector3D, cg::Location>();
  implicitly_convertible<cg::Location, cg::Vector3D>();

  class_<cg::Vector3D>("Vector3D")
    .def(init<float, float, float>((arg("x")=0.0f, arg("y")=0.0f, arg("z")=0.0f)))
    .def(init<const cg::Location &>((arg("rhs"))))
    .def_readwrite("x", &cg::Vector3D::x)
    .def_readwrite("y", &cg::Vector3D::y)
    .def_readwrite("z", &cg::Vector3D::z)
    .def("__eq__", &cg::Vector3D::operator==)
    .def("__ne__", &cg::Vector3D::operator!=)
    .def(self += self)
    .def(self + self)
    .def(self -= self)
    .def(self - self)
    .def(self *= double())
    .def(self * double())
    .def(double() * self)
    .def(self /= double())
    .def(self / double())
    .def(double() / self)
    .def(self_ns::str(self_ns::self))
  ;

  class_<cg::Location, bases<cg::Vector3D>>("Location")
    .def(init<float, float, float>((arg("x")=0.0f, arg("y")=0.0f, arg("z")=0.0f)))
    .def(init<const cg::Vector3D &>((arg("rhs"))))
    .add_property("x", +[](const cg::Location &self) { return self.x; }, +[](cg::Location &self, float x) { self.x = x; })
    .add_property("y", +[](const cg::Location &self) { return self.y; }, +[](cg::Location &self, float y) { self.y = y; })
    .add_property("z", +[](const cg::Location &self) { return self.z; }, +[](cg::Location &self, float z) { self.z = z; })
    .def("distance", &cg::Location::Distance, (arg("location")))
    .def("__eq__", &cg::Location::operator==)
    .def("__ne__", &cg::Location::operator!=)
    .def(self_ns::str(self_ns::self))
  ;

  class_<cg::Rotation>("Rotation")
    .def(init<float, float, float>((arg("pitch")=0.0f, arg("yaw")=0.0f, arg("roll")=0.0f)))
    .def_readwrite("pitch", &cg::Rotation::pitch)
    .def_readwrite("yaw", &cg::Rotation::yaw)
    .def_readwrite("roll", &cg::Rotation::roll)
    .def("get_forward_vector", &cg::Rotation::GetForwardVector)
    .def("get_right_vector", &cg::Rotation::GetRightVector)
    .def("get_up_vector", &cg::Rotation::GetUpVector)
    .def("__eq__", &cg::Rotation::operator==)
    .def("__ne__", &cg::Rotation::operator!=)
    .def(self_ns::str(self_ns::self))
  ;

  class_<cg::Transform>("Transform")
    .def(init<cg::Location, cg::Rotation>(
        (arg("location")=cg::Location(), arg("rotation")=cg::Rotation())))
    .def_readwrite("location", &cg::Transform::location)
    .def_readwrite("rotation", &cg::Transform::rotation)
    .def("transform", &TransformList)
    .def("transform", +[](const cg::Transform &self, cg::Vector3D &location) {
      self.TransformPoint(location);
      return location;
    }, arg("in_point"))
    .def("get_forward_vector", &cg::Transform::GetForwardVector)
    .def("get_right_vector", &cg::Transform::GetRightVector)
    .def("get_up_vector", &cg::Transform::GetUpVector)
    .def("get_matrix", &GetTransformMatrix)
    .def("get_inverse_matrix", &GetInverseTransformMatrix)
    .def("__eq__", &cg::Transform::operator==)
    .def("__ne__", &cg::Transform::operator!=)
    .def(self_ns::str(self_ns::self))
  ;

  class_<std::vector<cg::Transform>>("vector_of_transform")
      .def(boost::python::vector_indexing_suite<std::vector<cg::Transform>>())
      .def(self_ns::str(self_ns::self))
  ;

  class_<cg::BoundingBox>("BoundingBox")
    .def(init<cg::Location, cg::Vector3D>(
        (arg("location")=cg::Location(), arg("extent")=cg::Vector3D())))
    .def_readwrite("location", &cg::BoundingBox::location)
    .def_readwrite("extent", &cg::BoundingBox::extent)
    .def("contains", &cg::BoundingBox::Contains, arg("point"), arg("bbox_transform"))
    .def("get_local_vertices", CALL_RETURNING_LIST(cg::BoundingBox, GetLocalVertices))
    .def("get_world_vertices", CALL_RETURNING_LIST_1(cg::BoundingBox, GetWorldVertices, const cg::Transform&), arg("bbox_transform"))
    .def("__eq__", &cg::BoundingBox::operator==)
    .def("__ne__", &cg::BoundingBox::operator!=)
    .def(self_ns::str(self_ns::self))
  ;

  class_<cg::GeoLocation>("GeoLocation")
    .def(init<double, double, double>((arg("latitude")=0.0, arg("longitude")=0.0, arg("altitude")=0.0)))
    .def_readwrite("latitude", &cg::GeoLocation::latitude)
    .def_readwrite("longitude", &cg::GeoLocation::longitude)
    .def_readwrite("altitude", &cg::GeoLocation::altitude)
    .def("__eq__", &cg::GeoLocation::operator==)
    .def("__ne__", &cg::GeoLocation::operator!=)
    .def(self_ns::str(self_ns::self))
  ;
}


/*
 * shoebox.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <scitbx/array_family/flex_types.h>
#include <dials/model/data/shoebox.h>
#include <dials/config.h>

namespace dials { namespace model { namespace boost_python {

  using namespace boost::python;
  using scitbx::af::flex;
  using scitbx::af::flex_int;

  /** Set the data array as a flex array */
  template <typename FloatType>
  void set_data(Shoebox<FloatType> &obj, typename flex<FloatType>::type &data) {
    DIALS_ASSERT(data.accessor().all().size() == 3);
    obj.data = af::versa<FloatType, af::c_grid<3> >(data.handle(),
                                                    af::c_grid<3>(data.accessor()));
  }

  /** Set the mask array as a flex array */
  template <typename FloatType>
  void set_mask(Shoebox<FloatType> &obj, flex_int &mask) {
    DIALS_ASSERT(mask.accessor().all().size() == 3);
    obj.mask =
      af::versa<int, af::c_grid<3> >(mask.handle(), af::c_grid<3>(mask.accessor()));
  }

  /** Set the bgrd array as a flex array */
  template <typename FloatType>
  void set_background(Shoebox<FloatType> &obj,
                      typename flex<FloatType>::type &background) {
    DIALS_ASSERT(background.accessor().all().size() == 3);
    obj.background = af::versa<FloatType, af::c_grid<3> >(
      background.handle(), af::c_grid<3>(background.accessor()));
  }

  /** Get a list of shoebox pixel coordinates. */
  template <typename ShoeboxType>
  af::shared<vec3<double> > coords(const ShoeboxType &self) {
    af::shared<vec3<double> > result;
    for (std::size_t k = 0; k < self.zsize(); ++k) {
      for (std::size_t j = 0; j < self.ysize(); ++j) {
        for (std::size_t i = 0; i < self.xsize(); ++i) {
          double x = i + self.xoffset() + 0.5;
          double y = j + self.yoffset() + 0.5;
          double z = k + self.zoffset() + 0.5;
          result.push_back(vec3<double>(x, y, z));
        }
      }
    }
    return result;
  }

  /** Get a list of shoebox pixel coordinates. */
  template <typename ShoeboxType>
  af::shared<vec3<double> > coords_with_mask(
    const ShoeboxType &self,
    const af::const_ref<bool, af::c_grid<3> > mask) {
    DIALS_ASSERT(mask.accessor().all_eq(self.size()));
    af::shared<vec3<double> > result;
    for (std::size_t k = 0; k < self.zsize(); ++k) {
      for (std::size_t j = 0; j < self.ysize(); ++j) {
        for (std::size_t i = 0; i < self.xsize(); ++i) {
          if (mask(k, j, i)) {
            double x = i + self.xoffset() + 0.5;
            double y = j + self.yoffset() + 0.5;
            double z = k + self.zoffset() + 0.5;
            result.push_back(vec3<double>(x, y, z));
          }
        }
      }
    }
    return result;
  }

  /** Get a list of shoebox pixel values. */
  template <typename ShoeboxType>
  af::shared<double> values(const ShoeboxType &self) {
    DIALS_ASSERT(self.is_consistent());
    af::shared<double> result;
    for (std::size_t k = 0; k < self.zsize(); ++k) {
      for (std::size_t j = 0; j < self.ysize(); ++j) {
        for (std::size_t i = 0; i < self.xsize(); ++i) {
          result.push_back(self.data(k, j, i));
        }
      }
    }
    return result;
  }

  /** Get a list of shoebox pixel values. */
  template <typename ShoeboxType>
  af::shared<double> values_with_mask(const ShoeboxType &self,
                                      const af::const_ref<bool, af::c_grid<3> > mask) {
    DIALS_ASSERT(mask.accessor().all_eq(self.size()));
    DIALS_ASSERT(self.is_consistent());
    af::shared<double> result;
    for (std::size_t k = 0; k < self.zsize(); ++k) {
      for (std::size_t j = 0; j < self.ysize(); ++j) {
        for (std::size_t i = 0; i < self.xsize(); ++i) {
          if (mask(k, j, i)) {
            result.push_back(self.data(k, j, i));
          }
        }
      }
    }
    return result;
  }

  /** Get a list of shoebox pixel coordinates. */
  template <typename ShoeboxType>
  af::shared<vec3<double> > beam_vectors(const ShoeboxType &self,
                                         const Detector &detector) {
    af::shared<vec3<double> > result;
    for (std::size_t k = 0; k < self.zsize(); ++k) {
      for (std::size_t j = 0; j < self.ysize(); ++j) {
        for (std::size_t i = 0; i < self.xsize(); ++i) {
          double x = i + self.xoffset() + 0.5;
          double y = j + self.yoffset() + 0.5;
          vec2<double> c(x, y);
          result.push_back(detector[self.panel].get_pixel_lab_coord(c));
        }
      }
    }
    return result;
  }

  /** Get a list of shoebox pixel coordinates. */
  template <typename ShoeboxType>
  af::shared<vec3<double> > beam_vectors_with_mask(
    const ShoeboxType &self,
    const Detector &detector,
    const af::const_ref<bool, af::c_grid<3> > mask) {
    DIALS_ASSERT(mask.accessor().all_eq(self.size()));
    af::shared<vec3<double> > result;
    for (std::size_t k = 0; k < self.zsize(); ++k) {
      for (std::size_t j = 0; j < self.ysize(); ++j) {
        for (std::size_t i = 0; i < self.xsize(); ++i) {
          if (mask(k, j, i)) {
            double x = i + self.xoffset() + 0.5;
            double y = j + self.yoffset() + 0.5;
            vec2<double> c(x, y);
            result.push_back(detector[self.panel].get_pixel_lab_coord(c));
          }
        }
      }
    }
    return result;
  }

  template <typename FloatType>
  class_<Shoebox<FloatType> > shoebox_wrapper(const char *name) {
    typedef Shoebox<FloatType> shoebox_type;

    class_<shoebox_type> shoebox(name);
    shoebox.def(init<const shoebox_type &>())
      .def(init<std::size_t, const int6 &>())
      .def(init<const int6 &>())
      .add_property(
        "data",
        make_getter(&shoebox_type::data, return_value_policy<return_by_value>()),
        &set_data<FloatType>)
      .add_property(
        "mask",
        make_getter(&shoebox_type::mask, return_value_policy<return_by_value>()),
        &set_mask<FloatType>)
      .add_property(
        "background",
        make_getter(&shoebox_type::background, return_value_policy<return_by_value>()),
        &set_background<FloatType>)
      .add_property(
        "bbox",
        make_getter(&shoebox_type::bbox, return_value_policy<return_by_value>()),
        make_setter(&shoebox_type::bbox, return_value_policy<return_by_value>()))
      .def_readwrite("panel", &shoebox_type::panel)
      .def_readwrite("flat", &shoebox_type::flat)
      .def("allocate", &shoebox_type::allocate)
      .def("allocate", &shoebox_type::allocate_with_value)
      .def("deallocate", &shoebox_type::deallocate)
      .def("xoffset", &shoebox_type::xoffset)
      .def("yoffset", &shoebox_type::yoffset)
      .def("zoffset", &shoebox_type::zoffset)
      .def("offset", &shoebox_type::offset)
      .def("xsize", &shoebox_type::xsize)
      .def("ysize", &shoebox_type::ysize)
      .def("zsize", &shoebox_type::zsize)
      .def("size", &shoebox_type::size)
      .def("size_flat", &shoebox_type::size_flat)
      .def("is_consistent", &shoebox_type::is_consistent)
      .def("is_allocated", &shoebox_type::is_allocated)
      .def("is_bbox_within_image_volume",
           &shoebox_type::is_bbox_within_image_volume,
           (arg("image_size"), arg("scan_range")))
      .def("does_bbox_contain_bad_pixels",
           &shoebox_type::does_bbox_contain_bad_pixels,
           (arg("mask")))
      .def("count_mask_values", &shoebox_type::count_mask_values)
      .def("all_foreground_valid", &shoebox_type::all_foreground_valid)
      .def("centroid_all", &shoebox_type::centroid_all)
      .def("centroid_masked", &shoebox_type::centroid_masked)
      .def("centroid_valid", &shoebox_type::centroid_valid)
      .def("centroid_foreground", &shoebox_type::centroid_foreground)
      .def("centroid_strong", &shoebox_type::centroid_strong)
      .def("centroid_all_minus_background",
           &shoebox_type::centroid_all_minus_background)
      .def("centroid_masked_minus_background",
           &shoebox_type::centroid_masked_minus_background)
      .def("centroid_valid_minus_background",
           &shoebox_type::centroid_valid_minus_background)
      .def("centroid_foreground_minus_background",
           &shoebox_type::centroid_foreground_minus_background)
      .def("centroid_strong_minus_background",
           &shoebox_type::centroid_strong_minus_background)
      .def("bayesian_intensity", &shoebox_type::bayesian_intensity)
      .def("summed_intensity", &shoebox_type::summed_intensity)
      .def("flatten", &shoebox_type::flatten)
      .def("coords", &coords<shoebox_type>)
      .def("coords", &coords_with_mask<shoebox_type>)
      .def("values", &values<shoebox_type>)
      .def("values", &values_with_mask<shoebox_type>)
      .def("beam_vectors", &beam_vectors<shoebox_type>)
      .def("beam_vectors", &beam_vectors_with_mask<shoebox_type>)
      .def("__eq__", &shoebox_type::operator==)
      .def("__ne__", &shoebox_type::operator!=);

    return shoebox;
  }

  void export_shoebox() {
    shoebox_wrapper<ProfileFloatType>("Shoebox");
  }

}}}  // namespace dials::model::boost_python


#include <Spirit_Defines.h>
#include <engine/Method_EMA.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Eigenmodes.hpp>
#include <data/Spin_System.hpp>
#include <io/IO.hpp>
#include <utility/Constants.hpp>
#include <utility/Logging.hpp>

#include <fmt/format.h>
#include <Eigen/Dense>

#include <iostream>
#include <fstream>
#include <thread>
#include <chrono>

using namespace Utility;
namespace C = Utility::Constants;

namespace Engine
{
    Method_EMA::Method_EMA( std::shared_ptr<Data::Spin_System> system, int idx_img, int idx_chain ) :
        Method(system->ema_parameters, idx_img, idx_chain)
    {
        this->systems = std::vector<std::shared_ptr<Data::Spin_System>>(1, system);
        this->SenderName = Log_Sender::EMA;
        this->parameters_ema = system->ema_parameters;

        this->noi = this->systems.size();
        this->nos = this->systems[0]->nos;

        // Attributes needed for applying a mode the spins
        this->angle = scalarfield(this->nos);
        this->angle_initial = scalarfield(this->nos);
        this->axis = vectorfield(this->nos);
        this->spins_initial = *this->systems[0]->spins;

        Eigenmodes::Check_Eigenmode_Parameters(system);

        // Calculate eigenmodes only in case that the selected mode to follow is not computed yet.
        if ( this->systems[0]->modes[this->parameters_ema->n_mode_follow] == NULL )
            Eigenmodes::Calculate_Eigenmodes(system, idx_img, idx_chain);

        this->counter = 0;
        // Trigger update on first iteration
        this->following_mode = -1;
    }

    void Method_EMA::Iteration()
    {
        // If the mode index has changed
        if ( this->following_mode != this->parameters_ema->n_mode_follow )
        {
            this->counter = 0;

            // Re-check validity of parameters and system->modes
            Eigenmodes::Check_Eigenmode_Parameters(this->systems[0]);

            // Reset members
            this->following_mode = this->parameters_ema->n_mode_follow;
            // Restore the initial spin configuration
            (*this->systems[0]->spins) = this->spins_initial;

            // Set the new mode
            this->mode = *this->systems[0]->modes[following_mode];

            // Find the axes of rotation for the mode to visualize
            for (int idx=0; idx<this->nos; idx++)
            {
                this->angle_initial[idx] = this->mode[idx].norm();
                this->axis[idx] = this->spins_initial[idx].cross(this->mode[idx]).normalized();
            }
        }

        auto& image = *this->systems[0]->spins;

        // Calculate n for that iteration based on the initial n displacement vector
        scalar t_angle;
        if ( !this->parameters_ema->snapshot )
            t_angle = this->parameters_ema->amplitude *
                      std::sin( 2*C::Pi * this->counter * this->parameters_ema->frequency );
        else
            t_angle = this->parameters_ema->amplitude;

        this->angle = this->angle_initial;
        Vectormath::scale(this->angle, t_angle);

        // Rotate the spins
        Vectormath::rotate(this->spins_initial, this->axis, this->angle, image);

        ++this->counter;
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    void Method_EMA::Save_Current(std::string starttime, int iteration, bool initial, bool final)
    {
    }

    void Method_EMA::Hook_Pre_Iteration()
    {
    }

    void Method_EMA::Hook_Post_Iteration()
    {
    }

    void Method_EMA::Initialize()
    {
    }

    void Method_EMA::Finalize()
    {
        this->Lock();
        // The initial spin configuration must be restored
        (*this->systems[0]->spins) = this->spins_initial;
        this->Unlock();
    }

    void Method_EMA::Message_Start()
    {
        //---- Log messages
        Log.SendBlock(Log_Level::All, this->SenderName,
        {
            "------------  Started  " + this->Name() + " Visualization ------------",
            "    Mode frequency  " + fmt::format("{}", this->parameters_ema->frequency),
            "    Mode amplitude  " + fmt::format("{}", this->parameters_ema->amplitude),
            "    Number of modes " + fmt::format("{}", this->parameters_ema->n_modes ),
            "-----------------------------------------------------"
        }, this->idx_image, this->idx_chain);
    }

    void Method_EMA::Message_Step()
    {
    }

    void Method_EMA::Message_End()
    {
    }

    // Method name as string
    std::string Method_EMA::Name() { return "EMA"; }

    // Solver name as string
    std::string Method_EMA::SolverName() { return "None"; }
}


#include <NDTMatcherF2F.hh>
#include <NDTMatcher.hh>
#include <NDTMap.hh>
#include <OctTree.hh>
#include <AdaptiveOctTree.hh>
#include "ros/ros.h"
#include "pcl/point_cloud.h"
#include "sensor_msgs/PointCloud2.h"

#include "pcl/io/pcd_io.h"
#include <pcl/filters/radius_outlier_removal.h>

#include "pcl/features/feature.h"
#include <cstdio>
#include <Eigen/Eigen>
#include <PointCloudUtils.hh>
#include <fstream>
#include <NDTMap.hh>
#include <LazzyGrid.hh>

using namespace std;
using namespace lslgeneric;

void filterDensity(pcl::PointCloud<pcl::PointXYZ> &rawCloud, pcl::PointCloud<pcl::PointXYZ> &pc)
{
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> filter;
    filter.setRadiusSearch(0.2);
    filter.setMinNeighborsInRadius(3);
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcptr(new pcl::PointCloud<pcl::PointXYZ>());
    *pcptr = rawCloud;
    filter.setInputCloud(pcptr);
    filter.filter(pc);
}

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

    pcl::PointCloud<pcl::PointXYZ> prev, thisone1, thisone, cloudFinal;
    double []__res = {0.2, 0.4, 1, 2};
    std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
    lslgeneric::NDTMatcherF2F matcher(false, false, false, resolutions);
    //lslgeneric::NDTMatcher matcher;
    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tprev, Tloc, Treg, Tglob;

    pcl::PCDReader reader;
    Tprev.setIdentity();
    Tloc.setIdentity();
    Treg.setIdentity();
    Tglob.setIdentity();

    //we do a single scan to scan registration
    if(argc != 2)
    {
        cout<<"usage: ./reconstruct configFile\n";
        return -1;
    }

    FILE *fin = fopen(argv[1],"r");
    double xd,yd,zd, q1,q2,q3,q4;
    double xoffset=0, yoffset=0, zoffset=-0.01;
    string pcName;
    char *line = NULL;
    size_t len;
    bool first = true;

    while(getline(&line,&len,fin) > 0)
    {

        int n = sscanf(line,"%lf,%lf,%lf %lf",
                       &q1,&q2,&q3,&q4);
        if(n != 4)
        {
            cout<<"wrong format of pose at : "<<line<<endl;
            break;
        }
        if(!getline(&line,&len,fin) > 0) break;
        n = sscanf(line,"%lf,%lf,%lf",
                   &xd,&yd,&zd);
        if(n != 3)
        {
            cout<<"wrong format of pose at : "<<line<<endl;
            break;
        }

        if(!getline(&line,&len,fin) > 0) break;
        pcName = line;
        *(pcName.rbegin()) = '\0';

//	thisone = lslgeneric::readVRML(pcName.c_str());

//	thisone1.width = thisone1.height = 0;
        cout<<"reading "<<pcName<<endl;
        reader.read<pcl::PointXYZ>(pcName,thisone);
        //cout<<"filtering density..."<<thisone1.points.size()<<endl;
        //filterDensity(thisone1,thisone);
        cout<<" --> "<<thisone.points.size()<<endl;
        lslgeneric::writeToVRML("/home/tsv/ndt_tmp/last.wrl",thisone);

        Tloc = Tprev.inverse();
        Tprev = Eigen::Translation<double,3>(xd,yd,zd)*Eigen::AngleAxis<double>(q4,Eigen::Vector3d(q1,q2,q3));
        Tloc = Tloc*Tprev;

        transformPointCloudInPlace(Tloc,thisone);
        cout<<"old pose is t:"<<Tprev.translation().transpose()<<" r "<<Tprev.rotation().eulerAngles(0,1,2).transpose()<<endl;
        cout<<"local pose is t:"<<Tloc.translation().transpose()<<" r "<<Tloc.rotation().eulerAngles(0,1,2).transpose()<<endl;

        if(!first)
        {
            //register
            Treg.setIdentity();
            matcher.match(thisone,prev,Treg);
            cout<<"registration pose is t:"<<Treg.translation().transpose()<<" r "<<Treg.rotation().eulerAngles(0,1,2).transpose()<<endl;
            Tglob = Tglob*Tloc*Treg;
            cout<<"new global pose t:"<<Tglob.translation().transpose()<<" r "<<Tglob.rotation().eulerAngles(0,1,2).transpose()<<endl;
        }
        else
        {
            Tglob = Tloc;
            first = false;
        }

        //set prev to thisone
        prev = lslgeneric::readVRML(pcName.c_str());
        thisone = prev;
        //transform thisone
        transformPointCloudInPlace(Tglob,thisone);

        cout<<"read vrml file at "<<pcName<<endl;
        cloudFinal += thisone;
    }

    fclose(fin);
    lslgeneric::writeToVRML("/home/tsv/ndt_tmp/final.wrl",cloudFinal);
    lslgeneric::writeToVRML("/home/tsv/ndt_tmp/last.wrl",thisone);

#if 0
    //completely unrelated, needed it for other debug TSV
    pcl::PointCloud<pcl::PointXYZ> cl;
    pcl::PointXYZ pt;
    for(double x = 0; x<2; x+=0.1)
    {
        for(double y = 0; y<2; y+=0.1)
        {
            for(double z = 0; z<2; z+=0.1)
            {
                pt.x=x;
                pt.y=y;
                pt.z=z;
                cl.points.push_back(pt);
            }
        }
    }
    lslgeneric::writeToVRML("/home/tsv/ndt_tmp/final2.wrl",cl);
    LazzyGrid pr(0.33);
    NDTMap ndt2( &pr );
    ndt2.loadPointCloud( cl );
    ndt2.computeNDTCells();
    ndt2.writeToVRML("/home/tsv/ndt_tmp/example.wrl");

#endif

}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include "Molassembler/DistanceGeometry/ConformerGeneration.h"

#include <Eigen/Dense>
#include "Utils/Constants.h"
#include "Utils/Typenames.h"

#include "Molassembler/BondStereopermutator.h"
#include "Molassembler/DistanceGeometry/EigenRefinement.h"
#include "Molassembler/DistanceGeometry/Error.h"
#include "Molassembler/DistanceGeometry/ExplicitBoundsGraph.h"
#include "Molassembler/DistanceGeometry/MetricMatrix.h"
#include "Molassembler/DistanceGeometry/RefinementMeta.h"
#include "Molassembler/Graph/GraphAlgorithms.h"
#include "Utils/Math/QuaternionFit.h"

#include "Molassembler/Detail/Cartesian.h"
#include "Molassembler/Temple/Optimization/Lbfgs.h"
#include "Molassembler/Temple/Optionals.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/Random.h"

#include <iostream>

namespace Scine {
namespace Molassembler {
namespace DistanceGeometry {
namespace Detail {

Eigen::MatrixXd gather(const Eigen::VectorXd& vectorizedPositions) {
  constexpr unsigned dimensionality = 4;
  const unsigned N = vectorizedPositions.size() / dimensionality;
  Eigen::MatrixXd positionMatrix(N, 3);
  for(unsigned i = 0; i < N; ++i) {
    positionMatrix.row(i) = vectorizedPositions.template segment<3>(dimensionality * i);
  }
  return positionMatrix;
}

AngstromPositions convertToAngstromPositions(const Eigen::MatrixXd& positions) {
  assert(positions.cols() == 3);
  const unsigned N = positions.rows();
  AngstromPositions angstromWrapper {N};
  for(unsigned i = 0; i < N; ++i) {
    angstromWrapper.positions.row(i) = Utils::Position {
      positions.row(i).transpose()
    };
  }
  return angstromWrapper;
}

Eigen::MatrixXd fitAndSetFixedPositions(
  const Eigen::MatrixXd& positions,
  const Configuration& configuration
) {
  /* Fixed positions postprocessing:
   * - Rotate and translate the generated coordinates towards the fixed
   *   positions indicated for each.
   *
   * Maybe?
   * - Assuming the fit isn't absolutely exact, overwrite the existing
   *   positions with the fixed ones
   */
  /* Construct a reference matrix from the fixed positions (these are still in
   * bohr!) and a weights vector
   */
  assert(positions.cols() == 3);
  const unsigned N = positions.rows();
  Eigen::MatrixXd referenceMatrix = Eigen::MatrixXd::Zero(N, 3);
  Eigen::VectorXd weights = Eigen::VectorXd::Zero(N);
  for(const auto& indexPositionPair : configuration.fixedPositions) {
    referenceMatrix.row(indexPositionPair.first) = indexPositionPair.second.transpose();
    weights(indexPositionPair.first) = 1;
  }

  referenceMatrix *= Utils::Constants::angstrom_per_bohr;

  // Perform the QuaternionFit
  Utils::QuaternionFit fit(referenceMatrix, positions, weights);

  return fit.getFittedData();
}

Molecule narrow(Molecule molecule, Random::Engine& engine) {
  const auto& stereopermutatorList = molecule.stereopermutators();

  do {
    /* If we change any stereopermutator, we must re-check if there are still
     * unassigned stereopermutators since assigning a stereopermutator can
     * invalidate the entire stereopermutator list (because stereopermutators
     * may appear or disappear on assignment due to ranking)
     */
    std::vector<AtomIndex> unassignedAtomStereopermutators;

    for(const auto& atomStereopermutator : stereopermutatorList.atomStereopermutators()) {
      if(!atomStereopermutator.assigned()) {
        unassignedAtomStereopermutators.push_back(
          atomStereopermutator.placement()
        );
      }
    }

    if(!unassignedAtomStereopermutators.empty()) {
      molecule.assignStereopermutatorRandomly(
        Temple::Random::pick(unassignedAtomStereopermutators, engine),
        engine
      );

      // Re-check the loop condition
      continue;
    }

    std::vector<BondIndex> unassignedBondStereopermutators;

    for(const auto& bondStereopermutator : stereopermutatorList.bondStereopermutators()) {
      if(!bondStereopermutator.assigned()) {
        unassignedBondStereopermutators.push_back(
          bondStereopermutator.placement()
        );
      }
    }

    if(!unassignedBondStereopermutators.empty()) {
      molecule.assignStereopermutatorRandomly(
        Temple::Random::pick(unassignedBondStereopermutators, engine),
        engine
      );
    }

  } while(stereopermutatorList.hasUnassignedStereopermutators());

  return molecule;
}

template<typename EigenRefinementType>
struct InversionOrIterLimitStop {
  using VectorType = typename EigenRefinementType::VectorType;
  using FloatType = typename EigenRefinementType::FloatingPointType;

  InversionOrIterLimitStop(
    const unsigned passIter,
    const EigenRefinementType& functor
  ) : iterLimit(passIter),
      refinementFunctorReference(functor)
  {}

  template<typename StepValues>
  bool shouldContinue(unsigned iteration, const StepValues& step) {
    return (
      iteration < iterLimit
      && refinementFunctorReference.proportionChiralConstraintsCorrectSign < 1.0
      && (step.parameters.proposed - step.parameters.current).norm() > minParameterDiffNorm
    );
  }

  const unsigned iterLimit;
  const EigenRefinementType& refinementFunctorReference;
  double minParameterDiffNorm = 1e-3;
};

template<typename FloatType>
struct GradientOrIterLimitStop {
  using VectorType = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>;

  template<typename StepValues>
  bool shouldContinue(unsigned iteration, const StepValues& step) {
    return (
      iteration < iterLimit
      && step.gradients.current.template cast<double>().norm() > gradNorm
      && (step.parameters.proposed - step.parameters.current).norm() > minParameterDiffNorm
    );
  }

  unsigned iterLimit = 10000;
  double gradNorm = 1e-5;
  double minParameterDiffNorm = 1e-3;
};

template<unsigned dimensionality>
Eigen::Vector3d averagePosition(
  const Eigen::VectorXd& linearPositions,
  const std::vector<AtomIndex>& indices
) {
  if(indices.size() == 1) {
    return linearPositions.template segment<3>(dimensionality * indices.front());
  }

  Eigen::Vector3d average = Eigen::Vector3d::Zero();
  for(AtomIndex i : indices) {
    average += linearPositions.template segment<3>(dimensionality * i);
  }
  average /= indices.size();
  return average;
}

template<unsigned dimensionality>
void twistRotatableDihedrals(
  Eigen::Ref<Eigen::VectorXd> positions,
  const std::vector<DihedralConstraint>& constraints,
  const MoleculeDGInformation::GroupMapType& rotatableGroupMap
) {
  for(const DihedralConstraint& constraint : constraints) {
    const AtomIndex j = constraint.sites.at(1).front();
    const AtomIndex k = constraint.sites.at(2).front();
    BondIndex bond {j, k};

    const auto findIter = rotatableGroupMap.find(bond);
    if(findIter == std::end(rotatableGroupMap)) {
      continue;
    }

    const MoleculeDGInformation::RotatableGroup& group = findIter->second;

    const Eigen::Vector3d jVector = positions.template segment<3>(dimensionality * j);
    const Eigen::Vector3d kVector = positions.template segment<3>(dimensionality * k);

    const double measuredDihedral = Cartesian::dihedral(
      averagePosition<dimensionality>(positions, constraint.sites.at(0)),
      jVector,
      kVector,
      averagePosition<dimensionality>(positions, constraint.sites.at(3))
    );

    const double targetDihedral = Cartesian::dihedralAverage(
      constraint.lower,
      constraint.upper
    );

    Eigen::Vector3d axisVector = (kVector - jVector).normalized();
    if(group.side == j) {
      axisVector *= -1;
    }

    const Eigen::Matrix3d rotation = Eigen::AngleAxisd {
      targetDihedral - measuredDihedral,
      axisVector
    }.toRotationMatrix();

    const Eigen::Vector3d sideVector = positions.template segment<3>(dimensionality * group.side);
    for(AtomIndex i : group.vertices) {
      positions.template segment<3>(dimensionality * i) = sideVector + rotation * (
        positions.template segment<3>(dimensionality * i) - sideVector
      );
    }
  }
}

} // namespace Detail

MoleculeDGInformation::GroupMapType MoleculeDGInformation::make(
  const std::vector<DihedralConstraint>& constraints,
  const Molecule& molecule
) {
  MoleculeDGInformation::GroupMapType groups;

  Temple::forEach(constraints,
    [&](const DihedralConstraint& constraint) {
      // It is guaranteed that the sites in the middle have one index only
      BondIndex bond {
        constraint.sites.at(1).front(),
        constraint.sites.at(2).front(),
      };

      // Populate with group to rotate information on the smaller side
      if(molecule.graph().cycles().numCycleFamilies(bond) == 0) {
        auto sides = molecule.graph().splitAlongBridge(bond);
        if(sides.first.size() < sides.second.size()) {
          groups.emplace(
            bond,
            MoleculeDGInformation::RotatableGroup {bond.first, sides.first}
          );
        } else {
          groups.emplace(
            bond,
            MoleculeDGInformation::RotatableGroup {bond.second, sides.second}
          );
        }
      }
    }
  );

  return groups;
}

MoleculeDGInformation gatherDGInformation(
  const Molecule& molecule,
  const Configuration& configuration
) {
  // Generate a spatial model from the molecular graph and stereopermutators
  SpatialModel spatialModel {molecule, configuration};

  // Extract gathered data
  MoleculeDGInformation data;
  data.bounds = spatialModel.makePairwiseBounds();
  data.chiralConstraints = spatialModel.getChiralConstraints();
  data.dihedralConstraints = spatialModel.getDihedralConstraints();
  data.rotatableGroups = MoleculeDGInformation::make(data.dihedralConstraints, molecule);

  return data;
}

outcome::result<AngstromPositions> refine(
  Eigen::MatrixXd embeddedPositions,
  const DistanceBoundsMatrix& distanceBounds,
  const Configuration& configuration,
  const std::shared_ptr<MoleculeDGInformation>& DgDataPtr
) {
  /* Refinement problem compile-time settings
   * - Dimensionality four is needed to ensure chiral constraints invert
   *   nicely
   * - FloatType double is helpful for refinement stability, and float
   *   doesn't affect speed
   * - Using the alternative SIMD implementations of the refinement problems
   *   hardly affects speed at all, in fact, it commonly worsens it.
   */
  constexpr unsigned dimensionality = 4;
  using FloatType = double;
  constexpr bool SIMD = false;

  using FullRefinementType = EigenRefinementProblem<dimensionality, FloatType, SIMD>;
  using VectorType = typename FullRefinementType::VectorType;

  // Vectorize positions
  VectorType transformedPositions = Eigen::Map<Eigen::VectorXd>(
    embeddedPositions.data(),
    embeddedPositions.cols() * embeddedPositions.rows()
  ).template cast<FloatType>().eval();

  const unsigned N = transformedPositions.size() / dimensionality;

  const auto squaredBounds = static_cast<Eigen::MatrixXd>(
    distanceBounds.access().cwiseProduct(distanceBounds.access())
  );

  FullRefinementType refinementFunctor {
    squaredBounds,
    DgDataPtr->chiralConstraints,
    DgDataPtr->dihedralConstraints
  };

  /* If a count of chiral constraints reveals that more than half are
   * incorrect, we can invert the structure (by multiplying e.g. all y
   * coordinates with -1) and then have more than half of chirality
   * constraints correct! In the count, chiral constraints with a target
   * value of zero are not considered (this would skew the count as those
   * chiral constraints should not have to pass an energetic maximum to
   * converge properly as opposed to tetrahedra with volume).
   */
  double initiallyCorrectChiralConstraints = refinementFunctor.calculateProportionChiralConstraintsCorrectSign(transformedPositions);
  if(initiallyCorrectChiralConstraints < 0.5) {
    // Invert y coordinates
    for(unsigned i = 0; i < N; ++i) {
      transformedPositions(dimensionality * i + 1) *= -1;
    }

    initiallyCorrectChiralConstraints = 1 - initiallyCorrectChiralConstraints;
  }

  /* Refinement without penalty on fourth dimension only necessary if not all
   * chiral centers are correct. Of course, for molecules without chiral
   * centers at all, this stage is unnecessary
   */
  unsigned firstStageIterations = 0;
  if(initiallyCorrectChiralConstraints < 1) {
    Detail::InversionOrIterLimitStop<FullRefinementType> inversionChecker {
      configuration.refinementStepLimit,
      refinementFunctor
    };

    Temple::Lbfgs<FloatType, 32> optimizer;

    try {
      auto result = optimizer.minimize(
        transformedPositions,
        refinementFunctor,
        inversionChecker
      );
      firstStageIterations = result.iterations;
    } catch(std::runtime_error& e) {
      return DgError::RefinementException;
    }

    if(firstStageIterations >= configuration.refinementStepLimit) {
      return DgError::RefinementMaxIterationsReached;
    }

    if(refinementFunctor.proportionChiralConstraintsCorrectSign < 1.0) {
      return DgError::RefinedChiralsWrong;
    }
  }

  /* Set up the second stage of refinement where we compress out the fourth
   * dimension that we allowed expansion into to invert the chiralities.
   */
  refinementFunctor.compressFourthDimension = true;

  unsigned secondStageIterations = 0;
  Detail::GradientOrIterLimitStop<FloatType> gradientChecker;
  gradientChecker.gradNorm = 1e-3;
  gradientChecker.iterLimit = configuration.refinementStepLimit - firstStageIterations;

  try {
    Temple::Lbfgs<FloatType, 32> optimizer;

    auto result = optimizer.minimize(
      transformedPositions,
      refinementFunctor,
      gradientChecker
    );
    secondStageIterations = result.iterations;
  } catch(std::out_of_range& e) {
    return DgError::RefinementException;
  }

  // Max iterations reached
  if(secondStageIterations >= gradientChecker.iterLimit) {
    return DgError::RefinementMaxIterationsReached;
  }

  // Not all chiral constraints have the right sign
  if(refinementFunctor.proportionChiralConstraintsCorrectSign < 1) {
    return DgError::RefinedChiralsWrong;
  }

  /* Twist all freely rotatable dihedrals to their target values to avoid
   * conflicts between distance and dihedral errors to prevent rotations to
   * target values.
   */
  Detail::twistRotatableDihedrals<dimensionality>(
    transformedPositions,
    DgDataPtr->dihedralConstraints,
    DgDataPtr->rotatableGroups
  );

  /* Add dihedral terms and refine again */
  unsigned thirdStageIterations = 0;
  gradientChecker = Detail::GradientOrIterLimitStop<FloatType> {};
  gradientChecker.gradNorm = 1e-3;
  gradientChecker.iterLimit = (
    configuration.refinementStepLimit
    - firstStageIterations
    - secondStageIterations
  );

  refinementFunctor.dihedralTerms = true;

  try {
    Temple::Lbfgs<FloatType, 32> optimizer;

    auto result = optimizer.minimize(
      transformedPositions,
      refinementFunctor,
      gradientChecker
    );
    thirdStageIterations = result.iterations;
  } catch(std::out_of_range& e) {
    return DgError::RefinementException;
  }

  if(thirdStageIterations >= gradientChecker.iterLimit) {
    return DgError::RefinementMaxIterationsReached;
  }

  // Structure inacceptable
  if(
    !finalStructureAcceptable(
      refinementFunctor,
      distanceBounds,
      transformedPositions
    )
  ) {
    return DgError::RefinedStructureInacceptable;
  }

  auto gatheredPositions = Detail::gather(transformedPositions);

  if(!configuration.fixedPositions.empty()) {
    return Detail::convertToAngstromPositions(
      Detail::fitAndSetFixedPositions(gatheredPositions, configuration)
    );
  }

  return Detail::convertToAngstromPositions(gatheredPositions);
}

outcome::result<AngstromPositions> generateConformer(
  const Molecule& molecule,
  const Configuration& configuration,
  std::shared_ptr<MoleculeDGInformation>& DgDataPtr,
  bool regenerateDGDataEachStep,
  Random::Engine& engine
) {
  if(regenerateDGDataEachStep) {
    auto moleculeCopy = Detail::narrow(molecule, engine);

    if(moleculeCopy.stereopermutators().hasZeroAssignmentStereopermutators()) {
      return DgError::ZeroAssignmentStereopermutators;
    }

    DgDataPtr = std::make_shared<MoleculeDGInformation>(
      gatherDGInformation(moleculeCopy, configuration)
    );
  }

  ExplicitBoundsGraph explicitGraph {
    molecule.graph().inner(),
    DgDataPtr->bounds
  };

  // Get distance bounds matrix from the graph
  auto distanceBoundsResult = explicitGraph.makeDistanceBounds();
  if(!distanceBoundsResult) {
    return distanceBoundsResult.as_failure();
  }

  DistanceBoundsMatrix distanceBounds {std::move(distanceBoundsResult.value())};

  /* There should be no need to smooth the distance bounds, because the graph
   * type ought to create them within the triangle inequality bounds:
   */
  assert(distanceBounds.boundInconsistencies() == 0);

  // Generate a distances matrix from the graph
  auto distanceMatrixResult = explicitGraph.makeDistanceMatrix(
    engine,
    configuration.partiality
  );
  if(!distanceMatrixResult) {
    return distanceMatrixResult.as_failure();
  }

  // Make a metric matrix from the distances matrix
  MetricMatrix metric(
    std::move(distanceMatrixResult.value())
  );

  // Get a position matrix by embedding the metric matrix
  auto embeddedPositions = metric.embed();

  /* Refinement */
  return refine(
    std::move(embeddedPositions),
    distanceBounds,
    configuration,
    DgDataPtr
  );
}

std::vector<
  outcome::result<AngstromPositions>
> run(
  const Molecule& molecule,
  const unsigned numConformers,
  const Configuration& configuration,
  const boost::optional<unsigned> seedOption
) {
  using ReturnType = std::vector<
    outcome::result<AngstromPositions>
  >;

  // In case there are zero assignment stereopermutators, we give up immediately
  if(molecule.stereopermutators().hasZeroAssignmentStereopermutators()) {
    return ReturnType(numConformers, DgError::ZeroAssignmentStereopermutators);
  }

#ifdef _OPENMP
  /* Ensure the molecule's mutable properties are already generated so none are
   * generated on threaded const-access.
   */
  molecule.graph().inner().populateProperties();
#endif

  /* In case the molecule has unassigned stereopermutators, we need to randomly
   * assign them for each conformer generated prior to generating the distance
   * bounds matrix. If not, then modelling data can be kept across all
   * conformer generation runs since no randomness has entered the equation.
   */
  auto DgDataPtr = std::make_shared<MoleculeDGInformation>();
  bool regenerateEachStep = molecule.stereopermutators().hasUnassignedStereopermutators();
  if(!regenerateEachStep) {
    *DgDataPtr = gatherDGInformation(molecule, configuration);
  }

  ReturnType results(numConformers, static_cast<DgError>(0));

  /* If a seed is supplied, the global prng state is not to be advanced.
   * We create a random engine from the seed here if a seed is supplied.
   */
  auto engineOption = Temple::Optionals::map(
    seedOption,
    [](unsigned seed) { return Random::Engine(seed); }
  );

  /* Now we need something referencing either our new engine or the global prng
   * engine.
   */
  std::reference_wrapper<Random::Engine> backgroundEngineWrapper = randomnessEngine();
  if(engineOption) {
    backgroundEngineWrapper = engineOption.value();
  }
  Random::Engine& backgroundEngine = backgroundEngineWrapper.get();

  /* We have to distribute pseudo-randomness into each thread reproducibly
   * and want to avoid having to guard the global PRNG against access from
   * multiple threads, so we provide each thread its own Engine and pre-generate
   * each conformer's individual seed in the sequential section.
   */
#ifdef _OPENMP
  const unsigned nThreads = omp_get_max_threads();
#else
  const unsigned nThreads = 1;
#endif

  std::vector<Random::Engine> randomnessEngines(nThreads);
  const auto seeds = Temple::Random::getN<int>(
    0,
    std::numeric_limits<int>::max(),
    numConformers,
    backgroundEngine
  );

  /* Each thread has its own DgDataPtr, for the following reason: If we do
   * not need to regenerate the SpatialModel data, then having all threads
   * share access the underlying data to generate conformers is fine. If,
   * otherwise, we do need to regenerate the SpatialModel data for each
   * conformer, then each thread will reset its pointer to its self-generated
   * SpatialModel data, creating thread-private state.
   */
#pragma omp parallel for firstprivate(DgDataPtr) schedule(dynamic)
  for(unsigned i = 0; i < numConformers; ++i) {
    // Get thread-specific randomness engine reference
#ifdef _OPENMP
    Random::Engine& engine = randomnessEngines.at(
      omp_get_thread_num()
    );
#else
    Random::Engine& engine = randomnessEngines.front();
#endif

    // Re-seed the thread-local PRNG engine for each conformer
    engine.seed(seeds.at(i));

    /* We have to handle any and all exceptions here bceause this is a parallel
     * environment and exceptions are not propagated anywhere
     */
    try {
      // Generate the conformer
      auto conformerResult = generateConformer(
        molecule,
        configuration,
        DgDataPtr,
        regenerateEachStep,
        engine
      );

      results.at(i) = std::move(conformerResult);
    } catch(std::exception& e) {
#pragma omp critical(outputWarning)
      {
        std::cerr << "WARNING: Uncaught exception in conformer generation: " << e.what() << "\n";
      }
      results.at(i) = DgError::UnknownException;
    } // end catch
  } // end pragma omp for private(DgDataPtr)

  return results;
}

} // namespace DistanceGeometry
} // namespace Molassembler
} // namespace Scine


#ifndef LOGBROYDEN_HPP_
#define LOGBROYDEN_HPP_

#if defined(_MSC_VER)
#pragma once
#endif

/*
* LEGAL NOTICE
* This computer software was prepared by Battelle Memorial Institute,
* hereinafter the Contractor, under Contract No. DE-AC05-76RL0 1830
* with the Department of Energy ( DOE ). NEITHER THE GOVERNMENT NOR THE
* CONTRACTOR MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY
* LIABILITY FOR THE USE OF THIS SOFTWARE. This notice including this
* sentence must appear on any copies of this computer software.
*
* EXPORT CONTROL
* User agrees that the Software will not be shipped, transferred or
* exported into any country or used in any manner prohibited by the
* United States Export Administration Act or any other applicable
* export laws, restrictions or regulations (collectively the "Export Laws").
* Export of the Software may require some form of license or other
* authority from the U.S. Government, and failure to obtain such
* export control license may result in criminal liability under
* U.S. laws. In addition, if the Software is identified as export controlled
* items under the Export Laws, User represents and warrants that User
* is not a citizen, or otherwise located within, an embargoed nation
* (including without limitation Iran, Syria, Sudan, Cuba, and North Korea)
*     and that User is not otherwise prohibited
* under the Export Laws from receiving the Software.
*
* Copyright 2011 Battelle Memorial Institute.  All Rights Reserved.
* Distributed as open-source under the terms of the Educational Community
* License version 2.0 (ECL 2.0). http://www.opensource.org/licenses/ecl2.php
*
* For further details, see: http://www.globalchange.umd.edu/models/gcam/
*
*/

/*!
 * \file logbroyden.hpp
 * \ingroup objects
 * \brief Header file for the log Broyden solver component
 *
 * \author Robert Link
 */

#include <string>
#include <boost/numeric/ublas/matrix.hpp>
#include "solution/util/include/solvable_nr_solution_info_filter.h"
#include "solution/util/include/edfun.hpp"
#include "solution/util/include/ublas-helpers.hpp"

class CalcCounter;
class Marketplace;
class World;
class SolutionInfoSet;

/*!
 * \ingroup Objects
 * \brief SolverComponent based on the Broyden algorithm using
 * logarithmic prices and EDs.
 *
 * \details Broyden's method is exactly analogous to the
 * Newton-Raphson algorithm, including backtracking.  Broyden differs
 * from N-R in that it uses an approximate Jacobian matrix instead of
 * an exact one.  With each iteration step the approximate Jacobian is
 * updated using the secant formula.  For functions where we don't
 * have an analytic expression for the Jacobian (such as the GCAM
 * excess demand function), Broyden's method provides a huge advantage
 * over using finite-difference Jacobians.
 *
 * \author Robert Link
 */
class LogBroyden: public SolverComponent {
public:
  LogBroyden(Marketplace *mktplc, World *world, CalcCounter *ccounter, int itmax=250,
             double ftol=1.0e-4) :
      SolverComponent(mktplc,world,ccounter), mMaxIter( itmax ), mFTOL( ftol ),
      mLogPricep( true ), mMaxJacobainReuse( 100 ) {}
  virtual ~LogBroyden() {}

  // SolverComponent methods
  virtual void init() {
    if(!mSolutionInfoFilter.get())
      mSolutionInfoFilter.reset(new SolvableNRSolutionInfoFilter());
  }
  virtual ReturnCode solve( SolutionInfoSet& aSolutionSet, const int aPeriod );
  virtual const std::string& getXMLName() const {return SOLVER_NAME;}

  // IParsable methods
  virtual bool XMLParse( const xercesc::DOMNode* aNode );

  static const std::string & getXMLNameStatic( void ) {return SOLVER_NAME;}

protected:
  //! Perform the Broyden's method iterations.
  int bsolve(VecFVec &F, UBVECTOR &x, UBVECTOR &fx,
             UBMATRIX &B, int &neval);
  //! Additional logging for visualizing solver progress.
  void reportVec(const std::string &aname, const UBVECTOR &av, const std::vector<int> &amktids,
                 const std::vector<bool> &aissolvable);
  void reportPSD(UBVECTOR &arptvec, const std::vector<int> &amktids, const std::vector<bool> &aissolvable);

  //! Maximum number of main-loop iterations for the root-finding algorithm
  unsigned int mMaxIter;

  //! Tolerance for convergence test in root-finding algorithm
  //! \warning The SolutionInfo class has its own convergence
  //! tolerance, which it uses to flag certain markets as "unsolved".
  //! If that tolerance is different from this one, the SolutionInfo
  //! might regard a market as unsolved when the solver says it's
  //! solved, or vice versa.
  double mFTOL;

  //! Filter which will be used to determine which markets the solver
  //! will attempt to solve
  std::auto_ptr<ISolutionInfoFilter> mSolutionInfoFilter;

  bool mLogPricep;              //<! flag indicating whether we should work in price or log-price

  // These next two have to be class variables because we sometimes
  // have multiple logbroyden solvers operating.
  static int mLastPer;                 //<! used to detect when the period has changed, so we can reset mPerIter.
  static int mPerIter;                 //<! total iteration count within the period

  //! Control the number of times we can re-use the Jacobian using Broyden's method
  //! which if set to zero implies this algorithm just collapse to a regular NR algorithm
  int mMaxJacobainReuse;

private:
  static std::string SOLVER_NAME;
};

#endif  // LOGBROYDEN_HPP_