/*

CandyPoker
https://github.com/sweeterthancandy/CandyPoker

MIT License

Copyright (c) 2019 Gerry Candy

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

*/
#include "ps/eval/binary_strategy_description.h"
#include <thread>
#include <numeric>
#include <atomic>
#include <bitset>
#include <fstream>
#include <unordered_map>

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

#include "app/pretty_printer.h"
#include "ps/base/algorithm.h"
#include "ps/base/board_combination_iterator.h"
#include "ps/base/cards.h"
#include "ps/base/cards.h"
#include "ps/base/frontend.h"
#include "ps/base/holdem_board_decl.h"
#include "ps/base/rank_hasher.h"
#include "ps/base/suit_hasher.h"
#include "ps/base/tree.h"

#include "ps/detail/tree_printer.h"

#include "ps/eval/class_cache.h"
#include "ps/eval/pass_mask_eval.h"
#include "ps/eval/instruction.h"

#include "ps/support/config.h"
#include "ps/support/index_sequence.h"

#include <boost/timer/timer.hpp>

#include <boost/log/trivial.hpp>

#include <Eigen/Dense>

#include "ps/support/command.h"

#include <boost/iterator/indirect_iterator.hpp>
#include "ps/eval/holdem_class_vector_cache.h"

namespace ps{

        struct static_event : binary_strategy_description::event_decl{
                explicit static_event(std::string const& key, Eigen::VectorXd vec):
                        key_{key}, vec_{vec}
                {}
                virtual std::string key()const override{ return key_; }
                virtual void expected_value_given_event(Eigen::VectorXd& out, holdem_class_vector const& cv, double p)const override{
                        for(size_t idx=0;idx!=vec_.size();++idx){
                                out[idx] += vec_[idx] * p;
                        }
                }
                virtual std::string to_string()const override{
                        std::stringstream sstr;
                        sstr << "Static{key=" << key_ << ", vec_=" << vector_to_string(vec_) << "}";
                        return sstr.str();
                }
        private:
                std::string key_;
                Eigen::VectorXd vec_;
        };
        struct eval_event : binary_strategy_description::event_decl{
                enum{ Debug = 0 };
                /*
                        perm is the vector of players who are in the hand, ie for hu eval
                        it will always be {0,1}, whilst for three players it colud be
                        {0,1}, {0,2}, {1,2}, or {1,2,3}.
                 */
                explicit
                eval_event( binary_strategy_description::eval_view* eval,
                            std::string const& key,
                           std::vector<size_t> perm,
                           Eigen::VectorXd const& dead_money,
                           Eigen::VectorXd const& active)
                        :eval_{eval}, key_(key), perm_{perm}, dead_money_{dead_money}, active_{active}
                        ,pot_amt_{active_.sum() + dead_money_.sum()}
                {

                        delta_proto_.resize(dead_money_.size()+1);
                        delta_proto_.fill(0);
                        for(size_t idx=0;idx!=active_.size();++idx){
                                delta_proto_[idx] -= active_[idx];
                                delta_proto_[idx] -= dead_money_[idx];
                        }

                        if( Debug ){
                                std::cout << "perm => " << detail::to_string(perm) << "\n"; // __CandyPrint__(cxx-print-scalar,perm)
                                std::cout << "dead_money_ => " << vector_to_string(dead_money_) << "\n"; // __CandyPrint__(cxx-print-scalar,dead_money_)
                                std::cout << "active_ => " << vector_to_string(active_) << "\n"; // __CandyPrint__(cxx-print-scalar,active_)
                                std::cout << "delta_proto_ => " << vector_to_string(delta_proto_) << "\n"; // __CandyPrint__(cxx-print-scalar,delta_proto_)
                                std::cout << "pot_amt_ => " << pot_amt_ << "\n"; // __CandyPrint__(cxx-print-scalar,pot_amt_)
                        }

                }
                virtual std::string key()const override{ return key_; }
                virtual void expected_value_given_event(Eigen::VectorXd& out, holdem_class_vector const& cv, double p)const override{

                        // short circuit for optimization purposes
                        if( std::fabs(p) < 0.001 )
                                return;

                        #if 0
                        struct stride_view{
                                enum{ MaxSize = 9 };
                                void construct(std::vector<size_t> mask_, holdem_id* first, holdem_id* last){
                                        first_ = first;
                                        last_ = last;
                                }
                        private:
                                std::array<size_t, 9> view_;
                        };
                        #endif

                        /*
                                We have a perm of the players involed in the hold, perhaps P = (1,2).
                                What we need to do it then map this perm, so that
                                        CV[S[P[0]]] < CV[S[P[0]]],
                         */

                        std::array<size_t, 9> re_perm;
                        std::copy(perm_.begin(), perm_.end(), re_perm.begin());
                        auto re_perm_end = re_perm.begin() + perm_.size();
                        std::sort(re_perm.begin(), re_perm_end, [&](size_t l, size_t r){
                                return cv[l] < cv[r];
                        });

                        std::array<holdem_class_id, 9> tmp;
                        for(size_t idx=0;idx!=perm_.size();++idx){
                                tmp[idx] = cv[re_perm[idx]];
                        }

                        support::array_view<holdem_class_id> tmp_view(&tmp[0], perm_.size());
                        auto ev_ptr = eval_->eval_no_perm(tmp_view);
                        auto const& ev = *ev_ptr;

                        out += delta_proto_ * p;

                        size_t ev_idx = 0;
                        for(size_t idx=0;idx!=perm_.size();++idx){
                                out[re_perm[idx]] += pot_amt_ * ev[ev_idx] * p;
                                ++ev_idx;
                        }
                }
                virtual std::string to_string()const override{
                        std::stringstream sstr;
                        sstr << std::fixed;
                        sstr << "Eval  {key=" << key_ << ", perm=" << detail::to_string(perm_)
                                << ", dead=" << vector_to_string(dead_money_)
                                << ", active=" << vector_to_string(active_)
                                << ", proto=" << vector_to_string(delta_proto_)
                                << ", pot=" << pot_amt_
                                << "}";
                        return sstr.str();
                }
        private:
                binary_strategy_description::eval_view* eval_;
                std::string key_;
                std::vector<size_t> perm_;
                Eigen::VectorXd dead_money_; // not used
                Eigen::VectorXd active_; // not used
                Eigen::VectorXd delta_proto_;
                double pot_amt_;
        };

        struct heads_up_description : binary_strategy_description{
                heads_up_description(eval_view* eval, double sb, double bb, double eff)
                        : sb_{sb}, bb_{bb}, eff_{eff}
                {
                        eval_ = eval;

                        Eigen::VectorXd v_f_{2};
                        v_f_(0) = -sb_;
                        v_f_(1) =  sb_;
                        auto n_f_ = std::make_shared<static_event>("f", v_f_);
                        events_.push_back(n_f_);

                        Eigen::VectorXd v_pf{2};
                        v_pf(0) =  bb_;
                        v_pf(1) = -bb_;
                        auto n_pf = std::make_shared<static_event>("pf", v_pf);
                        events_.push_back(n_pf);

                        Eigen::VectorXd dead_money = Eigen::VectorXd::Zero(2);
                        Eigen::VectorXd active{2};
                        active[0] = eff_;
                        active[1] = eff_;
                        auto n_pp = std::make_shared<eval_event>(eval_, "pp", std::vector<size_t>{0,1}, dead_money, active);
                        events_.push_back(n_pp);

                        strats_.emplace_back(this, 0,0, "SB Pushing", "");
                        strats_.emplace_back(this, 1,1, "BB Calling, given a SB push", "p");

                        finish();
                }
                virtual strategy_impl_t make_inital_state()const override{
                        Eigen::VectorXd proto(169);
                        proto.fill(0.0);
                        strategy_impl_t vec;
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        return vec;
                }
                virtual double sb()const{ return sb_; }
                virtual double bb()const{ return bb_; }
                virtual double eff()const{ return eff_; }
                virtual size_t num_players()const{ return 2; }
                virtual size_t strat_vector_size()const{ return 2; }

                virtual double probability_of_event(std::string const& key, holdem_class_vector const& cv, strategy_impl_t const& impl)const{
                        auto a = impl[0][cv[0]];
                        auto b = impl[1][cv[1]];
                        //if( key == "p" ){ return a; }
                        if( key == "f" ){ return ( 1.0 - a); }
                        if( key == "pp"){ return a * b; }
                        if( key == "pf"){ return a * (1.0 - b ); }
                        std::stringstream sstr;
                        sstr << "unknown key " << key;
                        throw std::domain_error(sstr.str());
                }
                virtual Eigen::VectorXd expected_value_by_class_id(size_t player_idx, strategy_impl_t const& impl)const override{
                        Eigen::VectorXd result(169);
                        result.fill(0);
                        for(auto const& group : *Memory_TwoPlayerClassVector){
                                for(auto const& _ : group.vec){
                                        auto const& cv = _.cv;
                                        auto ev = expected_value_of_vector(aux_event_set_, cv, impl);
                                        result(cv[player_idx]) += _.prob * ev[player_idx];
                                }
                        }
                        return result;
                }
                virtual Eigen::VectorXd expected_value(strategy_impl_t const& impl)const override{
                        Eigen::VectorXd result(2);
                        result.fill(0);
                        for(auto const& group : *Memory_TwoPlayerClassVector){
                                for(auto const& _ : group.vec){
                                        auto const& cv = _.cv;
                                        check_probability_of_event(cv, impl);
                                        auto ev = expected_value_of_vector(aux_event_set_, cv, impl);
                                        result[0] += _.prob * ev[0];
                                        result[1] += _.prob * ev[1];
                                }
                        }
                        return result;
                }
                virtual double expected_value_for_class_id_es(event_set const& es, size_t player_idx, holdem_class_id class_id, strategy_impl_t const& impl)const override{
                        double result = 0.0;
                        holdem_class_vector cv;
                        for(auto const& group : *Memory_TwoPlayerClassVector){
                                if( group.cid != class_id)
                                        continue;
                                for(auto const& _ : group.vec){
                                        cv = _.cv;
                                        if( player_idx != 0 )
                                                std::swap(cv[0], cv[player_idx]);
                                        auto ev = expected_value_of_vector(es, cv, impl);
                                        result += _.prob * ev[player_idx];
                                }
                        }
                        return result;
                }
        private:
                double sb_;
                double bb_;
                double eff_;
        };

        struct three_player_description : binary_strategy_description{
                three_player_description(eval_view* eval, double sb, double bb, double eff)
                        : sb_{sb}, bb_{bb}, eff_{eff}
                {
                        eval_ = eval;

                        size_t num_players = 3;

                        Eigen::VectorXd stacks{num_players};
                        for(size_t idx=0;idx!=num_players;++idx){
                                stacks[idx] = eff_;
                        }

                        Eigen::VectorXd v_blinds{num_players};
                        v_blinds.fill(0.0);
                        v_blinds[1] = sb_;
                        v_blinds[2] = bb_;

                        auto make_static = [&](std::string const& key, size_t target){
                                Eigen::VectorXd sv = -v_blinds;
                                sv[target] += v_blinds.sum();
                                auto ptr = std::make_shared<static_event>(key, sv);
                                return ptr;
                        };

                        for(unsigned long long mask = ( 1 << num_players ); mask != 0;){
                                --mask;
                                std::bitset<32> bs = {mask};

                                std::string key;
                                std::vector<size_t> perm;
                                Eigen::VectorXd dead_money = Eigen::VectorXd::Zero(3);
                                Eigen::VectorXd active     = Eigen::VectorXd::Zero(3);

                                for(size_t idx=0;idx!= num_players;++idx){
                                        if( bs.test(idx) ){
                                                active[idx] = stacks[idx];
                                                perm.push_back(idx);
                                                key += "p";
                                        } else{
                                                dead_money[idx] = v_blinds[idx];
                                                key += "f";
                                        }
                                }

                                if( bs.count() == 0 )
                                        continue;
                                if( bs.count() == 1 && bs.test(num_players-1) ){
                                        // walk
                                        std::string degenerate_key(num_players-1, 'f');
                                        auto walk = make_static(degenerate_key, num_players-1);
                                        events_.push_back(walk);
                                } else if( bs.count() == 1 ){
                                        // steal
                                        auto steal = make_static(key, perm[0]);
                                        events_.push_back(steal);
                                } else {
                                        // push call

                                        auto allin = std::make_shared<eval_event>(eval_, key, perm, dead_money, active);
                                        events_.push_back(allin);
                                }

                        }

                        strats_.emplace_back(this, 0,0, "BTN Pushing"                        , ""  );
                        strats_.emplace_back(this, 1,1, "SB Calling, given BTN Push"         , "p" );
                        strats_.emplace_back(this, 2,1, "SB Pushing, given BTN Fold"         , "f" );
                        strats_.emplace_back(this, 3,2, "BB Calling, given BTN Push, SB Call", "pp");
                        strats_.emplace_back(this, 4,2, "BB Calling, given BTN Push, SB Fold", "pf");
                        strats_.emplace_back(this, 5,2, "BB Calling, given BTN Fold, SB Push", "fp");

                        finish();
                }
                virtual strategy_impl_t make_inital_state()const override{
                        Eigen::VectorXd proto(169);
                        proto.fill(.5);
                        strategy_impl_t vec;
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        vec.emplace_back(proto);
                        return vec;
                }
                virtual double sb()const{ return sb_; }
                virtual double bb()const{ return bb_; }
                virtual double eff()const{ return eff_; }
                virtual size_t num_players()const{ return 3; }
                virtual size_t strat_vector_size()const{ return 6; }

                /*
                         The index of the strategy vector, for example
                         for hu index 0 is for sb to push, whilst index
                         1 is for bb to call a push (given the action p),
                         ie
                                Index |Player|  key  | Given |   P
                                ------+------+-------+-------+------
                                  0   |   0  |   p   |       | P(p)
                                  1   |   1  |   pp  |   p   | P(p|p)

                        For three player this canonical mapping doesn't
                        apply, we have

                                Index |Player|  Key  | Given |   P
                                ------+------+-------+-------+------
                                  0   |   0  |   p   |       | P(p)
                                  1   |   1  |   pp  |   p   | P(p|p)
                                  2   |   1  |   fp  |   f   | P(p|f)
                                  3   |   2  |   ppp |   pp  | P(p|pp)
                                  4   |   2  |   pfp |   pf  | P(p|pf)
                                  5   |   2  |   fpp |   fp  | P(p|fp)
                */
                virtual double probability_of_event(std::string const& key, holdem_class_vector const& cv, strategy_impl_t const& impl)const{
                        enum{ Debug = 0 };
                        static std::unordered_map<std::string, size_t> reg_alloc = {
                                             //  Player
                                { ""  , 0 }, //   0
                                { "p" , 1 }, //   1
                                { "f" , 2 }, //   1
                                { "pp", 3 }, //   2
                                { "pf", 4 }, //   2
                                { "fp", 5 }  //   2
                        };
                        double result = 1.0;
                        std::string sub;
                        std::stringstream dbg;
                        for(size_t idx=0;idx!=key.size();++idx){
                                if( reg_alloc.count(sub) == 0 ){
                                        throw std::domain_error("bad");
                                }
                                auto reg = reg_alloc[sub];
                                switch(key[idx]){
                                case 'p':
                                case 'P':
                                {
                                        dbg << "P<" << reg << "," << ( impl[reg][cv[idx]] ) << ">";
                                        result *= impl[reg][cv[idx]];
                                        sub += 'p';
                                        break;
                                }
                                case 'f':
                                case 'F':
                                {
                                        dbg << "F<" << reg << "," << ( 1 - impl[reg][cv[idx]] )<<">";
                                        result *= ( 1 - impl[reg][cv[idx]] );
                                        sub += 'f';
                                        break;
                                }}
                        }
                        if( Debug ) std::cout << dbg.str() << "\n";
                        return result;
                }
                virtual Eigen::VectorXd expected_value_by_class_id(size_t player_idx, strategy_impl_t const& impl)const override{
                        Eigen::VectorXd result(169);
                        result.fill(0);
                        for(auto const& group : *Memory_ThreePlayerClassVector){
                                for(auto const& _ : group.vec){
                                        auto const& cv = _.cv;
                                        auto ev = expected_value_of_vector(aux_event_set_, cv, impl);
                                        result(cv[player_idx]) += _.prob * ev[player_idx];
                                }
                        }
                        return result;
                }
                virtual Eigen::VectorXd expected_value(strategy_impl_t const& impl)const override{
                        Eigen::VectorXd result(3);
                        result.fill(0);
                        for(auto const& group : *Memory_ThreePlayerClassVector){
                                for(auto const& _ : group.vec){
                                        auto const& cv = _.cv;
                                        auto ev = expected_value_of_vector(aux_event_set_, cv, impl);
                                        result[0] += _.prob * ev[0];
                                        result[1] += _.prob * ev[1];
                                        result[2] += _.prob * ev[2];
                                }
                        }
                        return result;
                }
                virtual double expected_value_for_class_id_es(event_set const& es, size_t player_idx, holdem_class_id class_id, strategy_impl_t const& impl)const override{
                        double result = 0.0;
                        holdem_class_vector cv;
                        for(auto const& group : *Memory_ThreePlayerClassVector){
                                if( group.cid != class_id)
                                        continue;
                                for(auto const& _ : group.vec){
                                        cv = _.cv;
                                        if( player_idx != 0 )
                                                std::swap(cv[0], cv[player_idx]);
                                        auto ev = expected_value_of_vector(es, cv, impl);
                                        result += _.prob * ev[player_idx];
                                }
                        }
                        return result;
                }
        private:
                double sb_;
                double bb_;
                double eff_;
        };

        std::shared_ptr<binary_strategy_description> binary_strategy_description::make_hu_description(eval_view* eval, double sb, double bb, double eff){
                return std::make_shared<heads_up_description>(eval, sb, bb, eff);
        }
        std::shared_ptr<binary_strategy_description> binary_strategy_description::make_three_player_description(eval_view* eval, double sb, double bb, double eff){
                return std::make_shared<three_player_description>(eval, sb, bb, eff);
        }

} // end namespace ps


//
//  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 <string>
#include <armadillo>
#include <cassert>

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

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

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

using namespace std;
using namespace arma;

class BPP_C
{
    private:

    int CC;  //current number of elements
    int GG; //base pairs current elements

    int S;
    int N;

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

    int    (*children2)[2];  //children from pruned tree
    double  *distances2;
    int *parent2;
    int root;
    vec pi;
    vec log_pi;

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

    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 ;
    //vector < vector<int> > Tg; //current history for each element, GG*N
    //vector<double> B;
    vector<mat > posterior_Tg;
    vector<mat > posterior_Tg_next;

    //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;

    vector<mat > log_cache_TM_neut;
    vector<mat > log_cache_TM_cons;
    vector<mat > log_cache_TM_null;

    vector<mat > log_cache_TM_neut_B;
    vector<mat > log_cache_TM_cons_B;
    vector<mat > log_cache_TM_null_B;

    vector< vector<vec> > lambda;  //GG *N *acgt
    vector< vector<vec> > message;  //GG *N *acgt

    //vector<vec> mean_lambda;  //N *acgt
    //vector<vec> mean_message;  //N *acgt

    vector<bool> isGB;

    // samples to output
    double logpZC;
    double logp_Z = 0;
    double MaxLoglik;
    int Max_m;
    vector <int > Max_Z;
    vector <bool> Max_B;
    double Max_b, Max_n, Max_c;

    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< vector<bool> > trace_GB;
    vector <double> trace_n_rate;  //iter*element
    vector <double> trace_c_rate;
    vector<double> trace_n_GB;
    //vector <double> trace_c_GB;

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

    double prop_n;  //for adaptive MCMC, changed by acceptance rate
    double prop_c;
    double prop_nGB;  //for adaptive MCMC, changed by acceptance rate
    vector<double> prop_nGB2;
    double prop_cGB;

    // GSL random number generator
    gsl_rng * RNG;

    time_t last_time;

    unsigned long int seed;

    bool verbose; //= false;

public:

    BPP_C(int c, PhyloProf _prof, BPP& bpp, char gapchar, double missing_thres, bool & filter, bool _verbose)//, double _indel)
    {

        RNG = gsl_rng_alloc(gsl_rng_default);
        gsl_rng_set(RNG, bpp.seed);

        num_burn = bpp.num_burn;   // num of burn-in updates
        num_mcmc = bpp.num_mcmc;   // num of MCMC updates
        adaptive_freq = bpp.num_thin;   // num of updates between two samples, 100

        N = bpp.N;
        CC = c;
        GG =bpp.element_size[c];
        //Tg = vector<vector<int> > (GG, vector<int>(N, -1));
        root = N-1;
        S = bpp.S;

        lambda = vector< vector<vec> >(GG, vector<vec>(N, zeros<vec>(bpp.num_base)));
        message = vector< vector<vec> >(GG, vector<vec>(N, zeros<vec>(bpp.num_base)));
        //ambiguousS_null = vector<vector<vec> > (GG, vector<vec>(S,zeros<vec>(bpp.num_base)));

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

        verbose = _verbose;

        posterior_Tg = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));  //variational posterior prob of I(Tp = a, Ts = b); for root, only use column 0
        posterior_Tg_next = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));  // for q(t+1 -> t+2) at t

        log_cache_TM_neut = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));
        log_cache_TM_cons = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));
        log_cache_TM_null = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));

        log_cache_TM_neut_B = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));
        log_cache_TM_cons_B = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));
        log_cache_TM_null_B = vector<mat >(N, zeros<mat> (bpp.num_base,bpp.num_base));

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

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

        int st = bpp.element_start[CC];
        //get leaves lambda
        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++){
                lambda[g][s].fill(LOG_ZERO);
                if( y[g]== gapchar) //y[g]=='n' ||
                {
                    num_missing[s] ++;
                    //Tg[g][s] = 4;  // for missing base pair
                }
                switch (y[g])
                {
                    case 'a':
                        lambda[g][s][0] = 0; break; //Tg[g][s] = 0;
                    case 'c':
                        lambda[g][s][1] = 0; break; //Tg[g][s] = 1;
                    case 'g':
                        lambda[g][s][2] = 0; break; //Tg[g][s] = 2;
                    case 't':
                        lambda[g][s][3] = 0; break;//Tg[g][s] = 3;
                    case 'r':
                        lambda[g][s][0] = 0;
                        lambda[g][s][2] = 0;
                        break;
                    case 'y':
                        lambda[g][s][1] = 0;
                        lambda[g][s][3] = 0;   break;
                    case 'k':
                        lambda[g][s][2] = 0;
                        lambda[g][s][3] = 0;  break;
                    case 'm':
                        lambda[g][s][0] = 0;
                        lambda[g][s][1] = 0;  break;
                    case 's':
                        lambda[g][s][1] = 0;
                        lambda[g][s][2] = 0;   break;
                    case 'w':
                        lambda[g][s][0] = 0;
                        lambda[g][s][3] = 0;   break;
                    case '-':
                        if(bpp.num_base <= 4){
                            for(int b =0;b<bpp.num_base;b++) lambda[g][s][b] = 0;
                        }
                        else{
                            lambda[g][s][4] = 0;
                        }
                        break; //Tg[g][s] = 4; break;
                    default:
                        for(int b =0;b<bpp.num_base;b++) lambda[g][s][b] = 0;
                        //Tg[g][s] = bpp.num_base;
                }
            }

        }

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

        // set missing
        missing = vector<bool>(N, false);

        for(int s=S; s<N; s++)
        {
            int* p = children2[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;

            }
        }

//        cout << "missing: ";
//        for(int s = 0; s<N;s++)
//        {
//            if(missing[s]) cout << s << " ";
//        }
//        cout <<endl;

        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
        getSubtree_missing(N-1, bpp.upper, -1);
        parent2[root] = N;  //parent2 not correct for all nodes, but children2 is correct

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

        getSubtree(root, nodes);
        for(vector<int>::iterator it = nodes.begin(); it < nodes.end(); it++)
        {
            if(*it>=S)
            {
                internal_nodes.push_back(*it);  // internal nodes
            }

            if(bpp.upper.find(*it)!=bpp.upper.end())
            {
                    upper_c.push_back(*it);
            }

        }

        log_pi = bpp.log_pi;

        // 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_GB = vector<vector<bool> >(num_burn+num_mcmc, vector<bool>(N,0));
        trace_n_rate = vector<double >(num_burn+num_mcmc, 0);
        trace_c_rate = vector<double >(num_burn+num_mcmc, 0);
        trace_n_GB = vector<double> (num_burn+num_mcmc, 0.0);
      //  trace_c_GB = vector<double >(num_burn+num_mcmc, 0);

        //getEmission_ambig();

        prop_c = (double) 1.2; //1.1 0.01; //1/GG;
        prop_n = (double) 1.2; //1.2 0.01; //1/GG ;//* 10;
        //prop_cGB = (double) 1/GG ;
        prop_nGB = 0.2; //1.2 0.4 for real case; //vector<double>(N, 0.1); //(double)1/GG);
        //prop_nGB2 = vector<double>(N, 0.5);  // prob to transit from 1->0
        accept_n_GB = 0; //vector<int>(N, 0);

        for(int s=0; s<N; s++)
        {
            //log_cache_TM_null[s] = bpp.log_cache_TM_null[s]; // already initialized in hpp, will do in below because change of distance
            log_TM_Int[s] = bpp.log_TM_Int[s];
        }

        for(vector<int>:: iterator it = upper_c.begin(); it!=upper_c.end();it++)
        {
            log_TM_Int[*it](1,1) = 0;
            log_TM_Int[*it](2,1) = log(0);
        }

    }

    ~BPP_C()
    {
        gsl_rng_free(RNG);
    }
    mat getRate(mat Q0, double s, double b);
    mat getTransition(int s, double sel, double propos, BPP & bpp);

    void getSubtree(int root, set<int>& child, vector<int> & visited_init);
    void getSubtree(int root, vector<int> & visited_init);
    void getSubtree_missing(int root, set<int>& upper, int child);

    //void getEmission_ambig();
    void initMCMC(int iter, BPP&bpp, int resZ);
    void Update_Tg(int g, vector<bool> visited,BPP& bpp, bool tosample);
    void getUpdateNode(vector<int> changedZ, vector<bool> & visited_init);
    void MonitorChain(int m, bool move_r, int& accept,int& accept2, BPP &bpp, const double add_loglik, const int resZ) ;
    void getUpdateNode(bool neut, bool bg, vector<bool> & visited_init);
    double ELBO();
    double log_f_Xz(vector<bool> visited, vector<vector<vec>>& lambda_tmp, vector<vector<vec>>& message_tmp, bool neut,bool gBC,  double propos, BPP& bpp);
    void update_cache_TM(vector<mat> & Qs, vector<vector<mat >>& log_cache_TM_tmp);
    void init_cache_TM(vector<mat> & Qs);
    //double sample_rate(int resZ, double old_rate, bool neut, vector<bool> visited, double & loglik_old, BPP& bpp, int M =1, bool adaptive = true, double adaptive_factor = 1);
    bool sample_rates(BPP& bpp,double & loglik_old, int resZ, int M =1);
    bool update_rates(BPP& bpp, double & elbo_old, int M = 1, bool adaptive = true, double adaptive_factor = 0.2);

   // double sample_gBC(double old_rate, vector<bool> visited, double & loglik_old, BPP& bpp, int M =1, bool adaptive = true, double adaptive_factor = 1);
    //double sample_gBC0(int s, double old_rate, BPP& bpp,  int M =1, bool adaptive = true, double adaptive_factor = 1);
    void Gibbs(int iter, BPP &bpp, ofstream & outZ, string output_path,string output_path2,int resZ, bool UpR, bool UpHyper, double lrate_prop, double grate_prop);
  //  vector<int> Update_ZB_subtree(BPP& bpp, bool tosample);
    void integrate_ZB(BPP &bpp, bool gb, bool neut, vector<mat>& Qs, vector<vector<double>> & log_emission_tmp, vector<vector<mat>> & log_cache_TM_tmp);
    vector<int> Update_ZB();

    void Output_init(string output_path, string output_path2, BPP& bpp,ofstream& outZ, int resZ);
    void Output_sampling(int iter, string output_path2, BPP &bpp, int resZ);
    void getEmission_update(bool neut, bool gb, vector<vector<double>> & log_emission_tmp,  vector<vector<mat >>& log_cache_TM_tmp, BPP &bpp, int restore);
    double initial_prob_back(BPP &bpp);

    vector<int>  Move_Z(int & propConf, int & revConf,  int & changeZ);
    //double log_f_Xz(vec log_pi, int num_base, vector<int>& Z, vector<mat> & log_cache_TM_neut,  vector<mat> & log_cache_TM_cons);
    double Update_f_Xz(vec log_pi, int num_base, vector<int>& Z, vector<mat> & log_cache_TM_neut, vector<mat> & log_cache_TM_cons, vector<bool> & visited);
    void log_f_Z(vector<int>& Z, vector<mat> & log_Int, double & MH_ratio_g, double & MH_ratio_l);

    //void Eval(BPP&bpp,int resZ); //, int numH,int numHZ);
    set<int> getShifts(vector<bool> & Z);
    void Eval2(BPP&bpp, int resZ);
    double prior_Z(vector<int> & tmpZ) ;
    double prior_Z_subtree(vector<int> & tmpZ) ;
    void adaptive_MCMC(double adaptive_factor, int M =1);

};

#endif /* bpp_c_hpp */


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include "NewtonEulerFrom1DLocalFrameR.hpp"
#include <boost/math/quaternion.hpp>
#include "NewtonEulerDS.hpp"
#include "Interaction.hpp"
#include "BlockVector.hpp"
#include <boost/math/quaternion.hpp>

//#define NERI_DEBUG

//#define NEFC3D_DEBUG
// #define DEBUG_NOCOLOR
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "debug.h"
/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);
#ifdef NEFC3D_DEBUG
  printf("contact normal:\n");
  _Nc->display();
  printf("point de contact :\n");
  _Pc1->display();
  printf("center of masse :\n");
  q1->display();
#endif
  _RotationAbsToContactFrame->setValue(0, 0, Nx);
  _RotationAbsToContactFrame->setValue(0, 1, Ny);
  _RotationAbsToContactFrame->setValue(0, 2, Nz);

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  computeRotationMatrix(q1,_rotationMatrixAbsToBody);
  prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);

  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj, _RotationAbsToContactFrame->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));

#ifdef NEFC3D_DEBUG
  printf("NewtonEulerFrom1DLocalFrameR jhqt\n");
  _jachqT->display();
#endif
}

void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);

  _RotationAbsToContactFrame->setValue(0, 0, Nx);
  _RotationAbsToContactFrame->setValue(0, 1, Ny);
  _RotationAbsToContactFrame->setValue(0, 2, Nz);

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  computeRotationMatrix(q1,_rotationMatrixAbsToBody);
  prod(*_NPG1, *_rotationMatrixAbsToBody, *_AUX1, true);
  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj, _RotationAbsToContactFrame->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));

  double G2x = q2->getValue(0);
  double G2y = q2->getValue(1);
  double G2z = q2->getValue(2);

  _NPG2->zero();
  (*_NPG2)(0, 0) = 0;
  (*_NPG2)(0, 1) = -(G2z - Pz);
  (*_NPG2)(0, 2) = (G2y - Py);
  (*_NPG2)(1, 0) = (G2z - Pz);
  (*_NPG2)(1, 1) = 0;
  (*_NPG2)(1, 2) = -(G2x - Px);
  (*_NPG2)(2, 0) = -(G2y - Py);
  (*_NPG2)(2, 1) = (G2x - Px);
  (*_NPG2)(2, 2) = 0;

  computeRotationMatrix(q2,_rotationMatrixAbsToBody);
  prod(*_NPG2, *_rotationMatrixAbsToBody, *_AUX1, true);

  prod(*_RotationAbsToContactFrame, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj + 6, -_RotationAbsToContactFrame->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj + 6, -_AUX2->getValue(0, jj - 3));
}

void NewtonEulerFrom1DLocalFrameR::initialize(Interaction& inter)
{
  NewtonEulerR::initialize(inter);
  //proj_with_q  _jachqProj.reset(new SimpleMatrix(_jachq->size(0),_jachq->size(1)));
  unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
  _jachq.reset(new SimpleMatrix(1, qSize));

  /* VA 12/04/2016 All of what follows should be put in WorkM*/
  _RotationAbsToContactFrame.reset(new SimpleMatrix(1, 3));
  _rotationMatrixAbsToBody.reset(new SimpleMatrix(3, 3));
  _AUX1.reset(new SimpleMatrix(3, 3));
  _AUX2.reset(new SimpleMatrix(1, 3));
  _NPG1.reset(new SimpleMatrix(3, 3));
  _NPG2.reset(new SimpleMatrix(3, 3));
  //  _isContact=1;
}

void NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0)
{

  DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 ) \n");
  DEBUG_PRINTF("with time =  %f\n",time);
  DEBUG_PRINTF("with inter =  %p\n",&inter);

  _jachq->setValue(0, 0, _Nc->getValue(0));
  _jachq->setValue(0, 1, _Nc->getValue(1));
  _jachq->setValue(0, 2, _Nc->getValue(2));
  if (inter.has2Bodies())
  {
    _jachq->setValue(0, 7, -_Nc->getValue(0));
    _jachq->setValue(0, 8, -_Nc->getValue(1));
    _jachq->setValue(0, 9, -_Nc->getValue(2));
  }

  for (unsigned int iDS =0 ; iDS < q0->numberOfBlocks()  ; iDS++)
  {
    SP::SiconosVector q = (q0->getAllVect())[iDS];
    double sign = 1.0;
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq : ds%d->q :", iDS);
    DEBUG_EXPR_WE(q->display(););

    ::boost::math::quaternion<double>    quatGP;
    if (iDS == 0)
    {
      ::boost::math::quaternion<double>    quatAux(0, _Pc1->getValue(0) - q->getValue(0), _Pc1->getValue(1) - q->getValue(1),
                                                   _Pc1->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    else
    {
      sign = -1.0;
      //cout<<"NewtonEulerFrom1DLocalFrameR::computeJachq sign is -1 \n";
      ::boost::math::quaternion<double>    quatAux(0, _Pc2->getValue(0) - q->getValue(0), _Pc2->getValue(1) - q->getValue(1),
                                                   _Pc2->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :GP :%lf, %lf, %lf\n", quatGP.R_component_2(), quatGP.R_component_3(), quatGP.R_component_4());
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :Q :%e,%e, %e, %e\n", q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatQ(q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatcQ(q->getValue(3), -q->getValue(4), -q->getValue(5), -q->getValue(6));
    ::boost::math::quaternion<double>    quat0(1, 0, 0, 0);
    ::boost::math::quaternion<double>    quatBuff;
    ::boost::math::quaternion<double>    _2qiquatGP;
    _2qiquatGP = quatGP;
    _2qiquatGP *= 2 * (q->getValue(3));
    quatBuff = (quatGP * quatQ) + (quatcQ * quatGP) - _2qiquatGP;

    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :quattBuuf : %e,%e,%e \n", quatBuff.R_component_2(), quatBuff.R_component_3(), quatBuff.R_component_4());

    _jachq->setValue(0, 7 * iDS + 3, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                             quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    //cout<<"WARNING NewtonEulerFrom1DLocalFrameR set jachq \n";
    //_jachq->setValue(0,7*iDS+3,0);
    for (unsigned int i = 1; i < 4; i++)
    {
      ::boost::math::quaternion<double>    quatei(0, (i == 1) ? 1 : 0, (i == 2) ? 1 : 0, (i == 3) ? 1 : 0);
      _2qiquatGP = quatGP;
      _2qiquatGP *= 2 * (q->getValue(3 + i));
      quatBuff = quatei * quatcQ * quatGP - quatGP * quatQ * quatei - _2qiquatGP;
      _jachq->setValue(0, 7 * iDS + 3 + i, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                                   quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    }
  }

  DEBUG_EXPR(_jachq->display(););
  DEBUG_END("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 \n");

}

void NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0 )
{
  DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0 \n")

  if (q0->numberOfBlocks()>1)
  {
    NIcomputeJachqTFromContacts((q0->getAllVect())[0], (q0->getAllVect())[1]);
  }
  else
  {
    NIcomputeJachqTFromContacts((q0->getAllVect())[0]);
  }

  DEBUG_END("NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0) \n");

}

double NewtonEulerFrom1DLocalFrameR::distance() const
{
  SiconosVector dpc(*_Pc2 - *_Pc1);
  return dpc.norm2() * (inner_prod(*_Nc, dpc) >= 0 ? -1 : 1);
}

void NewtonEulerFrom1DLocalFrameR::computeh(double time, BlockVector& q0,
                                            SiconosVector &y)
{
  // Contact points and normal are stored as relative to q1 and q2, if
  // no q2 then pc2 and normal are absolute.

  // Update pc1 based on q0 and relPc1
  SP::SiconosVector q1 = (q0.getAllVect())[0];
  ::boost::math::quaternion<double> qq1((*q1)(3), (*q1)(4), (*q1)(5), (*q1)(6));
  ::boost::math::quaternion<double> qpc1(0,(*_relPc1)(0),(*_relPc1)(1),(*_relPc1)(2));

  // apply q1 rotation and add
  qpc1 = qq1 * qpc1 / qq1;
  (*_Pc1)(0) = qpc1.R_component_2() + (*q1)(0);
  (*_Pc1)(1) = qpc1.R_component_3() + (*q1)(1);
  (*_Pc1)(2) = qpc1.R_component_4() + (*q1)(2);

  if (q0.numberOfBlocks() > 1)
  {
    // Update pc2 based on q0 and relPc2
    SP::SiconosVector q2 = (q0.getAllVect())[1];
    ::boost::math::quaternion<double> qq2((*q2)(3), (*q2)(4), (*q2)(5), (*q2)(6));
    ::boost::math::quaternion<double> qpc2(0,(*_relPc2)(0),(*_relPc2)(1),(*_relPc2)(2));

    // apply q2 rotation and add
    qpc2 = qq2 * qpc2 / qq2;
    (*_Pc2)(0) = qpc2.R_component_2() + (*q2)(0);
    (*_Pc2)(1) = qpc2.R_component_3() + (*q2)(1);
    (*_Pc2)(2) = qpc2.R_component_4() + (*q2)(2);

    // same for normal
    ::boost::math::quaternion<double> qnc(0, (*_relNc)(0), (*_relNc)(1), (*_relNc)(2));
    qnc = qq2 * qnc / qq2;
    (*_Nc)(0) = qnc.R_component_2();
    (*_Nc)(1) = qnc.R_component_3();
    (*_Nc)(2) = qnc.R_component_4();
  }
  else
  {
    *_Pc2 = *_relPc2;
    *_Nc = *_relNc;
  }

  NewtonEulerR::computeh(time, q0, y);
}


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

#include "distances.h"
#include "config.h"

mue::Distance_matrix::Distance_matrix(int teamcount) noexcept
:
	_table(teamcount, _Row(teamcount, 0)),
	_teamcount(teamcount),
	_min_cost(std::numeric_limits<Distance>::max())
{
	BOOST_ASSERT(teamcount <= MAX_TEAMS);
}

void mue::Distance_matrix::set_cost(Team_id const source, Team_id const destination, Distance cost)
{
	BOOST_ASSERT(source < _teamcount);
	BOOST_ASSERT(destination < _teamcount);

	_table[source][destination] = cost;
	_table[destination][source] = cost;

	if (cost < _min_cost)
		_min_cost = cost;
}

// Get "best" distances in the distance table in descending order
std::vector<mue::Distance>
mue::Distance_forecast::_min_costs(Distance_matrix const &distances, unsigned hostcount,
		                   unsigned multiply)
{
	std::vector<Distance> first_tmp;
	for (unsigned src_index = 0; src_index < hostcount; ++src_index) {
		for (unsigned dst_index = 0; dst_index < hostcount; ++dst_index) {
			first_tmp.push_back(distances.lookup(src_index + multiply * hostcount,
							     dst_index + (multiply + 1) * hostcount) * 3);
		}
	}

	std::sort(first_tmp.begin(), first_tmp.end(),
			[] (Distance const &a, Distance const &b) { return a > b; });

	BOOST_ASSERT(first_tmp.size() >= hostcount);

	return std::vector<Distance>(first_tmp.end() - hostcount, first_tmp.end());
}

std::vector<mue::Distance>
mue::Distance_forecast::_fill_costs(Distance_matrix const &distances, unsigned hostcount,
		                    unsigned multiply, Distance const &offset)
{
	std::vector<Distance> min_costs(_min_costs(distances, hostcount, multiply));

	std::vector<Distance> accumulated;
	for (auto i = min_costs.begin(); i != min_costs.end(); ++i) {
		accumulated.push_back(std::accumulate(i, min_costs.end(), offset,
				      [] (Distance const &a, Distance const &b) { return a + b; }));
	}
	accumulated.push_back(offset);

	BOOST_ASSERT(accumulated.size() == hostcount + 1);
	return accumulated;
}

mue::Distance_forecast::Distance_forecast(Distance_matrix const &distances)
:
	_hostcount(distances.teamcount() / 3),
	_second_costs(_fill_costs(distances, _hostcount, 1, 0)),
	_first_costs(_fill_costs(distances, _hostcount, 0, _second_costs[0]))
{
}


//
//  cf-cfm2vtk
//  cf-algorithms
//
//  Created by Jürgen Lorenz Simon on 28/1/13.
//  Copyright (c) 2012 Jürgen Lorenz Simon. All rights reserved.
//

#include <meanie3D/meanie3D.h>

#include <boost/tokenizer.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/exception/info.hpp>

#include <vector>
#include <string>
#include <sstream>
#include <fstream>
#include <exception>
#include <locale>

#include <netcdf>

using namespace boost;
using namespace m3D;
using namespace m3D::utils::vectors;
using namespace netCDF;
using namespace std;

/** Feature-space data type */
typedef double FS_TYPE;

/** Verbosity */
typedef enum {
    FileTypeClusters,
    FileTypeComposite,
    FileTypeUnknown
} FileType;

void parse_commmandline(program_options::variables_map vm,
                        string &filename,
                        string &destination,
                        string &variable,
                        vector<size_t> &vtk_dimension_indexes,
                        FileType &type,
                        bool &extract_skin,
                        bool &write_as_xml,
                        bool &write_displacement_vectors,
                        bool &mindTheTime) {
    // Version

    if (vm.count("version") != 0) {
        cout << m3D::VERSION << endl;
        exit(EXIT_SUCCESS);
    }

    // filename

    if (vm.count("file") == 0) {
        cerr << "Missing 'file' argument" << endl;
        exit(EXIT_FAILURE);
    } else {
        filename = vm["file"].as<string>();
    }

    // destination

    if (vm.count("destination") != 0) {
        destination = vm["destination"].as<string>();
    }

    // variable

    if (vm.count("variable") == 0) {
        cerr << "Missing 'variable' argument" << endl;
        exit(1);
    } else {
        variable = vm["variable"].as<string>();
    }

    // figure out file type

    if (vm.count("type") == 0) {
        cerr << "Missing 'type' argument" << endl;
        exit(EXIT_FAILURE);
    }

    string type_str = vm["type"].as<string>();

    boost::algorithm::to_lower(type_str);

    if (type_str == string("cluster")) {
        type = FileTypeClusters;
    } else if (type_str == string("composite")) {
        type = FileTypeComposite;
    } else {
        type = FileTypeUnknown;
    }

    // Delauney filtering?
    extract_skin = vm.count("extract-skin") > 0;

    // xml?
    write_as_xml = vm.count("write-as-xml") > 0;

    // write-displacement-vectors?
    write_displacement_vectors = vm.count("write-displacement-vectors");

    // VTK dimension mapping

    if (vm.count("vtk-dimensions") > 0) {
        // parse dimension list
        typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
        boost::char_separator<char> sep(",");
        string str_value = vm["vtk-dimensions"].as<string>();
        tokenizer dim_tokens(str_value, sep);
        try {
            NcFile *file = new NcFile(filename, NcFile::read);
            vector<NcDim> dimensions = file->getVar(variable).getDims();

            mindTheTime = false;
            for (size_t i = 0; i < dimensions.size() && !mindTheTime; i++) {
                mindTheTime = dimensions[i].getName() == "time";
            }

            for (tokenizer::iterator tok_iter = dim_tokens.begin(); tok_iter != dim_tokens.end(); ++tok_iter) {
                const char *name = (*tok_iter).c_str();
                NcDim dim = file->getDim(name);
                vector<NcDim>::const_iterator fi = find(dimensions.begin(), dimensions.end(), dim);
                if (fi == dimensions.end()) {
                    cerr << "--vtk-dimension parameter " << dim.getName() << " is not part of " << variable <<
                    "'s dimensions" << endl;
                    exit(EXIT_FAILURE);
                }
                size_t index = fi - dimensions.begin();
                vtk_dimension_indexes.push_back(mindTheTime ? index-1 : index);
            }

            if (vtk_dimension_indexes.size() != dimensions.size()) {
                bool numbersDontAddUp = true;
                if (mindTheTime) {
                    numbersDontAddUp = vtk_dimension_indexes.size() != (dimensions.size() - 1);
                }
                if (numbersDontAddUp) {
                    cerr << "The number of vtk-dimensions must be identical to the number of dimensions of " <<
                    variable << endl;
                    exit(EXIT_FAILURE);
                }
            }

            delete file;
        } catch (const netCDF::exceptions::NcException &e) {
            cerr << e.what() << endl;
            exit(EXIT_FAILURE);
        }
    } else {
        cerr << "Missing parameter --vtk-dimensions" << endl;
        exit(EXIT_FAILURE);;
    }
}

void convert_clusters(const string &filename,
                      const string &destination,
                      bool extract_skin,
                      bool write_as_xml,
                      bool write_displacement_vectors,
                      bool mind_the_time) {
    boost::filesystem::path path(filename);
    CoordinateSystem<FS_TYPE> *cs = NULL;
    ClusterList<FS_TYPE> *list = ClusterList<FS_TYPE>::read(filename, &cs);

    //::m3D::utils::VisitUtils<FS_TYPE>::write_clusters_vtr(list, cs, list->source_file, true, false, true);
    // ::m3D::utils::VisitUtils<FS_TYPE>::write_clusters_vtu(list, cs, list->source_file, 5, true, extract_skin, write_as_xml);

    ::m3D::utils::VisitUtils<FS_TYPE>::write_clusters_vtu(list, cs, list->source_file, 5, true, extract_skin,
                                                          write_as_xml);

    if (write_displacement_vectors) {
        vector<vector<FS_TYPE> > origins;
        vector<vector<FS_TYPE> > displacements;
        for (size_t i = 0; i < list->size(); i++) {
            Cluster<FS_TYPE>::ptr c = list->clusters.at(i);
            if (!c->displacement.empty()) {
                origins.push_back(c->geometrical_center());
                displacements.push_back(c->displacement);
            }
        }
        string displacements_path = path.filename().stem().string() + "-displacements.vtk";
        ::m3D::utils::VisitUtils<FS_TYPE>::write_vectors_vtk(displacements_path, origins, displacements,
                                                             "displacement");
    }

    string centers_path = path.filename().stem().string() + "-centers.vtk";
    ::m3D::utils::VisitUtils<FS_TYPE>::write_geometrical_cluster_centers_vtk(centers_path, list->clusters);

    delete list;
    delete cs;
}

void convert_composite(const string &filename,
                       const string &variable_name,
                       const string &destination) {
    try {
        // construct the destination path
        std::string filename_noext;
        string filename_only = boost::filesystem::path(filename).filename().string();
        boost::filesystem::path destination_path = boost::filesystem::path(destination);
        destination_path /= filename_only;
        destination_path.replace_extension("vtk");
        string dest_path = destination_path.generic_string();

        NcFile *file = new NcFile(filename, NcFile::read);
        vector<NcDim> dims = file->getVar(variable_name).getDims();

        vector<std::string> dimensions;
        vector<std::string> dimension_variables;
        for (size_t i = 0; i < dims.size(); i++) {
            std::string dimName = dims[i].getName();
            dimensions.push_back(dimName);
            dimension_variables.push_back(dimName);
        }

        vector<std::string> variables;
        variables.push_back(variable_name);

        const map<int, double> lower_thresholds, upper_thresholds, fill_values;

        NetCDFDataStore<FS_TYPE> *dataStore = new NetCDFDataStore<FS_TYPE>(
                filename, variables, dimensions, dimension_variables);

        FeatureSpace<FS_TYPE> *fs = new FeatureSpace<FS_TYPE>(
                dataStore->coordinate_system(),
                dataStore,
                lower_thresholds,
                upper_thresholds,
                fill_values);

        vector<string> feature_variables(dimension_variables);
        feature_variables.push_back(variable_name);

        VisitUtils<FS_TYPE>::write_featurespace_variables_vtk(
                filename, fs, feature_variables, variables);

        delete fs;
        delete file;
        delete dataStore;

    } catch (const netCDF::exceptions::NcException &e) {
        cerr << e.what() << endl;
        exit(EXIT_FAILURE);
    }
}

/**
 *
 *
 */
int main(int argc, char **argv) {
    // Declare the supported options.

    program_options::options_description desc("Options");
    desc.add_options()
            ("help,h", "produce help message")
            ("version", "print version information and exit")
            ("file,f", program_options::value<string>(), "CF-Metadata compliant NetCDF-file or a Meanie3D-cluster file")
            ("variable,v", program_options::value<string>(), "Name of the variable to be used")
            ("destination,d", program_options::value<string>()->default_value("."),
             "Name of output directory for the converted files (default '.')")
            ("type,t", program_options::value<string>(), "'clusters' or 'composite'")
#if WITH_VTK
            ("extract-skin,s", "Use delaunay filter to extract skin file")
            ("write-as-xml,x", "Write files in xml instead of ascii")
            ("write-displacement-vectors",
             "Write out an extra file containing the displacement vectors (clusters only).")
            ("vtk-dimensions", program_options::value<string>(),
             "VTK files are written in the order of dimensions given. This may lead to wrong results if the order of the dimensions is not x,y,z. Add the comma-separated list of dimensions here, in the order you would like them to be written as (x,y,z)")
#endif
            ;

    program_options::variables_map vm;
    try {
        program_options::store(program_options::parse_command_line(argc, argv, desc), vm);
        program_options::notify(vm);
    } catch (const netCDF::exceptions::NcException &e) {
        cerr << "NetCDF exception:" << e.what() << endl;
        exit(EXIT_FAILURE);
    } catch (std::exception &e) {
        cerr << "Error parsing command line: " << e.what() << endl;
        cerr << "Check meanie3D-cfm2vtk --help for command line options" << endl;
        exit(EXIT_FAILURE);
    }

    // Version
    if (vm.count("version") != 0) {
        cout << m3D::VERSION << endl;
        exit(EXIT_FAILURE);
    }

    if (vm.count("help") == 1 || argc < 2) {
        cout << desc << "\n";
        return 1;
    }

    // Evaluate user input
    string filename;
    string destination;
    FileType type;
    string variable;
    vector<size_t> vtk_dimension_indexes;
    bool extract_skin = false;
    bool write_as_xml = false;
    bool write_displacement_vectors = false;
    bool mind_the_time = false;

    try {
        parse_commmandline(vm,
                           filename,
                           destination,
                           variable,
                           vtk_dimension_indexes,
                           type,
                           extract_skin,
                           write_as_xml,
                           write_displacement_vectors,
                           mind_the_time);

        // Make the mapping known to the visualization routines
        VisitUtils<FS_TYPE>::VTK_DIMENSION_INDEXES = vtk_dimension_indexes;

        // Select the correct point factory
        PointFactory<FS_TYPE>::set_instance(new PointDefaultFactory<FS_TYPE>());

        switch (type) {
            case FileTypeClusters:
                convert_clusters(filename, destination, extract_skin, write_as_xml, write_displacement_vectors, mind_the_time);
                break;

            case FileTypeComposite:
                convert_composite(filename, variable, destination);
                break;

            default:
                cerr << "Unknown file type" << endl;
                break;
        }

    } catch (const netCDF::exceptions::NcException &e) {
        cerr << "NetCDF exception:" << e.what() << endl;
        exit(EXIT_FAILURE);;
    } catch (const std::exception &e) {
        cerr << "std::exception:" << e.what() << endl;
        cerr << e.what() << endl;
        exit(EXIT_FAILURE);;
    }

    return 0;
};


#include "test/test_bitcoin.h"
#include "test/test_random.h"

#include <stdint.h>
#include <vector>
#include <boost/test/unit_test.hpp>

#include "base58.h"
#include "key.h"
#include "utilstrencodings.h"
#include "script/script.h"
#include "coins.h"
#include "consensus/consensus.h"
#include "core_io.h"
#include "keystore.h"
#include "validation.h"

#include <univalue.h>
#include "rpc/server.h"
#include "rpc/client.h"

#include <boost/assign/list_of.hpp>
#include <boost/algorithm/string.hpp>

// https://www.qiujiawei.com/crypto-1-ecc/
// https://www.cnblogs.com/X-knight/p/9153209.html

BOOST_FIXTURE_TEST_SUITE(me_tests, BasicTestingSetup)

using namespace std;

UniValue test_createrawtransaction() {
    std::string args = "createrawtransaction [{\"txid\":\"db949bca827cbe1f5f18848cf55cbc2eedd3cf9633fb9b88e546171087cef4ff\",\"vout\":0}] {\"mtUjv2TWT2wMQ86LoshoK5U8dPTL213Zep\":0.0001}";
    std::vector<std::string> vArgs;
    boost::split(vArgs, args, boost::is_any_of(" \t"));
    std::string strMethod = vArgs[0];
    vArgs.erase(vArgs.begin());
    JSONRPCRequest request;
    request.params = RPCConvertValues(strMethod, vArgs);

    RPCTypeCheck(request.params, boost::assign::list_of(UniValue::VARR)(UniValue::VOBJ)(UniValue::VNUM), true);
    if (request.params[0].isNull() || request.params[1].isNull())
        throw JSONRPCError(RPC_INVALID_PARAMETER, "Invalid parameter, arguments 1 and 2 must be non-null");

    UniValue inputs = request.params[0].get_array();
    UniValue sendTo = request.params[1].get_obj();

    CMutableTransaction rawTx;

    if (request.params.size() > 2 && !request.params[2].isNull()) {
        int64_t nLockTime = request.params[2].get_int64();
        if (nLockTime < 0 || nLockTime > std::numeric_limits<uint32_t>::max())
            throw JSONRPCError(RPC_INVALID_PARAMETER, "Invalid parameter, locktime out of range");
        rawTx.nLockTime = nLockTime;
    }

    for (unsigned int idx = 0; idx < inputs.size(); idx++) {
        const UniValue& input = inputs[idx];
        const UniValue& o = input.get_obj();

        uint256 txid = ParseHashO(o, "txid");

        const UniValue& vout_v = find_value(o, "vout");
        if (!vout_v.isNum())
            throw JSONRPCError(RPC_INVALID_PARAMETER, "Invalid parameter, missing vout key");
        int nOutput = vout_v.get_int();
        if (nOutput < 0)
            throw JSONRPCError(RPC_INVALID_PARAMETER, "Invalid parameter, vout must be positive");

        uint32_t nSequence = (rawTx.nLockTime ? std::numeric_limits<uint32_t>::max() - 1 : std::numeric_limits<uint32_t>::max());

        // set the sequence number if passed in the parameters object
        const UniValue& sequenceObj = find_value(o, "sequence");
        if (sequenceObj.isNum()) {
            int64_t seqNr64 = sequenceObj.get_int64();
            if (seqNr64 < 0 || seqNr64 > std::numeric_limits<uint32_t>::max())
                throw JSONRPCError(RPC_INVALID_PARAMETER, "Invalid parameter, sequence number is out of range");
            else
                nSequence = (uint32_t)seqNr64;
        }

        CTxIn in(COutPoint(txid, nOutput), CScript(), nSequence);

        rawTx.vin.push_back(in);
    }

    set<CBitcoinAddress> setAddress;
    vector<string> addrList = sendTo.getKeys();
    BOOST_FOREACH(const string& name_, addrList) {

        if (name_ == "data") {
            std::vector<unsigned char> data = ParseHexV(sendTo[name_].getValStr(),"Data");

            CTxOut out(0, CScript() << OP_RETURN << data);
            rawTx.vout.push_back(out);
        } else {
            CBitcoinAddress address(name_);
            if (!address.IsValid())
                throw JSONRPCError(RPC_INVALID_ADDRESS_OR_KEY, string("Invalid Bitcoin address: ")+name_);

            if (setAddress.count(address))
                throw JSONRPCError(RPC_INVALID_PARAMETER, string("Invalid parameter, duplicated address: ")+name_);
            setAddress.insert(address);

            CScript scriptPubKey = GetScriptForDestination(address.Get());
            CAmount nAmount = AmountFromValue(sendTo[name_]);

            CTxOut out(nAmount, scriptPubKey);
            rawTx.vout.push_back(out);
        }
    }

    return EncodeHexTx(rawTx);
}

BOOST_AUTO_TEST_CASE(me_1)
{
    CBitcoinSecret vchSecret;
    vchSecret.SetString("cQ82WpB4FLdszixBJGbPkAEBVxwgaLJsPaUQxFduhMgYcTbAUzK5");
    CKey key = vchSecret.GetKey();
    CPubKey pubKey = key.GetPubKey();
    std::cout << vchSecret.ToString() << " \nkey: " << HexStr(key.begin(), key.end())
              << " \npubKey: " << HexStr(pubKey.begin(), pubKey.end()) << std::endl;
    CKeyID keyID = pubKey.GetID();
    CBitcoinAddress address(keyID);
    std::cout << "address: " << address.ToString() << std::endl;

    std::vector<unsigned char> ret = ParseHex("03873f59bd3550ad4556651e0c1e08d6d10efde5fa120c24aa9fd4cd05097fe694");
    CPubKey pubKey1(ret);
    CBitcoinAddress address1(pubKey1.GetID());
    std::cout << "address: " << address1.ToString() << std::endl;

    CHashWriter ss(SER_GETHASH, 0);
    ss << strMessageMagic;
    std::string strMessage = "hi";
    ss << strMessage;
    std::vector<unsigned char> vchSig;
    key.SignCompact(ss.GetHash(), vchSig);
    std::cout << EncodeBase64(&vchSig[0], vchSig.size()) << std::endl;

    UniValue rawtransaction = test_createrawtransaction();
    std::cout << rawtransaction.get_str() << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


<%
from bempp_operators import bops, compatible_dtypes, dtypes
%>\
#ifndef BEMPP_PYTHON_ASSEMBLY_PYTHON_H
#define BEMPP_PYTHON_ASSEMBLY_PYTHON_H

#include <boost/variant/multivisitors.hpp>

#include "bempp/assembly/boundary_operator.hpp"
#include "bempp/assembly/numerical_quadrature_strategy.hpp"
#include "bempp/assembly/context.hpp"
#include "bempp/assembly/variants.hpp"
#include "bempp/space/types.h"

% for description in bops.values():
#include "${description['header']}"
% endfor

namespace {
    using namespace Bempp;

    //! Basis and Result are compatible
    template<class BASIS, class RESULT> struct is_compatible
        : std::integral_constant<bool, false> {};
% for basis, results in compatible_dtypes.items():
%   for result in results:
    template<> struct is_compatible<${dtypes[basis]}, ${dtypes[result]}>
        : std::integral_constant<bool, true> {};
%   endfor
% endfor

% for opname, description in bops.items():
%     if description['implementation'] == 'standard':
    // \brief Factory functor for boundary operator
    // \details Works with a BoundaryOpVariants (and hence boost::variants) to
    // create a boundary operator via the factory function ${opname}
    template<class RESULT> struct CreateBoundaryOps${opname}
        : public boost::static_visitor<BoundaryOpVariants> {
            CreateBoundaryOps${opname}(
                const Fiber::AccuracyOptionsEx& _accuracyOptions,
                const AssemblyOptions& _assemblyOptions,
                const std::string& _label,
                int _symmetry
            ) : accuracyOptions(_accuracyOptions),
                assemblyOptions(_assemblyOptions),
                label(_label), symmetry(_symmetry) {}

        template<class SPACE> typename std::enable_if<
            // Make sure instantiate only if basis and result types are
            // compatible
            is_compatible<typename SPACE::BasisFunctionType, RESULT> :: value,
            BoundaryOpVariants
        > :: type operator()(
            const shared_ptr<SPACE>& _domain,
            const shared_ptr<SPACE>& _range,
            const shared_ptr<SPACE>& _dualToRange
          ) const {
            typedef typename SPACE::BasisFunctionType t_Basis;
            typedef Context<t_Basis, RESULT> t_Context;
            typedef typename t_Context::QuadratureStrategy t_Strategy;
            typedef NumericalQuadratureStrategy<t_Basis, RESULT> t_NumStrategy;

            const shared_ptr<const t_Strategy> quadStrategy(
                new const t_NumStrategy(accuracyOptions)
            );
            const shared_ptr<const t_Context> context(
                new const t_Context(quadStrategy, assemblyOptions)
            );
            // At least clang can't figure constructing shared_ptr to const
            // Space automatically. So do it by hand.
            shared_ptr<Space<t_Basis> const> domain(_domain);
            shared_ptr<Space<t_Basis> const> range(_range);
            shared_ptr<Space<t_Basis> const> dualToRange(_dualToRange);
            BoundaryOpVariants result;
            result.set(Bempp::${opname}(
                context,
                domain, range, dualToRange,
                label, symmetry
            ));
            return result;
          }

        template<class A, class B, class C> BoundaryOpVariants operator()(
            const shared_ptr<A>& _domain,
            const shared_ptr<B>& _range,
            const shared_ptr<C>& _dualToRange
        ) const {
            throw std::runtime_error("domain, range and/or dual to range "
                    "spaces are incompatible");
        }
        template<class BASIS> typename std::enable_if<
            not is_compatible<BASIS, RESULT> :: value,
            BoundaryOpVariants
        > :: type operator()(
            const shared_ptr<Space<BASIS> const>& _domain,
            const shared_ptr<Space<BASIS> const>& _range,
            const shared_ptr<Space<BASIS> const>& _dualToRange
        ) const {
            throw std::runtime_error("Incompatible basis and result types");
        }

        Fiber::AccuracyOptionsEx accuracyOptions;
        AssemblyOptions assemblyOptions;
        std::string label;
        int symmetry;
    };
    //! Creates a ${opname} operator and stores is in a BoundaryOpVariants
    template<class RESULT>
      BoundaryOpVariants ${description['c_creator']}(
            const Fiber::AccuracyOptionsEx& accuracyOptions,
            const AssemblyOptions& assemblyOptions,
            const SpaceVariants& domain,
            const SpaceVariants& range,
            const SpaceVariants& dualToRange,
            const std::string& label,
            int symmetry
        ) {
            return boost::apply_visitor(
                CreateBoundaryOps${opname}<RESULT>(accuracyOptions,
                    assemblyOptions, label, symmetry),
                domain.variants(), range.variants(), dualToRange.variants()
            );
        }
%     endif
% endfor
}
#endif


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is 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.
// __END_LICENSE__

#include <asp/Sessions/StereoSessionGdal.h>
#include <asp/Camera/XMLBase.h>
#include <asp/Camera/SPOT_XML.h>
#include <boost/scoped_ptr.hpp>
#include <test/Helpers.h>

#include <vw/Stereo/StereoModel.h>

#include <vw/Cartography/GeoTransform.h>

using namespace vw;
using namespace asp;
using namespace xercesc;
using namespace vw::test;

//TODO: Run tests!

/*
TEST(StereoSessionSpot, CreateCamera) {
  StereoSessionDG session;

  boost::shared_ptr<camera::CameraModel> cam1( session.camera_model("", "dg_example1.xml") ),
    cam2( session.camera_model("", "dg_example2.xml") );
  ASSERT_TRUE( cam1.get() != 0 );
  ASSERT_TRUE( cam2.get() != 0 );

  Vector2 m1(13864, 5351), m2(15045, 5183);
  m1 *= 2; m2 *= 2;

  stereo::StereoModel sm(cam1.get(), cam2.get());
  double error;
  Vector3 pos = sm(m1,m2,error);
  EXPECT_LT( error, 5.0 ); // Triangulation should be better than 5 meters.
  EXPECT_LT( norm_2(pos), norm_2(cam1->camera_center(m1)) ); // Point should be below camera.

  // Verify that projection back into the camera hits the right
  // spot. It will slightly be wrong due to the above triangulation error.
#if __GNUC__
#if __x86_64__ || __ppc64__
  EXPECT_VECTOR_NEAR( m1, cam1->point_to_pixel( pos ), 4 );
  EXPECT_VECTOR_NEAR( m2, cam2->point_to_pixel( pos ), 4 );
#else
  EXPECT_VECTOR_NEAR( m1, cam1->point_to_pixel( pos ), 9 );
  EXPECT_VECTOR_NEAR( m2, cam2->point_to_pixel( pos ), 25 );
#endif
#endif

  // A more accurate test is just to project out and back into the same camera
  for ( size_t i = 0; i < 30000; i += 500 ) {
    for ( size_t j = 0; j < 24000; j += 500 ) {
      EXPECT_VECTOR_NEAR( Vector2(i,j),
                          cam1->point_to_pixel( cam1->camera_center(Vector2(i,j)) +
                                                2e4 * cam1->pixel_to_vector( Vector2(i,j) ) ),
                          1e-1 / *pixels* /);
    }
  }

  // Create a camera that only has a single TLC entry. It will throw
  // an error in the event that first line time and TLC entry lookup
  // don't agree. That should be good enough for a test.
  EXPECT_NO_THROW( boost::shared_ptr<camera::CameraModel> cam3( session.camera_model("", "dg_example3.xml") ) );
}

TEST(StereoSessionDG, ProjectRPC) {
  XMLPlatformUtils::Initialize();

  // Read the RPC
  RPCXML xml;
  xml.read_from_file( "dg_example1.xml" );
  boost::scoped_ptr<RPCModel> rpc_model( new RPCModel(*xml.rpc_ptr()) );

  // Read the Digital Globe camera model
  StereoSessionDG session;
  boost::shared_ptr<camera::CameraModel> dg_model( session.camera_model("", "dg_example1.xml") );

  // Verify the measurement between the two cameras
  cartography::Datum datum("WGS84");
  // This should be 0,0
  Vector3 xyz = datum.geodetic_to_cartesian( Vector3(-105.42399,39.833107,2595.9) );
  EXPECT_VECTOR_NEAR( rpc_model->point_to_pixel( xyz ),
                      dg_model->point_to_pixel( xyz ), 25 );
  // This should be 35170, 0
  xyz = datum.geodetic_to_cartesian( Vector3(-105.35823,39.842179,2441.3) );
  EXPECT_VECTOR_NEAR( rpc_model->point_to_pixel( xyz ),
                      dg_model->point_to_pixel( xyz ), 25 );
  // This should be 35170, 23708
  xyz = datum.geodetic_to_cartesian( Vector3(-105.35795,39.803541,2612.6) );
  EXPECT_VECTOR_NEAR( rpc_model->point_to_pixel( xyz ),
                      dg_model->point_to_pixel( xyz ), 25 );
  // This should be 0, 23708
  xyz = datum.geodetic_to_cartesian( Vector3(-105.42418,39.793464,2801.8) );
  EXPECT_VECTOR_NEAR( rpc_model->point_to_pixel( xyz ),
                      dg_model->point_to_pixel( xyz ), 25 );
}

*/


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_SIMD_SHIFTED_ITERATOR_HPP_INCLUDED
#define BOOST_SIMD_SDK_SIMD_SHIFTED_ITERATOR_HPP_INCLUDED

#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/include/functions/unaligned_load.hpp>
#include <boost/simd/sdk/memory/align_on.hpp>
#include <boost/simd/sdk/memory/is_aligned.hpp>
#include <boost/simd/sdk/simd/meta/native_cardinal.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/fusion/container/vector.hpp>
#include <boost/array.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/utility/enable_if.hpp>

namespace boost { namespace simd
{
////////////////////////////////////////////////////////////////////////////////
// simd::shifted_iterator reference sequence of k pack of N elements of type T.
// Each inner simd iterator of the sequence is shifted by one element by default.
////////////////////////////////////////////////////////////////////////////////
  template<class Type, std::size_t Depth>
  struct shifted_iterator
  : public  boost
            ::iterator_facade< shifted_iterator<Type, Depth>
                             , boost::array< pack<Type>, Depth> const
                             , boost::forward_traversal_tag
                             >
  {
  public:

    typedef Type                                        value_type;
    typedef Type*                                       pointer_type;
    typedef pack<Type>                                  pack_type;
    typedef shifted_iterator<Type, Depth>               derived_type;
    typedef std::size_t                                 difference_type;
    typedef boost::array< pack<Type>, Depth>            sequence_type;
    typedef boost::mpl::size_t<Depth>                   static_size;
    //typedef boost::mpl::size_t<Step>                    static_step;

    explicit shifted_iterator(pointer_type p) : base(p)
    { fill(static_size::value-1); }

  protected:

    void fill(difference_type const& sequence_size)
    {
      if(sequence_size == 0) seq[0] = unaligned_load<pack_type>(base);
      else
      {
        seq[sequence_size] = unaligned_load<pack_type>(base+sequence_size);
        fill(sequence_size-1);
      }
    }

    void update( pointer_type const& p)
    {
      for(int i=0; i<static_size::value-1; ++i) { seq[i] = seq[i+1]; }
      seq[static_size::value-1] = unaligned_load<pack_type>(base+(static_size::value-1));
    }

  private:
    friend class boost::iterator_core_access;

    void increment()
    {
      base++;
      update(base);
    }

    bool equal(derived_type const& other) const { return (this->base == other.base); }

    sequence_type const& dereference() const { return seq; }

    pointer_type  base;
    mutable sequence_type seq;
  };

  template< std::size_t Depth, class Iterator>
  shifted_iterator<typename boost::pointee<Iterator>::type, Depth>
  begin(Iterator p)
  {
    typedef typename boost::pointee<Iterator>::type value_type;
    value_type* tmp = &(*p);
    return shifted_iterator<typename boost::pointee<Iterator>::type, Depth>(tmp);
  }

  template<std::size_t Depth,class Iterator>
  shifted_iterator<typename boost::pointee<Iterator>::type, Depth>
  end(Iterator p)
  {
    typedef typename boost::pointee<Iterator>::type value_type;
    value_type* tmp = &(*((p-1)-(meta::native_cardinal<value_type>::value+Depth)));
    return shifted_iterator<typename boost::pointee<Iterator>::type, Depth>(tmp+1);
  }

} }

#endif


//
// Copyright (c) 2019 INRIA
//

#ifndef __pinocchio_autodiff_casadi_hpp__
#define __pinocchio_autodiff_casadi_hpp__

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/math/quaternion.hpp"

#include <casadi/casadi.hpp>
#include <Eigen/Core>

namespace boost { namespace math { namespace constants { namespace detail {
  template<typename Scalar>
  struct constant_pi< ::casadi::Matrix<Scalar> > : constant_pi<double> {};
}}}}

// This is a workaround to make the code compiling with Eigen.
namespace casadi
{
  inline bool operator||(const bool & x, const casadi::Matrix<SXElem> & /*y*/)
  {
    return x;
  }
}

namespace pinocchio
{
  template<typename Scalar>
  struct TaylorSeriesExpansion< ::casadi::Matrix<Scalar> >
  : TaylorSeriesExpansion<Scalar>
  {
    typedef TaylorSeriesExpansion<Scalar> Base;
    using Base::precision;
  };
}

namespace Eigen
{
  namespace internal
  {
    // Specialization of Eigen::internal::cast_impl for Casadi input types
    template<typename Scalar>
    struct cast_impl<casadi::SX,Scalar>
    {
#if EIGEN_VERSION_AT_LEAST(3,2,90)
      EIGEN_DEVICE_FUNC
#endif
      static inline Scalar run(const casadi::SX & x)
      {
        return static_cast<Scalar>(x);
      }
    };

#if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
    template<typename Scalar, bool IsInteger>
    struct significant_decimals_default_impl< ::casadi::Matrix<Scalar>,IsInteger>
    {
      static inline int run()
      {
        return std::numeric_limits<Scalar>::digits10;
      }
    };
#endif
  }
}

namespace Eigen
{
  /// @brief Eigen::NumTraits<> specialization for casadi::SX
  ///
  template<typename Scalar>
  struct NumTraits< casadi::Matrix<Scalar> >
  {
    using Real = casadi::Matrix<Scalar>;
    using NonInteger = casadi::Matrix<Scalar>;
    using Literal = casadi::Matrix<Scalar>;
    using Nested = casadi::Matrix<Scalar>;

    enum {
      // does not support complex Base types
      IsComplex             = 0 ,
      // does not support integer Base types
      IsInteger             = 0 ,
      // only support signed Base types
      IsSigned              = 1 ,
      // must initialize an AD<Base> object
      RequireInitialization = 1 ,
      // computational cost of the corresponding operations
      ReadCost              = 1 ,
      AddCost               = 2 ,
      MulCost               = 2
    };

    static double epsilon()
    {
      return std::numeric_limits<double>::epsilon();
    }

    static double dummy_precision()
    {
      return NumTraits<double>::dummy_precision();
    }

    static double highest()
    {
      return std::numeric_limits<double>::max();
    }

    static double lowest()
    {
      return std::numeric_limits<double>::min();
    }

    static int digits10()
    {
      return std::numeric_limits<double>::digits10;
    }
  };
} // namespace Eigen

namespace pinocchio
{
  namespace casadi
  {
    // Copy casadi matrix to Eigen matrix
    template<typename MT, typename Scalar>
    inline void copy(::casadi::Matrix<Scalar> const & src,
                     Eigen::MatrixBase<MT> & dst)
    {
      Eigen::Index const m = src.size1();
      Eigen::Index const n = src.size2();

      dst.resize(m, n);

      for (Eigen::Index i = 0; i < m; ++i)
        for (Eigen::Index j = 0; j < n; ++j)
          dst(i, j) = src(i, j);
    }

    // Copy Eigen matrix to casadi matrix
    template<typename MT, typename Scalar>
    inline void copy(Eigen::MatrixBase<MT> const & src,
                     ::casadi::Matrix<Scalar> & dst)
    {
      Eigen::Index const m = src.rows();
      Eigen::Index const n = src.cols();

      dst.resize(m, n);

      for (Eigen::Index i = 0; i < m; ++i)
        for (Eigen::Index j = 0; j < n; ++j)
          dst(i, j) = src(i, j);
    }

    // Make an Eigen matrix consisting of pure casadi symbolics
    template<typename MatrixDerived>
    inline void sym(const Eigen::MatrixBase<MatrixDerived> & eig_mat,
                    std::string const & name)
    {
      typedef typename MatrixDerived::Scalar SX;

      MatrixDerived & eig_mat_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixDerived,eig_mat);
      for (Eigen::Index i = 0; i < eig_mat.rows(); ++i)
        for (Eigen::Index j = 0; j < eig_mat.cols(); ++j)
          eig_mat_(i, j) = SX::sym(name + "_" + std::to_string(i) + "_" + std::to_string(j));
    }

  } // namespace casadi
} // namespace pinocchio

#include "pinocchio/math/matrix.hpp"

namespace pinocchio
{
  namespace internal
  {
    template<typename Scalar>
    struct CallCorrectMatrixInverseAccordingToScalar< ::casadi::Matrix<Scalar> >
    {
      typedef ::casadi::Matrix<Scalar> SX;
      template<typename MatrixIn, typename MatrixOut>
      static void run(const Eigen::MatrixBase<MatrixIn> & mat,
                      const Eigen::MatrixBase<MatrixOut> & dest)
      {
        SX cs_mat(mat.rows(),mat.cols());
        casadi::copy(mat.derived(),cs_mat);

        SX cs_mat_inv = SX::inv(cs_mat);

        MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
        casadi::copy(cs_mat_inv,dest_);
      }

    };

  }
}

/// Overloading of max operator
namespace pinocchio
{
  namespace math
  {
    namespace internal
    {
      template<typename Scalar>
      struct return_type_max< ::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar>>
      {
        typedef ::casadi::Matrix<Scalar> type;
      };

      template<typename Scalar, typename T>
      struct return_type_max< ::casadi::Matrix<Scalar>,T>
      {
        typedef ::casadi::Matrix<Scalar> type;
      };

      template<typename Scalar, typename T>
      struct return_type_max<T,::casadi::Matrix<Scalar> >
      {
        typedef ::casadi::Matrix<Scalar> type;
      };

      template<typename Scalar>
      struct call_max< ::casadi::Matrix<Scalar>,::casadi::Matrix<Scalar> >
      {
        static inline ::casadi::Matrix<Scalar> run(const ::casadi::Matrix<Scalar> & a,
                                                   const ::casadi::Matrix<Scalar> & b)
        { return fmax(a,b); }
      };

      template<typename S1, typename S2>
      struct call_max< ::casadi::Matrix<S1>,S2>
      {
        typedef ::casadi::Matrix<S1> CasadiType;
        static inline ::casadi::Matrix<S1> run(const ::casadi::Matrix<S1> & a,
                                               const S2 & b)
        { return fmax(a,static_cast<CasadiType>(b)); }
      };

      template<typename S1, typename S2>
      struct call_max<S1,::casadi::Matrix<S2>>
      {
        typedef ::casadi::Matrix<S2> CasadiType;
        static inline ::casadi::Matrix<S2> run(const S1 & a,
                                               const ::casadi::Matrix<S2> & b)
        { return fmax(static_cast<CasadiType>(a),b); }
      };
    } // namespace internal

  } // namespace math

} // namespace pinocchio

#include "pinocchio/math/quaternion.hpp"
namespace pinocchio
{
  namespace quaternion
  {
    namespace internal
    {

      template<typename _Scalar>
      struct quaternionbase_assign_impl< ::casadi::Matrix<_Scalar> >
      {
        typedef ::casadi::Matrix<_Scalar> Scalar;
        template<typename Matrix3, typename QuaternionDerived>
        static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          typedef typename Eigen::internal::traits<QuaternionDerived>::Coefficients QuatCoefficients;

          typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(QuatCoefficients) QuatCoefficientsPlainType;
          typedef Eigen::Quaternion<Scalar,QuatCoefficientsPlainType::Options> QuaternionPlain;
          QuaternionPlain quat_t_positive;

          Scalar t = mat.trace();
          quaternionbase_assign_impl_if_t_positive::run(t,quat_t_positive,mat);

          QuaternionPlain quat_t_negative_0, quat_t_negative_1, quat_t_negative_2;

          quaternionbase_assign_impl_if_t_negative<0>::run(t,quat_t_negative_0,mat);
          quaternionbase_assign_impl_if_t_negative<1>::run(t,quat_t_negative_1,mat);
          quaternionbase_assign_impl_if_t_negative<2>::run(t,quat_t_negative_2,mat);

          // Build the expression graph
          const Scalar t_greater_than_zero = t > Scalar(0);
          const Scalar cond1 = mat.coeff(1,1) > mat.coeff(0,0);
          const Scalar cond2 = (cond1 && mat.coeff(2,2) > mat.coeff(1,1)) || (mat.coeff(2,2) > mat.coeff(0,0));

          for(Eigen::DenseIndex k = 0; k < 4; ++k)
          {
            Scalar t_is_negative_cond1 = Scalar::if_else(cond1,
                                                         quat_t_negative_1.coeffs().coeff(k),
                                                         quat_t_negative_0.coeffs().coeff(k));
            Scalar t_is_negative_cond2 = Scalar::if_else(cond2,
                                                         quat_t_negative_2.coeffs().coeff(k),
                                                         t_is_negative_cond1);

            q.coeffs().coeffRef(k) = Scalar::if_else(t_greater_than_zero,
                                                     quat_t_positive.coeffs().coeff(k),
                                                     t_is_negative_cond2);
          }

//          if (t > Scalar(0))
//            quaternionbase_assign_impl_if_t_positive::run(t,q,mat);
//          else
//          {
//            Eigen::DenseIndex i = 0;
//            if (mat.coeff(1,1) > mat.coeff(0,0))
//              i = 1;
//            if (mat.coeff(2,2) > mat.coeff(i,i))
//              i = 2;
//
//            if(i==0)
//              quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat);
//            else if(i==1)
//              quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat);
//            else
//              quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat);
//          }
        }
      };

    } // namespace internal

  } // namespace quaternion

} // namespace pinocchio

#include "pinocchio/utils/static-if.hpp"

namespace pinocchio
{
  namespace internal
  {
//    template<typename if_type, typename Scalar, typename else_type>
//    struct traits<if_then_else_impl<if_type,::casadi::Matrix<Scalar>,else_type> >
//    {
//      typedef ::casadi::Matrix<Scalar> ReturnType;
//    };
//
//    template<typename if_type, typename Scalar, typename then_type>
//    struct traits<if_then_else_impl<if_type,then_type,::casadi::Matrix<Scalar>> >
//    {
//      typedef ::casadi::Matrix<Scalar> ReturnType;
//    };

    template<typename Scalar, typename then_type, typename else_type>
    struct if_then_else_impl< ::casadi::Matrix<Scalar>,then_type,else_type>
    {
      typedef typename traits<if_then_else_impl>::ReturnType ReturnType;

      static inline ReturnType run(const ::casadi::Matrix<Scalar> & condition,
                                   const then_type & then_value,
                                   const else_type & else_value)
      {
        return ReturnType::if_else(condition,then_value,else_value);
      }
    };
  }
}

#endif // #ifndef __pinocchio_autodiff_casadi_hpp__


#include "CollisionHandler.h"
#include "operationVector.h"
#include <boost/numeric/ublas/matrix.hpp>
#include "Turn.h"
#include <SFML/Window.hpp>
#include <SFML/Graphics.hpp>
#include "VectorOfTurn.h"
#include "Game.h"
#include "bw.h"
#include "Position.h"
#include "beta.h"

using namespace std;
using namespace boost::numeric::ublas;

void game();
void test();

int main()
{
	game();
// 	test();
	return 0;
}

void game()
{
	Game game;
	game.init();
	game.mainLoop();
}

void test()
{

	matrix<double> P{8, 8};
	std::vector<double> pie{0, 0, 0, 0, 1, 0, 0, 0};

	fill(P, [&](int l, int c){
		if ( abs(l - c) < 2 || (l == 0 && c == 7) || (l == 7 && c == 0)) {
			return 1/3.0;
		}
		return 0.0;
	});

	matrix<double> Obs(6, 8);
	fill(Obs, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 2 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 3 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 4 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 5 && (c == 0 || c == 6 || c == 7)) {
			return 0;
		}
		return 1;
	});

matrix<double> Obs2(3, 8);
	fill(Obs2, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 1 || c == 2)) {
			return 0;
		}

		if (l == 2 && c != 2 ) {
			return 0;

		}

		return 1;
	});

	matrix<double> Obs3(5, 8);
	fill(Obs3, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 2 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 3 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 4 && c!= 5 ) {
			return 0;
		}

		return 1;
	});

		matrix<double> Obs4(9, 8);
	fill(Obs4, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 2 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 3 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 4 && (c == 5 || c == 6 || c == 4)) {
			return 0;
		}

		if (l == 5 && (c == 4 || c == 5 || c == 3)) {
			return 0;
		}

		if (l == 6 && (c == 5 || c == 6 || c == 4)) {
			return 0;
		}

		if (l == 7 && (c == 5 || c == 6 || c == 7)) {
			return 0;
		}

		if (l == 8 && c!= 7 ) {
			return 0;
		}

		return 1;
	});

	matrix<double> Obs5(3, 8);
	fill(Obs5, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 7 || c == 6)) {
			return 0;
		}

		if (l == 2 && c != 6 ) {
			return 0;

		}

		return 1;
	});

			matrix<double> Obs6(9, 8);
	fill(Obs6, [&](int l, int c){
		if (l == 0 && (c == 0 || c == 1 || c == 7)) {
			return 0;
		}

		if (l == 1 && (c == 0 || c == 1 || c == 2)) {
			return 0;
		}

		if (l == 2 && (c == 2 || c == 1 || c == 3)) {
			return 0;
		}

		if (l == 3 && (c == 3 || c == 2 || c == 4)) {
			return 0;
		}

		if (l == 4 && (c == 5 || c == 3 || c == 4)) {
			return 0;
		}

		if (l == 5 && (c == 4 || c == 5 || c == 3)) {
			return 0;
		}

		if (l == 6 && (c == 5 || c == 6 || c == 4)) {
			return 0;
		}

		if (l == 7 && (c == 5 || c == 4 || c == 3)) {
			return 0;
		}

		if (l == 8 && c!= 3 ) {
			return 0;
		}

		return 1;
	});

	// showMat(Beta(Obs, P, pie));

	showMat(BW({Obs,Obs2,Obs3,Obs4,Obs5,Obs6,Obs5,Obs5,Obs5,Obs5},P,pie,1000));

	//showMat(Obs6);
	//showMat(Position(Obs6,BW({Obs,Obs2,Obs3,Obs4,Obs5},P,pie,10),pie));
	//showMat(Position(Obs6,P,pie));
}